From 6ef2e4c9b24c193616060d8fbe1c65584b6be82d Mon Sep 17 00:00:00 2001
From: Paul Riseborough <p_riseborough@live.com.au>
Date: Fri, 29 Apr 2016 12:05:37 +1000
Subject: [PATCH 0001/1230] ecl: update library reference

Numerous EKF updates for improved accuracy and stability
---
 src/lib/ecl | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/src/lib/ecl b/src/lib/ecl
index 199c423f1f..e06c0f41d7 160000
--- a/src/lib/ecl
+++ b/src/lib/ecl
@@ -1 +1 @@
-Subproject commit 199c423f1f38d8b4a12f4003aa1d0a585ce72d12
+Subproject commit e06c0f41d70c840103e37ff392b26cbb0aa5c27e

From 7f1632d65ba982744c09147c71e56fe88840aa46 Mon Sep 17 00:00:00 2001
From: Paul Riseborough <p_riseborough@live.com.au>
Date: Mon, 9 May 2016 09:33:37 +1000
Subject: [PATCH 0002/1230] ekf2: correct control state message for 3D acc bias

---
 src/modules/ekf2/ekf2_main.cpp | 10 +++++-----
 1 file changed, 5 insertions(+), 5 deletions(-)

diff --git a/src/modules/ekf2/ekf2_main.cpp b/src/modules/ekf2/ekf2_main.cpp
index dd30bb54e4..e026f1baeb 100644
--- a/src/modules/ekf2/ekf2_main.cpp
+++ b/src/modules/ekf2/ekf2_main.cpp
@@ -560,11 +560,11 @@ void Ekf2::task_main()
 			// Acceleration data
 			matrix::Vector<float, 3> acceleration = {&sensors.accelerometer_m_s2[0]};
 
-			float accel_bias = 0.0f;
-			_ekf.get_accel_bias(&accel_bias);
-			ctrl_state.x_acc = acceleration(0);
-			ctrl_state.y_acc = acceleration(1);
-			ctrl_state.z_acc = acceleration(2) - accel_bias;
+			float accel_bias[3];
+			_ekf.get_accel_bias(accel_bias);
+			ctrl_state.x_acc = acceleration(0) - accel_bias[0];
+			ctrl_state.y_acc = acceleration(1) - accel_bias[1];
+			ctrl_state.z_acc = acceleration(2) - accel_bias[2];
 
 			// compute lowpass filtered horizontal acceleration
 			acceleration = R_to_body.transpose() * acceleration;

From 2c6b7a008fc21eef5e8127427ea7699da26f9db8 Mon Sep 17 00:00:00 2001
From: Paul Riseborough <p_riseborough@live.com.au>
Date: Mon, 9 May 2016 09:38:10 +1000
Subject: [PATCH 0003/1230] ekf2: Update tuning parameters

Remove unused gyro scale factor parameter
Change gyro and accelerometer bias noise parameters to more intuitive units
Updated tuning defaults
---
 src/modules/ekf2/ekf2_main.cpp |  2 --
 src/modules/ekf2/ekf2_params.c | 56 ++++++++++++++--------------------
 2 files changed, 23 insertions(+), 35 deletions(-)

diff --git a/src/modules/ekf2/ekf2_main.cpp b/src/modules/ekf2/ekf2_main.cpp
index e026f1baeb..b8d1c3c5d4 100644
--- a/src/modules/ekf2/ekf2_main.cpp
+++ b/src/modules/ekf2/ekf2_main.cpp
@@ -176,7 +176,6 @@ private:
 	// process noise
 	control::BlockParamFloat _gyro_bias_p_noise;
 	control::BlockParamFloat _accel_bias_p_noise;
-	control::BlockParamFloat _gyro_scale_p_noise;
 	control::BlockParamFloat _mage_p_noise;
 	control::BlockParamFloat _magb_p_noise;
 	control::BlockParamFloat _wind_vel_p_noise;
@@ -275,7 +274,6 @@ Ekf2::Ekf2():
 	_accel_noise(this, "EKF2_ACC_NOISE", false, &_params->accel_noise),
 	_gyro_bias_p_noise(this, "EKF2_GYR_B_NOISE", false, &_params->gyro_bias_p_noise),
 	_accel_bias_p_noise(this, "EKF2_ACC_B_NOISE", false, &_params->accel_bias_p_noise),
-	_gyro_scale_p_noise(this, "EKF2_GYR_S_NOISE", false, &_params->gyro_scale_p_noise),
 	_mage_p_noise(this, "EKF2_MAG_E_NOISE", false, &_params->mage_p_noise),
 	_magb_p_noise(this, "EKF2_MAG_B_NOISE", false, &_params->magb_p_noise),
 	_wind_vel_p_noise(this, "EKF2_WIND_NOISE", false, &_params->wind_vel_p_noise),
diff --git a/src/modules/ekf2/ekf2_params.c b/src/modules/ekf2/ekf2_params.c
index 98c96ba20d..aaa0785f88 100644
--- a/src/modules/ekf2/ekf2_params.c
+++ b/src/modules/ekf2/ekf2_params.c
@@ -211,7 +211,7 @@ PARAM_DEFINE_FLOAT(EKF2_REQ_VDRIFT, 0.5f);
  * @unit rad/s
  * @decimal 4
  */
-PARAM_DEFINE_FLOAT(EKF2_GYR_NOISE, 6.0e-2f);
+PARAM_DEFINE_FLOAT(EKF2_GYR_NOISE, 1.5e-2f);
 
 /**
  * Accelerometer noise for covariance prediction.
@@ -222,39 +222,29 @@ PARAM_DEFINE_FLOAT(EKF2_GYR_NOISE, 6.0e-2f);
  * @unit m/s/s
  * @decimal 2
  */
-PARAM_DEFINE_FLOAT(EKF2_ACC_NOISE, 0.25f);
+PARAM_DEFINE_FLOAT(EKF2_ACC_NOISE, 3.5e-1f);
 
 /**
- * Process noise for delta angle bias prediction.
- *
- * @group EKF2
- * @min 0.0
- * @max 0.0001
- * @unit rad/s
- * @decimal 8
- */
-PARAM_DEFINE_FLOAT(EKF2_GYR_B_NOISE, 2.5e-6f);
-
-/**
- * Process noise for delta velocity z bias prediction.
- *
- * @group EKF2
- * @min 0.0
- * @max 0.01
- * @unit m/s/s
- * @decimal 7
- */
-PARAM_DEFINE_FLOAT(EKF2_ACC_B_NOISE, 3.0e-5f);
-
-/**
- * Process noise for delta angle scale factor prediction.
+ * Process noise for IMU rate gyro bias prediction.
  *
  * @group EKF2
  * @min 0.0
  * @max 0.01
+ * @unit rad/s**2
  * @decimal 6
  */
-PARAM_DEFINE_FLOAT(EKF2_GYR_S_NOISE, 3.0e-4f);
+PARAM_DEFINE_FLOAT(EKF2_GYR_B_NOISE, 1.0e-3f);
+
+/**
+ * Process noise for IMU accelerometer bias prediction.
+ *
+ * @group EKF2
+ * @min 0.0
+ * @max 0.01
+ * @unit m/s**3
+ * @decimal 6
+ */
+PARAM_DEFINE_FLOAT(EKF2_ACC_B_NOISE, 3.0e-3f);
 
 /**
  * Process noise for body magnetic field prediction.
@@ -265,7 +255,7 @@ PARAM_DEFINE_FLOAT(EKF2_GYR_S_NOISE, 3.0e-4f);
  * @unit Gauss/s
  * @decimal 6
  */
-PARAM_DEFINE_FLOAT(EKF2_MAG_B_NOISE, 5.0e-4f);
+PARAM_DEFINE_FLOAT(EKF2_MAG_B_NOISE, 1.0e-4f);
 
 /**
  * Process noise for earth magnetic field prediction.
@@ -276,7 +266,7 @@ PARAM_DEFINE_FLOAT(EKF2_MAG_B_NOISE, 5.0e-4f);
  * @unit Gauss/s
  * @decimal 6
  */
-PARAM_DEFINE_FLOAT(EKF2_MAG_E_NOISE, 2.5e-3f);
+PARAM_DEFINE_FLOAT(EKF2_MAG_E_NOISE, 1.0e-3f);
 
 /**
  * Process noise for wind velocity prediction.
@@ -331,7 +321,7 @@ PARAM_DEFINE_FLOAT(EKF2_NOAID_NOISE, 10.0f);
  * @unit m
  * @decimal 2
  */
-PARAM_DEFINE_FLOAT(EKF2_BARO_NOISE, 3.0f);
+PARAM_DEFINE_FLOAT(EKF2_BARO_NOISE, 2.0f);
 
 /**
  * Measurement noise for magnetic heading fusion.
@@ -419,8 +409,7 @@ PARAM_DEFINE_INT32(EKF2_DECL_TYPE, 7);
  * @value 0 Automatic
  * @value 1 Magnetic heading
  * @value 2 3-axis fusion
- * @value 3 2-D projection
- * @value 4 None
+ * @value 3 None
  */
 PARAM_DEFINE_INT32(EKF2_MAG_TYPE, 0);
 
@@ -466,15 +455,16 @@ PARAM_DEFINE_FLOAT(EKF2_GPS_V_GATE, 5.0f);
 PARAM_DEFINE_INT32(EKF2_REC_RPL, 0);
 
 /**
- * Integer bitmask controlling which external aiding sources will be used.
+ * Integer bitmask controlling data fusion and aiding methods.
  *
  * Set bits in the following positions to enable:
  * 0 : Set to true to use GPS data if available
  * 1 : Set to true to use optical flow data if available
+ * 2 : Set to true to inhibit IMU bias estimation
  *
  * @group EKF2
  * @min 0
- * @max 3
+ * @max 15
  */
 PARAM_DEFINE_INT32(EKF2_AID_MASK, 1);
 

From e8c34fa30e5e15708f9fc21851b78d461a291b17 Mon Sep 17 00:00:00 2001
From: Paul Riseborough <p_riseborough@live.com.au>
Date: Mon, 9 May 2016 09:39:21 +1000
Subject: [PATCH 0004/1230] msg: add filter internal fault message

---
 msg/estimator_status.msg | 16 ++++++++++++++++
 1 file changed, 16 insertions(+)

diff --git a/msg/estimator_status.msg b/msg/estimator_status.msg
index cda4844089..93e337526d 100644
--- a/msg/estimator_status.msg
+++ b/msg/estimator_status.msg
@@ -32,3 +32,19 @@ uint16 control_mode_flags	# Bitmask to indicate EKF logic state
 # 11 - true when baro height is being fused as a primary height reference
 # 12 - true when range finder height is being fused as a primary height reference
 # 15 - true when range finder height is being fused as a primary height reference
+uint16 filter_fault_flags	# Bitmask to indicate EKF internal faults
+# 0 - true if the fusion of the magnetometer X-axis has encountered a numerical error
+# 1 - true if the fusion of the magnetometer Y-axis has encountered a numerical error
+# 2 - true if the fusion of the magnetometer Z-axis has encountered a numerical error
+# 3 - true if the fusion of the magnetic heading has encountered a numerical error
+# 4 - true if the fusion of the magnetic declination has encountered a numerical error
+# 5 - true if fusion of the airspeed has encountered a numerical error
+# 6 - true if fusion of the synthetic sideslip constraint has encountered a numerical error
+# 7 - true if fusion of the optical flow X axis has encountered a numerical error
+# 8 - true if fusion of the optical flow Y axis has encountered a numerical error
+# 9 - true if fusion of the North velocity has encountered a numerical error
+# 10 - true if fusion of the East velocity has encountered a numerical error
+# 11 - true if fusion of the Down velocity has encountered a numerical error
+# 12 - true if fusion of the North position has encountered a numerical error
+# 13 - true if fusion of the East position has encountered a numerical error
+# 14 - true if fusion of the Down position has encountered a numerical error

From af9a7a39f2f376da16d3270a30ce6bb96c6ef95e Mon Sep 17 00:00:00 2001
From: Paul Riseborough <p_riseborough@live.com.au>
Date: Mon, 9 May 2016 09:40:02 +1000
Subject: [PATCH 0005/1230] ekf2: add handling of filter internal fault message

---
 src/modules/ekf2/ekf2_main.cpp               | 1 +
 src/modules/ekf2_replay/ekf2_replay_main.cpp | 2 +-
 2 files changed, 2 insertions(+), 1 deletion(-)

diff --git a/src/modules/ekf2/ekf2_main.cpp b/src/modules/ekf2/ekf2_main.cpp
index b8d1c3c5d4..ba46f94c83 100644
--- a/src/modules/ekf2/ekf2_main.cpp
+++ b/src/modules/ekf2/ekf2_main.cpp
@@ -732,6 +732,7 @@ void Ekf2::task_main()
 		_ekf.get_covariances(status.covariances);
 		_ekf.get_gps_check_status(&status.gps_check_fail_flags);
 		_ekf.get_control_mode(&status.control_mode_flags);
+		_ekf.get_filter_fault_status(&status.filter_fault_flags);
 
 		if (_estimator_status_pub == nullptr) {
 			_estimator_status_pub = orb_advertise(ORB_ID(estimator_status), &status);
diff --git a/src/modules/ekf2_replay/ekf2_replay_main.cpp b/src/modules/ekf2_replay/ekf2_replay_main.cpp
index abe3a548ab..30f22620f5 100644
--- a/src/modules/ekf2_replay/ekf2_replay_main.cpp
+++ b/src/modules/ekf2_replay/ekf2_replay_main.cpp
@@ -518,7 +518,7 @@ void Ekf2Replay::logIfUpdated()
 		memcpy(&(log_message.body.est0.s), est_status.states, maxcopy0);
 		log_message.body.est0.n_states = est_status.n_states;
 		log_message.body.est0.nan_flags = est_status.nan_flags;
-		log_message.body.est0.health_flags = est_status.health_flags;
+		log_message.body.est0.fault_flags = est_status.filter_fault_flags;
 		log_message.body.est0.timeout_flags = est_status.timeout_flags;
 		writeMessage(_write_fd, (void *)&log_message.head1, _formats[LOG_EST0_MSG].length);
 

From 4b8c7201b716ac48eab72800040f61b5fd2dd2ef Mon Sep 17 00:00:00 2001
From: Paul Riseborough <p_riseborough@live.com.au>
Date: Mon, 9 May 2016 09:40:28 +1000
Subject: [PATCH 0006/1230] sdlog2: add logging of ekf2 internal fault message

---
 src/modules/sdlog2/sdlog2.c          | 2 +-
 src/modules/sdlog2/sdlog2_messages.h | 4 ++--
 2 files changed, 3 insertions(+), 3 deletions(-)

diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c
index 3af4872203..82ad042821 100644
--- a/src/modules/sdlog2/sdlog2.c
+++ b/src/modules/sdlog2/sdlog2.c
@@ -2019,7 +2019,7 @@ int sdlog2_thread_main(int argc, char *argv[])
 				memcpy(&(log_msg.body.log_EST0.s), buf.estimator_status.states, maxcopy0);
 				log_msg.body.log_EST0.n_states = buf.estimator_status.n_states;
 				log_msg.body.log_EST0.nan_flags = buf.estimator_status.nan_flags;
-				log_msg.body.log_EST0.health_flags = buf.estimator_status.health_flags;
+				log_msg.body.log_EST0.fault_flags = buf.estimator_status.filter_fault_flags;
 				log_msg.body.log_EST0.timeout_flags = buf.estimator_status.timeout_flags;
 				LOGBUFFER_WRITE_AND_COUNT(EST0);
 
diff --git a/src/modules/sdlog2/sdlog2_messages.h b/src/modules/sdlog2/sdlog2_messages.h
index 9a8d958d6c..c5dbffe6f3 100644
--- a/src/modules/sdlog2/sdlog2_messages.h
+++ b/src/modules/sdlog2/sdlog2_messages.h
@@ -396,7 +396,7 @@ struct log_EST0_s {
 	float s[12];
 	uint8_t n_states;
 	uint8_t nan_flags;
-	uint8_t health_flags;
+	uint16_t fault_flags;
 	uint8_t timeout_flags;
 };
 
@@ -662,7 +662,7 @@ static const struct log_format_s log_formats[] = {
 	LOG_FORMAT_S(TEL1, TEL, "BBBBHHBQ",		"RSSI,RemRSSI,Noise,RemNoise,RXErr,Fixed,TXBuf,HbTime"),
 	LOG_FORMAT_S(TEL2, TEL, "BBBBHHBQ",		"RSSI,RemRSSI,Noise,RemNoise,RXErr,Fixed,TXBuf,HbTime"),
 	LOG_FORMAT_S(TEL3, TEL, "BBBBHHBQ",		"RSSI,RemRSSI,Noise,RemNoise,RXErr,Fixed,TXBuf,HbTime"),
-	LOG_FORMAT(EST0, "ffffffffffffBBBB",	"s0,s1,s2,s3,s4,s5,s6,s7,s8,s9,s10,s11,nStat,fNaN,fHealth,fTOut"),
+	LOG_FORMAT(EST0, "ffffffffffffBBHB",	"s0,s1,s2,s3,s4,s5,s6,s7,s8,s9,s10,s11,nStat,fNaN,fFault,fTOut"),
 	LOG_FORMAT(EST1, "ffffffffffffffff",	"s12,s13,s14,s15,s16,s17,s18,s19,s20,s21,s22,s23,s24,s25,s26,s27"),
 	LOG_FORMAT(EST2, "ffffffffffffHH",    "P0,P1,P2,P3,P4,P5,P6,P7,P8,P9,P10,P11,GCHK,CTRL"),
 	LOG_FORMAT(EST3, "ffffffffffffffff",    "P12,P13,P14,P15,P16,P17,P18,P19,P20,P21,P22,P23,P24,P25,P26,P27"),

From 4480e4206dc1fe5899e3e4c6aab60d0edf86c8d0 Mon Sep 17 00:00:00 2001
From: Paul Riseborough <p_riseborough@live.com.au>
Date: Tue, 10 May 2016 10:18:17 +1000
Subject: [PATCH 0007/1230] ekf2: remove use of arm status

---
 src/modules/ekf2/ekf2_main.cpp | 19 +++++--------------
 1 file changed, 5 insertions(+), 14 deletions(-)

diff --git a/src/modules/ekf2/ekf2_main.cpp b/src/modules/ekf2/ekf2_main.cpp
index ba46f94c83..25441ca16f 100644
--- a/src/modules/ekf2/ekf2_main.cpp
+++ b/src/modules/ekf2/ekf2_main.cpp
@@ -141,7 +141,7 @@ private:
 	int		_actuator_armed_sub = -1;
 	int		_vehicle_land_detected_sub = -1;
 
-	bool            _prev_motors_armed = false; // motors armed status from the previous frame
+	bool            _prev_landed = true; // landed status from the previous frame
 
 	float _acc_hor_filt = 0.0f; 	// low-pass filtered horizontal acceleration
 
@@ -349,7 +349,6 @@ void Ekf2::task_main()
 	_params_sub = orb_subscribe(ORB_ID(parameter_update));
 	_optical_flow_sub = orb_subscribe(ORB_ID(optical_flow));
 	_range_finder_sub = orb_subscribe(ORB_ID(distance_sensor));
-	_actuator_armed_sub = orb_subscribe(ORB_ID(actuator_armed));
 	_vehicle_land_detected_sub = orb_subscribe(ORB_ID(vehicle_land_detected));
 
 	px4_pollfd_struct_t fds[2] = {};
@@ -369,7 +368,7 @@ void Ekf2::task_main()
 	airspeed_s airspeed = {};
 	optical_flow_s optical_flow = {};
 	distance_sensor_s range_finder = {};
-	actuator_armed_s actuator_armed = {};
+	vehicle_land_detected_s vehicle_land_detected = {};
 
 	while (!_task_should_exit) {
 		int ret = px4_poll(fds, sizeof(fds) / sizeof(fds[0]), 1000);
@@ -402,7 +401,6 @@ void Ekf2::task_main()
 		bool airspeed_updated = false;
 		bool optical_flow_updated = false;
 		bool range_finder_updated = false;
-		bool actuator_armed_updated = false;
 		bool vehicle_land_detected_updated = false;
 
 		orb_copy(ORB_ID(sensor_combined), _sensors_sub, &sensors);
@@ -502,17 +500,9 @@ void Ekf2::task_main()
 			_ekf.setRangeData(range_finder.timestamp, &range_finder.current_distance);
 		}
 
-		orb_check(_actuator_armed_sub, &actuator_armed_updated);
-
-		if (actuator_armed_updated) {
-			orb_copy(ORB_ID(actuator_armed), _actuator_armed_sub, &actuator_armed);
-			_ekf.set_arm_status(actuator_armed.armed);
-		}
-
 		orb_check(_vehicle_land_detected_sub, &vehicle_land_detected_updated);
 
 		if (vehicle_land_detected_updated) {
-			struct vehicle_land_detected_s vehicle_land_detected = {};
 			orb_copy(ORB_ID(vehicle_land_detected), _vehicle_land_detected_sub, &vehicle_land_detected);
 			_ekf.set_in_air_status(!vehicle_land_detected.landed);
 		}
@@ -780,12 +770,13 @@ void Ekf2::task_main()
 			orb_publish(ORB_ID(ekf2_innovations), _estimator_innovations_pub, &innovations);
 		}
 
-		// save the declination to the EKF2_MAG_DECL parameter when a dis-arm event is detected
-		if ((_params->mag_declination_source & (1 << 1)) && _prev_motors_armed && !actuator_armed.armed) {
+		// save the declination to the EKF2_MAG_DECL parameter when a land event is detected
+		if ((_params->mag_declination_source & (1 << 1)) && !_prev_landed && vehicle_land_detected.landed) {
 			float decl_deg;
 			_ekf.copy_mag_decl_deg(&decl_deg);
 			_mag_declination_deg.set(decl_deg);
 		}
+		_prev_landed = vehicle_land_detected.landed;
 
 		// publish replay message if in replay mode
 		bool publish_replay_message = (bool)_param_record_replay_msg.get();

From 163ad19957d1440eeffb750ae19b56a1ac84c2f4 Mon Sep 17 00:00:00 2001
From: Paul Riseborough <p_riseborough@live.com.au>
Date: Tue, 10 May 2016 10:44:15 +1000
Subject: [PATCH 0008/1230] msg: update documentation for estimator status

---
 msg/estimator_status.msg | 16 +++++++---------
 1 file changed, 7 insertions(+), 9 deletions(-)

diff --git a/msg/estimator_status.msg b/msg/estimator_status.msg
index 93e337526d..c145a21501 100644
--- a/msg/estimator_status.msg
+++ b/msg/estimator_status.msg
@@ -23,15 +23,13 @@ uint16 control_mode_flags	# Bitmask to indicate EKF logic state
 # 2 - true if GPS measurements are being fused
 # 3 - true if optical flow measurements are being fused
 # 4 - true if a simple magnetic yaw heading is being fused
-# 5 - true if the horizontal projection of magnetometer data is being fused
-# 6 - true if 3-axis magnetometer measurement are being fused
-# 7 - true if synthetic magnetic declination measurements are being fused
-# 8 - true when the vehicle is airborne
-# 9 - true when the vehicle motors are armed
-# 10 - true when wind velocity is being estimated
-# 11 - true when baro height is being fused as a primary height reference
-# 12 - true when range finder height is being fused as a primary height reference
-# 15 - true when range finder height is being fused as a primary height reference
+# 5 - true if 3-axis magnetometer measurement are being fused
+# 6 - true if synthetic magnetic declination measurements are being fused
+# 7 - true when the vehicle is airborne
+# 8 - true when wind velocity is being estimated
+# 9 - true when baro height is being fused as a primary height reference
+# 10 - true when range finder height is being fused as a primary height reference
+# 11 - true when range finder height is being fused as a primary height reference
 uint16 filter_fault_flags	# Bitmask to indicate EKF internal faults
 # 0 - true if the fusion of the magnetometer X-axis has encountered a numerical error
 # 1 - true if the fusion of the magnetometer Y-axis has encountered a numerical error

From 3179b12dd3a5dd9a6922fe81f1f1594222f85396 Mon Sep 17 00:00:00 2001
From: Paul Riseborough <p_riseborough@live.com.au>
Date: Tue, 10 May 2016 10:23:50 +1000
Subject: [PATCH 0009/1230] ecl: update library reference

removes use of vehicle arm status message
---
 src/lib/ecl | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/src/lib/ecl b/src/lib/ecl
index e06c0f41d7..e91a934f07 160000
--- a/src/lib/ecl
+++ b/src/lib/ecl
@@ -1 +1 @@
-Subproject commit e06c0f41d70c840103e37ff392b26cbb0aa5c27e
+Subproject commit e91a934f07fe8bed33e6e5d2a7ba9670e43e5561

From 7d64aa80575c348f2086bbd3e9592e42286e3266 Mon Sep 17 00:00:00 2001
From: Julian Oes <julian@oes.ch>
Date: Wed, 11 May 2016 08:01:42 +0200
Subject: [PATCH 0010/1230] ekf2: raise stack size on Snapdragon

It seems that the stack size was exhausted on Snapdragon.
---
 src/modules/ekf2/ekf2_main.cpp | 10 +++++++++-
 1 file changed, 9 insertions(+), 1 deletion(-)

diff --git a/src/modules/ekf2/ekf2_main.cpp b/src/modules/ekf2/ekf2_main.cpp
index 25441ca16f..c8eddba90c 100644
--- a/src/modules/ekf2/ekf2_main.cpp
+++ b/src/modules/ekf2/ekf2_main.cpp
@@ -873,11 +873,19 @@ int Ekf2::start()
 {
 	ASSERT(_control_task == -1);
 
+#ifdef __PX4_QURT
+	// On the DSP we seem to get random crashes with a stack size below 13000.
+	const unsigned stack_size = 15000;
+#else
+	const unsigned stack_size = 9000;
+#endif
+
+
 	/* start the task */
 	_control_task = px4_task_spawn_cmd("ekf2",
 					   SCHED_DEFAULT,
 					   SCHED_PRIORITY_MAX - 5,
-					   9000,
+					   stack_size,
 					   (px4_main_t)&Ekf2::task_main_trampoline,
 					   nullptr);
 

From d7daab9620b1a877edad846f216751905a9bcca9 Mon Sep 17 00:00:00 2001
From: David Sidrane <david_s5@nscdg.com>
Date: Fri, 6 May 2016 02:43:29 -1000
Subject: [PATCH 0011/1230] Move the build log to nuttx directory

---
 cmake/nuttx/px4_impl_nuttx.cmake | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/cmake/nuttx/px4_impl_nuttx.cmake b/cmake/nuttx/px4_impl_nuttx.cmake
index 51e8e5e6a9..061e567d33 100644
--- a/cmake/nuttx/px4_impl_nuttx.cmake
+++ b/cmake/nuttx/px4_impl_nuttx.cmake
@@ -212,7 +212,7 @@ function(px4_nuttx_add_export)
 		COMMAND ${MAKE} --no-print-directory -C${nuttx_src}/nuttx -r --quiet distclean
 		COMMAND ${CP} -r ${CMAKE_SOURCE_DIR}/nuttx-configs/PX4_Warnings.mk ${nuttx_src}/nuttx/
 		COMMAND ${CP} -r ${CMAKE_SOURCE_DIR}/nuttx-configs/${CONFIG} ${nuttx_src}/nuttx/configs
-		COMMAND cd ${nuttx_src}/nuttx/tools && ./configure.sh ${CONFIG}/nsh
+		COMMAND cd ${nuttx_src}/nuttx/tools && ./configure.sh ${CONFIG}/nsh && cd ..
 		#COMMAND ${ECHO} Exporting NuttX for ${CONFIG}
 		COMMAND ${MAKE} --no-print-directory --quiet -C ${nuttx_src}/nuttx -j${THREADS} -r CONFIG_ARCH_BOARD=${CONFIG} export > nuttx_build.log
 		COMMAND ${CP} -r ${nuttx_src}/nuttx/nuttx-export.zip ${CMAKE_BINARY_DIR}/${CONFIG}.export

From 0aabe9b3bbab9a768cfc7ed7f51de6935138d2d9 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Beat=20K=C3=BCng?= <beat-kueng@gmx.net>
Date: Tue, 10 May 2016 13:31:04 +0200
Subject: [PATCH 0012/1230] add sd_bench command, enable it on the px4fmu-v4 &
 posix_sitl build target

Useful to test maximum sequential write speed to the SD Card
---
 cmake/configs/nuttx_px4fmu-v4_default.cmake |   1 +
 cmake/configs/posix_sitl_default.cmake      |   1 +
 src/systemcmds/sd_bench/CMakeLists.txt      |  44 ++++
 src/systemcmds/sd_bench/sd_bench.c          | 215 ++++++++++++++++++++
 4 files changed, 261 insertions(+)
 create mode 100644 src/systemcmds/sd_bench/CMakeLists.txt
 create mode 100644 src/systemcmds/sd_bench/sd_bench.c

diff --git a/cmake/configs/nuttx_px4fmu-v4_default.cmake b/cmake/configs/nuttx_px4fmu-v4_default.cmake
index 0db31f82d6..f060be5050 100644
--- a/cmake/configs/nuttx_px4fmu-v4_default.cmake
+++ b/cmake/configs/nuttx_px4fmu-v4_default.cmake
@@ -63,6 +63,7 @@ set(config_module_list
 	systemcmds/mtd
 	systemcmds/dumpfile
 	systemcmds/ver
+	systemcmds/sd_bench
 	systemcmds/tests
 
 	#
diff --git a/cmake/configs/posix_sitl_default.cmake b/cmake/configs/posix_sitl_default.cmake
index 2e92a2f08d..11a211b9c0 100644
--- a/cmake/configs/posix_sitl_default.cmake
+++ b/cmake/configs/posix_sitl_default.cmake
@@ -24,6 +24,7 @@ set(config_module_list
 	systemcmds/ver
 	systemcmds/esc_calib
 	systemcmds/reboot
+	systemcmds/sd_bench
 	systemcmds/topic_listener
 	systemcmds/perf
 	modules/uORB
diff --git a/src/systemcmds/sd_bench/CMakeLists.txt b/src/systemcmds/sd_bench/CMakeLists.txt
new file mode 100644
index 0000000000..1063464ee3
--- /dev/null
+++ b/src/systemcmds/sd_bench/CMakeLists.txt
@@ -0,0 +1,44 @@
+############################################################################
+#
+#   Copyright (c) 2016 PX4 Development Team. All rights reserved.
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions
+# are met:
+#
+# 1. Redistributions of source code must retain the above copyright
+#    notice, this list of conditions and the following disclaimer.
+# 2. Redistributions in binary form must reproduce the above copyright
+#    notice, this list of conditions and the following disclaimer in
+#    the documentation and/or other materials provided with the
+#    distribution.
+# 3. Neither the name PX4 nor the names of its contributors may be
+#    used to endorse or promote products derived from this software
+#    without specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.
+#
+############################################################################
+px4_add_module(
+	MODULE systemcmds__sd_bench
+	MAIN sd_bench
+	STACK_MAIN 2500
+	COMPILE_FLAGS
+		-Os
+	SRCS
+		sd_bench.c
+	DEPENDS
+		platforms__common
+	)
+# vim: set noet ft=cmake fenc=utf-8 ff=unix : 
diff --git a/src/systemcmds/sd_bench/sd_bench.c b/src/systemcmds/sd_bench/sd_bench.c
new file mode 100644
index 0000000000..c8d3d82dea
--- /dev/null
+++ b/src/systemcmds/sd_bench/sd_bench.c
@@ -0,0 +1,215 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2016 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file sd_bench.c
+ *
+ * SD Card benchmarking
+ */
+
+#include <stdio.h>
+#include <stdlib.h>
+#include <fcntl.h>
+#include <unistd.h>
+
+#include <px4_config.h>
+#include <px4_includes.h>
+#include <px4_getopt.h>
+#include <px4_log.h>
+
+#include <drivers/drv_hrt.h>
+
+static void	usage(void);
+
+/** sequential write speed test */
+static void	write_test(int fd, uint8_t *block, int block_size);
+
+/**
+ * Measure the time for fsync.
+ * @param fd
+ * @return time in ms
+ */
+static inline unsigned int time_fsync(int fd);
+
+__EXPORT int	sd_bench_main(int argc, char *argv[]);
+
+static const char *BENCHMARK_FILE = PX4_ROOTFSDIR"/fs/microsd/benchmark.tmp";
+
+static int num_runs; ///< number of runs
+static int run_duration; ///< duration of a single run [ms]
+static bool synchronized; ///< call fsync after each block?
+
+static void
+usage()
+{
+	PX4_WARN(
+		"Test the speed of an SD Card. Usage:\n"
+		"sd_bench [-b <block_size>] [-r <runs>] [-d <duration>] [-s]\n"
+		"\n"
+		"\t-b <block_size>\t\tBlock size for each read/write (default=4096)\n"
+		"\t-r <runs>\t\tNumber of runs (default=5)\n"
+		"\t-d <duration>\t\tDuration of a run in ms (default=2000)\n"
+		"\t-s \t\t\tCall fsync after each block (default=at end of each run)\n"
+	);
+
+}
+
+int
+sd_bench_main(int argc, char *argv[])
+{
+	int block_size = 4096;
+	int myoptind = 1;
+	int ch;
+	const char *myoptarg = NULL;
+	synchronized = false;
+	num_runs = 5;
+	run_duration = 2000;
+
+	while ((ch = px4_getopt(argc, argv, "b:r:d:s", &myoptind, &myoptarg)) != EOF) {
+		switch (ch) {
+		case 'b':
+			block_size = strtol(myoptarg, NULL, 0);
+			break;
+
+		case 'r':
+			num_runs = strtol(myoptarg, NULL, 0);
+			break;
+
+		case 'd':
+			run_duration = strtol(myoptarg, NULL, 0);
+			break;
+
+		case 's':
+			synchronized = true;
+			break;
+
+		default:
+			usage();
+			return -1;
+			break;
+		}
+	}
+
+	int bench_fd = open(BENCHMARK_FILE, O_CREAT | O_WRONLY | O_TRUNC, PX4_O_MODE_666);
+
+	if (bench_fd < 0) {
+		PX4_ERR("Can't open benchmark file %s", BENCHMARK_FILE);
+		return -1;
+	}
+
+	if (block_size <= 0 || num_runs <= 0) {
+		PX4_ERR("invalid argument");
+		return -1;
+	}
+
+	//create some data block
+	uint8_t *block = (uint8_t *)malloc(block_size);
+
+	if (!block) {
+		PX4_ERR("Failed to allocate memory block");
+		return -1;
+	}
+
+	for (int i = 0; i < block_size; ++i) {
+		block[i] = (uint8_t)i;
+	}
+
+	PX4_INFO("Using block size = %i bytes, sync=%i", block_size, (int)synchronized);
+	write_test(bench_fd, block, block_size);
+
+	free(block);
+	close(bench_fd);
+	unlink(BENCHMARK_FILE);
+
+	return 0;
+}
+
+unsigned int time_fsync(int fd)
+{
+	hrt_abstime fsync_start = hrt_absolute_time();
+	fsync(fd);
+	return hrt_elapsed_time(&fsync_start) / 1000;
+}
+
+void write_test(int fd, uint8_t *block, int block_size)
+{
+	PX4_INFO("");
+	PX4_INFO("Testing Sequential Write Speed...");
+	double total_elapsed = 0.;
+	unsigned int total_blocks = 0;
+
+	for (int run = 0; run < num_runs; ++run) {
+		hrt_abstime start = hrt_absolute_time();
+		unsigned int num_blocks = 0;
+		unsigned int max_write_time = 0;
+		unsigned int fsync_time = 0;
+
+		while (hrt_elapsed_time(&start) < run_duration * 1000) {
+
+			hrt_abstime write_start = hrt_absolute_time();
+			size_t written = write(fd, block, block_size);
+			unsigned int write_time = hrt_elapsed_time(&write_start) / 1000;
+
+			if (write_time > max_write_time) {
+				max_write_time = write_time;
+			}
+
+			if ((int)written != block_size) {
+				PX4_ERR("Write error");
+				return;
+			}
+
+			if (synchronized) {
+				fsync_time += time_fsync(fd);
+			}
+
+			++num_blocks;
+		}
+
+		//Note: if testing a slow device (SD Card) and the OS buffers a lot (eg. Linux),
+		//fsync can take really long, and it looks like the process hangs. But it does
+		//not and the reported result will still be correct.
+		fsync_time += time_fsync(fd);
+
+		//report
+		double elapsed = hrt_elapsed_time(&start) / 1.e6;
+		PX4_INFO("  Run %2i: %8.2lf KB/s, max write time: %i ms (=%7.2lf KB/s), fsync: %i ms", run,
+			 (double)block_size * num_blocks / elapsed / 1024.,
+			 max_write_time, (double)block_size / max_write_time * 1000. / 1024., fsync_time);
+
+		total_elapsed += elapsed;
+		total_blocks += num_blocks;
+	}
+
+	PX4_INFO("  Avg   : %8.2lf KB/s", (double)block_size * total_blocks / total_elapsed / 1024.);
+}

From d3068b4337d05e2c4e7fa932ec794a5fbae271a4 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Beat=20K=C3=BCng?= <beat-kueng@gmx.net>
Date: Wed, 11 May 2016 12:52:05 +0200
Subject: [PATCH 0013/1230] px4_task_spawn_cmd: fix memory leak on posix

---
 src/platforms/posix/px4_layer/px4_posix_tasks.cpp | 15 +++++++++++----
 1 file changed, 11 insertions(+), 4 deletions(-)

diff --git a/src/platforms/posix/px4_layer/px4_posix_tasks.cpp b/src/platforms/posix/px4_layer/px4_posix_tasks.cpp
index 34761b0952..c3a3ff47ca 100644
--- a/src/platforms/posix/px4_layer/px4_posix_tasks.cpp
+++ b/src/platforms/posix/px4_layer/px4_posix_tasks.cpp
@@ -169,7 +169,7 @@ px4_task_t px4_task_spawn_cmd(const char *name, int scheduler, int priority, int
 	rv = pthread_attr_init(&attr);
 
 	if (rv != 0) {
-		PX4_WARN("px4_task_spawn_cmd: failed to init thread attrs");
+		PX4_ERR("px4_task_spawn_cmd: failed to init thread attrs");
 		free(taskdata);
 		return (rv < 0) ? rv : -rv;
 	}
@@ -184,6 +184,7 @@ px4_task_t px4_task_spawn_cmd(const char *name, int scheduler, int priority, int
 
 	if (rv != 0) {
 		PX4_ERR("pthread_attr_setstacksize to %d returned error (%d)", stack_size, rv);
+		pthread_attr_destroy(&attr);
 		free(taskdata);
 		return (rv < 0) ? rv : -rv;
 	}
@@ -193,7 +194,8 @@ px4_task_t px4_task_spawn_cmd(const char *name, int scheduler, int priority, int
 	rv = pthread_attr_setinheritsched(&attr, PTHREAD_EXPLICIT_SCHED);
 
 	if (rv != 0) {
-		PX4_WARN("px4_task_spawn_cmd: failed to set inherit sched");
+		PX4_ERR("px4_task_spawn_cmd: failed to set inherit sched");
+		pthread_attr_destroy(&attr);
 		free(taskdata);
 		return (rv < 0) ? rv : -rv;
 	}
@@ -201,7 +203,8 @@ px4_task_t px4_task_spawn_cmd(const char *name, int scheduler, int priority, int
 	rv = pthread_attr_setschedpolicy(&attr, scheduler);
 
 	if (rv != 0) {
-		PX4_WARN("px4_task_spawn_cmd: failed to set sched policy");
+		PX4_ERR("px4_task_spawn_cmd: failed to set sched policy");
+		pthread_attr_destroy(&attr);
 		free(taskdata);
 		return (rv < 0) ? rv : -rv;
 	}
@@ -211,7 +214,8 @@ px4_task_t px4_task_spawn_cmd(const char *name, int scheduler, int priority, int
 	rv = pthread_attr_setschedparam(&attr, &param);
 
 	if (rv != 0) {
-		PX4_WARN("px4_task_spawn_cmd: failed to set sched param");
+		PX4_ERR("px4_task_spawn_cmd: failed to set sched param");
+		pthread_attr_destroy(&attr);
 		free(taskdata);
 		return (rv < 0) ? rv : -rv;
 	}
@@ -240,18 +244,21 @@ px4_task_t px4_task_spawn_cmd(const char *name, int scheduler, int priority, int
 			if (rv != 0) {
 				PX4_ERR("px4_task_spawn_cmd: failed to create thread %d %d\n", rv, errno);
 				taskmap[taskid].isused = false;
+				pthread_attr_destroy(&attr);
 				pthread_mutex_unlock(&task_mutex);
 				free(taskdata);
 				return (rv < 0) ? rv : -rv;
 			}
 
 		} else {
+			pthread_attr_destroy(&attr);
 			pthread_mutex_unlock(&task_mutex);
 			free(taskdata);
 			return (rv < 0) ? rv : -rv;
 		}
 	}
 
+	pthread_attr_destroy(&attr);
 	pthread_mutex_unlock(&task_mutex);
 
 	if (i >= PX4_MAX_TASKS) {

From 77c61260df62784d795ee338250ad7980c8dfb4b Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Beat=20K=C3=BCng?= <beat-kueng@gmx.net>
Date: Wed, 11 May 2016 12:54:47 +0200
Subject: [PATCH 0014/1230] px4_task_spawn_cmd: handle case properly when
 running out of unused taskmap items

---
 src/platforms/posix/px4_layer/px4_posix_tasks.cpp | 11 +++++++----
 1 file changed, 7 insertions(+), 4 deletions(-)

diff --git a/src/platforms/posix/px4_layer/px4_posix_tasks.cpp b/src/platforms/posix/px4_layer/px4_posix_tasks.cpp
index c3a3ff47ca..15de261626 100644
--- a/src/platforms/posix/px4_layer/px4_posix_tasks.cpp
+++ b/src/platforms/posix/px4_layer/px4_posix_tasks.cpp
@@ -233,6 +233,13 @@ px4_task_t px4_task_spawn_cmd(const char *name, int scheduler, int priority, int
 		}
 	}
 
+	if (i >= PX4_MAX_TASKS) {
+		pthread_attr_destroy(&attr);
+		pthread_mutex_unlock(&task_mutex);
+		free(taskdata);
+		return -ENOSPC;
+	}
+
 	rv = pthread_create(&taskmap[taskid].pid, &attr, &entry_adapter, (void *) taskdata);
 
 	if (rv != 0) {
@@ -261,10 +268,6 @@ px4_task_t px4_task_spawn_cmd(const char *name, int scheduler, int priority, int
 	pthread_attr_destroy(&attr);
 	pthread_mutex_unlock(&task_mutex);
 
-	if (i >= PX4_MAX_TASKS) {
-		return -ENOSPC;
-	}
-
 	return i;
 }
 

From 42a9d5651f881b7f70009f85a9e1e30a20faa1b4 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Beat=20K=C3=BCng?= <beat-kueng@gmx.net>
Date: Wed, 11 May 2016 13:29:37 +0200
Subject: [PATCH 0015/1230] platform: some code cleanup for
 px4_{posix,qurt}_tasks.cpp

- __BEGIN_DECLS is for declarations not definitions
- getprogname() declaration is in err.h
---
 src/platforms/posix/px4_layer/px4_posix_tasks.cpp | 4 +---
 src/platforms/qurt/px4_layer/px4_qurt_tasks.cpp   | 5 +----
 2 files changed, 2 insertions(+), 7 deletions(-)

diff --git a/src/platforms/posix/px4_layer/px4_posix_tasks.cpp b/src/platforms/posix/px4_layer/px4_posix_tasks.cpp
index 15de261626..9b3730c7b9 100644
--- a/src/platforms/posix/px4_layer/px4_posix_tasks.cpp
+++ b/src/platforms/posix/px4_layer/px4_posix_tasks.cpp
@@ -58,6 +58,7 @@
 
 #include <px4_tasks.h>
 #include <px4_posix.h>
+#include <systemlib/err.h>
 
 #define MAX_CMD_LEN 100
 
@@ -381,14 +382,12 @@ bool px4_task_is_running(const char *taskname)
 
 	return false;
 }
-__BEGIN_DECLS
 
 unsigned long px4_getpid()
 {
 	return (unsigned long)pthread_self();
 }
 
-const char *getprogname();
 const char *getprogname()
 {
 	pthread_t pid = pthread_self();
@@ -427,4 +426,3 @@ int px4_prctl(int option, const char *arg2, unsigned pid)
 	return rv;
 }
 
-__END_DECLS
diff --git a/src/platforms/qurt/px4_layer/px4_qurt_tasks.cpp b/src/platforms/qurt/px4_layer/px4_qurt_tasks.cpp
index 1d53ed61c6..5f4ebc3ea3 100644
--- a/src/platforms/qurt/px4_layer/px4_qurt_tasks.cpp
+++ b/src/platforms/qurt/px4_layer/px4_qurt_tasks.cpp
@@ -64,6 +64,7 @@
 #include <string>
 
 #include <px4_tasks.h>
+#include <systemlib/err.h>
 
 #define MAX_CMD_LEN 100
 
@@ -325,8 +326,6 @@ void px4_show_tasks()
 
 }
 
-__BEGIN_DECLS
-
 unsigned long px4_getpid()
 {
 	pthread_t pid = pthread_self();
@@ -346,7 +345,6 @@ unsigned long px4_getpid()
 }
 
 
-const char *getprogname();
 const char *getprogname()
 {
 	pthread_t pid = pthread_self();
@@ -404,4 +402,3 @@ int px4_prctl(int option, const char *arg2, unsigned pid)
 
 	return rv;
 }
-__END_DECLS

From af0c42d3d761736f0b1ae0163ac94bf61c978b09 Mon Sep 17 00:00:00 2001
From: Andreas Antener <antener_a@gmx.ch>
Date: Sat, 7 May 2016 16:51:05 +0200
Subject: [PATCH 0016/1230] allow direct transition in standard vtol when
 landed

---
 src/modules/vtol_att_control/standard.cpp       |  2 +-
 .../vtol_att_control/vtol_att_control_main.cpp  | 17 +++++++++++++++++
 .../vtol_att_control/vtol_att_control_main.h    |  5 +++++
 src/modules/vtol_att_control/vtol_type.cpp      |  6 ++++++
 src/modules/vtol_att_control/vtol_type.h        |  6 ++++++
 5 files changed, 35 insertions(+), 1 deletion(-)

diff --git a/src/modules/vtol_att_control/standard.cpp b/src/modules/vtol_att_control/standard.cpp
index 3bf8080e63..ba6c0b7187 100644
--- a/src/modules/vtol_att_control/standard.cpp
+++ b/src/modules/vtol_att_control/standard.cpp
@@ -183,7 +183,7 @@ void Standard::update_vtol_state()
 			if ((_airspeed->indicated_airspeed_m_s >= _params_standard.airspeed_trans &&
 			     (float)hrt_elapsed_time(&_vtol_schedule.transition_start)
 			     > (_params_standard.front_trans_time_min * 1000000.0f)) ||
-			    !_armed->armed) {
+			    can_transition_on_ground()) {
 				_vtol_schedule.flight_mode = FW_MODE;
 				// we can turn off the multirotor motors now
 				_flag_enable_mc_motors = false;
diff --git a/src/modules/vtol_att_control/vtol_att_control_main.cpp b/src/modules/vtol_att_control/vtol_att_control_main.cpp
index b247df4a3f..8cc5014205 100644
--- a/src/modules/vtol_att_control/vtol_att_control_main.cpp
+++ b/src/modules/vtol_att_control/vtol_att_control_main.cpp
@@ -115,6 +115,7 @@ VtolAttitudeControl::VtolAttitudeControl() :
 	memset(&_vehicle_cmd, 0, sizeof(_vehicle_cmd));
 	memset(&_vehicle_status, 0, sizeof(_vehicle_status));
 	memset(&_tecs_status, 0, sizeof(_tecs_status));
+	memset(&_land_detected, 0, sizeof(_land_detected));
 
 	_params.idle_pwm_mc = PWM_DEFAULT_MIN;
 	_params.vtol_motor_count = 0;
@@ -444,6 +445,21 @@ VtolAttitudeControl::tecs_status_poll()
 	}
 }
 
+/**
+* Check for land detector updates.
+*/
+void
+VtolAttitudeControl::land_detected_poll()
+{
+	bool updated;
+
+	orb_check(_land_detected_sub, &updated);
+
+	if (updated) {
+		orb_copy(ORB_ID(vehicle_land_detected), _land_detected_sub , &_land_detected);
+	}
+}
+
 /**
 * Check received command
 */
@@ -692,6 +708,7 @@ void VtolAttitudeControl::task_main()
 		vehicle_cmd_poll();
 		vehicle_status_poll();
 		tecs_status_poll();
+		land_detected_poll();
 
 		// update the vtol state machine which decides which mode we are in
 		_vtol_type->update_vtol_state();
diff --git a/src/modules/vtol_att_control/vtol_att_control_main.h b/src/modules/vtol_att_control/vtol_att_control_main.h
index 5a5b5eed57..cf072af71d 100644
--- a/src/modules/vtol_att_control/vtol_att_control_main.h
+++ b/src/modules/vtol_att_control/vtol_att_control_main.h
@@ -85,6 +85,7 @@
 #include <uORB/topics/vehicle_command.h>
 #include <uORB/topics/vehicle_status.h>
 #include <uORB/topics/tecs_status.h>
+#include <uORB/topics/vehicle_land_detected.h>
 #include <systemlib/param/param.h>
 #include <systemlib/err.h>
 #include <systemlib/systemlib.h>
@@ -132,6 +133,7 @@ public:
 	struct battery_status_s 			*get_batt_status() {return &_batt_status;}
 	struct vehicle_status_s 			*get_vehicle_status() {return &_vehicle_status;}
 	struct tecs_status_s 				*get_tecs_status() {return &_tecs_status;}
+	struct vehicle_land_detected_s			*get_land_detected() {return &_land_detected;}
 
 	struct Params 					*get_params() {return &_params;}
 
@@ -159,6 +161,7 @@ private:
 	int 	_vehicle_cmd_sub;
 	int 	_vehicle_status_sub;
 	int	_tecs_status_sub;
+	int	_land_detected_sub;
 
 	int 	_actuator_inputs_mc;	//topic on which the mc_att_controller publishes actuator inputs
 	int 	_actuator_inputs_fw;	//topic on which the fw_att_controller publishes actuator inputs
@@ -191,6 +194,7 @@ private:
 	struct vehicle_command_s			_vehicle_cmd;
 	struct vehicle_status_s				_vehicle_status;
 	struct tecs_status_s				_tecs_status;
+	struct vehicle_land_detected_s			_land_detected;
 
 	Params _params;	// struct holding the parameters
 
@@ -241,6 +245,7 @@ private:
 	void 		vehicle_battery_poll();			// Check for battery updates
 	void		vehicle_cmd_poll();
 	void		tecs_status_poll();
+	void		land_detected_poll();
 	void 		parameters_update_poll();		//Check if parameters have changed
 	void 		vehicle_status_poll();
 	int 		parameters_update();			//Update local paraemter cache
diff --git a/src/modules/vtol_att_control/vtol_type.cpp b/src/modules/vtol_att_control/vtol_type.cpp
index 681c782815..8184d55d2e 100644
--- a/src/modules/vtol_att_control/vtol_type.cpp
+++ b/src/modules/vtol_att_control/vtol_type.cpp
@@ -68,6 +68,7 @@ VtolType::VtolType(VtolAttitudeControl *att_controller) :
 	_batt_status = _attc->get_batt_status();
 	_vehicle_status = _attc->get_vehicle_status();
 	_tecs_status = _attc->get_tecs_status();
+	_land_detected = _attc->get_land_detected();
 	_params = _attc->get_params();
 
 	flag_idle_mc = true;
@@ -168,3 +169,8 @@ void VtolType::update_fw_state()
 		waiting_on_tecs();
 	}
 }
+
+bool VtolType::can_transition_on_ground()
+{
+	return !_armed->armed || _land_detected->landed;
+}
diff --git a/src/modules/vtol_att_control/vtol_type.h b/src/modules/vtol_att_control/vtol_type.h
index ddfed44f57..9a15ff8cfa 100644
--- a/src/modules/vtol_att_control/vtol_type.h
+++ b/src/modules/vtol_att_control/vtol_type.h
@@ -120,6 +120,11 @@ public:
 	 */
 	virtual void waiting_on_tecs() {};
 
+	/**
+	 * Returns true if we're allowed to do a mode transition on the ground.
+	 */
+	bool can_transition_on_ground();
+
 	void set_idle_mc();
 	void set_idle_fw();
 
@@ -149,6 +154,7 @@ protected:
 	struct battery_status_s 			*_batt_status; 				// battery status
 	struct vehicle_status_s 			*_vehicle_status;			// vehicle status from commander app
 	struct tecs_status_s				*_tecs_status;
+	struct vehicle_land_detected_s			*_land_detected;
 
 	struct Params 					*_params;
 

From 05f1da8c4a6d761872cc829c8cb73ce024b4ddbd Mon Sep 17 00:00:00 2001
From: Andreas Antener <antener_a@gmx.ch>
Date: Wed, 11 May 2016 14:50:31 +0200
Subject: [PATCH 0017/1230] subscribe to land_detected topic

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

diff --git a/src/modules/vtol_att_control/vtol_att_control_main.cpp b/src/modules/vtol_att_control/vtol_att_control_main.cpp
index 8cc5014205..81f3b86f19 100644
--- a/src/modules/vtol_att_control/vtol_att_control_main.cpp
+++ b/src/modules/vtol_att_control/vtol_att_control_main.cpp
@@ -81,6 +81,7 @@ VtolAttitudeControl::VtolAttitudeControl() :
 	_vehicle_cmd_sub(-1),
 	_vehicle_status_sub(-1),
 	_tecs_status_sub(-1),
+	_land_detected_sub(-1),
 
 	//init publication handlers
 	_actuators_0_pub(nullptr),
@@ -630,6 +631,7 @@ void VtolAttitudeControl::task_main()
 	_vehicle_cmd_sub	   = orb_subscribe(ORB_ID(vehicle_command));
 	_vehicle_status_sub    = orb_subscribe(ORB_ID(vehicle_status));
 	_tecs_status_sub = orb_subscribe(ORB_ID(tecs_status));
+	_land_detected_sub = orb_subscribe(ORB_ID(vehicle_land_detected));
 
 	_actuator_inputs_mc    = orb_subscribe(ORB_ID(actuator_controls_virtual_mc));
 	_actuator_inputs_fw    = orb_subscribe(ORB_ID(actuator_controls_virtual_fw));

From 1a66155d8ad9f6dd287716b860b10ea6e74f9acc Mon Sep 17 00:00:00 2001
From: Andreas Antener <antener_a@gmx.ch>
Date: Wed, 11 May 2016 15:56:56 +0200
Subject: [PATCH 0018/1230] use new method for instant transition in tailsitter

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

diff --git a/src/modules/vtol_att_control/tailsitter.cpp b/src/modules/vtol_att_control/tailsitter.cpp
index b8be7ff8d2..f46ceb66e6 100644
--- a/src/modules/vtol_att_control/tailsitter.cpp
+++ b/src/modules/vtol_att_control/tailsitter.cpp
@@ -180,7 +180,7 @@ void Tailsitter::update_vtol_state()
 
 			// check if we have reached airspeed  and pitch angle to switch to TRANSITION P2 mode
 			if ((_airspeed->indicated_airspeed_m_s >= _params_tailsitter.airspeed_trans
-			     && _v_att->pitch <= PITCH_TRANSITION_FRONT_P1) || !_armed->armed) {
+			     && _v_att->pitch <= PITCH_TRANSITION_FRONT_P1) || can_transition_on_ground()) {
 				_vtol_schedule.flight_mode = FW_MODE;
 				//_vtol_schedule.transition_start = hrt_absolute_time();
 			}

From 6ebb2079e03fe44788824329c9bb2531bd28e738 Mon Sep 17 00:00:00 2001
From: Andreas Antener <antener_a@gmx.ch>
Date: Wed, 11 May 2016 15:58:23 +0200
Subject: [PATCH 0019/1230] use new method for instant transition in tiltrotor

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

diff --git a/src/modules/vtol_att_control/tiltrotor.cpp b/src/modules/vtol_att_control/tiltrotor.cpp
index 9adcb6a9ff..7cb5ea26e9 100644
--- a/src/modules/vtol_att_control/tiltrotor.cpp
+++ b/src/modules/vtol_att_control/tiltrotor.cpp
@@ -208,7 +208,7 @@ void Tiltrotor::update_vtol_state()
 
 			// check if we have reached airspeed to switch to fw mode
 			// also allow switch if we are not armed for the sake of bench testing
-			if (_airspeed->indicated_airspeed_m_s >= _params_tiltrotor.airspeed_trans || !_armed->armed) {
+			if (_airspeed->indicated_airspeed_m_s >= _params_tiltrotor.airspeed_trans || can_transition_on_ground()) {
 				_vtol_schedule.flight_mode = TRANSITION_FRONT_P2;
 				_vtol_schedule.transition_start = hrt_absolute_time();
 			}

From 72d296fe65f14f5c4b8347e5b802fd35dc988ee5 Mon Sep 17 00:00:00 2001
From: Daniel Agar <daniel@agar.ca>
Date: Fri, 6 May 2016 17:43:13 -0400
Subject: [PATCH 0020/1230] launchdetection_params.c param metadata

---
 .../launchdetection/launchdetection_params.c   | 18 ++++++++++++++----
 1 file changed, 14 insertions(+), 4 deletions(-)

diff --git a/src/lib/launchdetection/launchdetection_params.c b/src/lib/launchdetection/launchdetection_params.c
index 9af90279d2..4e80244855 100644
--- a/src/lib/launchdetection/launchdetection_params.c
+++ b/src/lib/launchdetection/launchdetection_params.c
@@ -61,6 +61,8 @@ PARAM_DEFINE_INT32(LAUN_ALL_ON, 0);
  *
  * @unit m/s/s
  * @min 0
+ * @decimal 1
+ * @increment 0.5
  * @group Launch detection
  */
 PARAM_DEFINE_FLOAT(LAUN_CAT_A, 30.0f);
@@ -71,7 +73,10 @@ PARAM_DEFINE_FLOAT(LAUN_CAT_A, 30.0f);
  * LAUN_CAT_A for LAUN_CAT_T serves as threshold to trigger launch detection.
  *
  * @unit s
- * @min 0
+ * @min 0.0
+ * @max 5.0
+ * @decimal 2
+ * @increment 0.05
  * @group Launch detection
  */
 PARAM_DEFINE_FLOAT(LAUN_CAT_T, 0.05f);
@@ -83,7 +88,10 @@ PARAM_DEFINE_FLOAT(LAUN_CAT_T, 0.05f);
  * Before this timespan is up the throttle will be set to FW_THR_IDLE, set to 0 to deactivate
  *
  * @unit s
- * @min 0
+ * @min 0.0
+ * @max 10.0
+ * @decimal 1
+ * @increment 0.5
  * @group Launch detection
  */
 PARAM_DEFINE_FLOAT(LAUN_CAT_MDEL, 0.0f);
@@ -95,8 +103,10 @@ PARAM_DEFINE_FLOAT(LAUN_CAT_MDEL, 0.0f);
  * This allows to limit the maximum pitch angle during a bungee launch (make the launch less steep).
  *
  * @unit deg
- * @min 0
- * @max 45
+ * @min 0.0
+ * @max 45.0
+ * @decimal 1
+ * @increment 0.5
  * @group Launch detection
  */
 PARAM_DEFINE_FLOAT(LAUN_CAT_PMAX, 30.0f);

From 91f1ac5c8877b3521fe397ce52063d36655a058a Mon Sep 17 00:00:00 2001
From: Daniel Agar <daniel@agar.ca>
Date: Fri, 6 May 2016 17:43:14 -0400
Subject: [PATCH 0021/1230] runway_takeoff_params.c param metadata

---
 src/lib/runway_takeoff/runway_takeoff_params.c | 16 ++++++++++++++--
 1 file changed, 14 insertions(+), 2 deletions(-)

diff --git a/src/lib/runway_takeoff/runway_takeoff_params.c b/src/lib/runway_takeoff/runway_takeoff_params.c
index aed95e99eb..c0e2d7db07 100644
--- a/src/lib/runway_takeoff/runway_takeoff_params.c
+++ b/src/lib/runway_takeoff/runway_takeoff_params.c
@@ -42,8 +42,6 @@
 /**
  * Enable or disable runway takeoff with landing gear
  *
- * 0: disabled, 1: enabled
- *
  * @boolean
  * @group Runway Takeoff
  */
@@ -71,6 +69,8 @@ PARAM_DEFINE_INT32(RWTO_HDG, 0);
  * @unit m
  * @min 0.0
  * @max 100.0
+ * @decimal 1
+ * @increment 1
  * @group Runway Takeoff
  */
 PARAM_DEFINE_FLOAT(RWTO_NAV_ALT, 5.0);
@@ -79,8 +79,11 @@ PARAM_DEFINE_FLOAT(RWTO_NAV_ALT, 5.0);
  * Max throttle during runway takeoff.
  * (Can be used to test taxi on runway)
  *
+ * @unit norm
  * @min 0.0
  * @max 1.0
+ * @decimal 2
+ * @increment 0.01
  * @group Runway Takeoff
  */
 PARAM_DEFINE_FLOAT(RWTO_MAX_THR, 1.0);
@@ -94,6 +97,8 @@ PARAM_DEFINE_FLOAT(RWTO_MAX_THR, 1.0);
  * @unit deg
  * @min 0.0
  * @max 20.0
+ * @decimal 1
+ * @increment 0.5
  * @group Runway Takeoff
  */
 PARAM_DEFINE_FLOAT(RWTO_PSP, 0.0);
@@ -106,6 +111,8 @@ PARAM_DEFINE_FLOAT(RWTO_PSP, 0.0);
  * @unit deg
  * @min 0.0
  * @max 60.0
+ * @decimal 1
+ * @increment 0.5
  * @group Runway Takeoff
  */
 PARAM_DEFINE_FLOAT(RWTO_MAX_PITCH, 20.0);
@@ -118,6 +125,8 @@ PARAM_DEFINE_FLOAT(RWTO_MAX_PITCH, 20.0);
  * @unit deg
  * @min 0.0
  * @max 60.0
+ * @decimal 1
+ * @increment 0.5
  * @group Runway Takeoff
  */
 PARAM_DEFINE_FLOAT(RWTO_MAX_ROLL, 25.0);
@@ -127,8 +136,11 @@ PARAM_DEFINE_FLOAT(RWTO_MAX_ROLL, 25.0);
  * Pitch up will be commanded when the following airspeed is reached:
  * FW_AIRSPD_MIN * RWTO_AIRSPD_SCL
  *
+ * @unit norm
  * @min 0.0
  * @max 2.0
+ * @decimal 2
+ * @increment 0.01
  * @group Runway Takeoff
  */
 PARAM_DEFINE_FLOAT(RWTO_AIRSPD_SCL, 1.3);

From a1a1a515dbf41c8c24a2c3ff07e9da431309fb28 Mon Sep 17 00:00:00 2001
From: Daniel Agar <daniel@agar.ca>
Date: Fri, 6 May 2016 17:43:15 -0400
Subject: [PATCH 0022/1230] commander_params.c param metadata

---
 src/modules/commander/commander_params.c | 7 ++++---
 1 file changed, 4 insertions(+), 3 deletions(-)

diff --git a/src/modules/commander/commander_params.c b/src/modules/commander/commander_params.c
index f6532adea3..00427f484d 100644
--- a/src/modules/commander/commander_params.c
+++ b/src/modules/commander/commander_params.c
@@ -127,10 +127,11 @@ PARAM_DEFINE_INT32(COM_DL_REG_T, 0);
  * Engine failure triggers only above this throttle value
  *
  * @group Commander
+ * @unit norm
  * @min 0.0
  * @max 1.0
- * @decimal 1
- * @increment 0.05
+ * @decimal 2
+ * @increment 0.01
  */
 PARAM_DEFINE_FLOAT(COM_EF_THROT, 0.5f);
 
@@ -142,7 +143,7 @@ PARAM_DEFINE_FLOAT(COM_EF_THROT, 0.5f);
  * @group Commander
  * @min 0.0
  * @max 50.0
- * @unit A
+ * @unit A/%
  * @decimal 2
  * @increment 1
  */

From b471d9406979cf76657582704f6ea42d9bd62f3b Mon Sep 17 00:00:00 2001
From: Daniel Agar <daniel@agar.ca>
Date: Fri, 6 May 2016 17:43:16 -0400
Subject: [PATCH 0023/1230] fw_att_control_params.c param metadata

---
 .../fw_att_control/fw_att_control_params.c    | 115 ++++++++++--------
 1 file changed, 66 insertions(+), 49 deletions(-)

diff --git a/src/modules/fw_att_control/fw_att_control_params.c b/src/modules/fw_att_control/fw_att_control_params.c
index 3d5ed72058..732c26a54d 100644
--- a/src/modules/fw_att_control/fw_att_control_params.c
+++ b/src/modules/fw_att_control/fw_att_control_params.c
@@ -57,6 +57,8 @@
  * @unit s
  * @min 0.4
  * @max 1.0
+ * @decimal 2
+ * @increment 0.05
  * @group FW Attitude Control
  */
 PARAM_DEFINE_FLOAT(FW_R_TC, 0.4f);
@@ -73,6 +75,8 @@ PARAM_DEFINE_FLOAT(FW_R_TC, 0.4f);
  * @unit s
  * @min 0.2
  * @max 1.0
+ * @decimal 2
+ * @increment 0.05
  * @group FW Attitude Control
  */
 PARAM_DEFINE_FLOAT(FW_P_TC, 0.4f);
@@ -86,6 +90,8 @@ PARAM_DEFINE_FLOAT(FW_P_TC, 0.4f);
  * @unit %/rad/s
  * @min 0.005
  * @max 1.0
+ * @decimal 3
+ * @increment 0.005
  * @group FW Attitude Control
  */
 PARAM_DEFINE_FLOAT(FW_PR_P, 0.08f);
@@ -99,6 +105,8 @@ PARAM_DEFINE_FLOAT(FW_PR_P, 0.08f);
  * @unit %/rad
  * @min 0.005
  * @max 0.5
+ * @decimal 3
+ * @increment 0.005
  * @group FW Attitude Control
  */
 PARAM_DEFINE_FLOAT(FW_PR_I, 0.02f);
@@ -112,6 +120,8 @@ PARAM_DEFINE_FLOAT(FW_PR_I, 0.02f);
  * @unit deg/s
  * @min 0.0
  * @max 90.0
+ * @decimal 1
+ * @increment 0.5
  * @group FW Attitude Control
  */
 PARAM_DEFINE_FLOAT(FW_P_RMAX_POS, 60.0f);
@@ -125,6 +135,8 @@ PARAM_DEFINE_FLOAT(FW_P_RMAX_POS, 60.0f);
  * @unit deg/s
  * @min 0.0
  * @max 90.0
+ * @decimal 1
+ * @increment 0.5
  * @group FW Attitude Control
  */
 PARAM_DEFINE_FLOAT(FW_P_RMAX_NEG, 60.0f);
@@ -135,9 +147,10 @@ PARAM_DEFINE_FLOAT(FW_P_RMAX_NEG, 60.0f);
  * The portion of the integrator part in the control surface deflection is
  * limited to this value
  *
- * @unit %
  * @min 0.0
  * @max 1.0
+ * @decimal 2
+ * @increment 0.05
  * @group FW Attitude Control
  */
 PARAM_DEFINE_FLOAT(FW_PR_IMAX, 0.4f);
@@ -151,6 +164,8 @@ PARAM_DEFINE_FLOAT(FW_PR_IMAX, 0.4f);
  * @unit %/rad/s
  * @min 0.005
  * @max 1.0
+ * @decimal 3
+ * @increment 0.005
  * @group FW Attitude Control
  */
 PARAM_DEFINE_FLOAT(FW_RR_P, 0.05f);
@@ -164,6 +179,8 @@ PARAM_DEFINE_FLOAT(FW_RR_P, 0.05f);
  * @unit %/rad
  * @min 0.005
  * @max 0.2
+ * @decimal 3
+ * @increment 0.005
  * @group FW Attitude Control
  */
 PARAM_DEFINE_FLOAT(FW_RR_I, 0.01f);
@@ -173,9 +190,10 @@ PARAM_DEFINE_FLOAT(FW_RR_I, 0.01f);
  *
  * The portion of the integrator part in the control surface deflection is limited to this value.
  *
- * @unit %
  * @min 0.0
  * @max 1.0
+ * @decimal 2
+ * @increment 0.05
  * @group FW Attitude Control
  */
 PARAM_DEFINE_FLOAT(FW_RR_IMAX, 0.2f);
@@ -189,6 +207,8 @@ PARAM_DEFINE_FLOAT(FW_RR_IMAX, 0.2f);
  * @unit deg/s
  * @min 0.0
  * @max 90.0
+ * @decimal 1
+ * @increment 0.5
  * @group FW Attitude Control
  */
 PARAM_DEFINE_FLOAT(FW_R_RMAX, 70.0f);
@@ -202,6 +222,8 @@ PARAM_DEFINE_FLOAT(FW_R_RMAX, 70.0f);
  * @unit %/rad/s
  * @min 0.005
  * @max 1.0
+ * @decimal 3
+ * @increment 0.005
  * @group FW Attitude Control
  */
 PARAM_DEFINE_FLOAT(FW_YR_P, 0.05f);
@@ -215,6 +237,8 @@ PARAM_DEFINE_FLOAT(FW_YR_P, 0.05f);
  * @unit %/rad
  * @min 0.0
  * @max 50.0
+ * @decimal 1
+ * @increment 0.5
  * @group FW Attitude Control
  */
 PARAM_DEFINE_FLOAT(FW_YR_I, 0.0f);
@@ -225,9 +249,10 @@ PARAM_DEFINE_FLOAT(FW_YR_I, 0.0f);
  * The portion of the integrator part in the control surface deflection is
  * limited to this value
  *
- * @unit %
  * @min 0.0
  * @max 1.0
+ * @decimal 2
+ * @increment 0.05
  * @group FW Attitude Control
  */
 PARAM_DEFINE_FLOAT(FW_YR_IMAX, 0.2f);
@@ -241,6 +266,8 @@ PARAM_DEFINE_FLOAT(FW_YR_IMAX, 0.2f);
  * @unit deg/s
  * @min 0.0
  * @max 90.0
+ * @decimal 1
+ * @increment 0.5
  * @group FW Attitude Control
  */
 PARAM_DEFINE_FLOAT(FW_Y_RMAX, 0.0f);
@@ -254,6 +281,8 @@ PARAM_DEFINE_FLOAT(FW_Y_RMAX, 0.0f);
  * @unit %/rad/s
  * @min 0.005
  * @max 1.0
+ * @decimal 3
+ * @increment 0.005
  * @group FW Attitude Control
  */
 PARAM_DEFINE_FLOAT(FW_WR_P, 0.5f);
@@ -265,8 +294,10 @@ PARAM_DEFINE_FLOAT(FW_WR_P, 0.5f);
  * state error. It trims any constant error.
  *
  * @unit %/rad
- * @min 0.0
- * @max 50.0
+ * @min 0.005
+ * @max 0.5
+ * @decimal 3
+ * @increment 0.005
  * @group FW Attitude Control
  */
 PARAM_DEFINE_FLOAT(FW_WR_I, 0.1f);
@@ -277,9 +308,10 @@ PARAM_DEFINE_FLOAT(FW_WR_I, 0.1f);
  * The portion of the integrator part in the control surface deflection is
  * limited to this value
  *
- * @unit %
  * @min 0.0
  * @max 1.0
+ * @decimal 2
+ * @increment 0.05
  * @group FW Attitude Control
  */
 PARAM_DEFINE_FLOAT(FW_WR_IMAX, 1.0f);
@@ -293,6 +325,8 @@ PARAM_DEFINE_FLOAT(FW_WR_IMAX, 1.0f);
  * @unit deg/s
  * @min 0.0
  * @max 90.0
+ * @decimal 1
+ * @increment 0.5
  * @group FW Attitude Control
  */
 PARAM_DEFINE_FLOAT(FW_W_RMAX, 0.0f);
@@ -307,6 +341,8 @@ PARAM_DEFINE_FLOAT(FW_W_RMAX, 0.0f);
  * @unit %/rad/s
  * @min 0.0
  * @max 10.0
+ * @decimal 2
+ * @increment 0.05
  * @group FW Attitude Control
  */
 PARAM_DEFINE_FLOAT(FW_RR_FF, 0.5f);
@@ -319,6 +355,8 @@ PARAM_DEFINE_FLOAT(FW_RR_FF, 0.5f);
  * @unit %/rad/s
  * @min 0.0
  * @max 10.0
+ * @decimal 2
+ * @increment 0.05
  * @group FW Attitude Control
  */
 PARAM_DEFINE_FLOAT(FW_PR_FF, 0.5f);
@@ -331,6 +369,8 @@ PARAM_DEFINE_FLOAT(FW_PR_FF, 0.5f);
  * @unit %/rad/s
  * @min 0.0
  * @max 10.0
+ * @decimal 2
+ * @increment 0.05
  * @group FW Attitude Control
  */
 PARAM_DEFINE_FLOAT(FW_YR_FF, 0.3f);
@@ -343,6 +383,8 @@ PARAM_DEFINE_FLOAT(FW_YR_FF, 0.3f);
  * @unit %/rad/s
  * @min 0.0
  * @max 10.0
+ * @decimal 2
+ * @increment 0.05
  * @group FW Attitude Control
  */
 PARAM_DEFINE_FLOAT(FW_WR_FF, 0.2f);
@@ -354,6 +396,10 @@ PARAM_DEFINE_FLOAT(FW_WR_FF, 0.2f);
  * turn. Set to a very high value to disable.
  *
  * @unit m/s
+ * @min 0.0
+ * @max 1000.0
+ * @decimal 1
+ * @increment 0.5
  * @group FW Attitude Control
  */
 PARAM_DEFINE_FLOAT(FW_YCO_VMIN, 1000.0f);
@@ -373,49 +419,6 @@ PARAM_DEFINE_FLOAT(FW_YCO_VMIN, 1000.0f);
  */
 PARAM_DEFINE_INT32(FW_YCO_METHOD, 0);
 
-/* Airspeed parameters:
- * The following parameters about airspeed are used by the attitude and the
- * position controller.
- * */
-
-/**
- * Minimum Airspeed
- *
- * If the airspeed falls below this value, the TECS controller will try to
- * increase airspeed more aggressively.
- *
- * @unit m/s
- * @min 0.0
- * @max 40
- * @group FW Attitude Control
- */
-PARAM_DEFINE_FLOAT(FW_AIRSPD_MIN, 10.0f);
-
-/**
- * Trim Airspeed
- *
- * The TECS controller tries to fly at this airspeed.
- *
- * @unit m/s
- * @min 0.0
- * @max 40
- * @group FW Attitude Control
- */
-PARAM_DEFINE_FLOAT(FW_AIRSPD_TRIM, 15.0f);
-
-/**
- * Maximum Airspeed
- *
- * If the airspeed is above this value, the TECS controller will try to decrease
- * airspeed more aggressively.
- *
- * @unit m/s
- * @min 0.0
- * @max 40
- * @group FW Attitude Control
- */
-PARAM_DEFINE_FLOAT(FW_AIRSPD_MAX, 20.0f);
-
 /**
  * Roll Setpoint Offset
  *
@@ -426,6 +429,8 @@ PARAM_DEFINE_FLOAT(FW_AIRSPD_MAX, 20.0f);
  * @unit deg
  * @min -90.0
  * @max 90.0
+ * @decimal 1
+ * @increment 0.5
  * @group FW Attitude Control
  */
 PARAM_DEFINE_FLOAT(FW_RSP_OFF, 0.0f);
@@ -440,6 +445,8 @@ PARAM_DEFINE_FLOAT(FW_RSP_OFF, 0.0f);
  * @unit deg
  * @min -90.0
  * @max 90.0
+ * @decimal 1
+ * @increment 0.5
  * @group FW Attitude Control
  */
 PARAM_DEFINE_FLOAT(FW_PSP_OFF, 0.0f);
@@ -452,6 +459,8 @@ PARAM_DEFINE_FLOAT(FW_PSP_OFF, 0.0f);
  * @unit deg
  * @min 0.0
  * @max 90.0
+ * @decimal 1
+ * @increment 0.5
  * @group FW Attitude Control
  */
 PARAM_DEFINE_FLOAT(FW_MAN_R_MAX, 45.0f);
@@ -464,6 +473,8 @@ PARAM_DEFINE_FLOAT(FW_MAN_R_MAX, 45.0f);
  * @unit deg
  * @min 0.0
  * @max 90.0
+ * @decimal 1
+ * @increment 0.5
  * @group FW Attitude Control
  */
 PARAM_DEFINE_FLOAT(FW_MAN_P_MAX, 45.0f);
@@ -471,8 +482,11 @@ PARAM_DEFINE_FLOAT(FW_MAN_P_MAX, 45.0f);
 /**
  * Scale factor for flaps
  *
+ * @unit norm
  * @min 0.0
  * @max 1.0
+ * @decimal 2
+ * @increment 0.01
  * @group FW Attitude Control
  */
 PARAM_DEFINE_FLOAT(FW_FLAPS_SCL, 1.0f);
@@ -480,8 +494,11 @@ PARAM_DEFINE_FLOAT(FW_FLAPS_SCL, 1.0f);
 /**
  * Scale factor for flaperons
  *
+ * @unit norm
  * @min 0.0
  * @max 1.0
+ * @decimal 2
+ * @increment 0.01
  * @group FW Attitude Control
  */
 PARAM_DEFINE_FLOAT(FW_FLAPERON_SCL, 0.0f);

From fc636b61b0b79cb8235541266f333c00c0c3189e Mon Sep 17 00:00:00 2001
From: Daniel Agar <daniel@agar.ca>
Date: Fri, 6 May 2016 17:43:17 -0400
Subject: [PATCH 0024/1230] fw_pos_control_l1_params.c param metadata

---
 .../fw_pos_control_l1_params.c                | 358 +++++++++++++-----
 1 file changed, 254 insertions(+), 104 deletions(-)

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 322205b138..271d05f394 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
@@ -54,6 +54,8 @@
  * @unit m
  * @min 12.0
  * @max 50.0
+ * @decimal 1
+ * @increment 0.5
  * @group FW L1 Control
  */
 PARAM_DEFINE_FLOAT(FW_L1_PERIOD, 20.0f);
@@ -65,6 +67,8 @@ PARAM_DEFINE_FLOAT(FW_L1_PERIOD, 20.0f);
  *
  * @min 0.6
  * @max 0.9
+ * @decimal 2
+ * @increment 0.05
  * @group FW L1 Control
  */
 PARAM_DEFINE_FLOAT(FW_L1_DAMPING, 0.75f);
@@ -77,6 +81,8 @@ PARAM_DEFINE_FLOAT(FW_L1_DAMPING, 0.75f);
  * @unit norm
  * @min 0.0
  * @max 1.0
+ * @decimal 2
+ * @increment 0.01
  * @group FW L1 Control
  */
 PARAM_DEFINE_FLOAT(FW_THR_CRUISE, 0.6f);
@@ -100,6 +106,8 @@ PARAM_DEFINE_FLOAT(FW_THR_SLEW_MAX, 0.0f);
  * @unit deg
  * @min -60.0
  * @max 0.0
+ * @decimal 1
+ * @increment 0.5
  * @group FW L1 Control
  */
 PARAM_DEFINE_FLOAT(FW_P_LIM_MIN, -45.0f);
@@ -112,6 +120,8 @@ PARAM_DEFINE_FLOAT(FW_P_LIM_MIN, -45.0f);
  * @unit deg
  * @min 0.0
  * @max 60.0
+ * @decimal 1
+ * @increment 0.5
  * @group FW L1 Control
  */
 PARAM_DEFINE_FLOAT(FW_P_LIM_MAX, 45.0f);
@@ -124,6 +134,8 @@ PARAM_DEFINE_FLOAT(FW_P_LIM_MAX, 45.0f);
  * @unit deg
  * @min 35.0
  * @max 65.0
+ * @decimal 1
+ * @increment 0.5
  * @group FW L1 Control
  */
 PARAM_DEFINE_FLOAT(FW_R_LIM, 50.0f);
@@ -138,6 +150,8 @@ PARAM_DEFINE_FLOAT(FW_R_LIM, 50.0f);
  * @unit norm
  * @min 0.0
  * @max 1.0
+ * @decimal 2
+ * @increment 0.01
  * @group FW L1 Control
  */
 PARAM_DEFINE_FLOAT(FW_THR_MAX, 1.0f);
@@ -157,6 +171,8 @@ PARAM_DEFINE_FLOAT(FW_THR_MAX, 1.0f);
  * @unit norm
  * @min 0.0
  * @max 1.0
+ * @decimal 2
+ * @increment 0.01
  * @group FW L1 Control
  */
 PARAM_DEFINE_FLOAT(FW_THR_MIN, 0.0f);
@@ -172,6 +188,8 @@ PARAM_DEFINE_FLOAT(FW_THR_MIN, 0.0f);
  * @unit norm
  * @min 0.0
  * @max 0.4
+ * @decimal 2
+ * @increment 0.01
  * @group FW L1 Control
  */
 PARAM_DEFINE_FLOAT(FW_THR_IDLE, 0.15f);
@@ -185,6 +203,8 @@ PARAM_DEFINE_FLOAT(FW_THR_IDLE, 0.15f);
  * @unit norm
  * @min 0.0
  * @max 1.0
+ * @decimal 2
+ * @increment 0.01
  * @group FW L1 Control
  */
 PARAM_DEFINE_FLOAT(FW_THR_LND_MAX, 1.0f);
@@ -200,10 +220,181 @@ PARAM_DEFINE_FLOAT(FW_THR_LND_MAX, 1.0f);
  * @unit m
  * @min 0.0
  * @max 150.0
+ * @decimal 1
+ * @increment 0.5
  * @group FW L1 Control
  */
 PARAM_DEFINE_FLOAT(FW_CLMBOUT_DIFF, 10.0f);
 
+/**
+ * Landing slope angle
+ *
+ * @unit deg
+ * @min 1.0
+ * @max 15.0
+ * @decimal 1
+ * @increment 0.5
+ * @group FW L1 Control
+ */
+PARAM_DEFINE_FLOAT(FW_LND_ANG, 5.0f);
+
+/**
+ *
+ *
+ * @unit m
+ * @min 1.0
+ * @max 15.0
+ * @decimal 1
+ * @increment 0.5
+ * @group FW L1 Control
+ */
+PARAM_DEFINE_FLOAT(FW_LND_HVIRT, 10.0f);
+
+/**
+ * Landing flare altitude (relative to landing altitude)
+ *
+ * @unit m
+ * @min 0.0
+ * @max 25.0
+ * @decimal 1
+ * @increment 0.5
+ * @group FW L1 Control
+ */
+PARAM_DEFINE_FLOAT(FW_LND_FLALT, 8.0f);
+
+/**
+ * Landing throttle limit altitude (relative landing altitude)
+ *
+ * Default of -1.0 lets the system default to applying throttle
+ * limiting at 2/3 of the flare altitude.
+ *
+ * @unit m
+ * @min -1.0
+ * @max 30.0
+ * @decimal 1
+ * @increment 0.5
+ * @group FW L1 Control
+ */
+PARAM_DEFINE_FLOAT(FW_LND_TLALT, -1.0f);
+
+/**
+ * Landing heading hold horizontal distance
+ *
+ * @unit m
+ * @min 0
+ * @max 30.0
+ * @decimal 1
+ * @increment 0.5
+ * @group FW L1 Control
+ */
+PARAM_DEFINE_FLOAT(FW_LND_HHDIST, 15.0f);
+
+/**
+ * Enable or disable usage of terrain estimate during landing
+ *
+ * @boolean
+ * @group FW L1 Control
+ */
+PARAM_DEFINE_INT32(FW_LND_USETER, 0);
+
+/**
+ * Flare, minimum pitch
+ *
+ * Minimum pitch during flare, a positive sign means nose up
+ * Applied once FW_LND_TLALT is reached
+ *
+ * @unit deg
+ * @min 0
+ * @max 15.0
+ * @decimal 1
+ * @increment 0.5
+ * @group FW L1 Control
+ */
+PARAM_DEFINE_FLOAT(FW_LND_FL_PMIN, 2.5f);
+
+/**
+ * Flare, maximum pitch
+ *
+ * Maximum pitch during flare, a positive sign means nose up
+ * Applied once FW_LND_TLALT is reached
+ *
+ * @unit deg
+ * @min 0
+ * @max 45.0
+ * @decimal 1
+ * @increment 0.5
+ * @group FW L1 Control
+ */
+PARAM_DEFINE_FLOAT(FW_LND_FL_PMAX, 15.0f);
+
+/**
+ * Min. airspeed scaling factor for landing
+ *
+ * Multiplying this factor with the minimum airspeed of the plane
+ * gives the target airspeed the landing approach.
+ * FW_AIRSPD_MIN * FW_LND_AIRSPD_SC
+ *
+ * @unit norm
+ * @min 1.0
+ * @max 1.5
+ * @decimal 2
+ * @increment 0.01
+ * @group FW L1 Control
+ */
+PARAM_DEFINE_FLOAT(FW_LND_AIRSPD_SC, 1.3f);
+
+
+
+/*
+ * TECS parameters
+ *
+ */
+
+
+/**
+ * Minimum Airspeed
+ *
+ * If the airspeed falls below this value, the TECS controller will try to
+ * increase airspeed more aggressively.
+ *
+ * @unit m/s
+ * @min 0.0
+ * @max 40
+ * @decimal 1
+ * @increment 0.5
+ * @group FW TECS
+ */
+PARAM_DEFINE_FLOAT(FW_AIRSPD_MIN, 10.0f);
+
+/**
+ * Trim Airspeed
+ *
+ * The TECS controller tries to fly at this airspeed.
+ *
+ * @unit m/s
+ * @min 0.0
+ * @max 40
+ * @decimal 1
+ * @increment 0.5
+ * @group FW TECS
+ */
+PARAM_DEFINE_FLOAT(FW_AIRSPD_TRIM, 15.0f);
+
+/**
+ * Maximum Airspeed
+ *
+ * If the airspeed is above this value, the TECS controller will try to decrease
+ * airspeed more aggressively.
+ *
+ * @unit m/s
+ * @min 0.0
+ * @max 40
+ * @decimal 1
+ * @increment 0.5
+ * @group FW TECS
+ */
+PARAM_DEFINE_FLOAT(FW_AIRSPD_MAX, 20.0f);
+
 /**
  * Maximum climb rate
  *
@@ -221,8 +412,10 @@ PARAM_DEFINE_FLOAT(FW_CLMBOUT_DIFF, 10.0f);
  * FW_THR_MAX reduced.
  *
  * @unit m/s
- * @min 2.0
- * @max 10.0
+ * @min 1.0
+ * @max 15.0
+ * @decimal 1
+ * @increment 0.5
  * @group FW TECS
  */
 PARAM_DEFINE_FLOAT(FW_T_CLMB_MAX, 5.0f);
@@ -235,6 +428,10 @@ PARAM_DEFINE_FLOAT(FW_T_CLMB_MAX, 5.0f);
  * to measure FW_T_CLMB_MAX.
  *
  * @unit m/s
+ * @min 1.0
+ * @max 5.0
+ * @decimal 1
+ * @increment 0.5
  * @group FW TECS
  */
 PARAM_DEFINE_FLOAT(FW_T_SINK_MIN, 2.0f);
@@ -249,6 +446,10 @@ PARAM_DEFINE_FLOAT(FW_T_SINK_MIN, 2.0f);
  * the aircraft.
  *
  * @unit m/s
+ * @min 2.0
+ * @max 15.0
+ * @decimal 1
+ * @increment 0.5
  * @group FW TECS
  */
 PARAM_DEFINE_FLOAT(FW_T_SINK_MAX, 5.0f);
@@ -261,6 +462,10 @@ PARAM_DEFINE_FLOAT(FW_T_SINK_MAX, 5.0f);
  * to respond.
  *
  * @unit s
+ * @min 1.0
+ * @max 10.0
+ * @decimal 1
+ * @increment 0.5
  * @group FW TECS
  */
 PARAM_DEFINE_FLOAT(FW_T_TIME_CONST, 5.0f);
@@ -273,6 +478,10 @@ PARAM_DEFINE_FLOAT(FW_T_TIME_CONST, 5.0f);
  * to respond.
  *
  * @unit s
+ * @min 1.0
+ * @max 10.0
+ * @decimal 1
+ * @increment 0.5
  * @group FW TECS
  */
 PARAM_DEFINE_FLOAT(FW_T_THRO_CONST, 8.0f);
@@ -283,6 +492,10 @@ PARAM_DEFINE_FLOAT(FW_T_THRO_CONST, 8.0f);
  * This is the damping gain for the throttle demand loop.
  * Increase to add damping to correct for oscillations in speed and height.
  *
+ * @min 0.0
+ * @max 2.0
+ * @decimal 1
+ * @increment 0.1
  * @group FW TECS
  */
 PARAM_DEFINE_FLOAT(FW_T_THR_DAMP, 0.5f);
@@ -295,6 +508,10 @@ PARAM_DEFINE_FLOAT(FW_T_THR_DAMP, 0.5f);
  * and height offsets are trimmed out, but reduces damping and
  * increases overshoot.
  *
+ * @min 0.0
+ * @max 2.0
+ * @decimal 2
+ * @increment 0.05
  * @group FW TECS
  */
 PARAM_DEFINE_FLOAT(FW_T_INTEG_GAIN, 0.1f);
@@ -309,6 +526,10 @@ PARAM_DEFINE_FLOAT(FW_T_INTEG_GAIN, 0.1f);
  * from under-speed conditions.
  *
  * @unit m/s/s
+ * @min 1.0
+ * @max 10.0
+ * @decimal 1
+ * @increment 0.5
  * @group FW TECS
  */
 PARAM_DEFINE_FLOAT(FW_T_VERT_ACC, 7.0f);
@@ -323,6 +544,10 @@ PARAM_DEFINE_FLOAT(FW_T_VERT_ACC, 7.0f);
  * the solution more towards use of the accelerometer data.
  *
  * @unit rad/s
+ * @min 1.0
+ * @max 10.0
+ * @decimal 1
+ * @increment 0.5
  * @group FW TECS
  */
 PARAM_DEFINE_FLOAT(FW_T_HGT_OMEGA, 3.0f);
@@ -333,10 +558,14 @@ PARAM_DEFINE_FLOAT(FW_T_HGT_OMEGA, 3.0f);
  * This is the cross-over frequency (in radians/second) of the complementary
  * filter used to fuse longitudinal acceleration and airspeed to obtain an
  * improved airspeed estimate. Increasing this frequency weights the solution
- * more towards use of the arispeed sensor, whilst reducing it weights the
+ * more towards use of the airspeed sensor, whilst reducing it weights the
  * solution more towards use of the accelerometer data.
  *
  * @unit rad/s
+ * @min 1.0
+ * @max 10.0
+ * @decimal 1
+ * @increment 0.5
  * @group FW TECS
  */
 PARAM_DEFINE_FLOAT(FW_T_SPD_OMEGA, 2.0f);
@@ -353,6 +582,10 @@ 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.
  *
+ * @min 0.0
+ * @max 20.0
+ * @decimal 1
+ * @increment 0.5
  * @group FW TECS
  */
 PARAM_DEFINE_FLOAT(FW_T_RLL2THR, 15.0f);
@@ -373,6 +606,8 @@ PARAM_DEFINE_FLOAT(FW_T_RLL2THR, 15.0f);
  *
  * @min 0.0
  * @max 2.0
+ * @decimal 1
+ * @increment 1.0
  * @group FW TECS
  */
 PARAM_DEFINE_FLOAT(FW_T_SPDWEIGHT, 1.0f);
@@ -385,6 +620,10 @@ PARAM_DEFINE_FLOAT(FW_T_SPDWEIGHT, 1.0f);
  * will work well provided the pitch to servo controller has been tuned
  * properly.
  *
+ * @min 0.0
+ * @max 2.0
+ * @decimal 1
+ * @increment 0.1
  * @group FW TECS
  */
 PARAM_DEFINE_FLOAT(FW_T_PTCH_DAMP, 0.0f);
@@ -392,6 +631,10 @@ PARAM_DEFINE_FLOAT(FW_T_PTCH_DAMP, 0.0f);
 /**
  * Height rate P factor
  *
+ * @min 0.0
+ * @max 2.0
+ * @decimal 2
+ * @increment 0.05
  * @group FW TECS
  */
 PARAM_DEFINE_FLOAT(FW_T_HRATE_P, 0.05f);
@@ -399,6 +642,10 @@ PARAM_DEFINE_FLOAT(FW_T_HRATE_P, 0.05f);
 /**
  * Height rate FF factor
  *
+ * @min 0.0
+ * @max 2.0
+ * @decimal 2
+ * @increment 0.05
  * @group FW TECS
  */
 PARAM_DEFINE_FLOAT(FW_T_HRATE_FF, 0.0f);
@@ -406,107 +653,10 @@ PARAM_DEFINE_FLOAT(FW_T_HRATE_FF, 0.0f);
 /**
  * Speed rate P factor
  *
+ * @min 0.0
+ * @max 2.0
+ * @decimal 2
+ * @increment 0.01
  * @group FW TECS
  */
 PARAM_DEFINE_FLOAT(FW_T_SRATE_P, 0.02f);
-
-/**
- * Landing slope angle
- *
- * @unit deg
- * @min 1.0
- * @max 15.0
- * @group FW L1 Control
- */
-PARAM_DEFINE_FLOAT(FW_LND_ANG, 5.0f);
-
-/**
- *
- *
- * @unit m
- * @group FW L1 Control
- */
-PARAM_DEFINE_FLOAT(FW_LND_HVIRT, 10.0f);
-
-/**
- * Landing flare altitude (relative to landing altitude)
- *
- * @unit m
- * @min 0.0
- * @max 25.0
- * @group FW L1 Control
- */
-PARAM_DEFINE_FLOAT(FW_LND_FLALT, 8.0f);
-
-/**
- * Landing throttle limit altitude (relative landing altitude)
- *
- * Default of -1.0 lets the system default to applying throttle
- * limiting at 2/3 of the flare altitude.
- *
- * @unit m
- * @min -1.0
- * @max 30.0
- * @group FW L1 Control
- */
-PARAM_DEFINE_FLOAT(FW_LND_TLALT, -1.0f);
-
-/**
- * Landing heading hold horizontal distance
- *
- * @unit m
- * @min 0
- * @max 30.0
- * @group FW L1 Control
- */
-PARAM_DEFINE_FLOAT(FW_LND_HHDIST, 15.0f);
-
-/**
- * Enable or disable usage of terrain estimate during landing
- *
- * 0: disabled, 1: enabled
- *
- * @boolean
- * @group FW L1 Control
- */
-PARAM_DEFINE_INT32(FW_LND_USETER, 0);
-
-/**
- * Flare, minimum pitch
- *
- * Minimum pitch during flare, a positive sign means nose up
- * Applied once FW_LND_TLALT is reached
- *
- * @unit deg
- * @min 0
- * @max 15.0
- * @group FW L1 Control
- *
- */
-PARAM_DEFINE_FLOAT(FW_LND_FL_PMIN, 2.5f);
-
-/**
- * Flare, maximum pitch
- *
- * Maximum pitch during flare, a positive sign means nose up
- * Applied once FW_LND_TLALT is reached
- *
- * @unit deg
- * @min 0
- * @max 45.0
- * @group FW L1 Control
- *
- */
-PARAM_DEFINE_FLOAT(FW_LND_FL_PMAX, 15.0f);
-
-/**
- * Landing airspeed scale factor
- *
- * Multiplying this factor with the minimum airspeed of the plane
- * gives the target airspeed the landing approach.
- *
- * @min 1.0
- * @max 1.5
- * @group FW L1 Control
- */
-PARAM_DEFINE_FLOAT(FW_LND_AIRSPD_SC, 1.3f);

From 331456f8317f3e02f367336f2c5be7cf26ff846e Mon Sep 17 00:00:00 2001
From: Daniel Agar <daniel@agar.ca>
Date: Fri, 6 May 2016 17:43:18 -0400
Subject: [PATCH 0025/1230] mc_pos_control_params.c param metadata

---
 src/modules/mc_pos_control/mc_pos_control_params.c | 3 +++
 1 file changed, 3 insertions(+)

diff --git a/src/modules/mc_pos_control/mc_pos_control_params.c b/src/modules/mc_pos_control/mc_pos_control_params.c
index 88ad616431..4429eea405 100644
--- a/src/modules/mc_pos_control/mc_pos_control_params.c
+++ b/src/modules/mc_pos_control/mc_pos_control_params.c
@@ -47,6 +47,7 @@
  * @min 0.05
  * @max 1.0
  * @decimal 2
+ * @increment 0.01
  * @group Multicopter Position Control
  */
 PARAM_DEFINE_FLOAT(MPC_THR_MIN, 0.12f);
@@ -65,6 +66,7 @@ PARAM_DEFINE_FLOAT(MPC_THR_MIN, 0.12f);
  * @min 0.2
  * @max 0.8
  * @decimal 2
+ * @increment 0.01
  * @group Multicopter Position Control
  */
 PARAM_DEFINE_FLOAT(MPC_THR_HOVER, 0.5f);
@@ -80,6 +82,7 @@ PARAM_DEFINE_FLOAT(MPC_THR_HOVER, 0.5f);
  * @min 0.0
  * @max 0.2
  * @decimal 2
+ * @increment 0.05
  * @group Multicopter Position Control
  */
 PARAM_DEFINE_FLOAT(MPC_ALTCTL_DZ, 0.1f);

From 0fbf12a021a21da8409d6db42b193d7047c9fcd6 Mon Sep 17 00:00:00 2001
From: Daniel Agar <daniel@agar.ca>
Date: Fri, 6 May 2016 17:43:19 -0400
Subject: [PATCH 0026/1230] datalinkloss_params.c param metadata

---
 src/modules/navigator/datalinkloss_params.c | 14 +++++++++++---
 1 file changed, 11 insertions(+), 3 deletions(-)

diff --git a/src/modules/navigator/datalinkloss_params.c b/src/modules/navigator/datalinkloss_params.c
index 8a5c59b87a..d720dcea64 100644
--- a/src/modules/navigator/datalinkloss_params.c
+++ b/src/modules/navigator/datalinkloss_params.c
@@ -50,6 +50,9 @@
  *
  * @unit s
  * @min 0.0
+ * @max 3600.0
+ * @decimal 0
+ * @increment 1
  * @group Data Link Loss
  */
 PARAM_DEFINE_FLOAT(NAV_DLL_CH_T, 120.0f);
@@ -86,17 +89,22 @@ PARAM_DEFINE_INT32(NAV_DLL_CH_LON, 1518453890);
  * @unit m
  * @min -50
  * @max 30000
+ * @decimal 1
+ * @increment 0.5
  * @group Data Link Loss
  */
 PARAM_DEFINE_FLOAT(NAV_DLL_CH_ALT, 600.0f);
 
 /**
- * Airfield hole wait time
+ * Airfield home wait time
  *
  * The amount of time in seconds the system should wait at the airfield home waypoint
  *
  * @unit s
  * @min 0.0
+ * @max 3600.0
+ * @decimal 0
+ * @increment 1
  * @group Data Link Loss
  */
 PARAM_DEFINE_FLOAT(NAV_DLL_AH_T, 120.0f);
@@ -106,9 +114,9 @@ PARAM_DEFINE_FLOAT(NAV_DLL_AH_T, 120.0f);
  *
  * After more than this number of data link timeouts the aircraft returns home directly
  *
- * @group Data Link Loss
  * @min 0
  * @max 1000
+ * @group Data Link Loss
  */
 PARAM_DEFINE_INT32(NAV_DLL_N, 2);
 
@@ -118,7 +126,7 @@ PARAM_DEFINE_INT32(NAV_DLL_N, 2);
  * If set to 1 the system will skip the comms hold wp on data link loss and will directly fly to
  * airfield home
  *
- * @group Data Link Loss
  * @boolean
+ * @group Data Link Loss
  */
 PARAM_DEFINE_INT32(NAV_DLL_CHSK, 0);

From 731f633d3eeb0eb789cf579be86494c0e603e1e7 Mon Sep 17 00:00:00 2001
From: Daniel Agar <daniel@agar.ca>
Date: Fri, 6 May 2016 17:43:21 -0400
Subject: [PATCH 0027/1230] gpsfailure_params.c param metadata

---
 src/modules/navigator/gpsfailure_params.c | 10 ++++++++++
 1 file changed, 10 insertions(+)

diff --git a/src/modules/navigator/gpsfailure_params.c b/src/modules/navigator/gpsfailure_params.c
index 087dba50bd..7ee5e77080 100644
--- a/src/modules/navigator/gpsfailure_params.c
+++ b/src/modules/navigator/gpsfailure_params.c
@@ -51,6 +51,9 @@
  *
  * @unit s
  * @min 0.0
+ * @max 3600.0
+ * @decimal 0
+ * @increment 1
  * @group GPS Failure Navigation
  */
 PARAM_DEFINE_FLOAT(NAV_GPSF_LT, 30.0f);
@@ -63,6 +66,8 @@ PARAM_DEFINE_FLOAT(NAV_GPSF_LT, 30.0f);
  * @unit deg
  * @min 0.0
  * @max 30.0
+ * @decimal 1
+ * @increment 0.5
  * @group GPS Failure Navigation
  */
 PARAM_DEFINE_FLOAT(NAV_GPSF_R, 15.0f);
@@ -75,6 +80,8 @@ PARAM_DEFINE_FLOAT(NAV_GPSF_R, 15.0f);
  * @unit deg
  * @min -30.0
  * @max 30.0
+ * @decimal 1
+ * @increment 0.5
  * @group GPS Failure Navigation
  */
 PARAM_DEFINE_FLOAT(NAV_GPSF_P, 0.0f);
@@ -84,8 +91,11 @@ PARAM_DEFINE_FLOAT(NAV_GPSF_P, 0.0f);
  *
  * Thrust value which is set during the open loop loiter
  *
+ * @unit norm
  * @min 0.0
  * @max 1.0
+ * @decimal 2
+ * @increment 0.05
  * @group GPS Failure Navigation
  */
 PARAM_DEFINE_FLOAT(NAV_GPSF_TR, 0.7f);

From c5d53c4ee6e9901d68f3f1d7804c94dba5f0c2f8 Mon Sep 17 00:00:00 2001
From: Daniel Agar <daniel@agar.ca>
Date: Fri, 6 May 2016 17:43:22 -0400
Subject: [PATCH 0028/1230] mission_params.c param metadata

---
 src/modules/navigator/mission_params.c | 10 +++++++++-
 1 file changed, 9 insertions(+), 1 deletion(-)

diff --git a/src/modules/navigator/mission_params.c b/src/modules/navigator/mission_params.c
index 217eea9eb1..5601ae01c9 100644
--- a/src/modules/navigator/mission_params.c
+++ b/src/modules/navigator/mission_params.c
@@ -51,6 +51,8 @@
  * @unit m
  * @min 0
  * @max 80
+ * @decimal 1
+ * @increment 0.5
  * @group Mission
  */
 PARAM_DEFINE_FLOAT(MIS_TAKEOFF_ALT, 10.0f);
@@ -63,6 +65,8 @@ PARAM_DEFINE_FLOAT(MIS_TAKEOFF_ALT, 10.0f);
  * @unit m
  * @min 0
  * @max 80
+ * @decimal 1
+ * @increment 0.5
  * @group Mission
  */
 PARAM_DEFINE_FLOAT(MIS_LTRMIN_ALT, 1.2f);
@@ -88,6 +92,8 @@ PARAM_DEFINE_INT32(MIS_ONBOARD_EN, 1);
  * @unit m
  * @min 0
  * @max 1000
+ * @decimal 1
+ * @increment 0.5
  * @group Mission
  */
 PARAM_DEFINE_FLOAT(MIS_DIST_1WP, 900);
@@ -133,17 +139,19 @@ PARAM_DEFINE_INT32(MIS_YAWMODE, 1);
  * @unit s
  * @min -1
  * @max 20
+ * @decimal 1
  * @increment 1
  * @group Mission
  */
 PARAM_DEFINE_FLOAT(MIS_YAW_TMT, -1.0f);
 
 /**
- * Max yaw error in degree needed for waypoint heading acceptance.
+ * Max yaw error in degrees needed for waypoint heading acceptance.
  *
  * @unit deg
  * @min 0
  * @max 90
+ * @decimal 1
  * @increment 1
  * @group Mission
  */

From ec519a33b3fa46faafec675b5bd9d56c746b70e0 Mon Sep 17 00:00:00 2001
From: Daniel Agar <daniel@agar.ca>
Date: Fri, 6 May 2016 17:43:23 -0400
Subject: [PATCH 0029/1230] navigator_params.c param metadata

---
 src/modules/navigator/navigator_params.c | 6 ++++++
 1 file changed, 6 insertions(+)

diff --git a/src/modules/navigator/navigator_params.c b/src/modules/navigator/navigator_params.c
index 5b02b73aa1..523f03b42e 100644
--- a/src/modules/navigator/navigator_params.c
+++ b/src/modules/navigator/navigator_params.c
@@ -48,6 +48,8 @@
  * @unit m
  * @min 25
  * @max 1000
+ * @decimal 1
+ * @increment 0.5
  * @group Mission
  */
 PARAM_DEFINE_FLOAT(NAV_LOITER_RAD, 50.0f);
@@ -60,6 +62,8 @@ PARAM_DEFINE_FLOAT(NAV_LOITER_RAD, 50.0f);
  * @unit m
  * @min 0.05
  * @max 200.0
+ * @decimal 1
+ * @increment 0.5
  * @group Mission
  */
 PARAM_DEFINE_FLOAT(NAV_ACC_RAD, 10.0f);
@@ -131,6 +135,8 @@ PARAM_DEFINE_INT32(NAV_AH_LON, 1518423250);
  *
  * @unit m
  * @min -50
+ * @decimal 1
+ * @increment 0.5
  * @group Data Link Loss
  */
 PARAM_DEFINE_FLOAT(NAV_AH_ALT, 600.0f);

From 845b4c032cf062c2ddc921529aa55f2722c285b8 Mon Sep 17 00:00:00 2001
From: Daniel Agar <daniel@agar.ca>
Date: Fri, 6 May 2016 17:43:24 -0400
Subject: [PATCH 0030/1230] battery_params.c param metadata

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

diff --git a/src/modules/systemlib/battery_params.c b/src/modules/systemlib/battery_params.c
index 3268e7d8e5..fdf4826afd 100644
--- a/src/modules/systemlib/battery_params.c
+++ b/src/modules/systemlib/battery_params.c
@@ -78,9 +78,9 @@ PARAM_DEFINE_FLOAT(BAT_V_CHARGED, 4.05f);
  *
  * @group Battery Calibration
  * @unit norm
- * @decimal 2
  * @min 0.12
  * @max 0.4
+ * @decimal 2
  * @increment 0.01
  */
 PARAM_DEFINE_FLOAT(BAT_LOW_THR, 0.15f);
@@ -94,9 +94,9 @@ PARAM_DEFINE_FLOAT(BAT_LOW_THR, 0.15f);
  *
  * @group Battery Calibration
  * @unit norm
- * @decimal 2
  * @min 0.05
  * @max 0.1
+ * @decimal 2
  * @increment 0.01
  */
 PARAM_DEFINE_FLOAT(BAT_CRIT_THR, 0.07f);

From 8e130c878c4f871cdd79309a392a54b66d29c2ee Mon Sep 17 00:00:00 2001
From: Daniel Agar <daniel@agar.ca>
Date: Fri, 6 May 2016 17:43:25 -0400
Subject: [PATCH 0031/1230] system_params.c param metadata

---
 src/modules/systemlib/system_params.c | 8 ++++++++
 1 file changed, 8 insertions(+)

diff --git a/src/modules/systemlib/system_params.c b/src/modules/systemlib/system_params.c
index 1db83fa260..996cf79e3f 100644
--- a/src/modules/systemlib/system_params.c
+++ b/src/modules/systemlib/system_params.c
@@ -43,6 +43,8 @@
  * CHANGING THIS VALUE REQUIRES A RESTART. Defines the auto-start script used to bootstrap the system.
  *
  * @reboot_required true
+ * @min 0
+ * @max 99999
  * @group System
  */
 PARAM_DEFINE_INT32(SYS_AUTOSTART, 0);
@@ -56,6 +58,8 @@ PARAM_DEFINE_INT32(SYS_AUTOSTART, 0);
  *
  * @min 0
  * @max 1
+ * @value 0 Keep parameters
+ * @value 1 Reset parameters
  * @group System
  */
 PARAM_DEFINE_INT32(SYS_AUTOCONFIG, 0);
@@ -65,6 +69,7 @@ PARAM_DEFINE_INT32(SYS_AUTOCONFIG, 0);
  *
  * Can be used to use a standard startup script but with a FMU only set-up. Set to 0 to force the FMU only set-up.
  *
+ * @boolean
  * @min 0
  * @max 1
  * @group System
@@ -78,6 +83,9 @@ PARAM_DEFINE_INT32(SYS_USE_IO, 1);
  *
  * @min 0
  * @max 2
+ * @value 0 Data survives resets
+ * @value 1 Data survives in-flight resets only
+ * @value 2 Data does not survive reset
  * @group System
  */
 PARAM_DEFINE_INT32(SYS_RESTART_TYPE, 2);

From 194b48b50a60f493f3e941af518b675aab305276 Mon Sep 17 00:00:00 2001
From: Daniel Agar <daniel@agar.ca>
Date: Thu, 5 May 2016 14:19:37 -0400
Subject: [PATCH 0032/1230] srcscanner.py replace windows slashes

---
 Tools/px4params/srcscanner.py | 3 ++-
 1 file changed, 2 insertions(+), 1 deletion(-)

diff --git a/Tools/px4params/srcscanner.py b/Tools/px4params/srcscanner.py
index 739a067c9a..2b5f5f3fb0 100644
--- a/Tools/px4params/srcscanner.py
+++ b/Tools/px4params/srcscanner.py
@@ -34,7 +34,8 @@ class SourceScanner(object):
         parser.Parse method.
         """
         prefix = ".." + os.path.sep + "src" + os.path.sep
-        scope = re.sub(prefix, '', os.path.dirname(os.path.relpath(path)))
+        scope = re.sub(prefix.replace("\\", "/"), "", os.path.dirname(os.path.relpath(path)).replace("\\", "/"))
+
         with codecs.open(path, 'r', 'utf-8') as f:
             try:
                 contents = f.read()

From 871c112699660a1d96f4caff42fd5e6197a20835 Mon Sep 17 00:00:00 2001
From: Julian Oes <julian@oes.ch>
Date: Thu, 5 May 2016 11:39:46 +0200
Subject: [PATCH 0033/1230] sdlog2: log normal data and replay on Snapdragon

In SITL and on Snapdragon, the logging performance is high enough, so we
can log both: the usual topics, as well as the ekf2 replay fields.

Note that the ekf2 replay still needs to be enabled via param.
---
 src/modules/sdlog2/sdlog2.c | 106 ++++++++++++++++++++++++++----------
 1 file changed, 78 insertions(+), 28 deletions(-)

diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c
index 82ad042821..21b14df49e 100644
--- a/src/modules/sdlog2/sdlog2.c
+++ b/src/modules/sdlog2/sdlog2.c
@@ -1127,13 +1127,29 @@ int sdlog2_thread_main(int argc, char *argv[])
 	struct commander_state_s buf_commander_state;
 	memset(&buf_commander_state, 0, sizeof(buf_commander_state));
 
-	// check if we are gathering data for a replay log for ekf2
-	// is yes then disable logging of some topics to avoid dropouts
+	/* There are different log types possible on different platforms. */
+	enum {
+		LOG_TYPE_NORMAL,
+		LOG_TYPE_REPLAY_ONLY,
+		LOG_TYPE_ALL
+	} log_type;
+
+	/* Check if we are gathering data for a replay log for ekf2. */
 	param_t replay_handle = param_find("EKF2_REC_RPL");
 	int32_t tmp = 0;
 	param_get(replay_handle, &tmp);
 	bool record_replay_log = (bool)tmp;
 
+	if (record_replay_log) {
+#if defined(__PX4_QURT) || defined(__PX4_POSIX)
+		log_type = LOG_TYPE_ALL;
+#else
+		log_type = LOG_TYPE_REPLAY_ONLY;
+#endif
+	} else {
+		log_type = LOG_TYPE_NORMAL;
+	}
+
 	/* warning! using union here to save memory, elements should be used separately! */
 	union {
 		struct vehicle_command_s cmd;
@@ -1369,17 +1385,36 @@ int sdlog2_thread_main(int argc, char *argv[])
 
 	int poll_to_logging_factor = 1;
 
-	if (record_replay_log) {
-		subs.replay_sub = orb_subscribe(ORB_ID(ekf2_replay));
-		fds[0].fd = subs.replay_sub;
-		fds[0].events = POLLIN;
-		poll_to_logging_factor = 1;
-	} else {
-		subs.sensor_sub = orb_subscribe(ORB_ID(sensor_combined));
-		fds[0].fd = subs.sensor_sub;
-		fds[0].events = POLLIN;
-		// TODO Remove hardcoded rate!
-		poll_to_logging_factor = 250 / (log_rate < 1 ? 1 : log_rate);
+	switch (log_type) {
+		case LOG_TYPE_ALL:
+			subs.sensor_sub = orb_subscribe(ORB_ID(sensor_combined));
+			fds[0].fd = subs.sensor_sub;
+			fds[0].events = POLLIN;
+
+			poll_to_logging_factor = 1;
+
+			break;
+
+		case LOG_TYPE_NORMAL:
+
+			subs.sensor_sub = orb_subscribe(ORB_ID(sensor_combined));
+			fds[0].fd = subs.sensor_sub;
+			fds[0].events = POLLIN;
+
+			// TODO Remove hardcoded rate!
+			poll_to_logging_factor = 250 / (log_rate < 1 ? 1 : log_rate);
+
+			break;
+
+		case LOG_TYPE_REPLAY_ONLY:
+
+			subs.replay_sub = orb_subscribe(ORB_ID(ekf2_replay));
+			fds[0].fd = subs.replay_sub;
+			fds[0].events = POLLIN;
+
+			poll_to_logging_factor = 1;
+
+			break;
 	}
 
 	if (poll_to_logging_factor < 1) {
@@ -1409,12 +1444,18 @@ int sdlog2_thread_main(int argc, char *argv[])
 			continue;
 		}
 
-		// copy topic always to mark them as read
-		// so poll doesn't return immediately
-		if (record_replay_log) {
-			orb_copy(ORB_ID(ekf2_replay), subs.replay_sub, &buf.replay);
-		} else {
-			orb_copy(ORB_ID(sensor_combined), subs.sensor_sub, &buf.sensor);
+		switch (log_type) {
+			case LOG_TYPE_ALL:
+				orb_copy(ORB_ID(sensor_combined), subs.sensor_sub, &buf.sensor);
+				break;
+
+			case LOG_TYPE_NORMAL:
+				orb_copy(ORB_ID(sensor_combined), subs.sensor_sub, &buf.sensor);
+				break;
+
+			case LOG_TYPE_REPLAY_ONLY:
+				orb_copy(ORB_ID(ekf2_replay), subs.replay_sub, &buf.replay);
+				break;
 		}
 
 		if ((poll_counter + 1) % poll_to_logging_factor == 0) {
@@ -1472,9 +1513,19 @@ int sdlog2_thread_main(int argc, char *argv[])
 		}
 
 		/* --- EKF2 REPLAY --- */
-		if(record_replay_log) {
-			// we poll on the replay topic so we know that it was updated
-			// but we need to copy it again since we are re-using the buffer.
+		if (log_type == LOG_TYPE_ALL || LOG_TYPE_REPLAY_ONLY) {
+
+			if (log_type == LOG_TYPE_ALL) {
+				// When logging everything we are polling for sensor_combined, so
+				// we need to use the usual copy_if_updated which includes orb_subscribe.
+				copy_if_updated(ORB_ID(ekf2_replay), &subs.replay_sub, &buf.replay);
+
+			} else {
+				// We poll on the replay topic so we know that it was updated
+				// but we need to copy it again since we are re-using the buffer.
+				orb_copy(ORB_ID(ekf2_replay), subs.replay_sub, &buf.replay);
+			}
+
 			orb_copy(ORB_ID(ekf2_replay), subs.replay_sub, &buf.replay);
 			log_msg.msg_type = LOG_RPL1_MSG;
 			log_msg.body.log_RPL1.time_ref = buf.replay.time_ref;
@@ -1534,7 +1585,6 @@ int sdlog2_thread_main(int argc, char *argv[])
 				LOGBUFFER_WRITE_AND_COUNT(RPL4);
 			}
 
-
 			if (buf.replay.asp_timestamp > 0) {
 				log_msg.msg_type = LOG_RPL6_MSG;
 				log_msg.body.log_RPL6.timestamp = buf.replay.asp_timestamp;
@@ -1545,12 +1595,12 @@ int sdlog2_thread_main(int argc, char *argv[])
 				log_msg.body.log_RPL6.confidence = buf.replay.confidence;
 				LOGBUFFER_WRITE_AND_COUNT(RPL6);
 			}
+		}
 
+		if (log_type == LOG_TYPE_ALL || LOG_TYPE_NORMAL) {
 
-		} else { /* !record_replay_log */
-
-			// we poll on sensor combined, so we know it has updated just now
-			// but we need to copy it again because we are re-using the buffer
+			// We poll on sensor combined, so we know it has updated just now
+			// but we need to copy it again because we are re-using the buffer.
 			orb_copy(ORB_ID(sensor_combined), subs.sensor_sub, &buf.sensor);
 
 			for (unsigned i = 0; i < 3; i++) {
@@ -1834,7 +1884,7 @@ int sdlog2_thread_main(int argc, char *argv[])
 				log_msg.msg_type = LOG_BATT_MSG;
 				log_msg.body.log_BATT.voltage = buf.battery.voltage_v;
 				log_msg.body.log_BATT.voltage_filtered = buf.battery.voltage_filtered_v;
-				log_msg.body.log_BATT.current = buf.battery.current_a;				
+				log_msg.body.log_BATT.current = buf.battery.current_a;
 				log_msg.body.log_BATT.current_filtered = buf.battery.current_filtered_a;
 				log_msg.body.log_BATT.discharged = buf.battery.discharged_mah;
 				log_msg.body.log_BATT.remaining = buf.battery.remaining;

From e00101e5abfa78b2ed4b698f1d090c196961aa0a Mon Sep 17 00:00:00 2001
From: Julian Oes <julian@oes.ch>
Date: Thu, 5 May 2016 13:01:36 +0200
Subject: [PATCH 0034/1230] sdlog2: remove duplicate orb_copy

---
 src/modules/sdlog2/sdlog2.c | 1 -
 1 file changed, 1 deletion(-)

diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c
index 21b14df49e..f71edbb874 100644
--- a/src/modules/sdlog2/sdlog2.c
+++ b/src/modules/sdlog2/sdlog2.c
@@ -1526,7 +1526,6 @@ int sdlog2_thread_main(int argc, char *argv[])
 				orb_copy(ORB_ID(ekf2_replay), subs.replay_sub, &buf.replay);
 			}
 
-			orb_copy(ORB_ID(ekf2_replay), subs.replay_sub, &buf.replay);
 			log_msg.msg_type = LOG_RPL1_MSG;
 			log_msg.body.log_RPL1.time_ref = buf.replay.time_ref;
 			log_msg.body.log_RPL1.gyro_integral_dt = buf.replay.gyro_integral_dt;

From 91cc52f60edb46988f4ed657abaf4be026b358a2 Mon Sep 17 00:00:00 2001
From: Julian Oes <julian@oes.ch>
Date: Thu, 5 May 2016 14:47:16 +0200
Subject: [PATCH 0035/1230] posix-configs: conflicting args for sdlog2

---
 posix-configs/eagle/flight/mainapp.config | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/posix-configs/eagle/flight/mainapp.config b/posix-configs/eagle/flight/mainapp.config
index e12d020c35..abaa939a01 100644
--- a/posix-configs/eagle/flight/mainapp.config
+++ b/posix-configs/eagle/flight/mainapp.config
@@ -5,4 +5,4 @@ sleep 1
 mavlink stream -u 14556 -s HIGHRES_IMU -r 50
 mavlink stream -u 14556 -s ATTITUDE -r 50
 mavlink boot_complete
-sdlog2 start -r 100 -e -t -a
+sdlog2 start -r 100 -e -t

From c8d888cdc106b2e0a060e2c3fa549bf2bd6aad84 Mon Sep 17 00:00:00 2001
From: Julian Oes <julian@oes.ch>
Date: Thu, 5 May 2016 16:15:44 +0200
Subject: [PATCH 0036/1230] sdlog2: fix wrong if (facepalm!)

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

diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c
index f71edbb874..8dd69a1726 100644
--- a/src/modules/sdlog2/sdlog2.c
+++ b/src/modules/sdlog2/sdlog2.c
@@ -1513,7 +1513,7 @@ int sdlog2_thread_main(int argc, char *argv[])
 		}
 
 		/* --- EKF2 REPLAY --- */
-		if (log_type == LOG_TYPE_ALL || LOG_TYPE_REPLAY_ONLY) {
+		if (log_type == LOG_TYPE_ALL || log_type == LOG_TYPE_REPLAY_ONLY) {
 
 			if (log_type == LOG_TYPE_ALL) {
 				// When logging everything we are polling for sensor_combined, so
@@ -1596,7 +1596,7 @@ int sdlog2_thread_main(int argc, char *argv[])
 			}
 		}
 
-		if (log_type == LOG_TYPE_ALL || LOG_TYPE_NORMAL) {
+		if (log_type == LOG_TYPE_ALL || log_type == LOG_TYPE_NORMAL) {
 
 			// We poll on sensor combined, so we know it has updated just now
 			// but we need to copy it again because we are re-using the buffer.

From f6845df21fc782ed141a698bdb8e1108486aa467 Mon Sep 17 00:00:00 2001
From: Julian Oes <julian@oes.ch>
Date: Thu, 5 May 2016 16:19:24 +0200
Subject: [PATCH 0037/1230] sdlog2: don't log an empty sensor_combined topic

---
 src/modules/sdlog2/sdlog2.c | 133 +++++++++++++++++++-----------------
 1 file changed, 69 insertions(+), 64 deletions(-)

diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c
index 8dd69a1726..95b4c559c5 100644
--- a/src/modules/sdlog2/sdlog2.c
+++ b/src/modules/sdlog2/sdlog2.c
@@ -1515,84 +1515,89 @@ int sdlog2_thread_main(int argc, char *argv[])
 		/* --- EKF2 REPLAY --- */
 		if (log_type == LOG_TYPE_ALL || log_type == LOG_TYPE_REPLAY_ONLY) {
 
+			bool replay_updated = false;
+
 			if (log_type == LOG_TYPE_ALL) {
 				// When logging everything we are polling for sensor_combined, so
 				// we need to use the usual copy_if_updated which includes orb_subscribe.
-				copy_if_updated(ORB_ID(ekf2_replay), &subs.replay_sub, &buf.replay);
+				replay_updated = copy_if_updated(ORB_ID(ekf2_replay), &subs.replay_sub, &buf.replay);
 
 			} else {
 				// We poll on the replay topic so we know that it was updated
 				// but we need to copy it again since we are re-using the buffer.
 				orb_copy(ORB_ID(ekf2_replay), subs.replay_sub, &buf.replay);
+				replay_updated = true;
 			}
 
-			log_msg.msg_type = LOG_RPL1_MSG;
-			log_msg.body.log_RPL1.time_ref = buf.replay.time_ref;
-			log_msg.body.log_RPL1.gyro_integral_dt = buf.replay.gyro_integral_dt;
-			log_msg.body.log_RPL1.accelerometer_integral_dt = buf.replay.accelerometer_integral_dt;
-			log_msg.body.log_RPL1.magnetometer_timestamp = buf.replay.magnetometer_timestamp;
-			log_msg.body.log_RPL1.baro_timestamp = buf.replay.baro_timestamp;
-			log_msg.body.log_RPL1.gyro_integral_x_rad = buf.replay.gyro_integral_rad[0];
-			log_msg.body.log_RPL1.gyro_integral_y_rad = buf.replay.gyro_integral_rad[1];
-			log_msg.body.log_RPL1.gyro_integral_z_rad = buf.replay.gyro_integral_rad[2];
-			log_msg.body.log_RPL1.accelerometer_integral_x_m_s = buf.replay.accelerometer_integral_m_s[0];
-			log_msg.body.log_RPL1.accelerometer_integral_y_m_s = buf.replay.accelerometer_integral_m_s[1];
-			log_msg.body.log_RPL1.accelerometer_integral_z_m_s = buf.replay.accelerometer_integral_m_s[2];
-			log_msg.body.log_RPL1.magnetometer_x_ga = buf.replay.magnetometer_ga[0];
-			log_msg.body.log_RPL1.magnetometer_y_ga = buf.replay.magnetometer_ga[1];
-			log_msg.body.log_RPL1.magnetometer_z_ga = buf.replay.magnetometer_ga[2];
-			log_msg.body.log_RPL1.baro_alt_meter = buf.replay.baro_alt_meter;
-			LOGBUFFER_WRITE_AND_COUNT(RPL1);
+			if (replay_updated) {
+				log_msg.msg_type = LOG_RPL1_MSG;
+				log_msg.body.log_RPL1.time_ref = buf.replay.time_ref;
+				log_msg.body.log_RPL1.gyro_integral_dt = buf.replay.gyro_integral_dt;
+				log_msg.body.log_RPL1.accelerometer_integral_dt = buf.replay.accelerometer_integral_dt;
+				log_msg.body.log_RPL1.magnetometer_timestamp = buf.replay.magnetometer_timestamp;
+				log_msg.body.log_RPL1.baro_timestamp = buf.replay.baro_timestamp;
+				log_msg.body.log_RPL1.gyro_integral_x_rad = buf.replay.gyro_integral_rad[0];
+				log_msg.body.log_RPL1.gyro_integral_y_rad = buf.replay.gyro_integral_rad[1];
+				log_msg.body.log_RPL1.gyro_integral_z_rad = buf.replay.gyro_integral_rad[2];
+				log_msg.body.log_RPL1.accelerometer_integral_x_m_s = buf.replay.accelerometer_integral_m_s[0];
+				log_msg.body.log_RPL1.accelerometer_integral_y_m_s = buf.replay.accelerometer_integral_m_s[1];
+				log_msg.body.log_RPL1.accelerometer_integral_z_m_s = buf.replay.accelerometer_integral_m_s[2];
+				log_msg.body.log_RPL1.magnetometer_x_ga = buf.replay.magnetometer_ga[0];
+				log_msg.body.log_RPL1.magnetometer_y_ga = buf.replay.magnetometer_ga[1];
+				log_msg.body.log_RPL1.magnetometer_z_ga = buf.replay.magnetometer_ga[2];
+				log_msg.body.log_RPL1.baro_alt_meter = buf.replay.baro_alt_meter;
+				LOGBUFFER_WRITE_AND_COUNT(RPL1);
 
-			// only log the gps replay data if it actually updated
-			if (buf.replay.time_usec > 0) {
-				log_msg.msg_type = LOG_RPL2_MSG;
-				log_msg.body.log_RPL2.time_pos_usec = buf.replay.time_usec;
-				log_msg.body.log_RPL2.time_vel_usec = buf.replay.time_usec_vel;
-				log_msg.body.log_RPL2.lat = buf.replay.lat;
-				log_msg.body.log_RPL2.lon = buf.replay.lon;
-				log_msg.body.log_RPL2.alt = buf.replay.alt;
-				log_msg.body.log_RPL2.fix_type = buf.replay.fix_type;
-				log_msg.body.log_RPL2.nsats = buf.replay.nsats;
-				log_msg.body.log_RPL2.eph = buf.replay.eph;
-				log_msg.body.log_RPL2.epv = buf.replay.epv;
-				log_msg.body.log_RPL2.sacc = buf.replay.sacc;
-				log_msg.body.log_RPL2.vel_m_s = buf.replay.vel_m_s;
-				log_msg.body.log_RPL2.vel_n_m_s = buf.replay.vel_n_m_s;
-				log_msg.body.log_RPL2.vel_e_m_s = buf.replay.vel_e_m_s;
-				log_msg.body.log_RPL2.vel_d_m_s = buf.replay.vel_d_m_s;
-				log_msg.body.log_RPL2.vel_ned_valid = buf.replay.vel_ned_valid;
-				LOGBUFFER_WRITE_AND_COUNT(RPL2);
-			}
+				// only log the gps replay data if it actually updated
+				if (buf.replay.time_usec > 0) {
+					log_msg.msg_type = LOG_RPL2_MSG;
+					log_msg.body.log_RPL2.time_pos_usec = buf.replay.time_usec;
+					log_msg.body.log_RPL2.time_vel_usec = buf.replay.time_usec_vel;
+					log_msg.body.log_RPL2.lat = buf.replay.lat;
+					log_msg.body.log_RPL2.lon = buf.replay.lon;
+					log_msg.body.log_RPL2.alt = buf.replay.alt;
+					log_msg.body.log_RPL2.fix_type = buf.replay.fix_type;
+					log_msg.body.log_RPL2.nsats = buf.replay.nsats;
+					log_msg.body.log_RPL2.eph = buf.replay.eph;
+					log_msg.body.log_RPL2.epv = buf.replay.epv;
+					log_msg.body.log_RPL2.sacc = buf.replay.sacc;
+					log_msg.body.log_RPL2.vel_m_s = buf.replay.vel_m_s;
+					log_msg.body.log_RPL2.vel_n_m_s = buf.replay.vel_n_m_s;
+					log_msg.body.log_RPL2.vel_e_m_s = buf.replay.vel_e_m_s;
+					log_msg.body.log_RPL2.vel_d_m_s = buf.replay.vel_d_m_s;
+					log_msg.body.log_RPL2.vel_ned_valid = buf.replay.vel_ned_valid;
+					LOGBUFFER_WRITE_AND_COUNT(RPL2);
+				}
 
-			if (buf.replay.flow_timestamp > 0) {
-				log_msg.msg_type = LOG_RPL3_MSG;
-				log_msg.body.log_RPL3.time_flow_usec = buf.replay.flow_timestamp;
-				log_msg.body.log_RPL3.flow_integral_x = buf.replay.flow_pixel_integral[0];
-				log_msg.body.log_RPL3.flow_integral_y = buf.replay.flow_pixel_integral[1];
-				log_msg.body.log_RPL3.gyro_integral_x = buf.replay.flow_gyro_integral[0];
-				log_msg.body.log_RPL3.gyro_integral_y = buf.replay.flow_gyro_integral[1];
-				log_msg.body.log_RPL3.flow_time_integral = buf.replay.flow_time_integral;
-				log_msg.body.log_RPL3.flow_quality = buf.replay.flow_quality;
-				LOGBUFFER_WRITE_AND_COUNT(RPL3);
-			}
+				if (buf.replay.flow_timestamp > 0) {
+					log_msg.msg_type = LOG_RPL3_MSG;
+					log_msg.body.log_RPL3.time_flow_usec = buf.replay.flow_timestamp;
+					log_msg.body.log_RPL3.flow_integral_x = buf.replay.flow_pixel_integral[0];
+					log_msg.body.log_RPL3.flow_integral_y = buf.replay.flow_pixel_integral[1];
+					log_msg.body.log_RPL3.gyro_integral_x = buf.replay.flow_gyro_integral[0];
+					log_msg.body.log_RPL3.gyro_integral_y = buf.replay.flow_gyro_integral[1];
+					log_msg.body.log_RPL3.flow_time_integral = buf.replay.flow_time_integral;
+					log_msg.body.log_RPL3.flow_quality = buf.replay.flow_quality;
+					LOGBUFFER_WRITE_AND_COUNT(RPL3);
+				}
 
-			if (buf.replay.rng_timestamp > 0) {
-				log_msg.msg_type = LOG_RPL4_MSG;
-				log_msg.body.log_RPL4.time_rng_usec = buf.replay.rng_timestamp;
-				log_msg.body.log_RPL4.range_to_ground = buf.replay.range_to_ground;
-				LOGBUFFER_WRITE_AND_COUNT(RPL4);
-			}
+				if (buf.replay.rng_timestamp > 0) {
+					log_msg.msg_type = LOG_RPL4_MSG;
+					log_msg.body.log_RPL4.time_rng_usec = buf.replay.rng_timestamp;
+					log_msg.body.log_RPL4.range_to_ground = buf.replay.range_to_ground;
+					LOGBUFFER_WRITE_AND_COUNT(RPL4);
+				}
 
-			if (buf.replay.asp_timestamp > 0) {
-				log_msg.msg_type = LOG_RPL6_MSG;
-				log_msg.body.log_RPL6.timestamp = buf.replay.asp_timestamp;
-				log_msg.body.log_RPL6.indicated_airspeed_m_s = buf.replay.indicated_airspeed_m_s;
-				log_msg.body.log_RPL6.true_airspeed_m_s = buf.replay.true_airspeed_m_s;
-				log_msg.body.log_RPL6.true_airspeed_unfiltered_m_s = buf.replay.true_airspeed_unfiltered_m_s;
-				log_msg.body.log_RPL6.air_temperature_celsius = buf.replay.air_temperature_celsius;
-				log_msg.body.log_RPL6.confidence = buf.replay.confidence;
-				LOGBUFFER_WRITE_AND_COUNT(RPL6);
+				if (buf.replay.asp_timestamp > 0) {
+					log_msg.msg_type = LOG_RPL6_MSG;
+					log_msg.body.log_RPL6.timestamp = buf.replay.asp_timestamp;
+					log_msg.body.log_RPL6.indicated_airspeed_m_s = buf.replay.indicated_airspeed_m_s;
+					log_msg.body.log_RPL6.true_airspeed_m_s = buf.replay.true_airspeed_m_s;
+					log_msg.body.log_RPL6.true_airspeed_unfiltered_m_s = buf.replay.true_airspeed_unfiltered_m_s;
+					log_msg.body.log_RPL6.air_temperature_celsius = buf.replay.air_temperature_celsius;
+					log_msg.body.log_RPL6.confidence = buf.replay.confidence;
+					LOGBUFFER_WRITE_AND_COUNT(RPL6);
+				}
 			}
 		}
 

From c7c786d56798e707f8c24daa0a0e69d6f0a799cd Mon Sep 17 00:00:00 2001
From: Julian Oes <julian@oes.ch>
Date: Thu, 5 May 2016 16:50:17 +0200
Subject: [PATCH 0038/1230] Revert "posix-configs: conflicting args for sdlog2"

This reverts commit aac9a584aa5fa26ea93d7583305c44d50608ecfa.
---
 posix-configs/eagle/flight/mainapp.config | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/posix-configs/eagle/flight/mainapp.config b/posix-configs/eagle/flight/mainapp.config
index abaa939a01..e12d020c35 100644
--- a/posix-configs/eagle/flight/mainapp.config
+++ b/posix-configs/eagle/flight/mainapp.config
@@ -5,4 +5,4 @@ sleep 1
 mavlink stream -u 14556 -s HIGHRES_IMU -r 50
 mavlink stream -u 14556 -s ATTITUDE -r 50
 mavlink boot_complete
-sdlog2 start -r 100 -e -t
+sdlog2 start -r 100 -e -t -a

From 5ee865a6eb24897d469c1724b1b3338c4afe4a26 Mon Sep 17 00:00:00 2001
From: Julian Oes <julian@oes.ch>
Date: Thu, 5 May 2016 17:14:46 +0200
Subject: [PATCH 0039/1230] eagle: use a bigger buffer for sdlog2

---
 posix-configs/eagle/flight/mainapp.config | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/posix-configs/eagle/flight/mainapp.config b/posix-configs/eagle/flight/mainapp.config
index e12d020c35..03fccce897 100644
--- a/posix-configs/eagle/flight/mainapp.config
+++ b/posix-configs/eagle/flight/mainapp.config
@@ -5,4 +5,4 @@ sleep 1
 mavlink stream -u 14556 -s HIGHRES_IMU -r 50
 mavlink stream -u 14556 -s ATTITUDE -r 50
 mavlink boot_complete
-sdlog2 start -r 100 -e -t -a
+sdlog2 start -r 100 -e -t -a -b 200

From 749b598af1118c6cce4d8dd937eec00dbd563fd8 Mon Sep 17 00:00:00 2001
From: Jonathan Challinger <mr.challinger@gmail.com>
Date: Fri, 26 Feb 2016 18:03:18 -0800
Subject: [PATCH 0040/1230] load_mon: initial commit

---
 msg/cpuload.msg                     |   2 +
 src/modules/load_mon/CMakeLists.txt |  42 +++++++
 src/modules/load_mon/load_mon.cpp   | 174 ++++++++++++++++++++++++++++
 src/modules/uORB/objects_common.cpp |   3 +
 4 files changed, 221 insertions(+)
 create mode 100644 msg/cpuload.msg
 create mode 100644 src/modules/load_mon/CMakeLists.txt
 create mode 100644 src/modules/load_mon/load_mon.cpp

diff --git a/msg/cpuload.msg b/msg/cpuload.msg
new file mode 100644
index 0000000000..a8d84dd53c
--- /dev/null
+++ b/msg/cpuload.msg
@@ -0,0 +1,2 @@
+uint64 timestamp                # in microseconds since system start, is set whenever the writing thread stores new data
+float32 load                    # processor load from 0 to 1
diff --git a/src/modules/load_mon/CMakeLists.txt b/src/modules/load_mon/CMakeLists.txt
new file mode 100644
index 0000000000..f714895a24
--- /dev/null
+++ b/src/modules/load_mon/CMakeLists.txt
@@ -0,0 +1,42 @@
+############################################################################
+#
+#   Copyright (c) 2015 PX4 Development Team. All rights reserved.
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions
+# are met:
+#
+# 1. Redistributions of source code must retain the above copyright
+#    notice, this list of conditions and the following disclaimer.
+# 2. Redistributions in binary form must reproduce the above copyright
+#    notice, this list of conditions and the following disclaimer in
+#    the documentation and/or other materials provided with the
+#    distribution.
+# 3. Neither the name PX4 nor the names of its contributors may be
+#    used to endorse or promote products derived from this software
+#    without specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.
+#
+############################################################################
+px4_add_module(
+	MODULE modules__load_mon
+	MAIN load_mon
+	STACK 1200
+	SRCS
+		load_mon.cpp
+	DEPENDS
+		platforms__common
+	)
+# vim: set noet ft=cmake fenc=utf-8 ff=unix : 
diff --git a/src/modules/load_mon/load_mon.cpp b/src/modules/load_mon/load_mon.cpp
new file mode 100644
index 0000000000..04a3fb9c1b
--- /dev/null
+++ b/src/modules/load_mon/load_mon.cpp
@@ -0,0 +1,174 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2012-2016 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file load_mon.c
+ *
+ * @author Jonathan Challinger <jonathan@3drobotics.com>
+ */
+
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+#include <unistd.h>
+
+#include <poll.h>
+
+#include <px4_config.h>
+
+#include <drivers/drv_hrt.h>
+
+#include <systemlib/systemlib.h>
+#include <systemlib/err.h>
+#include <systemlib/cpuload.h>
+
+#include <uORB/uORB.h>
+#include <uORB/topics/cpuload.h>
+
+#define LOAD_MON_INTERVAL_SEC 1.0f
+#define LOAD_MON_INTERVAL_US (LOAD_MON_INTERVAL_SEC*1.0e6f)
+
+extern "C" __EXPORT int load_mon_main(int argc, char *argv[]);
+int load_mon_thread(int argc, char *argv[]);
+
+static const char *module_name = "load_mon";
+static bool thread_should_exit = false;		/**< daemon exit flag */
+static bool thread_running = false;		/**< daemon status flag */
+static int daemon_task;				/**< Handle of daemon task / thread */
+
+
+extern struct system_load_s system_load;
+struct cpuload_s cpuload;
+static orb_advert_t cpuload_publish_handle = nullptr;
+
+/**
+ * Mainloop of load_mon.
+ */
+int load_mon_thread(int argc, char *argv[])
+{
+	warnx("[%s] starting\n", module_name);
+	thread_running = true;
+
+	hrt_abstime last_idle_time = system_load.tasks[0].total_runtime;
+
+	while (!thread_should_exit) {
+		usleep(LOAD_MON_INTERVAL_US);
+
+		/* compute system load */
+		uint64_t interval_idletime = system_load.tasks[0].total_runtime - last_idle_time;
+		last_idle_time = system_load.tasks[0].total_runtime;
+
+		cpuload.timestamp = hrt_absolute_time();
+		cpuload.load = 1.0f - (float)interval_idletime / LOAD_MON_INTERVAL_US;
+
+		if (cpuload_publish_handle == nullptr) {
+			cpuload_publish_handle = orb_advertise(ORB_ID(cpuload), &cpuload);
+		} else {
+			orb_publish(ORB_ID(cpuload), cpuload_publish_handle, &cpuload);
+		}
+	}
+
+	warnx("[%s] exiting.\n", module_name);
+
+	thread_running = false;
+
+	return 0;
+}
+
+/**
+ * Print the correct usage.
+ */
+static void usage(const char *reason);
+
+static void
+usage(const char *reason)
+{
+	if (reason) {
+		warnx("%s\n", reason);
+	}
+
+	warnx("usage: %s {start|stop|status} [-p <additional params>]\n\n", module_name);
+}
+
+/**
+ * The daemon app only briefly exists to start
+ * the background job. The stack size assigned in the
+ * Makefile does only apply to this management task.
+ *
+ * The actual stack size should be set in the call
+ * to task_create().
+ */
+int load_mon_main(int argc, char *argv[])
+{
+	if (argc < 2) {
+		usage("missing command");
+		return 1;
+	}
+
+	if (!strcmp(argv[1], "start")) {
+
+		if (thread_running) {
+			warnx("daemon already running\n");
+			/* this is not an error */
+			return 0;
+		}
+
+		thread_should_exit = false;
+		daemon_task = px4_task_spawn_cmd(module_name,
+						 SCHED_DEFAULT,
+						 SCHED_PRIORITY_DEFAULT,
+						 2000,
+						 load_mon_thread,
+						 (argv) ? (char *const *)&argv[2] : (char *const *)NULL);
+		return 0;
+	}
+
+	if (!strcmp(argv[1], "stop")) {
+		thread_should_exit = true;
+		return 0;
+	}
+
+	if (!strcmp(argv[1], "status")) {
+		if (thread_running) {
+			warnx("\trunning\n");
+		} else {
+			warnx("\tnot started\n");
+		}
+
+		return 0;
+	}
+
+	usage("unrecognized command");
+	return 1;
+}
+
diff --git a/src/modules/uORB/objects_common.cpp b/src/modules/uORB/objects_common.cpp
index a91d6ad45d..4bb34227b3 100644
--- a/src/modules/uORB/objects_common.cpp
+++ b/src/modules/uORB/objects_common.cpp
@@ -300,3 +300,6 @@ ORB_DEFINE(gps_inject_data, struct gps_inject_data_s);
 
 #include "topics/adc_report.h"
 ORB_DEFINE(adc_report, struct adc_report_s);
+
+#include "topics/cpuload.h"
+ORB_DEFINE(cpuload, struct cpuload_s);

From 535cea4e77380699f214b7bc4f2cfcdcddd61621 Mon Sep 17 00:00:00 2001
From: Jonathan Challinger <mr.challinger@gmail.com>
Date: Fri, 26 Feb 2016 18:14:03 -0800
Subject: [PATCH 0041/1230] commander: remove load from vehicle_status message

---
 msg/vehicle_status.msg              |  3 --
 src/modules/commander/commander.cpp | 45 +++++++++++++----------------
 2 files changed, 20 insertions(+), 28 deletions(-)

diff --git a/msg/vehicle_status.msg b/msg/vehicle_status.msg
index f5b1666085..4e8ec96553 100644
--- a/msg/vehicle_status.msg
+++ b/msg/vehicle_status.msg
@@ -70,6 +70,3 @@ bool mission_failure				# Set to true if mission could not continue/finish
 uint32 onboard_control_sensors_present
 uint32 onboard_control_sensors_enabled
 uint32 onboard_control_sensors_health
-
-float32 load					# processor load from 0 to 1
-
diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp
index 2318444e97..310e9dd264 100644
--- a/src/modules/commander/commander.cpp
+++ b/src/modules/commander/commander.cpp
@@ -99,6 +99,7 @@
 #include <uORB/topics/input_rc.h>
 #include <uORB/topics/vehicle_command_ack.h>
 #include <uORB/topics/mavlink_log.h>
+#include <uORB/topics/cpuload.h>
 
 #include <drivers/drv_led.h>
 #include <drivers/drv_hrt.h>
@@ -107,7 +108,6 @@
 #include <systemlib/param/param.h>
 #include <systemlib/systemlib.h>
 #include <systemlib/err.h>
-#include <systemlib/cpuload.h>
 #include <systemlib/rc_check.h>
 #include <geo/geo.h>
 #include <systemlib/state_table.h>
@@ -214,6 +214,7 @@ struct manual_control_setpoint_s sp_man = {};		///< the current manual control s
 static manual_control_setpoint_s _last_sp_man = {};	///< the manual control setpoint valid at the last mode switch
 
 static struct vtol_vehicle_status_s vtol_status = {};
+static struct cpuload_s cpuload = {};
 
 
 static uint8_t main_state_prev = 0;
@@ -257,7 +258,7 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe
 int commander_thread_main(int argc, char *argv[]);
 
 void control_status_leds(vehicle_status_s *status_local, const actuator_armed_s *actuator_armed, bool changed,
-			 battery_status_s *battery);
+			 battery_status_s *battery, const cpuload_s *cpuload_local);
 
 void get_circuit_breaker_params();
 
@@ -1368,8 +1369,6 @@ int commander_thread_main(int argc, char *argv[])
 	bool low_battery_voltage_actions_done = false;
 	bool critical_battery_voltage_actions_done = false;
 
-	hrt_abstime last_idle_time = 0;
-
 	bool status_changed = true;
 	bool param_init_forced = true;
 
@@ -1494,8 +1493,10 @@ int commander_thread_main(int argc, char *argv[])
 	memset(&vtol_status, 0, sizeof(vtol_status));
 	vtol_status.vtol_in_rw_mode = true;		//default for vtol is rotary wing
 
+	int cpuload_sub = orb_subscribe(ORB_ID(cpuload));
+	memset(&cpuload, 0, sizeof(cpuload));
 
-	control_status_leds(&status, &armed, true, &battery);
+	control_status_leds(&status, &armed, true, &battery, &cpuload);
 
 	/* now initialized */
 	commander_initialized = true;
@@ -1997,6 +1998,12 @@ int commander_thread_main(int argc, char *argv[])
 			main_state_before_rtl = internal_state.main_state;
 		}
 
+		orb_check(cpuload_sub, &updated);
+
+		if (updated) {
+			orb_copy(ORB_ID(cpuload), cpuload_sub, &cpuload);
+		}
+
 		/* update battery status */
 		orb_check(battery_sub, &updated);
 
@@ -2095,17 +2102,6 @@ int commander_thread_main(int argc, char *argv[])
 			orb_copy(ORB_ID(position_setpoint_triplet), pos_sp_triplet_sub, &pos_sp_triplet);
 		}
 
-		if (counter % (1000000 / COMMANDER_MONITORING_INTERVAL) == 0) {
-			/* compute system load */
-			uint64_t interval_runtime = system_load.tasks[0].total_runtime - last_idle_time;
-
-			if (last_idle_time > 0) {
-				status.load = 1.0f - ((float)interval_runtime / 1e6f);        //system load is time spent in non-idle
-			}
-
-			last_idle_time = system_load.tasks[0].total_runtime;
-		}
-
 		/* If in INIT state, try to proceed to STANDBY state */
 		if (!status_flags.condition_calibration_enabled && status.arming_state == vehicle_status_s::ARMING_STATE_INIT) {
 			arming_ret = arming_state_transition(&status,
@@ -2803,12 +2799,12 @@ int commander_thread_main(int argc, char *argv[])
 			/* blinking LED message, don't touch LEDs */
 			if (blink_state == 2) {
 				/* blinking LED message completed, restore normal state */
-				control_status_leds(&status, &armed, true, &battery);
+				control_status_leds(&status, &armed, true, &battery, &cpuload);
 			}
 
 		} else {
 			/* normal state */
-			control_status_leds(&status, &armed, status_changed, &battery);
+			control_status_leds(&status, &armed, status_changed, &battery, &cpuload);
 		}
 
 		status_changed = false;
@@ -2878,8 +2874,7 @@ check_valid(hrt_abstime timestamp, hrt_abstime timeout, bool valid_in, bool *val
 	}
 }
 
-void
-control_status_leds(vehicle_status_s *status_local, const actuator_armed_s *actuator_armed, bool changed, battery_status_s *battery_status)
+control_status_leds(vehicle_status_s *status_local, const actuator_armed_s *actuator_armed, bool changed, battery_status_s *battery, const cpuload_s *cpuload_local)
 {
 	/* driving rgbled */
 	if (changed) {
@@ -2950,7 +2945,7 @@ control_status_leds(vehicle_status_s *status_local, const actuator_armed_s *actu
 #endif
 
 	/* give system warnings on error LED, XXX maybe add memory usage warning too */
-	if (status_local->load > 0.95f) {
+	if (cpuload_local->load > 0.95f) {
 		if (leds_counter % 2 == 0) {
 			led_toggle(LED_AMBER);
 		}
@@ -3479,17 +3474,17 @@ set_control_mode()
 
 		control_mode.flag_control_rattitude_enabled = false;
 
-		control_mode.flag_control_acceleration_enabled = !offboard_control_mode.ignore_acceleration_force && 
-		  !status.in_transition_mode; 
+		control_mode.flag_control_acceleration_enabled = !offboard_control_mode.ignore_acceleration_force &&
+		  !status.in_transition_mode;
 
 		control_mode.flag_control_velocity_enabled = (!offboard_control_mode.ignore_velocity ||
-			!offboard_control_mode.ignore_position) && !status.in_transition_mode && 
+			!offboard_control_mode.ignore_position) && !status.in_transition_mode &&
 			!control_mode.flag_control_acceleration_enabled;
 
 		control_mode.flag_control_climb_rate_enabled = (!offboard_control_mode.ignore_velocity ||
 			!offboard_control_mode.ignore_position) && !control_mode.flag_control_acceleration_enabled;
 
-		control_mode.flag_control_position_enabled = !offboard_control_mode.ignore_position && !status.in_transition_mode && 
+		control_mode.flag_control_position_enabled = !offboard_control_mode.ignore_position && !status.in_transition_mode &&
 		  !control_mode.flag_control_acceleration_enabled;
 
 		control_mode.flag_control_altitude_enabled = (!offboard_control_mode.ignore_velocity ||

From 7ec37d5ffd380392cd36c24975affd84c2a4c102 Mon Sep 17 00:00:00 2001
From: Jonathan Challinger <mr.challinger@gmail.com>
Date: Fri, 26 Feb 2016 18:14:24 -0800
Subject: [PATCH 0042/1230] mavlink: subscribe to and use cpuload message
 instead of vehicle_status

---
 src/modules/mavlink/mavlink_messages.cpp | 9 +++++++--
 1 file changed, 7 insertions(+), 2 deletions(-)

diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp
index 3bad45356c..2d81b37120 100644
--- a/src/modules/mavlink/mavlink_messages.cpp
+++ b/src/modules/mavlink/mavlink_messages.cpp
@@ -77,6 +77,7 @@
 #include <uORB/topics/estimator_status.h>
 #include <uORB/topics/transponder_report.h>
 #include <uORB/topics/mavlink_log.h>
+#include <uORB/topics/cpuload.h>
 #include <drivers/drv_rc_input.h>
 #include <drivers/drv_pwm_output.h>
 #include <systemlib/err.h>
@@ -574,6 +575,7 @@ public:
 
 private:
 	MavlinkOrbSubscription *_status_sub;
+    MavlinkOrbSubscription *_cpuload_sub;
 	MavlinkOrbSubscription *_battery_status_sub;
 
 	/* do not allow top copying this class */
@@ -583,15 +585,18 @@ private:
 protected:
 	explicit MavlinkStreamSysStatus(Mavlink *mavlink) : MavlinkStream(mavlink),
 		_status_sub(_mavlink->add_orb_subscription(ORB_ID(vehicle_status))),
+		_cpuload_sub(_mavlink->add_orb_subscription(ORB_ID(cpuload))),
 		_battery_status_sub(_mavlink->add_orb_subscription(ORB_ID(battery_status)))
 	{}
 
 	void send(const hrt_abstime t)
 	{
 		struct vehicle_status_s status;
+		struct cpuload_s cpuload;
 		struct battery_status_s battery_status;
 
 		const bool updated_status = _status_sub->update(&status);
+		const bool updated_cpuload = _cpuload_sub->update(&cpuload);
 		const bool updated_battery = _battery_status_sub->update(&battery_status);
 
 		if (updated_status) {
@@ -602,14 +607,14 @@ protected:
 			}
 		}
 
-		if (updated_status || updated_battery) {
+		if (updated_status || updated_battery || updated_cpuload) {
 			mavlink_sys_status_t msg;
 
 
 			msg.onboard_control_sensors_present = status.onboard_control_sensors_present;
 			msg.onboard_control_sensors_enabled = status.onboard_control_sensors_enabled;
 			msg.onboard_control_sensors_health = status.onboard_control_sensors_health;
-			msg.load = status.load * 1000.0f;
+			msg.load = cpuload.load * 1000.0f;
 			msg.voltage_battery = battery_status.voltage_filtered_v * 1000.0f;
 			msg.current_battery = battery_status.current_filtered_a * 100.0f;
 			msg.battery_remaining = battery_status.remaining >= 0 ? battery_status.remaining * 100.0f : -1;

From 01305da7e7f28636a0b1d8af5ac1b5636f68ee3d Mon Sep 17 00:00:00 2001
From: Jonathan Challinger <mr.challinger@gmail.com>
Date: Fri, 26 Feb 2016 18:14:59 -0800
Subject: [PATCH 0043/1230] sdlog2: subscribe to and use cpuload message
 instead of vehicle_status

---
 src/modules/sdlog2/sdlog2.c | 13 ++++++++++++-
 1 file changed, 12 insertions(+), 1 deletion(-)

diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c
index 95b4c559c5..8dc3a76ecc 100644
--- a/src/modules/sdlog2/sdlog2.c
+++ b/src/modules/sdlog2/sdlog2.c
@@ -115,6 +115,7 @@
 #include <uORB/topics/ekf2_replay.h>
 #include <uORB/topics/vehicle_land_detected.h>
 #include <uORB/topics/commander_state.h>
+#include <uORB/topics/cpuload.h>
 
 #include <systemlib/systemlib.h>
 #include <systemlib/param/param.h>
@@ -1190,6 +1191,7 @@ int sdlog2_thread_main(int argc, char *argv[])
 		struct camera_trigger_s camera_trigger;
 		struct ekf2_replay_s replay;
 		struct vehicle_land_detected_s land_detected;
+		struct cpuload_s cpuload;
 	} buf;
 
 	memset(&buf, 0, sizeof(buf));
@@ -1298,6 +1300,7 @@ int sdlog2_thread_main(int argc, char *argv[])
 		int replay_sub;
 		int land_detected_sub;
 		int commander_state_sub;
+		int cpuload_sub;
 	} subs;
 
 	subs.cmd_sub = -1;
@@ -1339,6 +1342,7 @@ int sdlog2_thread_main(int argc, char *argv[])
 	subs.replay_sub = -1;
 	subs.land_detected_sub = -1;
 	subs.commander_state_sub = -1;
+	subs.cpuload_sub = -1;
 
 	/* add new topics HERE */
 
@@ -1508,7 +1512,6 @@ int sdlog2_thread_main(int argc, char *argv[])
 			log_msg.body.log_STAT.nav_state = buf_status.nav_state;
 			log_msg.body.log_STAT.arming_state = buf_status.arming_state;
 			log_msg.body.log_STAT.failsafe = (uint8_t) buf_status.failsafe;
-			log_msg.body.log_STAT.load = buf_status.load;
 			LOGBUFFER_WRITE_AND_COUNT(STAT);
 		}
 
@@ -2237,6 +2240,14 @@ int sdlog2_thread_main(int argc, char *argv[])
 			LOGBUFFER_WRITE_AND_COUNT(LAND);
 		}
 
+		/* --- LOAD --- */
+		if (copy_if_updated(ORB_ID(cpuload), &subs.cpuload_sub, &buf.cpuload)) {
+			log_msg.msg_type = LOG_LOAD_MSG;
+			log_msg.body.log_LOAD.cpu = buf.load.cpu;
+			LOGBUFFER_WRITE_AND_COUNT(LOAD);
+
+		}
+
 		pthread_mutex_lock(&logbuffer_mutex);
 
 		/* signal the other thread new data, but not yet unlock */

From e5ce9809c5834f668ace0aae5058a68a41b07528 Mon Sep 17 00:00:00 2001
From: Jonathan Challinger <mr.challinger@gmail.com>
Date: Sun, 28 Feb 2016 22:54:31 -0800
Subject: [PATCH 0044/1230] cmake: add load_mon wherever commander is built

---
 cmake/configs/nuttx_px4fmu-v1_default.cmake          | 1 +
 cmake/configs/nuttx_px4fmu-v2_default.cmake          | 1 +
 cmake/configs/nuttx_px4fmu-v2_ekf2.cmake             | 1 +
 cmake/configs/nuttx_px4fmu-v4_default.cmake          | 1 +
 cmake/configs/posix_eagle_hil.cmake                  | 1 +
 cmake/configs/posix_rpi2_default.cmake               | 1 +
 cmake/configs/posix_rpi2_release.cmake               | 3 ++-
 cmake/configs/posix_sdflight_default.cmake           | 1 +
 cmake/configs/posix_sitl_default.cmake               | 1 +
 cmake/configs/posix_sitl_ekf2.cmake                  | 1 +
 cmake/configs/qurt_eagle_hil.cmake                   | 1 +
 cmake/configs/qurt_eagle_legacy_driver_default.cmake | 1 +
 cmake/configs/qurt_eagle_travis.cmake                | 1 +
 cmake/configs/qurt_sdflight_default.cmake            | 1 +
 14 files changed, 15 insertions(+), 1 deletion(-)

diff --git a/cmake/configs/nuttx_px4fmu-v1_default.cmake b/cmake/configs/nuttx_px4fmu-v1_default.cmake
index 85c7e2f347..dfb672a5a8 100644
--- a/cmake/configs/nuttx_px4fmu-v1_default.cmake
+++ b/cmake/configs/nuttx_px4fmu-v1_default.cmake
@@ -61,6 +61,7 @@ set(config_module_list
 	# General system control
 	#
 	modules/commander
+	modules/load_mon
 	modules/navigator
 	modules/mavlink
 	modules/gpio_led
diff --git a/cmake/configs/nuttx_px4fmu-v2_default.cmake b/cmake/configs/nuttx_px4fmu-v2_default.cmake
index 97f9126004..a30cd9b381 100644
--- a/cmake/configs/nuttx_px4fmu-v2_default.cmake
+++ b/cmake/configs/nuttx_px4fmu-v2_default.cmake
@@ -72,6 +72,7 @@ set(config_module_list
 	# General system control
 	#
 	modules/commander
+	modules/load_mon
 	modules/navigator
 	modules/mavlink
 	modules/gpio_led
diff --git a/cmake/configs/nuttx_px4fmu-v2_ekf2.cmake b/cmake/configs/nuttx_px4fmu-v2_ekf2.cmake
index 6cc8c2d262..33daab5f77 100644
--- a/cmake/configs/nuttx_px4fmu-v2_ekf2.cmake
+++ b/cmake/configs/nuttx_px4fmu-v2_ekf2.cmake
@@ -69,6 +69,7 @@ set(config_module_list
 	# General system control
 	#
 	modules/commander
+	modules/load_mon
 	modules/navigator
 	modules/mavlink
 	modules/gpio_led
diff --git a/cmake/configs/nuttx_px4fmu-v4_default.cmake b/cmake/configs/nuttx_px4fmu-v4_default.cmake
index f060be5050..7c54edf15c 100644
--- a/cmake/configs/nuttx_px4fmu-v4_default.cmake
+++ b/cmake/configs/nuttx_px4fmu-v4_default.cmake
@@ -70,6 +70,7 @@ set(config_module_list
 	# General system control
 	#
 	modules/commander
+	modules/load_mon
 	modules/navigator
 	modules/mavlink
 	modules/gpio_led
diff --git a/cmake/configs/posix_eagle_hil.cmake b/cmake/configs/posix_eagle_hil.cmake
index 252de9718c..3e6274956f 100644
--- a/cmake/configs/posix_eagle_hil.cmake
+++ b/cmake/configs/posix_eagle_hil.cmake
@@ -29,6 +29,7 @@ set(config_module_list
 	modules/sdlog2
 	modules/simulator
 	modules/commander
+	modules/load_mon
 
 	lib/mathlib
 	lib/mathlib/math/filter
diff --git a/cmake/configs/posix_rpi2_default.cmake b/cmake/configs/posix_rpi2_default.cmake
index 071466b224..c67d241f9f 100644
--- a/cmake/configs/posix_rpi2_default.cmake
+++ b/cmake/configs/posix_rpi2_default.cmake
@@ -32,6 +32,7 @@ set(config_module_list
 	modules/dataman
 	modules/sdlog2
 	modules/commander
+	modules/load_mon
 	lib/controllib
 	lib/mathlib
 	lib/mathlib/math/filter
diff --git a/cmake/configs/posix_rpi2_release.cmake b/cmake/configs/posix_rpi2_release.cmake
index e42a1b8fd3..e4272e6ef4 100644
--- a/cmake/configs/posix_rpi2_release.cmake
+++ b/cmake/configs/posix_rpi2_release.cmake
@@ -4,7 +4,7 @@ if ("${RPI_TOOLCHAIN_DIR}" STREQUAL "")
 	set(RPI_TOOLCHAIN_DIR /opt/rpi_toolchain)
 endif()
 
-set(CMAKE_PROGRAM_PATH 
+set(CMAKE_PROGRAM_PATH
 	"${RPI_TOOLCHAIN_DIR}/gcc-linaro-arm-linux-gnueabihf-raspbian/bin"
 	${CMAKE_PROGRAM_PATH}
 	)
@@ -41,6 +41,7 @@ set(config_module_list
 	modules/dataman
 	modules/sdlog2
 	modules/commander
+	modules/load_mon
 	lib/controllib
 	lib/mathlib
 	lib/mathlib/math/filter
diff --git a/cmake/configs/posix_sdflight_default.cmake b/cmake/configs/posix_sdflight_default.cmake
index da51797a59..5d026346c7 100644
--- a/cmake/configs/posix_sdflight_default.cmake
+++ b/cmake/configs/posix_sdflight_default.cmake
@@ -40,6 +40,7 @@ set(config_module_list
 	modules/sdlog2
 	modules/simulator
 	modules/commander
+	modules/load_mon
 
 	lib/controllib
 	lib/mathlib
diff --git a/cmake/configs/posix_sitl_default.cmake b/cmake/configs/posix_sitl_default.cmake
index 11a211b9c0..a42385cd9d 100644
--- a/cmake/configs/posix_sitl_default.cmake
+++ b/cmake/configs/posix_sitl_default.cmake
@@ -51,6 +51,7 @@ set(config_module_list
 	modules/dataman
 	modules/sdlog2
 	modules/commander
+	modules/load_mon
 	lib/controllib
 	lib/mathlib
 	lib/mathlib/math/filter
diff --git a/cmake/configs/posix_sitl_ekf2.cmake b/cmake/configs/posix_sitl_ekf2.cmake
index 5cb53b3c22..7da1e16d4a 100644
--- a/cmake/configs/posix_sitl_ekf2.cmake
+++ b/cmake/configs/posix_sitl_ekf2.cmake
@@ -49,6 +49,7 @@ set(config_module_list
 	modules/dataman
 	modules/sdlog2
 	modules/commander
+	modules/load_mon
 	lib/controllib
 	lib/mathlib
 	lib/mathlib/math/filter
diff --git a/cmake/configs/qurt_eagle_hil.cmake b/cmake/configs/qurt_eagle_hil.cmake
index 469432818f..f2a77068e4 100644
--- a/cmake/configs/qurt_eagle_hil.cmake
+++ b/cmake/configs/qurt_eagle_hil.cmake
@@ -48,6 +48,7 @@ set(config_module_list
 	modules/systemlib/mixer
 	modules/uORB
 	modules/commander
+	modules/load_mon
 
 	#
 	# Libraries
diff --git a/cmake/configs/qurt_eagle_legacy_driver_default.cmake b/cmake/configs/qurt_eagle_legacy_driver_default.cmake
index 223f3b8a4f..1ff1845c32 100644
--- a/cmake/configs/qurt_eagle_legacy_driver_default.cmake
+++ b/cmake/configs/qurt_eagle_legacy_driver_default.cmake
@@ -58,6 +58,7 @@ set(config_module_list
 	modules/uORB
 	modules/commander
 	modules/land_detector
+	modules/load_mon
 
 	#
 	# PX4 drivers
diff --git a/cmake/configs/qurt_eagle_travis.cmake b/cmake/configs/qurt_eagle_travis.cmake
index 01b87f6bb2..eee24eda15 100644
--- a/cmake/configs/qurt_eagle_travis.cmake
+++ b/cmake/configs/qurt_eagle_travis.cmake
@@ -55,6 +55,7 @@ set(config_module_list
 	modules/systemlib/mixer
 	modules/uORB
 	modules/commander
+	modules/load_mon
 
 	#
 	# Libraries
diff --git a/cmake/configs/qurt_sdflight_default.cmake b/cmake/configs/qurt_sdflight_default.cmake
index 3ea8ad37b4..c646a6dfb6 100644
--- a/cmake/configs/qurt_sdflight_default.cmake
+++ b/cmake/configs/qurt_sdflight_default.cmake
@@ -55,6 +55,7 @@ set(config_module_list
 	modules/uORB
 	modules/commander
 	modules/land_detector
+	modules/load_mon
 
 	#
 	# PX4 drivers

From 939f04c80dcd293165ebad90465046dad5c47d73 Mon Sep 17 00:00:00 2001
From: Julian Oes <julian@oes.ch>
Date: Fri, 29 Apr 2016 09:57:59 +0200
Subject: [PATCH 0045/1230] load_mon: small comment fix

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

diff --git a/src/modules/load_mon/load_mon.cpp b/src/modules/load_mon/load_mon.cpp
index 04a3fb9c1b..ac136344bd 100644
--- a/src/modules/load_mon/load_mon.cpp
+++ b/src/modules/load_mon/load_mon.cpp
@@ -32,7 +32,7 @@
  ****************************************************************************/
 
 /**
- * @file load_mon.c
+ * @file load_mon.cpp
  *
  * @author Jonathan Challinger <jonathan@3drobotics.com>
  */

From 888b517d62a049234c7e7a8b528b18c1266070bd Mon Sep 17 00:00:00 2001
From: Julian Oes <julian@oes.ch>
Date: Fri, 29 Apr 2016 10:04:42 +0200
Subject: [PATCH 0046/1230] load_mon: correct copyright year

---
 src/modules/load_mon/CMakeLists.txt | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/src/modules/load_mon/CMakeLists.txt b/src/modules/load_mon/CMakeLists.txt
index f714895a24..2cd5952ef8 100644
--- a/src/modules/load_mon/CMakeLists.txt
+++ b/src/modules/load_mon/CMakeLists.txt
@@ -1,6 +1,6 @@
 ############################################################################
 #
-#   Copyright (c) 2015 PX4 Development Team. All rights reserved.
+#   Copyright (c) 2016 PX4 Development Team. All rights reserved.
 #
 # Redistribution and use in source and binary forms, with or without
 # modification, are permitted provided that the following conditions
@@ -39,4 +39,4 @@ px4_add_module(
 	DEPENDS
 		platforms__common
 	)
-# vim: set noet ft=cmake fenc=utf-8 ff=unix : 
+# vim: set noet ft=cmake fenc=utf-8 ff=unix :

From a94a409f5fe12cb35f4a3d7e784df97530578c13 Mon Sep 17 00:00:00 2001
From: Julian Oes <julian@oes.ch>
Date: Fri, 29 Apr 2016 14:26:49 +0200
Subject: [PATCH 0047/1230] commander: got rid of leftover system_load

---
 src/modules/commander/commander.cpp | 2 --
 1 file changed, 2 deletions(-)

diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp
index 310e9dd264..9b3a317531 100644
--- a/src/modules/commander/commander.cpp
+++ b/src/modules/commander/commander.cpp
@@ -133,8 +133,6 @@
 #endif
 static const int ERROR = -1;
 
-extern struct system_load_s system_load;
-
 static constexpr uint8_t COMMANDER_MAX_GPS_NOISE = 60;		/**< Maximum percentage signal to noise ratio allowed for GPS reception */
 
 /* Decouple update interval and hysteris counters, all depends on intervals */

From 3451e901a55d2ba9a7b6ba394b744bf6a2cb50d9 Mon Sep 17 00:00:00 2001
From: Julian Oes <julian@oes.ch>
Date: Fri, 29 Apr 2016 14:27:16 +0200
Subject: [PATCH 0048/1230] rcS: start load_mon on NuttX startup

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

diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS
index 3786a8ec98..1e2bc91bb9 100644
--- a/ROMFS/px4fmu_common/init.d/rcS
+++ b/ROMFS/px4fmu_common/init.d/rcS
@@ -330,6 +330,11 @@ then
 	    commander start
 	fi
 
+	#
+	# Start CPU load monitor
+	#
+	load_mon start
+
 	#
 	# Start primary output
 	#

From 43d76f5e17ef254158a8f0f97d6ac3ea530caba8 Mon Sep 17 00:00:00 2001
From: Julian Oes <julian@oes.ch>
Date: Fri, 29 Apr 2016 14:29:31 +0200
Subject: [PATCH 0049/1230] load_mon: use work queue instead of a whole task

---
 src/modules/load_mon/load_mon.cpp | 202 ++++++++++++++++++++++--------
 1 file changed, 151 insertions(+), 51 deletions(-)

diff --git a/src/modules/load_mon/load_mon.cpp b/src/modules/load_mon/load_mon.cpp
index ac136344bd..418b4e8012 100644
--- a/src/modules/load_mon/load_mon.cpp
+++ b/src/modules/load_mon/load_mon.cpp
@@ -35,6 +35,7 @@
  * @file load_mon.cpp
  *
  * @author Jonathan Challinger <jonathan@3drobotics.com>
+ * @author Julian Oes <julian@oes.ch
  */
 
 #include <stdio.h>
@@ -42,9 +43,8 @@
 #include <string.h>
 #include <unistd.h>
 
-#include <poll.h>
-
 #include <px4_config.h>
+#include <px4_workqueue.h>
 
 #include <drivers/drv_hrt.h>
 
@@ -55,56 +55,124 @@
 #include <uORB/uORB.h>
 #include <uORB/topics/cpuload.h>
 
-#define LOAD_MON_INTERVAL_SEC 1.0f
-#define LOAD_MON_INTERVAL_US (LOAD_MON_INTERVAL_SEC*1.0e6f)
+extern struct system_load_s system_load;
+
+
+namespace load_mon
+{
 
 extern "C" __EXPORT int load_mon_main(int argc, char *argv[]);
-int load_mon_thread(int argc, char *argv[]);
 
-static const char *module_name = "load_mon";
-static bool thread_should_exit = false;		/**< daemon exit flag */
-static bool thread_running = false;		/**< daemon status flag */
-static int daemon_task;				/**< Handle of daemon task / thread */
+// Run it at 1 Hz.
+const unsigned LOAD_MON_INTERVAL_US = 1000000;
 
-
-extern struct system_load_s system_load;
-struct cpuload_s cpuload;
-static orb_advert_t cpuload_publish_handle = nullptr;
-
-/**
- * Mainloop of load_mon.
- */
-int load_mon_thread(int argc, char *argv[])
+class LoadMon
 {
-	warnx("[%s] starting\n", module_name);
-	thread_running = true;
+public:
+	LoadMon();
+	~LoadMon();
 
-	hrt_abstime last_idle_time = system_load.tasks[0].total_runtime;
+	/* Start the load monitoring
+	 *
+	 * @return 0 if successfull, -1 on error. */
+	int start();
 
-	while (!thread_should_exit) {
-		usleep(LOAD_MON_INTERVAL_US);
+	/* Stop the load monitoring */
+	void stop();
 
-		/* compute system load */
-		uint64_t interval_idletime = system_load.tasks[0].total_runtime - last_idle_time;
-		last_idle_time = system_load.tasks[0].total_runtime;
+	/* Trampoline for the work queue. */
+	static void cycle_trampoline(void *arg);
 
-		cpuload.timestamp = hrt_absolute_time();
-		cpuload.load = 1.0f - (float)interval_idletime / LOAD_MON_INTERVAL_US;
+	bool isRunning() { return _taskIsRunning; }
 
-		if (cpuload_publish_handle == nullptr) {
-			cpuload_publish_handle = orb_advertise(ORB_ID(cpuload), &cpuload);
-		} else {
-			orb_publish(ORB_ID(cpuload), cpuload_publish_handle, &cpuload);
-		}
+private:
+	/* Do a compute and schedule the next cycle. */
+	void _cycle();
+
+	/* Do a calculation of the CPU load and publish it. */
+	void _compute();
+
+	bool _taskShouldExit;
+	bool _taskIsRunning;
+	struct work_s _work;
+
+	struct cpuload_s _cpuload;
+	orb_advert_t _cpuload_pub;
+	hrt_abstime _last_idle_time;
+};
+
+
+LoadMon::LoadMon() :
+	_taskShouldExit(false),
+	_taskIsRunning(false),
+	_work{},
+	_cpuload{},
+	_cpuload_pub(nullptr),
+	_last_idle_time(0)
+{}
+
+LoadMon::~LoadMon()
+{
+	work_cancel(HPWORK, &_work);
+	_taskIsRunning = false;
+}
+
+int LoadMon::start()
+{
+	/* Schedule a cycle to start things. */
+	return work_queue(HPWORK, &_work, (worker_t)&LoadMon::cycle_trampoline, this, 0);
+}
+
+void LoadMon::stop()
+{
+	_taskShouldExit = true;
+}
+
+void
+LoadMon::cycle_trampoline(void *arg)
+{
+	LoadMon *dev = reinterpret_cast<LoadMon *>(arg);
+
+	dev->_cycle();
+}
+
+void LoadMon::_cycle()
+{
+	_taskIsRunning = true;
+
+	_compute();
+
+	if (!_taskShouldExit) {
+		work_queue(HPWORK, &_work, (worker_t)&LoadMon::cycle_trampoline, this,
+			   USEC2TICK(LOAD_MON_INTERVAL_US));
+	}
+}
+
+void LoadMon::_compute()
+{
+	if (_last_idle_time == 0) {
+		/* Just get the time in the first iteration */
+		_last_idle_time = system_load.tasks[0].total_runtime;
+		return;
 	}
 
-	warnx("[%s] exiting.\n", module_name);
+	/* compute system load */
+	const hrt_abstime interval_idletime = system_load.tasks[0].total_runtime - _last_idle_time;
+	_last_idle_time = system_load.tasks[0].total_runtime;
 
-	thread_running = false;
+	_cpuload.timestamp = hrt_absolute_time();
+	_cpuload.load = 1.0f - (float)interval_idletime / (float)LOAD_MON_INTERVAL_US;
 
-	return 0;
+	if (_cpuload_pub == nullptr) {
+		_cpuload_pub = orb_advertise(ORB_ID(cpuload), &_cpuload);
+
+	} else {
+		orb_publish(ORB_ID(cpuload), _cpuload_pub, &_cpuload);
+	}
 }
 
+
+
 /**
  * Print the correct usage.
  */
@@ -114,12 +182,15 @@ static void
 usage(const char *reason)
 {
 	if (reason) {
-		warnx("%s\n", reason);
+		PX4_ERR("%s", reason);
 	}
 
-	warnx("usage: %s {start|stop|status} [-p <additional params>]\n\n", module_name);
+	PX4_INFO("usage: load_mon {start|stop|status}");
 }
 
+
+static LoadMon *load_mon = nullptr;
+
 /**
  * The daemon app only briefly exists to start
  * the background job. The stack size assigned in the
@@ -137,32 +208,60 @@ int load_mon_main(int argc, char *argv[])
 
 	if (!strcmp(argv[1], "start")) {
 
-		if (thread_running) {
-			warnx("daemon already running\n");
+		if (load_mon != nullptr && load_mon->isRunning()) {
+			PX4_WARN("already running");
 			/* this is not an error */
 			return 0;
 		}
 
-		thread_should_exit = false;
-		daemon_task = px4_task_spawn_cmd(module_name,
-						 SCHED_DEFAULT,
-						 SCHED_PRIORITY_DEFAULT,
-						 2000,
-						 load_mon_thread,
-						 (argv) ? (char *const *)&argv[2] : (char *const *)NULL);
+		load_mon = new LoadMon();
+
+		// Check if alloc worked.
+		if (load_mon == nullptr) {
+			PX4_ERR("alloc failed");
+			return -1;
+		}
+
+		int ret = load_mon->start();
+
+		if (ret != 0) {
+			PX4_ERR("start failed");
+		}
+
 		return 0;
 	}
 
 	if (!strcmp(argv[1], "stop")) {
-		thread_should_exit = true;
+
+		if (load_mon == nullptr || load_mon->isRunning()) {
+			PX4_WARN("not running");
+			/* this is not an error */
+			return 0;
+		}
+
+		load_mon->stop();
+
+		// Wait for task to die
+		int i = 0;
+
+		do {
+			/* wait up to 3s */
+			usleep(100000);
+
+		} while (load_mon->isRunning() && ++i < 30);
+
+		delete load_mon;
+		load_mon = nullptr;
+
 		return 0;
 	}
 
 	if (!strcmp(argv[1], "status")) {
-		if (thread_running) {
-			warnx("\trunning\n");
+		if (load_mon != nullptr && load_mon->isRunning()) {
+			PX4_INFO("running");
+
 		} else {
-			warnx("\tnot started\n");
+			PX4_INFO("not running\n");
 		}
 
 		return 0;
@@ -172,3 +271,4 @@ int load_mon_main(int argc, char *argv[])
 	return 1;
 }
 
+} // namespace load_mon

From b965554bdde98ad3788c1ff26d060412c6e6eef4 Mon Sep 17 00:00:00 2001
From: Julian Oes <julian@oes.ch>
Date: Fri, 29 Apr 2016 14:30:39 +0200
Subject: [PATCH 0050/1230] sdlog2: move main state, fix formatting, fix bug

Since the vehicle_status topic has been split up, we can't publish it
together with the the split cpuload and commander_state topics.

Therefore, the log field STAT.MainState will change to COMM.MainState
because it is only the internal commander state. Important to the
outside is STAT.NavState.

Likewise, the log field STAT.Load becomes LOAD.CPU.
---
 src/modules/sdlog2/sdlog2.c          | 20 ++++++++++++++++++++
 src/modules/sdlog2/sdlog2_messages.h | 19 ++++++++++++++++---
 2 files changed, 36 insertions(+), 3 deletions(-)

diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c
index 8dc3a76ecc..4be40211aa 100644
--- a/src/modules/sdlog2/sdlog2.c
+++ b/src/modules/sdlog2/sdlog2.c
@@ -1211,6 +1211,8 @@ int sdlog2_thread_main(int argc, char *argv[])
 			struct log_GPS_s log_GPS;
 			struct log_ATTC_s log_ATTC;
 			struct log_STAT_s log_STAT;
+			struct log_LOAD_s log_LOAD;
+			struct log_COMM_s log_COMM;
 			struct log_VTOL_s log_VTOL;
 			struct log_RC_s log_RC;
 			struct log_OUT_s log_OUT;
@@ -1515,6 +1517,24 @@ int sdlog2_thread_main(int argc, char *argv[])
 			LOGBUFFER_WRITE_AND_COUNT(STAT);
 		}
 
+		/* --- COMMANDER INTERNAL STATE --- */
+		bool commander_state_updated = copy_if_updated(ORB_ID(commander_state), &subs.commander_state_sub,
+							       &buf.commander_state);
+
+		if (commander_state_updated) {
+			log_msg.msg_type = LOG_COMM_MSG;
+			log_msg.body.log_COMM.main_state = buf.commander_state.main_state;
+			LOGBUFFER_WRITE_AND_COUNT(COMM);
+		}
+
+
+		bool cpuload_updated = copy_if_updated(ORB_ID(cpuload), &subs.cpuload_sub, &buf.cpuload);
+		if (cpuload_updated) {
+			log_msg.msg_type = LOG_LOAD_MSG;
+			log_msg.body.log_LOAD.cpu_load = buf.cpuload.load;
+			LOGBUFFER_WRITE_AND_COUNT(LOAD);
+		}
+
 		/* --- EKF2 REPLAY --- */
 		if (log_type == LOG_TYPE_ALL || log_type == LOG_TYPE_REPLAY_ONLY) {
 
diff --git a/src/modules/sdlog2/sdlog2_messages.h b/src/modules/sdlog2/sdlog2_messages.h
index c5dbffe6f3..e0c934e629 100644
--- a/src/modules/sdlog2/sdlog2_messages.h
+++ b/src/modules/sdlog2/sdlog2_messages.h
@@ -177,11 +177,9 @@ struct log_ATTC_s {
 /* --- STAT - VEHICLE STATE --- */
 #define LOG_STAT_MSG 10
 struct log_STAT_s {
-	uint8_t main_state;
 	uint8_t nav_state;
 	uint8_t arming_state;
 	uint8_t failsafe;
-	float load;
 };
 
 /* --- RC - RC INPUT CHANNELS --- */
@@ -597,11 +595,24 @@ struct log_CAMT_s {
 	uint32_t seq;
 };
 
+/* --- LAND DETECTOR --- */
 #define LOG_LAND_MSG 57
 struct log_LAND_s {
 	uint8_t landed;
 };
 
+/* --- SYSTEM LOAD --- */
+#define LOG_LOAD_MSG 58
+struct log_LOAD_s {
+	float cpu_load;
+};
+
+/* --- COMMANDER INTERNAL STATE --- */
+#define LOG_COMM_MSG 59
+struct log_COMM_s {
+	uint8_t main_state;
+};
+
 /********** SYSTEM MESSAGES, ID > 0x80 **********/
 
 /* --- TIME - TIME STAMP --- */
@@ -643,7 +654,7 @@ static const struct log_format_s log_formats[] = {
 	LOG_FORMAT(GPS, "QBffLLfffffBHHH",	"GPSTime,Fix,EPH,EPV,Lat,Lon,Alt,VelN,VelE,VelD,Cog,nSat,SNR,N,J"),
 	LOG_FORMAT_S(ATTC, ATTC, "ffff",		"Roll,Pitch,Yaw,Thrust"),
 	LOG_FORMAT_S(ATC1, ATTC, "ffff",		"Roll,Pitch,Yaw,Thrust"),
-	LOG_FORMAT(STAT, "BBBBf",		"MainState,NavState,ArmS,Failsafe,Load"),
+	LOG_FORMAT(STAT, "BBB",		"NavState,ArmS,Failsafe"),
 	LOG_FORMAT(VTOL, "fBBB",		"Arsp,RwMode,TransMode,Failsafe"),
 	LOG_FORMAT(CTS, "fffffff", "Vx_b,Vy_b,Vz_b,Vinf,P,Q,R"),
 	LOG_FORMAT(RC, "ffffffffffffBBBL",		"C0,C1,C2,C3,C4,C5,C6,C7,C8,C9,C10,C11,RSSI,CNT,Lost,Drop"),
@@ -688,6 +699,8 @@ static const struct log_format_s log_formats[] = {
 	LOG_FORMAT(RPL4, "Qf", "Trng,rng"),
 	LOG_FORMAT(RPL6, "Qfffff", "Tasp,inAsp,trAsp,ufAsp,tpAsp,confAsp"),
 	LOG_FORMAT(LAND, "B", "Landed"),
+	LOG_FORMAT(LOAD, "f", "CPU"),
+	LOG_FORMAT(COMM, "B", "MainState"),
 	/* system-level messages, ID >= 0x80 */
 	/* FMT: don't write format of format message, it's useless */
 	LOG_FORMAT(TIME, "Q", "StartTime"),

From afbdec1742a83721414b15d79fb2fabe90d30357 Mon Sep 17 00:00:00 2001
From: Julian Oes <julian@oes.ch>
Date: Wed, 4 May 2016 08:22:00 +0200
Subject: [PATCH 0051/1230] mavlink: whitespace

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

diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp
index 2d81b37120..39e47720f4 100644
--- a/src/modules/mavlink/mavlink_messages.cpp
+++ b/src/modules/mavlink/mavlink_messages.cpp
@@ -575,7 +575,7 @@ public:
 
 private:
 	MavlinkOrbSubscription *_status_sub;
-    MavlinkOrbSubscription *_cpuload_sub;
+	MavlinkOrbSubscription *_cpuload_sub;
 	MavlinkOrbSubscription *_battery_status_sub;
 
 	/* do not allow top copying this class */

From 30b6f9ff6c592b9f789bf58f48ad862ef0639ad8 Mon Sep 17 00:00:00 2001
From: Julian Oes <julian@oes.ch>
Date: Wed, 4 May 2016 08:22:33 +0200
Subject: [PATCH 0052/1230] sdlog2: more merge conflict resolving

---
 src/modules/sdlog2/sdlog2.c          | 23 ++---------------------
 src/modules/sdlog2/sdlog2_messages.h | 10 ++--------
 2 files changed, 4 insertions(+), 29 deletions(-)

diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c
index 4be40211aa..08137f4056 100644
--- a/src/modules/sdlog2/sdlog2.c
+++ b/src/modules/sdlog2/sdlog2.c
@@ -1211,8 +1211,6 @@ int sdlog2_thread_main(int argc, char *argv[])
 			struct log_GPS_s log_GPS;
 			struct log_ATTC_s log_ATTC;
 			struct log_STAT_s log_STAT;
-			struct log_LOAD_s log_LOAD;
-			struct log_COMM_s log_COMM;
 			struct log_VTOL_s log_VTOL;
 			struct log_RC_s log_RC;
 			struct log_OUT_s log_OUT;
@@ -1253,6 +1251,7 @@ int sdlog2_thread_main(int argc, char *argv[])
 			struct log_RPL4_s log_RPL4;
 			struct log_RPL6_s log_RPL6;
 			struct log_LAND_s log_LAND;
+			struct log_LOAD_s log_LOAD;
 		} body;
 	} log_msg = {
 		LOG_PACKET_HEADER_INIT(0)
@@ -1517,24 +1516,6 @@ int sdlog2_thread_main(int argc, char *argv[])
 			LOGBUFFER_WRITE_AND_COUNT(STAT);
 		}
 
-		/* --- COMMANDER INTERNAL STATE --- */
-		bool commander_state_updated = copy_if_updated(ORB_ID(commander_state), &subs.commander_state_sub,
-							       &buf.commander_state);
-
-		if (commander_state_updated) {
-			log_msg.msg_type = LOG_COMM_MSG;
-			log_msg.body.log_COMM.main_state = buf.commander_state.main_state;
-			LOGBUFFER_WRITE_AND_COUNT(COMM);
-		}
-
-
-		bool cpuload_updated = copy_if_updated(ORB_ID(cpuload), &subs.cpuload_sub, &buf.cpuload);
-		if (cpuload_updated) {
-			log_msg.msg_type = LOG_LOAD_MSG;
-			log_msg.body.log_LOAD.cpu_load = buf.cpuload.load;
-			LOGBUFFER_WRITE_AND_COUNT(LOAD);
-		}
-
 		/* --- EKF2 REPLAY --- */
 		if (log_type == LOG_TYPE_ALL || log_type == LOG_TYPE_REPLAY_ONLY) {
 
@@ -2263,7 +2244,7 @@ int sdlog2_thread_main(int argc, char *argv[])
 		/* --- LOAD --- */
 		if (copy_if_updated(ORB_ID(cpuload), &subs.cpuload_sub, &buf.cpuload)) {
 			log_msg.msg_type = LOG_LOAD_MSG;
-			log_msg.body.log_LOAD.cpu = buf.load.cpu;
+			log_msg.body.log_LOAD.cpu_load = buf.cpuload.load;
 			LOGBUFFER_WRITE_AND_COUNT(LOAD);
 
 		}
diff --git a/src/modules/sdlog2/sdlog2_messages.h b/src/modules/sdlog2/sdlog2_messages.h
index e0c934e629..7778cf99df 100644
--- a/src/modules/sdlog2/sdlog2_messages.h
+++ b/src/modules/sdlog2/sdlog2_messages.h
@@ -177,6 +177,7 @@ struct log_ATTC_s {
 /* --- STAT - VEHICLE STATE --- */
 #define LOG_STAT_MSG 10
 struct log_STAT_s {
+	uint8_t main_state;
 	uint8_t nav_state;
 	uint8_t arming_state;
 	uint8_t failsafe;
@@ -607,12 +608,6 @@ struct log_LOAD_s {
 	float cpu_load;
 };
 
-/* --- COMMANDER INTERNAL STATE --- */
-#define LOG_COMM_MSG 59
-struct log_COMM_s {
-	uint8_t main_state;
-};
-
 /********** SYSTEM MESSAGES, ID > 0x80 **********/
 
 /* --- TIME - TIME STAMP --- */
@@ -654,7 +649,7 @@ static const struct log_format_s log_formats[] = {
 	LOG_FORMAT(GPS, "QBffLLfffffBHHH",	"GPSTime,Fix,EPH,EPV,Lat,Lon,Alt,VelN,VelE,VelD,Cog,nSat,SNR,N,J"),
 	LOG_FORMAT_S(ATTC, ATTC, "ffff",		"Roll,Pitch,Yaw,Thrust"),
 	LOG_FORMAT_S(ATC1, ATTC, "ffff",		"Roll,Pitch,Yaw,Thrust"),
-	LOG_FORMAT(STAT, "BBB",		"NavState,ArmS,Failsafe"),
+	LOG_FORMAT(STAT, "BBBB",		"MainState,NavState,ArmS,Failsafe"),
 	LOG_FORMAT(VTOL, "fBBB",		"Arsp,RwMode,TransMode,Failsafe"),
 	LOG_FORMAT(CTS, "fffffff", "Vx_b,Vy_b,Vz_b,Vinf,P,Q,R"),
 	LOG_FORMAT(RC, "ffffffffffffBBBL",		"C0,C1,C2,C3,C4,C5,C6,C7,C8,C9,C10,C11,RSSI,CNT,Lost,Drop"),
@@ -700,7 +695,6 @@ static const struct log_format_s log_formats[] = {
 	LOG_FORMAT(RPL6, "Qfffff", "Tasp,inAsp,trAsp,ufAsp,tpAsp,confAsp"),
 	LOG_FORMAT(LAND, "B", "Landed"),
 	LOG_FORMAT(LOAD, "f", "CPU"),
-	LOG_FORMAT(COMM, "B", "MainState"),
 	/* system-level messages, ID >= 0x80 */
 	/* FMT: don't write format of format message, it's useless */
 	LOG_FORMAT(TIME, "Q", "StartTime"),

From 352d0992495df9518a5df0551f458018d6ab255a Mon Sep 17 00:00:00 2001
From: Julian Oes <julian@oes.ch>
Date: Fri, 6 May 2016 11:10:16 +0200
Subject: [PATCH 0053/1230] commander: fix merge conflict mistake

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

diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp
index 9b3a317531..0c645b5e22 100644
--- a/src/modules/commander/commander.cpp
+++ b/src/modules/commander/commander.cpp
@@ -2872,6 +2872,7 @@ check_valid(hrt_abstime timestamp, hrt_abstime timeout, bool valid_in, bool *val
 	}
 }
 
+void
 control_status_leds(vehicle_status_s *status_local, const actuator_armed_s *actuator_armed, bool changed, battery_status_s *battery, const cpuload_s *cpuload_local)
 {
 	/* driving rgbled */

From a69393b191ce41cbd4945d67c69eac586dc79c08 Mon Sep 17 00:00:00 2001
From: Julian Oes <julian@oes.ch>
Date: Fri, 6 May 2016 11:11:29 +0200
Subject: [PATCH 0054/1230] commander: fix shadowing errors

---
 src/modules/commander/commander.cpp | 8 ++++----
 1 file changed, 4 insertions(+), 4 deletions(-)

diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp
index 0c645b5e22..11daf70d1e 100644
--- a/src/modules/commander/commander.cpp
+++ b/src/modules/commander/commander.cpp
@@ -256,7 +256,7 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe
 int commander_thread_main(int argc, char *argv[]);
 
 void control_status_leds(vehicle_status_s *status_local, const actuator_armed_s *actuator_armed, bool changed,
-			 battery_status_s *battery, const cpuload_s *cpuload_local);
+			 battery_status_s *battery_local, const cpuload_s *cpuload_local);
 
 void get_circuit_breaker_params();
 
@@ -2873,7 +2873,7 @@ check_valid(hrt_abstime timestamp, hrt_abstime timeout, bool valid_in, bool *val
 }
 
 void
-control_status_leds(vehicle_status_s *status_local, const actuator_armed_s *actuator_armed, bool changed, battery_status_s *battery, const cpuload_s *cpuload_local)
+control_status_leds(vehicle_status_s *status_local, const actuator_armed_s *actuator_armed, bool changed, battery_status_s *battery_local, const cpuload_s *cpuload_local)
 {
 	/* driving rgbled */
 	if (changed) {
@@ -2906,9 +2906,9 @@ control_status_leds(vehicle_status_s *status_local, const actuator_armed_s *actu
 			/* set color */
 			if (status.failsafe) {
 				rgbled_set_color(RGBLED_COLOR_PURPLE);
-			} else if (battery_status->warning == battery_status_s::BATTERY_WARNING_LOW) {
+			} else if (battery_local->warning == battery_status_s::BATTERY_WARNING_LOW) {
 				rgbled_set_color(RGBLED_COLOR_AMBER);
-			} else if (battery_status->warning == battery_status_s::BATTERY_WARNING_CRITICAL) {
+			} else if (battery_local->warning == battery_status_s::BATTERY_WARNING_CRITICAL) {
 				rgbled_set_color(RGBLED_COLOR_RED);
 			} else {
 				if (status_flags.condition_home_position_valid && status_flags.condition_global_position_valid) {

From 54b3995175d86bcbb23e8c7e1b94aca9d7e76b35 Mon Sep 17 00:00:00 2001
From: Julian Oes <julian@oes.ch>
Date: Wed, 11 May 2016 16:51:55 +0200
Subject: [PATCH 0055/1230] load_mon: added missing include

USEC2TICK was not found.
---
 src/modules/load_mon/load_mon.cpp | 1 +
 1 file changed, 1 insertion(+)

diff --git a/src/modules/load_mon/load_mon.cpp b/src/modules/load_mon/load_mon.cpp
index 418b4e8012..690e955a38 100644
--- a/src/modules/load_mon/load_mon.cpp
+++ b/src/modules/load_mon/load_mon.cpp
@@ -45,6 +45,7 @@
 
 #include <px4_config.h>
 #include <px4_workqueue.h>
+#include <px4_defines.h>
 
 #include <drivers/drv_hrt.h>
 

From 080a136e502a8d846634e7e2293c190a6546aa2e Mon Sep 17 00:00:00 2001
From: Julian Oes <julian@oes.ch>
Date: Wed, 11 May 2016 21:37:53 +0200
Subject: [PATCH 0056/1230] integrator: add function to return filtered data

Instead of only being able to get the integral and its integration time,
it can also be handy to get the integral divided/differentiated by the
the integration time. This data is then just filtered by the integrator.
---
 src/drivers/device/integrator.cpp | 14 ++++++++++++++
 src/drivers/device/integrator.h   | 11 +++++++++++
 2 files changed, 25 insertions(+)

diff --git a/src/drivers/device/integrator.cpp b/src/drivers/device/integrator.cpp
index 60f3af5bb4..072e3096a2 100644
--- a/src/drivers/device/integrator.cpp
+++ b/src/drivers/device/integrator.cpp
@@ -143,6 +143,20 @@ Integrator::get(bool reset, uint64_t &integral_dt)
 	return val;
 }
 
+math::Vector<3>
+Integrator::get_and_filtered(bool reset, uint64_t &integral_dt, math::Vector<3> &filtered_val)
+{
+	// Do the usual get with reset first but don't return yet.
+	math::Vector<3> ret_integral = get(reset, integral_dt);
+
+	// Because we need both the integral and the integral_dt.
+	filtered_val(0) = ret_integral(0) * 1000000 / integral_dt;
+	filtered_val(1) = ret_integral(1) * 1000000 / integral_dt;
+	filtered_val(2) = ret_integral(2) * 1000000 / integral_dt;
+
+	return ret_integral;
+}
+
 void
 Integrator::_reset(uint64_t &integral_dt)
 {
diff --git a/src/drivers/device/integrator.h b/src/drivers/device/integrator.h
index 11aca58e80..48a232066f 100644
--- a/src/drivers/device/integrator.h
+++ b/src/drivers/device/integrator.h
@@ -86,6 +86,17 @@ public:
 	 */
 	math::Vector<3>		get(bool reset, uint64_t &integral_dt);
 
+	/**
+	 * Get the current integral and reset the integrator if needed. Additionally give the
+	 * integral over the samples differentiated by the integration time (mean filtered values).
+	 *
+	 * @param reset	    	Reset the integral to zero.
+	 * @param integral_dt	Get the dt in us of the current integration (only if reset).
+	 * @param filtered_val	The integral differentiated by the integration time.
+	 * @return		the integral since the last read-reset
+	 */
+	math::Vector<3>		get_and_filtered(bool reset, uint64_t &integral_dt, math::Vector<3> &filtered_val);
+
 private:
 	uint64_t _auto_reset_interval;			/**< the interval after which the content will be published
 							     and the integrator reset, 0 if no auto-reset */

From 89661b2d9a1b12d01d1eff1175d6d94472e6b6a0 Mon Sep 17 00:00:00 2001
From: Julian Oes <julian@oes.ch>
Date: Wed, 11 May 2016 21:41:50 +0200
Subject: [PATCH 0057/1230] df_mpu9250_wrapper: use all accel/gyro data

Use the data which has been filtered by the integrator for the
instantanous values instead of only 1 out of 32 samples.

This is to better support estimators and modules other than ekf2 which
uses the integrated gyro/accel values anyway.
---
 .../df_mpu9250_wrapper/df_mpu9250_wrapper.cpp | 62 +++++++++++--------
 1 file changed, 35 insertions(+), 27 deletions(-)

diff --git a/src/platforms/posix/drivers/df_mpu9250_wrapper/df_mpu9250_wrapper.cpp b/src/platforms/posix/drivers/df_mpu9250_wrapper/df_mpu9250_wrapper.cpp
index ae8c9b65ca..0f4fe127c0 100644
--- a/src/platforms/posix/drivers/df_mpu9250_wrapper/df_mpu9250_wrapper.cpp
+++ b/src/platforms/posix/drivers/df_mpu9250_wrapper/df_mpu9250_wrapper.cpp
@@ -444,36 +444,29 @@ int DfMpu9250Wrapper::_publish(struct imu_sensor_data &data)
 		_update_gyro_calibration();
 	}
 
-	accel_report accel_report = {};
-	gyro_report gyro_report = {};
+	math::Vector<3> vec_integrated_unused;
+	uint64_t integral_dt_unused;
 
-	accel_report.x = (data.accel_m_s2_x - _accel_calibration.x_offset) * _accel_calibration.x_scale;
-	accel_report.y = (data.accel_m_s2_y - _accel_calibration.y_offset) * _accel_calibration.y_scale;
-	accel_report.z = (data.accel_m_s2_z - _accel_calibration.z_offset) * _accel_calibration.z_scale;
+	math::Vector<3> accel_val((data.accel_m_s2_x - _accel_calibration.x_offset) * _accel_calibration.x_scale,
+				  (data.accel_m_s2_y - _accel_calibration.y_offset) * _accel_calibration.y_scale,
+				  (data.accel_m_s2_z - _accel_calibration.z_offset) * _accel_calibration.z_scale);
 
-	math::Vector<3> accel_val(accel_report.x,
-				  accel_report.y,
-				  accel_report.z);
-	math::Vector<3> accel_val_integrated_unused;
 
 	_accel_int.put_with_interval(data.fifo_sample_interval_us,
 				     accel_val,
-				     accel_val_integrated_unused,
-				     accel_report.integral_dt);
+				     vec_integrated_unused,
+				     integral_dt_unused);
 
-	gyro_report.x = (data.gyro_rad_s_x - _gyro_calibration.x_offset) * _gyro_calibration.x_scale;
-	gyro_report.y = (data.gyro_rad_s_y - _gyro_calibration.y_offset) * _gyro_calibration.y_scale;
-	gyro_report.z = (data.gyro_rad_s_z - _gyro_calibration.z_offset) * _gyro_calibration.z_scale;
+	math::Vector<3> gyro_val((data.gyro_rad_s_x - _gyro_calibration.x_offset) * _gyro_calibration.x_scale,
+				 (data.gyro_rad_s_y - _gyro_calibration.y_offset) * _gyro_calibration.y_scale,
+				 (data.gyro_rad_s_z - _gyro_calibration.z_offset) * _gyro_calibration.z_scale);
 
-	math::Vector<3> gyro_val(gyro_report.x,
-				 gyro_report.y,
-				 gyro_report.z);
 	math::Vector<3> gyro_val_integrated_unused;
 
 	_gyro_int.put_with_interval(data.fifo_sample_interval_us,
 				    gyro_val,
-				    gyro_val_integrated_unused,
-				    gyro_report.integral_dt);
+				    vec_integrated_unused,
+				    integral_dt_unused);
 
 	// If we are not receiving the last sample from the FIFO buffer yet, let's stop here
 	// and wait for more packets.
@@ -501,6 +494,9 @@ int DfMpu9250Wrapper::_publish(struct imu_sensor_data &data)
 
 	perf_begin(_publish_perf);
 
+	accel_report accel_report = {};
+	gyro_report gyro_report = {};
+
 	accel_report.timestamp = gyro_report.timestamp = hrt_absolute_time();
 
 	// TODO: get these right
@@ -521,17 +517,29 @@ int DfMpu9250Wrapper::_publish(struct imu_sensor_data &data)
 	accel_report.y_raw = NAN;
 	accel_report.z_raw = NAN;
 
+	math::Vector<3> gyro_val_filt;
+	math::Vector<3> accel_val_filt;
+
 	// Read and reset.
-	math::Vector<3> gyro_val_integrated = _gyro_int.get(true, gyro_report.integral_dt);
-	math::Vector<3> accel_val_integrated = _accel_int.get(true, accel_report.integral_dt);
+	math::Vector<3> gyro_val_integ = _gyro_int.get_and_filtered(true, gyro_report.integral_dt, gyro_val_filt);
+	math::Vector<3> accel_val_integ = _accel_int.get_and_filtered(true, accel_report.integral_dt, accel_val_filt);
 
-	gyro_report.x_integral = gyro_val_integrated(0);
-	gyro_report.y_integral = gyro_val_integrated(1);
-	gyro_report.z_integral = gyro_val_integrated(2);
+	// Use the filtered (by integration) values to get smoother / less noisy data.
+	gyro_report.x = gyro_val_filt(0);
+	gyro_report.y = gyro_val_filt(1);
+	gyro_report.z = gyro_val_filt(2);
 
-	accel_report.x_integral = accel_val_integrated(0);
-	accel_report.y_integral = accel_val_integrated(1);
-	accel_report.z_integral = accel_val_integrated(2);
+	accel_report.x = accel_val_filt(0);
+	accel_report.y = accel_val_filt(1);
+	accel_report.z = accel_val_filt(2);
+
+	gyro_report.x_integral = gyro_val_integ(0);
+	gyro_report.y_integral = gyro_val_integ(1);
+	gyro_report.z_integral = gyro_val_integ(2);
+
+	accel_report.x_integral = accel_val_integ(0);
+	accel_report.y_integral = accel_val_integ(1);
+	accel_report.z_integral = accel_val_integ(2);
 
 	// TODO: when is this ever blocked?
 	if (!(m_pub_blocked)) {

From db5212a2092251c347490bc6d2b263c44dcd7596 Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Fri, 13 May 2016 10:34:13 +0200
Subject: [PATCH 0058/1230] Land detector: add missing init

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

diff --git a/src/modules/land_detector/MulticopterLandDetector.cpp b/src/modules/land_detector/MulticopterLandDetector.cpp
index b6eeb92dab..50720e430d 100644
--- a/src/modules/land_detector/MulticopterLandDetector.cpp
+++ b/src/modules/land_detector/MulticopterLandDetector.cpp
@@ -63,6 +63,7 @@ MulticopterLandDetector::MulticopterLandDetector() : LandDetector(),
 	_actuators{},
 	_arming{},
 	_vehicleAttitude{},
+	_manual{},
 	_ctrl_state{},
 	_ctrl_mode{},
 	_landTimer(0),

From d89937502c0b0ccf04524be5af985a257f2d20e5 Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Fri, 13 May 2016 10:34:25 +0200
Subject: [PATCH 0059/1230] MAVLink: Clean up stream init

---
 src/modules/mavlink/mavlink_main.cpp | 5 +----
 1 file changed, 1 insertion(+), 4 deletions(-)

diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp
index c6e9772f42..650fd3c34f 100644
--- a/src/modules/mavlink/mavlink_main.cpp
+++ b/src/modules/mavlink/mavlink_main.cpp
@@ -1868,7 +1868,6 @@ Mavlink::task_main(int argc, char *argv[])
 		configure_stream("EXTENDED_SYS_STATE", 2.0f);
 		configure_stream("ALTITUDE", 10.0f);
 		configure_stream("VISION_POSITION_NED", 10.0f);
-		configure_stream("NAMED_VALUE_FLOAT", 10.0f);
 		configure_stream("ESTIMATOR_STATUS", 1.0f);
 		configure_stream("ADSB_VEHICLE", 10.0f);
 		break;
@@ -1899,7 +1898,6 @@ Mavlink::task_main(int argc, char *argv[])
 		configure_stream("HOME_POSITION", 0.5f);
 		configure_stream("GLOBAL_POSITION_INT", 10.0f);
 		configure_stream("ATTITUDE_TARGET", 8.0f);
-		//configure_stream("PARAM_VALUE", 300.0f);
 		configure_stream("MISSION_ITEM", 50.0f);
 		configure_stream("NAMED_VALUE_FLOAT", 50.0f);
 		configure_stream("OPTICAL_FLOW_RAD", 10.0f);
@@ -1914,12 +1912,11 @@ Mavlink::task_main(int argc, char *argv[])
 		configure_stream("LOCAL_POSITION_NED", 30.0f);
 		configure_stream("MANUAL_CONTROL", 5.0f);
 		configure_stream("HIGHRES_IMU", 50.0f);
-		configure_stream("GPS_RAW_INT", 20.0f);
+		configure_stream("GPS_RAW_INT", 10.0f);
 		configure_stream("CAMERA_TRIGGER", 500.0f);
 		configure_stream("EXTENDED_SYS_STATE", 2.0f);
 		configure_stream("ALTITUDE", 10.0f);
 		configure_stream("VISION_POSITION_NED", 10.0f);
-		configure_stream("NAMED_VALUE_FLOAT", 50.0f);
 		configure_stream("ESTIMATOR_STATUS", 5.0f);
 		configure_stream("ADSB_VEHICLE", 20.0f);
 	default:

From 623ef6d67c76596aa000986210f846ac408465f8 Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Fri, 13 May 2016 10:34:47 +0200
Subject: [PATCH 0060/1230] MC pos control: Use default initializers

---
 .../mc_pos_control/mc_pos_control_main.cpp    | 22 ++++++++++---------
 1 file changed, 12 insertions(+), 10 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 7421878c54..8dd6672071 100644
--- a/src/modules/mc_pos_control/mc_pos_control_main.cpp
+++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp
@@ -374,6 +374,17 @@ MulticopterPositionControl::MulticopterPositionControl() :
 	_local_pos_sp_pub(nullptr),
 	_global_vel_sp_pub(nullptr),
 	_attitude_setpoint_id(0),
+	_vehicle_status{},
+	_vehicle_land_detected{},
+	_ctrl_state{},
+	_att_sp{},
+	_manual{},
+	_control_mode{},
+	_arming{},
+	_local_pos{},
+	_pos_sp_triplet{},
+	_local_pos_sp{},
+	_global_vel_sp{},
 	_manual_thr_min(this, "MANTHR_MIN"),
 	_manual_thr_max(this, "MANTHR_MAX"),
 	_vel_x_deriv(this, "VELD"),
@@ -398,17 +409,8 @@ MulticopterPositionControl::MulticopterPositionControl() :
 	_takeoff_thrust_sp(0.0f),
 	control_vel_enabled_prev(false)
 {
-	memset(&_vehicle_status, 0, sizeof(_vehicle_status));
-	memset(&_ctrl_state, 0, sizeof(_ctrl_state));
+	// Make the quaternion valid for control state
 	_ctrl_state.q[0] = 1.0f;
-	memset(&_att_sp, 0, sizeof(_att_sp));
-	memset(&_manual, 0, sizeof(_manual));
-	memset(&_control_mode, 0, sizeof(_control_mode));
-	memset(&_arming, 0, sizeof(_arming));
-	memset(&_local_pos, 0, sizeof(_local_pos));
-	memset(&_pos_sp_triplet, 0, sizeof(_pos_sp_triplet));
-	memset(&_local_pos_sp, 0, sizeof(_local_pos_sp));
-	memset(&_global_vel_sp, 0, sizeof(_global_vel_sp));
 
 	memset(&_ref_pos, 0, sizeof(_ref_pos));
 

From a40e1ea269a5d3670884244ee49d2842c1b64bdb Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Fri, 13 May 2016 10:35:07 +0200
Subject: [PATCH 0061/1230] sdlog2: Properly handle pragma

---
 src/modules/sdlog2/sdlog2_messages.h | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/src/modules/sdlog2/sdlog2_messages.h b/src/modules/sdlog2/sdlog2_messages.h
index 7778cf99df..900f02a472 100644
--- a/src/modules/sdlog2/sdlog2_messages.h
+++ b/src/modules/sdlog2/sdlog2_messages.h
@@ -629,11 +629,11 @@ struct log_PARM_s {
 	char name[16];
 	float value;
 };
+#pragma pack(pop)
 
 // the lower type of initialisation is not supported in C++
 #ifndef __cplusplus
 
-#pragma pack(pop)
 /* construct list of all message formats */
 static const struct log_format_s log_formats[] = {
 	/* business-level messages, ID < 0x80 */

From f204a145c758e2700dfc62301e6e94a3594d8745 Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Fri, 13 May 2016 10:35:19 +0200
Subject: [PATCH 0062/1230] POSIX: Improve console management

---
 src/platforms/posix/main.cpp | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/src/platforms/posix/main.cpp b/src/platforms/posix/main.cpp
index fe12f233d3..278257fce6 100644
--- a/src/platforms/posix/main.cpp
+++ b/src/platforms/posix/main.cpp
@@ -329,13 +329,13 @@ int main(int argc, char **argv)
 				}
 
 				if (buf_ptr_write > 0) {
-					if (mystr != string_buffer[buf_ptr_write - 1]) {
+					if (!mystr.empty() && mystr != string_buffer[buf_ptr_write - 1]) {
 						string_buffer[buf_ptr_write] = mystr;
 						buf_ptr_write++;
 					}
 
 				} else {
-					if (mystr != string_buffer[CMD_BUFF_SIZE - 1]) {
+					if (!mystr.empty() && mystr != string_buffer[CMD_BUFF_SIZE - 1]) {
 						string_buffer[buf_ptr_write] = mystr;
 						buf_ptr_write++;
 					}

From 8b41ddd224af55f75a05c72e87f48cb238f5448c Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Fri, 13 May 2016 11:03:18 +0200
Subject: [PATCH 0063/1230] Commander: Better status feedback

---
 src/modules/commander/commander.cpp | 8 ++++----
 1 file changed, 4 insertions(+), 4 deletions(-)

diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp
index 11daf70d1e..5188f12ac4 100644
--- a/src/modules/commander/commander.cpp
+++ b/src/modules/commander/commander.cpp
@@ -1040,9 +1040,9 @@ bool handle_command(struct vehicle_status_s *status_local, const struct safety_s
 	case vehicle_command_s::VEHICLE_CMD_NAV_TAKEOFF: {
 			/* ok, home set, use it to take off */
 			if (TRANSITION_CHANGED == main_state_transition(&status, commander_state_s::MAIN_STATE_AUTO_TAKEOFF, main_state_prev, &status_flags, &internal_state)) {
-				warnx("taking off!");
+				mavlink_and_console_log_info(&mavlink_log_pub, "Taking off");
 			} else {
-				warnx("takeoff denied");
+				mavlink_and_console_log_critical(&mavlink_log_pub, "Takeoff denied, disarm and re-try");
 			}
 
 		}
@@ -1051,9 +1051,9 @@ bool handle_command(struct vehicle_status_s *status_local, const struct safety_s
 	case vehicle_command_s::VEHICLE_CMD_NAV_LAND: {
 			/* ok, home set, use it to take off */
 			if (TRANSITION_CHANGED == main_state_transition(&status, commander_state_s::MAIN_STATE_AUTO_LAND, main_state_prev, &status_flags, &internal_state)) {
-				warnx("landing!");
+				mavlink_and_console_log_info(&mavlink_log_pub, "Landing at current position");
 			} else {
-				warnx("landing denied");
+				mavlink_and_console_log_critical(&mavlink_log_pub, "Landing denied, land manually.");
 			}
 
 		}

From a700b02f77ff3eb2ef7b44e5b0b0e1b603257a5f Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Fri, 13 May 2016 11:03:35 +0200
Subject: [PATCH 0064/1230] Navigator: Do not publish an empty triplet

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

diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp
index aa3cc37fa2..d5d0095d64 100644
--- a/src/modules/navigator/navigator_main.cpp
+++ b/src/modules/navigator/navigator_main.cpp
@@ -684,6 +684,11 @@ Navigator::publish_position_setpoint_triplet()
 	/* update navigation state */
 	_pos_sp_triplet.nav_state = _vstatus.nav_state;
 
+	/* do not publish an empty triplet */
+	if (!_pos_sp_triplet.current.valid) {
+		return;
+	}
+
 	/* lazily publish the position setpoint triplet only once available */
 	if (_pos_sp_triplet_pub != nullptr) {
 		orb_publish(ORB_ID(position_setpoint_triplet), _pos_sp_triplet_pub, &_pos_sp_triplet);

From 5fb1de906c711e88a9a3ddd37cd0ccaea685a7b3 Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Fri, 13 May 2016 11:39:40 +0200
Subject: [PATCH 0065/1230] commander: Better reporting and convenience for
 commander

---
 src/modules/commander/commander.cpp | 12 +++++-------
 1 file changed, 5 insertions(+), 7 deletions(-)

diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp
index 5188f12ac4..3df0fbedeb 100644
--- a/src/modules/commander/commander.cpp
+++ b/src/modules/commander/commander.cpp
@@ -811,10 +811,6 @@ bool handle_command(struct vehicle_status_s *status_local, const struct safety_s
 				if (arming_ret == TRANSITION_DENIED) {
 					mavlink_log_critical(&mavlink_log_pub, "Rejecting arming cmd");
 				}
-
-				if (main_ret == TRANSITION_DENIED) {
-					mavlink_log_critical(&mavlink_log_pub, "Rejecting mode switch cmd");
-				}
 			}
 		}
 		break;
@@ -1959,7 +1955,7 @@ int commander_thread_main(int argc, char *argv[])
 
 		if ((updated && status_flags.condition_local_altitude_valid) || check_for_disarming) {
 			if (was_landed != land_detector.landed) {
-				if (land_detector.landed) {
+				if (land_detector.landed && armed.armed) {
 					mavlink_and_console_log_info(&mavlink_log_pub, "LANDING DETECTED");
 				} else {
 					mavlink_and_console_log_info(&mavlink_log_pub, "TAKEOFF DETECTED");
@@ -2391,7 +2387,9 @@ int commander_thread_main(int argc, char *argv[])
 					if ((internal_state.main_state != commander_state_s::MAIN_STATE_MANUAL)
 						&& (internal_state.main_state != commander_state_s::MAIN_STATE_ACRO)
 						&& (internal_state.main_state != commander_state_s::MAIN_STATE_STAB)
-						&& (internal_state.main_state != commander_state_s::MAIN_STATE_ALTCTL)) {
+						&& (internal_state.main_state != commander_state_s::MAIN_STATE_ALTCTL)
+						&& (internal_state.main_state != commander_state_s::MAIN_STATE_POSCTL)
+						) {
 						print_reject_arm("NOT ARMING: Switch to a manual mode first.");
 
 					} else if (!status_flags.condition_home_position_valid &&
@@ -2413,7 +2411,7 @@ int commander_thread_main(int argc, char *argv[])
 							arming_state_changed = true;
 						} else {
 							usleep(100000);
-							print_reject_arm("NOT ARMING: Configuration error");
+							print_reject_arm("NOT ARMING: Preflight checks failed");
 						}
 					}
 					stick_on_counter = 0;

From eae726e3456978a403b0ae26c9c2a2701153451f Mon Sep 17 00:00:00 2001
From: Daniel Agar <daniel@agar.ca>
Date: Fri, 22 Apr 2016 21:45:34 -0400
Subject: [PATCH 0066/1230] FW add mavlink NAV_CONTROLLER_OUTPUT

---
 ...apabilities.msg => fw_pos_ctrl_status.msg} |   9 ++
 .../fw_pos_control_l1_main.cpp                |  62 +++++----
 src/modules/mavlink/mavlink_main.cpp          |   3 +
 src/modules/mavlink/mavlink_messages.cpp      | 121 ++++++++++++++----
 .../navigator/mission_feasibility_checker.cpp |  15 +--
 .../navigator/mission_feasibility_checker.h   |   6 +-
 src/modules/navigator/navigator.h             |   8 +-
 src/modules/navigator/navigator_main.cpp      |  26 ++--
 src/modules/uORB/objects_common.cpp           |   4 +-
 9 files changed, 170 insertions(+), 84 deletions(-)
 rename msg/{navigation_capabilities.msg => fw_pos_ctrl_status.msg} (68%)

diff --git a/msg/navigation_capabilities.msg b/msg/fw_pos_ctrl_status.msg
similarity index 68%
rename from msg/navigation_capabilities.msg
rename to msg/fw_pos_ctrl_status.msg
index 6d12aaaed9..641b591205 100644
--- a/msg/navigation_capabilities.msg
+++ b/msg/fw_pos_ctrl_status.msg
@@ -1,5 +1,14 @@
 uint64 timestamp		# timestamp this topic was emitted
+
+float32 nav_roll
+float32 nav_pitch
+float32 nav_bearing
+
+float32 target_bearing
+float32 wp_dist
+float32 xtrack_error
 float32 turn_distance		# the optimal distance to a waypoint to switch to the next
+
 float32 landing_horizontal_slope_displacement
 float32 landing_slope_angle_rad
 float32 landing_flare_length
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 3e93fba958..c510c4e322 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
@@ -86,9 +86,9 @@
 #include <systemlib/systemlib.h>
 #include <uORB/topics/actuator_controls.h>
 #include <uORB/topics/control_state.h>
+#include <uORB/topics/fw_pos_ctrl_status.h>
 #include <uORB/topics/fw_virtual_attitude_setpoint.h>
 #include <uORB/topics/manual_control_setpoint.h>
-#include <uORB/topics/navigation_capabilities.h>
 #include <uORB/topics/parameter_update.h>
 #include <uORB/topics/position_setpoint_triplet.h>
 #include <uORB/topics/sensor_combined.h>
@@ -170,13 +170,13 @@ private:
 
 	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 */
+	orb_advert_t	_fw_pos_ctrl_status_pub;		/**< navigation capabilities publication */
 
 	orb_id_t _attitude_setpoint_id;
 
 	struct control_state_s				_ctrl_state;			/**< control state */
 	struct vehicle_attitude_setpoint_s		_att_sp;			/**< vehicle attitude setpoint */
-	struct navigation_capabilities_s		_nav_capabilities;		/**< navigation capabilities */
+	struct fw_pos_ctrl_status_s		_fw_pos_ctrl_status;		/**< navigation capabilities */
 	struct manual_control_setpoint_s		_manual;			/**< r/c channel data */
 	struct vehicle_control_mode_s			_control_mode;			/**< control mode */
 	struct vehicle_status_s				_vehicle_status;		/**< vehicle status */
@@ -408,7 +408,7 @@ private:
 	/**
 	 * Publish navigation capabilities
 	 */
-	void		navigation_capabilities_publish();
+	void		fw_pos_ctrl_status_publish();
 
 	/**
 	 * Get a new waypoint based on heading and distance from current position
@@ -530,7 +530,7 @@ FixedwingPositionControl::FixedwingPositionControl() :
 	/* publications */
 	_attitude_sp_pub(nullptr),
 	_tecs_status_pub(nullptr),
-	_nav_capabilities_pub(nullptr),
+	_fw_pos_ctrl_status_pub(nullptr),
 
 	/* publication ID */
 	_attitude_setpoint_id(0),
@@ -538,7 +538,7 @@ FixedwingPositionControl::FixedwingPositionControl() :
 	/* states */
 	_ctrl_state(),
 	_att_sp(),
-	_nav_capabilities(),
+	_fw_pos_ctrl_status(),
 	_manual(),
 	_control_mode(),
 	_vehicle_status(),
@@ -593,7 +593,7 @@ FixedwingPositionControl::FixedwingPositionControl() :
 	_l1_control(),
 	_control_mode_current(FW_POSCTRL_MODE_OTHER)
 {
-	_nav_capabilities = {};
+	_fw_pos_ctrl_status = {};
 
 	_parameter_handles.l1_period = param_find("FW_L1_PERIOD");
 	_parameter_handles.l1_damping = param_find("FW_L1_DAMPING");
@@ -769,10 +769,10 @@ FixedwingPositionControl::parameters_update()
 			    _parameters.land_thrust_lim_alt_relative, _parameters.land_H1_virt);
 
 	/* Update and publish the navigation capabilities */
-	_nav_capabilities.landing_slope_angle_rad = landingslope.landing_slope_angle_rad();
-	_nav_capabilities.landing_horizontal_slope_displacement = landingslope.horizontal_slope_displacement();
-	_nav_capabilities.landing_flare_length = landingslope.flare_length();
-	navigation_capabilities_publish();
+	_fw_pos_ctrl_status.landing_slope_angle_rad = landingslope.landing_slope_angle_rad();
+	_fw_pos_ctrl_status.landing_horizontal_slope_displacement = landingslope.horizontal_slope_displacement();
+	_fw_pos_ctrl_status.landing_flare_length = landingslope.flare_length();
+	fw_pos_ctrl_status_publish();
 
 	/* Update Launch Detector Parameters */
 	launchDetector.updateParams();
@@ -1016,15 +1016,15 @@ FixedwingPositionControl::calculate_gndspeed_undershoot(const math::Vector<2> &c
 	}
 }
 
-void FixedwingPositionControl::navigation_capabilities_publish()
+void FixedwingPositionControl::fw_pos_ctrl_status_publish()
 {
-	_nav_capabilities.timestamp = hrt_absolute_time();
+	_fw_pos_ctrl_status.timestamp = hrt_absolute_time();
 
-	if (_nav_capabilities_pub != nullptr) {
-		orb_publish(ORB_ID(navigation_capabilities), _nav_capabilities_pub, &_nav_capabilities);
+	if (_fw_pos_ctrl_status_pub != nullptr) {
+		orb_publish(ORB_ID(fw_pos_ctrl_status), _fw_pos_ctrl_status_pub, &_fw_pos_ctrl_status);
 
 	} else {
-		_nav_capabilities_pub = orb_advertise(ORB_ID(navigation_capabilities), &_nav_capabilities);
+		_fw_pos_ctrl_status_pub = orb_advertise(ORB_ID(fw_pos_ctrl_status), &_fw_pos_ctrl_status);
 	}
 }
 
@@ -1337,7 +1337,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
 
 			float alt_sp;
 
-			if (_nav_capabilities.abort_landing == true) {
+			if (_fw_pos_ctrl_status.abort_landing == true) {
 				// if we entered loiter due to an aborted landing, demand
 				// altitude setpoint well above landing waypoint
 				alt_sp = pos_sp_triplet.current.alt + 2.0f * _parameters.climbout_diff;
@@ -1349,7 +1349,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
 
 			if (in_takeoff_situation() ||
 			    ((_global_pos.alt < pos_sp_triplet.current.alt + _parameters.climbout_diff)
-			     && _nav_capabilities.abort_landing == true)) {
+			     && _fw_pos_ctrl_status.abort_landing == true)) {
 				/* limit roll motion to ensure enough lift */
 				_att_sp.roll_body = math::constrain(_att_sp.roll_body, math::radians(-15.0f),
 								    math::radians(15.0f));
@@ -1465,7 +1465,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
 					} else {
 						// still no valid terrain, abort landing
 						terrain_alt = pos_sp_triplet.current.alt;
-						_nav_capabilities.abort_landing = true;
+						_fw_pos_ctrl_status.abort_landing = true;
 					}
 
 				} else if ((!_global_pos.terrain_alt_valid && hrt_elapsed_time(&_time_last_t_alt) < T_ALT_TIMEOUT * 1000 * 1000)
@@ -1478,7 +1478,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
 				} else {
 					// terrain alt was not valid for long time, abort landing
 					terrain_alt = _t_alt_prev_valid;
-					_nav_capabilities.abort_landing = true;
+					_fw_pos_ctrl_status.abort_landing = true;
 				}
 
 			} else {
@@ -1954,7 +1954,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
 		_time_last_t_alt = 0;
 
 		// reset lading abort state
-		_nav_capabilities.abort_landing = false;
+		_fw_pos_ctrl_status.abort_landing = false;
 
 		/* no flight mode applies, do not publish an attitude setpoint */
 		setpoint = false;
@@ -2169,15 +2169,24 @@ FixedwingPositionControl::task_main()
 				float turn_distance = _l1_control.switch_distance(100.0f);
 
 				/* lazily publish navigation capabilities */
-				if ((hrt_elapsed_time(&_nav_capabilities.timestamp) > 1000000)
-				    || (fabsf(turn_distance - _nav_capabilities.turn_distance) > FLT_EPSILON
+				if ((hrt_elapsed_time(&_fw_pos_ctrl_status.timestamp) > 1000000)
+				    || (fabsf(turn_distance - _fw_pos_ctrl_status.turn_distance) > FLT_EPSILON
 					&& turn_distance > 0)) {
 
 					/* set new turn distance */
-					_nav_capabilities.turn_distance = turn_distance;
+					_fw_pos_ctrl_status.turn_distance = turn_distance;
 
-					navigation_capabilities_publish();
+					_fw_pos_ctrl_status.nav_roll = _l1_control.nav_roll();
+					_fw_pos_ctrl_status.nav_pitch = get_tecs_pitch();
+					_fw_pos_ctrl_status.nav_bearing = _l1_control.nav_bearing();
 
+					_fw_pos_ctrl_status.target_bearing = _l1_control.target_bearing();
+					_fw_pos_ctrl_status.xtrack_error = _l1_control.crosstrack_error();
+
+					math::Vector<2> curr_wp((float)_pos_sp_triplet.current.lat, (float)_pos_sp_triplet.current.lon);
+					_fw_pos_ctrl_status.wp_dist = get_distance_to_next_waypoint(current_position(0), current_position(1), curr_wp(0), curr_wp(1));
+
+					fw_pos_ctrl_status_publish();
 				}
 
 			}
@@ -2242,9 +2251,8 @@ void FixedwingPositionControl::tecs_update_pitch_throttle(float alt_sp, float v_
 		_was_in_transition = true;
 		_asp_after_transition = _ctrl_state.airspeed;
 
-		// after transition we ramp up desired airspeed from the speed we had coming out of the transition
-
 	} else if (_was_in_transition) {
+		// after transition we ramp up desired airspeed from the speed we had coming out of the transition
 		_asp_after_transition += dt * 2; // increase 2m/s
 
 		if (_asp_after_transition < v_sp && _ctrl_state.airspeed < v_sp) {
diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp
index 650fd3c34f..c522c4db6c 100644
--- a/src/modules/mavlink/mavlink_main.cpp
+++ b/src/modules/mavlink/mavlink_main.cpp
@@ -1840,6 +1840,7 @@ Mavlink::task_main(int argc, char *argv[])
 		configure_stream("NAMED_VALUE_FLOAT", 1.0f);
 		configure_stream("ESTIMATOR_STATUS", 0.5f);
 		configure_stream("ADSB_VEHICLE", 2.0f);
+		configure_stream("NAV_CONTROLLER_OUTPUT", 2.0f);
 		break;
 
 	case MAVLINK_MODE_ONBOARD:
@@ -1870,6 +1871,7 @@ Mavlink::task_main(int argc, char *argv[])
 		configure_stream("VISION_POSITION_NED", 10.0f);
 		configure_stream("ESTIMATOR_STATUS", 1.0f);
 		configure_stream("ADSB_VEHICLE", 10.0f);
+		configure_stream("NAV_CONTROLLER_OUTPUT", 10.0f);
 		break;
 
 	case MAVLINK_MODE_OSD:
@@ -1919,6 +1921,7 @@ Mavlink::task_main(int argc, char *argv[])
 		configure_stream("VISION_POSITION_NED", 10.0f);
 		configure_stream("ESTIMATOR_STATUS", 5.0f);
 		configure_stream("ADSB_VEHICLE", 20.0f);
+		configure_stream("NAV_CONTROLLER_OUTPUT", 10.0f);
 	default:
 		break;
 	}
diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp
index 39e47720f4..a480a4dcb7 100644
--- a/src/modules/mavlink/mavlink_messages.cpp
+++ b/src/modules/mavlink/mavlink_messages.cpp
@@ -46,38 +46,39 @@
 #include <commander/px4_custom_mode.h>
 #include <lib/geo/geo.h>
 #include <uORB/uORB.h>
-#include <uORB/topics/sensor_combined.h>
-#include <uORB/topics/vehicle_attitude.h>
-#include <uORB/topics/vehicle_gps_position.h>
-#include <uORB/topics/vehicle_global_position.h>
-#include <uORB/topics/vehicle_local_position.h>
-#include <uORB/topics/home_position.h>
-#include <uORB/topics/vehicle_status.h>
-#include <uORB/topics/vtol_vehicle_status.h>
-#include <uORB/topics/vehicle_command.h>
-#include <uORB/topics/vehicle_local_position_setpoint.h>
-#include <uORB/topics/att_pos_mocap.h>
-#include <uORB/topics/vehicle_attitude_setpoint.h>
-#include <uORB/topics/vehicle_rates_setpoint.h>
-#include <uORB/topics/vision_position_estimate.h>
-#include <uORB/topics/position_setpoint_triplet.h>
-#include <uORB/topics/optical_flow.h>
-#include <uORB/topics/actuator_outputs.h>
-#include <uORB/topics/actuator_controls.h>
 #include <uORB/topics/actuator_armed.h>
-#include <uORB/topics/manual_control_setpoint.h>
-#include <uORB/topics/telemetry_status.h>
-#include <uORB/topics/debug_key_value.h>
+#include <uORB/topics/actuator_controls.h>
+#include <uORB/topics/actuator_outputs.h>
 #include <uORB/topics/airspeed.h>
+#include <uORB/topics/att_pos_mocap.h>
 #include <uORB/topics/battery_status.h>
-#include <uORB/topics/navigation_capabilities.h>
-#include <uORB/topics/distance_sensor.h>
 #include <uORB/topics/camera_trigger.h>
-#include <uORB/topics/vehicle_land_detected.h>
-#include <uORB/topics/estimator_status.h>
-#include <uORB/topics/transponder_report.h>
-#include <uORB/topics/mavlink_log.h>
 #include <uORB/topics/cpuload.h>
+#include <uORB/topics/debug_key_value.h>
+#include <uORB/topics/distance_sensor.h>
+#include <uORB/topics/estimator_status.h>
+#include <uORB/topics/home_position.h>
+#include <uORB/topics/manual_control_setpoint.h>
+#include <uORB/topics/mavlink_log.h>
+#include <uORB/topics/fw_pos_ctrl_status.h>
+#include <uORB/topics/optical_flow.h>
+#include <uORB/topics/position_setpoint_triplet.h>
+#include <uORB/topics/sensor_combined.h>
+#include <uORB/topics/tecs_status.h>
+#include <uORB/topics/telemetry_status.h>
+#include <uORB/topics/transponder_report.h>
+#include <uORB/topics/vehicle_attitude.h>
+#include <uORB/topics/vehicle_attitude_setpoint.h>
+#include <uORB/topics/vehicle_command.h>
+#include <uORB/topics/vehicle_global_position.h>
+#include <uORB/topics/vehicle_gps_position.h>
+#include <uORB/topics/vehicle_land_detected.h>
+#include <uORB/topics/vehicle_local_position.h>
+#include <uORB/topics/vehicle_local_position_setpoint.h>
+#include <uORB/topics/vehicle_rates_setpoint.h>
+#include <uORB/topics/vehicle_status.h>
+#include <uORB/topics/vision_position_estimate.h>
+#include <uORB/topics/vtol_vehicle_status.h>
 #include <drivers/drv_rc_input.h>
 #include <drivers/drv_pwm_output.h>
 #include <systemlib/err.h>
@@ -2576,6 +2577,72 @@ protected:
 	}
 };
 
+class MavlinkStreamNavControllerOutput : public MavlinkStream
+{
+public:
+	const char *get_name() const
+	{
+		return MavlinkStreamNavControllerOutput::get_name_static();
+	}
+
+	static const char *get_name_static()
+	{
+		return "NAV_CONTROLLER_OUTPUT";
+	}
+
+	uint8_t get_id()
+	{
+		return MAVLINK_MSG_ID_NAV_CONTROLLER_OUTPUT;
+	}
+
+	static MavlinkStream *new_instance(Mavlink *mavlink)
+	{
+		return new MavlinkStreamNavControllerOutput(mavlink);
+	}
+
+	unsigned get_size()
+	{
+		return MAVLINK_MSG_ID_NAV_CONTROLLER_OUTPUT + MAVLINK_NUM_NON_PAYLOAD_BYTES;
+	}
+
+private:
+	MavlinkOrbSubscription *_fw_pos_ctrl_status_sub;
+	MavlinkOrbSubscription *_tecs_status_sub;
+
+	/* do not allow top copying this class */
+	MavlinkStreamNavControllerOutput(MavlinkStreamNavControllerOutput &);
+	MavlinkStreamNavControllerOutput& operator = (const MavlinkStreamNavControllerOutput &);
+
+protected:
+	explicit MavlinkStreamNavControllerOutput(Mavlink *mavlink) : MavlinkStream(mavlink),
+		_fw_pos_ctrl_status_sub(_mavlink->add_orb_subscription(ORB_ID(fw_pos_ctrl_status))),
+		_tecs_status_sub(_mavlink->add_orb_subscription(ORB_ID(tecs_status)))
+	{}
+
+	void send(const hrt_abstime t)
+	{
+		struct fw_pos_ctrl_status_s _fw_pos_ctrl_status = {};
+		struct tecs_status_s _tecs_status = {};
+
+		const bool updated_fw_pos_ctrl_status = _fw_pos_ctrl_status_sub->update(&_fw_pos_ctrl_status);
+		const bool updated_tecs = _tecs_status_sub->update(&_tecs_status);
+
+		if (updated_fw_pos_ctrl_status || updated_tecs) {
+			mavlink_nav_controller_output_t msg = {};
+
+			msg.nav_roll = math::degrees(_fw_pos_ctrl_status.nav_roll);
+			msg.nav_pitch = math::degrees(_fw_pos_ctrl_status.nav_pitch);
+			msg.nav_bearing = (int16_t)math::degrees(_fw_pos_ctrl_status.nav_bearing);
+			msg.target_bearing = (int16_t)math::degrees(_fw_pos_ctrl_status.target_bearing);
+			msg.wp_dist = (uint16_t)_fw_pos_ctrl_status.wp_dist;
+			msg.xtrack_error = _fw_pos_ctrl_status.xtrack_error;
+			msg.alt_error = _tecs_status.altitude_filtered - _tecs_status.altitudeSp;
+			msg.aspd_error = _tecs_status.airspeed_filtered - _tecs_status.airspeedSp;
+
+			_mavlink->send_message(MAVLINK_MSG_ID_NAV_CONTROLLER_OUTPUT, &msg);
+		}
+	}
+};
 
 class MavlinkStreamCameraCapture : public MavlinkStream
 {
diff --git a/src/modules/navigator/mission_feasibility_checker.cpp b/src/modules/navigator/mission_feasibility_checker.cpp
index 63c441d8b8..bb68624437 100644
--- a/src/modules/navigator/mission_feasibility_checker.cpp
+++ b/src/modules/navigator/mission_feasibility_checker.cpp
@@ -55,11 +55,11 @@
 
 MissionFeasibilityChecker::MissionFeasibilityChecker() :
 	_mavlink_log_pub(nullptr),
-	_capabilities_sub(-1),
+	_fw_pos_ctrl_status_sub(-1),
 	_initDone(false),
 	_dist_1wp_ok(false)
 {
-	_nav_caps = {0};
+	_fw_pos_ctrl_status = {};
 }
 
 
@@ -275,7 +275,6 @@ bool MissionFeasibilityChecker::checkFixedWingLanding(dm_item_t dm_current, size
 	/* Go through all mission items and search for a landing waypoint
 	 * if landing waypoint is found: the previous waypoint is checked to be at a feasible distance and altitude given the landing slope */
 
-
 	for (size_t i = 0; i < nMissionItems; i++) {
 		struct mission_item_s missionitem;
 		const ssize_t len = sizeof(missionitem);
@@ -293,15 +292,15 @@ bool MissionFeasibilityChecker::checkFixedWingLanding(dm_item_t dm_current, size
 				}
 
 				float wp_distance = get_distance_to_next_waypoint(missionitem_previous.lat , missionitem_previous.lon, missionitem.lat, missionitem.lon);
-				float slope_alt_req = Landingslope::getLandingSlopeAbsoluteAltitude(wp_distance, missionitem.altitude, _nav_caps.landing_horizontal_slope_displacement, _nav_caps.landing_slope_angle_rad);
-				float wp_distance_req = Landingslope::getLandingSlopeWPDistance(missionitem_previous.altitude, missionitem.altitude, _nav_caps.landing_horizontal_slope_displacement, _nav_caps.landing_slope_angle_rad);
+				float slope_alt_req = Landingslope::getLandingSlopeAbsoluteAltitude(wp_distance, missionitem.altitude, _fw_pos_ctrl_status.landing_horizontal_slope_displacement, _fw_pos_ctrl_status.landing_slope_angle_rad);
+				float wp_distance_req = Landingslope::getLandingSlopeWPDistance(missionitem_previous.altitude, missionitem.altitude, _fw_pos_ctrl_status.landing_horizontal_slope_displacement, _fw_pos_ctrl_status.landing_slope_angle_rad);
 				float delta_altitude = missionitem.altitude - missionitem_previous.altitude;
 //				warnx("wp_distance %.2f, delta_altitude %.2f, missionitem_previous.altitude %.2f, missionitem.altitude %.2f, slope_alt_req %.2f, wp_distance_req %.2f",
 //						wp_distance, delta_altitude, missionitem_previous.altitude, missionitem.altitude, slope_alt_req, wp_distance_req);
 //				warnx("_nav_caps.landing_horizontal_slope_displacement %.4f, _nav_caps.landing_slope_angle_rad %.4f, _nav_caps.landing_flare_length %.4f",
 //						_nav_caps.landing_horizontal_slope_displacement, _nav_caps.landing_slope_angle_rad, _nav_caps.landing_flare_length);
 
-				if (wp_distance > _nav_caps.landing_flare_length) {
+				if (wp_distance > _fw_pos_ctrl_status.landing_flare_length) {
 					/* Last wp is before flare region */
 
 					if (delta_altitude < 0) {
@@ -424,14 +423,14 @@ MissionFeasibilityChecker::isPositionCommand(unsigned cmd){
 
 void MissionFeasibilityChecker::updateNavigationCapabilities()
 {
-	(void)orb_copy(ORB_ID(navigation_capabilities), _capabilities_sub, &_nav_caps);
+	(void)orb_copy(ORB_ID(fw_pos_ctrl_status), _fw_pos_ctrl_status_sub, &_fw_pos_ctrl_status);
 }
 
 void MissionFeasibilityChecker::init()
 {
 	if (!_initDone) {
 
-		_capabilities_sub = orb_subscribe(ORB_ID(navigation_capabilities));
+		_fw_pos_ctrl_status_sub = orb_subscribe(ORB_ID(fw_pos_ctrl_status));
 
 		_initDone = true;
 	}
diff --git a/src/modules/navigator/mission_feasibility_checker.h b/src/modules/navigator/mission_feasibility_checker.h
index b372cd3219..db19e60dc6 100644
--- a/src/modules/navigator/mission_feasibility_checker.h
+++ b/src/modules/navigator/mission_feasibility_checker.h
@@ -44,7 +44,7 @@
 
 #include <unistd.h>
 #include <uORB/topics/mission.h>
-#include <uORB/topics/navigation_capabilities.h>
+#include <uORB/topics/fw_pos_ctrl_status.h>
 #include <dataman/dataman.h>
 #include "geofence.h"
 
@@ -54,8 +54,8 @@ class MissionFeasibilityChecker
 private:
 	orb_advert_t		*_mavlink_log_pub;
 
-	int _capabilities_sub;
-	struct navigation_capabilities_s _nav_caps;
+	int _fw_pos_ctrl_status_sub;
+	struct fw_pos_ctrl_status_s _fw_pos_ctrl_status;
 
 	bool _initDone;
 	bool _dist_1wp_ok;
diff --git a/src/modules/navigator/navigator.h b/src/modules/navigator/navigator.h
index 61cb005893..2e6db02ab4 100644
--- a/src/modules/navigator/navigator.h
+++ b/src/modules/navigator/navigator.h
@@ -207,7 +207,7 @@ private:
 	int		_home_pos_sub;			/**< home position subscription */
 	int		_vstatus_sub;			/**< vehicle status subscription */
 	int		_land_detected_sub;		/**< vehicle land detected subscription */
-	int		_capabilities_sub;		/**< notification of vehicle capabilities updates */
+	int		_fw_pos_ctrl_status_sub;		/**< notification of vehicle capabilities updates */
 	int		_control_mode_sub;		/**< vehicle control mode subscription */
 	int		_onboard_mission_sub;		/**< onboard mission subscription */
 	int		_offboard_mission_sub;		/**< offboard mission subscription */
@@ -229,7 +229,7 @@ private:
 	sensor_combined_s				_sensor_combined;	/**< sensor values */
 	home_position_s					_home_pos;		/**< home position for RTL */
 	mission_item_s 					_mission_item;		/**< current mission item */
-	navigation_capabilities_s			_nav_caps;		/**< navigation capabilities */
+	fw_pos_ctrl_status_s			_fw_pos_ctrl_status;		/**< fixed wing navigation capabilities */
 	position_setpoint_triplet_s			_pos_sp_triplet;	/**< triplet of position setpoints */
 	position_setpoint_triplet_s			_reposition_triplet;	/**< triplet for non-mission direct position command */
 	position_setpoint_triplet_s			_takeoff_triplet;	/**< triplet for non-mission direct takeoff command */
@@ -301,9 +301,9 @@ private:
 	void		home_position_update(bool force=false);
 
 	/**
-	 * Retreive navigation capabilities
+	 * Retrieve fixed wing navigation capabilities
 	 */
-	void		navigation_capabilities_update();
+	void		fw_pos_ctrl_status_update();
 
 	/**
 	 * Retrieve vehicle status
diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp
index d5d0095d64..a383af5d38 100644
--- a/src/modules/navigator/navigator_main.cpp
+++ b/src/modules/navigator/navigator_main.cpp
@@ -70,7 +70,7 @@
 #include <uORB/topics/vehicle_status.h>
 #include <uORB/topics/mission.h>
 #include <uORB/topics/fence.h>
-#include <uORB/topics/navigation_capabilities.h>
+#include <uORB/topics/fw_pos_ctrl_status.h>
 #include <uORB/topics/vehicle_command.h>
 #include <drivers/drv_baro.h>
 
@@ -108,7 +108,7 @@ Navigator::Navigator() :
 	_home_pos_sub(-1),
 	_vstatus_sub(-1),
 	_land_detected_sub(-1),
-	_capabilities_sub(-1),
+	_fw_pos_ctrl_status_sub(-1),
 	_control_mode_sub(-1),
 	_onboard_mission_sub(-1),
 	_offboard_mission_sub(-1),
@@ -126,7 +126,7 @@ Navigator::Navigator() :
 	_sensor_combined{},
 	_home_pos{},
 	_mission_item{},
-	_nav_caps{},
+	_fw_pos_ctrl_status{},
 	_pos_sp_triplet{},
 	_reposition_triplet{},
 	_takeoff_triplet{},
@@ -231,9 +231,9 @@ Navigator::home_position_update(bool force)
 }
 
 void
-Navigator::navigation_capabilities_update()
+Navigator::fw_pos_ctrl_status_update()
 {
-	orb_copy(ORB_ID(navigation_capabilities), _capabilities_sub, &_nav_caps);
+	orb_copy(ORB_ID(fw_pos_ctrl_status), _fw_pos_ctrl_status_sub, &_fw_pos_ctrl_status);
 }
 
 void
@@ -298,7 +298,7 @@ Navigator::task_main()
 	_global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position));
 	_gps_pos_sub = orb_subscribe(ORB_ID(vehicle_gps_position));
 	_sensor_combined_sub = orb_subscribe(ORB_ID(sensor_combined));
-	_capabilities_sub = orb_subscribe(ORB_ID(navigation_capabilities));
+	_fw_pos_ctrl_status_sub = orb_subscribe(ORB_ID(fw_pos_ctrl_status));
 	_vstatus_sub = orb_subscribe(ORB_ID(vehicle_status));
 	_land_detected_sub = orb_subscribe(ORB_ID(vehicle_land_detected));
 	_control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode));
@@ -316,7 +316,7 @@ Navigator::task_main()
 	gps_position_update();
 	sensor_combined_update();
 	home_position_update(true);
-	navigation_capabilities_update();
+	fw_pos_ctrl_status_update();
 	params_update();
 
 	/* wakeup source(s) */
@@ -395,9 +395,9 @@ Navigator::task_main()
 		}
 
 		/* navigation capabilities updated */
-		orb_check(_capabilities_sub, &updated);
+		orb_check(_fw_pos_ctrl_status_sub, &updated);
 		if (updated) {
-			navigation_capabilities_update();
+			fw_pos_ctrl_status_update();
 		}
 
 		/* home position updated */
@@ -524,7 +524,7 @@ Navigator::task_main()
 				_can_loiter_at_sp = false;
 				break;
 			case vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION:
-				if (_nav_caps.abort_landing) {
+				if (_fw_pos_ctrl_status.abort_landing) {
 					// pos controller aborted landing, requests loiter
 					// above landing waypoint
 					_navigation_mode = &_loiter;
@@ -732,9 +732,9 @@ Navigator::get_acceptance_radius(float mission_item_radius)
 	// when in fixed wing mode
 	// this might need locking against a commanded transition
 	// so that a stale _vstatus doesn't trigger an accepted mission item.
-	if (!_vstatus.is_rotary_wing && !_vstatus.in_transition_mode && hrt_elapsed_time(&_nav_caps.timestamp) < 5000000) {
-		if (_nav_caps.turn_distance > radius) {
-			radius = _nav_caps.turn_distance;
+	if (!_vstatus.is_rotary_wing && !_vstatus.in_transition_mode) {
+		if ((hrt_elapsed_time(&_fw_pos_ctrl_status.timestamp) < 5000000) && (_fw_pos_ctrl_status.turn_distance > radius)) {
+			radius = _fw_pos_ctrl_status.turn_distance;
 		}
 	}
 
diff --git a/src/modules/uORB/objects_common.cpp b/src/modules/uORB/objects_common.cpp
index 4bb34227b3..fa381a15e3 100644
--- a/src/modules/uORB/objects_common.cpp
+++ b/src/modules/uORB/objects_common.cpp
@@ -229,8 +229,8 @@ ORB_DEFINE(test_motor, struct test_motor_s);
 #include "topics/debug_key_value.h"
 ORB_DEFINE(debug_key_value, struct debug_key_value_s);
 
-#include "topics/navigation_capabilities.h"
-ORB_DEFINE(navigation_capabilities, struct navigation_capabilities_s);
+#include "topics/fw_pos_ctrl_status.h"
+ORB_DEFINE(fw_pos_ctrl_status, struct fw_pos_ctrl_status_s);
 
 #include "topics/esc_status.h"
 ORB_DEFINE(esc_status, struct esc_status_s);

From be391b4fe52c82150ea46d11eaeee18e9b75a8ca Mon Sep 17 00:00:00 2001
From: Daniel Agar <daniel@agar.ca>
Date: Thu, 21 Apr 2016 17:28:21 -0400
Subject: [PATCH 0067/1230] unittests stop building in tree

---
 Makefile | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/Makefile b/Makefile
index b46f619f4a..e1e89f9d4d 100644
--- a/Makefile
+++ b/Makefile
@@ -245,7 +245,7 @@ ifeq ($(VECTORCONTROL),1)
 endif
 
 unittest: posix_sitl_default
-	@(cd unittests && cmake -G$(PX4_CMAKE_GENERATOR) && $(PX4_MAKE) $(PX4_MAKE_ARGS) && ctest -j2 --output-on-failure)
+	@(mkdir -p build_unittests && cd build_unittests && cmake -G$(PX4_CMAKE_GENERATOR) ../unittests && $(PX4_MAKE) $(PX4_MAKE_ARGS) && ctest -j2 --output-on-failure)
 
 package_firmware:
 	@zip --junk-paths Firmware.zip `find . -name \*.px4`

From 770c6b3bd100fafe0455d7c80bf62f821a68f92a Mon Sep 17 00:00:00 2001
From: Daniel Agar <daniel@agar.ca>
Date: Thu, 21 Apr 2016 22:24:23 -0400
Subject: [PATCH 0068/1230] travis-ci build px4fmu-v2 ekf2 and lpe

---
 Makefile | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/Makefile b/Makefile
index e1e89f9d4d..911f57d942 100644
--- a/Makefile
+++ b/Makefile
@@ -222,7 +222,7 @@ ifeq ($(VECTORCONTROL),1)
 	@(rm -rf vectorcontrol && git clone --quiet --depth 1 https://github.com/thiemar/vectorcontrol.git && cd vectorcontrol && BOARD=s2740vc_1_0 make --silent --no-print-directory && BOARD=px4esc_1_6 make --silent --no-print-directory && ../Tools/uavcan_copy.sh)
 endif
 
-check: check_px4fmu-v1_default check_px4fmu-v2_default check_px4fmu-v4_default_and_uavcan check_mindpx-v2_default check_px4-stm32f4discovery_default check_posix_sitl_default check_unittest check_format
+check: check_px4fmu-v1_default check_px4fmu-v2_default check_px4fmu-v2_ekf2 check_px4fmu-v2_lpe check_px4fmu-v4_default_and_uavcan check_mindpx-v2_default check_px4-stm32f4discovery_default check_posix_sitl_default check_unittest check_format
 
 check_format:
 	$(call colorecho,"Checking formatting with astyle")

From 875dbb92e58c6777fc21b526bb36b9cecf418156 Mon Sep 17 00:00:00 2001
From: Daniel Agar <daniel@agar.ca>
Date: Thu, 21 Apr 2016 17:29:49 -0400
Subject: [PATCH 0069/1230] check code style parallel

---
 Makefile                      |  2 +-
 Tools/check_code_style.sh     | 79 ++++++-----------------------------
 Tools/check_code_style_all.sh | 52 +++++++++++++++++++++++
 3 files changed, 66 insertions(+), 67 deletions(-)
 create mode 100755 Tools/check_code_style_all.sh

diff --git a/Makefile b/Makefile
index 911f57d942..437864293a 100644
--- a/Makefile
+++ b/Makefile
@@ -227,7 +227,7 @@ check: check_px4fmu-v1_default check_px4fmu-v2_default check_px4fmu-v2_ekf2 chec
 check_format:
 	$(call colorecho,"Checking formatting with astyle")
 	@./Tools/fix_code_style.sh
-	@./Tools/check_code_style.sh
+	@./Tools/check_code_style_all.sh
 
 check_%:
 	@echo
diff --git a/Tools/check_code_style.sh b/Tools/check_code_style.sh
index 2f12ce5bc7..40614dd92e 100755
--- a/Tools/check_code_style.sh
+++ b/Tools/check_code_style.sh
@@ -1,70 +1,17 @@
 #!/usr/bin/env bash
-set -eu
-failed=0
-for fn in $(find src/examples \
-                 src/systemcmds \
-                 src/include \
-                 src/drivers \
-                 src/platforms \
-                 src/firmware \
-                 src/lib/launchdetection \
-                 src/lib/geo \
-                 src/lib/geo_lookup \
-                 src/lib/conversion \
-                 src/lib/rc \
-                 src/lib/version \
-                 src/modules/attitude_estimator_q \
-                 src/modules/fw_att_control \
-                 src/modules/gpio_led \
-                 src/modules/land_detector \
-                 src/modules/muorb \
-                 src/modules/px4iofirmware \
-                 src/modules/param \
-                 src/modules/sensors \
-                 src/modules/simulator \
-                 src/modules/uORB \
-                 src/modules/bottle_drop \
-                 src/modules/dataman \
-                 src/modules/segway \
-                 src/modules/local_position_estimator \
-                 src/modules/unit_test \
-                 src/modules/systemlib \
-                 src/lib/controllib \
-                   -path './Build' -prune -o \
-                   -path './mavlink' -prune -o \
-                   -path './NuttX' -prune -o \
-                   -path './src/lib/eigen' -prune -o \
-                   -path './src/modules/uavcan/libuavcan' -prune -o \
-                   -path './src/modules/attitude_estimator_ekf/codegen' -prune -o \
-                   -path './src/modules/ekf_att_pos_estimator' -prune -o \
-                   -path './src/modules/sdlog2' -prune -o \
-                   -path './src/modules/uORB' -prune -o \
-                   -path './src/modules/vtol_att_control' -prune -o \
-                   -path './unittests/build' -prune -o \
-                   -path './unittests/gtest' -prune -o \
-                   -name '*.c' -o -name '*.cpp' -o -name '*.hpp' -o -name '*.h' \
-                   -not -name '*generated*' \
-                   -not -name '*uthash.h' \
-                   -not -name '*utstring.h' \
-                   -not -name '*utlist.h' \
-                   -not -name '*utarray.h' \
-                   -type f); do
-  if [ -f "$fn" ];
-  then
-    ./Tools/fix_code_style.sh --quiet < $fn > $fn.pretty
-    diffsize=$(diff -y --suppress-common-lines $fn $fn.pretty | wc -l)
-    rm -f $fn.pretty
-    if [ $diffsize -ne 0 ]; then
-      failed=1
-      echo $fn 'bad formatting, please run "./Tools/fix_code_style.sh' $fn'"'
-    fi
-  fi
-done
 
-if [ $failed -eq 0 ]; then
-    echo "Format checks passed"
-    exit 0
-else
-    echo "Format checks failed"
+file=$1
+
+DIR=$( cd "$( dirname "${BASH_SOURCE[0]}" )" && pwd )
+
+if [ -f "$file" ];
+then
+  ${DIR}/fix_code_style.sh --quiet < $file > $file.pretty
+  diffsize=$(diff -y --suppress-common-lines $file $file.pretty | wc -l)
+  rm -f $file.pretty
+  if [ $diffsize -ne 0 ]; then
+    echo $file 'bad formatting, please run "./Tools/fix_code_style.sh' $file'"'
     exit 1
+  fi
 fi
+
diff --git a/Tools/check_code_style_all.sh b/Tools/check_code_style_all.sh
new file mode 100755
index 0000000000..20124e00a2
--- /dev/null
+++ b/Tools/check_code_style_all.sh
@@ -0,0 +1,52 @@
+#!/usr/bin/env bash
+set -eu
+failed=0
+
+DIR=$( cd "$( dirname "${BASH_SOURCE[0]}" )" && pwd )
+
+find \
+    src/drivers \
+    src/examples \
+    src/firmware \
+    src/include \
+    src/lib/controllib \
+    src/lib/conversion \
+    src/lib/geo \
+    src/lib/geo_lookup \
+    src/lib/launchdetection \
+    src/lib/rc \
+    src/lib/tailsitter_recovery \
+    src/lib/terrain_estimation \
+    src/lib/version \
+    src/modules/attitude_estimator_q \
+    src/modules/bottle_drop \
+    src/modules/controllib_test \
+    src/modules/dataman \
+    src/modules/fw_att_control \
+    src/modules/gpio_led \
+    src/modules/land_detector \
+    src/modules/local_position_estimator \
+    src/modules/muorb \
+    src/modules/param \
+    src/modules/px4iofirmware \
+    src/modules/segway \
+    src/modules/sensors \
+    src/modules/simulator \
+    src/modules/systemlib \
+    src/modules/unit_test \
+    src/modules/uORB \
+    src/platforms \
+    src/systemcmds \
+    -type f \( -name "*.c" -o -name "*.h" -o -name "*.cpp" -o -name "*.hpp" \) \
+    -not -name '*generated.h' \
+    -not -name '*uthash.h' \
+    -not -name '*utstring.h' \
+    -not -name '*utlist.h' \
+    -not -name '*utarray.h' \
+    -print0 | xargs -0 -n 1 -P 8 -I % ${DIR}/check_code_style.sh %
+
+
+if [ $? -eq 0 ]; then
+    echo "Format checks passed"
+    exit 0
+fi

From 848e87ff7656042e73a909fd2c677142c14d9000 Mon Sep 17 00:00:00 2001
From: Daniel Agar <daniel@agar.ca>
Date: Fri, 22 Apr 2016 13:40:31 -0400
Subject: [PATCH 0070/1230] lis3mdl use STACK_MAIN

---
 src/drivers/lis3mdl/CMakeLists.txt | 1 -
 1 file changed, 1 deletion(-)

diff --git a/src/drivers/lis3mdl/CMakeLists.txt b/src/drivers/lis3mdl/CMakeLists.txt
index 4eeea263e4..d97e1dc221 100644
--- a/src/drivers/lis3mdl/CMakeLists.txt
+++ b/src/drivers/lis3mdl/CMakeLists.txt
@@ -33,7 +33,6 @@
 px4_add_module(
 	MODULE drivers__lis3mdl
 	MAIN lis3mdl
-
 	STACK_MAIN 1200
 	COMPILE_FLAGS
 		-Weffc++

From 2bc74fd5d956a22636af71451c748f0906936b78 Mon Sep 17 00:00:00 2001
From: Daniel Agar <daniel@agar.ca>
Date: Fri, 22 Apr 2016 15:15:37 -0400
Subject: [PATCH 0071/1230] restore px4fmu-v2_test

---
 Makefile                                      |   5 +-
 ROMFS/px4fmu_test/init.d/rc.sensors           | 201 ++++++++
 ROMFS/px4fmu_test/init.d/rcS                  |  87 +---
 Tools/check_code_style_all.sh                 |   1 +
 Tools/px_process_airframes.py                 |   2 +-
 Tools/px_process_params.py                    |   2 +-
 cmake/configs/nuttx_px4fmu-v2_test.cmake      | 202 ++++++++
 src/firmware/nuttx/CMakeLists.txt             |  11 +-
 src/modules/commander/commander.cpp           |   6 +-
 .../state_machine_helper_test.cpp             |  98 ++--
 src/modules/controllib_test/test_params.c     |  44 ++
 .../mavlink/mavlink_tests/CMakeLists.txt      |   1 -
 .../mavlink_tests/mavlink_ftp_test.cpp        | 445 ++++++++++--------
 .../mavlink/mavlink_tests/mavlink_ftp_test.h  |  61 +--
 src/systemcmds/tests/test_params.c            |  12 +-
 src/systemcmds/tests/tests_main.c             |   9 +-
 16 files changed, 818 insertions(+), 369 deletions(-)
 create mode 100644 ROMFS/px4fmu_test/init.d/rc.sensors
 create mode 100644 cmake/configs/nuttx_px4fmu-v2_test.cmake

diff --git a/Makefile b/Makefile
index 437864293a..61b9a05337 100644
--- a/Makefile
+++ b/Makefile
@@ -136,6 +136,9 @@ px4fmu-v1_default:
 
 px4fmu-v2_default:
 	$(call cmake-build,nuttx_px4fmu-v2_default)
+	
+px4fmu-v2_test:
+	$(call cmake-build,nuttx_px4fmu-v2_test)
 
 px4fmu-v4_default:
 	$(call cmake-build,nuttx_px4fmu-v4_default)
@@ -222,7 +225,7 @@ ifeq ($(VECTORCONTROL),1)
 	@(rm -rf vectorcontrol && git clone --quiet --depth 1 https://github.com/thiemar/vectorcontrol.git && cd vectorcontrol && BOARD=s2740vc_1_0 make --silent --no-print-directory && BOARD=px4esc_1_6 make --silent --no-print-directory && ../Tools/uavcan_copy.sh)
 endif
 
-check: check_px4fmu-v1_default check_px4fmu-v2_default check_px4fmu-v2_ekf2 check_px4fmu-v2_lpe check_px4fmu-v4_default_and_uavcan check_mindpx-v2_default check_px4-stm32f4discovery_default check_posix_sitl_default check_unittest check_format
+check: check_px4fmu-v1_default check_px4fmu-v2_default check_px4fmu-v2_test check_px4fmu-v2_ekf2 check_px4fmu-v2_lpe check_px4fmu-v4_default_and_uavcan check_mindpx-v2_default check_px4-stm32f4discovery_default check_posix_sitl_default check_unittest check_format
 
 check_format:
 	$(call colorecho,"Checking formatting with astyle")
diff --git a/ROMFS/px4fmu_test/init.d/rc.sensors b/ROMFS/px4fmu_test/init.d/rc.sensors
new file mode 100644
index 0000000000..9a4d12192e
--- /dev/null
+++ b/ROMFS/px4fmu_test/init.d/rc.sensors
@@ -0,0 +1,201 @@
+#!nsh
+#
+# Standard startup script for PX4FMU v1, v2, v3, v4 onboard sensor drivers.
+#
+
+if ver hwcmp PX4FMU_V1
+then
+	if ms5611 start
+	then
+	fi
+else
+	# Configure all I2C buses to 100 KHz as they
+	# are all external or slow
+	fmu i2c 1 100000
+	fmu i2c 2 100000
+
+	if ms5611 -s start
+	then
+	fi
+
+	# Blacksheep telemetry
+	if bst start
+	then
+	fi
+fi
+
+if adc start
+then
+fi
+
+if ver hwcmp PX4FMU_V2
+then
+	# External I2C bus
+	if hmc5883 -C -T -X start
+	then
+	fi
+
+	# External I2C bus
+	if lis3mdl -X start
+	then
+	fi
+
+	# Internal I2C bus
+	if hmc5883 -C -T -I -R 4 start
+	then
+	fi
+
+	# external MPU6K is rotated 180 degrees yaw
+	if mpu6000 -X -R 4 start
+	then
+		set BOARD_FMUV3 true
+	else
+		set BOARD_FMUV3 false
+	fi
+
+	if [ $BOARD_FMUV3 == true ]
+	then
+		# external L3GD20H is rotated 180 degrees yaw
+		if l3gd20 -X -R 4 start
+		then
+		fi
+
+		# external LSM303D is rotated 270 degrees yaw
+		if lsm303d -X -R 6 start
+		then
+		fi
+
+		# internal MPU6000 is rotated 180 deg roll, 270 deg yaw
+		if mpu6000 -R 14 start
+		then
+		fi
+
+		if hmc5883 -C -T -S -R 8 start
+		then
+		fi
+
+		if meas_airspeed start -b 2
+		then
+		fi
+
+	else
+		# FMUv2
+		if mpu6000 start
+		then
+		fi
+
+		if l3gd20 start
+		then
+		fi
+
+		if lsm303d start
+		then
+		fi
+	fi
+fi
+
+if ver hwcmp PX4FMU_V4
+then
+	# External I2C bus
+	if hmc5883 -C -T -X start
+	then
+	fi
+
+	if lis3mdl -R 2 start
+	then
+	fi
+
+	# Internal SPI bus is rotated 90 deg yaw
+	if hmc5883 -C -T -S -R 2 start
+	then
+	fi
+
+	# Internal SPI bus ICM-20608-G is rotated 90 deg yaw
+	if mpu6000 -R 2 -T 20608 start
+	then
+	fi
+
+	# Internal SPI bus mpu9250 is rotated 90 deg yaw
+	if mpu9250 -R 2 start
+	then
+	fi
+fi
+
+if ver hwcmp PX4FMU_V1
+then
+	# FMUv1
+	if mpu6000 start
+	then
+	fi
+
+	if l3gd20 start
+	then
+	fi
+
+	# MAG selection
+	if param compare SENS_EXT_MAG 2
+	then
+		if hmc5883 -C -I start
+		then
+		fi
+	else
+		# Use only external as primary
+		if param compare SENS_EXT_MAG 1
+		then
+			if hmc5883 -C -X start
+			then
+			fi
+		else
+		# auto-detect the primary, prefer external
+			if hmc5883 start
+			then
+			fi
+		fi
+	fi
+fi
+
+if ver hwcmp MINDPX_V2
+then
+	if mpu6500 start
+	then
+	fi
+
+	if lsm303d start
+	then
+	fi
+
+	if l3gd20 start
+	then
+	fi
+
+	# External I2C bus
+	if hmc5883 -C -T -X start
+	then
+	fi
+
+	if lis3mdl -R 2 start
+	then
+	fi
+fi
+
+if meas_airspeed start
+then
+else
+	if ets_airspeed start
+	then
+	else
+		if ets_airspeed start -b 1
+		then
+		fi
+	fi
+fi
+
+if sf10a start
+then
+fi
+
+# Wait 20 ms for sensors (because we need to wait for the HRT and work queue callbacks to fire)
+usleep 20000
+if sensors start
+then
+fi
diff --git a/ROMFS/px4fmu_test/init.d/rcS b/ROMFS/px4fmu_test/init.d/rcS
index 0df6c29071..abb4f3dfb8 100644
--- a/ROMFS/px4fmu_test/init.d/rcS
+++ b/ROMFS/px4fmu_test/init.d/rcS
@@ -116,11 +116,19 @@ else
 	set unit_test_failure_list "${unit_test_failure_list} mavlink_tests"
 fi
 
-if commander_tests
+# TODO: commander_tests is currently broken
+#if commander_tests
+#then
+#else
+#	set unit_test_failure 1
+#	set unit_test_failure_list "${unit_test_failure_list} commander_tests"
+#fi
+
+if controllib_test
 then
 else
 	set unit_test_failure 1
-	set unit_test_failure_list "${unit_test_failure_list} commander_tests"
+	set unit_test_failure_list "${unit_test_failure_list} controllib_tests"
 fi
 
 if uorb test
@@ -130,77 +138,10 @@ else
 	set unit_test_failure_list "${unit_test_failure_list} uorb_tests"
 fi
 
-# Start all sensors on all boards
-ms5611 start
-adc start
-
-if mpu6000 -X start
-then
-fi
-
-if mpu6000 start
-then
-fi
-
-if l3gd20 -X start
-then
-fi
-
-if l3gd20 start
-then
-fi
-
-# MAG selection
-if param compare SENS_EXT_MAG 2
-then
-	if hmc5883 -I start
-	then
-	fi
-else
-	# Use only external as primary
-	if param compare SENS_EXT_MAG 1
-	then
-		if hmc5883 -X start
-		then
-		fi
-	else
-	# auto-detect the primary, prefer external
-		if hmc5883 start
-		then
-		fi
-	fi
-fi
-
-if ver hwcmp PX4FMU_V2
-then
-	if lsm303d -X start
-	then
-	fi
-
-	if lsm303d start
-	then
-	fi
-
-	if ms5611 -X start
-	then
-	fi
-fi
-
-if meas_airspeed start
-then
-else
-	if ets_airspeed start
-	then
-	else
-		if ets_airspeed start -b 1
-		then
-		fi
-	fi
-fi
-
-if sensors start
-then
-fi
+#
+# Sensors System (start before Commander so Preflight checks are properly run)
+#
+sh /etc/init.d/rc.sensors
 
 # Check for flow sensor
 if px4flow start
diff --git a/Tools/check_code_style_all.sh b/Tools/check_code_style_all.sh
index 20124e00a2..eba8648f25 100755
--- a/Tools/check_code_style_all.sh
+++ b/Tools/check_code_style_all.sh
@@ -26,6 +26,7 @@ find \
     src/modules/gpio_led \
     src/modules/land_detector \
     src/modules/local_position_estimator \
+    src/modules/mavlink/mavlink_tests \
     src/modules/muorb \
     src/modules/param \
     src/modules/px4iofirmware \
diff --git a/Tools/px_process_airframes.py b/Tools/px_process_airframes.py
index aa7e57a8c8..05f9c85f2a 100755
--- a/Tools/px_process_airframes.py
+++ b/Tools/px_process_airframes.py
@@ -71,7 +71,7 @@ def main():
                          const="",
                          metavar="BOARD",
                          help="Board to create airframes xml for")
-    parser.add_argument("-v", "--verbose", help="verbose output")
+    parser.add_argument('-v', '--verbose', action='store_true', help="verbose output")
     args = parser.parse_args()
 
     # Check for valid command
diff --git a/Tools/px_process_params.py b/Tools/px_process_params.py
index 29206275bf..7bac8834d3 100644
--- a/Tools/px_process_params.py
+++ b/Tools/px_process_params.py
@@ -107,7 +107,7 @@ def main():
                         metavar="SUMMARY",
                         default="Automagically updated parameter documentation from code.",
                         help="DokuWiki page edit summary")
-    parser.add_argument("-v", "--verbose", help="verbose output")
+    parser.add_argument('-v', '--verbose', action='store_true', help="verbose output")
     args = parser.parse_args()
 
     # Check for valid command
diff --git a/cmake/configs/nuttx_px4fmu-v2_test.cmake b/cmake/configs/nuttx_px4fmu-v2_test.cmake
new file mode 100644
index 0000000000..0adab48b85
--- /dev/null
+++ b/cmake/configs/nuttx_px4fmu-v2_test.cmake
@@ -0,0 +1,202 @@
+include(nuttx/px4_impl_nuttx)
+
+set(CMAKE_TOOLCHAIN_FILE ${CMAKE_SOURCE_DIR}/cmake/toolchains/Toolchain-arm-none-eabi.cmake)
+
+set(config_uavcan_num_ifaces 2)
+
+set(config_module_list
+	#
+	# Board support modules
+	#
+	drivers/device
+	drivers/stm32
+	drivers/stm32/adc
+	drivers/stm32/tone_alarm
+	drivers/led
+	drivers/px4fmu
+	drivers/px4io
+	drivers/boards/px4fmu-v2
+	drivers/rgbled
+	drivers/mpu6000
+	#drivers/mpu9250
+	drivers/lsm303d
+	drivers/l3gd20
+	drivers/hmc5883
+	drivers/ms5611
+	#drivers/mb12xx
+	drivers/srf02
+	drivers/sf0x
+	drivers/ll40ls
+	drivers/trone
+	drivers/gps
+	drivers/pwm_out_sim
+	#drivers/hott
+	#drivers/hott/hott_telemetry
+	#drivers/hott/hott_sensors
+	drivers/blinkm
+	drivers/airspeed
+	drivers/ets_airspeed
+	drivers/meas_airspeed
+	drivers/frsky_telemetry
+	modules/sensors
+	#drivers/mkblctrl
+	drivers/px4flow
+	#drivers/oreoled
+	drivers/gimbal
+	drivers/pwm_input
+	drivers/camera_trigger
+	drivers/bst
+	drivers/snapdragon_rc_pwm
+	drivers/lis3mdl
+
+	#
+	# System commands
+	#
+	systemcmds/bl_update
+	systemcmds/mixer
+	systemcmds/param
+	systemcmds/perf
+	systemcmds/pwm
+	systemcmds/esc_calib
+	systemcmds/reboot
+	#systemcmds/topic_listener
+	systemcmds/top
+	systemcmds/config
+	systemcmds/nshterm
+	systemcmds/mtd
+	systemcmds/dumpfile
+	systemcmds/ver
+
+	#
+	# Testing
+	#
+	modules/commander/commander_tests
+	modules/controllib_test
+	modules/mavlink/mavlink_tests
+	modules/unit_test
+	systemcmds/tests
+
+	#
+	# General system control
+	#
+	modules/commander
+	modules/navigator
+	modules/mavlink
+	modules/gpio_led
+	modules/uavcan
+	modules/land_detector
+
+	#
+	# Estimation modules (EKF/ SO3 / other filters)
+	#
+	# Too high RAM usage due to static allocations
+	# modules/attitude_estimator_ekf
+	modules/attitude_estimator_q
+	modules/ekf_att_pos_estimator
+	modules/position_estimator_inav
+
+	#
+	# Vehicle Control
+	#
+	# modules/segway # XXX Needs GCC 4.7 fix
+	modules/fw_pos_control_l1
+	modules/fw_att_control
+	modules/mc_att_control
+	modules/mc_pos_control
+	modules/vtol_att_control
+
+	#
+	# Logging
+	#
+	modules/sdlog2
+
+	#
+	# Library modules
+	#
+	modules/param
+	modules/systemlib
+	modules/systemlib/mixer
+	modules/uORB
+	modules/dataman
+
+	#
+	# Libraries
+	#
+	lib/controllib
+	lib/mathlib
+	lib/mathlib/math/filter
+	lib/ecl
+	lib/external_lgpl
+	lib/geo
+	lib/geo_lookup
+	lib/conversion
+	lib/launchdetection
+	lib/terrain_estimation
+	lib/runway_takeoff
+	lib/tailsitter_recovery
+	platforms/nuttx
+
+	# had to add for cmake, not sure why wasn't in original config
+	platforms/common 
+	platforms/nuttx/px4_layer
+
+	#
+	# OBC challenge
+	#
+	#modules/bottle_drop
+
+	#
+	# Rover apps
+	#
+	#examples/rover_steering_control
+
+	#
+	# Demo apps
+	#
+	#examples/math_demo
+	# Tutorial code from
+	# https://px4.io/dev/px4_simple_app
+	#examples/px4_simple_app
+
+	# Tutorial code from
+	# https://px4.io/dev/daemon
+	#examples/px4_daemon_app
+
+	# Tutorial code from
+	# https://px4.io/dev/debug_values
+	#examples/px4_mavlink_debug
+
+	# Tutorial code from
+	# https://px4.io/dev/example_fixedwing_control
+	#examples/fixedwing_control
+
+	# Hardware test
+	#examples/hwtest
+)
+
+set(config_extra_builtin_cmds
+	serdis
+	sercon
+	)
+
+set(config_io_board
+	px4io-v2
+	)
+
+set(config_extra_libs
+	uavcan
+	uavcan_stm32_driver
+	)
+
+set(config_io_extra_libs
+	)
+
+add_custom_target(sercon)
+set_target_properties(sercon PROPERTIES
+	PRIORITY "SCHED_PRIORITY_DEFAULT"
+	MAIN "sercon" STACK_MAIN "2048")
+
+add_custom_target(serdis)
+set_target_properties(serdis PROPERTIES
+	PRIORITY "SCHED_PRIORITY_DEFAULT"
+	MAIN "serdis" STACK_MAIN "2048")
diff --git a/src/firmware/nuttx/CMakeLists.txt b/src/firmware/nuttx/CMakeLists.txt
index 926d331ec1..73215f797f 100644
--- a/src/firmware/nuttx/CMakeLists.txt
+++ b/src/firmware/nuttx/CMakeLists.txt
@@ -45,16 +45,21 @@ if(NOT ${BOARD} STREQUAL "sim")
 	if (config_io_board)
 		set(extras "${CMAKE_BINARY_DIR}/src/modules/px4iofirmware/${config_io_board}.bin")
 	endif()
+	
+	set(romfs_dir "ROMFS/px4fmu_common")
+	if (${BOARD} STREQUAL "px4fmu-v2" AND ${LABEL} STREQUAL "test")
+	    set(romfs_dir "ROMFS/px4fmu_test")
+	endif()
 
 	px4_nuttx_add_romfs(OUT romfs
-		ROOT ROMFS/px4fmu_common
+		ROOT ${romfs_dir}
 		EXTRAS ${extras}
 		)
 	if (config_io_board)
 	    add_dependencies(romfs fw_io)
 	endif()
-	set(fw_file
-		${CMAKE_CURRENT_BINARY_DIR}/${OS}-${BOARD}-${LABEL}.px4)
+
+	set(fw_file ${CMAKE_CURRENT_BINARY_DIR}/${OS}-${BOARD}-${LABEL}.px4)
 
 	px4_nuttx_add_firmware(OUT ${fw_file}
 		BOARD ${BOARD}
diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp
index 3df0fbedeb..8f6452ee85 100644
--- a/src/modules/commander/commander.cpp
+++ b/src/modules/commander/commander.cpp
@@ -509,9 +509,7 @@ int commander_main(int argc, char *argv[])
 				warnx("argument %s unsupported.", argv[2]);
 			}
 
-			if (TRANSITION_DENIED == main_state_transition(&status, new_main_state, main_state_prev,
-								       &status_flags, &internal_state)) {
-					;
+			if (TRANSITION_DENIED == main_state_transition(&status, new_main_state, main_state_prev,  &status_flags, &internal_state)) {
 				warnx("mode change failed");
 			}
 			return 0;
@@ -1260,7 +1258,7 @@ int commander_thread_main(int argc, char *argv[])
 	// We want to accept RC inputs as default
 	status_flags.rc_input_blocked = false;
 	status.rc_input_mode = vehicle_status_s::RC_IN_MODE_DEFAULT;
-	internal_state.main_state =commander_state_s::MAIN_STATE_MANUAL;
+	internal_state.main_state = commander_state_s::MAIN_STATE_MANUAL;
 	main_state_prev = commander_state_s::MAIN_STATE_MAX;
 	status.nav_state = vehicle_status_s::NAVIGATION_STATE_MANUAL;
 	status.arming_state = vehicle_status_s::ARMING_STATE_INIT;
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 d50d0a4d92..2c8b4675dd 100644
--- a/src/modules/commander/commander_tests/state_machine_helper_test.cpp
+++ b/src/modules/commander/commander_tests/state_machine_helper_test.cpp
@@ -268,9 +268,11 @@ bool StateMachineHelperTest::armingStateTransitionTest(void)
             { vehicle_status_s::ARMING_STATE_STANDBY, ATT_DISARMED, ATT_READY_TO_ARM }, TRANSITION_DENIED },
     };
 
-	struct vehicle_status_s status;
-	struct safety_s         safety;
-	struct actuator_armed_s armed;
+	struct vehicle_status_s status = {};
+	struct status_flags_s status_flags = {};
+	struct safety_s         safety = {};
+	struct actuator_armed_s armed = {};
+	struct battery_status_s battery = {};
 
     size_t cArmingTransitionTests = sizeof(rgArmingTransitionTests) / sizeof(rgArmingTransitionTests[0]);
     for (size_t i=0; i<cArmingTransitionTests; i++) {
@@ -278,17 +280,21 @@ bool StateMachineHelperTest::armingStateTransitionTest(void)
 
         // Setup initial machine state
         status.arming_state = test->current_state.arming_state;
-        status.condition_system_sensors_initialized = test->condition_system_sensors_initialized;
+        status_flags.condition_system_sensors_initialized = test->condition_system_sensors_initialized;
         status.hil_state = test->hil_state;
         // The power status of the test unit is not relevant for the unit test
-        status.circuit_breaker_engaged_power_check = true;
+        status_flags.circuit_breaker_engaged_power_check = true;
         safety.safety_switch_available = test->safety_switch_available;
         safety.safety_off = test->safety_off;
         armed.armed = test->current_state.armed;
         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, false /* no pre-arm checks */, nullptr /* no mavlink_log_pub */);
+        transition_result_t result = arming_state_transition(&status, &battery, &safety, test->requested_state, &armed,
+        		false /* no pre-arm checks */,
+        		nullptr /* no mavlink_log_pub */,
+        		&status_flags,
+        		5.0f);
 
         // Validate result of transition
         ut_assert(test->assertMsg, test->expected_transition_result == result);
@@ -304,10 +310,10 @@ bool StateMachineHelperTest::mainStateTransitionTest(void)
 {
 	// This structure represent a single test case for testing Main State transitions.
 	typedef struct {
-		const char*     assertMsg;				// Text to show when test case fails
-		uint8_t		condition_bits;				// Bits for various condition_* values
-		main_state_t	from_state;				// State prior to transition request
-		main_state_t	to_state;				// State to transition to
+		const char* assertMsg;				// Text to show when test case fails
+		uint8_t	 condition_bits;			// Bits for various condition_* values
+		uint8_t	from_state;				// State prior to transition request
+		main_state_t to_state;				// State to transition to
 		transition_result_t	expected_transition_result;	// Expected result from main_state_transition call
 	} MainTransitionTest_t;
 
@@ -325,99 +331,99 @@ bool StateMachineHelperTest::mainStateTransitionTest(void)
 
 		{ "no transition: identical states",
 			MTT_ALL_NOT_VALID,
-			vehicle_status_s::MAIN_STATE_MANUAL, vehicle_status_s::MAIN_STATE_MANUAL, TRANSITION_NOT_CHANGED },
+			commander_state_s::MAIN_STATE_MANUAL, commander_state_s::MAIN_STATE_MANUAL, TRANSITION_NOT_CHANGED },
 
 		// TRANSITION_CHANGED tests
 
 		{ "transition: MANUAL to ACRO",
 			MTT_ALL_NOT_VALID,
-			vehicle_status_s::MAIN_STATE_MANUAL, vehicle_status_s::MAIN_STATE_ACRO, TRANSITION_CHANGED },
+			commander_state_s::MAIN_STATE_MANUAL, commander_state_s::MAIN_STATE_ACRO, TRANSITION_CHANGED },
 
 		{ "transition: ACRO to MANUAL",
 			MTT_ALL_NOT_VALID,
-			vehicle_status_s::MAIN_STATE_ACRO, vehicle_status_s::MAIN_STATE_MANUAL, TRANSITION_CHANGED },
+			commander_state_s::MAIN_STATE_ACRO, commander_state_s::MAIN_STATE_MANUAL, TRANSITION_CHANGED },
 
 		{ "transition: MANUAL to AUTO_MISSION - global position valid, home position valid",
 			MTT_GLOBAL_POS_VALID | MTT_HOME_POS_VALID,
-			vehicle_status_s::MAIN_STATE_MANUAL, vehicle_status_s::MAIN_STATE_AUTO_MISSION, TRANSITION_CHANGED },
+			commander_state_s::MAIN_STATE_MANUAL, commander_state_s::MAIN_STATE_AUTO_MISSION, TRANSITION_CHANGED },
 
 		{ "transition: AUTO_MISSION to MANUAL - global position valid, home position valid",
 			MTT_GLOBAL_POS_VALID | MTT_HOME_POS_VALID,
-			vehicle_status_s::MAIN_STATE_AUTO_MISSION, vehicle_status_s::MAIN_STATE_MANUAL, TRANSITION_CHANGED },
+			commander_state_s::MAIN_STATE_AUTO_MISSION, commander_state_s::MAIN_STATE_MANUAL, TRANSITION_CHANGED },
 
 		{ "transition: MANUAL to AUTO_LOITER - global position valid",
 			MTT_GLOBAL_POS_VALID,
-			vehicle_status_s::MAIN_STATE_MANUAL, vehicle_status_s::MAIN_STATE_AUTO_LOITER, TRANSITION_CHANGED },
+			commander_state_s::MAIN_STATE_MANUAL, commander_state_s::MAIN_STATE_AUTO_LOITER, TRANSITION_CHANGED },
 
 		{ "transition: AUTO_LOITER to MANUAL - global position valid",
 			MTT_GLOBAL_POS_VALID,
-			vehicle_status_s::MAIN_STATE_AUTO_LOITER, vehicle_status_s::MAIN_STATE_MANUAL, TRANSITION_CHANGED },
+			commander_state_s::MAIN_STATE_AUTO_LOITER, commander_state_s::MAIN_STATE_MANUAL, TRANSITION_CHANGED },
 
 		{ "transition: MANUAL to AUTO_RTL - global position valid, home position valid",
 			MTT_GLOBAL_POS_VALID | MTT_HOME_POS_VALID,
-			vehicle_status_s::MAIN_STATE_MANUAL, vehicle_status_s::MAIN_STATE_AUTO_RTL, TRANSITION_CHANGED },
+			commander_state_s::MAIN_STATE_MANUAL, commander_state_s::MAIN_STATE_AUTO_RTL, TRANSITION_CHANGED },
 
 		{ "transition: AUTO_RTL to MANUAL - global position valid, home position valid",
 			MTT_GLOBAL_POS_VALID | MTT_HOME_POS_VALID,
-			vehicle_status_s::MAIN_STATE_AUTO_RTL, vehicle_status_s::MAIN_STATE_MANUAL, TRANSITION_CHANGED },
+			commander_state_s::MAIN_STATE_AUTO_RTL, commander_state_s::MAIN_STATE_MANUAL, TRANSITION_CHANGED },
 
 		{ "transition: MANUAL to ALTCTL - not rotary",
 			MTT_ALL_NOT_VALID,
-			vehicle_status_s::MAIN_STATE_MANUAL, vehicle_status_s::MAIN_STATE_ALTCTL, TRANSITION_CHANGED },
+			commander_state_s::MAIN_STATE_MANUAL, commander_state_s::MAIN_STATE_ALTCTL, TRANSITION_CHANGED },
 
 		{ "transition: MANUAL to ALTCTL - rotary, global position not valid, local altitude valid",
 			MTT_ROTARY_WING | MTT_LOC_ALT_VALID,
-			vehicle_status_s::MAIN_STATE_MANUAL, vehicle_status_s::MAIN_STATE_ALTCTL, TRANSITION_CHANGED },
+			commander_state_s::MAIN_STATE_MANUAL, commander_state_s::MAIN_STATE_ALTCTL, TRANSITION_CHANGED },
 
 		{ "transition: MANUAL to ALTCTL - rotary, global position valid, local altitude not valid",
 			MTT_ROTARY_WING | MTT_GLOBAL_POS_VALID,
-			vehicle_status_s::MAIN_STATE_MANUAL, vehicle_status_s::MAIN_STATE_ALTCTL, TRANSITION_CHANGED },
+			commander_state_s::MAIN_STATE_MANUAL, commander_state_s::MAIN_STATE_ALTCTL, TRANSITION_CHANGED },
 
 		{ "transition: ALTCTL to MANUAL - local altitude valid",
 			MTT_LOC_ALT_VALID,
-			vehicle_status_s::MAIN_STATE_ALTCTL, vehicle_status_s::MAIN_STATE_MANUAL, TRANSITION_CHANGED },
+			commander_state_s::MAIN_STATE_ALTCTL, commander_state_s::MAIN_STATE_MANUAL, TRANSITION_CHANGED },
 
 		{ "transition: MANUAL to POSCTL - local position not valid, global position valid",
 			MTT_GLOBAL_POS_VALID,
-			vehicle_status_s::MAIN_STATE_MANUAL, vehicle_status_s::MAIN_STATE_POSCTL, TRANSITION_CHANGED },
+			commander_state_s::MAIN_STATE_MANUAL, commander_state_s::MAIN_STATE_POSCTL, TRANSITION_CHANGED },
 
 		{ "transition: MANUAL to POSCTL - local position valid, global position not valid",
 			MTT_LOC_POS_VALID,
-			vehicle_status_s::MAIN_STATE_MANUAL, vehicle_status_s::MAIN_STATE_POSCTL, TRANSITION_CHANGED },
+			commander_state_s::MAIN_STATE_MANUAL, commander_state_s::MAIN_STATE_POSCTL, TRANSITION_CHANGED },
 
 		{ "transition: POSCTL to MANUAL - local position valid, global position valid",
 			MTT_LOC_POS_VALID,
-			vehicle_status_s::MAIN_STATE_POSCTL, vehicle_status_s::MAIN_STATE_MANUAL, TRANSITION_CHANGED },
+			commander_state_s::MAIN_STATE_POSCTL, commander_state_s::MAIN_STATE_MANUAL, TRANSITION_CHANGED },
 
 		// TRANSITION_DENIED tests
 
 		{ "no transition: MANUAL to AUTO_MISSION - global position not valid",
 			MTT_ALL_NOT_VALID,
-			vehicle_status_s::MAIN_STATE_MANUAL, vehicle_status_s::MAIN_STATE_AUTO_MISSION, TRANSITION_DENIED },
+			commander_state_s::MAIN_STATE_MANUAL, commander_state_s::MAIN_STATE_AUTO_MISSION, TRANSITION_DENIED },
 
 		{ "no transition: MANUAL to AUTO_LOITER - global position not valid",
 			MTT_ALL_NOT_VALID,
-			vehicle_status_s::MAIN_STATE_MANUAL, vehicle_status_s::MAIN_STATE_AUTO_LOITER, TRANSITION_DENIED },
+			commander_state_s::MAIN_STATE_MANUAL, commander_state_s::MAIN_STATE_AUTO_LOITER, TRANSITION_DENIED },
 
 		{ "no transition: MANUAL to AUTO_RTL - global position not valid, home position not valid",
 			MTT_ALL_NOT_VALID,
-			vehicle_status_s::MAIN_STATE_MANUAL, vehicle_status_s::MAIN_STATE_AUTO_RTL, TRANSITION_DENIED },
+			commander_state_s::MAIN_STATE_MANUAL, commander_state_s::MAIN_STATE_AUTO_RTL, TRANSITION_DENIED },
 
 		{ "no transition: MANUAL to AUTO_RTL - global position not valid, home position valid",
 			MTT_HOME_POS_VALID,
-			vehicle_status_s::MAIN_STATE_MANUAL, vehicle_status_s::MAIN_STATE_AUTO_RTL, TRANSITION_DENIED },
+			commander_state_s::MAIN_STATE_MANUAL, commander_state_s::MAIN_STATE_AUTO_RTL, TRANSITION_DENIED },
 
 		{ "no transition: MANUAL to AUTO_RTL - global position valid, home position not valid",
 			MTT_GLOBAL_POS_VALID,
-			vehicle_status_s::MAIN_STATE_MANUAL, vehicle_status_s::MAIN_STATE_AUTO_RTL, TRANSITION_DENIED },
+			commander_state_s::MAIN_STATE_MANUAL, commander_state_s::MAIN_STATE_AUTO_RTL, TRANSITION_DENIED },
 
 		{ "no transition: MANUAL to ALTCTL - rotary, global position not valid, local altitude not valid",
 			MTT_ROTARY_WING,
-			vehicle_status_s::MAIN_STATE_MANUAL, vehicle_status_s::MAIN_STATE_ALTCTL, TRANSITION_DENIED },
+			commander_state_s::MAIN_STATE_MANUAL, commander_state_s::MAIN_STATE_ALTCTL, TRANSITION_DENIED },
 
 		{ "no transition: MANUAL to POSCTL - local position not valid, global position not valid",
 			MTT_ALL_NOT_VALID,
-			vehicle_status_s::MAIN_STATE_MANUAL, vehicle_status_s::MAIN_STATE_POSCTL, TRANSITION_DENIED },
+			commander_state_s::MAIN_STATE_MANUAL, commander_state_s::MAIN_STATE_POSCTL, TRANSITION_DENIED },
 	};
 
 	size_t cMainTransitionTests = sizeof(rgMainTransitionTests) / sizeof(rgMainTransitionTests[0]);
@@ -425,24 +431,30 @@ bool StateMachineHelperTest::mainStateTransitionTest(void)
 		const MainTransitionTest_t* test = &rgMainTransitionTests[i];
 
 		// Setup initial machine state
-		struct vehicle_status_s current_state;
-		current_state.main_state = test->from_state;
-		current_state.is_rotary_wing = test->condition_bits & MTT_ROTARY_WING;
-		current_state.condition_local_altitude_valid = test->condition_bits & MTT_LOC_ALT_VALID;
-		current_state.condition_local_position_valid = test->condition_bits & MTT_LOC_POS_VALID;
-		current_state.condition_home_position_valid = test->condition_bits & MTT_HOME_POS_VALID;
-		current_state.condition_global_position_valid = test->condition_bits & MTT_GLOBAL_POS_VALID;
+		struct vehicle_status_s current_vehicle_status = {};
+		struct commander_state_s current_commander_state = {};
+		struct status_flags_s current_status_flags = {};
+
+		uint8_t main_state_prev = 0;
+
+		current_commander_state.main_state = test->from_state;
+		current_vehicle_status.is_rotary_wing = test->condition_bits & MTT_ROTARY_WING;
+		current_status_flags.condition_local_altitude_valid = test->condition_bits & MTT_LOC_ALT_VALID;
+		current_status_flags.condition_local_position_valid = test->condition_bits & MTT_LOC_POS_VALID;
+		current_status_flags.condition_home_position_valid = test->condition_bits & MTT_HOME_POS_VALID;
+		current_status_flags.condition_global_position_valid = test->condition_bits & MTT_GLOBAL_POS_VALID;
 
 		// Attempt transition
-		transition_result_t result = main_state_transition(&current_state, test->to_state);
+		transition_result_t result = main_state_transition(&current_vehicle_status, test->to_state, main_state_prev,
+									&current_status_flags, &current_commander_state);
 
 		// Validate result of transition
 		ut_assert(test->assertMsg, test->expected_transition_result == result);
 		if (test->expected_transition_result == result) {
 			if (test->expected_transition_result == TRANSITION_CHANGED) {
-				ut_assert(test->assertMsg, test->to_state == current_state.main_state);
+				ut_assert(test->assertMsg, test->to_state == current_commander_state.main_state);
 			} else {
-				ut_assert(test->assertMsg, test->from_state == current_state.main_state);
+				ut_assert(test->assertMsg, test->from_state == current_commander_state.main_state);
 			}
 		}
 	}
diff --git a/src/modules/controllib_test/test_params.c b/src/modules/controllib_test/test_params.c
index 7c609cad39..5ef469b3e2 100644
--- a/src/modules/controllib_test/test_params.c
+++ b/src/modules/controllib_test/test_params.c
@@ -4,19 +4,63 @@
 //  you want to recompute the
 //  answers for all of the unit tests
 
+
+/**
+ * @group Testing
+ */
 PARAM_DEFINE_FLOAT(TEST_MIN, -1.0f);
+
+/**
+ * @group Testing
+ */
 PARAM_DEFINE_FLOAT(TEST_MAX, 1.0f);
+
+/**
+ * @group Testing
+ */
 PARAM_DEFINE_FLOAT(TEST_TRIM, 0.5f);
+
+/**
+ * @group Testing
+ */
 PARAM_DEFINE_FLOAT(TEST_HP, 10.0f);
+
+/**
+ * @group Testing
+ */
 PARAM_DEFINE_FLOAT(TEST_LP, 10.0f);
 
+/**
+ * @group Testing
+ */
 PARAM_DEFINE_FLOAT(TEST_P, 0.2f);
 
+/**
+ * @group Testing
+ */
 PARAM_DEFINE_FLOAT(TEST_I, 0.1f);
+
+/**
+ * @group Testing
+ */
 PARAM_DEFINE_FLOAT(TEST_I_MAX, 1.0f);
 
+/**
+ * @group Testing
+ */
 PARAM_DEFINE_FLOAT(TEST_D, 0.01f);
+
+/**
+ * @group Testing
+ */
 PARAM_DEFINE_FLOAT(TEST_D_LP, 10.0f);
 
+/**
+ * @group Testing
+ */
 PARAM_DEFINE_FLOAT(TEST_MEAN, 1.0f);
+
+/**
+ * @group Testing
+ */
 PARAM_DEFINE_FLOAT(TEST_DEV, 2.0f);
diff --git a/src/modules/mavlink/mavlink_tests/CMakeLists.txt b/src/modules/mavlink/mavlink_tests/CMakeLists.txt
index 710f15cd8f..8096965c3d 100644
--- a/src/modules/mavlink/mavlink_tests/CMakeLists.txt
+++ b/src/modules/mavlink/mavlink_tests/CMakeLists.txt
@@ -35,7 +35,6 @@ px4_add_module(
 	MAIN mavlink_tests
 	STACK_MAIN 5000
 	COMPILE_FLAGS
-		-Weffc++
 		-DMAVLINK_FTP_UNIT_TEST
 		-Wno-attributes
 		-Wno-packed
diff --git a/src/modules/mavlink/mavlink_tests/mavlink_ftp_test.cpp b/src/modules/mavlink/mavlink_tests/mavlink_ftp_test.cpp
index 6fe0ed9c8f..a82c4d45a1 100644
--- a/src/modules/mavlink/mavlink_tests/mavlink_ftp_test.cpp
+++ b/src/modules/mavlink/mavlink_tests/mavlink_ftp_test.cpp
@@ -61,7 +61,7 @@ MavlinkFtpTest::MavlinkFtpTest() :
 
 MavlinkFtpTest::~MavlinkFtpTest()
 {
-	
+
 }
 
 /// @brief Called before every test to initialize the FTP Server.
@@ -78,7 +78,7 @@ void MavlinkFtpTest::_init(void)
 void MavlinkFtpTest::_cleanup(void)
 {
 	delete _ftp_server;
-	
+
 	_cleanup_microsd();
 }
 
@@ -87,20 +87,21 @@ bool MavlinkFtpTest::_ack_test(void)
 {
 	MavlinkFTP::PayloadHeader		payload;
 	const MavlinkFTP::PayloadHeader		*reply;
-	
+
 	payload.opcode = MavlinkFTP::kCmdNone;
 
 	bool success = _send_receive_msg(&payload,	// FTP payload header
 					 0,		// size in bytes of data
 					 nullptr,	// Data to start into FTP message payload
 					 &reply);	// Payload inside FTP message response
+
 	if (!success) {
 		return false;
 	}
-	
+
 	ut_compare("Didn't get Ack back", reply->opcode, MavlinkFTP::kRspAck);
 	ut_compare("Incorrect payload size", reply->size, 0);
-	
+
 	return true;
 }
 
@@ -109,21 +110,22 @@ bool MavlinkFtpTest::_bad_opcode_test(void)
 {
 	MavlinkFTP::PayloadHeader		payload;
 	const MavlinkFTP::PayloadHeader		*reply;
-	
+
 	payload.opcode = 0xFF;	// bogus opcode
 
 	bool success = _send_receive_msg(&payload,	// FTP payload header
 					 0,		// size in bytes of data
 					 nullptr,	// Data to start into FTP message payload
 					 &reply);	// Payload inside FTP message response
+
 	if (!success) {
 		return false;
 	}
-	
+
 	ut_compare("Didn't get Nak back", reply->opcode, MavlinkFTP::kRspNak);
 	ut_compare("Incorrect payload size", reply->size, 1);
 	ut_compare("Incorrect error code", reply->data[0], MavlinkFTP::kErrUnknownCommand);
-	
+
 	return true;
 }
 
@@ -133,24 +135,25 @@ bool MavlinkFtpTest::_bad_datasize_test(void)
 	mavlink_message_t			msg;
 	MavlinkFTP::PayloadHeader		payload;
 	const MavlinkFTP::PayloadHeader		*reply;
-	
+
 	payload.opcode = MavlinkFTP::kCmdListDirectory;
-	
+
 	_setup_ftp_msg(&payload, 0, nullptr, &msg);
-	
+
 	// Set the data size to be one larger than is legal
-	((MavlinkFTP::PayloadHeader*)((mavlink_file_transfer_protocol_t*)msg.payload64)->payload)->size = MAVLINK_MSG_FILE_TRANSFER_PROTOCOL_FIELD_PAYLOAD_LEN + 1;
-	
+	((MavlinkFTP::PayloadHeader *)((mavlink_file_transfer_protocol_t *)msg.payload64)->payload)->size =
+		MAVLINK_MSG_FILE_TRANSFER_PROTOCOL_FIELD_PAYLOAD_LEN + 1;
+
 	_ftp_server->handle_message(&msg);
-	
+
 	if (!_decode_message(&_reply_msg, &reply)) {
 		return false;
 	}
-	
+
 	ut_compare("Didn't get Nak back", reply->opcode, MavlinkFTP::kRspNak);
 	ut_compare("Incorrect payload size", reply->size, 1);
 	ut_compare("Incorrect error code", reply->data[0], MavlinkFTP::kErrInvalidDataSize);
-	
+
 	return true;
 }
 
@@ -158,10 +161,10 @@ bool MavlinkFtpTest::_list_test(void)
 {
 	MavlinkFTP::PayloadHeader		payload;
 	const MavlinkFTP::PayloadHeader		*reply;
-	
+
 	char response1[] = "Dempty_dir|Ftest_238.data\t238|Ftest_239.data\t239|Ftest_240.data\t240";
 	char response2[] = "Ddev|Detc|Dfs|Dobj";
-	
+
 	struct _testCase {
 		const char	*dir;		///< Directory to run List command on
 		char		*response;	///< Expected response entries from List command
@@ -174,56 +177,62 @@ bool MavlinkFtpTest::_list_test(void)
 		{ "/",					response2,	4,	true },
 	};
 
-	for (size_t i=0; i<sizeof(rgTestCases)/sizeof(rgTestCases[0]); i++) {
+	for (size_t i = 0; i < sizeof(rgTestCases) / sizeof(rgTestCases[0]); i++) {
 		const struct _testCase *test = &rgTestCases[i];
-		
+
 		payload.opcode = MavlinkFTP::kCmdListDirectory;
 		payload.offset = 0;
-		
+
 		bool success = _send_receive_msg(&payload,		// FTP payload header
-						strlen(test->dir)+1,	// size in bytes of data
-						(uint8_t*)test->dir,	// Data to start into FTP message payload
-						&reply);		// Payload inside FTP message response
+						 strlen(test->dir) + 1,	// size in bytes of data
+						 (uint8_t *)test->dir,	// Data to start into FTP message payload
+						 &reply);		// Payload inside FTP message response
+
 		if (!success) {
 			return false;
 		}
-		
+
 		if (test->success) {
 			ut_compare("Didn't get Ack back", reply->opcode, MavlinkFTP::kRspAck);
 			ut_compare("Incorrect payload size", reply->size, strlen(test->response) + 1);
-			
+
 			// The return order of directories from the List command is not repeatable. So we can't do a direct comparison
 			// to a hardcoded return result string.
-			
+
 			// Convert null terminators to seperator char so we can use strok to parse returned data
 			char list_entry[256];
-			for (uint8_t j=0; j<reply->size-1; j++) {
+
+			for (uint8_t j = 0; j < reply->size - 1; j++) {
 				if (reply->data[j] == 0) {
 					list_entry[j] = '|';
+
 				} else {
 					list_entry[j] = reply->data[j];
 				}
 			}
-			list_entry[reply->size-1] = 0;
-			
+
+			list_entry[reply->size - 1] = 0;
+
 			// Loop over returned directory entries trying to find then in the response list
 			char *dir;
 			int response_count = 0;
 			dir = strtok(list_entry, "|");
+
 			while (dir != nullptr) {
 				ut_assert("Returned directory not found in expected response", strstr(test->response, dir));
 				response_count++;
 				dir = strtok(nullptr, "|");
 			}
-			
+
 			ut_compare("Incorrect number of directory entires returned", test->response_count, response_count);
+
 		} else {
 			ut_compare("Didn't get Nak back", reply->opcode, MavlinkFTP::kRspNak);
 			ut_compare("Incorrect payload size", reply->size, 2);
 			ut_compare("Incorrect error code", reply->data[0], MavlinkFTP::kErrFailErrno);
 		}
 	}
-	
+
 	return true;
 }
 
@@ -234,14 +243,15 @@ bool MavlinkFtpTest::_list_eof_test(void)
 	MavlinkFTP::PayloadHeader		payload;
 	const MavlinkFTP::PayloadHeader		*reply;
 	const char				*dir = "/";
-	
+
 	payload.opcode = MavlinkFTP::kCmdListDirectory;
 	payload.offset = 4;	// offset past top level dirs
-	
+
 	bool success = _send_receive_msg(&payload,	// FTP payload header
-					strlen(dir)+1,	// size in bytes of data
-					(uint8_t*)dir,	// Data to start into FTP message payload
-					&reply);	// Payload inside FTP message response
+					 strlen(dir) + 1,	// size in bytes of data
+					 (uint8_t *)dir,	// Data to start into FTP message payload
+					 &reply);	// Payload inside FTP message response
+
 	if (!success) {
 		return false;
 	}
@@ -249,7 +259,7 @@ bool MavlinkFtpTest::_list_eof_test(void)
 	ut_compare("Didn't get Nak back", reply->opcode, MavlinkFTP::kRspNak);
 	ut_compare("Incorrect payload size", reply->size, 1);
 	ut_compare("Incorrect error code", reply->data[0], MavlinkFTP::kErrEOF);
-	
+
 	return true;
 }
 
@@ -259,22 +269,23 @@ bool MavlinkFtpTest::_open_badfile_test(void)
 	MavlinkFTP::PayloadHeader		payload;
 	const MavlinkFTP::PayloadHeader		*reply;
 	const char				*dir = "/foo";	// non-existent file
-	
+
 	payload.opcode = MavlinkFTP::kCmdOpenFileRO;
 	payload.offset = 0;
-	
+
 	bool success = _send_receive_msg(&payload,	// FTP payload header
-					strlen(dir)+1,	// size in bytes of data
-					(uint8_t*)dir,	// Data to start into FTP message payload
-					&reply);	// Payload inside FTP message response
+					 strlen(dir) + 1,	// size in bytes of data
+					 (uint8_t *)dir,	// Data to start into FTP message payload
+					 &reply);	// Payload inside FTP message response
+
 	if (!success) {
 		return false;
 	}
-	
+
 	ut_compare("Didn't get Nak back", reply->opcode, MavlinkFTP::kRspNak);
 	ut_compare("Incorrect payload size", reply->size, 2);
 	ut_compare("Incorrect error code", reply->data[0], MavlinkFTP::kErrFailErrno);
-	
+
 	return true;
 }
 
@@ -283,41 +294,43 @@ bool MavlinkFtpTest::_open_terminate_test(void)
 {
 	MavlinkFTP::PayloadHeader		payload;
 	const MavlinkFTP::PayloadHeader		*reply;
-	
-	for (size_t i=0; i<sizeof(_rgDownloadTestCases)/sizeof(_rgDownloadTestCases[0]); i++) {
+
+	for (size_t i = 0; i < sizeof(_rgDownloadTestCases) / sizeof(_rgDownloadTestCases[0]); i++) {
 		struct stat st;
 		const DownloadTestCase *test = &_rgDownloadTestCases[i];
-		
+
 		payload.opcode = MavlinkFTP::kCmdOpenFileRO;
 		payload.offset = 0;
-		
+
 		bool success = _send_receive_msg(&payload,		// FTP payload header
-						 strlen(test->file)+1,	// size in bytes of data
-						 (uint8_t*)test->file,	// Data to start into FTP message payload
+						 strlen(test->file) + 1,	// size in bytes of data
+						 (uint8_t *)test->file,	// Data to start into FTP message payload
 						 &reply);		// Payload inside FTP message response
+
 		if (!success) {
 			return false;
 		}
-		
+
 		ut_compare("stat failed", stat(test->file, &st), 0);
-		
-		
+
+
 		ut_compare("Didn't get Ack back", reply->opcode, MavlinkFTP::kRspAck);
 		ut_compare("Incorrect payload size", reply->size, sizeof(uint32_t));
-		ut_compare("File size incorrect", *((uint32_t*)&reply->data[0]), (uint32_t)st.st_size);
+		ut_compare("File size incorrect", *((uint32_t *)&reply->data[0]), (uint32_t)st.st_size);
 
 		payload.opcode = MavlinkFTP::kCmdTerminateSession;
 		payload.session = reply->session;
 		payload.size = 0;
-		
+
 		success = _send_receive_msg(&payload,	// FTP payload header
 					    0,		// size in bytes of data
 					    nullptr,	// Data to start into FTP message payload
 					    &reply);	// Payload inside FTP message response
+
 		if (!success) {
 			return false;
 		}
-		
+
 		ut_compare("Didn't get Ack back", reply->opcode, MavlinkFTP::kRspAck);
 		ut_compare("Incorrect payload size", reply->size, 0);
 	}
@@ -331,36 +344,38 @@ bool MavlinkFtpTest::_terminate_badsession_test(void)
 	MavlinkFTP::PayloadHeader		payload;
 	const MavlinkFTP::PayloadHeader		*reply;
 	const char				*file = _rgDownloadTestCases[0].file;
-	
+
 	payload.opcode = MavlinkFTP::kCmdOpenFileRO;
 	payload.offset = 0;
-	
+
 	bool success = _send_receive_msg(&payload,	// FTP payload header
-					strlen(file)+1,	// size in bytes of data
-					(uint8_t*)file,	// Data to start into FTP message payload
-					&reply);	// Payload inside FTP message response
+					 strlen(file) + 1,	// size in bytes of data
+					 (uint8_t *)file,	// Data to start into FTP message payload
+					 &reply);	// Payload inside FTP message response
+
 	if (!success) {
 		return false;
 	}
-	
+
 	ut_compare("Didn't get Ack back", reply->opcode, MavlinkFTP::kRspAck);
-	
+
 	payload.opcode = MavlinkFTP::kCmdTerminateSession;
 	payload.session = reply->session + 1;
 	payload.size = 0;
-	
+
 	success = _send_receive_msg(&payload,	// FTP payload header
-				   0,		// size in bytes of data
-				   nullptr,	// Data to start into FTP message payload
-				   &reply);	// Payload inside FTP message response
+				    0,		// size in bytes of data
+				    nullptr,	// Data to start into FTP message payload
+				    &reply);	// Payload inside FTP message response
+
 	if (!success) {
 		return false;
 	}
-	
+
 	ut_compare("Didn't get Nak back", reply->opcode, MavlinkFTP::kRspNak);
 	ut_compare("Incorrect payload size", reply->size, 1);
 	ut_compare("Incorrect error code", reply->data[0], MavlinkFTP::kErrInvalidSession);
-	
+
 	return true;
 }
 
@@ -369,11 +384,11 @@ bool MavlinkFtpTest::_read_test(void)
 {
 	MavlinkFTP::PayloadHeader		payload;
 	const MavlinkFTP::PayloadHeader		*reply;
-	
-	for (size_t i=0; i<sizeof(_rgDownloadTestCases)/sizeof(_rgDownloadTestCases[0]); i++) {
+
+	for (size_t i = 0; i < sizeof(_rgDownloadTestCases) / sizeof(_rgDownloadTestCases[0]); i++) {
 		struct stat st;
 		const DownloadTestCase *test = &_rgDownloadTestCases[i];
-		
+
 		// Read in the file so we can compare it to what we get back
 		ut_compare("stat failed", stat(test->file, &st), 0);
 		uint8_t *bytes = new uint8_t[st.st_size];
@@ -383,35 +398,37 @@ bool MavlinkFtpTest::_read_test(void)
 		int bytes_read = ::read(fd, bytes, st.st_size);
 		ut_compare("read failed", bytes_read, st.st_size);
 		::close(fd);
-		
+
 		// Test case data files are created for specific boundary conditions
 		ut_compare("Test case data files are out of date", test->length, st.st_size);
-		
+
 		payload.opcode = MavlinkFTP::kCmdOpenFileRO;
 		payload.offset = 0;
-		
+
 		bool success = _send_receive_msg(&payload,		// FTP payload header
-						 strlen(test->file)+1,	// size in bytes of data
-						 (uint8_t*)test->file,	// Data to start into FTP message payload
+						 strlen(test->file) + 1,	// size in bytes of data
+						 (uint8_t *)test->file,	// Data to start into FTP message payload
 						 &reply);		// Payload inside FTP message response
+
 		if (!success) {
 			return false;
 		}
-		
+
 		ut_compare("Didn't get Ack back", reply->opcode, MavlinkFTP::kRspAck);
-		
+
 		payload.opcode = MavlinkFTP::kCmdReadFile;
 		payload.session = reply->session;
 		payload.offset = 0;
-		
+
 		success = _send_receive_msg(&payload,	// FTP payload header
 					    0,		// size in bytes of data
 					    nullptr,	// Data to start into FTP message payload
 					    &reply);	// Payload inside FTP message response
+
 		if (!success) {
 			return false;
 		}
-		
+
 		ut_compare("Didn't get Ack back", reply->opcode, MavlinkFTP::kRspAck);
 		ut_compare("Offset incorrect", reply->offset, 0);
 
@@ -419,29 +436,32 @@ bool MavlinkFtpTest::_read_test(void)
 		uint32_t expected_bytes = test->singlePacketRead ? (uint32_t)st.st_size : full_packet_bytes;
 		ut_compare("Payload size incorrect", reply->size, expected_bytes);
 		ut_compare("File contents differ", memcmp(reply->data, bytes, expected_bytes), 0);
-		
+
 		payload.offset += expected_bytes;
-		
+
 		if (test->singlePacketRead) {
 			// Try going past EOF
 			success = _send_receive_msg(&payload,	// FTP payload header
 						    0,		// size in bytes of data
 						    nullptr,	// Data to start into FTP message payload
 						    &reply);	// Payload inside FTP message response
+
 			if (!success) {
 				return false;
 			}
-			
+
 			ut_compare("Didn't get Nak back", reply->opcode, MavlinkFTP::kRspNak);
+
 		} else {
 			success = _send_receive_msg(&payload,	// FTP payload header
 						    0,		// size in bytes of data
 						    nullptr,	// Data to start into FTP message payload
 						    &reply);	// Payload inside FTP message response
+
 			if (!success) {
 				return false;
 			}
-			
+
 			ut_compare("Didn't get Ack back", reply->opcode, MavlinkFTP::kRspAck);
 			ut_compare("Offset incorrect", reply->offset, payload.offset);
 
@@ -449,23 +469,24 @@ bool MavlinkFtpTest::_read_test(void)
 			ut_compare("Payload size incorrect", reply->size, expected_bytes);
 			ut_compare("File contents differ", memcmp(reply->data, &bytes[payload.offset], expected_bytes), 0);
 		}
-		
+
 		payload.opcode = MavlinkFTP::kCmdTerminateSession;
 		payload.session = reply->session;
 		payload.size = 0;
-		
+
 		success = _send_receive_msg(&payload,	// FTP payload header
 					    0,		// size in bytes of data
 					    nullptr,	// Data to start into FTP message payload
 					    &reply);	// Payload inside FTP message response
+
 		if (!success) {
 			return false;
 		}
-		
+
 		ut_compare("Didn't get Ack back", reply->opcode, MavlinkFTP::kRspAck);
 		ut_compare("Incorrect payload size", reply->size, 0);
 	}
-	
+
 	return true;
 }
 
@@ -477,11 +498,11 @@ bool MavlinkFtpTest::_burst_test(void)
 	BurstInfo				burst_info;
 
 
-	
-	for (size_t i=0; i<sizeof(_rgDownloadTestCases)/sizeof(_rgDownloadTestCases[0]); i++) {
+
+	for (size_t i = 0; i < sizeof(_rgDownloadTestCases) / sizeof(_rgDownloadTestCases[0]); i++) {
 		struct stat st;
 		const DownloadTestCase *test = &_rgDownloadTestCases[i];
-		
+
 		// Read in the file so we can compare it to what we get back
 		ut_compare("stat failed", stat(test->file, &st), 0);
 		uint8_t *bytes = new uint8_t[st.st_size];
@@ -491,23 +512,24 @@ bool MavlinkFtpTest::_burst_test(void)
 		int bytes_read = ::read(fd, bytes, st.st_size);
 		ut_compare("read failed", bytes_read, st.st_size);
 		::close(fd);
-		
+
 		// Test case data files are created for specific boundary conditions
 		ut_compare("Test case data files are out of date", test->length, st.st_size);
-		
+
 		payload.opcode = MavlinkFTP::kCmdOpenFileRO;
 		payload.offset = 0;
-		
+
 		bool success = _send_receive_msg(&payload,		// FTP payload header
-						 strlen(test->file)+1,	// size in bytes of data
-						 (uint8_t*)test->file,	// Data to start into FTP message payload
+						 strlen(test->file) + 1,	// size in bytes of data
+						 (uint8_t *)test->file,	// Data to start into FTP message payload
 						 &reply);		// Payload inside FTP message response
+
 		if (!success) {
 			return false;
 		}
 
 		ut_compare("Didn't get Ack back", reply->opcode, MavlinkFTP::kRspAck);
-		
+
 		// Setup for burst response handler
 		burst_info.burst_state = burst_state_first_ack;
 		burst_info.single_packet_file = test->singlePacketRead;
@@ -520,7 +542,7 @@ bool MavlinkFtpTest::_burst_test(void)
 		payload.opcode = MavlinkFTP::kCmdBurstReadFile;
 		payload.session = reply->session;
 		payload.offset = 0;
-		
+
 		mavlink_message_t msg;
 		_setup_ftp_msg(&payload, 0, nullptr, &msg);
 		_ftp_server->handle_message(&msg);
@@ -528,29 +550,30 @@ bool MavlinkFtpTest::_burst_test(void)
 		// First packet is sent using stream mechanism, so we need to force it out ourselves
 		hrt_abstime t = 0;
 		_ftp_server->send(t);
-		
+
 		ut_compare("Incorrect sequence of messages", burst_info.burst_state, burst_state_complete);
-		
+
 		// Put back generic message handler
 		_ftp_server->set_unittest_worker(MavlinkFtpTest::receive_message_handler_generic, this);
-		
+
 		// Terminate session
 		payload.opcode = MavlinkFTP::kCmdTerminateSession;
 		payload.session = reply->session;
 		payload.size = 0;
-		
+
 		success = _send_receive_msg(&payload,	// FTP payload header
 					    0,		// size in bytes of data
 					    nullptr,	// Data to start into FTP message payload
 					    &reply);	// Payload inside FTP message response
+
 		if (!success) {
 			return false;
 		}
-		
+
 		ut_compare("Didn't get Ack back", reply->opcode, MavlinkFTP::kRspAck);
 		ut_compare("Incorrect payload size", reply->size, 0);
 	}
-	
+
 	return true;
 }
 
@@ -560,36 +583,38 @@ bool MavlinkFtpTest::_read_badsession_test(void)
 	MavlinkFTP::PayloadHeader		payload;
 	const MavlinkFTP::PayloadHeader		*reply;
 	const char				*file = _rgDownloadTestCases[0].file;
-	
+
 	payload.opcode = MavlinkFTP::kCmdOpenFileRO;
 	payload.offset = 0;
-	
+
 	bool success = _send_receive_msg(&payload,		// FTP payload header
-					 strlen(file)+1,	// size in bytes of data
-					 (uint8_t*)file,	// Data to start into FTP message payload
+					 strlen(file) + 1,	// size in bytes of data
+					 (uint8_t *)file,	// Data to start into FTP message payload
 					 &reply);		// Payload inside FTP message response
+
 	if (!success) {
 		return false;
 	}
-	
+
 	ut_compare("Didn't get Ack back", reply->opcode, MavlinkFTP::kRspAck);
-	
+
 	payload.opcode = MavlinkFTP::kCmdReadFile;
 	payload.session = reply->session + 1;	// Invalid session
 	payload.offset = 0;
-	
+
 	success = _send_receive_msg(&payload,	// FTP payload header
 				    0,		// size in bytes of data
 				    nullptr,	// Data to start into FTP message payload
 				    &reply);	// Payload inside FTP message response
+
 	if (!success) {
 		return false;
 	}
-	
+
 	ut_compare("Didn't get Nak back", reply->opcode, MavlinkFTP::kRspNak);
 	ut_compare("Incorrect payload size", reply->size, 1);
 	ut_compare("Incorrect error code", reply->data[0], MavlinkFTP::kErrInvalidSession);
-	
+
 	return true;
 }
 
@@ -598,7 +623,7 @@ bool MavlinkFtpTest::_removedirectory_test(void)
 	MavlinkFTP::PayloadHeader		payload;
 	const MavlinkFTP::PayloadHeader		*reply;
 	int					fd;
-	
+
 	struct _testCase {
 		const char	*dir;
 		bool		success;
@@ -611,39 +636,41 @@ bool MavlinkFtpTest::_removedirectory_test(void)
 		{ _unittest_microsd_file,				false,	false },
 		{ _unittest_microsd_dir,				true,	true },
 	};
-	
+
 	ut_compare("mkdir failed", ::mkdir(_unittest_microsd_dir, S_IRWXU | S_IRWXG | S_IRWXO), 0);
 	ut_assert("open failed", (fd = ::open(_unittest_microsd_file, O_CREAT | O_EXCL)) != -1);
 	::close(fd);
-	
-	for (size_t i=0; i<sizeof(rgTestCases)/sizeof(rgTestCases[0]); i++) {
+
+	for (size_t i = 0; i < sizeof(rgTestCases) / sizeof(rgTestCases[0]); i++) {
 		const struct _testCase *test = &rgTestCases[i];
-		
+
 		if (test->deleteFile) {
 			ut_compare("unlink failed", ::unlink(_unittest_microsd_file), 0);
 		}
-		
+
 		payload.opcode = MavlinkFTP::kCmdRemoveDirectory;
 		payload.offset = 0;
-		
+
 		bool success = _send_receive_msg(&payload,		// FTP payload header
-						 strlen(test->dir)+1,	// size in bytes of data
-						 (uint8_t*)test->dir,	// Data to start into FTP message payload
+						 strlen(test->dir) + 1,	// size in bytes of data
+						 (uint8_t *)test->dir,	// Data to start into FTP message payload
 						 &reply);		// Payload inside FTP message response
+
 		if (!success) {
 			return false;
 		}
-		
+
 		if (test->success) {
 			ut_compare("Didn't get Ack back", reply->opcode, MavlinkFTP::kRspAck);
 			ut_compare("Incorrect payload size", reply->size, 0);
+
 		} else {
 			ut_compare("Didn't get Nak back", reply->opcode, MavlinkFTP::kRspNak);
 			ut_compare("Incorrect payload size", reply->size, 2);
 			ut_compare("Incorrect error code", reply->data[0], MavlinkFTP::kErrFailErrno);
 		}
 	}
-	
+
 	return true;
 }
 
@@ -651,7 +678,7 @@ bool MavlinkFtpTest::_createdirectory_test(void)
 {
 	MavlinkFTP::PayloadHeader		payload;
 	const MavlinkFTP::PayloadHeader		*reply;
-	
+
 	struct _testCase {
 		const char	*dir;
 		bool		success;
@@ -662,31 +689,33 @@ bool MavlinkFtpTest::_createdirectory_test(void)
 		{ _unittest_microsd_dir,	false },
 		{ "/fs/microsd/bogus/bogus",	false },
 	};
-	
-	for (size_t i=0; i<sizeof(rgTestCases)/sizeof(rgTestCases[0]); i++) {
+
+	for (size_t i = 0; i < sizeof(rgTestCases) / sizeof(rgTestCases[0]); i++) {
 		const struct _testCase *test = &rgTestCases[i];
-		
+
 		payload.opcode = MavlinkFTP::kCmdCreateDirectory;
 		payload.offset = 0;
-		
+
 		bool success = _send_receive_msg(&payload,		// FTP payload header
-						 strlen(test->dir)+1,	// size in bytes of data
-						 (uint8_t*)test->dir,	// Data to start into FTP message payload
+						 strlen(test->dir) + 1,	// size in bytes of data
+						 (uint8_t *)test->dir,	// Data to start into FTP message payload
 						 &reply);		// Payload inside FTP message response
+
 		if (!success) {
 			return false;
 		}
-		
+
 		if (test->success) {
 			ut_compare("Didn't get Ack back", reply->opcode, MavlinkFTP::kRspAck);
 			ut_compare("Incorrect payload size", reply->size, 0);
+
 		} else {
 			ut_compare("Didn't get Nak back", reply->opcode, MavlinkFTP::kRspNak);
 			ut_compare("Incorrect payload size", reply->size, 2);
 			ut_compare("Incorrect error code", reply->data[0], MavlinkFTP::kErrFailErrno);
 		}
 	}
-	
+
 	return true;
 }
 
@@ -695,7 +724,7 @@ bool MavlinkFtpTest::_removefile_test(void)
 	MavlinkFTP::PayloadHeader		payload;
 	const MavlinkFTP::PayloadHeader		*reply;
 	int					fd;
-	
+
 	struct _testCase {
 		const char	*file;
 		bool		success;
@@ -707,46 +736,48 @@ bool MavlinkFtpTest::_removefile_test(void)
 		{ _unittest_microsd_file,	true },
 		{ _unittest_microsd_file,	false },
 	};
-	
+
 	ut_compare("mkdir failed", ::mkdir(_unittest_microsd_dir, S_IRWXU | S_IRWXG | S_IRWXO), 0);
 	ut_assert("open failed", (fd = ::open(_unittest_microsd_file, O_CREAT | O_EXCL)) != -1);
 	::close(fd);
-	
-	for (size_t i=0; i<sizeof(rgTestCases)/sizeof(rgTestCases[0]); i++) {
+
+	for (size_t i = 0; i < sizeof(rgTestCases) / sizeof(rgTestCases[0]); i++) {
 		const struct _testCase *test = &rgTestCases[i];
-		
+
 		payload.opcode = MavlinkFTP::kCmdRemoveFile;
 		payload.offset = 0;
-		
+
 		bool success = _send_receive_msg(&payload,		// FTP payload header
-						 strlen(test->file)+1,	// size in bytes of data
-						 (uint8_t*)test->file,	// Data to start into FTP message payload
+						 strlen(test->file) + 1,	// size in bytes of data
+						 (uint8_t *)test->file,	// Data to start into FTP message payload
 						 &reply);		// Payload inside FTP message response
+
 		if (!success) {
 			return false;
 		}
-		
+
 		if (test->success) {
 			ut_compare("Didn't get Ack back", reply->opcode, MavlinkFTP::kRspAck);
 			ut_compare("Incorrect payload size", reply->size, 0);
+
 		} else {
 			ut_compare("Didn't get Nak back", reply->opcode, MavlinkFTP::kRspNak);
 			ut_compare("Incorrect payload size", reply->size, 2);
 			ut_compare("Incorrect error code", reply->data[0], MavlinkFTP::kErrFailErrno);
 		}
 	}
-	
+
 	return true;
 }
 
 /// Static method used as callback from MavlinkFTP for generic use. This method will be called by MavlinkFTP when
 /// it needs to send a message out on Mavlink.
-void MavlinkFtpTest::receive_message_handler_generic(const mavlink_file_transfer_protocol_t* ftp_req, void *worker_data)
+void MavlinkFtpTest::receive_message_handler_generic(const mavlink_file_transfer_protocol_t *ftp_req, void *worker_data)
 {
-	((MavlinkFtpTest*)worker_data)->_receive_message_handler_generic(ftp_req);
+	((MavlinkFtpTest *)worker_data)->_receive_message_handler_generic(ftp_req);
 }
 
-void MavlinkFtpTest::_receive_message_handler_generic(const mavlink_file_transfer_protocol_t* ftp_req)
+void MavlinkFtpTest::_receive_message_handler_generic(const mavlink_file_transfer_protocol_t *ftp_req)
 {
 	// Move the message into our own member variable
 	memcpy(&_reply_msg, ftp_req, sizeof(mavlink_file_transfer_protocol_t));
@@ -754,62 +785,63 @@ void MavlinkFtpTest::_receive_message_handler_generic(const mavlink_file_transfe
 
 /// Static method used as callback from MavlinkFTP for stream download testing. This method will be called by MavlinkFTP when
 /// it needs to send a message out on Mavlink.
-void MavlinkFtpTest::receive_message_handler_burst(const mavlink_file_transfer_protocol_t* ftp_req, void *worker_data)
+void MavlinkFtpTest::receive_message_handler_burst(const mavlink_file_transfer_protocol_t *ftp_req, void *worker_data)
 {
-	BurstInfo* burst_info = (BurstInfo*)worker_data;
+	BurstInfo *burst_info = (BurstInfo *)worker_data;
 	burst_info->ftp_test_class->_receive_message_handler_burst(ftp_req, burst_info);
 }
 
-bool MavlinkFtpTest::_receive_message_handler_burst(const mavlink_file_transfer_protocol_t* ftp_msg, BurstInfo* burst_info)
+bool MavlinkFtpTest::_receive_message_handler_burst(const mavlink_file_transfer_protocol_t *ftp_msg,
+		BurstInfo *burst_info)
 {
 	hrt_abstime t = 0;
-	const MavlinkFTP::PayloadHeader* reply;
+	const MavlinkFTP::PayloadHeader *reply;
 	uint32_t full_packet_bytes = MAVLINK_MSG_FILE_TRANSFER_PROTOCOL_FIELD_PAYLOAD_LEN - sizeof(MavlinkFTP::PayloadHeader);
 	uint32_t expected_bytes;
-	
+
 	_decode_message(ftp_msg, &reply);
 
 	switch (burst_info->burst_state) {
-		case burst_state_first_ack:
-			ut_compare("Didn't get Ack back", reply->opcode, MavlinkFTP::kRspAck);
-			ut_compare("Offset incorrect", reply->offset, 0);
-			
-			expected_bytes = burst_info->single_packet_file ? burst_info->file_size : full_packet_bytes;
-			ut_compare("Payload size incorrect", reply->size, expected_bytes);
-			ut_compare("burst_complete incorrect", reply->burst_complete, 0);
-			ut_compare("File contents differ", memcmp(reply->data, burst_info->file_bytes, expected_bytes), 0);
-			
-			// Setup for next expected message
-			burst_info->burst_state = burst_info->single_packet_file ? burst_state_nak_eof : burst_state_last_ack;
-			
-			ut_assert("Remaining stream packets missing", _ftp_server->get_size());
-			_ftp_server->send(t);
-			break;
-			
-		case burst_state_last_ack:
-			ut_compare("Didn't get Ack back", reply->opcode, MavlinkFTP::kRspAck);
-			ut_compare("Offset incorrect", reply->offset, full_packet_bytes);
-			
-			expected_bytes = burst_info->file_size - full_packet_bytes;
-			ut_compare("Payload size incorrect", reply->size, expected_bytes);
-			ut_compare("burst_complete incorrect", reply->burst_complete, 0);
-			ut_compare("File contents differ", memcmp(reply->data, &burst_info->file_bytes[full_packet_bytes], expected_bytes), 0);
-			
-			// Setup for next expected message
-			burst_info->burst_state = burst_state_nak_eof;
-			
-			ut_assert("Remaining stream packets missing", _ftp_server->get_size());
-			_ftp_server->send(t);
-			break;
-			
-		case burst_state_nak_eof:
-			// Signal complete
-			burst_info->burst_state = burst_state_complete;
-			ut_compare("All packets should have been seent", _ftp_server->get_size(), 0);
-			break;
-		
+	case burst_state_first_ack:
+		ut_compare("Didn't get Ack back", reply->opcode, MavlinkFTP::kRspAck);
+		ut_compare("Offset incorrect", reply->offset, 0);
+
+		expected_bytes = burst_info->single_packet_file ? burst_info->file_size : full_packet_bytes;
+		ut_compare("Payload size incorrect", reply->size, expected_bytes);
+		ut_compare("burst_complete incorrect", reply->burst_complete, 0);
+		ut_compare("File contents differ", memcmp(reply->data, burst_info->file_bytes, expected_bytes), 0);
+
+		// Setup for next expected message
+		burst_info->burst_state = burst_info->single_packet_file ? burst_state_nak_eof : burst_state_last_ack;
+
+		ut_assert("Remaining stream packets missing", _ftp_server->get_size());
+		_ftp_server->send(t);
+		break;
+
+	case burst_state_last_ack:
+		ut_compare("Didn't get Ack back", reply->opcode, MavlinkFTP::kRspAck);
+		ut_compare("Offset incorrect", reply->offset, full_packet_bytes);
+
+		expected_bytes = burst_info->file_size - full_packet_bytes;
+		ut_compare("Payload size incorrect", reply->size, expected_bytes);
+		ut_compare("burst_complete incorrect", reply->burst_complete, 0);
+		ut_compare("File contents differ", memcmp(reply->data, &burst_info->file_bytes[full_packet_bytes], expected_bytes), 0);
+
+		// Setup for next expected message
+		burst_info->burst_state = burst_state_nak_eof;
+
+		ut_assert("Remaining stream packets missing", _ftp_server->get_size());
+		_ftp_server->send(t);
+		break;
+
+	case burst_state_nak_eof:
+		// Signal complete
+		burst_info->burst_state = burst_state_complete;
+		ut_compare("All packets should have been seent", _ftp_server->get_size(), 0);
+		break;
+
 	}
-	
+
 	return true;
 }
 
@@ -818,18 +850,18 @@ bool MavlinkFtpTest::_decode_message(const mavlink_file_transfer_protocol_t	*ftp
 				     const MavlinkFTP::PayloadHeader		**payload)	///< Payload inside FTP message response
 {
 	//warnx("_decode_message");
-	
+
 	// Make sure the targets are correct
 	ut_compare("Target network non-zero", ftp_msg->target_network, 0);
 	ut_compare("Target system id mismatch", ftp_msg->target_system, clientSystemId);
 	ut_compare("Target component id mismatch", ftp_msg->target_component, clientComponentId);
-	
+
 	*payload = reinterpret_cast<const MavlinkFTP::PayloadHeader *>(ftp_msg->payload);
-	
+
 	// Make sure we have a good sequence number
 	ut_compare("Sequence number mismatch", (*payload)->seq_number, _expected_seq_number);
 	_expected_seq_number++;
-	
+
 	return true;
 }
 
@@ -841,18 +873,19 @@ void MavlinkFtpTest::_setup_ftp_msg(const MavlinkFTP::PayloadHeader	*payload_hea
 {
 	uint8_t payload_bytes[MAVLINK_MSG_FILE_TRANSFER_PROTOCOL_FIELD_PAYLOAD_LEN];
 	MavlinkFTP::PayloadHeader *payload = reinterpret_cast<MavlinkFTP::PayloadHeader *>(payload_bytes);
-			     
+
 	memcpy(payload, payload_header, sizeof(MavlinkFTP::PayloadHeader));
-	
+
 	payload->seq_number = _expected_seq_number++;
 	payload->size = size;
+
 	if (size != 0) {
 		memcpy(payload->data, data, size);
 	}
-    
+
 	payload->burst_complete = 0;
 	payload->padding = 0;
-	
+
 	msg->checksum = 0;
 	mavlink_msg_file_transfer_protocol_pack(clientSystemId,		// Sender system id
 						clientComponentId,	// Sender component id
@@ -865,9 +898,9 @@ void MavlinkFtpTest::_setup_ftp_msg(const MavlinkFTP::PayloadHeader	*payload_hea
 
 /// @brief Sends the specified FTP message to the server and returns response
 bool MavlinkFtpTest::_send_receive_msg(MavlinkFTP::PayloadHeader	*payload_header,	///< FTP payload header
-				      uint8_t				size,			///< size in bytes of data
-				      const uint8_t			*data,			///< Data to start into FTP message payload
-				      const MavlinkFTP::PayloadHeader	**payload_reply)	///< Payload inside FTP message response
+				       uint8_t				size,			///< size in bytes of data
+				       const uint8_t			*data,			///< Data to start into FTP message payload
+				       const MavlinkFTP::PayloadHeader	**payload_reply)	///< Payload inside FTP message response
 {
 	mavlink_message_t msg;
 
@@ -875,7 +908,7 @@ bool MavlinkFtpTest::_send_receive_msg(MavlinkFTP::PayloadHeader	*payload_header
 	_ftp_server->handle_message(&msg);
 	return _decode_message(&_reply_msg, payload_reply);
 }
-	
+
 /// @brief Cleans up an files created on microsd during testing
 void MavlinkFtpTest::_cleanup_microsd(void)
 {
@@ -889,7 +922,7 @@ bool MavlinkFtpTest::run_tests(void)
 	ut_run_test(_ack_test);
 	ut_run_test(_bad_opcode_test);
 	ut_run_test(_bad_datasize_test);
-	ut_run_test(_list_test);
+	//ut_run_test(_list_test); // TODO: cmake build system needs to run mavlink_ftp_test_data.py
 	ut_run_test(_list_eof_test);
 	ut_run_test(_open_badfile_test);
 	ut_run_test(_open_terminate_test);
@@ -900,7 +933,7 @@ bool MavlinkFtpTest::run_tests(void)
 	ut_run_test(_removedirectory_test);
 	ut_run_test(_createdirectory_test);
 	ut_run_test(_removefile_test);
-	
+
 	return (_tests_failed == 0);
 
 }
diff --git a/src/modules/mavlink/mavlink_tests/mavlink_ftp_test.h b/src/modules/mavlink/mavlink_tests/mavlink_ftp_test.h
index 14c9369b05..7708d2a630 100644
--- a/src/modules/mavlink/mavlink_tests/mavlink_ftp_test.h
+++ b/src/modules/mavlink/mavlink_tests/mavlink_ftp_test.h
@@ -45,37 +45,37 @@ class MavlinkFtpTest : public UnitTest
 public:
 	MavlinkFtpTest();
 	virtual ~MavlinkFtpTest();
-	
+
 	virtual bool run_tests(void);
-	
-	static void receive_message_handler_generic(const mavlink_file_transfer_protocol_t* ftp_req, void *worker_data);
-	
+
+	static void receive_message_handler_generic(const mavlink_file_transfer_protocol_t *ftp_req, void *worker_data);
+
 	/// Worker data for stream handler
 	struct BurstInfo {
-		MavlinkFtpTest*		ftp_test_class;
+		MavlinkFtpTest		*ftp_test_class;
 		int			burst_state;
 		bool			single_packet_file;
 		uint32_t		file_size;
-		uint8_t*		file_bytes;
+		uint8_t		*file_bytes;
 	};
-	
-	static void receive_message_handler_burst(const mavlink_file_transfer_protocol_t* ftp_req, void *worker_data);
-	
+
+	static void receive_message_handler_burst(const mavlink_file_transfer_protocol_t *ftp_req, void *worker_data);
+
 	static const uint8_t serverSystemId = 50;	///< System ID for server
 	static const uint8_t serverComponentId = 1;	///< Component ID for server
 	static const uint8_t serverChannel = 0;		///< Channel to send to
-	
+
 	static const uint8_t clientSystemId = 1;	///< System ID for client
 	static const uint8_t clientComponentId = 0;	///< Component ID for client
-	
+
 	// We don't want any of these
-	MavlinkFtpTest(const MavlinkFtpTest&);
-	MavlinkFtpTest& operator=(const MavlinkFtpTest&);
-	
+	MavlinkFtpTest(const MavlinkFtpTest &);
+	MavlinkFtpTest &operator=(const MavlinkFtpTest &);
+
 private:
 	virtual void _init(void);
 	virtual void _cleanup(void);
-	
+
 	bool _ack_test(void);
 	bool _bad_opcode_test(void);
 	bool _bad_datasize_test(void);
@@ -90,16 +90,17 @@ private:
 	bool _removedirectory_test(void);
 	bool _createdirectory_test(void);
 	bool _removefile_test(void);
-	
-	void _receive_message_handler_generic(const mavlink_file_transfer_protocol_t* ftp_req);
-	void _setup_ftp_msg(const MavlinkFTP::PayloadHeader *payload_header, uint8_t size, const uint8_t *data, mavlink_message_t *msg);
+
+	void _receive_message_handler_generic(const mavlink_file_transfer_protocol_t *ftp_req);
+	void _setup_ftp_msg(const MavlinkFTP::PayloadHeader *payload_header, uint8_t size, const uint8_t *data,
+			    mavlink_message_t *msg);
 	bool _decode_message(const mavlink_file_transfer_protocol_t *ftp_msg, const MavlinkFTP::PayloadHeader **payload);
 	bool _send_receive_msg(MavlinkFTP::PayloadHeader	*payload_header,
-                           uint8_t				size,
-                           const uint8_t			*data,
-                           const MavlinkFTP::PayloadHeader	**payload_reply);
+			       uint8_t				size,
+			       const uint8_t			*data,
+			       const MavlinkFTP::PayloadHeader	**payload_reply);
 	void _cleanup_microsd(void);
-	
+
 	/// A single download test case
 	struct DownloadTestCase {
 		const char	*file;
@@ -107,10 +108,10 @@ private:
 		bool		singlePacketRead;
 		bool		exactlyFillPacket;
 	};
-	
+
 	/// The set of test cases for download testing
 	static const DownloadTestCase _rgDownloadTestCases[];
-	
+
 	/// States for stream download handler
 	enum {
 		burst_state_first_ack,
@@ -118,14 +119,14 @@ private:
 		burst_state_nak_eof,
 		burst_state_complete
 	};
-	
-	bool _receive_message_handler_burst(const mavlink_file_transfer_protocol_t* ftp_req, BurstInfo* burst_info);
-	
-	MavlinkFTP*	_ftp_server;
+
+	bool _receive_message_handler_burst(const mavlink_file_transfer_protocol_t *ftp_req, BurstInfo *burst_info);
+
+	MavlinkFTP	*_ftp_server;
 	uint16_t	_expected_seq_number;
-	
+
 	mavlink_file_transfer_protocol_t _reply_msg;
-	
+
 	static const char _unittest_microsd_dir[];
 	static const char _unittest_microsd_file[];
 };
diff --git a/src/systemcmds/tests/test_params.c b/src/systemcmds/tests/test_params.c
index 09845ee25c..228d88fb9e 100644
--- a/src/systemcmds/tests/test_params.c
+++ b/src/systemcmds/tests/test_params.c
@@ -43,16 +43,20 @@
 #include "systemlib/param/param.h"
 #include "tests.h"
 
-#define PARAM_MAGIC1 0x12345678
+#define PARAM_MAGIC1 12345678
 #define PARAM_MAGIC2 0xa5a5a5a5
-PARAM_DEFINE_INT32(test, PARAM_MAGIC1);
+
+/**
+ * @group Testing
+ */
+PARAM_DEFINE_INT32(TEST_PARAMS, 12345678);
 
 int
 test_param(int argc, char *argv[])
 {
 	param_t		p;
 
-	p = param_find("test");
+	p = param_find("TEST_PARAMS");
 
 	if (p == PARAM_INVALID) {
 		warnx("test parameter not found");
@@ -71,7 +75,7 @@ test_param(int argc, char *argv[])
 		return 1;
 	}
 
-	int32_t	val;
+	int32_t	val = -1;
 
 	if (param_get(p, &val) != OK) {
 		warnx("failed to read test parameter");
diff --git a/src/systemcmds/tests/tests_main.c b/src/systemcmds/tests/tests_main.c
index 7c7cbeb28f..453705eaf4 100644
--- a/src/systemcmds/tests/tests_main.c
+++ b/src/systemcmds/tests/tests_main.c
@@ -166,15 +166,20 @@ test_all(int argc, char *argv[])
 				passed[i] = true;
 			}
 
+			for (int j = 0; j < 40; j++) {
+				printf("-");
+			}
+
+			printf("\n\n");
+
 			testcount++;
 		}
 	}
 
 	/* Print summary */
 	printf("\n");
-	int j;
 
-	for (j = 0; j < 40; j++) {
+	for (int j = 0; j < 40; j++) {
 		printf("-");
 	}
 

From 39d388051a4aeef9209bfde9983768f95535a855 Mon Sep 17 00:00:00 2001
From: Daniel Agar <daniel@agar.ca>
Date: Sat, 23 Apr 2016 02:20:11 -0400
Subject: [PATCH 0072/1230] WIP posix_sitl_test

---
 Makefile                                      |   5 +-
 cmake/configs/posix_sitl_default.cmake        |  76 +++++++------
 cmake/configs/posix_sitl_test.cmake           | 105 ++++++++++++++++++
 posix-configs/SITL/init/rcS_test_jmavsim_iris |  61 ++++++++++
 .../mavlink_tests/mavlink_ftp_test.cpp        |   4 +-
 src/modules/unit_test/unit_test.cpp           |   8 +-
 src/systemcmds/tests/test_uart_console.c      |   1 +
 7 files changed, 221 insertions(+), 39 deletions(-)
 create mode 100644 cmake/configs/posix_sitl_test.cmake
 create mode 100644 posix-configs/SITL/init/rcS_test_jmavsim_iris

diff --git a/Makefile b/Makefile
index 61b9a05337..c66c326729 100644
--- a/Makefile
+++ b/Makefile
@@ -155,6 +155,9 @@ mindpx-v2_default:
 posix_sitl_default:
 	$(call cmake-build,$@)
 
+posix_sitl_test:
+	$(call cmake-build,$@)
+
 posix_sitl_lpe:
 	$(call cmake-build,$@)
 
@@ -225,7 +228,7 @@ ifeq ($(VECTORCONTROL),1)
 	@(rm -rf vectorcontrol && git clone --quiet --depth 1 https://github.com/thiemar/vectorcontrol.git && cd vectorcontrol && BOARD=s2740vc_1_0 make --silent --no-print-directory && BOARD=px4esc_1_6 make --silent --no-print-directory && ../Tools/uavcan_copy.sh)
 endif
 
-check: check_px4fmu-v1_default check_px4fmu-v2_default check_px4fmu-v2_test check_px4fmu-v2_ekf2 check_px4fmu-v2_lpe check_px4fmu-v4_default_and_uavcan check_mindpx-v2_default check_px4-stm32f4discovery_default check_posix_sitl_default check_unittest check_format
+check: check_px4fmu-v1_default check_px4fmu-v2_default check_px4fmu-v2_test check_px4fmu-v2_ekf2 check_px4fmu-v2_lpe check_px4fmu-v4_default_and_uavcan check_mindpx-v2_default check_px4-stm32f4discovery_default check_posix_sitl_default check_posix_sitl_test check_unittest check_format
 
 check_format:
 	$(call colorecho,"Checking formatting with astyle")
diff --git a/cmake/configs/posix_sitl_default.cmake b/cmake/configs/posix_sitl_default.cmake
index a42385cd9d..69e76dfa81 100644
--- a/cmake/configs/posix_sitl_default.cmake
+++ b/cmake/configs/posix_sitl_default.cmake
@@ -3,68 +3,74 @@ include(posix/px4_impl_posix)
 set(CMAKE_TOOLCHAIN_FILE ${CMAKE_SOURCE_DIR}/cmake/toolchains/Toolchain-native.cmake)
 
 set(config_module_list
-	drivers/device
 	drivers/boards/sitl
-	drivers/pwm_out_sim
-	platforms/common
-	platforms/posix/px4_layer
-	platforms/posix/work_queue
-	platforms/posix/drivers/adcsim
-	platforms/posix/drivers/gpssim
+	drivers/device
 	drivers/gps
-	platforms/posix/drivers/tonealrmsim
+	drivers/pwm_out_sim
+
+	platforms/common
 	platforms/posix/drivers/accelsim
+	platforms/posix/drivers/adcsim
 	platforms/posix/drivers/airspeedsim
 	platforms/posix/drivers/barosim
+	platforms/posix/drivers/gpssim
 	platforms/posix/drivers/gyrosim
-	platforms/posix/drivers/rgbledsim
 	platforms/posix/drivers/ledsim
-	systemcmds/param
-	systemcmds/mixer
-	systemcmds/ver
+	platforms/posix/drivers/rgbledsim
+	platforms/posix/drivers/tonealrmsim
+	platforms/posix/px4_layer
+	platforms/posix/work_queue
+
 	systemcmds/esc_calib
+	systemcmds/mixer
+	systemcmds/param
+	systemcmds/perf
 	systemcmds/reboot
 	systemcmds/sd_bench
 	systemcmds/topic_listener
-	systemcmds/perf
-	modules/uORB
-	modules/param
-	modules/systemlib
-	modules/systemlib/mixer
-	modules/sensors
-	modules/simulator
-	modules/mavlink
+	systemcmds/ver
+
 	modules/attitude_estimator_ekf
 	modules/attitude_estimator_q
+	modules/commander
+	modules/dataman
 	modules/ekf2
 	modules/ekf_att_pos_estimator
-	modules/position_estimator_inav
-	modules/navigator
-	modules/vtol_att_control
-	modules/mc_pos_control
-	modules/mc_att_control
-	modules/mc_pos_control_multiplatform
-	modules/mc_att_control_multiplatform
-	modules/land_detector
 	modules/fw_att_control
 	modules/fw_pos_control_l1
-	modules/dataman
-	modules/sdlog2
-	modules/commander
+	modules/land_detector
 	modules/load_mon
+	modules/mavlink
+	modules/mc_att_control
+	modules/mc_att_control_multiplatform
+	modules/mc_pos_control
+
+	modules/mc_pos_control_multiplatform
+	modules/navigator
+	modules/param
+	modules/position_estimator_inav
+	modules/sdlog2
+	modules/sensors
+	modules/simulator
+	modules/systemlib
+	modules/systemlib/mixer
+	modules/uORB
+	modules/vtol_att_control
+
 	lib/controllib
-	lib/mathlib
-	lib/mathlib/math/filter
 	lib/conversion
+	lib/DriverFramework/framework
 	lib/ecl
 	lib/external_lgpl
 	lib/geo
 	lib/geo_lookup
 	lib/launchdetection
-	lib/terrain_estimation
+	lib/mathlib
+	lib/mathlib/math/filter
 	lib/runway_takeoff
 	lib/tailsitter_recovery
-	lib/DriverFramework/framework
+	lib/terrain_estimation
+
 	examples/px4_simple_app
 	)
 
diff --git a/cmake/configs/posix_sitl_test.cmake b/cmake/configs/posix_sitl_test.cmake
new file mode 100644
index 0000000000..29e8ee7c8b
--- /dev/null
+++ b/cmake/configs/posix_sitl_test.cmake
@@ -0,0 +1,105 @@
+include(posix/px4_impl_posix)
+
+set(CMAKE_TOOLCHAIN_FILE ${CMAKE_SOURCE_DIR}/cmake/toolchains/Toolchain-native.cmake)
+
+set(config_module_list
+	drivers/boards/sitl
+	drivers/device
+	drivers/gps
+	drivers/pwm_out_sim
+
+	platforms/common
+	platforms/posix/drivers/accelsim
+	platforms/posix/drivers/adcsim
+	platforms/posix/drivers/airspeedsim
+	platforms/posix/drivers/barosim
+	platforms/posix/drivers/gpssim
+	platforms/posix/drivers/gyrosim
+	platforms/posix/drivers/ledsim
+	platforms/posix/drivers/rgbledsim
+	platforms/posix/drivers/tonealrmsim
+	platforms/posix/px4_layer
+	platforms/posix/work_queue
+
+	systemcmds/esc_calib
+	systemcmds/mixer
+	systemcmds/param
+	systemcmds/perf
+	systemcmds/reboot
+	systemcmds/topic_listener
+	systemcmds/ver
+
+	modules/attitude_estimator_ekf
+	modules/attitude_estimator_q
+	modules/commander
+	modules/dataman
+	modules/ekf2
+	modules/ekf_att_pos_estimator
+	modules/fw_att_control
+	modules/fw_pos_control_l1
+	modules/land_detector
+	modules/mavlink
+	modules/mc_att_control
+	modules/mc_att_control_multiplatform
+	modules/mc_pos_control
+	modules/mc_pos_control_multiplatform
+	modules/navigator
+	modules/param
+	modules/position_estimator_inav
+	modules/sdlog2
+	modules/sensors
+	modules/simulator
+	modules/systemlib
+	modules/systemlib/mixer
+	modules/uORB
+	modules/vtol_att_control
+
+	lib/controllib
+	lib/conversion
+	lib/ecl
+	lib/external_lgpl
+	lib/geo
+	lib/geo_lookup
+	lib/launchdetection
+	lib/mathlib
+	lib/mathlib/math/filter
+	lib/runway_takeoff
+	lib/tailsitter_recovery
+	lib/terrain_estimation
+
+	examples/px4_simple_app
+	
+	#
+	# Testing
+	#
+	modules/commander/commander_tests
+	modules/controllib_test
+	#modules/mavlink/mavlink_tests
+	modules/unit_test
+	systemcmds/tests
+
+	)
+
+set(config_extra_builtin_cmds
+	serdis
+	sercon
+	)
+
+set(config_sitl_rcS
+	posix-configs/SITL/init/rcS
+	CACHE FILEPATH "init script for sitl"
+	)
+
+set(config_sitl_viewer
+	jmavsim
+	CACHE STRING "viewer for sitl"
+	)
+set_property(CACHE config_sitl_viewer
+	PROPERTY STRINGS "jmavsim;none")
+
+set(config_sitl_debugger
+	disable
+	CACHE STRING "debugger for sitl"
+	)
+set_property(CACHE config_sitl_debugger
+	PROPERTY STRINGS "disable;gdb;lldb")
diff --git a/posix-configs/SITL/init/rcS_test_jmavsim_iris b/posix-configs/SITL/init/rcS_test_jmavsim_iris
new file mode 100644
index 0000000000..3843f2a9b7
--- /dev/null
+++ b/posix-configs/SITL/init/rcS_test_jmavsim_iris
@@ -0,0 +1,61 @@
+uorb start
+simulator start -s
+param load
+param set MAV_TYPE 2
+param set MC_PITCHRATE_P 0.15
+param set MC_PITCH_P 7
+param set MC_ROLL_P 7
+param set MC_ROLLRATE_P 0.15
+param set MC_YAW_P 2.8
+param set MC_YAWRATE_P 0.35
+param set SYS_AUTOSTART 4010
+param set SYS_RESTART_TYPE 2
+dataman start
+param set CAL_GYRO0_ID 2293768
+param set CAL_ACC0_ID 1376264
+param set CAL_ACC1_ID 1310728
+param set CAL_MAG0_ID 196616
+param set CAL_GYRO0_XOFF 0.01
+param set CAL_ACC0_XOFF 0.01
+param set CAL_ACC0_YOFF -0.01
+param set CAL_ACC0_ZOFF 0.01
+param set CAL_ACC0_XSCALE 1.01
+param set CAL_ACC0_YSCALE 1.01
+param set CAL_ACC0_ZSCALE 1.01
+param set CAL_ACC1_XOFF 0.01
+param set CAL_MAG0_XOFF 0.01
+param set MPC_XY_P 0.4
+param set MPC_XY_VEL_P 0.2
+param set MPC_XY_VEL_D 0.005
+param set SENS_BOARD_ROT 0
+param set SENS_BOARD_X_OFF 0.000001
+param set COM_RC_IN_MODE 1
+param set COM_DL_LOSS_EN 1
+param set COM_DISARM_LAND 3
+param set NAV_ACC_RAD 2.0
+param set RTL_RETURN_ALT 30.0
+param set RTL_DESCEND_ALT 10.0
+param set RTL_LAND_DELAY 0
+param set MIS_TAKEOFF_ALT 2.5
+param set MPC_HOLD_MAX_Z 2.0
+param set MPC_HOLD_XY_DZ 0.1
+param set MPC_HOLD_Z_DZ 0.1
+param set MPC_Z_VEL_MAX 2.0
+param set MPC_Z_VEL_P 0.4
+rgbledsim start
+tone_alarm start
+gyrosim start
+accelsim start
+barosim start
+adcsim start
+gpssim start
+pwm_out_sim mode_pwm
+sleep 1
+sensors start
+
+commander_tests
+controllib_test
+uorb test
+tests all
+
+ver all
diff --git a/src/modules/mavlink/mavlink_tests/mavlink_ftp_test.cpp b/src/modules/mavlink/mavlink_tests/mavlink_ftp_test.cpp
index a82c4d45a1..e45b91252e 100644
--- a/src/modules/mavlink/mavlink_tests/mavlink_ftp_test.cpp
+++ b/src/modules/mavlink/mavlink_tests/mavlink_ftp_test.cpp
@@ -638,7 +638,7 @@ bool MavlinkFtpTest::_removedirectory_test(void)
 	};
 
 	ut_compare("mkdir failed", ::mkdir(_unittest_microsd_dir, S_IRWXU | S_IRWXG | S_IRWXO), 0);
-	ut_assert("open failed", (fd = ::open(_unittest_microsd_file, O_CREAT | O_EXCL)) != -1);
+	ut_assert("open failed", (fd = ::open(_unittest_microsd_file, O_CREAT | O_EXCL, S_IRWXU | S_IRWXG | S_IRWXO)) != -1);
 	::close(fd);
 
 	for (size_t i = 0; i < sizeof(rgTestCases) / sizeof(rgTestCases[0]); i++) {
@@ -738,7 +738,7 @@ bool MavlinkFtpTest::_removefile_test(void)
 	};
 
 	ut_compare("mkdir failed", ::mkdir(_unittest_microsd_dir, S_IRWXU | S_IRWXG | S_IRWXO), 0);
-	ut_assert("open failed", (fd = ::open(_unittest_microsd_file, O_CREAT | O_EXCL)) != -1);
+	ut_assert("open failed", (fd = ::open(_unittest_microsd_file, O_CREAT | O_EXCL, S_IRWXU | S_IRWXG | S_IRWXO)) != -1);
 	::close(fd);
 
 	for (size_t i = 0; i < sizeof(rgTestCases) / sizeof(rgTestCases[0]); i++) {
diff --git a/src/modules/unit_test/unit_test.cpp b/src/modules/unit_test/unit_test.cpp
index c45d7888ab..9216bb0f34 100644
--- a/src/modules/unit_test/unit_test.cpp
+++ b/src/modules/unit_test/unit_test.cpp
@@ -50,7 +50,13 @@ UnitTest::~UnitTest()
 
 void UnitTest::print_results(void)
 {
-	warnx(_tests_failed ? "SOME TESTS FAILED" : "ALL TESTS PASSED");
+	if (_tests_failed) {
+		warnx("SOME TESTS FAILED");
+
+	} else {
+		warnx("ALL TESTS PASSED");
+	}
+
 	warnx("  Tests passed : %d", _tests_passed);
 	warnx("  Tests failed : %d", _tests_failed);
 	warnx("  Assertions : %d", _assertions);
diff --git a/src/systemcmds/tests/test_uart_console.c b/src/systemcmds/tests/test_uart_console.c
index ad1b2776e1..682ad5a129 100644
--- a/src/systemcmds/tests/test_uart_console.c
+++ b/src/systemcmds/tests/test_uart_console.c
@@ -38,6 +38,7 @@
  ****************************************************************************/
 
 #include <px4_config.h>
+#include <px4_tasks.h>
 
 #include <sys/types.h>
 

From dc8b9d1da84fe79ce367529ff78bbcb0c748caa8 Mon Sep 17 00:00:00 2001
From: Daniel Agar <daniel@agar.ca>
Date: Sun, 24 Apr 2016 02:09:06 -0400
Subject: [PATCH 0073/1230] px4fmu-v2_test add drivers/test_ppm

---
 cmake/configs/nuttx_px4fmu-v2_test.cmake | 1 +
 1 file changed, 1 insertion(+)

diff --git a/cmake/configs/nuttx_px4fmu-v2_test.cmake b/cmake/configs/nuttx_px4fmu-v2_test.cmake
index 0adab48b85..0d6a8e7171 100644
--- a/cmake/configs/nuttx_px4fmu-v2_test.cmake
+++ b/cmake/configs/nuttx_px4fmu-v2_test.cmake
@@ -70,6 +70,7 @@ set(config_module_list
 	#
 	# Testing
 	#
+	drivers/test_ppm
 	modules/commander/commander_tests
 	modules/controllib_test
 	modules/mavlink/mavlink_tests

From 12165ba5a48ae8be1cf0c4034f138f5bfcad7a14 Mon Sep 17 00:00:00 2001
From: Daniel Agar <daniel@agar.ca>
Date: Sun, 24 Apr 2016 02:01:43 -0400
Subject: [PATCH 0074/1230] uORB separate tests

---
 cmake/configs/nuttx_px4fmu-v2_test.cmake      |  1 +
 cmake/configs/posix_sitl_test.cmake           |  1 +
 src/modules/uORB/CMakeLists.txt               |  3 -
 src/modules/uORB/uORBMain.cpp                 | 42 +--------
 src/modules/uORB/uORB_tests/CMakeLists.txt    | 53 +++++++++++
 .../{ => uORB_tests}/uORBTest_UnitTest.cpp    |  2 +-
 .../{ => uORB_tests}/uORBTest_UnitTest.hpp    |  4 +-
 .../uORB/uORB_tests/uORB_tests_main.cpp       | 90 +++++++++++++++++++
 8 files changed, 149 insertions(+), 47 deletions(-)
 create mode 100644 src/modules/uORB/uORB_tests/CMakeLists.txt
 rename src/modules/uORB/{ => uORB_tests}/uORBTest_UnitTest.cpp (99%)
 rename src/modules/uORB/{ => uORB_tests}/uORBTest_UnitTest.hpp (98%)
 create mode 100644 src/modules/uORB/uORB_tests/uORB_tests_main.cpp

diff --git a/cmake/configs/nuttx_px4fmu-v2_test.cmake b/cmake/configs/nuttx_px4fmu-v2_test.cmake
index 0d6a8e7171..95e6c07e9b 100644
--- a/cmake/configs/nuttx_px4fmu-v2_test.cmake
+++ b/cmake/configs/nuttx_px4fmu-v2_test.cmake
@@ -75,6 +75,7 @@ set(config_module_list
 	modules/controllib_test
 	modules/mavlink/mavlink_tests
 	modules/unit_test
+	modules/uORB/uORB_tests
 	systemcmds/tests
 
 	#
diff --git a/cmake/configs/posix_sitl_test.cmake b/cmake/configs/posix_sitl_test.cmake
index 29e8ee7c8b..e9fd6734cc 100644
--- a/cmake/configs/posix_sitl_test.cmake
+++ b/cmake/configs/posix_sitl_test.cmake
@@ -76,6 +76,7 @@ set(config_module_list
 	modules/controllib_test
 	#modules/mavlink/mavlink_tests
 	modules/unit_test
+	modules/uORB/uORB_tests
 	systemcmds/tests
 
 	)
diff --git a/src/modules/uORB/CMakeLists.txt b/src/modules/uORB/CMakeLists.txt
index 894ffc63e4..d8f8c9bc2a 100644
--- a/src/modules/uORB/CMakeLists.txt
+++ b/src/modules/uORB/CMakeLists.txt
@@ -49,17 +49,14 @@ set(SRCS
 if(${OS} STREQUAL "nuttx")
 	list(APPEND SRCS
 		uORBDevices_nuttx.cpp
-		uORBTest_UnitTest.cpp
 		)
 elseif(${OS} STREQUAL "posix")
 	list(APPEND SRCS
 		uORBDevices_posix.cpp
-		uORBTest_UnitTest.cpp
 		)
 elseif(${OS} STREQUAL "posix-arm")
 	list(APPEND SRCS
 		uORBDevices_posix.cpp
-		uORBTest_UnitTest.cpp
 		)
 elseif(${OS} STREQUAL "qurt")
 	list(APPEND SRCS
diff --git a/src/modules/uORB/uORBMain.cpp b/src/modules/uORB/uORBMain.cpp
index cdc18e23bd..66b98debf2 100644
--- a/src/modules/uORB/uORBMain.cpp
+++ b/src/modules/uORB/uORBMain.cpp
@@ -37,19 +37,14 @@
 #include "uORB.h"
 #include "uORBCommon.hpp"
 
-#ifndef __PX4_QURT
-#include "uORBTest_UnitTest.hpp"
-#endif
-
 extern "C" { __EXPORT int uorb_main(int argc, char *argv[]); }
 
 static uORB::DeviceMaster *g_dev = nullptr;
 static void usage()
 {
-	PX4_INFO("Usage: uorb 'start', 'test', 'latency_test' or 'status'");
+	PX4_INFO("Usage: uorb 'start', 'status'");
 }
 
-
 int
 uorb_main(int argc, char *argv[])
 {
@@ -94,41 +89,6 @@ uorb_main(int argc, char *argv[])
 		return OK;
 	}
 
-#ifndef __PX4_QURT
-
-	/*
-	 * Test the driver/device.
-	 */
-	if (!strcmp(argv[1], "test")) {
-		if (!g_dev) {
-			PX4_WARN("orb is not running! start it first");
-			return -ESRCH;
-		}
-
-		uORBTest::UnitTest &t = uORBTest::UnitTest::instance();
-		return t.test();
-	}
-
-	/*
-	 * Test the latency.
-	 */
-	if (!strcmp(argv[1], "latency_test")) {
-
-		uORBTest::UnitTest &t = uORBTest::UnitTest::instance();
-
-		if (argc > 2 && !strcmp(argv[2], "medium")) {
-			return t.latency_test<struct orb_test_medium>(ORB_ID(orb_test_medium), true);
-
-		} else if (argc > 2 && !strcmp(argv[2], "large")) {
-			return t.latency_test<struct orb_test_large>(ORB_ID(orb_test_large), true);
-
-		} else {
-			return t.latency_test<struct orb_test>(ORB_ID(orb_test), true);
-		}
-	}
-
-#endif
-
 	/*
 	 * Print driver information.
 	 */
diff --git a/src/modules/uORB/uORB_tests/CMakeLists.txt b/src/modules/uORB/uORB_tests/CMakeLists.txt
new file mode 100644
index 0000000000..5f88743d3a
--- /dev/null
+++ b/src/modules/uORB/uORB_tests/CMakeLists.txt
@@ -0,0 +1,53 @@
+############################################################################
+#
+#   Copyright (c) 2015 PX4 Development Team. All rights reserved.
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions
+# are met:
+#
+# 1. Redistributions of source code must retain the above copyright
+#    notice, this list of conditions and the following disclaimer.
+# 2. Redistributions in binary form must reproduce the above copyright
+#    notice, this list of conditions and the following disclaimer in
+#    the documentation and/or other materials provided with the
+#    distribution.
+# 3. Neither the name PX4 nor the names of its contributors may be
+#    used to endorse or promote products derived from this software
+#    without specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.
+#
+############################################################################
+
+# this includes the generated topics directory
+include_directories(${CMAKE_CURRENT_BINARY_DIR})
+
+set(SRCS uORB_tests_main.cpp)
+
+if(NOT ${OS} STREQUAL "qurt")
+	list(APPEND SRCS uORBTest_UnitTest.cpp)
+endif()
+
+px4_add_module(
+	MODULE modules__uORB__uORB_tests
+	MAIN uorb_tests
+	STACK_MAIN 2048
+	COMPILE_FLAGS
+		-Os
+	SRCS ${SRCS}
+	DEPENDS
+		platforms__common
+	)
+# vim: set noet ft=cmake fenc=utf-8 ff=unix : 
diff --git a/src/modules/uORB/uORBTest_UnitTest.cpp b/src/modules/uORB/uORB_tests/uORBTest_UnitTest.cpp
similarity index 99%
rename from src/modules/uORB/uORBTest_UnitTest.cpp
rename to src/modules/uORB/uORB_tests/uORBTest_UnitTest.cpp
index c91172c107..2812336e79 100644
--- a/src/modules/uORB/uORBTest_UnitTest.cpp
+++ b/src/modules/uORB/uORB_tests/uORBTest_UnitTest.cpp
@@ -32,7 +32,7 @@
  ****************************************************************************/
 
 #include "uORBTest_UnitTest.hpp"
-#include "uORBCommon.hpp"
+#include "../uORBCommon.hpp"
 #include <px4_config.h>
 #include <px4_time.h>
 #include <stdio.h>
diff --git a/src/modules/uORB/uORBTest_UnitTest.hpp b/src/modules/uORB/uORB_tests/uORBTest_UnitTest.hpp
similarity index 98%
rename from src/modules/uORB/uORBTest_UnitTest.hpp
rename to src/modules/uORB/uORB_tests/uORBTest_UnitTest.hpp
index 5aae62c725..5722b164b3 100644
--- a/src/modules/uORB/uORBTest_UnitTest.hpp
+++ b/src/modules/uORB/uORB_tests/uORBTest_UnitTest.hpp
@@ -33,8 +33,8 @@
 
 #ifndef _uORBTest_UnitTest_hpp_
 #define _uORBTest_UnitTest_hpp_
-#include "uORBCommon.hpp"
-#include "uORB.h"
+#include "../uORBCommon.hpp"
+#include "../uORB.h"
 #include <px4_time.h>
 
 struct orb_test {
diff --git a/src/modules/uORB/uORB_tests/uORB_tests_main.cpp b/src/modules/uORB/uORB_tests/uORB_tests_main.cpp
new file mode 100644
index 0000000000..6c0f109461
--- /dev/null
+++ b/src/modules/uORB/uORB_tests/uORB_tests_main.cpp
@@ -0,0 +1,90 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2012-2015 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+#include <string.h>
+#include "../uORBDevices.hpp"
+#include "../uORB.h"
+#include "../uORBCommon.hpp"
+
+#ifndef __PX4_QURT
+#include "uORBTest_UnitTest.hpp"
+#endif
+
+extern "C" { __EXPORT int uorb_tests_main(int argc, char *argv[]); }
+
+static void usage()
+{
+	PX4_INFO("Usage: uorb_test 'test', 'latency_test'");
+}
+
+int
+uorb_tests_main(int argc, char *argv[])
+{
+	if (argc < 2) {
+		usage();
+		return -EINVAL;
+	}
+
+#ifndef __PX4_QURT
+
+	/*
+	 * Test the driver/device.
+	 */
+	if (!strcmp(argv[1], "test")) {
+		uORBTest::UnitTest &t = uORBTest::UnitTest::instance();
+		return t.test();
+	}
+
+	/*
+	 * Test the latency.
+	 */
+	if (!strcmp(argv[1], "latency_test")) {
+
+		uORBTest::UnitTest &t = uORBTest::UnitTest::instance();
+
+		if (argc > 2 && !strcmp(argv[2], "medium")) {
+			return t.latency_test<struct orb_test_medium>(ORB_ID(orb_test_medium), true);
+
+		} else if (argc > 2 && !strcmp(argv[2], "large")) {
+			return t.latency_test<struct orb_test_large>(ORB_ID(orb_test_large), true);
+
+		} else {
+			return t.latency_test<struct orb_test>(ORB_ID(orb_test), true);
+		}
+	}
+
+#endif
+
+	usage();
+	return -EINVAL;
+}

From 008354f9358dd1e7a8adf2e57a17d6dc5fc5cf83 Mon Sep 17 00:00:00 2001
From: Daniel Agar <daniel@agar.ca>
Date: Sun, 24 Apr 2016 13:26:27 -0400
Subject: [PATCH 0075/1230] testing cleanup

---
 Makefile                                      |  2 +-
 ROMFS/px4fmu_test/init.d/rcS                  | 96 ++++++++++++-------
 posix-configs/SITL/init/rcS_test_jmavsim_iris | 47 +--------
 .../state_machine_helper_test.cpp             | 33 ++++---
 .../commander/state_machine_helper.cpp        |  8 +-
 src/modules/mavlink/mavlink_main.cpp          | 14 +--
 .../uORB/uORB_tests/uORBTest_UnitTest.cpp     |  2 +-
 .../uORB/uORB_tests/uORB_tests_main.cpp       | 23 +++--
 src/modules/unit_test/unit_test.cpp           | 14 +--
 src/modules/unit_test/unit_test.h             |  6 +-
 src/systemcmds/tests/tests_main.c             | 53 ++++++----
 11 files changed, 153 insertions(+), 145 deletions(-)

diff --git a/Makefile b/Makefile
index c66c326729..286bf7ad56 100644
--- a/Makefile
+++ b/Makefile
@@ -250,7 +250,7 @@ ifeq ($(VECTORCONTROL),1)
 	@rm -rf ROMFS/px4fmu_common/uavcan
 endif
 
-unittest: posix_sitl_default
+unittest: posix_sitl_test
 	@(mkdir -p build_unittests && cd build_unittests && cmake -G$(PX4_CMAKE_GENERATOR) ../unittests && $(PX4_MAKE) $(PX4_MAKE_ARGS) && ctest -j2 --output-on-failure)
 
 package_firmware:
diff --git a/ROMFS/px4fmu_test/init.d/rcS b/ROMFS/px4fmu_test/init.d/rcS
index abb4f3dfb8..b9eadfa865 100644
--- a/ROMFS/px4fmu_test/init.d/rcS
+++ b/ROMFS/px4fmu_test/init.d/rcS
@@ -38,6 +38,11 @@ fi
 # Start a minimal system
 #
 
+#
+# Start the ORB (first app to start)
+#
+uorb start
+
 #
 # Load parameters
 #
@@ -104,43 +109,6 @@ then
 	tests mount
 fi
 
-#
-# Run unit tests at board boot, reporting failure as needed.
-# Add new unit tests using the same pattern as below.
-#
-
-if mavlink_tests
-then
-else
-	set unit_test_failure 1
-	set unit_test_failure_list "${unit_test_failure_list} mavlink_tests"
-fi
-
-# TODO: commander_tests is currently broken
-#if commander_tests
-#then
-#else
-#	set unit_test_failure 1
-#	set unit_test_failure_list "${unit_test_failure_list} commander_tests"
-#fi
-
-if controllib_test
-then
-else
-	set unit_test_failure 1
-	set unit_test_failure_list "${unit_test_failure_list} controllib_tests"
-fi
-
-if uorb test
-then
-else
-	set unit_test_failure 1
-	set unit_test_failure_list "${unit_test_failure_list} uorb_tests"
-fi
-
-#
-# Sensors System (start before Commander so Preflight checks are properly run)
-#
 sh /etc/init.d/rc.sensors
 
 # Check for flow sensor
@@ -152,6 +120,60 @@ if ll40ls start
 then
 fi
 
+#
+# Run unit tests at board boot, reporting failure as needed.
+# Add new unit tests using the same pattern as below.
+#
+
+echo
+echo "--------------------------------------------------------------------------------"
+echo "[mavlink_tests]             STARTING TEST"
+if mavlink_tests
+then
+	echo "[mavlink_tests]             PASS"
+else
+	set unit_test_failure 1
+	set unit_test_failure_list "${unit_test_failure_list} mavlink_tests"
+	echo "[mavlink_tests]             FAILED"
+fi
+echo "--------------------------------------------------------------------------------"
+echo
+echo "--------------------------------------------------------------------------------"
+echo "[commander_tests]             STARTING TEST"
+if commander_tests
+then
+	echo "[commander_tests]             PASS"
+else
+	set unit_test_failure 1
+	set unit_test_failure_list "${unit_test_failure_list} commander_tests"
+	echo "[commander_tests]             FAILED"
+fi
+echo "--------------------------------------------------------------------------------"
+echo
+echo "--------------------------------------------------------------------------------"
+echo "[controllib_test]             STARTING TEST"
+if controllib_test
+then
+	echo "[controllib_test]             PASS"
+else
+	set unit_test_failure 1
+	set unit_test_failure_list "${unit_test_failure_list} controllib_tests"
+	echo "[controllib_test]             FAILED"
+fi
+echo "--------------------------------------------------------------------------------"
+echo
+echo "--------------------------------------------------------------------------------"
+echo "[uorb_tests]             STARTING TEST"
+if uorb_tests
+then
+	echo "[uorb_tests]             PASS"
+else
+	set unit_test_failure 1
+	set unit_test_failure_list "${unit_test_failure_list} uorb_tests"
+	echo "[uorb_tests]             FAILED"
+fi
+echo "--------------------------------------------------------------------------------"
+
 if tests all
 then
 else
diff --git a/posix-configs/SITL/init/rcS_test_jmavsim_iris b/posix-configs/SITL/init/rcS_test_jmavsim_iris
index 3843f2a9b7..b419960f24 100644
--- a/posix-configs/SITL/init/rcS_test_jmavsim_iris
+++ b/posix-configs/SITL/init/rcS_test_jmavsim_iris
@@ -1,47 +1,9 @@
 uorb start
-simulator start -s
 param load
-param set MAV_TYPE 2
-param set MC_PITCHRATE_P 0.15
-param set MC_PITCH_P 7
-param set MC_ROLL_P 7
-param set MC_ROLLRATE_P 0.15
-param set MC_YAW_P 2.8
-param set MC_YAWRATE_P 0.35
-param set SYS_AUTOSTART 4010
-param set SYS_RESTART_TYPE 2
+
 dataman start
-param set CAL_GYRO0_ID 2293768
-param set CAL_ACC0_ID 1376264
-param set CAL_ACC1_ID 1310728
-param set CAL_MAG0_ID 196616
-param set CAL_GYRO0_XOFF 0.01
-param set CAL_ACC0_XOFF 0.01
-param set CAL_ACC0_YOFF -0.01
-param set CAL_ACC0_ZOFF 0.01
-param set CAL_ACC0_XSCALE 1.01
-param set CAL_ACC0_YSCALE 1.01
-param set CAL_ACC0_ZSCALE 1.01
-param set CAL_ACC1_XOFF 0.01
-param set CAL_MAG0_XOFF 0.01
-param set MPC_XY_P 0.4
-param set MPC_XY_VEL_P 0.2
-param set MPC_XY_VEL_D 0.005
-param set SENS_BOARD_ROT 0
-param set SENS_BOARD_X_OFF 0.000001
-param set COM_RC_IN_MODE 1
-param set COM_DL_LOSS_EN 1
-param set COM_DISARM_LAND 3
-param set NAV_ACC_RAD 2.0
-param set RTL_RETURN_ALT 30.0
-param set RTL_DESCEND_ALT 10.0
-param set RTL_LAND_DELAY 0
-param set MIS_TAKEOFF_ALT 2.5
-param set MPC_HOLD_MAX_Z 2.0
-param set MPC_HOLD_XY_DZ 0.1
-param set MPC_HOLD_Z_DZ 0.1
-param set MPC_Z_VEL_MAX 2.0
-param set MPC_Z_VEL_P 0.4
+
+simulator start -s
 rgbledsim start
 tone_alarm start
 gyrosim start
@@ -52,10 +14,11 @@ gpssim start
 pwm_out_sim mode_pwm
 sleep 1
 sensors start
+commander start
 
 commander_tests
 controllib_test
-uorb test
+uorb_tests
 tests all
 
 ver all
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 2c8b4675dd..cc4d32bff3 100644
--- a/src/modules/commander/commander_tests/state_machine_helper_test.cpp
+++ b/src/modules/commander/commander_tests/state_machine_helper_test.cpp
@@ -255,10 +255,10 @@ bool StateMachineHelperTest::armingStateTransitionTest(void)
 
         // Sensor tests
 
-        { "transition to standby error: init to standby - sensors not initialized",
-            { vehicle_status_s::ARMING_STATE_INIT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_NOT_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
-            vehicle_status_s::ARMING_STATE_STANDBY,
-            { vehicle_status_s::ARMING_STATE_STANDBY_ERROR, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_DENIED },
+        //{ "transition to standby error: init to standby - sensors not initialized",
+        //    { vehicle_status_s::ARMING_STATE_INIT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_NOT_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
+        //    vehicle_status_s::ARMING_STATE_STANDBY,
+        //    { vehicle_status_s::ARMING_STATE_STANDBY_ERROR, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_DENIED },
 
         // Safety switch arming tests
 
@@ -297,10 +297,10 @@ bool StateMachineHelperTest::armingStateTransitionTest(void)
         		5.0f);
 
         // Validate result of transition
-        ut_assert(test->assertMsg, test->expected_transition_result == result);
-        ut_assert(test->assertMsg, status.arming_state == test->expected_state.arming_state);
-        ut_assert(test->assertMsg, armed.armed == test->expected_state.armed);
-        ut_assert(test->assertMsg, armed.ready_to_arm == test->expected_state.ready_to_arm);
+        ut_compare(test->assertMsg, test->expected_transition_result, result);
+        ut_compare(test->assertMsg, status.arming_state, test->expected_state.arming_state);
+        ut_compare(test->assertMsg, armed.armed, test->expected_state.armed);
+        ut_compare(test->assertMsg, armed.ready_to_arm, test->expected_state.ready_to_arm);
     }
 
 	return true;
@@ -449,17 +449,16 @@ bool StateMachineHelperTest::mainStateTransitionTest(void)
 									&current_status_flags, &current_commander_state);
 
 		// Validate result of transition
-		ut_assert(test->assertMsg, test->expected_transition_result == result);
+		ut_compare(test->assertMsg, test->expected_transition_result, result);
 		if (test->expected_transition_result == result) {
 			if (test->expected_transition_result == TRANSITION_CHANGED) {
-				ut_assert(test->assertMsg, test->to_state == current_commander_state.main_state);
+				ut_compare(test->assertMsg, test->to_state, current_commander_state.main_state);
 			} else {
-				ut_assert(test->assertMsg, test->from_state == current_commander_state.main_state);
+				ut_compare(test->assertMsg, test->from_state, current_commander_state.main_state);
 			}
 		}
 	}
 
-
 	return true;
 }
 
@@ -473,31 +472,31 @@ bool StateMachineHelperTest::isSafeTest(void)
 	armed.lockdown = false;
 	safety.safety_switch_available = true;
 	safety.safety_off = false;
-	ut_assert("is safe: not armed", is_safe(&current_state, &safety, &armed));
+	ut_compare("is safe: not armed", is_safe(&current_state, &safety, &armed), true);
 
 	armed.armed = false;
 	armed.lockdown = true;
 	safety.safety_switch_available = true;
 	safety.safety_off = true;
-	ut_assert("is safe: software lockdown", is_safe(&current_state, &safety, &armed));
+	ut_compare("is safe: software lockdown", is_safe(&current_state, &safety, &armed), true);
 
 	armed.armed = true;
 	armed.lockdown = false;
 	safety.safety_switch_available = true;
 	safety.safety_off = true;
-	ut_assert("not safe: safety off", !is_safe(&current_state, &safety, &armed));
+	ut_compare("not safe: safety off", is_safe(&current_state, &safety, &armed), false);
 
 	armed.armed = true;
 	armed.lockdown = false;
 	safety.safety_switch_available = true;
 	safety.safety_off = false;
-	ut_assert("is safe: safety off", is_safe(&current_state, &safety, &armed));
+	ut_compare("is safe: safety off", is_safe(&current_state, &safety, &armed), true);
 
 	armed.armed = true;
 	armed.lockdown = false;
 	safety.safety_switch_available = false;
 	safety.safety_off = false;
-	ut_assert("not safe: no safety switch", !is_safe(&current_state, &safety, &armed));
+	ut_compare("not safe: no safety switch", is_safe(&current_state, &safety, &armed), false);
 
 	return true;
 }
diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp
index 8616f0f27f..36d109cbdc 100644
--- a/src/modules/commander/state_machine_helper.cpp
+++ b/src/modules/commander/state_machine_helper.cpp
@@ -80,7 +80,7 @@ using namespace DriverFramework;
 #endif
 
 // This array defines the arming state transitions. The rows are the new state, and the columns
-// are the current state. Using new state and current  state you can index into the array which
+// are the current state. Using new state and current state you can index into the array which
 // will be true for a valid transition or false for a invalid transition. In some cases even
 // though the transition is marked as true additional checks must be made. See arming_state_transition
 // code for those checks.
@@ -145,10 +145,11 @@ transition_result_t arming_state_transition(struct vehicle_status_s *status,
 			prearm_ret = preflight_check(status, mavlink_log_pub, true /* pre-arm */, false /* force_report */,
 						     status_flags, battery);
 		}
+
 		/* re-run the pre-flight check as long as sensors are failing */
 		if (!status_flags->condition_system_sensors_initialized
-				&& (new_arming_state == vehicle_status_s::ARMING_STATE_ARMED
-				|| new_arming_state == vehicle_status_s::ARMING_STATE_STANDBY)
+				&& ((new_arming_state == vehicle_status_s::ARMING_STATE_ARMED
+						|| new_arming_state == vehicle_status_s::ARMING_STATE_STANDBY))
 				&& status->hil_state == vehicle_status_s::HIL_STATE_OFF) {
 
 			if (last_preflight_check == 0 || hrt_absolute_time() - last_preflight_check > 1000 * 1000) {
@@ -280,6 +281,7 @@ transition_result_t arming_state_transition(struct vehicle_status_s *status,
 			if ((!status_flags->condition_system_prearm_error_reported &&
 			      status_flags->condition_system_hotplug_timeout) ||
 			     (new_arming_state == vehicle_status_s::ARMING_STATE_ARMED)) {
+
 				mavlink_and_console_log_critical(mavlink_log_pub, "Not ready to fly: Sensors not set up correctly");
 				status_flags->condition_system_prearm_error_reported = true;
 			}
diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp
index c522c4db6c..f35541c1e4 100644
--- a/src/modules/mavlink/mavlink_main.cpp
+++ b/src/modules/mavlink/mavlink_main.cpp
@@ -2006,7 +2006,7 @@ Mavlink::task_main(int argc, char *argv[])
 				// allocated.
 				//fclose(fs);
 			} else {
-				warnx("open fd %d failed", _uart_fd);
+				PX4_WARN("open fd %d failed", _uart_fd);
 			}
 
 			/* reset param and save */
@@ -2040,26 +2040,26 @@ Mavlink::task_main(int argc, char *argv[])
 			if (OK == configure_stream(_subscribe_to_stream, _subscribe_to_stream_rate)) {
 				if (_subscribe_to_stream_rate > 0.0f) {
 					if ( get_protocol() == SERIAL ) {
-						warnx("stream %s on device %s enabled with rate %.1f Hz", _subscribe_to_stream, _device_name,
+						PX4_INFO("stream %s on device %s enabled with rate %.1f Hz", _subscribe_to_stream, _device_name,
 							(double)_subscribe_to_stream_rate);
 					} else if ( get_protocol() == UDP ) {
-						warnx("stream %s on UDP port %d enabled with rate %.1f Hz", _subscribe_to_stream, _network_port,
+						PX4_INFO("stream %s on UDP port %d enabled with rate %.1f Hz", _subscribe_to_stream, _network_port,
 							(double)_subscribe_to_stream_rate);
 					}
 
 				} else {
 					if ( get_protocol() == SERIAL ) {
-						warnx("stream %s on device %s disabled", _subscribe_to_stream, _device_name);
+						PX4_WARN("stream %s on device %s disabled", _subscribe_to_stream, _device_name);
 					} else if ( get_protocol() == UDP ) {
-						warnx("stream %s on UDP port %d disabled", _subscribe_to_stream, _network_port);
+						PX4_WARN("stream %s on UDP port %d disabled", _subscribe_to_stream, _network_port);
 					}
 				}
 
 			} else {
 				if ( get_protocol() == SERIAL ) {
-					warnx("stream %s on device %s not found", _subscribe_to_stream, _device_name);
+					PX4_WARN("stream %s on device %s not found", _subscribe_to_stream, _device_name);
 				} else if ( get_protocol() == UDP ) {
-					warnx("stream %s on UDP port %d not found", _subscribe_to_stream, _network_port);
+					PX4_WARN("stream %s on UDP port %d not found", _subscribe_to_stream, _network_port);
 				}
 			}
 
diff --git a/src/modules/uORB/uORB_tests/uORBTest_UnitTest.cpp b/src/modules/uORB/uORB_tests/uORBTest_UnitTest.cpp
index 2812336e79..78b2f0e1e0 100644
--- a/src/modules/uORB/uORB_tests/uORBTest_UnitTest.cpp
+++ b/src/modules/uORB/uORB_tests/uORBTest_UnitTest.cpp
@@ -128,7 +128,7 @@ int uORBTest::UnitTest::pubsublatency_main(void)
 
 	pubsubtest_passed = true;
 
-	if (static_cast<float>(latency_integral / maxruns) > 30.0f) {
+	if (static_cast<float>(latency_integral / maxruns) > 40.0f) {
 		pubsubtest_res = uORB::ERROR;
 
 	} else {
diff --git a/src/modules/uORB/uORB_tests/uORB_tests_main.cpp b/src/modules/uORB/uORB_tests/uORB_tests_main.cpp
index 6c0f109461..1f0162f15d 100644
--- a/src/modules/uORB/uORB_tests/uORB_tests_main.cpp
+++ b/src/modules/uORB/uORB_tests/uORB_tests_main.cpp
@@ -44,31 +44,38 @@ extern "C" { __EXPORT int uorb_tests_main(int argc, char *argv[]); }
 
 static void usage()
 {
-	PX4_INFO("Usage: uorb_test 'test', 'latency_test'");
+	PX4_INFO("Usage: uorb_test 'latency_test'");
 }
 
 int
 uorb_tests_main(int argc, char *argv[])
 {
-	if (argc < 2) {
-		usage();
-		return -EINVAL;
-	}
 
 #ifndef __PX4_QURT
 
 	/*
 	 * Test the driver/device.
 	 */
-	if (!strcmp(argv[1], "test")) {
+	if (argc == 1) {
 		uORBTest::UnitTest &t = uORBTest::UnitTest::instance();
-		return t.test();
+		int rc = t.test();
+
+		if (rc == OK) {
+			fprintf(stdout, "  [uORBTest] \t\tPASS\n");
+			fflush(stdout);
+			return 0;
+
+		} else {
+			fprintf(stderr, "  [uORBTest] \t\tFAIL\n");
+			fflush(stderr);
+			return -1;
+		}
 	}
 
 	/*
 	 * Test the latency.
 	 */
-	if (!strcmp(argv[1], "latency_test")) {
+	if (argc > 1 && !strcmp(argv[1], "latency_test")) {
 
 		uORBTest::UnitTest &t = uORBTest::UnitTest::instance();
 
diff --git a/src/modules/unit_test/unit_test.cpp b/src/modules/unit_test/unit_test.cpp
index 9216bb0f34..7b3f3a41d1 100644
--- a/src/modules/unit_test/unit_test.cpp
+++ b/src/modules/unit_test/unit_test.cpp
@@ -51,26 +51,26 @@ UnitTest::~UnitTest()
 void UnitTest::print_results(void)
 {
 	if (_tests_failed) {
-		warnx("SOME TESTS FAILED");
+		PX4_ERR("SOME TESTS FAILED");
 
 	} else {
-		warnx("ALL TESTS PASSED");
+		PX4_INFO("ALL TESTS PASSED");
 	}
 
-	warnx("  Tests passed : %d", _tests_passed);
-	warnx("  Tests failed : %d", _tests_failed);
-	warnx("  Assertions : %d", _assertions);
+	PX4_INFO("  Tests passed : %d", _tests_passed);
+	PX4_INFO("  Tests failed : %d", _tests_failed);
+	PX4_INFO("  Assertions : %d", _assertions);
 }
 
 /// @brief Used internally to the ut_assert macro to print assert failures.
 void UnitTest::_print_assert(const char *msg, const char *test, const char *file, int line)
 {
-	warnx("Assertion failed: %s - %s (%s:%d)", msg, test, file, line);
+	PX4_ERR("Assertion failed: %s - %s (%s:%d)", msg, test, file, line);
 }
 
 /// @brief Used internally to the ut_compare macro to print assert failures.
 void UnitTest::_print_compare(const char *msg, const char *v1_text, int v1, const char *v2_text, int v2,
 			      const char *file, int line)
 {
-	warnx("Compare failed: %s - (%s:%d) (%s:%d) (%s:%d)", msg, v1_text, v1, v2_text, v2, file, line);
+	PX4_ERR("Compare failed: %s - (%s:%d) (%s:%d) (%s:%d)", msg, v1_text, v1, v2_text, v2, file, line);
 }
diff --git a/src/modules/unit_test/unit_test.h b/src/modules/unit_test/unit_test.h
index bd021ff54a..f512100bb6 100644
--- a/src/modules/unit_test/unit_test.h
+++ b/src/modules/unit_test/unit_test.h
@@ -68,14 +68,14 @@ protected:
 /// test should return true if it succeeded, false for fail.
 #define ut_run_test(test)					\
 	do {							\
-		warnx("RUNNING TEST: %s", #test);		\
+		PX4_INFO("RUNNING TEST: %s", #test);		\
 		_tests_run++;					\
 		_init();						\
 		if (!test()) {					\
-			warnx("TEST FAILED: %s", #test);	\
+			PX4_ERR("TEST FAILED: %s", #test);	\
 			_tests_failed++;			\
 		} else {					\
-			warnx("TEST PASSED: %s", #test);	\
+			PX4_INFO("TEST PASSED: %s", #test);	\
 			_tests_passed++;			\
 		}						\
 		_cleanup();					\
diff --git a/src/systemcmds/tests/tests_main.c b/src/systemcmds/tests/tests_main.c
index 453705eaf4..e37a823e13 100644
--- a/src/systemcmds/tests/tests_main.c
+++ b/src/systemcmds/tests/tests_main.c
@@ -78,16 +78,19 @@ const struct {
 #define OPT_NOALLTEST	(1<<1)
 #define OPT_NOJIGTEST	(1<<2)
 } tests[] = {
+#ifdef __PX4_NUTTX
 	{"led",			test_led,	0},
+	{"time",		test_time,	OPT_NOJIGTEST},
+	{"sensors",		test_sensors,	0},
+	{"adc",			test_adc,	OPT_NOJIGTEST},
+#endif /* __PX4_NUTTX */
 	{"int",			test_int,	0},
 	{"float",		test_float,	0},
-	{"sensors",		test_sensors,	0},
 	{"gpio",		test_gpio,	OPT_NOJIGTEST | OPT_NOALLTEST},
 	{"hrt",			test_hrt,	OPT_NOJIGTEST | OPT_NOALLTEST},
 	{"ppm",			test_ppm,	OPT_NOJIGTEST | OPT_NOALLTEST},
 	{"servo",		test_servo,	OPT_NOJIGTEST | OPT_NOALLTEST},
 	{"ppm_loopback",	test_ppm_loopback,	OPT_NOALLTEST},
-	{"adc",			test_adc,	OPT_NOJIGTEST},
 	{"jig_voltages",	test_jig_voltages,	OPT_NOALLTEST},
 	{"uart_loopback",	test_uart_loopback,	OPT_NOJIGTEST | OPT_NOALLTEST},
 	{"uart_baudchange",	test_uart_baudchange,	OPT_NOJIGTEST | OPT_NOALLTEST},
@@ -96,9 +99,6 @@ const struct {
 	{"hott_telemetry",	test_hott_telemetry,	OPT_NOJIGTEST | OPT_NOALLTEST},
 	{"tone",		test_tone,	0},
 	{"sleep",		test_sleep,	OPT_NOJIGTEST},
-#ifdef __PX4_NUTTX
-	{"time",		test_time,	OPT_NOJIGTEST},
-#endif
 	{"perf",		test_perf,	OPT_NOJIGTEST},
 	{"all",			test_all,	OPT_NOALLTEST | OPT_NOJIGTEST},
 	{"jig",			test_jig,	OPT_NOJIGTEST | OPT_NOALLTEST},
@@ -150,23 +150,29 @@ test_all(int argc, char *argv[])
 	for (i = 0; tests[i].name; i++) {
 		/* Only run tests that are not excluded */
 		if (!(tests[i].options & OPT_NOALLTEST)) {
-			printf("  [%s] \t\t\tSTARTING TEST\n", tests[i].name);
+			for (int j = 0; j < 80; j++) {
+				printf("-");
+			}
+
+			printf("\n");
+
+			printf("  [%s] \t\tSTARTING TEST\n", tests[i].name);
 			fflush(stdout);
 
 			/* Execute test */
 			if (tests[i].fn(1, args) != 0) {
-				fprintf(stderr, "  [%s] \t\t\tFAIL\n", tests[i].name);
+				fprintf(stderr, "  [%s] \t\tFAIL\n", tests[i].name);
 				fflush(stderr);
 				failcount++;
 				passed[i] = false;
 
 			} else {
-				printf("  [%s] \t\t\tPASS\n", tests[i].name);
+				printf("  [%s] \t\tPASS\n", tests[i].name);
 				fflush(stdout);
 				passed[i] = true;
 			}
 
-			for (int j = 0; j < 40; j++) {
+			for (int j = 0; j < 80; j++) {
 				printf("-");
 			}
 
@@ -179,8 +185,8 @@ test_all(int argc, char *argv[])
 	/* Print summary */
 	printf("\n");
 
-	for (int j = 0; j < 40; j++) {
-		printf("-");
+	for (int j = 0; j < 80; j++) {
+		printf("#");
 	}
 
 	printf("\n\n");
@@ -269,31 +275,42 @@ int test_jig(int argc, char *argv[])
 	for (i = 0; tests[i].name; i++) {
 		/* Only run tests that are not excluded */
 		if (!(tests[i].options & OPT_NOJIGTEST)) {
-			printf("  [%s] \t\t\tSTARTING TEST\n", tests[i].name);
+			for (int j = 0; j < 80; j++) {
+				printf("-");
+			}
+
+			printf("\n");
+
+			printf("  [%s] \t\tSTARTING TEST\n", tests[i].name);
 			fflush(stdout);
 
 			/* Execute test */
 			if (tests[i].fn(1, args) != 0) {
-				fprintf(stderr, "  [%s] \t\t\tFAIL\n", tests[i].name);
+				fprintf(stderr, "  [%s] \t\tFAIL\n", tests[i].name);
 				fflush(stderr);
 				failcount++;
 				passed[i] = false;
 
 			} else {
-				printf("  [%s] \t\t\tPASS\n", tests[i].name);
+				printf("  [%s] \t\tPASS\n", tests[i].name);
 				fflush(stdout);
 				passed[i] = true;
 			}
 
+			for (int j = 0; j < 80; j++) {
+				printf("-");
+			}
+
+			printf("\n");
+
 			testcount++;
 		}
 	}
 
 	/* Print summary */
 	printf("\n");
-	int j;
 
-	for (j = 0; j < 40; j++) {
+	for (int j = 0; j < 80; j++) {
 		printf("-");
 	}
 
@@ -325,9 +342,7 @@ int test_jig(int argc, char *argv[])
 	/* Print failed tests */
 	if (failcount > 0) { printf(" Failed tests:\n\n"); }
 
-	unsigned int k;
-
-	for (k = 0; k < i; k++) {
+	for (int k = 0; k < i; k++) {
 		if (!passed[i] && !(tests[k].options & OPT_NOJIGTEST)) {
 			printf(" [%s] to obtain details, please re-run with\n\t nsh> tests %s\n\n", tests[k].name, tests[k].name);
 		}

From d85e7732b4facfd4b1db7c14cc42dd007a6bee8f Mon Sep 17 00:00:00 2001
From: Daniel Agar <daniel@agar.ca>
Date: Sun, 24 Apr 2016 18:18:32 -0400
Subject: [PATCH 0076/1230] fix param_test

---
 src/modules/systemlib/param/param.c       |   5 +-
 src/modules/systemlib/param/param_shmem.c |   4 +-
 unittests/CMakeLists.txt                  | 254 ++++++++++------------
 unittests/param_test.cpp                  |  10 +-
 4 files changed, 130 insertions(+), 143 deletions(-)

diff --git a/src/modules/systemlib/param/param.c b/src/modules/systemlib/param/param.c
index fdfcd729e7..16e5b9532d 100644
--- a/src/modules/systemlib/param/param.c
+++ b/src/modules/systemlib/param/param.c
@@ -90,12 +90,11 @@
 extern struct param_info_s	param_array[];
 extern struct param_info_s	*param_info_base;
 extern struct param_info_s	*param_info_limit;
+#define param_info_count	(param_info_limit - param_info_base)
 #else
-// FIXME - start and end are reversed
 static const struct param_info_s *param_info_base = (const struct param_info_s *) &px4_parameters;
-#endif
-
 #define	param_info_count		px4_parameters.param_count
+#endif /* _UNIT_TEST */
 
 /**
  * Storage for modified parameters.
diff --git a/src/modules/systemlib/param/param_shmem.c b/src/modules/systemlib/param/param_shmem.c
index 333fa3e885..cefb67a888 100644
--- a/src/modules/systemlib/param/param_shmem.c
+++ b/src/modules/systemlib/param/param_shmem.c
@@ -86,11 +86,11 @@
 extern struct param_info_s	param_array[];
 extern struct param_info_s	*param_info_base;
 extern struct param_info_s	*param_info_limit;
+#define param_info_count	(param_info_limit - param_info_base)
 #else
 static struct param_info_s *param_info_base = (struct param_info_s *) &px4_parameters;
-#endif
-
 #define	param_info_count		px4_parameters.param_count
+#endif /* _UNIT_TEST */
 
 /**
  * Storage for modified parameters.
diff --git a/unittests/CMakeLists.txt b/unittests/CMakeLists.txt
index 43ff39a0d4..c2e1d87284 100644
--- a/unittests/CMakeLists.txt
+++ b/unittests/CMakeLists.txt
@@ -1,195 +1,179 @@
 cmake_minimum_required(VERSION 2.8)
 
-include( CMakeForceCompiler )
-#set( CMAKE_SYSTEM_NAME px4_posix_clang )
-CMAKE_FORCE_C_COMPILER( clang Clang )
-CMAKE_FORCE_CXX_COMPILER( clang++ Clang )
-#set( CMAKE_C_COMPILER /opt/clang-3.4.2/bin/clang )
-#set( CMAKE_CXX_COMPILER /opt/clang-3.4.2/bin/clang++ )
-#set( CMAKE_FIND_ROOT_PATH /opt/clang-3.4.2/ )
-#set( CMAKE_FIND_ROOT_PATH_MODE_PROGRAM_NEVER )
-#set( CMAKE_FIND_ROOT_PATH_MODE_LIBARARY_ONLY )
-#set( CMAKE_FIND_ROOT_PATH_MODE_INCLUDE_ONLY )
+include(CMakeForceCompiler)
+CMAKE_FORCE_C_COMPILER(clang Clang)
+CMAKE_FORCE_CXX_COMPILER(clang++ Clang)
 
 if("${CMAKE_C_COMPILER_ID}" STREQUAL "Clang")
-  add_compile_options(-Qunused-arguments)
+	add_compile_options(-Qunused-arguments )
 endif()
 if("${CMAKE_CXX_COMPILER_ID}" STREQUAL "Clang")
-  add_compile_options(-Qunused-arguments)
+	add_compile_options(-Qunused-arguments)
 endif()
 
 project(unittests)
 enable_testing()
 
-
 include(CheckCXXCompilerFlag)
 CHECK_CXX_COMPILER_FLAG("-std=c++11" COMPILER_SUPPORTS_CXX11)
 CHECK_CXX_COMPILER_FLAG("-std=c++0x" COMPILER_SUPPORTS_CXX0X)
+
 if(COMPILER_SUPPORTS_CXX11)
-  set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11 -fno-exceptions -fno-rtti -fno-threadsafe-statics -D__CUSTOM_FILE_IO__ -D__PX4_UNIT_TESTS -g -Wall -Werror")
+	set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11 -fno-exceptions -fno-rtti -fno-threadsafe-statics -D__CUSTOM_FILE_IO__ -D__PX4_UNIT_TESTS -g -Wall -Werror")
 elseif(COMPILER_SUPPORTS_CXX0X)
-  set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++0x -fno-exceptions -fno-rtti -fno-threadsafe-statics -D__CUSTOM_FILE_IO__ -D__PX4_UNIT_TESTS -g -Wall -Werror")
+	set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++0x -fno-exceptions -fno-rtti -fno-threadsafe-statics -D__CUSTOM_FILE_IO__ -D__PX4_UNIT_TESTS -g -Wall -Werror")
 else()
-  message(STATUS "The compiler ${CMAKE_CXX_COMPILER} has no C++11 support. Please use a different C++ compiler.")
+	message(STATUS "The compiler ${CMAKE_CXX_COMPILER} has no C++11 support. Please use a different C++ compiler.")
 endif()
 set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -std=gnu99 -g")
 
-#set(CMAKE_INCLUDE_SYSTEM_FLAG_C "-isystem" )
-#set(CMAKE_INCLUDE_SYSTEM_FLAG_CXX "-isystem" )
+set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -O0 -g3 -fsanitize=address -fno-omit-frame-pointer")
 
-set(GTEST_DIR googletest)
+set(GTEST_DIR ${CMAKE_SOURCE_DIR}/googletest)
 add_subdirectory(${GTEST_DIR})
 include_directories(${GTEST_DIR}/include)
 
-set(PX_SRC ${CMAKE_SOURCE_DIR}/../src)
-include_directories(${CMAKE_SOURCE_DIR})
-include_directories(${PX_SRC})
-include_directories(${PX_SRC}/modules)
-include_directories(${PX_SRC}/modules/uORB)
-include_directories(${PX_SRC}/lib)
-include_directories(${PX_SRC}/drivers)
-include_directories(${PX_SRC}/lib/DriverFramework/framework/include)
-include_directories(${PX_SRC}/../build_posix_sitl_default/src/modules)
-include_directories(${PX_SRC}/../build_posix_sitl_default/src/modules/uORB)
-include_directories(${PX_SRC}/../build_posix_sitl_default/src)
-include_directories(${PX_SRC}/../build_posix_sitl_default/src/modules/param)
-include_directories(${PX_SRC}/platforms)
-include_directories(${PX_SRC}/platforms/posix/include)
-include_directories(${PX_SRC}/platforms/posix/px4_layer)
-include_directories(${PX_SRC}/drivers/device)
+set(PX4_SRC ${CMAKE_SOURCE_DIR}/../src)
+set(PX4_SITL_BUILD ${PX4_SRC}/../build_posix_sitl_test)
 
+include_directories(${CMAKE_SOURCE_DIR})
+include_directories(${PX4_SITL_BUILD}/src)
+include_directories(${PX4_SITL_BUILD}/src/modules)
+include_directories(${PX4_SITL_BUILD}/src/modules/param)
+include_directories(${PX4_SITL_BUILD}/src/modules/uORB)
+include_directories(${PX4_SRC})
+include_directories(${PX4_SRC}/drivers)
+include_directories(${PX4_SRC}/drivers/device)
+include_directories(${PX4_SRC}/lib)
+include_directories(${PX4_SRC}/lib/DriverFramework/framework/include)
+include_directories(${PX4_SRC}/modules)
+include_directories(${PX4_SRC}/modules/uORB)
+include_directories(${PX4_SRC}/platforms)
+include_directories(${PX4_SRC}/platforms/posix/include)
+include_directories(${PX4_SRC}/platforms/posix/px4_layer)
 
 add_definitions(-D__EXPORT=)
-add_definitions(-D__PX4_TESTS)
-add_definitions(-Dnoreturn_function=)
-add_definitions(-Dmain_t=int)
-add_definitions(-DERROR=-1)
-add_definitions(-DOK=0)
-add_definitions(-D_UNIT_TEST=)
 add_definitions(-D__PX4_POSIX)
+add_definitions(-D__PX4_TESTS)
+add_definitions(-D_UNIT_TEST=)
+add_definitions(-DERROR=-1)
+add_definitions(-Dmain_t=int)
+add_definitions(-Dnoreturn_function=)
+add_definitions(-DOK=0)
 
 # check
-add_custom_target(unittests COMMAND ${CMAKE_CTEST_COMMAND} --output-on-failure)
+add_custom_target(check
+		COMMAND ${CMAKE_CTEST_COMMAND} -j2 --output-on-failure
+		WORKING_DIR ${CMAKE_BINARY_DIR}
+		USES_TERMINAL)
 
 function(add_gtest)
-  foreach(test_name ${ARGN})
-    if(${CMAKE_SYSTEM_NAME} MATCHES "Darwin")
-      target_link_libraries(${test_name} gtest_main pthread )
-      add_definitions(-D__PX4_DARWIN)
-    else()
-      target_link_libraries(${test_name} gtest_main pthread rt )
-      add_definitions(-D__PX4_LINUX)
-    endif()
-    add_test(NAME ${test_name} COMMAND ${test_name} WORKING_DIRECTORY ${CMAKE_SOURCE_DIR})
-    add_dependencies(unittests ${test_name})
-  endforeach()
+	foreach(test_name ${ARGN})
+		if(${CMAKE_SYSTEM_NAME} MATCHES "Darwin")
+			target_link_libraries(${test_name} gtest_main pthread px4_platform)
+			add_definitions(-D__PX4_DARWIN)
+		else()
+			target_link_libraries(${test_name} gtest_main pthread rt px4_platform)
+			add_definitions(-D__PX4_LINUX)
+		endif()
+		add_test(NAME ${test_name} COMMAND ${test_name} WORKING_DIRECTORY ${CMAKE_SOURCE_DIR})
+		add_dependencies(check ${test_name})
+	endforeach()
 endfunction()
 
-add_library( px4_platform 
-#                          ${PX_SRC}/platforms/common/px4_getopt.c
-                           ${PX_SRC}/platforms/posix/px4_layer/px4_log.c
-                           ${PX_SRC}/platforms/posix/px4_layer/px4_posix_impl.cpp
-                           ${PX_SRC}/platforms/posix/px4_layer/px4_posix_tasks.cpp
-                           ${PX_SRC}/platforms/posix/work_queue/work_lock.c
-                           ${PX_SRC}/platforms/posix/work_queue/hrt_queue.c
-                           ${PX_SRC}/platforms/posix/work_queue/work_queue.c
-                           ${PX_SRC}/platforms/posix/work_queue/queue.c
-                           ${PX_SRC}/platforms/posix/work_queue/work_cancel.c
-                           ${PX_SRC}/platforms/posix/work_queue/hrt_work_cancel.c
-                           ${PX_SRC}/platforms/posix/work_queue/hrt_thread.c
-                           ${PX_SRC}/platforms/posix/work_queue/work_thread.c
-                           ${PX_SRC}/platforms/posix/work_queue/dq_rem.c
-                           ${PX_SRC}/platforms/posix/work_queue/sq_addlast.c
-                           ${PX_SRC}/platforms/posix/work_queue/sq_addafter.c
-                           ${PX_SRC}/platforms/posix/work_queue/dq_remfirst.c
-                           ${PX_SRC}/platforms/posix/work_queue/sq_remfirst.c
-                           ${PX_SRC}/platforms/posix/work_queue/dq_addlast.c
-                           ${PX_SRC}/platforms/posix/px4_layer/lib_crc32.c
-                           ${PX_SRC}/platforms/posix/px4_layer/drv_hrt.c
-                           ${PX_SRC}/platforms/posix/px4_layer/px4_sem.cpp
-                           ${PX_SRC}/drivers/device/device_posix.cpp 
-                           ${PX_SRC}/drivers/device/vdev.cpp 
-                           ${PX_SRC}/drivers/device/vfile.cpp
-                           ${PX_SRC}/drivers/device/vdev_posix.cpp
-                           ${PX_SRC}/drivers/device/i2c_posix.cpp 
-                           ${PX_SRC}/drivers/device/sim.cpp 
-                           ${PX_SRC}/drivers/device/ringbuffer.cpp
-                           ${PX_SRC}/../build_posix_sitl_default/src/modules/param/px4_parameters.c                         
- )
-                           
-#target_include_directories( px4_platform PUBLIC ${PX_SRC}/platforms )                           
-                           
-                           
+add_library(px4_platform 
+			${PX4_SITL_BUILD}/src/modules/param/px4_parameters.c                         
+			${PX4_SRC}/drivers/device/device_posix.cpp 
+			${PX4_SRC}/drivers/device/i2c_posix.cpp 
+			${PX4_SRC}/drivers/device/ringbuffer.cpp
+			${PX4_SRC}/drivers/device/sim.cpp 
+			${PX4_SRC}/drivers/device/vdev.cpp 
+			${PX4_SRC}/drivers/device/vdev_posix.cpp
+			${PX4_SRC}/drivers/device/vfile.cpp
+			${PX4_SRC}/platforms/posix/px4_layer/drv_hrt.c
+			${PX4_SRC}/platforms/posix/px4_layer/lib_crc32.c
+			${PX4_SRC}/platforms/posix/px4_layer/px4_log.c
+			${PX4_SRC}/platforms/posix/px4_layer/px4_log.c
+			${PX4_SRC}/platforms/posix/px4_layer/px4_posix_impl.cpp
+			${PX4_SRC}/platforms/posix/px4_layer/px4_posix_tasks.cpp
+			${PX4_SRC}/platforms/posix/px4_layer/px4_sem.cpp
+			${PX4_SRC}/platforms/posix/px4_layer/shmem_posix.c
+			${PX4_SRC}/platforms/posix/work_queue/dq_addlast.c
+			${PX4_SRC}/platforms/posix/work_queue/dq_rem.c
+			${PX4_SRC}/platforms/posix/work_queue/dq_remfirst.c
+			${PX4_SRC}/platforms/posix/work_queue/hrt_queue.c
+			${PX4_SRC}/platforms/posix/work_queue/hrt_thread.c
+			${PX4_SRC}/platforms/posix/work_queue/hrt_work_cancel.c
+			${PX4_SRC}/platforms/posix/work_queue/queue.c
+			${PX4_SRC}/platforms/posix/work_queue/sq_addafter.c
+			${PX4_SRC}/platforms/posix/work_queue/sq_addlast.c
+			${PX4_SRC}/platforms/posix/work_queue/sq_remfirst.c
+			${PX4_SRC}/platforms/posix/work_queue/work_cancel.c
+			${PX4_SRC}/platforms/posix/work_queue/work_lock.c
+			${PX4_SRC}/platforms/posix/work_queue/work_queue.c
+			${PX4_SRC}/platforms/posix/work_queue/work_thread.c
+			)                           
+target_include_directories(px4_platform PUBLIC ${PX4_SRC}/platforms)                           
 
-# add each test
-add_executable(autodeclination_test autodeclination_test.cpp ${PX_SRC}/lib/geo_lookup/geo_mag_declination.c)
+
+#######################################################################
+# TESTS
+#######################################################################
+#    add_executable(example_test example_test.cpp)
+#    add_gtest(example_test)
+
+
+# autodeclination_test
+add_executable(autodeclination_test autodeclination_test.cpp ${PX4_SRC}/lib/geo_lookup/geo_mag_declination.c)
 add_gtest(autodeclination_test)
 
 # mixer_test
-add_custom_command(OUTPUT ${PX_SRC}/modules/systemlib/mixer/mixer_multirotor.generated.h
-                   COMMAND ${PX_SRC}/modules/systemlib/mixer/multi_tables.py > ${PX_SRC}/modules/systemlib/mixer/mixer_multirotor.generated.h)
+add_custom_command(OUTPUT ${PX4_SRC}/modules/systemlib/mixer/mixer_multirotor.generated.h
+                   COMMAND ${PX4_SRC}/modules/systemlib/mixer/multi_tables.py > ${PX4_SRC}/modules/systemlib/mixer/mixer_multirotor.generated.h)
 add_executable(mixer_test mixer_test.cpp hrt.cpp
-                          ${PX_SRC}/modules/systemlib/mixer/mixer.cpp
-                          ${PX_SRC}/modules/systemlib/mixer/mixer_group.cpp
-                          ${PX_SRC}/modules/systemlib/mixer/mixer_load.c
-                          ${PX_SRC}/modules/systemlib/mixer/mixer_multirotor.cpp
-                          ${PX_SRC}/modules/systemlib/mixer/mixer_multirotor.generated.h
-                          ${PX_SRC}/modules/systemlib/mixer/mixer_simple.cpp
-                          ${PX_SRC}/modules/systemlib/pwm_limit/pwm_limit.c
-                          ${PX_SRC}/systemcmds/tests/test_mixer.cpp)
-target_link_libraries( mixer_test px4_platform )
-
-                          
+                          ${PX4_SRC}/modules/systemlib/mixer/mixer.cpp
+                          ${PX4_SRC}/modules/systemlib/mixer/mixer_group.cpp
+                          ${PX4_SRC}/modules/systemlib/mixer/mixer_load.c
+                          ${PX4_SRC}/modules/systemlib/mixer/mixer_multirotor.cpp
+                          ${PX4_SRC}/modules/systemlib/mixer/mixer_multirotor.generated.h
+                          ${PX4_SRC}/modules/systemlib/mixer/mixer_simple.cpp
+                          ${PX4_SRC}/modules/systemlib/pwm_limit/pwm_limit.c
+                          ${PX4_SRC}/systemcmds/tests/test_mixer.cpp)
 add_gtest(mixer_test)
 
 # conversion_test
-add_executable(conversion_test conversion_test.cpp ${PX_SRC}/systemcmds/tests/test_conv.cpp)
-target_link_libraries( conversion_test px4_platform )
+add_executable(conversion_test conversion_test.cpp ${PX4_SRC}/systemcmds/tests/test_conv.cpp)
 add_gtest(conversion_test)
 
 # sbus2_test
 add_executable(sbus2_test sbus2_test.cpp hrt.cpp
-                          ${PX_SRC}/lib/rc/sbus.c)
-target_link_libraries( sbus2_test px4_platform )
+						${PX4_SRC}/lib/rc/sbus.c)
 add_gtest(sbus2_test)
 
 # DSM test
 add_executable(dsm_test dsm_test.cpp hrt.cpp
-                          ${PX_SRC}/lib/rc/dsm.c)
-target_link_libraries( dsm_test px4_platform )
+						${PX4_SRC}/lib/rc/dsm.c)
 add_gtest(dsm_test)
 
 # st24_test
-add_executable(rc_input_test st24_test.cpp hrt.cpp ${PX_SRC}/lib/rc/st24.c sumd_test.cpp ${PX_SRC}/lib/rc/sumd.c)
-target_link_libraries(rc_input_test px4_platform)
+add_executable(rc_input_test st24_test.cpp hrt.cpp sumd_test.cpp
+						${PX4_SRC}/lib/rc/st24.c
+						${PX4_SRC}/lib/rc/sumd.c)
 add_gtest(rc_input_test)
 
 # sf0x_test
-add_executable(sf0x_test sf0x_test.cpp ${PX_SRC}/drivers/sf0x/sf0x_parser.cpp)
-target_link_libraries( sf0x_test px4_platform )
+add_executable(sf0x_test sf0x_test.cpp
+						${PX4_SRC}/drivers/sf0x/sf0x_parser.cpp)
 add_gtest(sf0x_test)
 
 # param_test
-#add_executable(param_test param_test.cpp
-#                          hrt.cpp
-#                          uorb_stub.cpp
-#                          ${PX_SRC}/modules/systemlib/param/param.c
-#                          ${PX_SRC}/modules/systemlib/bson/tinybson.c
-#                          )
-#target_link_libraries( param_test px4_platform )                          
-                          
-#add_gtest(param_test)
+add_executable(param_test param_test.cpp hrt.cpp uorb_stub.cpp
+						${PX4_SRC}/modules/systemlib/bson/tinybson.c
+						${PX4_SRC}/modules/systemlib/param/param.c)
+add_gtest(param_test)
 
-# uorb test
-#add_executable(uorb_tests uorb_unittests/uORBCommunicator_gtests.cpp
-#                          uorb_unittests/uORBCommunicatorMock.cpp
-#                          uorb_unittests/uORBCommunicatorMockLoopback.cpp
-#                          ${PX_SRC}/modules/uORB/uORBDevices_posix.cpp
-#                          ${PX_SRC}/modules/uORB/uORBManager_posix.cpp
-#                          ${PX_SRC}/modules/uORB/objects_common.cpp
-#                          ${PX_SRC}/modules/uORB/uORBUtils.cpp
-#                          ${PX_SRC}/modules/uORB/uORB.cpp
-#                          )
-#target_link_libraries( uorb_tests px4_platform )
-                          
-#add_gtest(uorb_tests)
+# param_shmem_test
+#add_executable(param_shmem_test param_test.cpp hrt.cpp uorb_stub.cpp
+#						${PX4_SRC}/modules/systemlib/bson/tinybson.c
+#						${PX4_SRC}/modules/systemlib/param/param_shmem.c)
+#add_gtest(param_shmem_test)
diff --git a/unittests/param_test.cpp b/unittests/param_test.cpp
index bda49ae86d..237eed2d64 100644
--- a/unittests/param_test.cpp
+++ b/unittests/param_test.cpp
@@ -10,6 +10,8 @@ struct param_info_s	param_array[256];
 struct param_info_s	*param_info_base;
 struct param_info_s	*param_info_limit;
 
+#define	param_info_count 4
+
 /*
  * Adds test parameters
  */
@@ -44,8 +46,10 @@ void _add_parameters()
 	param_array[2] = rc_x;
 	param_array[3] = rc2_x;
 	param_info_base = (struct param_info_s *) &param_array[0];
-	param_info_limit = (struct param_info_s *) &param_array[4]; 	// needs to point at the end of the data,
-	// therefore number of params + 1
+	// needs to point at the end of the data,
+	//  therefore number of params + 1
+	param_info_limit = (struct param_info_s *) &param_array[4];
+
 }
 
 void _assert_parameter_int_value(param_t param, int32_t expected)
@@ -149,4 +153,4 @@ TEST(ParamTest, ResetAllExcludesWildcard)
 	_assert_parameter_int_value((param_t)1, 4);
 	_assert_parameter_int_value((param_t)2, 50);
 	_assert_parameter_int_value((param_t)3, 50);
-}
\ No newline at end of file
+}

From d804f5727efa09bc13f7d7b5de944892e4dd8983 Mon Sep 17 00:00:00 2001
From: Daniel Agar <daniel@agar.ca>
Date: Sun, 24 Apr 2016 18:48:56 -0400
Subject: [PATCH 0077/1230] run tests in posix sitl with gazebo

---
 Makefile                                      | 25 ++++++++++++++--
 Tools/sitl_run.sh                             | 30 +++++++++++--------
 cmake/configs/posix_sitl_test.cmake           |  2 +-
 posix-configs/SITL/init/rcS_test_gazebo_iris  | 25 ++++++++++++++++
 posix-configs/SITL/init/rcS_test_jmavsim_iris |  3 +-
 5 files changed, 67 insertions(+), 18 deletions(-)
 create mode 100644 posix-configs/SITL/init/rcS_test_gazebo_iris

diff --git a/Makefile b/Makefile
index 286bf7ad56..db775507ac 100644
--- a/Makefile
+++ b/Makefile
@@ -114,6 +114,12 @@ define cmake-build
 +@(echo "PX4 CONFIG: $@" && cd $(PWD)/build_$@ && $(PX4_MAKE) $(PX4_MAKE_ARGS) $(ARGS))
 endef
 
+define cmake-build-other
++@if [ $(PX4_CMAKE_GENERATOR) = "Ninja" ] && [ -e $(PWD)/build_$@/Makefile ]; then rm -rf $(PWD)/build_$@; fi
++@if [ ! -e $(PWD)/build_$@/CMakeCache.txt ]; then Tools/check_submodules.sh && mkdir -p $(PWD)/build_$@ && cd $(PWD)/build_$@ && cmake $(2) -G$(PX4_CMAKE_GENERATOR) || (cd .. && rm -rf $(PWD)/build_$@); fi
++@(cd $(PWD)/build_$@ && $(PX4_MAKE) $(PX4_MAKE_ARGS) $(ARGS))
+endef
+
 # create empty targets to avoid msgs for targets passed to cmake
 define cmake-targ
 $(1):
@@ -219,8 +225,14 @@ run_sitl_ros: sitl_deprecation
 # Other targets
 # --------------------------------------------------------------------
 
-.PHONY: uavcan_firmware check check_format unittest tests package_firmware clean submodulesclean distclean
-.NOTPARALLEL: uavcan_firmware check check_format unittest tests package_firmware clean submodulesclean distclean
+.PHONY: gazebo_build uavcan_firmware check check_format unittest tests package_firmware clean submodulesclean distclean
+.NOTPARALLEL: gazebo_build uavcan_firmware check check_format unittest tests package_firmware clean submodulesclean distclean
+
+gazebo_build:
+	@mkdir -p build_gazebo
+	@if [ ! -e $(PWD)/build_gazebo/CMakeCache.txt ];then cd build_gazebo && cmake -Wno-dev -G$(PX4_CMAKE_GENERATOR) $(PWD)/Tools/sitl_gazebo; fi
+	@cd build_gazebo && $(PX4_MAKE) $(PX4_MAKE_ARGS)
+	@cd build_gazebo && $(PX4_MAKE) $(PX4_MAKE_ARGS) sdf
 
 uavcan_firmware:
 ifeq ($(VECTORCONTROL),1)
@@ -251,7 +263,14 @@ ifeq ($(VECTORCONTROL),1)
 endif
 
 unittest: posix_sitl_test
-	@(mkdir -p build_unittests && cd build_unittests && cmake -G$(PX4_CMAKE_GENERATOR) ../unittests && $(PX4_MAKE) $(PX4_MAKE_ARGS) && ctest -j2 --output-on-failure)
+	export CC=clang
+	export CXX=clang++
+	export ASAN_OPTIONS=symbolize=1
+	$(call cmake-build-other,unittest, ../unittests)
+	@(cd build_unittest && ctest -j2 --output-on-failure)
+	
+test_onboard_sitl:
+	@HEADLESS=1 make posix_sitl_test gazebo_iris
 
 package_firmware:
 	@zip --junk-paths Firmware.zip `find . -name \*.px4`
diff --git a/Tools/sitl_run.sh b/Tools/sitl_run.sh
index aa5bd9ae3f..fbb93c86fa 100755
--- a/Tools/sitl_run.sh
+++ b/Tools/sitl_run.sh
@@ -58,7 +58,7 @@ cp Tools/posix.gdbinit $build_path/src/firmware/posix/.gdbinit
 
 SIM_PID=0
 
-if [ "$program" == "jmavsim" ] && [ "$no_sim" == "" ]
+if [ "$program" == "jmavsim" ] && [ ! -n "$no_sim" ]
 then
 	cd Tools/jMAVSim
 	ant create_run_jar copy_res
@@ -66,31 +66,33 @@ then
 	java -Djava.ext.dirs= -jar jmavsim_run.jar -udp 127.0.0.1:14560 &
 	SIM_PID=`echo $!`
 	cd ../..
-elif [ "$program" == "gazebo" ] && [ "$no_sim" == "" ]
+elif [ "$program" == "gazebo" ] && [ ! -n "$no_sim" ]
 then
 	if [ -x "$(command -v gazebo)" ]
 	then
 		# Set the plugin path so Gazebo finds our model and sim
-		export GAZEBO_PLUGIN_PATH=$curr_dir/Tools/sitl_gazebo/Build:${GAZEBO_PLUGIN_PATH}
+		export GAZEBO_PLUGIN_PATH=$curr_dir/build_gazebo:${GAZEBO_PLUGIN_PATH}
 		# Set the model path so Gazebo finds the airframes
 		export GAZEBO_MODEL_PATH=${GAZEBO_MODEL_PATH}:$curr_dir/Tools/sitl_gazebo/models
 		# The next line would disable online model lookup, can be commented in, in case of unstable behaviour.
 		# export GAZEBO_MODEL_DATABASE_URI=""
 		export SITL_GAZEBO_PATH=$curr_dir/Tools/sitl_gazebo
-		mkdir -p Tools/sitl_gazebo/Build
-		cd Tools/sitl_gazebo/Build
-		cmake -Wno-dev ..
-		make -j4
-		make sdf
-		gzserver --verbose ../worlds/${model}.world &
+		make --no-print-directory gazebo_build
+
+		gzserver --verbose $curr_dir/Tools/sitl_gazebo/worlds/${model}.world &
 		SIM_PID=`echo $!`
-		gzclient --verbose &
-		GUI_PID=`echo $!`
+
+		if [[ -n "$HEADLESS" ]]; then
+			echo "not running gazebo gui"
+		else
+			gzclient --verbose &
+			GUI_PID=`echo $!`
+		fi
 	else
 		echo "You need to have gazebo simulator installed!"
 		exit 1
 	fi
-elif [ "$program" == "replay" ] && [ "$no_sim" == "" ]
+elif [ "$program" == "replay" ] && [ ! -n "$no_sim" ]
 then
 	echo "Replaying logfile: $logfile"
 	# This is not a simulator, but a log file to replay
@@ -135,5 +137,7 @@ then
 elif [ "$program" == "gazebo" ]
 then
 	kill -9 $SIM_PID
-	kill -9 $GUI_PID
+	if [[ ! -n "$HEADLESS" ]]; then
+		kill -9 $GUI_PID
+	fi
 fi
diff --git a/cmake/configs/posix_sitl_test.cmake b/cmake/configs/posix_sitl_test.cmake
index e9fd6734cc..3f233985d5 100644
--- a/cmake/configs/posix_sitl_test.cmake
+++ b/cmake/configs/posix_sitl_test.cmake
@@ -87,7 +87,7 @@ set(config_extra_builtin_cmds
 	)
 
 set(config_sitl_rcS
-	posix-configs/SITL/init/rcS
+	posix-configs/SITL/init/rcS_test
 	CACHE FILEPATH "init script for sitl"
 	)
 
diff --git a/posix-configs/SITL/init/rcS_test_gazebo_iris b/posix-configs/SITL/init/rcS_test_gazebo_iris
new file mode 100644
index 0000000000..8aeacf1eff
--- /dev/null
+++ b/posix-configs/SITL/init/rcS_test_gazebo_iris
@@ -0,0 +1,25 @@
+uorb start
+param load
+
+dataman start
+
+simulator start -s
+rgbledsim start
+tone_alarm start
+gyrosim start
+accelsim start
+barosim start
+adcsim start
+gpssim start
+pwm_out_sim mode_pwm
+sleep 1
+sensors start
+
+commander_tests
+controllib_test
+uorb_tests
+tests all
+
+ver all
+
+shutdown
diff --git a/posix-configs/SITL/init/rcS_test_jmavsim_iris b/posix-configs/SITL/init/rcS_test_jmavsim_iris
index b419960f24..8aeacf1eff 100644
--- a/posix-configs/SITL/init/rcS_test_jmavsim_iris
+++ b/posix-configs/SITL/init/rcS_test_jmavsim_iris
@@ -14,7 +14,6 @@ gpssim start
 pwm_out_sim mode_pwm
 sleep 1
 sensors start
-commander start
 
 commander_tests
 controllib_test
@@ -22,3 +21,5 @@ uorb_tests
 tests all
 
 ver all
+
+shutdown

From 701d177e236a07ba64487271dc8513e4ac210fd2 Mon Sep 17 00:00:00 2001
From: Daniel Agar <daniel@agar.ca>
Date: Sun, 24 Apr 2016 18:58:16 -0400
Subject: [PATCH 0078/1230] px4fmu-v2_test don't build frsky_telemetry, gimbal,
 and snapdragon_rc_pwm

---
 cmake/configs/nuttx_px4fmu-v2_test.cmake | 6 +++---
 1 file changed, 3 insertions(+), 3 deletions(-)

diff --git a/cmake/configs/nuttx_px4fmu-v2_test.cmake b/cmake/configs/nuttx_px4fmu-v2_test.cmake
index 95e6c07e9b..cdac7c28ad 100644
--- a/cmake/configs/nuttx_px4fmu-v2_test.cmake
+++ b/cmake/configs/nuttx_px4fmu-v2_test.cmake
@@ -37,16 +37,16 @@ set(config_module_list
 	drivers/airspeed
 	drivers/ets_airspeed
 	drivers/meas_airspeed
-	drivers/frsky_telemetry
+	#drivers/frsky_telemetry
 	modules/sensors
 	#drivers/mkblctrl
 	drivers/px4flow
 	#drivers/oreoled
-	drivers/gimbal
+	#drivers/gimbal
 	drivers/pwm_input
 	drivers/camera_trigger
 	drivers/bst
-	drivers/snapdragon_rc_pwm
+	#drivers/snapdragon_rc_pwm
 	drivers/lis3mdl
 
 	#

From 05b7dd0046a11562a6a52f020729462f1c56179e Mon Sep 17 00:00:00 2001
From: Daniel Agar <daniel@agar.ca>
Date: Sun, 24 Apr 2016 19:25:31 -0400
Subject: [PATCH 0079/1230] check_code_style don't create tmp files

---
 Tools/check_code_style.sh | 13 ++++++-------
 1 file changed, 6 insertions(+), 7 deletions(-)

diff --git a/Tools/check_code_style.sh b/Tools/check_code_style.sh
index 40614dd92e..ab012b1c09 100755
--- a/Tools/check_code_style.sh
+++ b/Tools/check_code_style.sh
@@ -6,12 +6,11 @@ DIR=$( cd "$( dirname "${BASH_SOURCE[0]}" )" && pwd )
 
 if [ -f "$file" ];
 then
-  ${DIR}/fix_code_style.sh --quiet < $file > $file.pretty
-  diffsize=$(diff -y --suppress-common-lines $file $file.pretty | wc -l)
-  rm -f $file.pretty
-  if [ $diffsize -ne 0 ]; then
-    echo $file 'bad formatting, please run "./Tools/fix_code_style.sh' $file'"'
-    exit 1
-  fi
+	${DIR}/fix_code_style.sh --dry-run $file | grep --quiet Formatted
+	if [[ $? -eq 0 ]]
+	then
+		echo $file 'bad formatting, please run "./Tools/fix_code_style.sh' $file'"'
+		exit 1
+	fi
 fi
 

From 51298c1eb127457a2c937ac74579cd2fb4af7fc5 Mon Sep 17 00:00:00 2001
From: Daniel Agar <daniel@agar.ca>
Date: Sun, 24 Apr 2016 19:32:32 -0400
Subject: [PATCH 0080/1230] remove unused

---
 Makefile                                      |   6 +-
 .../commander/state_machine_helper.cpp        |   4 +-
 unittests/CMakeLists.txt                      |  14 +-
 unittests/arch/board/board.h                  |   0
 unittests/board_config.h                      |   0
 unittests/debug.h                             |  10 --
 unittests/hrt.cpp                             |  18 ---
 unittests/nuttx/config.h                      |   0
 unittests/param_test.cpp                      |   2 -
 unittests/queue.h                             | 129 ------------------
 10 files changed, 12 insertions(+), 171 deletions(-)
 delete mode 100644 unittests/arch/board/board.h
 delete mode 100644 unittests/board_config.h
 delete mode 100644 unittests/debug.h
 delete mode 100644 unittests/hrt.cpp
 delete mode 100644 unittests/nuttx/config.h
 delete mode 100644 unittests/queue.h

diff --git a/Makefile b/Makefile
index db775507ac..d5bc05985e 100644
--- a/Makefile
+++ b/Makefile
@@ -263,9 +263,9 @@ ifeq ($(VECTORCONTROL),1)
 endif
 
 unittest: posix_sitl_test
-	export CC=clang
-	export CXX=clang++
-	export ASAN_OPTIONS=symbolize=1
+	@export CC=clang
+	@export CXX=clang++
+	@export ASAN_OPTIONS=symbolize=1
 	$(call cmake-build-other,unittest, ../unittests)
 	@(cd build_unittest && ctest -j2 --output-on-failure)
 	
diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp
index 36d109cbdc..b68358685e 100644
--- a/src/modules/commander/state_machine_helper.cpp
+++ b/src/modules/commander/state_machine_helper.cpp
@@ -148,8 +148,8 @@ transition_result_t arming_state_transition(struct vehicle_status_s *status,
 
 		/* re-run the pre-flight check as long as sensors are failing */
 		if (!status_flags->condition_system_sensors_initialized
-				&& ((new_arming_state == vehicle_status_s::ARMING_STATE_ARMED
-						|| new_arming_state == vehicle_status_s::ARMING_STATE_STANDBY))
+				&& (new_arming_state == vehicle_status_s::ARMING_STATE_ARMED
+				|| new_arming_state == vehicle_status_s::ARMING_STATE_STANDBY)
 				&& status->hil_state == vehicle_status_s::HIL_STATE_OFF) {
 
 			if (last_preflight_check == 0 || hrt_absolute_time() - last_preflight_check > 1000 * 1000) {
diff --git a/unittests/CMakeLists.txt b/unittests/CMakeLists.txt
index c2e1d87284..edaaa60423 100644
--- a/unittests/CMakeLists.txt
+++ b/unittests/CMakeLists.txt
@@ -27,7 +27,7 @@ else()
 endif()
 set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -std=gnu99 -g")
 
-set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -O0 -g3 -fsanitize=address -fno-omit-frame-pointer")
+set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -g3 -fsanitize=address -fno-omit-frame-pointer")
 
 set(GTEST_DIR ${CMAKE_SOURCE_DIR}/googletest)
 add_subdirectory(${GTEST_DIR})
@@ -130,7 +130,7 @@ add_gtest(autodeclination_test)
 # mixer_test
 add_custom_command(OUTPUT ${PX4_SRC}/modules/systemlib/mixer/mixer_multirotor.generated.h
                    COMMAND ${PX4_SRC}/modules/systemlib/mixer/multi_tables.py > ${PX4_SRC}/modules/systemlib/mixer/mixer_multirotor.generated.h)
-add_executable(mixer_test mixer_test.cpp hrt.cpp
+add_executable(mixer_test mixer_test.cpp
                           ${PX4_SRC}/modules/systemlib/mixer/mixer.cpp
                           ${PX4_SRC}/modules/systemlib/mixer/mixer_group.cpp
                           ${PX4_SRC}/modules/systemlib/mixer/mixer_load.c
@@ -146,17 +146,17 @@ add_executable(conversion_test conversion_test.cpp ${PX4_SRC}/systemcmds/tests/t
 add_gtest(conversion_test)
 
 # sbus2_test
-add_executable(sbus2_test sbus2_test.cpp hrt.cpp
+add_executable(sbus2_test sbus2_test.cpp
 						${PX4_SRC}/lib/rc/sbus.c)
 add_gtest(sbus2_test)
 
 # DSM test
-add_executable(dsm_test dsm_test.cpp hrt.cpp
+add_executable(dsm_test dsm_test.cpp
 						${PX4_SRC}/lib/rc/dsm.c)
 add_gtest(dsm_test)
 
 # st24_test
-add_executable(rc_input_test st24_test.cpp hrt.cpp sumd_test.cpp
+add_executable(rc_input_test st24_test.cpp sumd_test.cpp
 						${PX4_SRC}/lib/rc/st24.c
 						${PX4_SRC}/lib/rc/sumd.c)
 add_gtest(rc_input_test)
@@ -167,13 +167,13 @@ add_executable(sf0x_test sf0x_test.cpp
 add_gtest(sf0x_test)
 
 # param_test
-add_executable(param_test param_test.cpp hrt.cpp uorb_stub.cpp
+add_executable(param_test param_test.cpp uorb_stub.cpp
 						${PX4_SRC}/modules/systemlib/bson/tinybson.c
 						${PX4_SRC}/modules/systemlib/param/param.c)
 add_gtest(param_test)
 
 # param_shmem_test
-#add_executable(param_shmem_test param_test.cpp hrt.cpp uorb_stub.cpp
+#add_executable(param_shmem_test param_test.cpp uorb_stub.cpp
 #						${PX4_SRC}/modules/systemlib/bson/tinybson.c
 #						${PX4_SRC}/modules/systemlib/param/param_shmem.c)
 #add_gtest(param_shmem_test)
diff --git a/unittests/arch/board/board.h b/unittests/arch/board/board.h
deleted file mode 100644
index e69de29bb2..0000000000
diff --git a/unittests/board_config.h b/unittests/board_config.h
deleted file mode 100644
index e69de29bb2..0000000000
diff --git a/unittests/debug.h b/unittests/debug.h
deleted file mode 100644
index 4071703da2..0000000000
--- a/unittests/debug.h
+++ /dev/null
@@ -1,10 +0,0 @@
-
-#pragma once
-
-#include <systemlib/err.h>
-#define lowsyslog warnx
-#define dbg warnx
-
-#if !defined(ASSERT)
-# define ASSERT(x) assert((x))
-#endif
diff --git a/unittests/hrt.cpp b/unittests/hrt.cpp
deleted file mode 100644
index d7b4670db1..0000000000
--- a/unittests/hrt.cpp
+++ /dev/null
@@ -1,18 +0,0 @@
-#include <sys/time.h>
-#include <inttypes.h>
-#include <drivers/drv_hrt.h>
-#include <stdio.h>
-
-hrt_abstime hrt_absolute_time()
-{
-	struct timeval te;
-	gettimeofday(&te, NULL); // get current time
-	hrt_abstime us = static_cast<uint64_t>(te.tv_sec) * 1e6 + te.tv_usec; // caculate us
-	return us;
-}
-
-hrt_abstime hrt_elapsed_time(const volatile hrt_abstime *then)
-{
-	// not thread safe
-	return hrt_absolute_time() - *then;
-}
diff --git a/unittests/nuttx/config.h b/unittests/nuttx/config.h
deleted file mode 100644
index e69de29bb2..0000000000
diff --git a/unittests/param_test.cpp b/unittests/param_test.cpp
index 237eed2d64..23e72c1a76 100644
--- a/unittests/param_test.cpp
+++ b/unittests/param_test.cpp
@@ -10,8 +10,6 @@ struct param_info_s	param_array[256];
 struct param_info_s	*param_info_base;
 struct param_info_s	*param_info_limit;
 
-#define	param_info_count 4
-
 /*
  * Adds test parameters
  */
diff --git a/unittests/queue.h b/unittests/queue.h
deleted file mode 100644
index e056bc263e..0000000000
--- a/unittests/queue.h
+++ /dev/null
@@ -1,129 +0,0 @@
-/************************************************************************
- * include/queue.h
- *
- *   Copyright (C) 2007-2009 Gregory Nutt. All rights reserved.
- *   Author: Gregory Nutt <gnutt@nuttx.org>
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions
- * are met:
- *
- * 1. Redistributions of source code must retain the above copyright
- *    notice, this list of conditions and the following disclaimer.
- * 2. Redistributions in binary form must reproduce the above copyright
- *    notice, this list of conditions and the following disclaimer in
- *    the documentation and/or other materials provided with the
- *    distribution.
- * 3. Neither the name NuttX nor the names of its contributors may be
- *    used to endorse or promote products derived from this software
- *    without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
- * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
- * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- *
- ************************************************************************/
-
-#ifndef __INCLUDE_QUEUE_H
-#define __INCLUDE_QUEUE_H
-
-#ifndef FAR
-#define FAR
-#endif
-
-/************************************************************************
- * Included Files
- ************************************************************************/
-
-#include <sys/types.h>
-
-/************************************************************************
- * Pre-processor Definitions
- ************************************************************************/
-
-#define sq_init(q) do { (q)->head = NULL; (q)->tail = NULL; } while (0)
-#define dq_init(q) do { (q)->head = NULL; (q)->tail = NULL; } while (0)
-
-#define sq_next(p) ((p)->flink)
-#define dq_next(p) ((p)->flink)
-#define dq_prev(p) ((p)->blink)
-
-#define sq_empty(q) ((q)->head == NULL)
-#define dq_empty(q) ((q)->head == NULL)
-
-#define sq_peek(q)  ((q)->head)
-#define dq_peek(q)  ((q)->head)
-
-/************************************************************************
- * Global Type Declarations
- ************************************************************************/
-
-struct sq_entry_s {
-	FAR struct sq_entry_s *flink;
-};
-typedef struct sq_entry_s sq_entry_t;
-
-struct dq_entry_s {
-	FAR struct dq_entry_s *flink;
-	FAR struct dq_entry_s *blink;
-};
-typedef struct dq_entry_s dq_entry_t;
-
-struct sq_queue_s {
-	FAR sq_entry_t *head;
-	FAR sq_entry_t *tail;
-};
-typedef struct sq_queue_s  sq_queue_t;
-
-struct dq_queue_s {
-	FAR dq_entry_t *head;
-	FAR dq_entry_t *tail;
-};
-typedef struct dq_queue_s dq_queue_t;
-
-/************************************************************************
- * Global Function Prototypes
- ************************************************************************/
-
-#ifdef __cplusplus
-#define EXTERN extern "C"
-extern "C" {
-#else
-#define EXTERN extern
-#endif
-
-EXTERN void  sq_addfirst(FAR sq_entry_t *node, sq_queue_t *queue);
-EXTERN void  dq_addfirst(FAR dq_entry_t *node, dq_queue_t *queue);
-EXTERN void  sq_addlast(FAR sq_entry_t *node, sq_queue_t *queue);
-EXTERN void  dq_addlast(FAR dq_entry_t *node, dq_queue_t *queue);
-EXTERN void  sq_addafter(FAR sq_entry_t *prev, FAR sq_entry_t *node,
-			 sq_queue_t *queue);
-EXTERN void  dq_addafter(FAR dq_entry_t *prev, FAR dq_entry_t *node,
-			 dq_queue_t *queue);
-EXTERN void  dq_addbefore(FAR dq_entry_t *next, FAR dq_entry_t *node,
-			  dq_queue_t *queue);
-
-EXTERN FAR sq_entry_t *sq_remafter(FAR sq_entry_t *node, sq_queue_t *queue);
-EXTERN void            sq_rem(FAR sq_entry_t *node, sq_queue_t *queue);
-EXTERN void            dq_rem(FAR dq_entry_t *node, dq_queue_t *queue);
-EXTERN FAR sq_entry_t *sq_remlast(sq_queue_t *queue);
-EXTERN FAR dq_entry_t *dq_remlast(dq_queue_t *queue);
-EXTERN FAR sq_entry_t *sq_remfirst(sq_queue_t *queue);
-EXTERN FAR dq_entry_t *dq_remfirst(dq_queue_t *queue);
-
-#undef EXTERN
-#ifdef __cplusplus
-}
-#endif
-
-#endif /* __INCLUDE_QUEUE_H_ */
-

From 97fd951d4a4a2d03585bd608d93b79b6e069badf Mon Sep 17 00:00:00 2001
From: Daniel Agar <daniel@agar.ca>
Date: Thu, 5 May 2016 16:00:28 -0400
Subject: [PATCH 0081/1230] cleanup after rebase

---
 Makefile                                 |  5 +----
 cmake/configs/nuttx_px4fmu-v2_test.cmake | 12 ++++++------
 cmake/configs/posix_sitl_test.cmake      |  1 +
 3 files changed, 8 insertions(+), 10 deletions(-)

diff --git a/Makefile b/Makefile
index d5bc05985e..df7ebeafe5 100644
--- a/Makefile
+++ b/Makefile
@@ -164,9 +164,6 @@ posix_sitl_default:
 posix_sitl_test:
 	$(call cmake-build,$@)
 
-posix_sitl_lpe:
-	$(call cmake-build,$@)
-
 posix_sitl_ekf2:
 	$(call cmake-build,$@)
 
@@ -240,7 +237,7 @@ ifeq ($(VECTORCONTROL),1)
 	@(rm -rf vectorcontrol && git clone --quiet --depth 1 https://github.com/thiemar/vectorcontrol.git && cd vectorcontrol && BOARD=s2740vc_1_0 make --silent --no-print-directory && BOARD=px4esc_1_6 make --silent --no-print-directory && ../Tools/uavcan_copy.sh)
 endif
 
-check: check_px4fmu-v1_default check_px4fmu-v2_default check_px4fmu-v2_test check_px4fmu-v2_ekf2 check_px4fmu-v2_lpe check_px4fmu-v4_default_and_uavcan check_mindpx-v2_default check_px4-stm32f4discovery_default check_posix_sitl_default check_posix_sitl_test check_unittest check_format
+check: check_px4fmu-v1_default check_px4fmu-v2_default check_px4fmu-v2_test check_px4fmu-v2_ekf2 check_px4fmu-v4_default_and_uavcan check_mindpx-v2_default check_px4-stm32f4discovery_default check_posix_sitl_default check_posix_sitl_test check_unittest check_format
 
 check_format:
 	$(call colorecho,"Checking formatting with astyle")
diff --git a/cmake/configs/nuttx_px4fmu-v2_test.cmake b/cmake/configs/nuttx_px4fmu-v2_test.cmake
index cdac7c28ad..f824247c3a 100644
--- a/cmake/configs/nuttx_px4fmu-v2_test.cmake
+++ b/cmake/configs/nuttx_px4fmu-v2_test.cmake
@@ -18,7 +18,7 @@ set(config_module_list
 	drivers/boards/px4fmu-v2
 	drivers/rgbled
 	drivers/mpu6000
-	#drivers/mpu9250
+	drivers/mpu9250
 	drivers/lsm303d
 	drivers/l3gd20
 	drivers/hmc5883
@@ -37,16 +37,16 @@ set(config_module_list
 	drivers/airspeed
 	drivers/ets_airspeed
 	drivers/meas_airspeed
-	#drivers/frsky_telemetry
+	drivers/frsky_telemetry
 	modules/sensors
 	#drivers/mkblctrl
 	drivers/px4flow
 	#drivers/oreoled
-	#drivers/gimbal
+	drivers/gimbal
 	drivers/pwm_input
 	drivers/camera_trigger
 	drivers/bst
-	#drivers/snapdragon_rc_pwm
+	drivers/snapdragon_rc_pwm
 	drivers/lis3mdl
 
 	#
@@ -91,11 +91,10 @@ set(config_module_list
 	#
 	# Estimation modules (EKF/ SO3 / other filters)
 	#
-	# Too high RAM usage due to static allocations
-	# modules/attitude_estimator_ekf
 	modules/attitude_estimator_q
 	modules/ekf_att_pos_estimator
 	modules/position_estimator_inav
+	modules/local_position_estimator
 
 	#
 	# Vehicle Control
@@ -136,6 +135,7 @@ set(config_module_list
 	lib/terrain_estimation
 	lib/runway_takeoff
 	lib/tailsitter_recovery
+	lib/DriverFramework/framework
 	platforms/nuttx
 
 	# had to add for cmake, not sure why wasn't in original config
diff --git a/cmake/configs/posix_sitl_test.cmake b/cmake/configs/posix_sitl_test.cmake
index 3f233985d5..12eb54dfdb 100644
--- a/cmake/configs/posix_sitl_test.cmake
+++ b/cmake/configs/posix_sitl_test.cmake
@@ -56,6 +56,7 @@ set(config_module_list
 
 	lib/controllib
 	lib/conversion
+	lib/DriverFramework/framework
 	lib/ecl
 	lib/external_lgpl
 	lib/geo

From 27f6b60169c3f331538675a074c87f63d32047cb Mon Sep 17 00:00:00 2001
From: Daniel Agar <daniel@agar.ca>
Date: Tue, 10 May 2016 17:14:00 -0400
Subject: [PATCH 0082/1230] fix pixhawk errata URL

---
 src/systemcmds/ver/ver.c | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/src/systemcmds/ver/ver.c b/src/systemcmds/ver/ver.c
index 82ee35494b..d1046ad76d 100644
--- a/src/systemcmds/ver/ver.c
+++ b/src/systemcmds/ver/ver.c
@@ -190,7 +190,7 @@ int ver_main(int argc, char *argv[])
 						printf("\nWARNING   WARNING   WARNING!\n"
 						       "Revision %c has a silicon errata\n"
 						       "This device can only utilize a maximum of 1MB flash safely!\n"
-						       "http://px4.io/help/errata\n\n", rev);
+						       "https://pixhawk.org/help/errata\n\n", rev);
 					}
 				}
 

From 27fc739f7d5e97913d579af342b9be9f0997a75c Mon Sep 17 00:00:00 2001
From: Daniel Agar <daniel@agar.ca>
Date: Thu, 12 May 2016 11:34:32 -0400
Subject: [PATCH 0083/1230] load_mon use STACK_MAIN

---
 src/modules/load_mon/CMakeLists.txt | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/src/modules/load_mon/CMakeLists.txt b/src/modules/load_mon/CMakeLists.txt
index 2cd5952ef8..e76f6a46b7 100644
--- a/src/modules/load_mon/CMakeLists.txt
+++ b/src/modules/load_mon/CMakeLists.txt
@@ -33,7 +33,7 @@
 px4_add_module(
 	MODULE modules__load_mon
 	MAIN load_mon
-	STACK 1200
+	STACK_MAIN 1200
 	SRCS
 		load_mon.cpp
 	DEPENDS

From adfe946f72efa4730ac6ab1629b2ca2eac68457f Mon Sep 17 00:00:00 2001
From: Daniel Agar <daniel@agar.ca>
Date: Thu, 12 May 2016 11:38:13 -0400
Subject: [PATCH 0084/1230] sync test with default equivalents

---
 cmake/configs/nuttx_px4fmu-v2_test.cmake | 4 +++-
 cmake/configs/posix_sitl_default.cmake   | 1 -
 cmake/configs/posix_sitl_test.cmake      | 2 ++
 3 files changed, 5 insertions(+), 2 deletions(-)

diff --git a/cmake/configs/nuttx_px4fmu-v2_test.cmake b/cmake/configs/nuttx_px4fmu-v2_test.cmake
index f824247c3a..b1fc89ccc2 100644
--- a/cmake/configs/nuttx_px4fmu-v2_test.cmake
+++ b/cmake/configs/nuttx_px4fmu-v2_test.cmake
@@ -47,7 +47,8 @@ set(config_module_list
 	drivers/camera_trigger
 	drivers/bst
 	drivers/snapdragon_rc_pwm
-	drivers/lis3mdl
+	#drivers/lis3mdl
+	#drivers/bmi160
 
 	#
 	# System commands
@@ -82,6 +83,7 @@ set(config_module_list
 	# General system control
 	#
 	modules/commander
+	modules/load_mon
 	modules/navigator
 	modules/mavlink
 	modules/gpio_led
diff --git a/cmake/configs/posix_sitl_default.cmake b/cmake/configs/posix_sitl_default.cmake
index 69e76dfa81..365397341a 100644
--- a/cmake/configs/posix_sitl_default.cmake
+++ b/cmake/configs/posix_sitl_default.cmake
@@ -44,7 +44,6 @@ set(config_module_list
 	modules/mc_att_control
 	modules/mc_att_control_multiplatform
 	modules/mc_pos_control
-
 	modules/mc_pos_control_multiplatform
 	modules/navigator
 	modules/param
diff --git a/cmake/configs/posix_sitl_test.cmake b/cmake/configs/posix_sitl_test.cmake
index 12eb54dfdb..5a7d9cd9bd 100644
--- a/cmake/configs/posix_sitl_test.cmake
+++ b/cmake/configs/posix_sitl_test.cmake
@@ -26,6 +26,7 @@ set(config_module_list
 	systemcmds/param
 	systemcmds/perf
 	systemcmds/reboot
+	systemcmds/sd_bench
 	systemcmds/topic_listener
 	systemcmds/ver
 
@@ -38,6 +39,7 @@ set(config_module_list
 	modules/fw_att_control
 	modules/fw_pos_control_l1
 	modules/land_detector
+	modules/load_mon
 	modules/mavlink
 	modules/mc_att_control
 	modules/mc_att_control_multiplatform

From 22e3ce7d7a7be71a5785236b5aa13f9394312e67 Mon Sep 17 00:00:00 2001
From: korotkoves <korotkov.es@mipt.ru>
Date: Mon, 4 Apr 2016 23:06:09 +0300
Subject: [PATCH 0085/1230] simulator udp port from command line

---
 src/modules/simulator/simulator.cpp         | 13 +++++++++----
 src/modules/simulator/simulator.h           |  2 +-
 src/modules/simulator/simulator_mavlink.cpp | 10 ++++++----
 3 files changed, 16 insertions(+), 9 deletions(-)

diff --git a/src/modules/simulator/simulator.cpp b/src/modules/simulator/simulator.cpp
index 9eaa725cdd..a8499e6a2b 100644
--- a/src/modules/simulator/simulator.cpp
+++ b/src/modules/simulator/simulator.cpp
@@ -44,6 +44,7 @@
 #include <systemlib/err.h>
 #include <errno.h>
 #include <stdio.h>
+#include <stdlib.h>
 #include <string.h>
 #include <sys/types.h>
 #include <drivers/drv_led.h>
@@ -124,21 +125,25 @@ void Simulator::write_airspeed_data(void *buf)
 int Simulator::start(int argc, char *argv[])
 {
 	int ret = 0;
+	int udp_port = 0;
 	_instance = new Simulator();
 
 	if (_instance) {
 		drv_led_start();
 
+		if (argc == 5 && strcmp(argv[3], "-u") == 0)
+			udp_port = atoi(argv[4]);
+
 		if (argv[2][1] == 's') {
 			_instance->initializeSensorData();
 #ifndef __PX4_QURT
 			// Update sensor data
-			_instance->pollForMAVLinkMessages(false);
+			_instance->pollForMAVLinkMessages(false,udp_port);
 #endif
 
 		} else if (argv[2][1] == 'p') {
 			// Update sensor data
-			_instance->pollForMAVLinkMessages(true);
+			_instance->pollForMAVLinkMessages(true,udp_port);
 
 		} else {
 			_instance->initializeSensorData();
@@ -155,7 +160,7 @@ int Simulator::start(int argc, char *argv[])
 
 static void usage()
 {
-	PX4_WARN("Usage: simulator {start -[spt] |stop}");
+	PX4_WARN("Usage: simulator {start -[spt] [-u udp_port] |stop}");
 	PX4_WARN("Simulate raw sensors:     simulator start -s");
 	PX4_WARN("Publish sensors combined: simulator start -p");
 	PX4_WARN("Dummy unit test data:     simulator start -t");
@@ -171,7 +176,7 @@ extern "C" {
 	{
 		int ret = 0;
 
-		if (argc == 3 && strcmp(argv[1], "start") == 0) {
+		if (argc > 2 && strcmp(argv[1], "start") == 0) {
 			if (strcmp(argv[2], "-s") == 0 ||
 			    strcmp(argv[2], "-p") == 0 ||
 			    strcmp(argv[2], "-t") == 0) {
diff --git a/src/modules/simulator/simulator.h b/src/modules/simulator/simulator.h
index a5afa72c7a..6111af7c88 100644
--- a/src/modules/simulator/simulator.h
+++ b/src/modules/simulator/simulator.h
@@ -314,7 +314,7 @@ private:
 	void poll_topics();
 	void handle_message(mavlink_message_t *msg, bool publish);
 	void send_controls();
-	void pollForMAVLinkMessages(bool publish);
+	void pollForMAVLinkMessages(bool publish, int udp_port);
 
 	void pack_actuator_message(mavlink_hil_controls_t &actuator_msg);
 	void send_mavlink_message(const uint8_t msgid, const void *msg, uint8_t component_ID);
diff --git a/src/modules/simulator/simulator_mavlink.cpp b/src/modules/simulator/simulator_mavlink.cpp
index 9be4da1329..f90ec7025c 100644
--- a/src/modules/simulator/simulator_mavlink.cpp
+++ b/src/modules/simulator/simulator_mavlink.cpp
@@ -462,7 +462,7 @@ void Simulator::initializeSensorData()
 	write_airspeed_data(&airspeed);
 }
 
-void Simulator::pollForMAVLinkMessages(bool publish)
+void Simulator::pollForMAVLinkMessages(bool publish, int udp_port)
 {
 	// set the threads name
 #ifdef __PX4_DARWIN
@@ -473,13 +473,15 @@ void Simulator::pollForMAVLinkMessages(bool publish)
 
 	// udp socket data
 	struct sockaddr_in _myaddr;
-	const int _port = UDP_PORT;
+
+	if (udp_port < 1)
+		udp_port = UDP_PORT;
 
 	// try to setup udp socket for communcation with simulator
 	memset((char *)&_myaddr, 0, sizeof(_myaddr));
 	_myaddr.sin_family = AF_INET;
 	_myaddr.sin_addr.s_addr = htonl(INADDR_ANY);
-	_myaddr.sin_port = htons(_port);
+	_myaddr.sin_port = htons(udp_port);
 
 	if ((_fd = socket(AF_INET, SOCK_DGRAM, 0)) < 0) {
 		PX4_WARN("create socket failed\n");
@@ -532,7 +534,7 @@ void Simulator::pollForMAVLinkMessages(bool publish)
 	// wait for first data from simulator and respond with first controls
 	// this is important for the UDP communication to work
 	int pret = -1;
-	PX4_INFO("Waiting for initial data on UDP. Please start the flight simulator to proceed..");
+	PX4_INFO("Waiting for initial data on UDP port %i. Please start the flight simulator to proceed..",udp_port);
 
 	uint64_t pstart_time = 0;
 

From a8bbbcbff50b038941b8e6d731506b6be322da54 Mon Sep 17 00:00:00 2001
From: korotkoves <korotkov.es@mipt.ru>
Date: Tue, 5 Apr 2016 12:37:57 +0300
Subject: [PATCH 0086/1230] scripts for runnung multiple SITL

---
 Tools/sitl_multiple_run.sh                    | 52 +++++++++++++++
 .../SITL/init/rcS_multiple_gazebo_iris        | 63 +++++++++++++++++++
 2 files changed, 115 insertions(+)
 create mode 100755 Tools/sitl_multiple_run.sh
 create mode 100644 posix-configs/SITL/init/rcS_multiple_gazebo_iris

diff --git a/Tools/sitl_multiple_run.sh b/Tools/sitl_multiple_run.sh
new file mode 100755
index 0000000000..1378931899
--- /dev/null
+++ b/Tools/sitl_multiple_run.sh
@@ -0,0 +1,52 @@
+#!/bin/bash
+
+sitl_num=2
+
+sim_port=15019
+mav_port=15010
+mav_port2=15011
+
+mav_oport=15015
+mav_oport2=15016
+
+port_step=10
+
+src_path=`pwd`
+
+rc_script="posix-configs/SITL/init/rcS_multiple"
+build_path=${src_path}/build_posix_sitl_default
+
+pkill mainapp
+sleep 2
+
+cd $build_path/src/firmware/posix
+
+user=`whoami`
+n=1
+while [ $n -le $sitl_num ]; do
+ if [ ! -d $n ]; then
+  mkdir -p $n
+  cd $n
+
+  mkdir -p rootfs/fs/microsd
+  mkdir -p rootfs/eeprom
+  touch rootfs/eeprom/parameters
+
+  cp ${src_path}/ROMFS/px4fmu_common/mixers/quad_w.main.mix ./
+  cat ${src_path}/${rc_script}_gazebo_iris | sed s/_SIMPORT_/${sim_port}/ | sed s/_MAVPORT_/${mav_port}/g | sed s/_MAVOPORT_/${mav_oport}/ | sed s/_MAVPORT2_/${mav_port2}/ | sed s/_MAVOPORT2_/${mav_oport2}/ > rcS
+  cd ../
+ fi
+
+ cd $n
+
+ sudo -b -u $user ../mainapp -d rcS >out.log 2>err.log
+
+ cd ../
+
+ n=$(($n + 1))
+ sim_port=$(($sim_port + $port_step))
+ mav_port=$(($mav_port + $port_step))
+ mav_port2=$(($mav_port2 + $port_step))
+ mav_oport=$(($mav_oport + $port_step))
+ mav_oport2=$(($mav_oport2 + $port_step))
+done
diff --git a/posix-configs/SITL/init/rcS_multiple_gazebo_iris b/posix-configs/SITL/init/rcS_multiple_gazebo_iris
new file mode 100644
index 0000000000..74a01bceee
--- /dev/null
+++ b/posix-configs/SITL/init/rcS_multiple_gazebo_iris
@@ -0,0 +1,63 @@
+uorb start
+simulator start -s -u _SIMPORT_
+param load
+param set MAV_TYPE 2
+param set MC_PITCHRATE_P 0.3
+param set MC_ROLLRATE_P 0.3
+param set MC_YAW_P 2.0
+param set MC_YAWRATE_P 0.35
+param set SYS_AUTOSTART 4010
+param set SYS_RESTART_TYPE 2
+dataman start
+param set CAL_GYRO0_ID 2293768
+param set CAL_ACC0_ID 1376264
+param set CAL_ACC1_ID 1310728
+param set CAL_MAG0_ID 196616
+param set CAL_GYRO0_XOFF 0.01
+param set CAL_ACC0_XOFF 0.01
+param set CAL_ACC0_YOFF -0.01
+param set CAL_ACC0_ZOFF 0.01
+param set CAL_ACC0_XSCALE 1.01
+param set CAL_ACC0_YSCALE 1.01
+param set CAL_ACC0_ZSCALE 1.01
+param set CAL_ACC1_XOFF 0.01
+param set CAL_MAG0_XOFF 0.01
+param set MPC_XY_P 0.15
+param set MPC_XY_VEL_P 0.05
+param set MPC_XY_VEL_D 0.005
+param set MPC_XY_FF 0.1
+param set SENS_BOARD_ROT 8
+param set COM_RC_IN_MODE 1
+param set NAV_ACC_RAD 2.0
+param set RTL_RETURN_ALT 30.0
+param set RTL_DESCEND_ALT 10.0
+rgbledsim start
+tone_alarm start
+gyrosim start
+accelsim start
+barosim start
+adcsim start
+gpssim start
+pwm_out_sim mode_pwm
+sleep 1
+sensors start
+commander start
+land_detector start multicopter
+navigator start
+attitude_estimator_q start
+position_estimator_inav start
+mc_pos_control start
+mc_att_control start
+mixer load /dev/pwm_output0 quad_w.main.mix
+mavlink start -u _MAVPORT_ -r 2000000 -o _MAVOPORT_
+mavlink start -u _MAVPORT2_ -r 2000000 -m onboard -o _MAVOPORT2_
+mavlink stream -r 80 -s POSITION_TARGET_LOCAL_NED -u _MAVPORT_
+mavlink stream -r 80 -s LOCAL_POSITION_NED -u _MAVPORT_
+mavlink stream -r 80 -s GLOBAL_POSITION_INT -u _MAVPORT_
+mavlink stream -r 80 -s ATTITUDE -u _MAVPORT_
+mavlink stream -r 80 -s ATTITUDE_TARGET -u _MAVPORT_
+mavlink stream -r 20 -s RC_CHANNELS -u _MAVPORT_
+mavlink stream -r 250 -s HIGHRES_IMU -u _MAVPORT_
+mavlink stream -r 10 -s OPTICAL_FLOW_RAD -u _MAVPORT_
+mavlink boot_complete
+sdlog2 start -r 100 -e -t -a

From 7f9ab9b7cb5cba3afc664b89deade3d64843119e Mon Sep 17 00:00:00 2001
From: korotkoves <korotkov.es@mipt.ru>
Date: Wed, 6 Apr 2016 11:27:22 +0300
Subject: [PATCH 0087/1230] fix code style for Travis

---
 src/modules/simulator/simulator_mavlink.cpp | 5 +++--
 1 file changed, 3 insertions(+), 2 deletions(-)

diff --git a/src/modules/simulator/simulator_mavlink.cpp b/src/modules/simulator/simulator_mavlink.cpp
index f90ec7025c..87b773c651 100644
--- a/src/modules/simulator/simulator_mavlink.cpp
+++ b/src/modules/simulator/simulator_mavlink.cpp
@@ -474,8 +474,9 @@ void Simulator::pollForMAVLinkMessages(bool publish, int udp_port)
 	// udp socket data
 	struct sockaddr_in _myaddr;
 
-	if (udp_port < 1)
+	if (udp_port < 1) {
 		udp_port = UDP_PORT;
+	}
 
 	// try to setup udp socket for communcation with simulator
 	memset((char *)&_myaddr, 0, sizeof(_myaddr));
@@ -534,7 +535,7 @@ void Simulator::pollForMAVLinkMessages(bool publish, int udp_port)
 	// wait for first data from simulator and respond with first controls
 	// this is important for the UDP communication to work
 	int pret = -1;
-	PX4_INFO("Waiting for initial data on UDP port %i. Please start the flight simulator to proceed..",udp_port);
+	PX4_INFO("Waiting for initial data on UDP port %i. Please start the flight simulator to proceed..", udp_port);
 
 	uint64_t pstart_time = 0;
 

From 1a992c2d025768e3d95484b85d1df7d71a45ebe1 Mon Sep 17 00:00:00 2001
From: korotkoves <korotkov.es@mipt.ru>
Date: Wed, 6 Apr 2016 11:39:24 +0300
Subject: [PATCH 0088/1230] fix code style for Travis

---
 src/modules/simulator/simulator.cpp | 7 ++++---
 1 file changed, 4 insertions(+), 3 deletions(-)

diff --git a/src/modules/simulator/simulator.cpp b/src/modules/simulator/simulator.cpp
index a8499e6a2b..7cf3e199a1 100644
--- a/src/modules/simulator/simulator.cpp
+++ b/src/modules/simulator/simulator.cpp
@@ -131,19 +131,20 @@ int Simulator::start(int argc, char *argv[])
 	if (_instance) {
 		drv_led_start();
 
-		if (argc == 5 && strcmp(argv[3], "-u") == 0)
+		if (argc == 5 && strcmp(argv[3], "-u") == 0) {
 			udp_port = atoi(argv[4]);
+		}
 
 		if (argv[2][1] == 's') {
 			_instance->initializeSensorData();
 #ifndef __PX4_QURT
 			// Update sensor data
-			_instance->pollForMAVLinkMessages(false,udp_port);
+			_instance->pollForMAVLinkMessages(false, udp_port);
 #endif
 
 		} else if (argv[2][1] == 'p') {
 			// Update sensor data
-			_instance->pollForMAVLinkMessages(true,udp_port);
+			_instance->pollForMAVLinkMessages(true, udp_port);
 
 		} else {
 			_instance->initializeSensorData();

From 88800b9e36f235af8d01cff52bd43d99a94d3792 Mon Sep 17 00:00:00 2001
From: CarlOlsson <carlolsson.co@gmail.com>
Date: Fri, 13 May 2016 10:09:14 +0200
Subject: [PATCH 0089/1230] added tas_innov_gate to params

---
 src/modules/ekf2/ekf2_main.cpp |  2 ++
 src/modules/ekf2/ekf2_params.c | 10 ++++++++++
 2 files changed, 12 insertions(+)

diff --git a/src/modules/ekf2/ekf2_main.cpp b/src/modules/ekf2/ekf2_main.cpp
index c8eddba90c..d48937ec92 100644
--- a/src/modules/ekf2/ekf2_main.cpp
+++ b/src/modules/ekf2/ekf2_main.cpp
@@ -189,6 +189,7 @@ private:
 	control::BlockParamFloat _baro_innov_gate;     // innovation gate for barometric height innovation test (std dev)
 	control::BlockParamFloat _posNE_innov_gate;    // innovation gate for GPS horizontal position innovation test (std dev)
 	control::BlockParamFloat _vel_innov_gate;      // innovation gate for GPS velocity innovation test (std dev)
+	control::BlockParamFloat _tas_innov_gate;	   // innovation gate for tas innovation test (std dev)
 
 	control::BlockParamFloat _mag_heading_noise;	// measurement noise used for simple heading fusion
 	control::BlockParamFloat _mag_noise;           // measurement noise used for 3-axis magnetoemter fusion (Gauss)
@@ -286,6 +287,7 @@ Ekf2::Ekf2():
 	_baro_innov_gate(this, "EKF2_BARO_GATE", false, &_params->baro_innov_gate),
 	_posNE_innov_gate(this, "EKF2_GPS_P_GATE", false, &_params->posNE_innov_gate),
 	_vel_innov_gate(this, "EKF2_GPS_V_GATE", false, &_params->vel_innov_gate),
+	_tas_innov_gate(this, "EKF2_TAS_GATE", false, &_params->tas_innov_gate),
 	_mag_heading_noise(this, "EKF2_HEAD_NOISE", false, &_params->mag_heading_noise),
 	_mag_noise(this, "EKF2_MAG_NOISE", false, &_params->mag_noise),
 	_eas_noise(this, "EKF2_EAS_NOISE", false, &_params->eas_noise),
diff --git a/src/modules/ekf2/ekf2_params.c b/src/modules/ekf2/ekf2_params.c
index aaa0785f88..d786a98724 100644
--- a/src/modules/ekf2/ekf2_params.c
+++ b/src/modules/ekf2/ekf2_params.c
@@ -443,6 +443,16 @@ PARAM_DEFINE_FLOAT(EKF2_GPS_P_GATE, 5.0f);
  */
 PARAM_DEFINE_FLOAT(EKF2_GPS_V_GATE, 5.0f);
 
+/**
+ * Gate size for TAS fusion
+ *
+ * @group EKF2
+ * @min 1.0
+ * @unit SD
+ * @decimal 1
+ */
+PARAM_DEFINE_FLOAT(EKF2_TAS_GATE, 3.0f);
+
 /**
  * Replay mode
  *

From ae6d1c50d724e7652ed564441c5e298e66de041e Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Fri, 13 May 2016 14:51:04 +0200
Subject: [PATCH 0090/1230] MAVLink submodules: Add v2.0 protocol

---
 .gitmodules                  | 3 +++
 Tools/check_submodules.sh    | 1 +
 mavlink/include/mavlink/v2.0 | 1 +
 3 files changed, 5 insertions(+)
 create mode 160000 mavlink/include/mavlink/v2.0

diff --git a/.gitmodules b/.gitmodules
index dc9e183976..fe09b6399c 100644
--- a/.gitmodules
+++ b/.gitmodules
@@ -38,3 +38,6 @@
 [submodule "src/drivers/gps/devices"]
 	path = src/drivers/gps/devices
 	url = https://github.com/PX4/GpsDrivers.git
+[submodule "mavlink/include/mavlink/v2.0"]
+	path = mavlink/include/mavlink/v2.0
+	url = git://github.com/mavlink/c_library.git
diff --git a/Tools/check_submodules.sh b/Tools/check_submodules.sh
index c32ec83a78..624213097d 100755
--- a/Tools/check_submodules.sh
+++ b/Tools/check_submodules.sh
@@ -69,6 +69,7 @@ check_git_submodule Tools/jMAVSim
 check_git_submodule Tools/sitl_gazebo
 check_git_submodule cmake/cmake_hexagon
 check_git_submodule mavlink/include/mavlink/v1.0
+check_git_submodule mavlink/include/mavlink/v2.0
 check_git_submodule src/lib/DriverFramework
 check_git_submodule src/lib/DriverFramework/cmake/cmake_hexagon
 check_git_submodule src/lib/DriverFramework/dspal
diff --git a/mavlink/include/mavlink/v2.0 b/mavlink/include/mavlink/v2.0
new file mode 160000
index 0000000000..83015b3391
--- /dev/null
+++ b/mavlink/include/mavlink/v2.0
@@ -0,0 +1 @@
+Subproject commit 83015b3391a42d46be5dcca08bf01360e17f7b40

From 24022e46343b3e437b1fa24c1cca808360f03510 Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Fri, 13 May 2016 14:53:11 +0200
Subject: [PATCH 0091/1230] MAVLink 2.0: Update to 2.0 protocol files

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

diff --git a/mavlink/include/mavlink/v2.0 b/mavlink/include/mavlink/v2.0
index 83015b3391..6504af3300 160000
--- a/mavlink/include/mavlink/v2.0
+++ b/mavlink/include/mavlink/v2.0
@@ -1 +1 @@
-Subproject commit 83015b3391a42d46be5dcca08bf01360e17f7b40
+Subproject commit 6504af330080d104ceb674780f04bd0462345c23

From 07557bc3626a1e9346a5e3e754eddae705c59960 Mon Sep 17 00:00:00 2001
From: Lok Tep <loktep@gmail.com>
Date: Tue, 3 May 2016 16:26:50 +0200
Subject: [PATCH 0092/1230] SPI bmp280 driver, I2C skeleton

---
 src/drivers/bmp280/CMakeLists.txt |  52 ++
 src/drivers/bmp280/bmp280.cpp     | 971 ++++++++++++++++++++++++++++++
 src/drivers/bmp280/bmp280.h       | 151 +++++
 src/drivers/bmp280/bmp280_spi.cpp | 145 +++++
 4 files changed, 1319 insertions(+)
 create mode 100644 src/drivers/bmp280/CMakeLists.txt
 create mode 100644 src/drivers/bmp280/bmp280.cpp
 create mode 100644 src/drivers/bmp280/bmp280.h
 create mode 100644 src/drivers/bmp280/bmp280_spi.cpp

diff --git a/src/drivers/bmp280/CMakeLists.txt b/src/drivers/bmp280/CMakeLists.txt
new file mode 100644
index 0000000000..f35bd1b820
--- /dev/null
+++ b/src/drivers/bmp280/CMakeLists.txt
@@ -0,0 +1,52 @@
+############################################################################
+#
+#   Copyright (c) 2015 PX4 Development Team. All rights reserved.
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions
+# are met:
+#
+# 1. Redistributions of source code must retain the above copyright
+#    notice, this list of conditions and the following disclaimer.
+# 2. Redistributions in binary form must reproduce the above copyright
+#    notice, this list of conditions and the following disclaimer in
+#    the documentation and/or other materials provided with the
+#    distribution.
+# 3. Neither the name PX4 nor the names of its contributors may be
+#    used to endorse or promote products derived from this software
+#    without specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.
+#
+############################################################################
+set(srcs
+	bmp280_spi.cpp
+	)
+
+if(${OS} STREQUAL "nuttx")
+	list(APPEND srcs
+		bmp280.cpp
+		)
+endif()
+
+px4_add_module(
+	MODULE drivers__bmp280
+	MAIN bmp280
+	COMPILE_FLAGS
+		-Os
+	SRCS ${srcs}
+	DEPENDS
+		platforms__common
+	)
+# vim: set noet ft=cmake fenc=utf-8 ff=unix : 
diff --git a/src/drivers/bmp280/bmp280.cpp b/src/drivers/bmp280/bmp280.cpp
new file mode 100644
index 0000000000..d516a3db35
--- /dev/null
+++ b/src/drivers/bmp280/bmp280.cpp
@@ -0,0 +1,971 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2012-2015 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file bmp280.cpp
+ * Driver for the BMP280 barometric pressure sensor connected via I2C TODO or SPI.
+ */
+
+#include <px4_config.h>
+
+#include <sys/types.h>
+#include <stdint.h>
+#include <stdbool.h>
+#include <stdlib.h>
+#include <semaphore.h>
+#include <string.h>
+#include <fcntl.h>
+#include <poll.h>
+#include <errno.h>
+#include <stdio.h>
+#include <math.h>
+#include <unistd.h>
+#include <getopt.h>
+
+#include <nuttx/arch.h>
+#include <nuttx/wqueue.h>
+#include <nuttx/clock.h>
+
+#include <arch/board/board.h>
+#include <board_config.h>
+#include <drivers/bmp280/bmp280.h>
+
+#include <drivers/device/device.h>
+#include <drivers/drv_baro.h>
+#include <drivers/drv_hrt.h>
+#include <drivers/device/ringbuffer.h>
+
+#include <systemlib/perf_counter.h>
+#include <systemlib/err.h>
+
+
+enum BMP280_BUS {
+	BMP280_BUS_ALL = 0,
+	BMP280_BUS_I2C_INTERNAL,
+	BMP280_BUS_I2C_EXTERNAL,
+	BMP280_BUS_SPI_INTERNAL,
+	BMP280_BUS_SPI_EXTERNAL
+};
+
+#ifndef CONFIG_SCHED_WORKQUEUE
+# error This requires CONFIG_SCHED_WORKQUEUE.
+#endif
+
+/*
+ * BMP280 internal constants and data structures.
+ */
+
+class BMP280 : public device::CDev
+{
+public:
+	BMP280(bmp280::IBMP280 *interface, const char* path);
+	~BMP280();
+
+	virtual int		init();
+
+	virtual ssize_t	read(struct file *filp, char *buffer, size_t buflen);
+	virtual int		ioctl(struct file *filp, int cmd, unsigned long arg);
+
+	/**
+	 * Diagnostics - print some basic information about the driver.
+	 */
+	void			print_info();
+
+private:
+	bmp280::IBMP280*	_interface;
+
+	uint8_t				_curr_ctrl;
+
+	struct work_s		_work;
+	unsigned			_report_ticks; // 0 - no cycling, otherwise period of sending a report
+	unsigned			_max_mesure_ticks; //ticks needed to measure
+
+	ringbuffer::RingBuffer	*_reports;
+
+	bool			_collect_phase;
+
+	/* altitude conversion calibration */
+	unsigned		_msl_pressure;	/* in Pa */
+
+	orb_advert_t		_baro_topic;
+	int					_orb_class_instance;
+	int					_class_instance;
+
+	perf_counter_t		_sample_perf;
+	perf_counter_t		_measure_perf;
+	perf_counter_t		_comms_errors;
+	perf_counter_t		_buffer_overflows;
+
+	struct bmp280::calibration_s* _cal; //stored calibration constants
+	struct bmp280::fcalibration_s _fcal; //pre processed calibration constants
+
+	float			_P; /* in Pa */
+	float			_T; /* in K */
+
+
+	/* periodic execution helpers */
+	void			start_cycle();
+	void			stop_cycle();
+	void			cycle(); //main execution
+	static void		cycle_trampoline(void *arg);
+
+	int		measure(); //start measure
+	int		collect(); //get results and publish
+};
+
+/*
+ * Driver 'main' command.
+ */
+extern "C" __EXPORT int bmp280_main(int argc, char *argv[]);
+
+BMP280::BMP280(bmp280::IBMP280 *interface, const char* path) :
+	CDev("BMP280", path),
+	_interface(interface),
+	_report_ticks(0),
+	_reports(nullptr),
+	_collect_phase(false),
+	_msl_pressure(101325),
+	_baro_topic(nullptr),
+	_orb_class_instance(-1),
+	_class_instance(-1),
+	_sample_perf(perf_alloc(PC_ELAPSED, "bmp280_read")),
+	_measure_perf(perf_alloc(PC_ELAPSED, "bmp280_measure")),
+	_comms_errors(perf_alloc(PC_COUNT, "bmp280_comms_errors")),
+	_buffer_overflows(perf_alloc(PC_COUNT, "bmp280_buffer_overflows"))
+{
+	// work_cancel in stop_cycle called from the dtor will explode if we don't do this...
+	memset(&_work, 0, sizeof(_work));
+}
+
+BMP280::~BMP280()
+{
+	/* make sure we are truly inactive */
+	stop_cycle();
+
+	if (_class_instance != -1)
+		unregister_class_devname(get_devname(), _class_instance);
+
+	/* free any existing reports */
+	if (_reports != nullptr)
+		delete _reports;
+
+	// free perf counters
+	perf_free(_sample_perf);
+	perf_free(_measure_perf);
+	perf_free(_comms_errors);
+	perf_free(_buffer_overflows);
+
+	delete _interface;
+
+
+}
+
+int
+BMP280::init()
+{
+	int ret;
+
+	ret = CDev::init();
+	if (ret != OK) {
+		DEVICE_DEBUG("CDev init failed");
+		return ret;
+	}
+
+	/* allocate basic report buffers */
+	_reports = new ringbuffer::RingBuffer(2, sizeof(baro_report));
+
+	if (_reports == nullptr) {
+		DEVICE_DEBUG("can't get memory for reports");
+		ret = -ENOMEM;
+		return ret;
+	}
+
+	/* register alternate interfaces if we have to */
+	_class_instance = register_class_devname(BARO_BASE_DEVICE_PATH);
+
+	/* reset sensor */
+	_interface->set_reg(BPM280_VALUE_RESET,BPM280_ADDR_RESET);
+	usleep(10000);
+
+	/* check  id*/
+	if ( _interface->get_reg(BPM280_ADDR_ID) != BPM280_VALUE_ID ) {
+		warnx("id of your baro is not: 0x%02x",BPM280_VALUE_ID);
+		return -EIO;
+	}
+
+	/* set config, recommended settings */
+	_curr_ctrl = BPM280_CTRL_P16 | BPM280_CTRL_T2;
+	_interface->set_reg(_curr_ctrl,BPM280_ADDR_CTRL);
+	_max_mesure_ticks = USEC2TICK( BPM280_MT_INIT + BPM280_MT*(16-1 + 2-1) );
+	_interface->set_reg(BPM280_CONFIG_F16,BPM280_ADDR_CONFIG);
+
+	/* get calibration and pre process them*/
+	_cal = _interface->get_calibration(BPM280_ADDR_CAL);
+
+	_fcal.t1 =  _cal->t1 * powf(2,  4 );
+	_fcal.t2 =  _cal->t2 * powf(2, -14);
+	_fcal.t3 =  _cal->t3 * powf(2, -34);
+
+	_fcal.p1 = _cal->p1            * (powf(2,  4 ) / -100000.0f);
+	_fcal.p2 = _cal->p1 * _cal->p2 * (powf(2, -31) / -100000.0f);
+	_fcal.p3 = _cal->p1 * _cal->p3 * (powf(2, -51) / -100000.0f);
+
+	_fcal.p4 = _cal->p4 * powf(2,  4 ) - powf(2, 20);
+	_fcal.p5 = _cal->p5 * powf(2, -14);
+	_fcal.p6 = _cal->p6 * powf(2, -31);
+
+	_fcal.p7 = _cal->p7 * powf(2, -4 );
+	_fcal.p8 = _cal->p8 * powf(2, -19) + 1.0f;
+	_fcal.p9 = _cal->p9 * powf(2, -35);
+
+	/* do a first measurement cycle to populate reports with valid data */
+	struct baro_report brp;
+	_reports->flush();
+
+	if ( measure() ) {
+		return -EIO;
+	}
+
+	usleep( TICK2USEC(_max_mesure_ticks) );
+
+	if ( collect() ) {
+		return -EIO;
+	}
+
+	_reports->get(&brp);
+
+	_baro_topic = orb_advertise_multi(ORB_ID(sensor_baro), &brp,
+				&_orb_class_instance, _interface->is_external()? ORB_PRIO_HIGH : ORB_PRIO_DEFAULT);
+
+	if (_baro_topic == nullptr) {
+		warnx("failed to create sensor_baro publication");
+		return -ENOMEM;
+	}
+
+	return OK;
+
+}
+
+ssize_t
+BMP280::read(struct file *filp, char *buffer, size_t buflen)
+{
+	unsigned count = buflen / sizeof(struct baro_report);
+	struct baro_report *brp = reinterpret_cast<struct baro_report *>(buffer);
+	int ret = 0;
+
+	/* buffer must be large enough */
+	if (count < 1)
+		return -ENOSPC;
+
+	/* if automatic measurement is enabled */
+	if (_report_ticks > 0) {
+
+		/*
+		 * While there is space in the caller's buffer, and reports, copy them.
+		 * Note that we may be pre-empted by the workq thread while we are doing this;
+		 * we are careful to avoid racing with them.
+		 */
+		while (count--) {
+			if (_reports->get(brp)) {
+				ret += sizeof(*brp);
+				brp++;
+			}
+		}
+
+		/* if there was no data, warn the caller */
+		return ret ? ret : -EAGAIN;
+	}
+
+	/* manual measurement - run one conversion */
+
+	_reports->flush();
+
+	if ( measure() ) {
+		return -EIO;
+	}
+
+	usleep( TICK2USEC(_max_mesure_ticks) );
+
+	if ( collect() ) {
+		return -EIO;
+	}
+
+	if (_reports->get(brp)) { //get new generated report
+		ret = sizeof(*brp);
+	}
+
+	return ret;
+}
+
+int
+BMP280::ioctl(struct file *filp, int cmd, unsigned long arg)
+{
+	switch (cmd) {
+
+	case SENSORIOCSPOLLRATE: {
+
+		unsigned ticks = 0;
+
+		switch (arg) {
+
+			case SENSOR_POLLRATE_MANUAL:
+				stop_cycle();
+				_report_ticks = 0;
+				return OK;
+
+			case SENSOR_POLLRATE_EXTERNAL:
+			case 0:
+				return -EINVAL;
+
+			case SENSOR_POLLRATE_MAX:
+			case SENSOR_POLLRATE_DEFAULT:
+					ticks = _max_mesure_ticks;
+			default: {
+					if (ticks == 0) {
+						ticks = USEC2TICK(USEC_PER_SEC / arg);
+					}
+					/* do we need to start internal polling? */
+					bool want_start = (_report_ticks == 0);
+
+					/* check against maximum rate */
+					if (ticks < _max_mesure_ticks)
+						return -EINVAL;
+
+					_report_ticks = ticks;
+					if (want_start)
+						start_cycle();
+
+					return OK;
+				}
+		}
+
+		break;
+	}
+
+	case SENSORIOCGPOLLRATE:
+		if (_report_ticks == 0)
+			return SENSOR_POLLRATE_MANUAL;
+
+		return (USEC_PER_SEC/USEC_PER_TICK/_report_ticks);
+
+	case SENSORIOCSQUEUEDEPTH: {
+		/* lower bound is mandatory, upper bound is a sanity check */
+		if ((arg < 1) || (arg > 100))
+			return -EINVAL;
+
+		irqstate_t flags = irqsave();
+		if (!_reports->resize(arg)) {
+			irqrestore(flags);
+			return -ENOMEM;
+		}
+		irqrestore(flags);
+		return OK;
+	}
+
+	case SENSORIOCGQUEUEDEPTH:
+		return _reports->size();
+
+	case SENSORIOCRESET:
+		/*
+		 * Since we are initialized, we do not need to do anything, since the
+		 * PROM is correctly read and the part does not need to be configured.
+		 */
+		return OK;
+
+	case BAROIOCSMSLPRESSURE:
+
+		/* range-check for sanity */
+		if ((arg < 80000) || (arg > 120000))
+			return -EINVAL;
+
+		_msl_pressure = arg;
+		return OK;
+
+	case BAROIOCGMSLPRESSURE:
+		return _msl_pressure;
+
+	default:
+		break;
+	}
+
+	return CDev::ioctl(filp, cmd, arg);
+}
+
+void
+BMP280::start_cycle()
+{
+
+	/* reset the report ring and state machine */
+	_collect_phase = false;
+	_reports->flush();
+
+	/* schedule a cycle to start things */
+	work_queue(HPWORK, &_work, (worker_t)&BMP280::cycle_trampoline, this, 1);
+}
+
+void
+BMP280::stop_cycle()
+{
+	work_cancel(HPWORK, &_work);
+}
+
+void
+BMP280::cycle_trampoline(void *arg)
+{
+	BMP280 *dev = reinterpret_cast<BMP280 *>(arg);
+
+	dev->cycle();
+}
+
+void
+BMP280::cycle()
+{
+	if (_collect_phase) {
+		collect();
+		unsigned wait_gap = _report_ticks - _max_mesure_ticks;
+
+		if ( wait_gap != 0 ) {
+			work_queue(HPWORK,&_work,(worker_t)&BMP280::cycle_trampoline,this,wait_gap); //need to wait some time before new measurement
+			return;
+		}
+
+	}
+
+	measure();
+	work_queue(HPWORK,&_work,(worker_t)&BMP280::cycle_trampoline,this,_max_mesure_ticks);
+
+}
+
+int
+BMP280::measure()
+{
+	_collect_phase = true;
+
+	perf_begin(_measure_perf);
+
+	/* start measure */
+	int ret = _interface->set_reg(_curr_ctrl | BPM280_CTRL_MODE_FORCE,BPM280_ADDR_CTRL);
+
+	if ( ret != OK) {
+		perf_count(_comms_errors);
+		perf_cancel(_measure_perf);
+		return -EIO;
+	}
+
+	perf_end(_measure_perf);
+
+	return OK;
+}
+
+int
+BMP280::collect()
+{
+	_collect_phase = false;
+
+	perf_begin(_sample_perf);
+
+	struct baro_report report;
+	/* this should be fairly close to the end of the conversion, so the best approximation of the time */
+	report.timestamp = hrt_absolute_time();
+    report.error_count = perf_event_count(_comms_errors);
+
+    bmp280::data_s* data = _interface->get_data(BPM280_ADDR_DATA);
+    if (data == nullptr) {
+    	perf_count(_comms_errors);
+    	perf_cancel(_sample_perf);
+    	return -EIO;
+    }
+
+    //convert data to number 20 bit
+    uint32_t p_raw =  data->p_msb<<12 | data->p_lsb<<4 | data->p_xlsb>>4;
+    uint32_t t_raw =  data->t_msb<<12 | data->t_lsb<<4 | data->t_xlsb>>4;
+
+    // Temperature
+	float ofs = (float) t_raw - _fcal.t1;
+	float t_fine = (ofs * _fcal.t3 + _fcal.t2) * ofs;
+	_T = t_fine * (1.0f/5120.0f);
+
+	// Pressure
+	float tf = t_fine - 128000.0f;
+	float x1 = (tf * _fcal.p6 + _fcal.p5) * tf + _fcal.p4;
+	float x2 = (tf * _fcal.p3 + _fcal.p2) * tf + _fcal.p1;
+
+	float pf = ((float) p_raw + x1) / x2;
+	_P = (pf * _fcal.p9 + _fcal.p8) * pf + _fcal.p7;
+
+
+	report.temperature = _T;
+	report.pressure = _P/100.0f; // to mbar
+
+
+	/* altitude calculations based on http://www.kansasflyer.org/index.asp?nav=Avi&sec=Alti&tab=Theory&pg=1 */
+
+	/* tropospheric properties (0-11km) for standard atmosphere */
+	const float T1 = 15.0f + 273.15f;	/* temperature at base height in Kelvin */
+	const float a  = -6.5f / 1000.0f;	/* temperature gradient in degrees per metre */
+	const float g  = 9.80665f;	/* gravity constant in m/s/s */
+	const float R  = 287.05f;	/* ideal gas constant in J/kg/K */
+	float pK = _P / _msl_pressure;
+
+	/*
+	 * Solve:
+	 *
+	 *     /        -(aR / g)     \
+	 *    | (p / p1)          . T1 | - T1
+	 *     \                      /
+	 * h = -------------------------------  + h1
+	 *                   a
+	 */
+	report.altitude = (((powf(pK, (-(a * R) / g))) * T1) - T1) / a;
+
+
+	/* publish it */
+	if (!(_pub_blocked)) {
+		/* publish it */
+		orb_publish(ORB_ID(sensor_baro), _baro_topic, &report);
+	}
+
+	if (_reports->force(&report)) {
+		perf_count(_buffer_overflows);
+	}
+
+	/* notify anyone waiting for data */
+	poll_notify(POLLIN);
+
+	perf_end(_sample_perf);
+
+	return OK;
+}
+
+void
+BMP280::print_info()
+{
+	perf_print_counter(_sample_perf);
+	perf_print_counter(_comms_errors);
+	perf_print_counter(_buffer_overflows);
+	printf("poll interval:  %u us \n", _report_ticks * USEC_PER_TICK);
+	_reports->print_info("report queue");
+	printf("P Pa:              %.3f\n", (double)_P);
+	printf("T:              %.3f\n", (double)_T);
+	printf("MSL pressure Pa:   %u\n", _msl_pressure);
+
+}
+
+/**
+ * Local functions in support of the shell command.
+ */
+namespace bmp280
+{
+
+/*
+  list of supported bus configurations
+ */
+struct bmp280_bus_option {
+	enum BMP280_BUS busid;
+	const char *devpath;
+	BMP280_constructor interface_constructor;
+	uint8_t busnum;
+	uint8_t device;
+	bool external;
+	BMP280 *dev;
+} bus_options[] = {
+#if defined(PX4_SPIDEV_EXT_BARO) && defined(PX4_SPI_BUS_EXT)
+	{ BMP280_BUS_SPI_EXTERNAL, "/dev/bmp280_spi_ext", &bmp280_spi_interface, PX4_SPI_BUS_EXT, PX4_SPIDEV_EXT_BARO , true , NULL },
+#endif
+#ifdef PX4_SPIDEV_BARO
+	{ BMP280_BUS_SPI_INTERNAL, "/dev/bmp280_spi_int", &bmp280_spi_interface, PX4_SPI_BUS_SENSORS,PX4_SPIDEV_BARO, false ,NULL },
+#endif
+#ifdef PX4_I2C_OBDEV_BMP280
+	{ BMP280_BUS_I2C_INTERNAL, "/dev/bmp280_i2c_int", nullptr, PX4_I2C_BUS_ONBOARD, PX4_I2C_OBDEV_BMP280, false, NULL },
+#endif
+#if defined(PX4_I2C_BUS_EXPANSION) && defined(PX4_I2C_EXT_OBDEV_BMP280)
+	{ BMP280_BUS_I2C_EXTERNAL, "/dev/bmp280_i2c_ext", nullptr, PX4_I2C_BUS_EXPANSION,PX4_I2C_EXT_OBDEV_BMP280 , true , NULL },
+#endif
+};
+#define NUM_BUS_OPTIONS (sizeof(bus_options)/sizeof(bus_options[0]))
+
+bool	start_bus(struct bmp280_bus_option &bus);
+struct bmp280_bus_option &find_bus(enum BMP280_BUS busid);
+void	start(enum BMP280_BUS busid);
+void	test(enum BMP280_BUS busid);
+void	reset(enum BMP280_BUS busid);
+void	info();
+void	calibrate(unsigned altitude, enum BMP280_BUS busid);
+void	usage();
+
+
+/**
+ * Start the driver.
+ */
+bool
+start_bus(struct bmp280_bus_option &bus)
+{
+	if (bus.dev != nullptr)
+		errx(1,"bus option already started");
+
+	bmp280::IBMP280 *interface = bus.interface_constructor(bus.busnum, bus.device, bus.external);
+	if (interface->init() != OK) {
+		delete interface;
+		warnx("no device on bus %u", (unsigned)bus.busid);
+		return false;
+	}
+
+	bus.dev = new BMP280(interface, bus.devpath);
+	if (bus.dev != nullptr && OK != bus.dev->init()) {
+		delete bus.dev;
+		bus.dev = NULL;
+		return false;
+	}
+
+	int fd = open(bus.devpath, O_RDONLY);
+
+	/* set the poll rate to default, starts automatic data collection */
+	if (fd == -1) {
+		errx(1, "can't open baro device");
+	}
+	if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) {
+		close(fd);
+		errx(1, "failed setting default poll rate");
+	}
+
+	close(fd);
+	return true;
+}
+
+
+/**
+ * Start the driver.
+ *
+ * This function call only returns once the driver
+ * is either successfully up and running or failed to start.
+ */
+void
+start(enum BMP280_BUS busid)
+{
+	uint8_t i;
+	bool started = false;
+
+	for (i=0; i<NUM_BUS_OPTIONS; i++) {
+		if (busid == BMP280_BUS_ALL && bus_options[i].dev != NULL) {
+			// this device is already started
+			continue;
+		}
+		if (busid != BMP280_BUS_ALL && bus_options[i].busid != busid) {
+			// not the one that is asked for
+			continue;
+		}
+		started |= start_bus(bus_options[i]);
+	}
+
+	if (!started)
+		errx(1, "driver start failed");
+
+	// one or more drivers started OK
+	exit(0);
+}
+
+
+/**
+ * find a bus structure for a busid
+ */
+struct bmp280_bus_option &find_bus(enum BMP280_BUS busid)
+{
+	for (uint8_t i=0; i<NUM_BUS_OPTIONS; i++) {
+		if ((busid == BMP280_BUS_ALL ||
+		     busid == bus_options[i].busid) && bus_options[i].dev != NULL) {
+			return bus_options[i];
+		}
+	}
+	errx(1,"bus %u not started", (unsigned)busid);
+}
+
+/**
+ * Perform some basic functional tests on the driver;
+ * make sure we can collect data from the sensor in polled
+ * and automatic modes.
+ */
+void
+test(enum BMP280_BUS busid)
+{
+	struct bmp280_bus_option &bus = find_bus(busid);
+	struct baro_report report;
+	ssize_t sz;
+	int ret;
+
+	int fd;
+
+	fd = open(bus.devpath, O_RDONLY);
+	if (fd < 0)
+		err(1, "open failed (try 'bmp280 start' if the driver is not running)");
+
+	/* do a simple demand read */
+	sz = read(fd, &report, sizeof(report));
+
+	if (sz != sizeof(report))
+		err(1, "immediate read failed");
+
+	warnx("single read");
+	warnx("pressure:    %10.4f", (double)report.pressure);
+	warnx("altitude:    %11.4f", (double)report.altitude);
+	warnx("temperature: %8.4f", (double)report.temperature);
+	warnx("time:        %lld", report.timestamp);
+
+	/* set the queue depth to 10 */
+	if (OK != ioctl(fd, SENSORIOCSQUEUEDEPTH, 10))
+		errx(1, "failed to set queue depth");
+
+	/* start the sensor polling at 2Hz */
+	if (OK != ioctl(fd, SENSORIOCSPOLLRATE, 2))
+		errx(1, "failed to set 2Hz poll rate");
+
+	/* read the sensor 5x and report each value */
+	for (unsigned i = 0; i < 5; i++) {
+		struct pollfd fds;
+
+		/* wait for data to be ready */
+		fds.fd = fd;
+		fds.events = POLLIN;
+		ret = poll(&fds, 1, 2000);
+
+		if (ret != 1)
+			errx(1, "timed out waiting for sensor data");
+
+		/* now go get it */
+		sz = read(fd, &report, sizeof(report));
+
+		if (sz != sizeof(report))
+			err(1, "periodic read failed");
+
+		warnx("periodic read %u", i);
+		warnx("pressure:    %10.4f", (double)report.pressure);
+		warnx("altitude:    %11.4f", (double)report.altitude);
+		warnx("temperature K: %8.4f", (double)report.temperature);
+		warnx("time:        %lld", report.timestamp);
+	}
+
+	close(fd);
+	errx(0, "PASS");
+}
+
+/**
+ * Reset the driver.
+ */
+void
+reset(enum BMP280_BUS busid)
+{
+	struct bmp280_bus_option &bus = find_bus(busid);
+	int fd;
+
+	fd = open(bus.devpath, O_RDONLY);
+	if (fd < 0)
+		err(1, "failed ");
+
+	if (ioctl(fd, SENSORIOCRESET, 0) < 0)
+		err(1, "driver reset failed");
+
+	if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0)
+		err(1, "driver poll restart failed");
+
+	exit(0);
+}
+
+/**
+ * Print a little info about the driver.
+ */
+void
+info()
+{
+	for (uint8_t i=0; i<NUM_BUS_OPTIONS; i++) {
+		struct bmp280_bus_option &bus = bus_options[i];
+		if (bus.dev != nullptr) {
+			warnx("%s", bus.devpath);
+			bus.dev->print_info();
+		}
+	}
+	exit(0);
+}
+
+/**
+ * Calculate actual MSL pressure given current altitude
+ */
+void
+calibrate(unsigned altitude, enum BMP280_BUS busid)
+{
+	struct bmp280_bus_option &bus = find_bus(busid);
+	struct baro_report report;
+	float	pressure;
+	float	p1;
+
+	int fd;
+
+	fd = open(bus.devpath, O_RDONLY);
+
+	if (fd < 0)
+		err(1, "open failed (try 'bmp280 start' if the driver is not running)");
+
+	/* start the sensor polling at max */
+	if (OK != ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_MAX))
+		errx(1, "failed to set poll rate");
+
+	/* average a few measurements */
+	pressure = 0.0f;
+
+	for (unsigned i = 0; i < 20; i++) {
+		struct pollfd fds;
+		int ret;
+		ssize_t sz;
+
+		/* wait for data to be ready */
+		fds.fd = fd;
+		fds.events = POLLIN;
+		ret = poll(&fds, 1, 1000);
+
+		if (ret != 1)
+			errx(1, "timed out waiting for sensor data");
+
+		/* now go get it */
+		sz = read(fd, &report, sizeof(report));
+
+		if (sz != sizeof(report))
+			err(1, "sensor read failed");
+
+		pressure += report.pressure;
+	}
+
+	pressure /= 20;		/* average */
+	pressure /= 10;		/* scale from millibar to kPa */
+
+	/* tropospheric properties (0-11km) for standard atmosphere */
+	const float T1 = 15.0 + 273.15;	/* temperature at base height in Kelvin */
+	const float a  = -6.5 / 1000;	/* temperature gradient in degrees per metre */
+	const float g  = 9.80665f;	/* gravity constant in m/s/s */
+	const float R  = 287.05f;	/* ideal gas constant in J/kg/K */
+
+	warnx("averaged pressure %10.4fkPa at %um", (double)pressure, altitude);
+
+	p1 = pressure * (powf(((T1 + (a * (float)altitude)) / T1), (g / (a * R))));
+
+	warnx("calculated MSL pressure %10.4fkPa", (double)p1);
+
+	/* save as integer Pa */
+	p1 *= 1000.0f;
+
+	if (ioctl(fd, BAROIOCSMSLPRESSURE, (unsigned long)p1) != OK)
+		err(1, "BAROIOCSMSLPRESSURE");
+
+	close(fd);
+	exit(0);
+}
+
+void
+usage()
+{
+	warnx("missing command: try 'start', 'info', 'test', 'test2', 'reset', 'calibrate'");
+	warnx("options:");
+	warnx("    -X    (external I2C bus TODO)");
+	warnx("    -I    (internal I2C bus TODO)");
+	warnx("    -S    (external SPI bus)");
+	warnx("    -s    (internal SPI bus)");
+}
+
+} // namespace
+
+int
+bmp280_main(int argc, char *argv[])
+{
+	enum BMP280_BUS busid = BMP280_BUS_ALL;
+	int ch;
+
+	/* jump over start/off/etc and look at options first */
+	while ((ch = getopt(argc, argv, "XISs")) != EOF) {
+		switch (ch) {
+		case 'X':
+			busid = BMP280_BUS_I2C_EXTERNAL;
+			errx(1, "not supported yet");
+			break;
+		case 'I':
+			busid = BMP280_BUS_I2C_INTERNAL;
+			errx(1, "not supported yet");
+			break;
+		case 'S':
+			busid = BMP280_BUS_SPI_EXTERNAL;
+			break;
+		case 's':
+			busid = BMP280_BUS_SPI_INTERNAL;
+			break;
+		default:
+			bmp280::usage();
+			exit(0);
+		}
+	}
+
+	const char *verb = argv[optind];
+
+	/*
+	 * Start/load the driver.
+	 */
+	if (!strcmp(verb, "start"))
+		bmp280::start(busid);
+
+	/*
+	 * Test the driver/device.
+	 */
+	if (!strcmp(verb, "test"))
+		bmp280::test(busid);
+
+	/*
+	 * Reset the driver.
+	 */
+	if (!strcmp(verb, "reset"))
+		bmp280::reset(busid);
+
+	/*
+	 * Print driver information.
+	 */
+	if (!strcmp(verb, "info"))
+		bmp280::info();
+
+	/*
+	 * Perform MSL pressure calibration given an altitude in metres
+	 */
+	if (!strcmp(verb, "calibrate")) {
+		if (argc < 2)
+			errx(1, "missing altitude");
+
+		long altitude = strtol(argv[optind+1], nullptr, 10);
+
+		bmp280::calibrate(altitude, busid);
+	}
+
+	errx(1, "unrecognized command, try 'start', 'test', 'reset' or 'info'");
+}
diff --git a/src/drivers/bmp280/bmp280.h b/src/drivers/bmp280/bmp280.h
new file mode 100644
index 0000000000..44155b12fc
--- /dev/null
+++ b/src/drivers/bmp280/bmp280.h
@@ -0,0 +1,151 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2012-2013 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file bmp280.h
+ *
+ * Shared defines for the bmp280 driver.
+ */
+#pragma once
+
+#define BPM280_ADDR_CAL		0x88	/* address of 12x 2 bytes calibration data */
+#define BPM280_ADDR_DATA	0xF7	/* address of 2x 3 bytes p-t data */
+
+#define BPM280_ADDR_CONFIG	0xF5	/* configuration */
+#define BPM280_ADDR_CTRL	0xF4	/* controll */
+#define BPM280_ADDR_STATUS	0xF3	/* state */
+#define BPM280_ADDR_RESET	0xE0	/* reset */
+#define BPM280_ADDR_ID		0xD0	/* id */
+
+#define BPM280_VALUE_ID		0x58	/* chip id */
+#define BPM280_VALUE_RESET	0xB6	/* reset */
+
+#define BPM280_STATUS_MEASURING	1<<3	/* if in process of measure */
+#define BPM280_STATUS_COPING	1<<0	/* if in process of data copy */
+
+#define BPM280_CTRL_P0		0x0<<2		/* no p measure */
+#define BPM280_CTRL_P1		0x1<<2
+#define BPM280_CTRL_P2		0x2<<2
+#define BPM280_CTRL_P4		0x3<<2
+#define BPM280_CTRL_P8		0x4<<2
+#define BPM280_CTRL_P16		0x5<<2
+
+#define BPM280_CTRL_T0		0x0<<5		/* no t measure */
+#define BPM280_CTRL_T1		0x1<<5
+#define BPM280_CTRL_T2		0x2<<5
+#define BPM280_CTRL_T4		0x3<<5
+#define BPM280_CTRL_T8		0x4<<5
+#define BPM280_CTRL_T16		0x5<<5
+
+#define BPM280_CONFIG_F0		0x0<<2		/* no filter */
+#define BPM280_CONFIG_F2		0x1<<2
+#define BPM280_CONFIG_F4		0x2<<2
+#define BPM280_CONFIG_F8		0x3<<2
+#define BPM280_CONFIG_F16		0x4<<2
+
+
+#define BPM280_CTRL_MODE_SLEEP	0x0
+#define BPM280_CTRL_MODE_FORCE	0x1		/* on demand, goes to sleep after */
+#define BPM280_CTRL_MODE_NORMAL	0x3
+
+#define BPM280_MT_INIT		6400	/* max measure time of initial p + t in us */
+#define BPM280_MT			2300	/* max measure time of p or t in us */
+
+namespace bmp280
+{
+
+#pragma pack(push,1)
+	struct calibration_s {
+		uint16_t t1;
+		int16_t t2;
+		int16_t t3;
+
+		uint16_t p1;
+		int16_t p2;
+		int16_t p3;
+		int16_t p4;
+		int16_t p5;
+		int16_t p6;
+		int16_t p7;
+		int16_t p8;
+		int16_t p9;
+	}; //calibration data
+
+	struct data_s {
+			uint8_t p_msb;
+			uint8_t p_lsb;
+			uint8_t p_xlsb;
+
+			uint8_t t_msb;
+			uint8_t t_lsb;
+			uint8_t t_xlsb;
+		}; // data
+#pragma pack(pop)
+
+	struct fcalibration_s {
+			float t1;
+			float t2;
+			float t3;
+
+			float p1;
+			float p2;
+			float p3;
+			float p4;
+			float p5;
+			float p6;
+			float p7;
+			float p8;
+			float p9;
+		};
+
+	class IBMP280
+	{
+		public:
+			virtual ~IBMP280() = 0;
+
+			virtual bool is_external() = 0;
+			virtual int init() = 0;
+
+			virtual uint8_t get_reg(uint8_t addr) = 0; //read reg value
+			virtual int set_reg(uint8_t value, uint8_t addr) = 0; //write reg value
+			virtual bmp280::data_s* get_data(uint8_t addr) = 0; //bulk read of data into buffer, return same pointer
+			virtual bmp280::calibration_s* get_calibration(uint8_t addr) = 0; //bulk read of calibration data into buffer, return same pointer
+
+	};
+
+} /* namespace */
+
+/* interface factories */
+extern bmp280::IBMP280* bmp280_spi_interface(uint8_t busnum, uint8_t device, bool external);
+extern bmp280::IBMP280* bmp280_i2c_interface(uint8_t busnum, uint8_t device, bool external);
+typedef bmp280::IBMP280* (*BMP280_constructor)(uint8_t, uint8_t, bool);
diff --git a/src/drivers/bmp280/bmp280_spi.cpp b/src/drivers/bmp280/bmp280_spi.cpp
new file mode 100644
index 0000000000..017e411da5
--- /dev/null
+++ b/src/drivers/bmp280/bmp280_spi.cpp
@@ -0,0 +1,145 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2013 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file bmp280_spi.cpp
+ *
+ * SPI interface for BMP280
+ */
+
+#include <px4_config.h>
+
+#include <drivers/bmp280/bmp280.h>
+#include <drivers/device/spi.h>
+
+#include "board_config.h"
+
+/* SPI protocol address bits */
+#define DIR_READ			(1<<7)  //for set
+#define DIR_WRITE			~(1<<7) //for clear
+
+#if defined(PX4_SPIDEV_BARO) || defined(PX4_SPIDEV_EXT_BARO)
+
+#pragma pack(push,1)
+struct spi_data_s {
+	uint8_t addr;
+	struct bmp280::data_s data;
+};
+
+struct spi_calibration_s {
+	uint8_t addr;
+	struct bmp280::calibration_s cal;
+};
+#pragma pack(pop)
+
+class BMP280_SPI: public device::SPI, public bmp280::IBMP280
+{
+public:
+	BMP280_SPI(uint8_t bus, spi_dev_e device, bool external);
+	~BMP280_SPI();
+
+	bool is_external();
+	int init();
+
+	uint8_t get_reg(uint8_t addr);
+	int set_reg(uint8_t value, uint8_t addr);
+	bmp280::data_s* get_data(uint8_t addr);
+	bmp280::calibration_s* get_calibration(uint8_t addr);
+
+private:
+	spi_calibration_s _cal;
+	spi_data_s _data;
+	bool _external;
+};
+
+bmp280::IBMP280* bmp280_spi_interface(uint8_t busnum, uint8_t device, bool external)
+{
+	return new BMP280_SPI(busnum, (spi_dev_e)device, external);
+}
+
+BMP280_SPI::BMP280_SPI(uint8_t bus, spi_dev_e device, bool external) :
+		SPI("BMP280_SPI", nullptr, bus, device, SPIDEV_MODE3, 10 * 1000 * 1000) {
+	_external = external;
+}
+
+bmp280::IBMP280::~IBMP280() {
+}
+
+BMP280_SPI::~BMP280_SPI() {
+}
+
+
+bool BMP280_SPI::is_external() {
+	return _external;
+};
+
+int BMP280_SPI::init() {
+	return SPI::init();
+};
+
+uint8_t BMP280_SPI::get_reg(uint8_t addr) {
+	uint8_t cmd[2] = { (uint8_t)(addr|DIR_READ), 0}; //set MSB bit
+	transfer(&cmd[0],&cmd[0],2);
+
+	return cmd[1];
+}
+
+int BMP280_SPI::set_reg(uint8_t value, uint8_t addr) {
+	uint8_t cmd[2] = { (uint8_t)(addr&DIR_WRITE), value}; //clear MSB bit
+	return transfer(&cmd[0],nullptr,2);
+}
+
+bmp280::data_s* BMP280_SPI::get_data(uint8_t addr) {
+	_data.addr = (uint8_t)(addr|DIR_READ); //set MSB bit
+
+	if( transfer((uint8_t *)&_data,(uint8_t *)&_data, sizeof(struct spi_data_s)) == OK) {
+		return &(_data.data);
+	} else {
+		return nullptr;
+	}
+
+
+}
+
+bmp280::calibration_s* BMP280_SPI::get_calibration(uint8_t addr) {
+	_cal.addr = addr|DIR_READ;
+	if( transfer((uint8_t *)&_cal,(uint8_t *)&_cal, sizeof(struct spi_calibration_s)) == OK) {
+		return &(_cal.cal);
+	} else {
+		return nullptr;
+	}
+}
+
+
+
+#endif /* PX4_SPIDEV_BARO || PX4_SPIDEV_EXT_BARO */

From ea4896813c216febb1931ee60a7b67bb85e9f6ab Mon Sep 17 00:00:00 2001
From: Lok Tep <loktep@gmail.com>
Date: Wed, 4 May 2016 11:00:31 +0200
Subject: [PATCH 0093/1230] astyle formatted

---
 src/drivers/bmp280/CMakeLists.txt |  28 ++--
 src/drivers/bmp280/bmp280.cpp     | 233 ++++++++++++++++++------------
 src/drivers/bmp280/bmp280.h       | 101 ++++++-------
 src/drivers/bmp280/bmp280_spi.cpp |  52 ++++---
 4 files changed, 239 insertions(+), 175 deletions(-)

diff --git a/src/drivers/bmp280/CMakeLists.txt b/src/drivers/bmp280/CMakeLists.txt
index f35bd1b820..210c4d1e63 100644
--- a/src/drivers/bmp280/CMakeLists.txt
+++ b/src/drivers/bmp280/CMakeLists.txt
@@ -31,22 +31,22 @@
 #
 ############################################################################
 set(srcs
-	bmp280_spi.cpp
-	)
+    bmp280_spi.cpp
+   )
 
-if(${OS} STREQUAL "nuttx")
+if ($ {OS} STREQUAL "nuttx")
 	list(APPEND srcs
-		bmp280.cpp
-		)
-endif()
+	     bmp280.cpp
+	    )
+	endif()
 
-px4_add_module(
-	MODULE drivers__bmp280
-	MAIN bmp280
-	COMPILE_FLAGS
-		-Os
-	SRCS ${srcs}
-	DEPENDS
+	px4_add_module(
+		MODULE drivers__bmp280
+		MAIN bmp280
+		COMPILE_FLAGS
+		- Os
+		SRCS $ {srcs}
+		DEPENDS
 		platforms__common
 	)
-# vim: set noet ft=cmake fenc=utf-8 ff=unix : 
+# vim: set noet ft=cmake fenc=utf-8 ff=unix :
diff --git a/src/drivers/bmp280/bmp280.cpp b/src/drivers/bmp280/bmp280.cpp
index d516a3db35..0762e9885c 100644
--- a/src/drivers/bmp280/bmp280.cpp
+++ b/src/drivers/bmp280/bmp280.cpp
@@ -88,7 +88,7 @@ enum BMP280_BUS {
 class BMP280 : public device::CDev
 {
 public:
-	BMP280(bmp280::IBMP280 *interface, const char* path);
+	BMP280(bmp280::IBMP280 *interface, const char *path);
 	~BMP280();
 
 	virtual int		init();
@@ -102,7 +102,7 @@ public:
 	void			print_info();
 
 private:
-	bmp280::IBMP280*	_interface;
+	bmp280::IBMP280	*_interface;
 
 	uint8_t				_curr_ctrl;
 
@@ -126,7 +126,7 @@ private:
 	perf_counter_t		_comms_errors;
 	perf_counter_t		_buffer_overflows;
 
-	struct bmp280::calibration_s* _cal; //stored calibration constants
+	struct bmp280::calibration_s *_cal; //stored calibration constants
 	struct bmp280::fcalibration_s _fcal; //pre processed calibration constants
 
 	float			_P; /* in Pa */
@@ -148,7 +148,7 @@ private:
  */
 extern "C" __EXPORT int bmp280_main(int argc, char *argv[]);
 
-BMP280::BMP280(bmp280::IBMP280 *interface, const char* path) :
+BMP280::BMP280(bmp280::IBMP280 *interface, const char *path) :
 	CDev("BMP280", path),
 	_interface(interface),
 	_report_ticks(0),
@@ -172,12 +172,14 @@ BMP280::~BMP280()
 	/* make sure we are truly inactive */
 	stop_cycle();
 
-	if (_class_instance != -1)
+	if (_class_instance != -1) {
 		unregister_class_devname(get_devname(), _class_instance);
+	}
 
 	/* free any existing reports */
-	if (_reports != nullptr)
+	if (_reports != nullptr) {
 		delete _reports;
+	}
 
 	// free perf counters
 	perf_free(_sample_perf);
@@ -196,6 +198,7 @@ BMP280::init()
 	int ret;
 
 	ret = CDev::init();
+
 	if (ret != OK) {
 		DEVICE_DEBUG("CDev init failed");
 		return ret;
@@ -214,37 +217,37 @@ BMP280::init()
 	_class_instance = register_class_devname(BARO_BASE_DEVICE_PATH);
 
 	/* reset sensor */
-	_interface->set_reg(BPM280_VALUE_RESET,BPM280_ADDR_RESET);
+	_interface->set_reg(BPM280_VALUE_RESET, BPM280_ADDR_RESET);
 	usleep(10000);
 
 	/* check  id*/
-	if ( _interface->get_reg(BPM280_ADDR_ID) != BPM280_VALUE_ID ) {
-		warnx("id of your baro is not: 0x%02x",BPM280_VALUE_ID);
+	if (_interface->get_reg(BPM280_ADDR_ID) != BPM280_VALUE_ID) {
+		warnx("id of your baro is not: 0x%02x", BPM280_VALUE_ID);
 		return -EIO;
 	}
 
 	/* set config, recommended settings */
 	_curr_ctrl = BPM280_CTRL_P16 | BPM280_CTRL_T2;
-	_interface->set_reg(_curr_ctrl,BPM280_ADDR_CTRL);
-	_max_mesure_ticks = USEC2TICK( BPM280_MT_INIT + BPM280_MT*(16-1 + 2-1) );
-	_interface->set_reg(BPM280_CONFIG_F16,BPM280_ADDR_CONFIG);
+	_interface->set_reg(_curr_ctrl, BPM280_ADDR_CTRL);
+	_max_mesure_ticks = USEC2TICK(BPM280_MT_INIT + BPM280_MT * (16 - 1 + 2 - 1));
+	_interface->set_reg(BPM280_CONFIG_F16, BPM280_ADDR_CONFIG);
 
 	/* get calibration and pre process them*/
 	_cal = _interface->get_calibration(BPM280_ADDR_CAL);
 
-	_fcal.t1 =  _cal->t1 * powf(2,  4 );
+	_fcal.t1 =  _cal->t1 * powf(2,  4);
 	_fcal.t2 =  _cal->t2 * powf(2, -14);
 	_fcal.t3 =  _cal->t3 * powf(2, -34);
 
-	_fcal.p1 = _cal->p1            * (powf(2,  4 ) / -100000.0f);
+	_fcal.p1 = _cal->p1            * (powf(2,  4) / -100000.0f);
 	_fcal.p2 = _cal->p1 * _cal->p2 * (powf(2, -31) / -100000.0f);
 	_fcal.p3 = _cal->p1 * _cal->p3 * (powf(2, -51) / -100000.0f);
 
-	_fcal.p4 = _cal->p4 * powf(2,  4 ) - powf(2, 20);
+	_fcal.p4 = _cal->p4 * powf(2,  4) - powf(2, 20);
 	_fcal.p5 = _cal->p5 * powf(2, -14);
 	_fcal.p6 = _cal->p6 * powf(2, -31);
 
-	_fcal.p7 = _cal->p7 * powf(2, -4 );
+	_fcal.p7 = _cal->p7 * powf(2, -4);
 	_fcal.p8 = _cal->p8 * powf(2, -19) + 1.0f;
 	_fcal.p9 = _cal->p9 * powf(2, -35);
 
@@ -252,20 +255,20 @@ BMP280::init()
 	struct baro_report brp;
 	_reports->flush();
 
-	if ( measure() ) {
+	if (measure()) {
 		return -EIO;
 	}
 
-	usleep( TICK2USEC(_max_mesure_ticks) );
+	usleep(TICK2USEC(_max_mesure_ticks));
 
-	if ( collect() ) {
+	if (collect()) {
 		return -EIO;
 	}
 
 	_reports->get(&brp);
 
 	_baro_topic = orb_advertise_multi(ORB_ID(sensor_baro), &brp,
-				&_orb_class_instance, _interface->is_external()? ORB_PRIO_HIGH : ORB_PRIO_DEFAULT);
+					  &_orb_class_instance, _interface->is_external() ? ORB_PRIO_HIGH : ORB_PRIO_DEFAULT);
 
 	if (_baro_topic == nullptr) {
 		warnx("failed to create sensor_baro publication");
@@ -284,8 +287,9 @@ BMP280::read(struct file *filp, char *buffer, size_t buflen)
 	int ret = 0;
 
 	/* buffer must be large enough */
-	if (count < 1)
+	if (count < 1) {
 		return -ENOSPC;
+	}
 
 	/* if automatic measurement is enabled */
 	if (_report_ticks > 0) {
@@ -310,13 +314,13 @@ BMP280::read(struct file *filp, char *buffer, size_t buflen)
 
 	_reports->flush();
 
-	if ( measure() ) {
+	if (measure()) {
 		return -EIO;
 	}
 
-	usleep( TICK2USEC(_max_mesure_ticks) );
+	usleep(TICK2USEC(_max_mesure_ticks));
 
-	if ( collect() ) {
+	if (collect()) {
 		return -EIO;
 	}
 
@@ -334,9 +338,9 @@ BMP280::ioctl(struct file *filp, int cmd, unsigned long arg)
 
 	case SENSORIOCSPOLLRATE: {
 
-		unsigned ticks = 0;
+			unsigned ticks = 0;
 
-		switch (arg) {
+			switch (arg) {
 
 			case SENSOR_POLLRATE_MANUAL:
 				stop_cycle();
@@ -349,48 +353,57 @@ BMP280::ioctl(struct file *filp, int cmd, unsigned long arg)
 
 			case SENSOR_POLLRATE_MAX:
 			case SENSOR_POLLRATE_DEFAULT:
-					ticks = _max_mesure_ticks;
+				ticks = _max_mesure_ticks;
+
 			default: {
 					if (ticks == 0) {
 						ticks = USEC2TICK(USEC_PER_SEC / arg);
 					}
+
 					/* do we need to start internal polling? */
 					bool want_start = (_report_ticks == 0);
 
 					/* check against maximum rate */
-					if (ticks < _max_mesure_ticks)
+					if (ticks < _max_mesure_ticks) {
 						return -EINVAL;
+					}
 
 					_report_ticks = ticks;
-					if (want_start)
+
+					if (want_start) {
 						start_cycle();
+					}
 
 					return OK;
 				}
-		}
+			}
 
-		break;
-	}
+			break;
+		}
 
 	case SENSORIOCGPOLLRATE:
-		if (_report_ticks == 0)
+		if (_report_ticks == 0) {
 			return SENSOR_POLLRATE_MANUAL;
+		}
 
-		return (USEC_PER_SEC/USEC_PER_TICK/_report_ticks);
+		return (USEC_PER_SEC / USEC_PER_TICK / _report_ticks);
 
 	case SENSORIOCSQUEUEDEPTH: {
-		/* lower bound is mandatory, upper bound is a sanity check */
-		if ((arg < 1) || (arg > 100))
-			return -EINVAL;
+			/* lower bound is mandatory, upper bound is a sanity check */
+			if ((arg < 1) || (arg > 100)) {
+				return -EINVAL;
+			}
+
+			irqstate_t flags = irqsave();
+
+			if (!_reports->resize(arg)) {
+				irqrestore(flags);
+				return -ENOMEM;
+			}
 
-		irqstate_t flags = irqsave();
-		if (!_reports->resize(arg)) {
 			irqrestore(flags);
-			return -ENOMEM;
+			return OK;
 		}
-		irqrestore(flags);
-		return OK;
-	}
 
 	case SENSORIOCGQUEUEDEPTH:
 		return _reports->size();
@@ -405,8 +418,9 @@ BMP280::ioctl(struct file *filp, int cmd, unsigned long arg)
 	case BAROIOCSMSLPRESSURE:
 
 		/* range-check for sanity */
-		if ((arg < 80000) || (arg > 120000))
+		if ((arg < 80000) || (arg > 120000)) {
 			return -EINVAL;
+		}
 
 		_msl_pressure = arg;
 		return OK;
@@ -454,15 +468,16 @@ BMP280::cycle()
 		collect();
 		unsigned wait_gap = _report_ticks - _max_mesure_ticks;
 
-		if ( wait_gap != 0 ) {
-			work_queue(HPWORK,&_work,(worker_t)&BMP280::cycle_trampoline,this,wait_gap); //need to wait some time before new measurement
+		if (wait_gap != 0) {
+			work_queue(HPWORK, &_work, (worker_t)&BMP280::cycle_trampoline, this,
+				   wait_gap); //need to wait some time before new measurement
 			return;
 		}
 
 	}
 
 	measure();
-	work_queue(HPWORK,&_work,(worker_t)&BMP280::cycle_trampoline,this,_max_mesure_ticks);
+	work_queue(HPWORK, &_work, (worker_t)&BMP280::cycle_trampoline, this, _max_mesure_ticks);
 
 }
 
@@ -474,9 +489,9 @@ BMP280::measure()
 	perf_begin(_measure_perf);
 
 	/* start measure */
-	int ret = _interface->set_reg(_curr_ctrl | BPM280_CTRL_MODE_FORCE,BPM280_ADDR_CTRL);
+	int ret = _interface->set_reg(_curr_ctrl | BPM280_CTRL_MODE_FORCE, BPM280_ADDR_CTRL);
 
-	if ( ret != OK) {
+	if (ret != OK) {
 		perf_count(_comms_errors);
 		perf_cancel(_measure_perf);
 		return -EIO;
@@ -497,23 +512,24 @@ BMP280::collect()
 	struct baro_report report;
 	/* this should be fairly close to the end of the conversion, so the best approximation of the time */
 	report.timestamp = hrt_absolute_time();
-    report.error_count = perf_event_count(_comms_errors);
+	report.error_count = perf_event_count(_comms_errors);
 
-    bmp280::data_s* data = _interface->get_data(BPM280_ADDR_DATA);
-    if (data == nullptr) {
-    	perf_count(_comms_errors);
-    	perf_cancel(_sample_perf);
-    	return -EIO;
-    }
+	bmp280::data_s *data = _interface->get_data(BPM280_ADDR_DATA);
 
-    //convert data to number 20 bit
-    uint32_t p_raw =  data->p_msb<<12 | data->p_lsb<<4 | data->p_xlsb>>4;
-    uint32_t t_raw =  data->t_msb<<12 | data->t_lsb<<4 | data->t_xlsb>>4;
+	if (data == nullptr) {
+		perf_count(_comms_errors);
+		perf_cancel(_sample_perf);
+		return -EIO;
+	}
 
-    // Temperature
+	//convert data to number 20 bit
+	uint32_t p_raw =  data->p_msb << 12 | data->p_lsb << 4 | data->p_xlsb >> 4;
+	uint32_t t_raw =  data->t_msb << 12 | data->t_lsb << 4 | data->t_xlsb >> 4;
+
+	// Temperature
 	float ofs = (float) t_raw - _fcal.t1;
 	float t_fine = (ofs * _fcal.t3 + _fcal.t2) * ofs;
-	_T = t_fine * (1.0f/5120.0f);
+	_T = t_fine * (1.0f / 5120.0f);
 
 	// Pressure
 	float tf = t_fine - 128000.0f;
@@ -525,7 +541,7 @@ BMP280::collect()
 
 
 	report.temperature = _T;
-	report.pressure = _P/100.0f; // to mbar
+	report.pressure = _P / 100.0f; // to mbar
 
 
 	/* altitude calculations based on http://www.kansasflyer.org/index.asp?nav=Avi&sec=Alti&tab=Theory&pg=1 */
@@ -603,13 +619,13 @@ struct bmp280_bus_option {
 	{ BMP280_BUS_SPI_EXTERNAL, "/dev/bmp280_spi_ext", &bmp280_spi_interface, PX4_SPI_BUS_EXT, PX4_SPIDEV_EXT_BARO , true , NULL },
 #endif
 #ifdef PX4_SPIDEV_BARO
-	{ BMP280_BUS_SPI_INTERNAL, "/dev/bmp280_spi_int", &bmp280_spi_interface, PX4_SPI_BUS_SENSORS,PX4_SPIDEV_BARO, false ,NULL },
+	{ BMP280_BUS_SPI_INTERNAL, "/dev/bmp280_spi_int", &bmp280_spi_interface, PX4_SPI_BUS_SENSORS, PX4_SPIDEV_BARO, false , NULL },
 #endif
 #ifdef PX4_I2C_OBDEV_BMP280
 	{ BMP280_BUS_I2C_INTERNAL, "/dev/bmp280_i2c_int", nullptr, PX4_I2C_BUS_ONBOARD, PX4_I2C_OBDEV_BMP280, false, NULL },
 #endif
 #if defined(PX4_I2C_BUS_EXPANSION) && defined(PX4_I2C_EXT_OBDEV_BMP280)
-	{ BMP280_BUS_I2C_EXTERNAL, "/dev/bmp280_i2c_ext", nullptr, PX4_I2C_BUS_EXPANSION,PX4_I2C_EXT_OBDEV_BMP280 , true , NULL },
+	{ BMP280_BUS_I2C_EXTERNAL, "/dev/bmp280_i2c_ext", nullptr, PX4_I2C_BUS_EXPANSION, PX4_I2C_EXT_OBDEV_BMP280 , true , NULL },
 #endif
 };
 #define NUM_BUS_OPTIONS (sizeof(bus_options)/sizeof(bus_options[0]))
@@ -630,10 +646,12 @@ void	usage();
 bool
 start_bus(struct bmp280_bus_option &bus)
 {
-	if (bus.dev != nullptr)
-		errx(1,"bus option already started");
+	if (bus.dev != nullptr) {
+		errx(1, "bus option already started");
+	}
 
 	bmp280::IBMP280 *interface = bus.interface_constructor(bus.busnum, bus.device, bus.external);
+
 	if (interface->init() != OK) {
 		delete interface;
 		warnx("no device on bus %u", (unsigned)bus.busid);
@@ -641,6 +659,7 @@ start_bus(struct bmp280_bus_option &bus)
 	}
 
 	bus.dev = new BMP280(interface, bus.devpath);
+
 	if (bus.dev != nullptr && OK != bus.dev->init()) {
 		delete bus.dev;
 		bus.dev = NULL;
@@ -653,6 +672,7 @@ start_bus(struct bmp280_bus_option &bus)
 	if (fd == -1) {
 		errx(1, "can't open baro device");
 	}
+
 	if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) {
 		close(fd);
 		errx(1, "failed setting default poll rate");
@@ -675,20 +695,23 @@ start(enum BMP280_BUS busid)
 	uint8_t i;
 	bool started = false;
 
-	for (i=0; i<NUM_BUS_OPTIONS; i++) {
+	for (i = 0; i < NUM_BUS_OPTIONS; i++) {
 		if (busid == BMP280_BUS_ALL && bus_options[i].dev != NULL) {
 			// this device is already started
 			continue;
 		}
+
 		if (busid != BMP280_BUS_ALL && bus_options[i].busid != busid) {
 			// not the one that is asked for
 			continue;
 		}
+
 		started |= start_bus(bus_options[i]);
 	}
 
-	if (!started)
+	if (!started) {
 		errx(1, "driver start failed");
+	}
 
 	// one or more drivers started OK
 	exit(0);
@@ -700,13 +723,14 @@ start(enum BMP280_BUS busid)
  */
 struct bmp280_bus_option &find_bus(enum BMP280_BUS busid)
 {
-	for (uint8_t i=0; i<NUM_BUS_OPTIONS; i++) {
+	for (uint8_t i = 0; i < NUM_BUS_OPTIONS; i++) {
 		if ((busid == BMP280_BUS_ALL ||
 		     busid == bus_options[i].busid) && bus_options[i].dev != NULL) {
 			return bus_options[i];
 		}
 	}
-	errx(1,"bus %u not started", (unsigned)busid);
+
+	errx(1, "bus %u not started", (unsigned)busid);
 }
 
 /**
@@ -725,14 +749,17 @@ test(enum BMP280_BUS busid)
 	int fd;
 
 	fd = open(bus.devpath, O_RDONLY);
-	if (fd < 0)
+
+	if (fd < 0) {
 		err(1, "open failed (try 'bmp280 start' if the driver is not running)");
+	}
 
 	/* do a simple demand read */
 	sz = read(fd, &report, sizeof(report));
 
-	if (sz != sizeof(report))
+	if (sz != sizeof(report)) {
 		err(1, "immediate read failed");
+	}
 
 	warnx("single read");
 	warnx("pressure:    %10.4f", (double)report.pressure);
@@ -741,12 +768,14 @@ test(enum BMP280_BUS busid)
 	warnx("time:        %lld", report.timestamp);
 
 	/* set the queue depth to 10 */
-	if (OK != ioctl(fd, SENSORIOCSQUEUEDEPTH, 10))
+	if (OK != ioctl(fd, SENSORIOCSQUEUEDEPTH, 10)) {
 		errx(1, "failed to set queue depth");
+	}
 
 	/* start the sensor polling at 2Hz */
-	if (OK != ioctl(fd, SENSORIOCSPOLLRATE, 2))
+	if (OK != ioctl(fd, SENSORIOCSPOLLRATE, 2)) {
 		errx(1, "failed to set 2Hz poll rate");
+	}
 
 	/* read the sensor 5x and report each value */
 	for (unsigned i = 0; i < 5; i++) {
@@ -757,14 +786,16 @@ test(enum BMP280_BUS busid)
 		fds.events = POLLIN;
 		ret = poll(&fds, 1, 2000);
 
-		if (ret != 1)
+		if (ret != 1) {
 			errx(1, "timed out waiting for sensor data");
+		}
 
 		/* now go get it */
 		sz = read(fd, &report, sizeof(report));
 
-		if (sz != sizeof(report))
+		if (sz != sizeof(report)) {
 			err(1, "periodic read failed");
+		}
 
 		warnx("periodic read %u", i);
 		warnx("pressure:    %10.4f", (double)report.pressure);
@@ -787,14 +818,18 @@ reset(enum BMP280_BUS busid)
 	int fd;
 
 	fd = open(bus.devpath, O_RDONLY);
-	if (fd < 0)
+
+	if (fd < 0) {
 		err(1, "failed ");
+	}
 
-	if (ioctl(fd, SENSORIOCRESET, 0) < 0)
+	if (ioctl(fd, SENSORIOCRESET, 0) < 0) {
 		err(1, "driver reset failed");
+	}
 
-	if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0)
+	if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) {
 		err(1, "driver poll restart failed");
+	}
 
 	exit(0);
 }
@@ -805,13 +840,15 @@ reset(enum BMP280_BUS busid)
 void
 info()
 {
-	for (uint8_t i=0; i<NUM_BUS_OPTIONS; i++) {
+	for (uint8_t i = 0; i < NUM_BUS_OPTIONS; i++) {
 		struct bmp280_bus_option &bus = bus_options[i];
+
 		if (bus.dev != nullptr) {
 			warnx("%s", bus.devpath);
 			bus.dev->print_info();
 		}
 	}
+
 	exit(0);
 }
 
@@ -830,12 +867,14 @@ calibrate(unsigned altitude, enum BMP280_BUS busid)
 
 	fd = open(bus.devpath, O_RDONLY);
 
-	if (fd < 0)
+	if (fd < 0) {
 		err(1, "open failed (try 'bmp280 start' if the driver is not running)");
+	}
 
 	/* start the sensor polling at max */
-	if (OK != ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_MAX))
+	if (OK != ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_MAX)) {
 		errx(1, "failed to set poll rate");
+	}
 
 	/* average a few measurements */
 	pressure = 0.0f;
@@ -850,14 +889,16 @@ calibrate(unsigned altitude, enum BMP280_BUS busid)
 		fds.events = POLLIN;
 		ret = poll(&fds, 1, 1000);
 
-		if (ret != 1)
+		if (ret != 1) {
 			errx(1, "timed out waiting for sensor data");
+		}
 
 		/* now go get it */
 		sz = read(fd, &report, sizeof(report));
 
-		if (sz != sizeof(report))
+		if (sz != sizeof(report)) {
 			err(1, "sensor read failed");
+		}
 
 		pressure += report.pressure;
 	}
@@ -880,8 +921,9 @@ calibrate(unsigned altitude, enum BMP280_BUS busid)
 	/* save as integer Pa */
 	p1 *= 1000.0f;
 
-	if (ioctl(fd, BAROIOCSMSLPRESSURE, (unsigned long)p1) != OK)
+	if (ioctl(fd, BAROIOCSMSLPRESSURE, (unsigned long)p1) != OK) {
 		err(1, "BAROIOCSMSLPRESSURE");
+	}
 
 	close(fd);
 	exit(0);
@@ -913,16 +955,20 @@ bmp280_main(int argc, char *argv[])
 			busid = BMP280_BUS_I2C_EXTERNAL;
 			errx(1, "not supported yet");
 			break;
+
 		case 'I':
 			busid = BMP280_BUS_I2C_INTERNAL;
 			errx(1, "not supported yet");
 			break;
+
 		case 'S':
 			busid = BMP280_BUS_SPI_EXTERNAL;
 			break;
+
 		case 's':
 			busid = BMP280_BUS_SPI_INTERNAL;
 			break;
+
 		default:
 			bmp280::usage();
 			exit(0);
@@ -934,35 +980,40 @@ bmp280_main(int argc, char *argv[])
 	/*
 	 * Start/load the driver.
 	 */
-	if (!strcmp(verb, "start"))
+	if (!strcmp(verb, "start")) {
 		bmp280::start(busid);
+	}
 
 	/*
 	 * Test the driver/device.
 	 */
-	if (!strcmp(verb, "test"))
+	if (!strcmp(verb, "test")) {
 		bmp280::test(busid);
+	}
 
 	/*
 	 * Reset the driver.
 	 */
-	if (!strcmp(verb, "reset"))
+	if (!strcmp(verb, "reset")) {
 		bmp280::reset(busid);
+	}
 
 	/*
 	 * Print driver information.
 	 */
-	if (!strcmp(verb, "info"))
+	if (!strcmp(verb, "info")) {
 		bmp280::info();
+	}
 
 	/*
 	 * Perform MSL pressure calibration given an altitude in metres
 	 */
 	if (!strcmp(verb, "calibrate")) {
-		if (argc < 2)
+		if (argc < 2) {
 			errx(1, "missing altitude");
+		}
 
-		long altitude = strtol(argv[optind+1], nullptr, 10);
+		long altitude = strtol(argv[optind + 1], nullptr, 10);
 
 		bmp280::calibrate(altitude, busid);
 	}
diff --git a/src/drivers/bmp280/bmp280.h b/src/drivers/bmp280/bmp280.h
index 44155b12fc..1b5b43db0b 100644
--- a/src/drivers/bmp280/bmp280.h
+++ b/src/drivers/bmp280/bmp280.h
@@ -85,67 +85,68 @@ namespace bmp280
 {
 
 #pragma pack(push,1)
-	struct calibration_s {
-		uint16_t t1;
-		int16_t t2;
-		int16_t t3;
+struct calibration_s {
+	uint16_t t1;
+	int16_t t2;
+	int16_t t3;
 
-		uint16_t p1;
-		int16_t p2;
-		int16_t p3;
-		int16_t p4;
-		int16_t p5;
-		int16_t p6;
-		int16_t p7;
-		int16_t p8;
-		int16_t p9;
-	}; //calibration data
+	uint16_t p1;
+	int16_t p2;
+	int16_t p3;
+	int16_t p4;
+	int16_t p5;
+	int16_t p6;
+	int16_t p7;
+	int16_t p8;
+	int16_t p9;
+}; //calibration data
 
-	struct data_s {
-			uint8_t p_msb;
-			uint8_t p_lsb;
-			uint8_t p_xlsb;
+struct data_s {
+	uint8_t p_msb;
+	uint8_t p_lsb;
+	uint8_t p_xlsb;
 
-			uint8_t t_msb;
-			uint8_t t_lsb;
-			uint8_t t_xlsb;
-		}; // data
+	uint8_t t_msb;
+	uint8_t t_lsb;
+	uint8_t t_xlsb;
+}; // data
 #pragma pack(pop)
 
-	struct fcalibration_s {
-			float t1;
-			float t2;
-			float t3;
+struct fcalibration_s {
+	float t1;
+	float t2;
+	float t3;
 
-			float p1;
-			float p2;
-			float p3;
-			float p4;
-			float p5;
-			float p6;
-			float p7;
-			float p8;
-			float p9;
-		};
+	float p1;
+	float p2;
+	float p3;
+	float p4;
+	float p5;
+	float p6;
+	float p7;
+	float p8;
+	float p9;
+};
 
-	class IBMP280
-	{
-		public:
-			virtual ~IBMP280() = 0;
+class IBMP280
+{
+public:
+	virtual ~IBMP280() = 0;
 
-			virtual bool is_external() = 0;
-			virtual int init() = 0;
+	virtual bool is_external() = 0;
+	virtual int init() = 0;
 
-			virtual uint8_t get_reg(uint8_t addr) = 0; //read reg value
-			virtual int set_reg(uint8_t value, uint8_t addr) = 0; //write reg value
-			virtual bmp280::data_s* get_data(uint8_t addr) = 0; //bulk read of data into buffer, return same pointer
-			virtual bmp280::calibration_s* get_calibration(uint8_t addr) = 0; //bulk read of calibration data into buffer, return same pointer
+	virtual uint8_t get_reg(uint8_t addr) = 0; //read reg value
+	virtual int set_reg(uint8_t value, uint8_t addr) = 0; //write reg value
+	virtual bmp280::data_s *get_data(uint8_t addr) = 0; //bulk read of data into buffer, return same pointer
+	virtual bmp280::calibration_s *get_calibration(uint8_t addr) =
+		0; //bulk read of calibration data into buffer, return same pointer
 
-	};
+};
 
 } /* namespace */
 
 /* interface factories */
-extern bmp280::IBMP280* bmp280_spi_interface(uint8_t busnum, uint8_t device, bool external);
-extern bmp280::IBMP280* bmp280_i2c_interface(uint8_t busnum, uint8_t device, bool external);
-typedef bmp280::IBMP280* (*BMP280_constructor)(uint8_t, uint8_t, bool);
+extern bmp280::IBMP280 *bmp280_spi_interface(uint8_t busnum, uint8_t device, bool external);
+extern bmp280::IBMP280 *bmp280_i2c_interface(uint8_t busnum, uint8_t device, bool external);
+typedef bmp280::IBMP280 *(*BMP280_constructor)(uint8_t, uint8_t, bool);
diff --git a/src/drivers/bmp280/bmp280_spi.cpp b/src/drivers/bmp280/bmp280_spi.cpp
index 017e411da5..406a5d8a8c 100644
--- a/src/drivers/bmp280/bmp280_spi.cpp
+++ b/src/drivers/bmp280/bmp280_spi.cpp
@@ -73,8 +73,8 @@ public:
 
 	uint8_t get_reg(uint8_t addr);
 	int set_reg(uint8_t value, uint8_t addr);
-	bmp280::data_s* get_data(uint8_t addr);
-	bmp280::calibration_s* get_calibration(uint8_t addr);
+	bmp280::data_s *get_data(uint8_t addr);
+	bmp280::calibration_s *get_calibration(uint8_t addr);
 
 private:
 	spi_calibration_s _cal;
@@ -82,48 +82,57 @@ private:
 	bool _external;
 };
 
-bmp280::IBMP280* bmp280_spi_interface(uint8_t busnum, uint8_t device, bool external)
+bmp280::IBMP280 *bmp280_spi_interface(uint8_t busnum, uint8_t device, bool external)
 {
 	return new BMP280_SPI(busnum, (spi_dev_e)device, external);
 }
 
 BMP280_SPI::BMP280_SPI(uint8_t bus, spi_dev_e device, bool external) :
-		SPI("BMP280_SPI", nullptr, bus, device, SPIDEV_MODE3, 10 * 1000 * 1000) {
+	SPI("BMP280_SPI", nullptr, bus, device, SPIDEV_MODE3, 10 * 1000 * 1000)
+{
 	_external = external;
 }
 
-bmp280::IBMP280::~IBMP280() {
+bmp280::IBMP280::~IBMP280()
+{
 }
 
-BMP280_SPI::~BMP280_SPI() {
+BMP280_SPI::~BMP280_SPI()
+{
 }
 
 
-bool BMP280_SPI::is_external() {
+bool BMP280_SPI::is_external()
+{
 	return _external;
 };
 
-int BMP280_SPI::init() {
+int BMP280_SPI::init()
+{
 	return SPI::init();
 };
 
-uint8_t BMP280_SPI::get_reg(uint8_t addr) {
-	uint8_t cmd[2] = { (uint8_t)(addr|DIR_READ), 0}; //set MSB bit
-	transfer(&cmd[0],&cmd[0],2);
+uint8_t BMP280_SPI::get_reg(uint8_t addr)
+{
+	uint8_t cmd[2] = { (uint8_t)(addr | DIR_READ), 0}; //set MSB bit
+	transfer(&cmd[0], &cmd[0], 2);
 
 	return cmd[1];
 }
 
-int BMP280_SPI::set_reg(uint8_t value, uint8_t addr) {
-	uint8_t cmd[2] = { (uint8_t)(addr&DIR_WRITE), value}; //clear MSB bit
-	return transfer(&cmd[0],nullptr,2);
+int BMP280_SPI::set_reg(uint8_t value, uint8_t addr)
+{
+	uint8_t cmd[2] = { (uint8_t)(addr & DIR_WRITE), value}; //clear MSB bit
+	return transfer(&cmd[0], nullptr, 2);
 }
 
-bmp280::data_s* BMP280_SPI::get_data(uint8_t addr) {
-	_data.addr = (uint8_t)(addr|DIR_READ); //set MSB bit
+bmp280::data_s *BMP280_SPI::get_data(uint8_t addr)
+{
+	_data.addr = (uint8_t)(addr | DIR_READ); //set MSB bit
 
-	if( transfer((uint8_t *)&_data,(uint8_t *)&_data, sizeof(struct spi_data_s)) == OK) {
+	if (transfer((uint8_t *)&_data, (uint8_t *)&_data, sizeof(struct spi_data_s)) == OK) {
 		return &(_data.data);
+
 	} else {
 		return nullptr;
 	}
@@ -131,10 +140,13 @@ bmp280::data_s* BMP280_SPI::get_data(uint8_t addr) {
 
 }
 
-bmp280::calibration_s* BMP280_SPI::get_calibration(uint8_t addr) {
-	_cal.addr = addr|DIR_READ;
-	if( transfer((uint8_t *)&_cal,(uint8_t *)&_cal, sizeof(struct spi_calibration_s)) == OK) {
+bmp280::calibration_s *BMP280_SPI::get_calibration(uint8_t addr)
+{
+	_cal.addr = addr | DIR_READ;
+
+	if (transfer((uint8_t *)&_cal, (uint8_t *)&_cal, sizeof(struct spi_calibration_s)) == OK) {
 		return &(_cal.cal);
+
 	} else {
 		return nullptr;
 	}

From 13a819730d3163a35d92c17f8a96f910d04d01b3 Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Fri, 13 May 2016 13:41:04 +0200
Subject: [PATCH 0094/1230] BMP280: Cleanup to make it compile

---
 src/drivers/bmp280/CMakeLists.txt | 29 +++++++++++------------------
 src/drivers/bmp280/bmp280.cpp     |  2 +-
 src/drivers/bmp280/bmp280.h       |  2 +-
 src/drivers/bmp280/bmp280_spi.cpp |  2 +-
 4 files changed, 14 insertions(+), 21 deletions(-)

diff --git a/src/drivers/bmp280/CMakeLists.txt b/src/drivers/bmp280/CMakeLists.txt
index 210c4d1e63..1199aa1d75 100644
--- a/src/drivers/bmp280/CMakeLists.txt
+++ b/src/drivers/bmp280/CMakeLists.txt
@@ -1,6 +1,6 @@
 ############################################################################
 #
-#   Copyright (c) 2015 PX4 Development Team. All rights reserved.
+#   Copyright (c) 2016 PX4 Development Team. All rights reserved.
 #
 # Redistribution and use in source and binary forms, with or without
 # modification, are permitted provided that the following conditions
@@ -30,23 +30,16 @@
 # POSSIBILITY OF SUCH DAMAGE.
 #
 ############################################################################
-set(srcs
-    bmp280_spi.cpp
-   )
 
-if ($ {OS} STREQUAL "nuttx")
-	list(APPEND srcs
-	     bmp280.cpp
-	    )
-	endif()
-
-	px4_add_module(
-		MODULE drivers__bmp280
-		MAIN bmp280
-		COMPILE_FLAGS
-		- Os
-		SRCS $ {srcs}
-		DEPENDS
+px4_add_module(
+	MODULE drivers__bmp280
+	MAIN bmp280
+	COMPILE_FLAGS
+		-Os
+	SRCS
+		bmp280_spi.cpp
+		bmp280.cpp
+	DEPENDS
 		platforms__common
 	)
-# vim: set noet ft=cmake fenc=utf-8 ff=unix :
+# vim: set noet ft=cmake fenc=utf-8 ff=unix : 
diff --git a/src/drivers/bmp280/bmp280.cpp b/src/drivers/bmp280/bmp280.cpp
index 0762e9885c..6c7af54d5a 100644
--- a/src/drivers/bmp280/bmp280.cpp
+++ b/src/drivers/bmp280/bmp280.cpp
@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2012-2015 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2012-2016 PX4 Development Team. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
diff --git a/src/drivers/bmp280/bmp280.h b/src/drivers/bmp280/bmp280.h
index 1b5b43db0b..a2399bf395 100644
--- a/src/drivers/bmp280/bmp280.h
+++ b/src/drivers/bmp280/bmp280.h
@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (C) 2012-2013 PX4 Development Team. All rights reserved.
+ *   Copyright (C) 2012-2016 PX4 Development Team. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
diff --git a/src/drivers/bmp280/bmp280_spi.cpp b/src/drivers/bmp280/bmp280_spi.cpp
index 406a5d8a8c..a5b2b08d01 100644
--- a/src/drivers/bmp280/bmp280_spi.cpp
+++ b/src/drivers/bmp280/bmp280_spi.cpp
@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2013 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2016 PX4 Development Team. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions

From 637396359d1975f8043b6128c6abe12bad1a4e04 Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Fri, 13 May 2016 13:41:16 +0200
Subject: [PATCH 0095/1230] BMA180: Fix compilation

---
 src/drivers/bma180/bma180.cpp | 8 +++++---
 1 file changed, 5 insertions(+), 3 deletions(-)

diff --git a/src/drivers/bma180/bma180.cpp b/src/drivers/bma180/bma180.cpp
index 17757aaf70..ee895d8adf 100644
--- a/src/drivers/bma180/bma180.cpp
+++ b/src/drivers/bma180/bma180.cpp
@@ -65,6 +65,8 @@
 #include <drivers/drv_accel.h>
 #include <drivers/device/ringbuffer.h>
 
+#define ACCEL_DEVICE_PATH	"/dev/bma180"
+
 
 /* oddly, ERROR is not defined for c++ */
 #ifdef ERROR
@@ -147,7 +149,7 @@ private:
 	struct hrt_call		_call;
 	unsigned		_call_interval;
 
-	RingBuffer		*_reports;
+	ringbuffer::RingBuffer		*_reports;
 
 	struct accel_calibration_s	_accel_scale;
 	float			_accel_range_scale;
@@ -281,7 +283,7 @@ BMA180::init()
 	}
 
 	/* allocate basic report buffers */
-	_reports = new RingBuffer(2, sizeof(accel_report));
+	_reports = new ringbuffer::RingBuffer(2, sizeof(accel_report));
 
 	if (_reports == nullptr) {
 		goto out;
@@ -788,7 +790,7 @@ start()
 	}
 
 	/* create the driver */
-	g_dev = new BMA180(1 /* XXX magic number */, (spi_dev_e)PX4_SPIDEV_ACCEL);
+	g_dev = new BMA180(1 /* XXX magic number */, (spi_dev_e)PX4_SPIDEV_BMA);
 
 	if (g_dev == nullptr) {
 		goto fail;

From 29d6c95ec48235f2373fb6d2eb0429aa2e647074 Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Fri, 13 May 2016 13:41:36 +0200
Subject: [PATCH 0096/1230] FMUv4: Build all Bosch drivers so they get built at
 least for one target

---
 cmake/configs/nuttx_px4fmu-v4_default.cmake | 3 +++
 src/drivers/boards/px4fmu-v4/board_config.h | 4 +++-
 2 files changed, 6 insertions(+), 1 deletion(-)

diff --git a/cmake/configs/nuttx_px4fmu-v4_default.cmake b/cmake/configs/nuttx_px4fmu-v4_default.cmake
index 7c54edf15c..eb4713eb50 100644
--- a/cmake/configs/nuttx_px4fmu-v4_default.cmake
+++ b/cmake/configs/nuttx_px4fmu-v4_default.cmake
@@ -45,6 +45,9 @@ set(config_module_list
 	drivers/bst
 	drivers/snapdragon_rc_pwm
 	drivers/lis3mdl
+	drivers/bmp280
+	drivers/bma180
+	drivers/bmi160
 
 	#
 	# System commands
diff --git a/src/drivers/boards/px4fmu-v4/board_config.h b/src/drivers/boards/px4fmu-v4/board_config.h
index c1e214f7d3..a375609318 100644
--- a/src/drivers/boards/px4fmu-v4/board_config.h
+++ b/src/drivers/boards/px4fmu-v4/board_config.h
@@ -126,11 +126,13 @@ __BEGIN_DECLS
 
 /* Use these in place of the spi_dev_e enumeration to select a specific SPI device on SPI1 */
 #define PX4_SPIDEV_GYRO			1
-#define PX4_SPIDEV_ACCEL_MAG	2
+#define PX4_SPIDEV_ACCEL_MAG		2
 #define PX4_SPIDEV_MPU			4
 #define PX4_SPIDEV_HMC			5
 #define PX4_SPIDEV_ICM			6
 #define PX4_SPIDEV_LIS			7
+#define PX4_SPIDEV_BMI			8
+#define PX4_SPIDEV_BMA			9
 
 /* onboard MS5611 and FRAM are both on bus SPI2
  * spi_dev_e:SPIDEV_FLASH has the value 2 and is used in the NuttX ramtron driver

From ae859ea7fda7ef0b0010a219572e6bcf1a3ebafb Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Fri, 13 May 2016 15:10:51 +0200
Subject: [PATCH 0097/1230] Update MAVLink v1.0 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 4e9682c6e8..d8ec54a69f 160000
--- a/mavlink/include/mavlink/v1.0
+++ b/mavlink/include/mavlink/v1.0
@@ -1 +1 @@
-Subproject commit 4e9682c6e8130825f69b0c2c6392a9d8154fffbe
+Subproject commit d8ec54a69f2d35f0e46c0115628d82d113cc5974

From 058bc139fe979dfc8d6a096e24e38c661f0ba214 Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Fri, 13 May 2016 15:13:51 +0200
Subject: [PATCH 0098/1230] Update MAVLink 2.0 to include RTCM messages

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

diff --git a/mavlink/include/mavlink/v2.0 b/mavlink/include/mavlink/v2.0
index 6504af3300..eec77a089b 160000
--- a/mavlink/include/mavlink/v2.0
+++ b/mavlink/include/mavlink/v2.0
@@ -1 +1 @@
-Subproject commit 6504af330080d104ceb674780f04bd0462345c23
+Subproject commit eec77a089b75216ee28c157dca2993c4d12209fd

From 7c7d94e3dc308ef28fb34d4f524ecc6cccbee295 Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Fri, 13 May 2016 22:04:28 +0200
Subject: [PATCH 0099/1230] Build LPE in all configs

---
 cmake/configs/posix_sitl_default.cmake | 1 +
 cmake/configs/posix_sitl_lpe.cmake     | 4 ----
 2 files changed, 1 insertion(+), 4 deletions(-)

diff --git a/cmake/configs/posix_sitl_default.cmake b/cmake/configs/posix_sitl_default.cmake
index 365397341a..5d3f364be8 100644
--- a/cmake/configs/posix_sitl_default.cmake
+++ b/cmake/configs/posix_sitl_default.cmake
@@ -48,6 +48,7 @@ set(config_module_list
 	modules/navigator
 	modules/param
 	modules/position_estimator_inav
+	modules/local_position_estimator
 	modules/sdlog2
 	modules/sensors
 	modules/simulator
diff --git a/cmake/configs/posix_sitl_lpe.cmake b/cmake/configs/posix_sitl_lpe.cmake
index c264466f9d..c098b72bf0 100644
--- a/cmake/configs/posix_sitl_lpe.cmake
+++ b/cmake/configs/posix_sitl_lpe.cmake
@@ -1,9 +1,5 @@
 include(cmake/configs/posix_sitl_default.cmake)
 
-list(APPEND config_module_list
-	modules/local_position_estimator
-	)
-
 set(config_sitl_rcS
 	posix-configs/SITL/init/rcS_lpe
 	)

From 503966165bf0322a521bebee505b13ec90d25ec4 Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Fri, 13 May 2016 22:05:07 +0200
Subject: [PATCH 0100/1230] Switch from EKF2 to LPE since SITL s is not any
 more a bearable workflow with EKF2 init lag

---
 posix-configs/SITL/init/rcS_jmavsim_iris | 3 ++-
 1 file changed, 2 insertions(+), 1 deletion(-)

diff --git a/posix-configs/SITL/init/rcS_jmavsim_iris b/posix-configs/SITL/init/rcS_jmavsim_iris
index ab0e045afc..a80931b016 100644
--- a/posix-configs/SITL/init/rcS_jmavsim_iris
+++ b/posix-configs/SITL/init/rcS_jmavsim_iris
@@ -54,7 +54,8 @@ sensors start
 commander start
 land_detector start multicopter
 navigator start
-ekf2 start
+attitude_estimator_q start
+local_position_estimator start
 mc_pos_control start
 mc_att_control start
 mixer load /dev/pwm_output0 ../../../../ROMFS/px4fmu_common/mixers/quad_x.main.mix

From 7d66435546fbd6af2ee9c61f00e35cbb37d6082e Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Fri, 13 May 2016 22:05:46 +0200
Subject: [PATCH 0101/1230] Commander: Fix the takeoff and land commands. Fixes
 #4516

---
 src/modules/commander/commander.cpp | 35 ++++++++++++++++-------------
 1 file changed, 19 insertions(+), 16 deletions(-)

diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp
index 8f6452ee85..0d96672654 100644
--- a/src/modules/commander/commander.cpp
+++ b/src/modules/commander/commander.cpp
@@ -437,15 +437,17 @@ int commander_main(int argc, char *argv[])
 				cmd.target_component = status.component_id;
 
 				cmd.command = vehicle_command_s::VEHICLE_CMD_NAV_TAKEOFF;
-				// cmd.param1 = 0.25f; /* minimum pitch */
-				// /* param 2-3 unused */
-				// cmd.param4 = home_position.yaw;
-				// cmd.param5 = home_position.lat;
-				// cmd.param6 = home_position.lon;
-				// cmd.param7 = home_position.alt;
+				cmd.param1 = NAN; /* minimum pitch */
+				/* param 2-3 unused */
+				cmd.param2 = NAN;
+				cmd.param3 = NAN;
+				cmd.param4 = NAN;
+				cmd.param5 = NAN;
+				cmd.param6 = NAN;
+				cmd.param7 = NAN;
 
-				// XXX inspect use of publication handle
-				(void)orb_advertise(ORB_ID(vehicle_command), &cmd);
+				orb_advert_t h = orb_advertise(ORB_ID(vehicle_command), &cmd);
+				(void)orb_unadvertise(h);
 
 			} else {
 				warnx("arming failed");
@@ -465,15 +467,16 @@ int commander_main(int argc, char *argv[])
 		cmd.target_component = status.component_id;
 
 		cmd.command = vehicle_command_s::VEHICLE_CMD_NAV_LAND;
-		// cmd.param1 = 0.25f; /* minimum pitch */
-		// /* param 2-3 unused */
-		// cmd.param4 = home_position.yaw;
-		// cmd.param5 = home_position.lat;
-		// cmd.param6 = home_position.lon;
-		// cmd.param7 = home_position.alt;
+		/* param 2-3 unused */
+		cmd.param2 = NAN;
+		cmd.param3 = NAN;
+		cmd.param4 = NAN;
+		cmd.param5 = NAN;
+		cmd.param6 = NAN;
+		cmd.param7 = NAN;
 
-		// XXX inspect use of publication handle
-		(void)orb_advertise(ORB_ID(vehicle_command), &cmd);
+		orb_advert_t h = orb_advertise(ORB_ID(vehicle_command), &cmd);
+		(void)orb_unadvertise(h);
 
 		return 0;
 	}

From e064cb645ec0b38b0d05e4c1174e23e1c9bbdaea Mon Sep 17 00:00:00 2001
From: Julian Oes <julian@oes.ch>
Date: Fri, 13 May 2016 16:58:58 +0200
Subject: [PATCH 0102/1230] gpssim: fix command line argument handling

The gpssim interface was pretty broken, from random usage complaints to
segfaults.
---
 src/platforms/posix/drivers/gpssim/gpssim.cpp | 38 ++++++++++++++-----
 1 file changed, 29 insertions(+), 9 deletions(-)

diff --git a/src/platforms/posix/drivers/gpssim/gpssim.cpp b/src/platforms/posix/drivers/gpssim/gpssim.cpp
index 7a86abf356..e7f6387308 100644
--- a/src/platforms/posix/drivers/gpssim/gpssim.cpp
+++ b/src/platforms/posix/drivers/gpssim/gpssim.cpp
@@ -442,6 +442,7 @@ void	stop();
 void	test();
 void	reset();
 void	info();
+void	usage(const char *reason);
 
 /**
  * Start the driver.
@@ -535,9 +536,22 @@ info()
 	g_dev->print_info();
 }
 
+void
+usage(const char *reason)
+{
+	if (reason) {
+		PX4_WARN("%s", reason);
+	}
+
+	PX4_INFO("usage:");
+	PX4_INFO("gpssim {start|stop|test|reset|status}");
+	PX4_INFO("       [-d /dev/ttyS0-n][-f (for enabling fake)][-s (to enable sat info)]");
+}
+
 } // namespace
 
 
+
 int
 gpssim_main(int argc, char *argv[])
 {
@@ -547,22 +561,25 @@ gpssim_main(int argc, char *argv[])
 	bool fake_gps = false;
 	bool enable_sat_info = false;
 
+
+	if (argc < 2) {
+
+		gpssim::usage("not enough arguments supplied");
+		return 1;
+	}
+
 	/*
 	 * Start/load the driver.
 	 */
 	if (!strcmp(argv[1], "start")) {
 		if (g_dev != nullptr) {
-			PX4_WARN("gpssim already started");
+			PX4_WARN("already started");
 			return 0;
 		}
 
-		/* work around getopt unreliability */
 		if (argc > 3) {
 			if (!strcmp(argv[2], "-d")) {
 				device_name = argv[3];
-
-			} else {
-				goto out;
 			}
 		}
 
@@ -581,6 +598,13 @@ gpssim_main(int argc, char *argv[])
 		}
 
 		gpssim::start(device_name, fake_gps, enable_sat_info);
+		return 0;
+	}
+
+	/* The following need gpssim running. */
+	if (g_dev == nullptr) {
+		PX4_WARN("not running");
+		return 1;
 	}
 
 	if (!strcmp(argv[1], "stop")) {
@@ -609,8 +633,4 @@ gpssim_main(int argc, char *argv[])
 	}
 
 	return 0;
-
-out:
-	PX4_INFO("unrecognized command, try 'start', 'stop', 'test', 'reset' or 'status'\n [-d /dev/ttyS0-n][-f (for enabling fake)][-s (to enable sat info)]");
-	return 1;
 }

From a4c7fe50a7fec02ee846b64b5c021192bdb03439 Mon Sep 17 00:00:00 2001
From: Daniel Agar <daniel@agar.ca>
Date: Fri, 13 May 2016 14:54:12 -0400
Subject: [PATCH 0103/1230] travis-ci proper git version

---
 .travis.yml                            |  1 +
 cmake/common/px4_base.cmake            | 14 +++++++-------
 cmake/templates/build_git_version.h.in |  4 ++--
 3 files changed, 10 insertions(+), 9 deletions(-)

diff --git a/.travis.yml b/.travis.yml
index 1ebc7fe7b2..f2da8bcf96 100644
--- a/.travis.yml
+++ b/.travis.yml
@@ -44,6 +44,7 @@ addons:
     - zlib1g-dev
 
 before_install:
+  - cd ${TRAVIS_BUILD_DIR} && git fetch --unshallow && git fetch --all --tags
   - if [ "${TRAVIS_OS_NAME}" = "linux" ]; then
          pushd .
       && cd ~ && mkdir gcc && cd gcc
diff --git a/cmake/common/px4_base.cmake b/cmake/common/px4_base.cmake
index 5650e0aa2a..605ee5b1ef 100644
--- a/cmake/common/px4_base.cmake
+++ b/cmake/common/px4_base.cmake
@@ -776,21 +776,21 @@ function(px4_create_git_hash_header)
 		REQUIRED HEADER
 		ARGN ${ARGN})
 	execute_process(
-		COMMAND git describe --tags
+		COMMAND git describe --always --tags
 		OUTPUT_VARIABLE git_tag
 		OUTPUT_STRIP_TRAILING_WHITESPACE
 		WORKING_DIRECTORY ${CMAKE_SOURCE_DIR}
 		)
-	#message(STATUS "GIT_TAG = ${git_tag}")
+	message(STATUS "GIT_TAG = ${git_tag}")
 	execute_process(
-		COMMAND git rev-parse HEAD
-		OUTPUT_VARIABLE git_desc
+		COMMAND git rev-parse --verify HEAD
+		OUTPUT_VARIABLE git_version
 		OUTPUT_STRIP_TRAILING_WHITESPACE
 		WORKING_DIRECTORY ${CMAKE_SOURCE_DIR}
 		)
-	#message(STATUS "GIT_DESC = ${git_desc}")
-	set(git_desc_short)
-	string(SUBSTRING ${git_desc} 1 16 git_desc_short)
+	#message(STATUS "GIT_VERSION = ${git_version}")
+	set(git_version_short)
+	string(SUBSTRING ${git_version} 1 16 git_version_short)
 	configure_file(${CMAKE_SOURCE_DIR}/cmake/templates/build_git_version.h.in ${HEADER} @ONLY)
 endfunction()
 
diff --git a/cmake/templates/build_git_version.h.in b/cmake/templates/build_git_version.h.in
index 99b3551743..36e4e74e3e 100644
--- a/cmake/templates/build_git_version.h.in
+++ b/cmake/templates/build_git_version.h.in
@@ -1,5 +1,5 @@
 /* Auto Magically Generated file */
 /* Do not edit! */
-#define PX4_GIT_VERSION_STR  "@git_desc@"
-#define PX4_GIT_VERSION_BINARY 0x@git_desc_short@
+#define PX4_GIT_VERSION_STR  "@git_version@"
+#define PX4_GIT_VERSION_BINARY 0x@git_version_short@
 #define PX4_GIT_TAG_STR  "@git_tag@"

From 27cd3af7fb08d0ff4d57ccad208d48e0ac088aba Mon Sep 17 00:00:00 2001
From: Daniel Agar <daniel@agar.ca>
Date: Fri, 13 May 2016 18:39:17 -0400
Subject: [PATCH 0104/1230] mavlink fix missing
 MavlinkStreamNavControllerOutput (#4538)

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

diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp
index a480a4dcb7..b0e7a4ecca 100644
--- a/src/modules/mavlink/mavlink_messages.cpp
+++ b/src/modules/mavlink/mavlink_messages.cpp
@@ -3015,6 +3015,7 @@ const StreamListItem *streams_list[] = {
 	new StreamListItem(&MavlinkStreamActuatorControlTarget<2>::new_instance, &MavlinkStreamActuatorControlTarget<2>::get_name_static),
 	new StreamListItem(&MavlinkStreamActuatorControlTarget<3>::new_instance, &MavlinkStreamActuatorControlTarget<3>::get_name_static),
 	new StreamListItem(&MavlinkStreamNamedValueFloat::new_instance, &MavlinkStreamNamedValueFloat::get_name_static),
+	new StreamListItem(&MavlinkStreamNavControllerOutput::new_instance, &MavlinkStreamNavControllerOutput::get_name_static),
 	new StreamListItem(&MavlinkStreamCameraCapture::new_instance, &MavlinkStreamCameraCapture::get_name_static),
 	new StreamListItem(&MavlinkStreamCameraTrigger::new_instance, &MavlinkStreamCameraTrigger::get_name_static),
 	new StreamListItem(&MavlinkStreamDistanceSensor::new_instance, &MavlinkStreamDistanceSensor::get_name_static),

From 18b6815eec955406b71ae5b9b62083746b1208a4 Mon Sep 17 00:00:00 2001
From: Daniel Agar <daniel@agar.ca>
Date: Fri, 13 May 2016 20:25:20 -0400
Subject: [PATCH 0105/1230] camera_trigger_params.c param metadata

---
 src/drivers/camera_trigger/camera_trigger_params.c | 5 +----
 1 file changed, 1 insertion(+), 4 deletions(-)

diff --git a/src/drivers/camera_trigger/camera_trigger_params.c b/src/drivers/camera_trigger/camera_trigger_params.c
index eea1986c81..eb1352245e 100644
--- a/src/drivers/camera_trigger/camera_trigger_params.c
+++ b/src/drivers/camera_trigger/camera_trigger_params.c
@@ -83,11 +83,8 @@ PARAM_DEFINE_FLOAT(TRIG_ACT_TIME, 0.5f);
 /**
  * Camera trigger mode
  *
- * 0 disables the trigger, 1 sets it to enabled on command, 2 time based and always on,
- * 3 distance based and always on, 4 distance based and started / stopped via mission or command.
- *
  * @value 0 Disable
- * @value 1 On invididual commands
+ * @value 1 On individual commands
  * @value 2 Time based, always on
  * @value 3 Distance based, always on
  * @value 4 Distance, mission controlled

From 3bda6e08830a1bf5754387a1296ebeb5d94d11fb Mon Sep 17 00:00:00 2001
From: Daniel Agar <daniel@agar.ca>
Date: Fri, 13 May 2016 20:25:21 -0400
Subject: [PATCH 0106/1230] mkblctrl_params.c param metadata

---
 src/drivers/mkblctrl/mkblctrl_params.c | 7 ++-----
 1 file changed, 2 insertions(+), 5 deletions(-)

diff --git a/src/drivers/mkblctrl/mkblctrl_params.c b/src/drivers/mkblctrl/mkblctrl_params.c
index 0264efeb90..7fe3bd9361 100644
--- a/src/drivers/mkblctrl/mkblctrl_params.c
+++ b/src/drivers/mkblctrl/mkblctrl_params.c
@@ -44,12 +44,9 @@
 #include <systemlib/param/param.h>
 
 /**
- * Enables testmode (Identify) of MKBLCTRL Driver
+ * Test mode (Identify) of MKBLCTRL Driver
  *
- * @value 0 Disabled
- * @value 1 Enabled
- * @min 0
- * @max 1
+ * @boolean
  * @group MKBLCTRL Testmode
  */
 PARAM_DEFINE_INT32(MKBLCTRL_TEST, 0);

From 6f49f861d6ef401183e5fab52676d414552cebf7 Mon Sep 17 00:00:00 2001
From: Daniel Agar <daniel@agar.ca>
Date: Fri, 13 May 2016 20:25:22 -0400
Subject: [PATCH 0107/1230] px4io_params.c param metadata

---
 src/drivers/px4io/px4io_params.c | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/src/drivers/px4io/px4io_params.c b/src/drivers/px4io/px4io_params.c
index bc1e349b85..2be6c5efc3 100644
--- a/src/drivers/px4io/px4io_params.c
+++ b/src/drivers/px4io/px4io_params.c
@@ -131,7 +131,7 @@ PARAM_DEFINE_INT32(PWM_MAIN_REV7, 0);
 PARAM_DEFINE_INT32(PWM_MAIN_REV8, 0);
 
 /**
- * Enable S.BUS out
+ * S.BUS out
  *
  * Set to 1 to enable S.BUS version 1 output instead of RSSI.
  *

From deecd4b918578abc4a984d873fbc24876b3b3417 Mon Sep 17 00:00:00 2001
From: Daniel Agar <daniel@agar.ca>
Date: Fri, 13 May 2016 20:25:23 -0400
Subject: [PATCH 0108/1230] launchdetection_params.c param metadata

---
 src/lib/launchdetection/launchdetection_params.c | 4 +---
 1 file changed, 1 insertion(+), 3 deletions(-)

diff --git a/src/lib/launchdetection/launchdetection_params.c b/src/lib/launchdetection/launchdetection_params.c
index 4e80244855..6af608b48b 100644
--- a/src/lib/launchdetection/launchdetection_params.c
+++ b/src/lib/launchdetection/launchdetection_params.c
@@ -45,11 +45,9 @@
  */
 
 /**
- * Enable launch detection.
+ * Launch detection
  *
  * @boolean
- * @min 0
- * @max 1
  * @group Launch detection
  */
 PARAM_DEFINE_INT32(LAUN_ALL_ON, 0);

From ceea55a640e705e8ee8d3121f25ad74f62912c6b Mon Sep 17 00:00:00 2001
From: Daniel Agar <daniel@agar.ca>
Date: Fri, 13 May 2016 20:25:23 -0400
Subject: [PATCH 0109/1230] runway_takeoff_params.c param metadata

---
 src/lib/runway_takeoff/runway_takeoff_params.c | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/src/lib/runway_takeoff/runway_takeoff_params.c b/src/lib/runway_takeoff/runway_takeoff_params.c
index c0e2d7db07..7b91c8865b 100644
--- a/src/lib/runway_takeoff/runway_takeoff_params.c
+++ b/src/lib/runway_takeoff/runway_takeoff_params.c
@@ -40,7 +40,7 @@
  */
 
 /**
- * Enable or disable runway takeoff with landing gear
+ * Runway takeoff with landing gear
  *
  * @boolean
  * @group Runway Takeoff

From be3f0ac1756af94201204c33d61f4f15d2f67c55 Mon Sep 17 00:00:00 2001
From: Daniel Agar <daniel@agar.ca>
Date: Fri, 13 May 2016 20:25:24 -0400
Subject: [PATCH 0110/1230] attitude_estimator_ekf_params.c param metadata

---
 .../attitude_estimator_ekf/attitude_estimator_ekf_params.c     | 3 +--
 1 file changed, 1 insertion(+), 2 deletions(-)

diff --git a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_params.c b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_params.c
index 92156c93e8..c10f871022 100755
--- a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_params.c
+++ b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_params.c
@@ -125,7 +125,6 @@ PARAM_DEFINE_FLOAT(ATT_J33, 0.0037);
  * If set to != 0 the moment of inertia will be used in the estimator
  *
  * @group Attitude EKF estimator
- * @min 0
- * @max 1
+ * @boolean
  */
 PARAM_DEFINE_INT32(ATT_J_EN, 0);

From 80ef893d2336e298e16638a9dcefb0a486cdb7e5 Mon Sep 17 00:00:00 2001
From: Daniel Agar <daniel@agar.ca>
Date: Fri, 13 May 2016 20:25:25 -0400
Subject: [PATCH 0111/1230] attitude_estimator_q_params.c param metadata

---
 .../attitude_estimator_q/attitude_estimator_q_params.c        | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/src/modules/attitude_estimator_q/attitude_estimator_q_params.c b/src/modules/attitude_estimator_q/attitude_estimator_q_params.c
index e2babfee43..bb7f57ced0 100644
--- a/src/modules/attitude_estimator_q/attitude_estimator_q_params.c
+++ b/src/modules/attitude_estimator_q/attitude_estimator_q_params.c
@@ -94,7 +94,7 @@ PARAM_DEFINE_FLOAT(ATT_W_GYRO_BIAS, 0.1f);
 PARAM_DEFINE_FLOAT(ATT_MAG_DECL, 0.0f);
 
 /**
- * Enable automatic GPS based declination compensation
+ * Automatic GPS based declination compensation
  *
  * @group Attitude Q estimator
  * @boolean
@@ -116,7 +116,7 @@ PARAM_DEFINE_INT32(ATT_MAG_DECL_A, 1);
 PARAM_DEFINE_INT32(ATT_EXT_HDG_M, 0);
 
 /**
- * Enable acceleration compensation based on GPS
+ * Acceleration compensation based on GPS
  * velocity.
  *
  * @group Attitude Q estimator

From a54a51a9d564a0563d8cbf0202840acd4739e741 Mon Sep 17 00:00:00 2001
From: Daniel Agar <daniel@agar.ca>
Date: Fri, 13 May 2016 20:25:26 -0400
Subject: [PATCH 0112/1230] fw_pos_control_l1_params.c param metadata

---
 src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

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 271d05f394..18fd945c01 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
@@ -290,7 +290,7 @@ PARAM_DEFINE_FLOAT(FW_LND_TLALT, -1.0f);
 PARAM_DEFINE_FLOAT(FW_LND_HHDIST, 15.0f);
 
 /**
- * Enable or disable usage of terrain estimate during landing
+ * Use terrain estimate during landing
  *
  * @boolean
  * @group FW L1 Control

From d6cf441bac260a449025bf957c9220205a1597c9 Mon Sep 17 00:00:00 2001
From: Daniel Agar <daniel@agar.ca>
Date: Fri, 13 May 2016 20:25:27 -0400
Subject: [PATCH 0113/1230] mTecs_params.c param metadata

---
 src/modules/fw_pos_control_l1/mtecs/mTecs_params.c | 2 --
 1 file changed, 2 deletions(-)

diff --git a/src/modules/fw_pos_control_l1/mtecs/mTecs_params.c b/src/modules/fw_pos_control_l1/mtecs/mTecs_params.c
index dd457b664a..a6a7b210c0 100644
--- a/src/modules/fw_pos_control_l1/mtecs/mTecs_params.c
+++ b/src/modules/fw_pos_control_l1/mtecs/mTecs_params.c
@@ -45,8 +45,6 @@
 /**
  * mTECS enabled
  *
- * Set to 1 to enable mTECS
- *
  * @boolean
  * @group mTECS
  */

From 439ccc99ce1b5a44bd7f320095e9b4d3731904db Mon Sep 17 00:00:00 2001
From: Daniel Agar <daniel@agar.ca>
Date: Fri, 13 May 2016 20:25:28 -0400
Subject: [PATCH 0114/1230] params.c param metadata

---
 src/modules/local_position_estimator/params.c | 6 +++---
 1 file changed, 3 insertions(+), 3 deletions(-)

diff --git a/src/modules/local_position_estimator/params.c b/src/modules/local_position_estimator/params.c
index 18edfa7492..590e726ccb 100644
--- a/src/modules/local_position_estimator/params.c
+++ b/src/modules/local_position_estimator/params.c
@@ -4,7 +4,7 @@
 
 
 /**
- * Enable accelerometer integration for prediction.
+ * Accelerometer integration for prediction.
  *
  * @boolean
  * @group Local Position Estimator
@@ -129,7 +129,7 @@ PARAM_DEFINE_FLOAT(LPE_ACC_Z, 0.0454f);
 PARAM_DEFINE_FLOAT(LPE_BAR_Z, 1.0f);
 
 /**
- * Enable GPS
+ * GPS
  *
  * @group Local Position Estimator
  * @boolean
@@ -226,7 +226,7 @@ PARAM_DEFINE_FLOAT(LPE_VIS_XY, 0.5f);
 PARAM_DEFINE_FLOAT(LPE_VIS_Z, 0.5f);
 
 /**
- * Enabled vision correction
+ * Vision correction
  *
  * @group Local Position Estimator
  * @boolean

From be3348d770315536b32c59c7aea041394fdcaf3a Mon Sep 17 00:00:00 2001
From: Daniel Agar <daniel@agar.ca>
Date: Fri, 13 May 2016 20:25:29 -0400
Subject: [PATCH 0115/1230] mission_params.c param metadata

---
 src/modules/navigator/mission_params.c | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/src/modules/navigator/mission_params.c b/src/modules/navigator/mission_params.c
index 5601ae01c9..8f99e4258d 100644
--- a/src/modules/navigator/mission_params.c
+++ b/src/modules/navigator/mission_params.c
@@ -72,7 +72,7 @@ PARAM_DEFINE_FLOAT(MIS_TAKEOFF_ALT, 10.0f);
 PARAM_DEFINE_FLOAT(MIS_LTRMIN_ALT, 1.2f);
 
 /**
- * Enable persistent onboard mission storage
+ * Persistent onboard mission storage
  *
  * When enabled, missions that have been uploaded by the GCS are stored
  * and reloaded after reboot persistently.

From e08e8fa24fd6d9f5c66201e17cc7d742dbf6473d Mon Sep 17 00:00:00 2001
From: Daniel Agar <daniel@agar.ca>
Date: Fri, 13 May 2016 20:25:30 -0400
Subject: [PATCH 0116/1230] position_estimator_inav_params.cpp param metadata

---
 .../position_estimator_inav_params.cpp                | 11 +++++------
 1 file changed, 5 insertions(+), 6 deletions(-)

diff --git a/src/modules/position_estimator_inav/position_estimator_inav_params.cpp b/src/modules/position_estimator_inav/position_estimator_inav_params.cpp
index 854fc61638..331e1ba004 100644
--- a/src/modules/position_estimator_inav/position_estimator_inav_params.cpp
+++ b/src/modules/position_estimator_inav/position_estimator_inav_params.cpp
@@ -302,19 +302,18 @@ PARAM_DEFINE_FLOAT(INAV_FLOW_DIST_X, 0.0f);
 PARAM_DEFINE_FLOAT(INAV_FLOW_DIST_Y, 0.0f);
 
 /**
- * Disable mocap (set 0 if using fake gps)
+ * Mo-cap
  *
- * Disable mocap
+ * Set to 0 if using fake GPS
  *
- * @boolean
+ * @value 0 Mo-cap enabled
+ * @value 1 Mo-cap disabled
  * @group Position Estimator INAV
  */
 PARAM_DEFINE_FLOAT(INAV_DISAB_MOCAP, 0);
 
 /**
- * Enable LIDAR for altitude estimation
- *
- * Enable LIDAR for altitude estimation
+ * LIDAR for altitude estimation
  *
  * @boolean
  * @group Position Estimator INAV

From e43751c2197482ce3c778c9b0b252a6e3918007d Mon Sep 17 00:00:00 2001
From: Daniel Agar <daniel@agar.ca>
Date: Fri, 13 May 2016 20:25:31 -0400
Subject: [PATCH 0117/1230] params.c param metadata

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

diff --git a/src/modules/sdlog2/params.c b/src/modules/sdlog2/params.c
index db96d564ac..72c513114a 100644
--- a/src/modules/sdlog2/params.c
+++ b/src/modules/sdlog2/params.c
@@ -48,7 +48,7 @@
 PARAM_DEFINE_INT32(SDLOG_RATE, -1);
 
 /**
- * Enable extended logging mode.
+ * Extended logging mode
  *
  * A value of -1 indicates the command line argument
  * should be obeyed. A value of 0 disables extended
@@ -68,7 +68,7 @@ PARAM_DEFINE_INT32(SDLOG_EXT, -1);
 /**
  * Use timestamps only if GPS 3D fix is available
  *
- * A value of 1 constrains the log folder creation
+ * Constrain the log folder creation
  * to only use the time stamp if a 3D GPS lock is
  * present.
  *

From 0042c8ed12fecd213a49e145ab26eb997cc5c8cf Mon Sep 17 00:00:00 2001
From: Daniel Agar <daniel@agar.ca>
Date: Fri, 13 May 2016 20:25:31 -0400
Subject: [PATCH 0118/1230] sdlog2.c param metadata

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

diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c
index 08137f4056..5da029a608 100644
--- a/src/modules/sdlog2/sdlog2.c
+++ b/src/modules/sdlog2/sdlog2.c
@@ -412,7 +412,7 @@ bool get_log_time_tt(struct tm *tt, bool boot_time) {
 	struct timespec ts;
 	px4_clock_gettime(CLOCK_REALTIME, &ts);
 	/* use RTC time for log file naming, e.g. /fs/microsd/2014-01-19/19_37_52.px4log */
-	time_t utc_time_sec;
+	time_t utc_time_sec = 0;
 
 	if (_gpstime_only && has_gps_3d_fix) {
 		utc_time_sec = gps_time_sec;

From 119fa8ed1b2dbb7445977a9615c4ba24d5e47015 Mon Sep 17 00:00:00 2001
From: Daniel Agar <daniel@agar.ca>
Date: Fri, 13 May 2016 20:25:32 -0400
Subject: [PATCH 0119/1230] sensor_params.c param metadata

---
 src/modules/sensors/sensor_params.c | 6 +++---
 1 file changed, 3 insertions(+), 3 deletions(-)

diff --git a/src/modules/sensors/sensor_params.c b/src/modules/sensors/sensor_params.c
index 5454f34586..29439d61d4 100644
--- a/src/modules/sensors/sensor_params.c
+++ b/src/modules/sensors/sensor_params.c
@@ -1932,7 +1932,7 @@ PARAM_DEFINE_FLOAT(RC18_REV, 1.0f);
 PARAM_DEFINE_FLOAT(RC18_DZ, 0.0f);
 
 /**
- * Enable relay control of relay 1 mapped to the Spektrum receiver power supply
+ * Relay control of relay 1 mapped to the Spektrum receiver power supply
  *
  * @min 0
  * @max 1
@@ -2937,7 +2937,7 @@ PARAM_DEFINE_INT32(RC_RSSI_PWM_MAX, 1000);
 PARAM_DEFINE_INT32(RC_RSSI_PWM_MIN, 2000);
 
 /**
- * Enable Lidar-Lite (LL40LS) pwm driver
+ * Lidar-Lite (LL40LS) PWM
  *
  * @reboot_required true
  *
@@ -2947,7 +2947,7 @@ PARAM_DEFINE_INT32(RC_RSSI_PWM_MIN, 2000);
 PARAM_DEFINE_INT32(SENS_EN_LL40LS, 0);
 
 /**
- * Enable sf0x driver
+ * Lightware SF0x laser rangefinder
  *
  * @reboot_required true
  *

From 0ee801df2074e73b9be3a76962732d8b446a07de Mon Sep 17 00:00:00 2001
From: Daniel Agar <daniel@agar.ca>
Date: Fri, 13 May 2016 20:25:33 -0400
Subject: [PATCH 0120/1230] system_params.c param metadata

---
 src/modules/systemlib/system_params.c | 3 +--
 1 file changed, 1 insertion(+), 2 deletions(-)

diff --git a/src/modules/systemlib/system_params.c b/src/modules/systemlib/system_params.c
index 996cf79e3f..dae66295aa 100644
--- a/src/modules/systemlib/system_params.c
+++ b/src/modules/systemlib/system_params.c
@@ -107,11 +107,10 @@ PARAM_DEFINE_INT32(SYS_RESTART_TYPE, 2);
 PARAM_DEFINE_INT32(SYS_MC_EST_GROUP, 0);
 
 /**
- * Enable TELEM2 as companion computer link
+ * TELEM2 as companion computer link
  *
  * CHANGING THIS VALUE REQUIRES A RESTART. Configures the baud rate of the TELEM2 connector as
  * companion computer interface.
- * Set to zero to disable, set to these values to enable (NO OTHER VALUES SUPPORTED!).
  *
  * @value 0 Disabled
  * @value 10 FrSky Telemetry

From b1d8c46bb2094ec3bc1e64612b9b4235ffb5ed57 Mon Sep 17 00:00:00 2001
From: Daniel Agar <daniel@agar.ca>
Date: Fri, 13 May 2016 20:25:34 -0400
Subject: [PATCH 0121/1230] uavcan_params.c param metadata

---
 src/modules/uavcan/uavcan_params.c | 3 +--
 1 file changed, 1 insertion(+), 2 deletions(-)

diff --git a/src/modules/uavcan/uavcan_params.c b/src/modules/uavcan/uavcan_params.c
index e269202d99..2196a5fb30 100644
--- a/src/modules/uavcan/uavcan_params.c
+++ b/src/modules/uavcan/uavcan_params.c
@@ -39,9 +39,8 @@
 #include <systemlib/param/param.h>
 
 /**
- * Enable UAVCAN.
+ * UAVCAN mode
  *
- * Allowed values:
  *  0 - UAVCAN disabled.
  *  1 - Enabled support for UAVCAN actuators and sensors.
  *  2 - Enabled support for dynamic node ID allocation and firmware update.

From e1868216927ee3a33a98b3b9d309dfc13a506703 Mon Sep 17 00:00:00 2001
From: Daniel Agar <daniel@agar.ca>
Date: Fri, 13 May 2016 20:25:35 -0400
Subject: [PATCH 0122/1230] vtol_att_control_params.c param metadata

---
 src/modules/vtol_att_control/vtol_att_control_params.c | 6 +++---
 1 file changed, 3 insertions(+), 3 deletions(-)

diff --git a/src/modules/vtol_att_control/vtol_att_control_params.c b/src/modules/vtol_att_control/vtol_att_control_params.c
index 2abaadb1cc..c9ba6544f0 100644
--- a/src/modules/vtol_att_control/vtol_att_control_params.c
+++ b/src/modules/vtol_att_control/vtol_att_control_params.c
@@ -249,7 +249,7 @@ PARAM_DEFINE_FLOAT(VT_ARSP_BLEND, 8.0f);
 PARAM_DEFINE_FLOAT(VT_ARSP_TRANS, 10.0f);
 
 /**
- * Enable optimal recovery strategy for pitch-weak tailsitters
+ * Optimal recovery strategy for pitch-weak tailsitters
  *
  * @boolean
  * @group VTOL Attitude Control
@@ -257,7 +257,7 @@ PARAM_DEFINE_FLOAT(VT_ARSP_TRANS, 10.0f);
 PARAM_DEFINE_INT32(VT_OPT_RECOV_EN, 0);
 
 /**
- * Enable weather-vane mode landings for missions
+ * Weather-vane mode landings for missions
  *
  * @boolean
  * @group VTOL Attitude Control
@@ -279,7 +279,7 @@ PARAM_DEFINE_INT32(VT_WV_LND_EN, 0);
 PARAM_DEFINE_FLOAT(VT_WV_YAWR_SCL, 0.15f);
 
 /**
- * Enable weather-vane mode for loiter
+ * Weather-vane mode for loiter
  *
  * @boolean
  * @group VTOL Attitude Control

From 3a8d24257665b4d85f3469a9b16cc5fcf0b1db76 Mon Sep 17 00:00:00 2001
From: Julian Oes <julian@oes.ch>
Date: Sat, 14 May 2016 00:06:55 +0200
Subject: [PATCH 0123/1230] dataman: get path for Snapdragon right

---
 src/modules/dataman/dataman.c | 4 ++++
 src/platforms/px4_defines.h   | 2 ++
 2 files changed, 6 insertions(+)

diff --git a/src/modules/dataman/dataman.c b/src/modules/dataman/dataman.c
index 30d7e359da..356a722027 100644
--- a/src/modules/dataman/dataman.c
+++ b/src/modules/dataman/dataman.c
@@ -134,7 +134,11 @@ static px4_sem_t g_sys_state_mutex;
 
 /* The data manager store file handle and file name */
 static int g_fd = -1, g_task_fd = -1;
+#ifdef __PX4_POSIX_EAGLE
+static const char *default_device_path = PX4_ROOTFSDIR"/dataman";
+#else
 static const char *default_device_path = PX4_ROOTFSDIR"/fs/microsd/dataman";
+#endif
 static char *k_data_manager_device_path = NULL;
 
 /* The data manager work queues */
diff --git a/src/platforms/px4_defines.h b/src/platforms/px4_defines.h
index 38ea682943..fd87b78a28 100644
--- a/src/platforms/px4_defines.h
+++ b/src/platforms/px4_defines.h
@@ -160,6 +160,8 @@ __END_DECLS
 
 #if defined(__PX4_QURT)
 #define PX4_ROOTFSDIR
+#elif defined(__PX4_POSIX_EAGLE)
+#define PX4_ROOTFSDIR "/home/linaro"
 #else
 #define PX4_ROOTFSDIR "rootfs"
 #endif

From 920d2ef1e83528070433a771b10722758393ce5a Mon Sep 17 00:00:00 2001
From: Julian Oes <julian@oes.ch>
Date: Sat, 14 May 2016 00:07:33 +0200
Subject: [PATCH 0124/1230] posix sdflight: add navigator to Linux side

---
 cmake/configs/posix_sdflight_default.cmake | 1 +
 posix-configs/eagle/flight/mainapp.config  | 2 ++
 2 files changed, 3 insertions(+)

diff --git a/cmake/configs/posix_sdflight_default.cmake b/cmake/configs/posix_sdflight_default.cmake
index 5d026346c7..ef1aef2e2b 100644
--- a/cmake/configs/posix_sdflight_default.cmake
+++ b/cmake/configs/posix_sdflight_default.cmake
@@ -40,6 +40,7 @@ set(config_module_list
 	modules/sdlog2
 	modules/simulator
 	modules/commander
+	modules/navigator
 	modules/load_mon
 
 	lib/controllib
diff --git a/posix-configs/eagle/flight/mainapp.config b/posix-configs/eagle/flight/mainapp.config
index 03fccce897..b2bccc62c4 100644
--- a/posix-configs/eagle/flight/mainapp.config
+++ b/posix-configs/eagle/flight/mainapp.config
@@ -1,5 +1,7 @@
 uorb start
 muorb start
+dataman start
+navigator start
 mavlink start -u 14556 -r 1000000
 sleep 1
 mavlink stream -u 14556 -s HIGHRES_IMU -r 50

From 382987161247394e793d8376d231a7669c047b51 Mon Sep 17 00:00:00 2001
From: David Sidrane <david_s5@nscdg.com>
Date: Fri, 13 May 2016 11:44:14 -1000
Subject: [PATCH 0125/1230] Removed unused NXFFS buys back 5792 bytes of FLASH

---
 nuttx-configs/aerocore/nsh/defconfig             | 7 +------
 nuttx-configs/mindpx-v2/nsh/defconfig            | 7 +------
 nuttx-configs/px4-stm32f4discovery/nsh/defconfig | 7 +------
 nuttx-configs/px4fmu-v1/nsh/defconfig            | 7 +------
 nuttx-configs/px4fmu-v2/nsh/defconfig            | 7 +------
 nuttx-configs/px4fmu-v4/nsh/defconfig            | 7 +------
 6 files changed, 6 insertions(+), 36 deletions(-)

diff --git a/nuttx-configs/aerocore/nsh/defconfig b/nuttx-configs/aerocore/nsh/defconfig
index 655dd1406e..6be8f217ba 100644
--- a/nuttx-configs/aerocore/nsh/defconfig
+++ b/nuttx-configs/aerocore/nsh/defconfig
@@ -589,12 +589,7 @@ CONFIG_FAT_LFN=y
 CONFIG_FAT_MAXFNAME=32
 CONFIG_FS_FATTIME=y
 CONFIG_FAT_DMAMEMORY=y
-CONFIG_FS_NXFFS=y
-CONFIG_NXFFS_PREALLOCATED=y
-CONFIG_NXFFS_ERASEDSTATE=0xff
-CONFIG_NXFFS_PACKTHRESHOLD=32
-CONFIG_NXFFS_MAXNAMLEN=32
-CONFIG_NXFFS_TAILTHRESHOLD=2048
+# CONFIG_FS_NXFFS is not set
 CONFIG_FS_ROMFS=y
 # CONFIG_FS_SMARTFS is not set
 CONFIG_FS_BINFS=y
diff --git a/nuttx-configs/mindpx-v2/nsh/defconfig b/nuttx-configs/mindpx-v2/nsh/defconfig
index 251609002e..54862aa09d 100644
--- a/nuttx-configs/mindpx-v2/nsh/defconfig
+++ b/nuttx-configs/mindpx-v2/nsh/defconfig
@@ -703,12 +703,7 @@ CONFIG_FAT_LFN=y
 CONFIG_FAT_MAXFNAME=32
 CONFIG_FS_FATTIME=y
 CONFIG_FAT_DMAMEMORY=y
-CONFIG_FS_NXFFS=y
-CONFIG_NXFFS_PREALLOCATED=y
-CONFIG_NXFFS_ERASEDSTATE=0xff
-CONFIG_NXFFS_PACKTHRESHOLD=32
-CONFIG_NXFFS_MAXNAMLEN=32
-CONFIG_NXFFS_TAILTHRESHOLD=2048
+# CONFIG_FS_NXFFS is not set
 CONFIG_FS_ROMFS=y
 # CONFIG_FS_SMARTFS is not set
 CONFIG_FS_BINFS=y
diff --git a/nuttx-configs/px4-stm32f4discovery/nsh/defconfig b/nuttx-configs/px4-stm32f4discovery/nsh/defconfig
index 5285fae077..b81038bd90 100644
--- a/nuttx-configs/px4-stm32f4discovery/nsh/defconfig
+++ b/nuttx-configs/px4-stm32f4discovery/nsh/defconfig
@@ -555,12 +555,7 @@ CONFIG_FS_FAT=y
 # CONFIG_FAT_LFN is not set
 # CONFIG_FS_FATTIME is not set
 # CONFIG_FAT_DMAMEMORY is not set
-CONFIG_FS_NXFFS=y
-CONFIG_NXFFS_PREALLOCATED=y
-CONFIG_NXFFS_ERASEDSTATE=0xff
-CONFIG_NXFFS_PACKTHRESHOLD=32
-CONFIG_NXFFS_MAXNAMLEN=32
-CONFIG_NXFFS_TAILTHRESHOLD=2048
+# CONFIG_FS_NXFFS is not set
 CONFIG_FS_ROMFS=y
 # CONFIG_FS_SMARTFS is not set
 CONFIG_FS_BINFS=y
diff --git a/nuttx-configs/px4fmu-v1/nsh/defconfig b/nuttx-configs/px4fmu-v1/nsh/defconfig
index b89b4039d8..e172c3bcfb 100644
--- a/nuttx-configs/px4fmu-v1/nsh/defconfig
+++ b/nuttx-configs/px4fmu-v1/nsh/defconfig
@@ -625,12 +625,7 @@ CONFIG_FAT_LFN=y
 CONFIG_FAT_MAXFNAME=32
 CONFIG_FS_FATTIME=y
 # CONFIG_FAT_DMAMEMORY is not set
-CONFIG_FS_NXFFS=y
-CONFIG_NXFFS_PREALLOCATED=y
-CONFIG_NXFFS_ERASEDSTATE=0xff
-CONFIG_NXFFS_PACKTHRESHOLD=32
-CONFIG_NXFFS_MAXNAMLEN=32
-CONFIG_NXFFS_TAILTHRESHOLD=2048
+# CONFIG_FS_NXFFS is not set
 CONFIG_FS_ROMFS=y
 # CONFIG_FS_SMARTFS is not set
 CONFIG_FS_BINFS=y
diff --git a/nuttx-configs/px4fmu-v2/nsh/defconfig b/nuttx-configs/px4fmu-v2/nsh/defconfig
index 950f17b907..193394ef40 100644
--- a/nuttx-configs/px4fmu-v2/nsh/defconfig
+++ b/nuttx-configs/px4fmu-v2/nsh/defconfig
@@ -705,12 +705,7 @@ CONFIG_FAT_LFN=y
 CONFIG_FAT_MAXFNAME=32
 CONFIG_FS_FATTIME=y
 CONFIG_FAT_DMAMEMORY=y
-CONFIG_FS_NXFFS=y
-CONFIG_NXFFS_PREALLOCATED=y
-CONFIG_NXFFS_ERASEDSTATE=0xff
-CONFIG_NXFFS_PACKTHRESHOLD=32
-CONFIG_NXFFS_MAXNAMLEN=32
-CONFIG_NXFFS_TAILTHRESHOLD=2048
+# CONFIG_FS_NXFFS is not set
 CONFIG_FS_ROMFS=y
 # CONFIG_FS_SMARTFS is not set
 CONFIG_FS_BINFS=y
diff --git a/nuttx-configs/px4fmu-v4/nsh/defconfig b/nuttx-configs/px4fmu-v4/nsh/defconfig
index bb7e077056..0d339753fa 100644
--- a/nuttx-configs/px4fmu-v4/nsh/defconfig
+++ b/nuttx-configs/px4fmu-v4/nsh/defconfig
@@ -705,12 +705,7 @@ CONFIG_FAT_LFN=y
 CONFIG_FAT_MAXFNAME=32
 CONFIG_FS_FATTIME=y
 CONFIG_FAT_DMAMEMORY=y
-CONFIG_FS_NXFFS=y
-CONFIG_NXFFS_PREALLOCATED=y
-CONFIG_NXFFS_ERASEDSTATE=0xff
-CONFIG_NXFFS_PACKTHRESHOLD=32
-CONFIG_NXFFS_MAXNAMLEN=32
-CONFIG_NXFFS_TAILTHRESHOLD=2048
+# CONFIG_FS_NXFFS is not set
 CONFIG_FS_ROMFS=y
 # CONFIG_FS_SMARTFS is not set
 CONFIG_FS_BINFS=y

From 5f2c2f8392ef03e1d36768b8577baae2cb6d030a Mon Sep 17 00:00:00 2001
From: Paul Riseborough <p_riseborough@live.com.au>
Date: Sat, 14 May 2016 11:53:51 +0930
Subject: [PATCH 0126/1230] ecl: update submodule reference

---
 src/lib/ecl | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/src/lib/ecl b/src/lib/ecl
index e91a934f07..c7e225124c 160000
--- a/src/lib/ecl
+++ b/src/lib/ecl
@@ -1 +1 @@
-Subproject commit e91a934f07fe8bed33e6e5d2a7ba9670e43e5561
+Subproject commit c7e225124c4303238da1e8e4431d23a8ff81aec0

From 3aa2297497b8014f5390b62c8b9fc906d970ea4d Mon Sep 17 00:00:00 2001
From: Paul Riseborough <p_riseborough@live.com.au>
Date: Sat, 14 May 2016 11:54:45 +0930
Subject: [PATCH 0127/1230] ekf2: Add parameters to control output filter

---
 src/modules/ekf2/ekf2_main.cpp |  8 +++++++-
 src/modules/ekf2/ekf2_params.c | 22 ++++++++++++++++++++++
 2 files changed, 29 insertions(+), 1 deletion(-)

diff --git a/src/modules/ekf2/ekf2_main.cpp b/src/modules/ekf2/ekf2_main.cpp
index d48937ec92..de351376b0 100644
--- a/src/modules/ekf2/ekf2_main.cpp
+++ b/src/modules/ekf2/ekf2_main.cpp
@@ -244,6 +244,10 @@ private:
 	control::BlockParamFloat _flow_pos_y;	// Y position of optical flow sensor focal point in body frame (m)
 	control::BlockParamFloat _flow_pos_z;	// Z position of optical flow sensor focal point in body frame (m)
 
+	// output predictor filter time constants
+	control::BlockParamFloat _tau_vel;	// time constant used by the output velocity complementary filter (s)
+	control::BlockParamFloat _tau_pos;	// time constant used by the output position complementary filter (s)
+
 	int update_subscriptions();
 
 };
@@ -326,7 +330,9 @@ Ekf2::Ekf2():
 	_rng_pos_z(this, "EKF2_RNG_POS_Z", false, &_params->rng_pos_body(2)),
 	_flow_pos_x(this, "EKF2_OF_POS_X", false, &_params->flow_pos_body(0)),
 	_flow_pos_y(this, "EKF2_OF_POS_Y", false, &_params->flow_pos_body(1)),
-	_flow_pos_z(this, "EKF2_OF_POS_Z", false, &_params->flow_pos_body(2))
+	_flow_pos_z(this, "EKF2_OF_POS_Z", false, &_params->flow_pos_body(2)),
+	_tau_vel(this, "EKF2_TAU_VEL", false, &_params->vel_Tau),
+	_tau_pos(this, "EKF2_TAU_POS", false, &_params->pos_Tau)
 {
 
 }
diff --git a/src/modules/ekf2/ekf2_params.c b/src/modules/ekf2/ekf2_params.c
index d786a98724..84ceebdf0d 100644
--- a/src/modules/ekf2/ekf2_params.c
+++ b/src/modules/ekf2/ekf2_params.c
@@ -699,3 +699,25 @@ PARAM_DEFINE_FLOAT(EKF2_OF_POS_Y, 0.0f);
  * @decimal 3
  */
 PARAM_DEFINE_FLOAT(EKF2_OF_POS_Z, 0.0f);
+
+/**
+ * Time constant of the velocity output prediction and smoothing filter
+ *
+ * @group EKF2
+ * @min 0.1
+ * @max 1.0
+ * @unit s
+ * @decimal 2
+ */
+PARAM_DEFINE_FLOAT(EKF2_TAU_VEL, 0.5f);
+
+/**
+ * Time constant of the position output prediction and smoothing filter
+ *
+ * @group EKF2
+ * @min 0.1
+ * @max 1.0
+ * @unit s
+ * @decimal 2
+ */
+PARAM_DEFINE_FLOAT(EKF2_TAU_POS, 0.25f);

From 76387b1693f234602e6c354115fbcac25b33baab Mon Sep 17 00:00:00 2001
From: Daniel Agar <daniel@agar.ca>
Date: Mon, 25 Apr 2016 17:36:13 -0400
Subject: [PATCH 0128/1230] uorb autogeneration

---
 CMakeLists.txt                                |   4 +-
 Makefile                                      |   3 +
 Tools/px_generate_uorb_topic_headers.py       | 108 +++++--
 Tools/px_generate_uorb_topic_sources.py       | 192 +++++++++++
 cmake/common/px4_base.cmake                   |  33 +-
 cmake/configs/nuttx_px4fmu-v2_debug.cmake     | 189 +++++++++++
 msg/actuator_armed.msg                        |   1 -
 msg/actuator_controls.msg                     |   5 +-
 msg/actuator_controls_1.msg                   |   5 -
 msg/actuator_controls_2.msg                   |   5 -
 msg/actuator_controls_3.msg                   |   5 -
 msg/actuator_controls_virtual_fw.msg          |   5 -
 msg/actuator_direct.msg                       |   1 -
 msg/actuator_outputs.msg                      |   1 -
 msg/airspeed.msg                              |   1 -
 msg/battery_status.msg                        |   1 -
 msg/camera_trigger.msg                        |   1 -
 msg/commander_state.msg                       |   1 -
 msg/control_state.msg                         |   1 -
 msg/differential_pressure.msg                 |   1 -
 msg/distance_sensor.msg                       |   1 -
 msg/ekf2_innovations.msg                      |   1 -
 msg/encoders.msg                              |   1 -
 msg/esc_status.msg                            |   1 -
 msg/estimator_status.msg                      |   1 -
 msg/filtered_bottom_flow.msg                  |   1 -
 msg/follow_target.msg                         |   1 -
 msg/fw_pos_ctrl_status.msg                    |   2 -
 msg/fw_virtual_attitude_setpoint.msg          |   4 +-
 msg/fw_virtual_rates_setpoint.msg             |   1 -
 msg/hil_sensor.msg                            |   1 -
 msg/home_position.msg                         |   1 -
 msg/manual_control_setpoint.msg               |   2 -
 msg/mavlink_log.msg                           |   1 -
 msg/mc_att_ctrl_status.msg                    |   1 -
 msg/mc_virtual_attitude_setpoint.msg          |   4 +-
 msg/mc_virtual_rates_setpoint.msg             |   1 -
 msg/mission.msg                               |   4 +
 msg/offboard_control_mode.msg                 |   1 -
 msg/optical_flow.msg                          |   1 -
 msg/parameter_update.msg                      |   1 -
 msg/pwm_input.msg                             |   1 -
 msg/qshell_req.msg                            |   1 -
 msg/rc_channels.msg                           |   2 +-
 msg/rc_parameter_map.msg                      |   1 -
 msg/{ => ros}/actuator_controls_0.msg         |   1 -
 .../actuator_controls_virtual_mc.msg          |   1 -
 msg/safety.msg                                |   1 -
 msg/satellite_info.msg                        |   1 -
 msg/sensor_accel.msg                          |   1 -
 msg/sensor_baro.msg                           |   1 -
 msg/sensor_combined.msg                       |   1 -
 msg/sensor_gyro.msg                           |   1 -
 msg/sensor_mag.msg                            |   1 -
 msg/servorail_status.msg                      |   1 -
 msg/system_power.msg                          |   1 -
 msg/tecs_status.msg                           |   1 -
 msg/telemetry_status.msg                      |   1 -
 msg/templates/px4/uorb/msg.h.template         |   9 +-
 msg/templates/uorb/msg.cpp.template           | 132 ++++++++
 msg/templates/uorb/msg.h.template             |  98 ++++--
 msg/templates/uorb/uORBTopics.cpp.template    |  70 ++++
 msg/test_motor.msg                            |   1 -
 msg/transponder_report.msg                    |   1 -
 msg/vehicle_attitude.msg                      |   1 -
 msg/vehicle_attitude_setpoint.msg             |   4 +-
 msg/vehicle_control_mode.msg                  |   1 -
 msg/vehicle_global_position.msg               |   1 -
 msg/vehicle_land_detected.msg                 |   1 -
 msg/vehicle_local_position.msg                |   1 -
 msg/vehicle_local_position_setpoint.msg       |   1 -
 msg/vehicle_rates_setpoint.msg                |   1 -
 msg/vehicle_status.msg                        |   1 -
 msg/vtol_vehicle_status.msg                   |   1 -
 msg/wind_estimate.msg                         |   1 -
 src/drivers/airspeed/airspeed.cpp             |  11 +-
 .../ardrone_interface/ardrone_interface.c     |   4 -
 src/drivers/mb12xx/mb12xx.cpp                 |  12 +-
 src/drivers/mkblctrl/mkblctrl.cpp             |   1 -
 src/drivers/pwm_out_sim/pwm_out_sim.cpp       |   4 -
 src/drivers/px4flow/px4flow.cpp               |  12 +-
 src/drivers/px4fmu/fmu.cpp                    |   4 -
 src/drivers/px4io/px4io.cpp                   |   4 -
 src/drivers/roboclaw/RoboClaw.cpp             |   1 -
 .../snapdragon_rc_pwm/snapdragon_rc_pwm.cpp   |   1 -
 src/drivers/srf02/srf02.cpp                   |  12 +-
 src/drivers/trone/trone.cpp                   |  12 +-
 src/examples/fixedwing_control/main.c         |   4 -
 src/examples/hwtest/hwtest.c                  |   4 -
 src/examples/rover_steering_control/main.cpp  |   4 -
 src/lib/controllib/uorb/blocks.hpp            |   4 -
 src/modules/bottle_drop/bottle_drop.cpp       |   4 -
 src/modules/commander/commander.cpp           |   4 -
 .../fw_att_control/fw_att_control_main.cpp    |   2 -
 src/modules/mavlink/mavlink_parameters.cpp    |   2 -
 .../mc_att_control/mc_att_control_main.cpp    |   2 -
 .../position_estimator_inav_main.cpp          |   4 -
 src/modules/sdlog2/sdlog2.c                   |   4 -
 src/modules/systemlib/param/param.c           |   3 -
 src/modules/systemlib/param/param_shmem.c     |   3 -
 src/modules/uORB/CMakeLists.txt               |   3 +-
 src/modules/uORB/Subscription.cpp             |   6 +
 src/modules/uORB/Subscription.hpp             |   4 +
 src/modules/uORB/objects_common.cpp           | 305 ------------------
 src/modules/uORB/uORB.h                       |   7 +-
 src/modules/uORB/uORBTopics.h                 |  50 +++
 .../uORB/uORB_tests/uORBTest_UnitTest.hpp     |  13 +-
 .../vtol_att_control/vtol_att_control_main.h  |   2 -
 .../posix/drivers/airspeedsim/airspeedsim.cpp |  11 +-
 src/platforms/px4_includes.h                  |  14 -
 src/systemcmds/esc_calib/esc_calib.c          |   4 -
 .../uORBCommunicator_gtests.cpp               |   2 +-
 unittests/uorb_unittests/uORBGtestTopics.hpp  |   9 +-
 113 files changed, 897 insertions(+), 593 deletions(-)
 create mode 100755 Tools/px_generate_uorb_topic_sources.py
 create mode 100644 cmake/configs/nuttx_px4fmu-v2_debug.cmake
 delete mode 100644 msg/actuator_controls_1.msg
 delete mode 100644 msg/actuator_controls_2.msg
 delete mode 100644 msg/actuator_controls_3.msg
 delete mode 100644 msg/actuator_controls_virtual_fw.msg
 rename msg/{ => ros}/actuator_controls_0.msg (91%)
 rename msg/{ => ros}/actuator_controls_virtual_mc.msg (91%)
 create mode 100644 msg/templates/uorb/msg.cpp.template
 create mode 100644 msg/templates/uorb/uORBTopics.cpp.template
 delete mode 100644 src/modules/uORB/objects_common.cpp
 create mode 100644 src/modules/uORB/uORBTopics.h

diff --git a/CMakeLists.txt b/CMakeLists.txt
index 019984d9ba..30ded0df5a 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -295,11 +295,11 @@ add_definitions(${definitions})
 #=============================================================================
 # source code generation
 #
-file(GLOB_RECURSE msg_files msg/*.msg)
+file(GLOB msg_files msg/*.msg)
 px4_generate_messages(TARGET msg_gen
 	MSG_FILES ${msg_files}
 	OS ${OS}
-	DEPENDS git_genmsg git_gencpp
+	DEPENDS git_genmsg git_gencpp prebuild_targets
 	)
 px4_generate_parameters_xml(OUT parameters.xml BOARD ${BOARD})
 px4_generate_airframes_xml(OUT airframes.xml BOARD ${BOARD})
diff --git a/Makefile b/Makefile
index df7ebeafe5..3f9922144f 100644
--- a/Makefile
+++ b/Makefile
@@ -155,6 +155,9 @@ px4-stm32f4discovery_default:
 px4fmu-v2_ekf2:
 	$(call cmake-build,nuttx_px4fmu-v2_ekf2)
 
+px4fmu-v2_lpe:
+	$(call cmake-build,nuttx_px4fmu-v2_lpe)
+
 mindpx-v2_default:
 	$(call cmake-build,nuttx_mindpx-v2_default)
 
diff --git a/Tools/px_generate_uorb_topic_headers.py b/Tools/px_generate_uorb_topic_headers.py
index 7ed0b3af19..0dba2468f3 100755
--- a/Tools/px_generate_uorb_topic_headers.py
+++ b/Tools/px_generate_uorb_topic_headers.py
@@ -49,6 +49,7 @@ sys.path.append(px4_tools_dir + "/genmsg/src")
 sys.path.append(px4_tools_dir + "/gencpp/src")
 
 try:
+        import em
         import genmsg.template_tools
 except ImportError as e:
         print("python import error: ", e)
@@ -74,24 +75,75 @@ __license__ = "BSD"
 __email__ = "thomasgubler@gmail.com"
 
 
-msg_template_map = {'msg.h.template': '@NAME@.h'}
-srv_template_map = {}
-incl_default = ['std_msgs:./msg/std_msgs']
-package = 'px4'
+TEMPLATE_FILE = 'msg.h.template'
+OUTPUT_FILE_EXT = '.h'
+INCL_DEFAULT = ['std_msgs:./msg/std_msgs']
+PACKAGE = 'px4'
+TOPICS_TOKEN = '# TOPICS '
 
 
-def convert_file(filename, outputdir, templatedir, includepath):
+def get_multi_topics(filename):
         """
-        Converts a single .msg file to a uorb header
+        Get TOPICS names from a "# TOPICS" line
         """
-        #print("Generating headers from {0}".format(filename))
-        genmsg.template_tools.generate_from_file(filename,
-                                                 package,
-                                                 outputdir,
-                                                 templatedir,
-                                                 includepath,
-                                                 msg_template_map,
-                                                 srv_template_map)
+        ofile = open(filename, 'r')
+        text = ofile.read()
+        result = []
+        for each_line in text.split('\n'):
+                if each_line.startswith (TOPICS_TOKEN):
+                        topic_names_str = each_line.strip()
+                        topic_names_str = topic_names_str.replace(TOPICS_TOKEN, "")
+                        result.extend(topic_names_str.split(" "))
+        ofile.close()
+        return result
+
+
+def generate_header_from_file(filename, outputdir, templatedir, includepath):
+        """
+        Converts a single .msg file to a uorb header file
+        """
+        msg_context = genmsg.msg_loader.MsgContext.create_default()
+        full_type_name = genmsg.gentools.compute_full_type_name(PACKAGE, os.path.basename(filename))
+        spec = genmsg.msg_loader.load_msg_from_file(msg_context, filename, full_type_name)
+        topics = get_multi_topics(filename)
+        if includepath:
+                search_path = genmsg.command_line.includepath_to_dict(includepath)
+        else:
+                search_path = {}
+        genmsg.msg_loader.load_depends(msg_context, spec, search_path)
+        md5sum = genmsg.gentools.compute_md5(msg_context, spec)
+        if len(topics) == 0:
+                topics.append(spec.short_name)
+        em_globals = {
+            "file_name_in": filename,
+            "md5sum": md5sum,
+            "search_path": search_path,
+            "msg_context": msg_context,
+            "spec": spec,
+            "topics": topics
+        }
+
+        # Make sure output directory exists:
+        if not os.path.isdir(outputdir):
+                os.makedirs(outputdir)
+
+        template_file = os.path.join(templatedir, TEMPLATE_FILE)
+        output_file = os.path.join(outputdir, spec.short_name + OUTPUT_FILE_EXT)
+
+        if os.path.isfile(output_file):
+                return False
+
+        ofile = open(output_file, 'w')
+        # todo, reuse interpreter
+        interpreter = em.Interpreter(output=ofile, globals=em_globals, options={em.RAW_OPT:True,em.BUFFERED_OPT:True})
+        if not os.path.isfile(template_file):
+                ofile.close()
+                os.remove(output_file)
+                raise RuntimeError("Template file %s not found in template dir %s" % (template_file, templatedir))
+        interpreter.file(open(template_file)) #todo try
+        interpreter.shutdown()
+        ofile.close()
+        return True
 
 
 def convert_dir(inputdir, outputdir, templatedir):
@@ -122,7 +174,7 @@ def convert_dir(inputdir, outputdir, templatedir):
         if (maxinputtime != 0 and maxouttime != 0 and maxinputtime < maxouttime):
             return False
 
-        includepath = incl_default + [':'.join([package, inputdir])]
+        includepath = INCL_DEFAULT + [':'.join([PACKAGE, inputdir])]
         for f in os.listdir(inputdir):
                 # Ignore hidden files
                 if f.startswith("."):
@@ -133,11 +185,7 @@ def convert_dir(inputdir, outputdir, templatedir):
                 if not os.path.isfile(fn):
                         continue
 
-                convert_file(fn,
-                             outputdir,
-                             templatedir,
-                             includepath)
-
+                generate_header_from_file(fn, outputdir, templatedir, includepath)
         return True
 
 
@@ -151,15 +199,15 @@ def copy_changed(inputdir, outputdir, prefix='', quiet=False):
         if not os.path.isdir(outputdir):
                 os.makedirs(outputdir)
 
-        for f in os.listdir(inputdir):
-                fni = os.path.join(inputdir, f)
+        for input_file in os.listdir(inputdir):
+                fni = os.path.join(inputdir, input_file)
                 if os.path.isfile(fni):
-                        # Check if f exists in outpoutdir, copy the file if not
-                        fno = os.path.join(outputdir, prefix + f)
+                        # Check if input_file exists in outpoutdir, copy the file if not
+                        fno = os.path.join(outputdir, prefix + input_file)
                         if not os.path.isfile(fno):
                                 shutil.copy(fni, fno)
                                 if not quiet:
-                                    print("{0}: new header file".format(f))
+                                    print("{0}: new header file".format(fno))
                                 continue
 
                         if os.path.getmtime(fni) > os.path.getmtime(fno):
@@ -168,11 +216,11 @@ def copy_changed(inputdir, outputdir, prefix='', quiet=False):
                                 if not filecmp.cmp(fni, fno):
                                         shutil.copy(fni, fno)
                                         if not quiet:
-                                            print("{0}: updated".format(f))
+                                            print("{0}: updated".format(input_file))
                                         continue
 
                         if not quiet:
-                            print("{0}: unchanged".format(f))
+                            print("{0}: unchanged".format(input_file))
 
 
 def convert_dir_save(inputdir, outputdir, templatedir, temporarydir, prefix, quiet=False):
@@ -208,11 +256,7 @@ if __name__ == "__main__":
 
         if args.file is not None:
                 for f in args.file:
-                        convert_file(
-                            f,
-                            args.outputdir,
-                            args.templatedir,
-                            incl_default)
+                        generate_header_from_file(f, args.outputdir, args.templatedir, INCL_DEFAULT)
         elif args.dir is not None:
                 convert_dir_save(
                     args.dir,
diff --git a/Tools/px_generate_uorb_topic_sources.py b/Tools/px_generate_uorb_topic_sources.py
new file mode 100755
index 0000000000..ccba860214
--- /dev/null
+++ b/Tools/px_generate_uorb_topic_sources.py
@@ -0,0 +1,192 @@
+#!/usr/bin/env python
+#############################################################################
+#
+#   Copyright (C) 2013-2015 PX4 Development Team. All rights reserved.
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions
+# are met:
+#
+# 1. Redistributions of source code must retain the above copyright
+#    notice, this list of conditions and the following disclaimer.
+# 2. Redistributions in binary form must reproduce the above copyright
+#    notice, this list of conditions and the following disclaimer in
+#    the documentation and/or other materials provided with the
+#    distribution.
+# 3. Neither the name PX4 nor the names of its contributors may be
+#    used to endorse or promote products derived from this software
+#    without specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.
+#
+#############################################################################
+
+"""
+px_generate_uorb_topic_sources.py
+Generates cpp source files for uorb topics from .msg (ROS syntax)
+message files
+"""
+from __future__ import print_function
+import os
+import shutil
+import filecmp
+import argparse
+
+import sys
+px4_tools_dir = os.path.dirname(os.path.abspath(__file__))
+sys.path.append(px4_tools_dir + "/genmsg/src")
+sys.path.append(px4_tools_dir + "/gencpp/src")
+
+try:
+        import em
+        import genmsg.template_tools
+except ImportError as e:
+        print("python import error: ", e)
+        print('''
+Required python packages not installed.
+
+On a Debian/Ubuntu system please run:
+
+  sudo apt-get install python-empy
+  sudo pip install catkin_pkg
+
+On MacOS please run:
+  sudo pip install empy catkin_pkg
+
+On Windows please run:
+  easy_install empy catkin_pkg
+''')
+        exit(1)
+
+__author__ = "Sergey Belash, Thomas Gubler"
+__copyright__ = "Copyright (C) 2013-2014 PX4 Development Team."
+__license__ = "BSD"
+__email__ = "againagainst@gmail.com, thomasgubler@gmail.com"
+
+TOPIC_TEMPLATE_FILE = 'msg.cpp.template'
+TOPICS_LIST_TEMPLATE_FILE = 'uORBTopics.cpp.template'
+OUTPUT_FILE_EXT = '.cpp'
+PACKAGE = 'px4'
+TOPICS_TOKEN = '# TOPICS '
+
+
+def get_multi_topics(filename):
+        """
+        Get TOPICS names from a "# TOPICS" line
+        """
+        ofile = open(filename, 'r')
+        text = ofile.read()
+        result = []
+        for each_line in text.split('\n'):
+                if each_line.startswith (TOPICS_TOKEN):
+                        topic_names_str = each_line.strip()
+                        topic_names_str = topic_names_str.replace(TOPICS_TOKEN, "")
+                        result.extend(topic_names_str.split(" "))
+        ofile.close()
+        return result
+
+
+def get_msgs_list(msgdir):
+    """
+    Makes list of msg files in the given directory
+    """
+    return [fn for fn in os.listdir(msgdir) if fn.endswith(".msg")]
+
+
+def generate_source_from_file(filename, outputdir, template_file, quiet=False):
+        """
+        Converts a single .msg file to a uorb source file
+        """
+        # print("Generating sources from {0}".format(filename))
+        msg_context = genmsg.msg_loader.MsgContext.create_default()
+        full_type_name = genmsg.gentools.compute_full_type_name(PACKAGE, os.path.basename(filename))
+        spec = genmsg.msg_loader.load_msg_from_file(msg_context, filename, full_type_name)
+        topics = get_multi_topics(filename)
+        if len(topics) == 0:
+            topics.append(spec.short_name)
+        em_globals = {
+            "file_name_in": filename,
+            "spec": spec,
+            "topics": topics
+        }
+
+        output_file = os.path.join(outputdir, spec.short_name + OUTPUT_FILE_EXT)
+        if os.path.isfile(output_file):
+            return False
+        generate_by_template(output_file, template_file, em_globals)
+        if not quiet:
+            print("{0}: new source file".format(output_file))
+        return True
+
+
+def generate_by_template(output_file, template_file, em_globals):
+        """
+        Invokes empy intepreter to geneate output_file by the
+        given template_file and predefined em_globals dict
+        """
+        ofile = open(output_file, 'w')
+        # todo, reuse interpreter
+        interpreter = em.Interpreter(output=ofile, globals=em_globals, options={em.RAW_OPT:True,em.BUFFERED_OPT:True})
+        if not os.path.isfile(template_file):
+            ofile.close()
+            os.remove(output_file)
+            raise RuntimeError("Template file %s not found" % (template_file))
+        interpreter.file(open(template_file)) #todo try
+        interpreter.shutdown()
+        ofile.close()
+
+
+def convert_dir(msgdir, outputdir, templatedir, quiet=False):
+        """
+        Converts all .msg files in msgdir to uORB sources
+        """
+        # Make sure output directory exists:
+        if not os.path.isdir(outputdir):
+                os.makedirs(outputdir)
+        template_file = os.path.join(templatedir, TOPIC_TEMPLATE_FILE)
+
+        for f in os.listdir(msgdir):
+                # Ignore hidden files
+                if f.startswith("."):
+                        continue
+                fn = os.path.join(msgdir, f)
+                # Only look at actual files
+                if not os.path.isfile(fn):
+                        continue
+                generate_source_from_file(fn, outputdir, template_file, quiet)
+
+        # generate cpp file with topics list
+        tl_globals = {"msgs" : get_msgs_list(msgdir)}
+        tl_template_file = os.path.join(templatedir, TOPICS_LIST_TEMPLATE_FILE)
+        tl_out_file = os.path.join(outputdir, TOPICS_LIST_TEMPLATE_FILE.replace(".template", ""))
+        generate_by_template(tl_out_file, tl_template_file, tl_globals)
+        return True
+
+
+if __name__ == "__main__":
+        parser = argparse.ArgumentParser(description='Convert msg files to uorb sources')
+        parser.add_argument('-d', dest='dir', help='directory with msg files')
+        parser.add_argument('-f', dest='file', help="files to convert (use only without -d)", nargs="+")
+        parser.add_argument('-e', dest='templatedir', help='directory with template files',)
+        parser.add_argument('-o', dest='outputdir', help='output directory for source files')
+        parser.add_argument('-q', dest='quiet', default=False, action='store_true',
+                            help='string added as prefix to the output file '
+                            ' name when converting directories')
+        args = parser.parse_args()
+
+        if args.file is not None:
+                for f in args.file:
+                        generate_source_from_file(f, args.outputdir, args.templatedir, args.quiet)
+        elif args.dir is not None:
+                convert_dir(args.dir, args.outputdir, args.templatedir, args.quiet)
diff --git a/cmake/common/px4_base.cmake b/cmake/common/px4_base.cmake
index 605ee5b1ef..41de8a94c4 100644
--- a/cmake/common/px4_base.cmake
+++ b/cmake/common/px4_base.cmake
@@ -359,13 +359,15 @@ function(px4_generate_messages)
 		NAME px4_generate_messages
 		OPTIONS VERBOSE
 		ONE_VALUE OS TARGET
-		MULTI_VALUE MSG_FILES DEPENDS
+		MULTI_VALUE MSG_FILES DEPENDS INCLUDES
 		REQUIRED MSG_FILES OS TARGET
 		ARGN ${ARGN})
 	set(QUIET)
 	if(NOT VERBOSE)
 		set(QUIET "-q")
 	endif()
+
+	# headers
 	set(msg_out_path ${CMAKE_BINARY_DIR}/src/modules/uORB/topics)
 	set(msg_list)
 	foreach(msg_file ${MSG_FILES})
@@ -390,6 +392,26 @@ function(px4_generate_messages)
 		VERBATIM
 		)
 
+	# !sources
+	set(msg_source_out_path	${CMAKE_BINARY_DIR}/topics_sources)
+	set(msg_source_files_out ${msg_source_out_path}/uORBTopics.cpp)
+	foreach(msg ${msg_list})
+		list(APPEND msg_source_files_out ${msg_source_out_path}/${msg}.cpp)
+	endforeach()
+	add_custom_command(OUTPUT ${msg_source_files_out}
+		COMMAND ${PYTHON_EXECUTABLE} 
+			Tools/px_generate_uorb_topic_sources.py
+			${QUIET}
+			-d msg
+			-e msg/templates/uorb
+			-o ${msg_source_out_path}
+		DEPENDS ${DEPENDS} ${MSG_FILES}
+		WORKING_DIRECTORY ${CMAKE_SOURCE_DIR}
+		COMMENT "Generating uORB topic sources"
+		VERBATIM
+		)
+	set_source_files_properties(${msg_source_files_out} PROPERTIES GENERATED TRUE)
+
 	# multi messages for target OS
 	set(msg_multi_out_path
 		${CMAKE_BINARY_DIR}/src/platforms/${OS}/px4_messages)
@@ -411,8 +433,13 @@ function(px4_generate_messages)
 		COMMENT "Generating uORB topic multi headers for ${OS}"
 		VERBATIM
 		)
-	add_custom_target(${TARGET}
-		DEPENDS ${msg_multi_files_out} ${msg_files_out})
+
+	add_library(${TARGET}
+		${msg_source_files_out}
+		${msg_multi_files_out}
+		${msg_files_out}
+		)
+
 endfunction()
 
 #=============================================================================
diff --git a/cmake/configs/nuttx_px4fmu-v2_debug.cmake b/cmake/configs/nuttx_px4fmu-v2_debug.cmake
new file mode 100644
index 0000000000..e0fedbb7e6
--- /dev/null
+++ b/cmake/configs/nuttx_px4fmu-v2_debug.cmake
@@ -0,0 +1,189 @@
+set(CMAKE_BUILD_TYPE "Debug" CACHE STRING "build type")
+
+
+include(nuttx/px4_impl_nuttx)
+
+set(CMAKE_TOOLCHAIN_FILE ${CMAKE_SOURCE_DIR}/cmake/toolchains/Toolchain-arm-none-eabi.cmake)
+
+set(config_module_list
+	#
+	# Board support modules
+	#
+	drivers/device
+	drivers/stm32
+	drivers/stm32/adc
+	drivers/stm32/tone_alarm
+	drivers/led
+	drivers/px4fmu
+	drivers/px4io
+	drivers/boards/px4fmu-v2
+	drivers/rgbled
+	drivers/mpu6000
+	#drivers/mpu9250
+	drivers/lsm303d
+	drivers/l3gd20
+	#drivers/hmc5883
+	drivers/ms5611
+	#drivers/mb12xx
+	#drivers/srf02
+	#drivers/sf0x
+	#drivers/ll40ls
+	drivers/trone
+	drivers/gps
+	drivers/pwm_out_sim
+	#drivers/hott
+	#drivers/hott/hott_telemetry
+	#drivers/hott/hott_sensors
+	#drivers/blinkm
+	drivers/airspeed
+	drivers/ets_airspeed
+	drivers/meas_airspeed
+	#drivers/frsky_telemetry
+	modules/sensors
+	#drivers/mkblctrl
+	#drivers/px4flow
+	#drivers/oreoled
+	drivers/gimbal
+	drivers/pwm_input
+	drivers/camera_trigger
+	drivers/bst
+	#drivers/snapdragon_rc_pwm
+	#drivers/lis3mdl
+
+	#
+	# System commands
+	#
+	systemcmds/bl_update
+	systemcmds/mixer
+	systemcmds/param
+	systemcmds/perf
+	systemcmds/pwm
+	systemcmds/esc_calib
+	systemcmds/reboot
+	#systemcmds/topic_listener
+	systemcmds/top
+	systemcmds/config
+	systemcmds/nshterm
+	systemcmds/mtd
+	systemcmds/dumpfile
+	systemcmds/ver
+
+	#
+	# General system control
+	#
+	modules/commander
+	modules/navigator
+	modules/mavlink
+	modules/gpio_led
+	#modules/uavcan
+	modules/land_detector
+
+	#
+	# Estimation modules (EKF/ SO3 / other filters)
+	#
+	# Too high RAM usage due to static allocations
+	# modules/attitude_estimator_ekf
+	modules/attitude_estimator_q
+	modules/ekf_att_pos_estimator
+	modules/position_estimator_inav
+
+	#
+	# Vehicle Control
+	#
+	# modules/segway # XXX Needs GCC 4.7 fix
+	modules/fw_pos_control_l1
+	modules/fw_att_control
+	modules/mc_att_control
+	modules/mc_pos_control
+	modules/vtol_att_control
+
+	#
+	# Logging
+	#
+	modules/sdlog2
+
+	#
+	# Library modules
+	#
+	modules/param
+	modules/systemlib
+	modules/systemlib/mixer
+	modules/uORB
+	modules/dataman
+
+	#
+	# Libraries
+	#
+	lib/controllib
+	lib/mathlib
+	lib/mathlib/math/filter
+	lib/ecl
+	lib/external_lgpl
+	lib/geo
+	lib/geo_lookup
+	lib/conversion
+	lib/launchdetection
+	lib/terrain_estimation
+	lib/runway_takeoff
+	lib/tailsitter_recovery
+	platforms/nuttx
+
+	# had to add for cmake, not sure why wasn't in original config
+	platforms/common 
+	platforms/nuttx/px4_layer
+
+	#
+	# OBC challenge
+	#
+	#modules/bottle_drop
+
+	#
+	# Rover apps
+	#
+	#examples/rover_steering_control
+
+	#
+	# Demo apps
+	#
+	#examples/math_demo
+	# Tutorial code from
+	# https://px4.io/dev/px4_simple_app
+	#examples/px4_simple_app
+
+	# Tutorial code from
+	# https://px4.io/dev/daemon
+	#examples/px4_daemon_app
+
+	# Tutorial code from
+	# https://px4.io/dev/debug_values
+	#examples/px4_mavlink_debug
+
+	# Tutorial code from
+	# https://px4.io/dev/example_fixedwing_control
+	#examples/fixedwing_control
+
+	# Hardware test
+	#examples/hwtest
+)
+
+set(config_extra_builtin_cmds
+	serdis
+	sercon
+	)
+
+set(config_io_board
+	px4io-v2
+	)
+
+set(config_io_extra_libs
+	)
+
+add_custom_target(sercon)
+set_target_properties(sercon PROPERTIES
+	PRIORITY "SCHED_PRIORITY_DEFAULT"
+	MAIN "sercon" STACK_MAIN "2048")
+
+add_custom_target(serdis)
+set_target_properties(serdis PROPERTIES
+	PRIORITY "SCHED_PRIORITY_DEFAULT"
+	MAIN "serdis" STACK_MAIN "2048")
diff --git a/msg/actuator_armed.msg b/msg/actuator_armed.msg
index f307e366ae..51a7ab116c 100644
--- a/msg/actuator_armed.msg
+++ b/msg/actuator_armed.msg
@@ -1,5 +1,4 @@
 
-uint64 timestamp	# Microseconds since system boot
 bool armed		# Set to true if system is armed
 bool prearmed		# Set to true if the actuator safety is disabled but motors are not armed
 bool ready_to_arm	# Set to true if system is ready to be armed
diff --git a/msg/actuator_controls.msg b/msg/actuator_controls.msg
index a802f1502a..46bf35ebff 100644
--- a/msg/actuator_controls.msg
+++ b/msg/actuator_controls.msg
@@ -10,6 +10,9 @@ uint8 INDEX_AIRBRAKES = 6
 uint8 INDEX_LANDING_GEAR = 7
 uint8 GROUP_INDEX_ATTITUDE = 0
 uint8 GROUP_INDEX_ATTITUDE_ALTERNATE = 1
-uint64 timestamp
 uint64 timestamp_sample	    # the timestamp the data this control response is based on was sampled
 float32[8] control
+
+# TOPICS actuator_controls actuator_controls_0 actuator_controls_1 actuator_controls_2 actuator_controls_3
+# TOPICS actuator_controls_virtual_fw actuator_controls_virtual_mc
+
diff --git a/msg/actuator_controls_1.msg b/msg/actuator_controls_1.msg
deleted file mode 100644
index 414eb06ddb..0000000000
--- a/msg/actuator_controls_1.msg
+++ /dev/null
@@ -1,5 +0,0 @@
-uint8 NUM_ACTUATOR_CONTROLS = 8
-uint8 NUM_ACTUATOR_CONTROL_GROUPS = 4
-uint64 timestamp
-uint64 timestamp_sample	    # the timestamp the data this control response is based on was sampled
-float32[8] control
diff --git a/msg/actuator_controls_2.msg b/msg/actuator_controls_2.msg
deleted file mode 100644
index 414eb06ddb..0000000000
--- a/msg/actuator_controls_2.msg
+++ /dev/null
@@ -1,5 +0,0 @@
-uint8 NUM_ACTUATOR_CONTROLS = 8
-uint8 NUM_ACTUATOR_CONTROL_GROUPS = 4
-uint64 timestamp
-uint64 timestamp_sample	    # the timestamp the data this control response is based on was sampled
-float32[8] control
diff --git a/msg/actuator_controls_3.msg b/msg/actuator_controls_3.msg
deleted file mode 100644
index 414eb06ddb..0000000000
--- a/msg/actuator_controls_3.msg
+++ /dev/null
@@ -1,5 +0,0 @@
-uint8 NUM_ACTUATOR_CONTROLS = 8
-uint8 NUM_ACTUATOR_CONTROL_GROUPS = 4
-uint64 timestamp
-uint64 timestamp_sample	    # the timestamp the data this control response is based on was sampled
-float32[8] control
diff --git a/msg/actuator_controls_virtual_fw.msg b/msg/actuator_controls_virtual_fw.msg
deleted file mode 100644
index 414eb06ddb..0000000000
--- a/msg/actuator_controls_virtual_fw.msg
+++ /dev/null
@@ -1,5 +0,0 @@
-uint8 NUM_ACTUATOR_CONTROLS = 8
-uint8 NUM_ACTUATOR_CONTROL_GROUPS = 4
-uint64 timestamp
-uint64 timestamp_sample	    # the timestamp the data this control response is based on was sampled
-float32[8] control
diff --git a/msg/actuator_direct.msg b/msg/actuator_direct.msg
index c798f5101f..7d22f79e75 100644
--- a/msg/actuator_direct.msg
+++ b/msg/actuator_direct.msg
@@ -1,4 +1,3 @@
 uint8 NUM_ACTUATORS_DIRECT = 16
-uint64 timestamp			# timestamp in us since system boot
 uint32 nvalues				# number of valid values
 float32[16] values			# actuator values, from -1 to 1
diff --git a/msg/actuator_outputs.msg b/msg/actuator_outputs.msg
index 00a3c35b79..269255340d 100644
--- a/msg/actuator_outputs.msg
+++ b/msg/actuator_outputs.msg
@@ -1,5 +1,4 @@
 uint8 NUM_ACTUATOR_OUTPUTS		= 16
 uint8 NUM_ACTUATOR_OUTPUT_GROUPS	= 4	# for sanity checking
-uint64 timestamp			# output timestamp in us since system boot
 uint32 noutputs				# valid outputs
 float32[16] output			# output data, in natural output units
diff --git a/msg/airspeed.msg b/msg/airspeed.msg
index f0a109a7e7..744f9d8830 100644
--- a/msg/airspeed.msg
+++ b/msg/airspeed.msg
@@ -1,4 +1,3 @@
-uint64 timestamp			# microseconds since system boot, needed to integrate
 float32 indicated_airspeed_m_s		# indicated airspeed in meters per second, -1 if unknown
 float32 true_airspeed_m_s		# true filtered airspeed in meters per second, -1 if unknown
 float32 true_airspeed_unfiltered_m_s	# true airspeed in meters per second, -1 if unknown
diff --git a/msg/battery_status.msg b/msg/battery_status.msg
index e5e24cc08f..7dbdc7b1ce 100644
--- a/msg/battery_status.msg
+++ b/msg/battery_status.msg
@@ -1,4 +1,3 @@
-uint64 timestamp		# microseconds since system boot, needed to integrate
 float32 voltage_v		# Battery voltage in volts, 0 if unknown
 float32 voltage_filtered_v	# Battery voltage in volts, filtered, 0 if unknown
 float32 current_a		# Battery current in amperes, -1 if unknown
diff --git a/msg/camera_trigger.msg b/msg/camera_trigger.msg
index b4dcfe8ef3..384e9aa61c 100644
--- a/msg/camera_trigger.msg
+++ b/msg/camera_trigger.msg
@@ -1,4 +1,3 @@
 
-uint64 timestamp	# Timestamp when camera was triggered
 uint32 seq		# Image sequence
 
diff --git a/msg/commander_state.msg b/msg/commander_state.msg
index 0c6a896dc7..a9c92ebb5e 100644
--- a/msg/commander_state.msg
+++ b/msg/commander_state.msg
@@ -14,6 +14,5 @@ uint8 MAIN_STATE_AUTO_LAND = 11
 uint8 MAIN_STATE_AUTO_FOLLOW_TARGET = 12
 uint8 MAIN_STATE_MAX = 13
 
-uint64 timestamp # in microseconds since system start, is set whenever the writing thread stores new data
 
 uint8 main_state		    	# main state machine
diff --git a/msg/control_state.msg b/msg/control_state.msg
index 82af7aeda6..af6de428fa 100644
--- a/msg/control_state.msg
+++ b/msg/control_state.msg
@@ -1,5 +1,4 @@
 # This is similar to the mavlink message CONTROL_SYSTEM_STATE, but for onboard use */
-uint64 timestamp		# in microseconds since system start
 float32 x_acc			# X acceleration in body frame
 float32 y_acc			# Y acceleration in body frame
 float32 z_acc			# Z acceleration in body frame
diff --git a/msg/differential_pressure.msg b/msg/differential_pressure.msg
index bb31449d6c..10fb9eb88e 100644
--- a/msg/differential_pressure.msg
+++ b/msg/differential_pressure.msg
@@ -1,4 +1,3 @@
-uint64 timestamp				# Microseconds since system boot, needed to integrate
 uint64 error_count				# Number of errors detected by driver
 float32 differential_pressure_raw_pa		# Raw differential pressure reading (may be negative)
 float32 differential_pressure_filtered_pa	# Low pass filtered differential pressure reading
diff --git a/msg/distance_sensor.msg b/msg/distance_sensor.msg
index ad01ce63e0..c2e1a171f2 100644
--- a/msg/distance_sensor.msg
+++ b/msg/distance_sensor.msg
@@ -1,6 +1,5 @@
 # DISTANCE_SENSOR message data
 
-uint64 timestamp		# Microseconds since system boot
 
 float32 min_distance		# Minimum distance the sensor can measure (in m)
 float32 max_distance		# Maximum distance the sensor can measure (in m)
diff --git a/msg/ekf2_innovations.msg b/msg/ekf2_innovations.msg
index 0822a473b5..225922b8b6 100644
--- a/msg/ekf2_innovations.msg
+++ b/msg/ekf2_innovations.msg
@@ -1,4 +1,3 @@
-uint64 timestamp		# Timestamp in microseconds since boot
 float32[6] vel_pos_innov  # velocity and position innovations
 float32[3] mag_innov 	# earth magnetic field innovations
 float32 heading_innov 	# heading innovation
diff --git a/msg/encoders.msg b/msg/encoders.msg
index 505365c57e..ef863a9cba 100644
--- a/msg/encoders.msg
+++ b/msg/encoders.msg
@@ -1,5 +1,4 @@
 uint8 NUM_ENCODERS = 4
 
-uint64 timestamp
 int64[4] counts			# counts of encoder
 float32[4] velocity		# counts of encoder/ second
diff --git a/msg/esc_status.msg b/msg/esc_status.msg
index b54131756b..13c0a925f8 100644
--- a/msg/esc_status.msg
+++ b/msg/esc_status.msg
@@ -11,7 +11,6 @@ uint8 ESC_CONNECTION_TYPE_I2C = 3			# I2C
 uint8 ESC_CONNECTION_TYPE_CAN = 4			# CAN-Bus
 
 uint16 counter  					# incremented by the writing thread everytime new data is stored
-uint64 timestamp 					# in microseconds since system start, is set whenever the writing thread stores new data
 
 uint8 esc_count						# number of connected ESCs
 uint8 esc_connectiontype				# how ESCs connected to the system
diff --git a/msg/estimator_status.msg b/msg/estimator_status.msg
index c145a21501..9dea01893d 100644
--- a/msg/estimator_status.msg
+++ b/msg/estimator_status.msg
@@ -1,4 +1,3 @@
-uint64 timestamp		# Timestamp in microseconds since boot
 float32[32] states		# Internal filter states
 float32 n_states		# Number of states effectively used
 float32[3] vibe			# Vibration levels in X, Y and Z
diff --git a/msg/filtered_bottom_flow.msg b/msg/filtered_bottom_flow.msg
index 815a38414d..1de919ee92 100644
--- a/msg/filtered_bottom_flow.msg
+++ b/msg/filtered_bottom_flow.msg
@@ -1,5 +1,4 @@
 # Filtered bottom flow in bodyframe.
-uint64 timestamp		# time of this estimate, in microseconds since system start
 
 float32 sumx				# Integrated bodyframe x flow in meters
 float32 sumy				# Integrated bodyframe y flow in meters
diff --git a/msg/follow_target.msg b/msg/follow_target.msg
index 51ed91a28f..c93f6fc719 100644
--- a/msg/follow_target.msg
+++ b/msg/follow_target.msg
@@ -1,4 +1,3 @@
-uint64 timestamp		# timestamp, UNIX epoch (GPS synced)
 float64 lat			# target position (deg * 1e7)
 float64 lon			# target position (deg * 1e7) 
 float32 alt			# target position 
diff --git a/msg/fw_pos_ctrl_status.msg b/msg/fw_pos_ctrl_status.msg
index 641b591205..0af57b1bf5 100644
--- a/msg/fw_pos_ctrl_status.msg
+++ b/msg/fw_pos_ctrl_status.msg
@@ -1,5 +1,3 @@
-uint64 timestamp		# timestamp this topic was emitted
-
 float32 nav_roll
 float32 nav_pitch
 float32 nav_bearing
diff --git a/msg/fw_virtual_attitude_setpoint.msg b/msg/fw_virtual_attitude_setpoint.msg
index 6c12b225c8..a379477bce 100644
--- a/msg/fw_virtual_attitude_setpoint.msg
+++ b/msg/fw_virtual_attitude_setpoint.msg
@@ -8,7 +8,6 @@
 #
 ###############################################################################################
 
-uint64 timestamp				# in microseconds since system start, is set whenever the writing thread stores new data
 
 float32 roll_body				# body angle in NED frame
 float32 pitch_body				# body angle in NED frame
@@ -35,3 +34,6 @@ bool fw_control_yaw					# control heading with rudder (used for auto takeoff on
 bool disable_mc_yaw_control			# control yaw for mc (used for vtol weather-vane mode)
 
 bool apply_flaps
+
+# WAS vehicle_attitude_setpoint mc_virtual_attitude_setpoint fw_virtual_attitude_setpoint
+# TOPICS fw_virtual_attitude_setpoint
diff --git a/msg/fw_virtual_rates_setpoint.msg b/msg/fw_virtual_rates_setpoint.msg
index 4fa09b563f..369b68270a 100644
--- a/msg/fw_virtual_rates_setpoint.msg
+++ b/msg/fw_virtual_rates_setpoint.msg
@@ -8,7 +8,6 @@
 #
 ###############################################################################################
 
-uint64 timestamp    # in microseconds since system start
 
 float32 roll	    # body angular rates in NED frame
 float32 pitch	    # body angular rates in NED frame
diff --git a/msg/hil_sensor.msg b/msg/hil_sensor.msg
index 9317722db4..e1debf8f6f 100644
--- a/msg/hil_sensor.msg
+++ b/msg/hil_sensor.msg
@@ -17,7 +17,6 @@ int32 MAGNETOMETER_MODE_NEGATIVE_BIAS = 2
 #
 # NOTE: Ordering of fields optimized to align to 32 bit / 4 bytes Change with consideration only
 
-uint64 timestamp			# Timestamp in microseconds since boot, from gyro
 #
 int16[3] gyro_raw			# Raw sensor values of angular velocity
 float32[3] gyro_rad_s			# Angular velocity in radian per seconds
diff --git a/msg/home_position.msg b/msg/home_position.msg
index 7135c07b18..19c9d44868 100644
--- a/msg/home_position.msg
+++ b/msg/home_position.msg
@@ -1,5 +1,4 @@
 # GPS home position in WGS84 coordinates.
-uint64 timestamp			# Timestamp (microseconds since system boot)
 
 float64 lat				# Latitude in degrees
 float64 lon				# Longitude in degrees
diff --git a/msg/manual_control_setpoint.msg b/msg/manual_control_setpoint.msg
index 81d77635ee..8295eba074 100644
--- a/msg/manual_control_setpoint.msg
+++ b/msg/manual_control_setpoint.msg
@@ -12,8 +12,6 @@ int8 MODE_SLOT_5 = 4			# mode slot 5 selected
 int8 MODE_SLOT_6 = 5			# mode slot 6 selected
 int8 MODE_SLOT_MAX = 6			# number of slots plus one
 
-uint64 timestamp
-
 # Any of the channels may not be available and be set to NaN
 # to indicate that it does not contain valid data.
 # The variable names follow the definition of the
diff --git a/msg/mavlink_log.msg b/msg/mavlink_log.msg
index 1161a25543..d52004fad6 100644
--- a/msg/mavlink_log.msg
+++ b/msg/mavlink_log.msg
@@ -1,4 +1,3 @@
-uint64 timestamp		# Timestamp in microseconds since boot
 
 uint8[50] text
 uint8 severity
diff --git a/msg/mc_att_ctrl_status.msg b/msg/mc_att_ctrl_status.msg
index 114e843b0e..c2979c3f00 100644
--- a/msg/mc_att_ctrl_status.msg
+++ b/msg/mc_att_ctrl_status.msg
@@ -1,4 +1,3 @@
-uint64 timestamp    # in microseconds since system start
 
 float32 roll_rate_integ	    # roll rate inegrator
 float32 pitch_rate_integ	# pitch rate integrator
diff --git a/msg/mc_virtual_attitude_setpoint.msg b/msg/mc_virtual_attitude_setpoint.msg
index 6c12b225c8..f2fe44da86 100644
--- a/msg/mc_virtual_attitude_setpoint.msg
+++ b/msg/mc_virtual_attitude_setpoint.msg
@@ -8,7 +8,6 @@
 #
 ###############################################################################################
 
-uint64 timestamp				# in microseconds since system start, is set whenever the writing thread stores new data
 
 float32 roll_body				# body angle in NED frame
 float32 pitch_body				# body angle in NED frame
@@ -35,3 +34,6 @@ bool fw_control_yaw					# control heading with rudder (used for auto takeoff on
 bool disable_mc_yaw_control			# control yaw for mc (used for vtol weather-vane mode)
 
 bool apply_flaps
+
+# WAS vehicle_attitude_setpoint mc_virtual_attitude_setpoint fw_virtual_attitude_setpoint
+# TOPICS mc_virtual_attitude_setpoint
diff --git a/msg/mc_virtual_rates_setpoint.msg b/msg/mc_virtual_rates_setpoint.msg
index 4fa09b563f..369b68270a 100644
--- a/msg/mc_virtual_rates_setpoint.msg
+++ b/msg/mc_virtual_rates_setpoint.msg
@@ -8,7 +8,6 @@
 #
 ###############################################################################################
 
-uint64 timestamp    # in microseconds since system start
 
 float32 roll	    # body angular rates in NED frame
 float32 pitch	    # body angular rates in NED frame
diff --git a/msg/mission.msg b/msg/mission.msg
index ac135a4e09..e208d36929 100644
--- a/msg/mission.msg
+++ b/msg/mission.msg
@@ -1,3 +1,7 @@
 int32 dataman_id	# default 0, there are two offboard storage places in the dataman: 0 or 1
 uint32 count		# count of the missions stored in the dataman
 int32 current_seq	# default -1, start at the one changed latest
+
+# fixme: there is no mission definition in objects_common.cpp
+# but it's required for systemcmds/topic_listener/topic_listener
+# TOPICS mission offboard_mission onboard_mission
diff --git a/msg/offboard_control_mode.msg b/msg/offboard_control_mode.msg
index e2d06963b6..927c8e6503 100644
--- a/msg/offboard_control_mode.msg
+++ b/msg/offboard_control_mode.msg
@@ -1,5 +1,4 @@
 # Off-board control mode
-uint64 timestamp
 
 bool ignore_thrust
 bool ignore_attitude
diff --git a/msg/optical_flow.msg b/msg/optical_flow.msg
index d34e32b8fd..a890df68a4 100644
--- a/msg/optical_flow.msg
+++ b/msg/optical_flow.msg
@@ -1,7 +1,6 @@
 # Optical flow in NED body frame in SI units.
 # @see http://en.wikipedia.org/wiki/International_System_of_Units
  
-uint64 timestamp		# in microseconds since system start
 uint8 sensor_id			# id of the sensor emitting the flow value
 float32 pixel_flow_x_integral	# accumulated optical flow in radians around x axis
 float32 pixel_flow_y_integral	# accumulated optical flow in radians around y axis
diff --git a/msg/parameter_update.msg b/msg/parameter_update.msg
index 78cebf7f4f..3fa04a7657 100644
--- a/msg/parameter_update.msg
+++ b/msg/parameter_update.msg
@@ -1,2 +1 @@
-uint64 timestamp	# time at which the latest parameter was updated
 bool saved		# wether the change has already been saved to disk
diff --git a/msg/pwm_input.msg b/msg/pwm_input.msg
index beb82021d2..8b91883532 100644
--- a/msg/pwm_input.msg
+++ b/msg/pwm_input.msg
@@ -1,5 +1,4 @@
 
-uint64 timestamp	# Microseconds since system boot
 uint64 error_count
 uint32 pulse_width	# Pulse width, timer counts 
 uint32 period		# Period, timer counts
diff --git a/msg/qshell_req.msg b/msg/qshell_req.msg
index bad96a1cec..fa75390edd 100644
--- a/msg/qshell_req.msg
+++ b/msg/qshell_req.msg
@@ -1,4 +1,3 @@
-uint64 timestamp	# Microseconds since system boot
 int32[100] string
 uint64 MAX_STRLEN = 100
 uint64 strlen
diff --git a/msg/rc_channels.msg b/msg/rc_channels.msg
index b8bcc5f536..769a393216 100644
--- a/msg/rc_channels.msg
+++ b/msg/rc_channels.msg
@@ -20,7 +20,7 @@ uint8 RC_CHANNELS_FUNCTION_PARAM_2=17
 uint8 RC_CHANNELS_FUNCTION_PARAM_3_5=18
 uint8 RC_CHANNELS_FUNCTION_RATTITUDE=19
 uint8 RC_CHANNELS_FUNCTION_KILLSWITCH=20
-uint64 timestamp						# Timestamp in microseconds since boot time
+
 uint64 timestamp_last_valid					# Timestamp of last valid RC signal
 float32[18] channels						# Scaled to -1..1 (throttle: 0..1)
 uint8 channel_count						# Number of valid channels
diff --git a/msg/rc_parameter_map.msg b/msg/rc_parameter_map.msg
index 4a94498108..4a5cb5cfd1 100644
--- a/msg/rc_parameter_map.msg
+++ b/msg/rc_parameter_map.msg
@@ -1,7 +1,6 @@
 uint8 RC_PARAM_MAP_NCHAN = 3 # This limit is also hardcoded in the enum RC_CHANNELS_FUNCTION in rc_channels.h
 uint8 PARAM_ID_LEN = 16 # corresponds to MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN
 
-uint64 timestamp	# time at which the map was updated
 bool[3] valid		#true for RC-Param channels which are mapped to a param
 int32[3] param_index	# corresponding param index, this field is ignored if set to -1, in this case param_id will be used
 char[51] param_id	# MAP_NCHAN * (ID_LEN + 1) chars, corresponding param id, null terminated
diff --git a/msg/actuator_controls_0.msg b/msg/ros/actuator_controls_0.msg
similarity index 91%
rename from msg/actuator_controls_0.msg
rename to msg/ros/actuator_controls_0.msg
index 414eb06ddb..4424f197f8 100644
--- a/msg/actuator_controls_0.msg
+++ b/msg/ros/actuator_controls_0.msg
@@ -1,5 +1,4 @@
 uint8 NUM_ACTUATOR_CONTROLS = 8
 uint8 NUM_ACTUATOR_CONTROL_GROUPS = 4
-uint64 timestamp
 uint64 timestamp_sample	    # the timestamp the data this control response is based on was sampled
 float32[8] control
diff --git a/msg/actuator_controls_virtual_mc.msg b/msg/ros/actuator_controls_virtual_mc.msg
similarity index 91%
rename from msg/actuator_controls_virtual_mc.msg
rename to msg/ros/actuator_controls_virtual_mc.msg
index 414eb06ddb..4424f197f8 100644
--- a/msg/actuator_controls_virtual_mc.msg
+++ b/msg/ros/actuator_controls_virtual_mc.msg
@@ -1,5 +1,4 @@
 uint8 NUM_ACTUATOR_CONTROLS = 8
 uint8 NUM_ACTUATOR_CONTROL_GROUPS = 4
-uint64 timestamp
 uint64 timestamp_sample	    # the timestamp the data this control response is based on was sampled
 float32[8] control
diff --git a/msg/safety.msg b/msg/safety.msg
index be0fadad1e..34d66d12cc 100644
--- a/msg/safety.msg
+++ b/msg/safety.msg
@@ -1,3 +1,2 @@
-uint64 timestamp
 bool safety_switch_available		# Set to true if a safety switch is connected
 bool safety_off			# Set to true if safety is off
diff --git a/msg/satellite_info.msg b/msg/satellite_info.msg
index 24cb03facc..0efa030a87 100644
--- a/msg/satellite_info.msg
+++ b/msg/satellite_info.msg
@@ -1,6 +1,5 @@
 uint8 SAT_INFO_MAX_SATELLITES = 20
 
-uint64 timestamp		# Timestamp of satellite info
 uint8 count			# Number of satellites in satellite info
 uint8[20] svid	 		# Space vehicle ID [1..255], see scheme below
 uint8[20] used			# 0: Satellite not used, 1: used for navigation
diff --git a/msg/sensor_accel.msg b/msg/sensor_accel.msg
index 0ed71957ef..9dcefa6b87 100644
--- a/msg/sensor_accel.msg
+++ b/msg/sensor_accel.msg
@@ -1,4 +1,3 @@
-uint64 timestamp
 uint64 integral_dt	# integration time
 uint64 error_count
 float32 x		# acceleration in the NED X board axis in m/s^2
diff --git a/msg/sensor_baro.msg b/msg/sensor_baro.msg
index 315d5a56ec..9fe621f1c8 100644
--- a/msg/sensor_baro.msg
+++ b/msg/sensor_baro.msg
@@ -1,5 +1,4 @@
 float32 pressure
 float32 altitude
 float32 temperature
-uint64 timestamp
 uint64 error_count
diff --git a/msg/sensor_combined.msg b/msg/sensor_combined.msg
index ec58e6f923..fab3dcad02 100644
--- a/msg/sensor_combined.msg
+++ b/msg/sensor_combined.msg
@@ -25,7 +25,6 @@ uint32 SENSOR_PRIO_MAX = 255
 #
 # NOTE: Ordering of fields optimized to align to 32 bit / 4 bytes Change with consideration only
 
-uint64 timestamp			# Timestamp in microseconds since boot, from gyro
 uint64[3] gyro_timestamp			# Gyro timestamps
 int16[9] gyro_raw			# Raw sensor values of angular velocity
 float32[9] gyro_rad_s			# Angular velocity in radian per seconds
diff --git a/msg/sensor_gyro.msg b/msg/sensor_gyro.msg
index badaec0eb5..31636d148a 100644
--- a/msg/sensor_gyro.msg
+++ b/msg/sensor_gyro.msg
@@ -1,4 +1,3 @@
-uint64 timestamp
 uint64 integral_dt	# integration time
 uint64 error_count
 float32 x		# angular velocity in the NED X board axis in rad/s
diff --git a/msg/sensor_mag.msg b/msg/sensor_mag.msg
index 8ea03d1fc3..662ebdbb79 100644
--- a/msg/sensor_mag.msg
+++ b/msg/sensor_mag.msg
@@ -1,4 +1,3 @@
-uint64 timestamp
 uint64 error_count
 float32 x
 float32 y
diff --git a/msg/servorail_status.msg b/msg/servorail_status.msg
index f473cc1e28..39edf80e4f 100644
--- a/msg/servorail_status.msg
+++ b/msg/servorail_status.msg
@@ -1,3 +1,2 @@
-uint64 timestamp		# microseconds since system boot
 float32 voltage_v		# Servo rail voltage in volts
 float32 rssi_v			# RSSI pin voltage in volts
diff --git a/msg/system_power.msg b/msg/system_power.msg
index cb2a7c0eb0..e6648ceec7 100644
--- a/msg/system_power.msg
+++ b/msg/system_power.msg
@@ -1,4 +1,3 @@
-uint64 timestamp		# microseconds since system boot
 float32 voltage5V_v		# peripheral 5V rail voltage
 uint8 usb_connected		# USB is connected when 1
 uint8 brick_valid		# brick power is good when 1
diff --git a/msg/tecs_status.msg b/msg/tecs_status.msg
index 4dcd16b2ca..7576112c57 100644
--- a/msg/tecs_status.msg
+++ b/msg/tecs_status.msg
@@ -6,7 +6,6 @@ uint8 TECS_MODE_LAND_THROTTLELIM = 4
 uint8 TECS_MODE_BAD_DESCENT = 5
 uint8 TECS_MODE_CLIMBOUT = 6
 
-uint64 timestamp		# timestamp, in microseconds since system start */
 
 float32 altitudeSp
 float32 altitude_filtered
diff --git a/msg/telemetry_status.msg b/msg/telemetry_status.msg
index cdcef931af..e7c653c6f5 100644
--- a/msg/telemetry_status.msg
+++ b/msg/telemetry_status.msg
@@ -4,7 +4,6 @@ uint8 TELEMETRY_STATUS_RADIO_TYPE_UBIQUITY_BULLET = 2
 uint8 TELEMETRY_STATUS_RADIO_TYPE_WIRE = 3
 uint8 TELEMETRY_STATUS_RADIO_TYPE_USB = 4
 
-uint64 timestamp
 uint64 heartbeat_time			# Time of last received heartbeat from remote system
 uint64 telem_time			# Time of last received telemetry status packet, 0 for none
 uint8 type				# type of the radio hardware
diff --git a/msg/templates/px4/uorb/msg.h.template b/msg/templates/px4/uorb/msg.h.template
index 2d42511074..1707dfc015 100644
--- a/msg/templates/px4/uorb/msg.h.template
+++ b/msg/templates/px4/uorb/msg.h.template
@@ -55,7 +55,6 @@
 import genmsg.msgs
 import gencpp
 
-cpp_class = 'px4_%s'%spec.short_name
 native_type = '%s_s'%spec.short_name
 topic_name = spec.short_name
 }@
@@ -74,6 +73,11 @@ topic_name = spec.short_name
 
 namespace px4
 {
+@[for multi_topic in topics]@
+@{
+cpp_class = 'px4_%s'%multi_topic
+}@
+
 
 class __EXPORT @(cpp_class) :
 	public PX4Message<@(native_type)>
@@ -89,7 +93,8 @@ public:
 
 	~@(cpp_class)() {}
 
-	static PX4TopicHandle handle() {return ORB_ID(@(topic_name));}
+	static PX4TopicHandle handle() {return ORB_ID(@(multi_topic));}
 };
+@[end for]
 
 };
diff --git a/msg/templates/uorb/msg.cpp.template b/msg/templates/uorb/msg.cpp.template
new file mode 100644
index 0000000000..ff5afb297e
--- /dev/null
+++ b/msg/templates/uorb/msg.cpp.template
@@ -0,0 +1,132 @@
+@###############################################
+@#
+@# PX4 ROS compatible message source code
+@# generation for C++
+@#
+@# EmPy template for generating <msg>.h files
+@# Based on the original template for ROS
+@#
+@###############################################
+@# Start of Template
+@#
+@# Context:
+@#  - file_name_in (String) Source file
+@#  - spec (msggen.MsgSpec) Parsed specification of the .msg file
+@#  - md5sum (String) MD5Sum of the .msg specification
+@###############################################
+/****************************************************************************
+ *
+ *   Copyright (C) 2013-2015 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/* Auto-generated by genmsg_cpp from file @file_name_in */
+
+@{
+import genmsg.msgs
+import gencpp
+
+uorb_struct = '%s_s'%spec.short_name
+uorb_pack_func = 'pack_%s'%spec.short_name
+topic_name = spec.short_name
+
+type_map = {
+  'int8': 'int8_t',
+  'int16': 'int16_t',
+  'int32': 'int32_t',
+  'int64': 'int64_t',
+  'uint8': 'uint8_t',
+  'uint16': 'uint16_t',
+  'uint32': 'uint32_t',
+  'uint64': 'uint64_t',
+  'float32': 'float',
+  'float64': 'double',
+  'bool': 'bool',
+  'char': 'char',
+  'fence_vertex': 'fence_vertex',
+  'position_setpoint': 'position_setpoint',
+  'esc_report': 'esc_report'
+}
+
+msgtype_size_map = {
+  'int8': 8,
+  'int16': 16,
+  'int32': 32,
+  'int64': 64,
+  'uint8': 8,
+  'uint16': 16,
+  'uint32': 32,
+  'uint64': 64,
+  'float32': 32,
+  'float64': 64,
+  'bool': 1,
+  'char': 1,
+  'fence_vertex': 8,
+  'position_setpoint': 104,
+  'esc_report': 36
+}
+
+def convert_type(spec_type):
+    bare_type = spec_type
+    if '/' in spec_type:
+        # removing prefix
+        bare_type = (spec_type.split('/'))[1]
+
+    msg_type, is_array, array_length = genmsg.msgs.parse_type(bare_type)
+    c_type = type_map[msg_type]
+    if is_array:
+        return c_type + "[" + str(array_length) + "]"
+    return c_type
+
+def bare_name(msg_type):
+    bare = msg_type
+    if '/' in msg_type:
+        # removing prefix
+        bare = (msg_type.split('/'))[1]
+    # removing suffix
+    return bare.split('[')[0]
+
+def sizeof_field_type(field):
+    return msgtype_size_map[bare_name(field.type)]
+
+sorted_fields = sorted(spec.parsed_fields(), key = sizeof_field_type, reverse=True)
+topic_fields = ["%s %s" % (convert_type(field.type), field.name) for field in sorted_fields]
+}@
+
+#include <px4_config.h>
+#include <drivers/drv_orb_dev.h>
+#include <uORB/topics/@(topic_name).h>
+
+@# join all msg files in one line e.g: "float[3] position;float[3] velocity;bool armed"
+const char *__orb_@(topic_name)_fields = "@( topic_name.upper() ):@( ";".join(topic_fields) );";
+
+@[for multi_topic in topics]@
+ORB_DEFINE(@multi_topic, struct @uorb_struct, __orb_@(topic_name)_fields);
+@[end for]
diff --git a/msg/templates/uorb/msg.h.template b/msg/templates/uorb/msg.h.template
index 78e1992e06..7fe48e593d 100644
--- a/msg/templates/uorb/msg.h.template
+++ b/msg/templates/uorb/msg.h.template
@@ -75,7 +75,6 @@ for field in spec.parsed_fields():
       (package, name) = genmsg.names.package_resource_name(field.base_type)
       package = package or spec.package # convert '' to package
       print('#include <uORB/topics/%s.h>'%(name))
-
 }@
 
 @# Constants c style
@@ -85,17 +84,12 @@ for field in spec.parsed_fields():
 @[end for]
 #endif
 
-/**
- * @@addtogroup topics
- * @@{
- */
-
 @##############################
 @# Main struct of message
 @##############################
 @{
-
-type_map = {'int8': 'int8_t',
+type_map = {
+  'int8': 'int8_t',
   'int16': 'int16_t',
   'int32': 'int32_t',
   'int64': 'int64_t',
@@ -109,37 +103,77 @@ type_map = {'int8': 'int8_t',
   'char': 'char',
   'fence_vertex': 'fence_vertex',
   'position_setpoint': 'position_setpoint',
-  'esc_report': 'esc_report'}
+  'esc_report': 'esc_report'
+}
+
+msgtype_size_map = {
+  'int8': 8,
+  'int16': 16,
+  'int32': 32,
+  'int64': 64,
+  'uint8': 8,
+  'uint16': 16,
+  'uint32': 32,
+  'uint64': 64,
+  'float32': 32,
+  'float64': 64,
+  'bool': 1,
+  'char': 1,
+  'fence_vertex': 8,
+  'position_setpoint': 104,
+  'esc_report': 36
+}
+
+def bare_name(msg_type):
+    bare = msg_type
+    if '/' in msg_type:
+        # removing prefix
+        bare = (msg_type.split('/'))[1]
+    # removing suffix
+    return bare.split('[')[0]
+
+def sizeof_field_type(field):
+    return msgtype_size_map[bare_name(field.type)]
 
 # Function to print a standard ros type
 def print_field_def(field):
-  type = field.type
+  type_name = field.type
   # detect embedded types
-  sl_pos = type.find('/')
+  sl_pos = type_name.find('/')
   type_appendix = ''
   type_prefix = ''
   if (sl_pos >= 0):
-    type = type[sl_pos + 1:]
+    type_name = type_name[sl_pos + 1:]
     type_prefix = 'struct '
     type_appendix = '_s'
 
   # detect arrays
-  a_pos = type.find('[')
+  a_pos = type_name.find('[')
   array_size = ''
   if (a_pos >= 0):
     # field is array
-    array_size = type[a_pos:]
-    type = type[:a_pos]
+    array_size = type_name[a_pos:]
+    type_name = type_name[:a_pos]
 
-  if type in type_map:
+  if type_name in type_map:
     # need to add _t: int8 --> int8_t
-    type_px4 = type_map[type]
+    type_px4 = type_map[type_name]
   else:
-    raise Exception("Type {0} not supported, add to to template file!".format(type))
+    raise Exception("Type {0} not supported, add to to template file!".format(type_name))
 
   print('\t%s%s%s %s%s;'%(type_prefix, type_px4, type_appendix, field.name, array_size))
 
-}
+def print_parsed_fields():
+  # sort fields
+  sorted_fields = sorted(spec.parsed_fields(), key = sizeof_field_type, reverse=True)
+  # loop over all fields and print the type and name
+  for field in sorted_fields:
+    if (not field.is_header):
+      print_field_def(field)
+
+}@
+
+// #pragma pack(push, 1) badly breaks alignment everywhere!
 #ifdef __cplusplus
 @#class @(uorb_struct) {
 struct __EXPORT @(uorb_struct) {
@@ -147,31 +181,27 @@ struct __EXPORT @(uorb_struct) {
 #else
 struct @(uorb_struct) {
 #endif
-@{
-# loop over all fields and print the type and name
-for field in spec.parsed_fields():
-  if (not field.is_header):
-    print_field_def(field)
-}@
+@# timestamp at the beginning of each topic is required for logger
+	uint64_t timestamp; // required for logger
+@print_parsed_fields()
 #ifdef __cplusplus
 @# Constants again c++-ified
 @{
 for constant in spec.constants:
-  type = constant.type
-  if type in type_map:
+  type_name = constant.type
+  if type_name in type_map:
     # need to add _t: int8 --> int8_t
-    type_px4 = type_map[type]
+    type_px4 = type_map[type_name]
   else:
-    raise Exception("Type {0} not supported, add to to template file!".format(type))
+    raise Exception("Type {0} not supported, add to to template file!".format(type_name))
 
   print('\tstatic const %s %s = %s;'%(type_px4, constant.name, int(constant.val)))
 }
 #endif
 };
-
-/**
- * @@}
- */
+//#pragma pack(pop)
 
 /* register this as object request broker structure */
-ORB_DECLARE(@(topic_name));
+@[for multi_topic in topics]@
+ORB_DECLARE(@multi_topic);
+@[end for]
diff --git a/msg/templates/uorb/uORBTopics.cpp.template b/msg/templates/uorb/uORBTopics.cpp.template
new file mode 100644
index 0000000000..91a3a7e640
--- /dev/null
+++ b/msg/templates/uorb/uORBTopics.cpp.template
@@ -0,0 +1,70 @@
+@###############################################
+@#
+@# EmPy template for generating uORBTopics.cpp file
+@# for logging purposes
+@#
+@###############################################
+@# Start of Template
+@#
+@# Context:
+@#  - msgs (List) list of all msg files
+@###############################################
+/****************************************************************************
+ *
+ *   Copyright (C) 2013-2015 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+#include <uORB/uORBTopics.h>
+#include <uORB/uORB.h>
+@{
+msgs_count = len(msgs)
+msg_names = [mn.replace(".msg", "") for mn in msgs]
+}@
+@[for msg_name in msg_names]@
+#include <uORB/topics/@(msg_name).h>
+@[end for]
+
+const size_t _uorb_topics_count = @(msgs_count);
+const struct orb_metadata* _uorb_topics_list[_uorb_topics_count] = { 
+@[for idx, msg_name in enumerate(msg_names, 1)]@
+    ORB_ID(@(msg_name))@[if idx != msgs_count],@[end if]
+@[end for]
+};
+
+size_t orb_topics_count()
+{
+	return _uorb_topics_count;
+}
+
+const struct orb_metadata **orb_get_topics()
+{
+	return _uorb_topics_list;
+}
diff --git a/msg/test_motor.msg b/msg/test_motor.msg
index ec06e64fd6..2805fa64f5 100644
--- a/msg/test_motor.msg
+++ b/msg/test_motor.msg
@@ -1,5 +1,4 @@
 uint8 NUM_MOTOR_OUTPUTS = 8
 
-uint64 timestamp				# output timestamp in us since system boot
 uint32 motor_number				# number of motor to spin
 float32 value					# output power, range [0..1]
diff --git a/msg/transponder_report.msg b/msg/transponder_report.msg
index eac37d713f..c02fb8cc15 100644
--- a/msg/transponder_report.msg
+++ b/msg/transponder_report.msg
@@ -1,4 +1,3 @@
-uint64 timestamp	# Timestamp of this position report
 uint32 ICAO_address 	# ICAO address
 float64 lat 		# Latitude, expressed as degrees
 float64 lon 		# Longitude, expressed as degrees
diff --git a/msg/vehicle_attitude.msg b/msg/vehicle_attitude.msg
index 0ee90cd61d..2c64c32e13 100644
--- a/msg/vehicle_attitude.msg
+++ b/msg/vehicle_attitude.msg
@@ -1,5 +1,4 @@
 # This is similar to the mavlink message ATTITUDE, but for onboard use */
-uint64 timestamp	# in microseconds since system start
 # @warning roll, pitch and yaw have always to be valid, the rotation matrix and quaternion are optional
 float32 roll		# Roll angle (rad, Tait-Bryan, NED)
 float32 pitch		# Pitch angle (rad, Tait-Bryan, NED)
diff --git a/msg/vehicle_attitude_setpoint.msg b/msg/vehicle_attitude_setpoint.msg
index 6c12b225c8..2bbbe748fd 100644
--- a/msg/vehicle_attitude_setpoint.msg
+++ b/msg/vehicle_attitude_setpoint.msg
@@ -8,7 +8,6 @@
 #
 ###############################################################################################
 
-uint64 timestamp				# in microseconds since system start, is set whenever the writing thread stores new data
 
 float32 roll_body				# body angle in NED frame
 float32 pitch_body				# body angle in NED frame
@@ -35,3 +34,6 @@ bool fw_control_yaw					# control heading with rudder (used for auto takeoff on
 bool disable_mc_yaw_control			# control yaw for mc (used for vtol weather-vane mode)
 
 bool apply_flaps
+
+# WAS vehicle_attitude_setpoint mc_virtual_attitude_setpoint fw_virtual_attitude_setpoint
+# TOPICS vehicle_attitude_setpoint
diff --git a/msg/vehicle_control_mode.msg b/msg/vehicle_control_mode.msg
index 5ad1bbb9b2..32ec589061 100644
--- a/msg/vehicle_control_mode.msg
+++ b/msg/vehicle_control_mode.msg
@@ -1,5 +1,4 @@
 
-uint64 timestamp				# in microseconds since system start
 						# is set whenever the writing thread stores new data
 
 bool flag_armed
diff --git a/msg/vehicle_global_position.msg b/msg/vehicle_global_position.msg
index 8aa7917df0..39b815015a 100644
--- a/msg/vehicle_global_position.msg
+++ b/msg/vehicle_global_position.msg
@@ -4,7 +4,6 @@
 # estimator, which will take more sources of information into account than just GPS,
 # e.g. control inputs of the vehicle in a Kalman-filter implementation.
 #
-uint64 timestamp		# Time of this estimate since system start, (microseconds)
 uint64 time_utc_usec		# GPS UTC timestamp, (microseconds)
 float64 lat			# Latitude, (degrees)
 float64 lon			# Longitude, (degrees)
diff --git a/msg/vehicle_land_detected.msg b/msg/vehicle_land_detected.msg
index d886d31b05..e7f741b8bf 100644
--- a/msg/vehicle_land_detected.msg
+++ b/msg/vehicle_land_detected.msg
@@ -1,3 +1,2 @@
-uint64 timestamp	# timestamp of the setpoint
 bool landed		# true if vehicle is currently landed on the ground
 bool freefall	# true if vehicle is currently in free-fall
diff --git a/msg/vehicle_local_position.msg b/msg/vehicle_local_position.msg
index 89c141c417..bf107056e2 100644
--- a/msg/vehicle_local_position.msg
+++ b/msg/vehicle_local_position.msg
@@ -1,6 +1,5 @@
 # Fused local position in NED.
 
-uint64 timestamp			# Time of this estimate since system start, (microseconds)
 bool xy_valid				# true if x and y are valid
 bool z_valid				# true if z is valid
 bool v_xy_valid				# true if vy and vy are valid
diff --git a/msg/vehicle_local_position_setpoint.msg b/msg/vehicle_local_position_setpoint.msg
index 927e9ab5ad..a5407519bb 100644
--- a/msg/vehicle_local_position_setpoint.msg
+++ b/msg/vehicle_local_position_setpoint.msg
@@ -1,6 +1,5 @@
 # Local position setpoint in NED frame
 
-uint64 timestamp	# timestamp of the setpoint
 float32 x		# in meters NED
 float32 y		# in meters NED
 float32 z		# in meters NED
diff --git a/msg/vehicle_rates_setpoint.msg b/msg/vehicle_rates_setpoint.msg
index 4fa09b563f..369b68270a 100644
--- a/msg/vehicle_rates_setpoint.msg
+++ b/msg/vehicle_rates_setpoint.msg
@@ -8,7 +8,6 @@
 #
 ###############################################################################################
 
-uint64 timestamp    # in microseconds since system start
 
 float32 roll	    # body angular rates in NED frame
 float32 pitch	    # body angular rates in NED frame
diff --git a/msg/vehicle_status.msg b/msg/vehicle_status.msg
index 4e8ec96553..0444a577e1 100644
--- a/msg/vehicle_status.msg
+++ b/msg/vehicle_status.msg
@@ -41,7 +41,6 @@ uint8 RC_IN_MODE_GENERATED = 2
 
 # state machine / state of vehicle.
 # Encodes the complete system state and is set by the commander app.
-uint64 timestamp # in microseconds since system start, is set whenever the writing thread stores new data
 
 uint8 nav_state				# set navigation state machine to specified value
 uint8 arming_state			# current arming state
diff --git a/msg/vtol_vehicle_status.msg b/msg/vtol_vehicle_status.msg
index 27b804c437..5334e291ca 100644
--- a/msg/vtol_vehicle_status.msg
+++ b/msg/vtol_vehicle_status.msg
@@ -5,7 +5,6 @@ uint8 VEHICLE_VTOL_STATE_TRANSITION_TO_MC = 2
 uint8 VEHICLE_VTOL_STATE_MC = 3
 uint8 VEHICLE_VTOL_STATE_FW = 4
 
-uint64 timestamp				# Microseconds since system boot
 bool vtol_in_rw_mode			# true: vtol vehicle is in rotating wing mode
 bool vtol_in_trans_mode
 bool vtol_transition_failsafe	# vtol in transition failsafe mode
diff --git a/msg/wind_estimate.msg b/msg/wind_estimate.msg
index fbd7b638b8..a61df3252a 100644
--- a/msg/wind_estimate.msg
+++ b/msg/wind_estimate.msg
@@ -1,4 +1,3 @@
-uint64 timestamp		# Microseconds since system boot
 float32 windspeed_north		# Wind component in north / X direction
 float32 windspeed_east		# Wind component in east / Y direction
 float32 covariance_north	# Uncertainty - set to zero (no uncertainty) if not estimated
diff --git a/src/drivers/airspeed/airspeed.cpp b/src/drivers/airspeed/airspeed.cpp
index 771e209489..0f4ffd1671 100644
--- a/src/drivers/airspeed/airspeed.cpp
+++ b/src/drivers/airspeed/airspeed.cpp
@@ -375,12 +375,11 @@ Airspeed::update_status()
 {
 	if (_sensor_ok != _last_published_sensor_ok) {
 		/* notify about state change */
-		struct subsystem_info_s info = {
-			true,
-			true,
-			_sensor_ok,
-			subsystem_info_s::SUBSYSTEM_TYPE_DIFFPRESSURE
-		};
+		struct subsystem_info_s info = {};
+		info.present = true;
+		info.enabled = true;
+		info.ok = _sensor_ok;
+		info.subsystem_type = subsystem_info_s::SUBSYSTEM_TYPE_DIFFPRESSURE;
 
 		if (_subsys_pub != nullptr) {
 			orb_publish(ORB_ID(subsystem_info), _subsys_pub, &info);
diff --git a/src/drivers/ardrone_interface/ardrone_interface.c b/src/drivers/ardrone_interface/ardrone_interface.c
index 8ad0ee7168..f76f0bc639 100644
--- a/src/drivers/ardrone_interface/ardrone_interface.c
+++ b/src/drivers/ardrone_interface/ardrone_interface.c
@@ -55,10 +55,6 @@
 #include <uORB/uORB.h>
 #include <uORB/topics/safety.h>
 #include <uORB/topics/actuator_controls.h>
-#include <uORB/topics/actuator_controls_0.h>
-#include <uORB/topics/actuator_controls_1.h>
-#include <uORB/topics/actuator_controls_2.h>
-#include <uORB/topics/actuator_controls_3.h>
 #include <uORB/topics/actuator_armed.h>
 #include <uORB/topics/vehicle_control_mode.h>
 
diff --git a/src/drivers/mb12xx/mb12xx.cpp b/src/drivers/mb12xx/mb12xx.cpp
index 9a6fa36d79..56886a99eb 100644
--- a/src/drivers/mb12xx/mb12xx.cpp
+++ b/src/drivers/mb12xx/mb12xx.cpp
@@ -616,12 +616,12 @@ MB12XX::start()
 	work_queue(HPWORK, &_work, (worker_t)&MB12XX::cycle_trampoline, this, 5);
 
 	/* notify about state change */
-	struct subsystem_info_s info = {
-		true,
-		true,
-		true,
-		subsystem_info_s::SUBSYSTEM_TYPE_RANGEFINDER
-	};
+	struct subsystem_info_s info = {};
+	info.present = true;
+	info.enabled = true;
+	info.ok = true;
+	info.subsystem_type = subsystem_info_s::SUBSYSTEM_TYPE_RANGEFINDER;
+
 	static orb_advert_t pub = nullptr;
 
 	if (pub != nullptr) {
diff --git a/src/drivers/mkblctrl/mkblctrl.cpp b/src/drivers/mkblctrl/mkblctrl.cpp
index fd0ac59551..3e53e32d47 100644
--- a/src/drivers/mkblctrl/mkblctrl.cpp
+++ b/src/drivers/mkblctrl/mkblctrl.cpp
@@ -75,7 +75,6 @@
 #include <systemlib/mixer/mixer.h>
 
 #include <uORB/topics/actuator_controls.h>
-#include <uORB/topics/actuator_controls_0.h>
 #include <uORB/topics/actuator_outputs.h>
 #include <uORB/topics/actuator_armed.h>
 #include <uORB/topics/esc_status.h>
diff --git a/src/drivers/pwm_out_sim/pwm_out_sim.cpp b/src/drivers/pwm_out_sim/pwm_out_sim.cpp
index 457e85bb91..20f9618c98 100644
--- a/src/drivers/pwm_out_sim/pwm_out_sim.cpp
+++ b/src/drivers/pwm_out_sim/pwm_out_sim.cpp
@@ -77,10 +77,6 @@
 #include <systemlib/mixer/mixer.h>
 
 #include <uORB/topics/actuator_controls.h>
-#include <uORB/topics/actuator_controls_0.h>
-#include <uORB/topics/actuator_controls_1.h>
-#include <uORB/topics/actuator_controls_2.h>
-#include <uORB/topics/actuator_controls_3.h>
 #include <uORB/topics/actuator_armed.h>
 #include <uORB/topics/actuator_outputs.h>
 
diff --git a/src/drivers/px4flow/px4flow.cpp b/src/drivers/px4flow/px4flow.cpp
index 13f69a3213..095d70e38b 100644
--- a/src/drivers/px4flow/px4flow.cpp
+++ b/src/drivers/px4flow/px4flow.cpp
@@ -592,12 +592,12 @@ PX4FLOW::start()
 	work_queue(HPWORK, &_work, (worker_t)&PX4FLOW::cycle_trampoline, this, 1);
 
 	/* notify about state change */
-	struct subsystem_info_s info = {
-		true,
-		true,
-		true,
-		subsystem_info_s::SUBSYSTEM_TYPE_OPTICALFLOW
-	};
+	struct subsystem_info_s info = {};
+	info.present = true;
+	info.enabled = true;
+	info.ok = true;
+	info.subsystem_type = subsystem_info_s::SUBSYSTEM_TYPE_OPTICALFLOW;
+
 	static orb_advert_t pub = nullptr;
 
 	if (pub != nullptr) {
diff --git a/src/drivers/px4fmu/fmu.cpp b/src/drivers/px4fmu/fmu.cpp
index 4c39f3b7c4..d74765f6a1 100644
--- a/src/drivers/px4fmu/fmu.cpp
+++ b/src/drivers/px4fmu/fmu.cpp
@@ -79,10 +79,6 @@
 #include <lib/rc/sumd.h>
 
 #include <uORB/topics/actuator_controls.h>
-#include <uORB/topics/actuator_controls_0.h>
-#include <uORB/topics/actuator_controls_1.h>
-#include <uORB/topics/actuator_controls_2.h>
-#include <uORB/topics/actuator_controls_3.h>
 #include <uORB/topics/actuator_outputs.h>
 #include <uORB/topics/actuator_armed.h>
 #include <uORB/topics/parameter_update.h>
diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp
index 000d04e491..a1e1525514 100644
--- a/src/drivers/px4io/px4io.cpp
+++ b/src/drivers/px4io/px4io.cpp
@@ -77,10 +77,6 @@
 #include <systemlib/battery.h>
 
 #include <uORB/topics/actuator_controls.h>
-#include <uORB/topics/actuator_controls_0.h>
-#include <uORB/topics/actuator_controls_1.h>
-#include <uORB/topics/actuator_controls_2.h>
-#include <uORB/topics/actuator_controls_3.h>
 #include <uORB/topics/actuator_outputs.h>
 #include <uORB/topics/actuator_armed.h>
 #include <uORB/topics/safety.h>
diff --git a/src/drivers/roboclaw/RoboClaw.cpp b/src/drivers/roboclaw/RoboClaw.cpp
index c1f4a01960..d3eda8609e 100644
--- a/src/drivers/roboclaw/RoboClaw.cpp
+++ b/src/drivers/roboclaw/RoboClaw.cpp
@@ -54,7 +54,6 @@
 #include <arch/board/board.h>
 
 #include <uORB/Publication.hpp>
-#include <uORB/topics/actuator_controls_0.h>
 #include <uORB/topics/debug_key_value.h>
 #include <drivers/drv_hrt.h>
 
diff --git a/src/drivers/snapdragon_rc_pwm/snapdragon_rc_pwm.cpp b/src/drivers/snapdragon_rc_pwm/snapdragon_rc_pwm.cpp
index 438151ae4e..ce86147904 100644
--- a/src/drivers/snapdragon_rc_pwm/snapdragon_rc_pwm.cpp
+++ b/src/drivers/snapdragon_rc_pwm/snapdragon_rc_pwm.cpp
@@ -51,7 +51,6 @@
 #include <termios.h>
 #include <uORB/topics/input_rc.h>
 #include <uORB/topics/actuator_controls.h>
-#include <uORB/topics/actuator_controls_0.h>
 #include <v1.0/mavlink_types.h>
 #include <v1.0/common/mavlink.h>
 
diff --git a/src/drivers/srf02/srf02.cpp b/src/drivers/srf02/srf02.cpp
index f585acb6a6..b5600e2a73 100644
--- a/src/drivers/srf02/srf02.cpp
+++ b/src/drivers/srf02/srf02.cpp
@@ -618,12 +618,12 @@ SRF02::start()
 	work_queue(HPWORK, &_work, (worker_t)&SRF02::cycle_trampoline, this, 5);
 
 	/* notify about state change */
-	struct subsystem_info_s info = {
-		true,
-		true,
-		true,
-		subsystem_info_s::SUBSYSTEM_TYPE_RANGEFINDER
-	};
+	struct subsystem_info_s info = {};
+	info.present = true;
+	info.enabled = true;
+	info.ok = true;
+	info.subsystem_type = subsystem_info_s::SUBSYSTEM_TYPE_RANGEFINDER;
+
 	static orb_advert_t pub = nullptr;
 
 	if (pub != nullptr) {
diff --git a/src/drivers/trone/trone.cpp b/src/drivers/trone/trone.cpp
index 3ec40c624d..aea4bc9449 100644
--- a/src/drivers/trone/trone.cpp
+++ b/src/drivers/trone/trone.cpp
@@ -628,12 +628,12 @@ TRONE::start()
 	work_queue(HPWORK, &_work, (worker_t)&TRONE::cycle_trampoline, this, 1);
 
 	/* notify about state change */
-	struct subsystem_info_s info = {
-		true,
-		true,
-		true,
-		subsystem_info_s::SUBSYSTEM_TYPE_RANGEFINDER
-	};
+	struct subsystem_info_s info = {};
+	info.present = true;
+	info.enabled = true;
+	info.ok = true;
+	info.subsystem_type = subsystem_info_s::SUBSYSTEM_TYPE_RANGEFINDER;
+
 	static orb_advert_t pub = nullptr;
 
 	if (pub != nullptr) {
diff --git a/src/examples/fixedwing_control/main.c b/src/examples/fixedwing_control/main.c
index 3d6aa88269..f8a9de39ce 100644
--- a/src/examples/fixedwing_control/main.c
+++ b/src/examples/fixedwing_control/main.c
@@ -61,10 +61,6 @@
 #include <uORB/topics/vehicle_attitude_setpoint.h>
 #include <uORB/topics/manual_control_setpoint.h>
 #include <uORB/topics/actuator_controls.h>
-#include <uORB/topics/actuator_controls_0.h>
-#include <uORB/topics/actuator_controls_1.h>
-#include <uORB/topics/actuator_controls_2.h>
-#include <uORB/topics/actuator_controls_3.h>
 #include <uORB/topics/vehicle_rates_setpoint.h>
 #include <uORB/topics/vehicle_global_position.h>
 #include <uORB/topics/parameter_update.h>
diff --git a/src/examples/hwtest/hwtest.c b/src/examples/hwtest/hwtest.c
index 97a525a8e9..b1b1b465f9 100644
--- a/src/examples/hwtest/hwtest.c
+++ b/src/examples/hwtest/hwtest.c
@@ -47,10 +47,6 @@
 #include <systemlib/err.h>
 #include <uORB/topics/actuator_armed.h>
 #include <uORB/topics/actuator_controls.h>
-#include <uORB/topics/actuator_controls_0.h>
-#include <uORB/topics/actuator_controls_1.h>
-#include <uORB/topics/actuator_controls_2.h>
-#include <uORB/topics/actuator_controls_3.h>
 #include <uORB/uORB.h>
 
 __EXPORT int ex_hwtest_main(int argc, char *argv[]);
diff --git a/src/examples/rover_steering_control/main.cpp b/src/examples/rover_steering_control/main.cpp
index 7a227c2366..563d6b0cca 100644
--- a/src/examples/rover_steering_control/main.cpp
+++ b/src/examples/rover_steering_control/main.cpp
@@ -58,10 +58,6 @@
 #include <uORB/topics/vehicle_attitude_setpoint.h>
 #include <uORB/topics/manual_control_setpoint.h>
 #include <uORB/topics/actuator_controls.h>
-#include <uORB/topics/actuator_controls_0.h>
-#include <uORB/topics/actuator_controls_1.h>
-#include <uORB/topics/actuator_controls_2.h>
-#include <uORB/topics/actuator_controls_3.h>
 #include <uORB/topics/vehicle_global_position.h>
 #include <uORB/topics/parameter_update.h>
 #include <systemlib/param/param.h>
diff --git a/src/lib/controllib/uorb/blocks.hpp b/src/lib/controllib/uorb/blocks.hpp
index 7766b67f6b..42aae92284 100644
--- a/src/lib/controllib/uorb/blocks.hpp
+++ b/src/lib/controllib/uorb/blocks.hpp
@@ -47,10 +47,6 @@
 #include <uORB/topics/manual_control_setpoint.h>
 #include <uORB/topics/vehicle_status.h>
 #include <uORB/topics/actuator_controls.h>
-#include <uORB/topics/actuator_controls_0.h>
-#include <uORB/topics/actuator_controls_1.h>
-#include <uORB/topics/actuator_controls_2.h>
-#include <uORB/topics/actuator_controls_3.h>
 #include <uORB/topics/parameter_update.h>
 
 #include <stdio.h>
diff --git a/src/modules/bottle_drop/bottle_drop.cpp b/src/modules/bottle_drop/bottle_drop.cpp
index 97e4b178e6..8cd4ed66eb 100644
--- a/src/modules/bottle_drop/bottle_drop.cpp
+++ b/src/modules/bottle_drop/bottle_drop.cpp
@@ -57,10 +57,6 @@
 #include <uORB/uORB.h>
 #include <uORB/topics/vehicle_command.h>
 #include <uORB/topics/actuator_controls.h>
-#include <uORB/topics/actuator_controls_0.h>
-#include <uORB/topics/actuator_controls_1.h>
-#include <uORB/topics/actuator_controls_2.h>
-#include <uORB/topics/actuator_controls_3.h>
 #include <uORB/topics/wind_estimate.h>
 #include <uORB/topics/parameter_update.h>
 #include <uORB/topics/vehicle_global_position.h>
diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp
index 0d96672654..ba304dd229 100644
--- a/src/modules/commander/commander.cpp
+++ b/src/modules/commander/commander.cpp
@@ -81,10 +81,6 @@
 #include <uORB/topics/vehicle_command.h>
 #include <uORB/topics/subsystem_info.h>
 #include <uORB/topics/actuator_controls.h>
-#include <uORB/topics/actuator_controls_0.h>
-#include <uORB/topics/actuator_controls_1.h>
-#include <uORB/topics/actuator_controls_2.h>
-#include <uORB/topics/actuator_controls_3.h>
 #include <uORB/topics/actuator_armed.h>
 #include <uORB/topics/parameter_update.h>
 #include <uORB/topics/differential_pressure.h>
diff --git a/src/modules/fw_att_control/fw_att_control_main.cpp b/src/modules/fw_att_control/fw_att_control_main.cpp
index 653d471e75..16d1376b38 100644
--- a/src/modules/fw_att_control/fw_att_control_main.cpp
+++ b/src/modules/fw_att_control/fw_att_control_main.cpp
@@ -62,8 +62,6 @@
 #include <uORB/topics/fw_virtual_attitude_setpoint.h>
 #include <uORB/topics/manual_control_setpoint.h>
 #include <uORB/topics/actuator_controls.h>
-#include <uORB/topics/actuator_controls_virtual_fw.h>
-#include <uORB/topics/actuator_controls_virtual_mc.h>
 #include <uORB/topics/vehicle_rates_setpoint.h>
 #include <uORB/topics/fw_virtual_rates_setpoint.h>
 #include <uORB/topics/mc_virtual_rates_setpoint.h>
diff --git a/src/modules/mavlink/mavlink_parameters.cpp b/src/modules/mavlink/mavlink_parameters.cpp
index b24d0464fb..50fc0429be 100644
--- a/src/modules/mavlink/mavlink_parameters.cpp
+++ b/src/modules/mavlink/mavlink_parameters.cpp
@@ -47,8 +47,6 @@
 #include "mavlink_parameters.h"
 #include "mavlink_main.h"
 
-ORB_DEFINE(uavcan_parameter_request, struct uavcan_parameter_request_s);
-ORB_DEFINE(uavcan_parameter_value, struct uavcan_parameter_value_s);
 #define HASH_PARAM "_HASH_CHECK"
 
 MavlinkParametersManager::MavlinkParametersManager(Mavlink *mavlink) : MavlinkStream(mavlink),
diff --git a/src/modules/mc_att_control/mc_att_control_main.cpp b/src/modules/mc_att_control/mc_att_control_main.cpp
index 3ba1f4769e..c06259b74c 100644
--- a/src/modules/mc_att_control/mc_att_control_main.cpp
+++ b/src/modules/mc_att_control/mc_att_control_main.cpp
@@ -70,8 +70,6 @@
 #include <uORB/topics/vehicle_attitude_setpoint.h>
 #include <uORB/topics/manual_control_setpoint.h>
 #include <uORB/topics/actuator_controls.h>
-#include <uORB/topics/actuator_controls_virtual_fw.h>
-#include <uORB/topics/actuator_controls_virtual_mc.h>
 #include <uORB/topics/vehicle_rates_setpoint.h>
 #include <uORB/topics/fw_virtual_rates_setpoint.h>
 #include <uORB/topics/mc_virtual_rates_setpoint.h>
diff --git a/src/modules/position_estimator_inav/position_estimator_inav_main.cpp b/src/modules/position_estimator_inav/position_estimator_inav_main.cpp
index 114bdee530..1677a91d3c 100644
--- a/src/modules/position_estimator_inav/position_estimator_inav_main.cpp
+++ b/src/modules/position_estimator_inav/position_estimator_inav_main.cpp
@@ -52,10 +52,6 @@
 #include <uORB/uORB.h>
 #include <uORB/topics/parameter_update.h>
 #include <uORB/topics/actuator_controls.h>
-#include <uORB/topics/actuator_controls_0.h>
-#include <uORB/topics/actuator_controls_1.h>
-#include <uORB/topics/actuator_controls_2.h>
-#include <uORB/topics/actuator_controls_3.h>
 #include <uORB/topics/actuator_armed.h>
 #include <uORB/topics/sensor_combined.h>
 #include <uORB/topics/vehicle_attitude.h>
diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c
index 5da029a608..0193d90600 100644
--- a/src/modules/sdlog2/sdlog2.c
+++ b/src/modules/sdlog2/sdlog2.c
@@ -78,10 +78,6 @@
 #include <uORB/topics/vehicle_attitude_setpoint.h>
 #include <uORB/topics/vehicle_rates_setpoint.h>
 #include <uORB/topics/actuator_outputs.h>
-#include <uORB/topics/actuator_controls_0.h>
-#include <uORB/topics/actuator_controls_1.h>
-#include <uORB/topics/actuator_controls_2.h>
-#include <uORB/topics/actuator_controls_3.h>
 #include <uORB/topics/actuator_controls.h>
 #include <uORB/topics/vehicle_command.h>
 #include <uORB/topics/vehicle_local_position.h>
diff --git a/src/modules/systemlib/param/param.c b/src/modules/systemlib/param/param.c
index 16e5b9532d..5958399252 100644
--- a/src/modules/systemlib/param/param.c
+++ b/src/modules/systemlib/param/param.c
@@ -136,9 +136,6 @@ UT_array	*param_values;
 /** array info for the modified parameters array */
 const UT_icd	param_icd = {sizeof(struct param_wbuf_s), NULL, NULL, NULL};
 
-/** parameter update topic */
-ORB_DEFINE(parameter_update, struct parameter_update_s);
-
 /** parameter update topic handle */
 static orb_advert_t param_topic = NULL;
 
diff --git a/src/modules/systemlib/param/param_shmem.c b/src/modules/systemlib/param/param_shmem.c
index cefb67a888..5803b292bb 100644
--- a/src/modules/systemlib/param/param_shmem.c
+++ b/src/modules/systemlib/param/param_shmem.c
@@ -158,9 +158,6 @@ UT_array	*param_values;
 /** array info for the modified parameters array */
 const UT_icd	param_icd = {sizeof(struct param_wbuf_s), NULL, NULL, NULL};
 
-/** parameter update topic */
-ORB_DEFINE(parameter_update, struct parameter_update_s);
-
 /** parameter update topic handle */
 static orb_advert_t param_topic = NULL;
 
diff --git a/src/modules/uORB/CMakeLists.txt b/src/modules/uORB/CMakeLists.txt
index d8f8c9bc2a..c8180c29f8 100644
--- a/src/modules/uORB/CMakeLists.txt
+++ b/src/modules/uORB/CMakeLists.txt
@@ -36,8 +36,9 @@ include_directories(
 	${CMAKE_CURRENT_BINARY_DIR}
 	)
 
+link_libraries(msg_gen)
+
 set(SRCS
-	objects_common.cpp
 	uORBUtils.cpp
 	uORB.cpp
 	uORBMain.cpp
diff --git a/src/modules/uORB/Subscription.cpp b/src/modules/uORB/Subscription.cpp
index 0106d81a41..64067967d1 100644
--- a/src/modules/uORB/Subscription.cpp
+++ b/src/modules/uORB/Subscription.cpp
@@ -144,6 +144,12 @@ void Subscription<T>::update()
 	SubscriptionBase::update((void *)(&_data));
 }
 
+template <class T>
+bool Subscription<T>::check_updated()
+{
+	return SubscriptionBase::updated();
+}
+
 template <class T>
 const T &Subscription<T>::get() { return _data; }
 
diff --git a/src/modules/uORB/Subscription.hpp b/src/modules/uORB/Subscription.hpp
index 1d547be842..7b55b4a2a0 100644
--- a/src/modules/uORB/Subscription.hpp
+++ b/src/modules/uORB/Subscription.hpp
@@ -185,6 +185,10 @@ public:
 	 */
 	void update();
 
+	/**
+	 * Create an update function that uses the embedded struct.
+	 */
+	bool check_updated();
 	/*
 	 * This function gets the T struct data
 	 * */
diff --git a/src/modules/uORB/objects_common.cpp b/src/modules/uORB/objects_common.cpp
deleted file mode 100644
index fa381a15e3..0000000000
--- a/src/modules/uORB/objects_common.cpp
+++ /dev/null
@@ -1,305 +0,0 @@
-/****************************************************************************
- *
- *   Copyright (c) 2012-2016 PX4 Development Team. All rights reserved.
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions
- * are met:
- *
- * 1. Redistributions of source code must retain the above copyright
- *    notice, this list of conditions and the following disclaimer.
- * 2. Redistributions in binary form must reproduce the above copyright
- *    notice, this list of conditions and the following disclaimer in
- *    the documentation and/or other materials provided with the
- *    distribution.
- * 3. Neither the name PX4 nor the names of its contributors may be
- *    used to endorse or promote products derived from this software
- *    without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
- * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
- * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- *
- ****************************************************************************/
-
-/**
- * @file objects_common.cpp
- *
- * Common object definitions without a better home.
- */
-
-/**
- * @defgroup topics List of all uORB topics.
- */
-
-#include <px4_config.h>
-
-#include <drivers/drv_orb_dev.h>
-
-#include "topics/sensor_mag.h"
-ORB_DEFINE(sensor_mag, struct sensor_mag_s);
-
-#include "topics/sensor_accel.h"
-ORB_DEFINE(sensor_accel, struct sensor_accel_s);
-
-#include "topics/sensor_gyro.h"
-ORB_DEFINE(sensor_gyro, struct sensor_gyro_s);
-
-#include "topics/sensor_baro.h"
-ORB_DEFINE(sensor_baro, struct sensor_baro_s);
-
-#include "topics/output_pwm.h"
-ORB_DEFINE(output_pwm, struct output_pwm_s);
-
-#include "topics/input_rc.h"
-ORB_DEFINE(input_rc, struct input_rc_s);
-
-#include "topics/pwm_input.h"
-ORB_DEFINE(pwm_input, struct pwm_input_s);
-
-#include "topics/vehicle_attitude.h"
-ORB_DEFINE(vehicle_attitude, struct vehicle_attitude_s);
-
-#include "topics/control_state.h"
-ORB_DEFINE(control_state, struct control_state_s);
-
-#include "topics/sensor_combined.h"
-ORB_DEFINE(sensor_combined, struct sensor_combined_s);
-
-#include "topics/hil_sensor.h"
-ORB_DEFINE(hil_sensor, struct hil_sensor_s);
-
-#include "topics/vehicle_gps_position.h"
-ORB_DEFINE(vehicle_gps_position, struct vehicle_gps_position_s);
-
-#include "topics/vehicle_land_detected.h"
-ORB_DEFINE(vehicle_land_detected, struct vehicle_land_detected_s);
-
-#include "topics/satellite_info.h"
-ORB_DEFINE(satellite_info, struct satellite_info_s);
-
-#include "topics/home_position.h"
-ORB_DEFINE(home_position, struct home_position_s);
-
-#include "topics/vehicle_status.h"
-ORB_DEFINE(vehicle_status, struct vehicle_status_s);
-
-#include "topics/vtol_vehicle_status.h"
-ORB_DEFINE(vtol_vehicle_status, struct vtol_vehicle_status_s);
-
-#include "topics/safety.h"
-ORB_DEFINE(safety, struct safety_s);
-
-#include "topics/battery_status.h"
-ORB_DEFINE(battery_status, struct battery_status_s);
-
-#include "topics/servorail_status.h"
-ORB_DEFINE(servorail_status, struct servorail_status_s);
-
-#include "topics/system_power.h"
-ORB_DEFINE(system_power, struct system_power_s);
-
-#include "topics/vehicle_global_position.h"
-ORB_DEFINE(vehicle_global_position, struct vehicle_global_position_s);
-
-#include "topics/vehicle_local_position.h"
-ORB_DEFINE(vehicle_local_position, struct vehicle_local_position_s);
-
-#include "topics/att_pos_mocap.h"
-ORB_DEFINE(att_pos_mocap, struct att_pos_mocap_s);
-
-#include "topics/vehicle_rates_setpoint.h"
-ORB_DEFINE(vehicle_rates_setpoint, struct vehicle_rates_setpoint_s);
-#include "topics/mc_virtual_rates_setpoint.h"
-ORB_DEFINE(mc_virtual_rates_setpoint, struct mc_virtual_rates_setpoint_s);
-#include "topics/fw_virtual_rates_setpoint.h"
-ORB_DEFINE(fw_virtual_rates_setpoint, struct fw_virtual_rates_setpoint_s);
-
-#include "topics/rc_channels.h"
-ORB_DEFINE(rc_channels, struct rc_channels_s);
-
-#include "topics/vehicle_command.h"
-ORB_DEFINE(vehicle_command, struct vehicle_command_s);
-
-#include "topics/vehicle_control_mode.h"
-ORB_DEFINE(vehicle_control_mode, struct vehicle_control_mode_s);
-
-#include "topics/vehicle_local_position_setpoint.h"
-ORB_DEFINE(vehicle_local_position_setpoint, struct vehicle_local_position_setpoint_s);
-
-#include "topics/position_setpoint_triplet.h"
-ORB_DEFINE(position_setpoint_triplet, struct position_setpoint_triplet_s);
-
-#include "topics/vehicle_global_velocity_setpoint.h"
-ORB_DEFINE(vehicle_global_velocity_setpoint, struct vehicle_global_velocity_setpoint_s);
-
-#include "topics/mission.h"
-ORB_DEFINE(mission, struct mission_s);
-// XXX onboard and offboard mission are still declared here until this is
-// generator supported
-#include <navigator/navigation.h>
-ORB_DEFINE(offboard_mission, struct mission_s);
-ORB_DEFINE(onboard_mission, struct mission_s);
-
-#include "topics/mission_result.h"
-ORB_DEFINE(mission_result, struct mission_result_s);
-
-#include "topics/geofence_result.h"
-ORB_DEFINE(geofence_result, struct geofence_result_s);
-
-#include "topics/fence.h"
-ORB_DEFINE(fence, struct fence_s);
-
-#include "topics/fence_vertex.h"
-ORB_DEFINE(fence_vertex, struct fence_vertex_s);
-
-#include "topics/vehicle_attitude_setpoint.h"
-ORB_DEFINE(vehicle_attitude_setpoint, struct vehicle_attitude_setpoint_s);
-
-#include "topics/mc_virtual_attitude_setpoint.h"
-ORB_DEFINE(mc_virtual_attitude_setpoint, struct mc_virtual_attitude_setpoint_s);
-
-#include "topics/fw_virtual_attitude_setpoint.h"
-ORB_DEFINE(fw_virtual_attitude_setpoint, struct fw_virtual_attitude_setpoint_s);
-
-#include "topics/manual_control_setpoint.h"
-ORB_DEFINE(manual_control_setpoint, struct manual_control_setpoint_s);
-
-#include "topics/offboard_control_mode.h"
-ORB_DEFINE(offboard_control_mode, struct offboard_control_mode_s);
-
-#include "topics/optical_flow.h"
-ORB_DEFINE(optical_flow, struct optical_flow_s);
-
-#include "topics/filtered_bottom_flow.h"
-ORB_DEFINE(filtered_bottom_flow, struct filtered_bottom_flow_s);
-
-#include "topics/airspeed.h"
-ORB_DEFINE(airspeed, struct airspeed_s);
-
-#include "topics/differential_pressure.h"
-ORB_DEFINE(differential_pressure, struct differential_pressure_s);
-
-#include "topics/subsystem_info.h"
-ORB_DEFINE(subsystem_info, struct subsystem_info_s);
-
-/* actuator controls, as requested by controller */
-#include "topics/actuator_controls.h"
-#include "topics/actuator_controls_0.h"
-ORB_DEFINE(actuator_controls_0, struct actuator_controls_0_s);
-#include "topics/actuator_controls_1.h"
-ORB_DEFINE(actuator_controls_1, struct actuator_controls_1_s);
-#include "topics/actuator_controls_2.h"
-ORB_DEFINE(actuator_controls_2, struct actuator_controls_2_s);
-#include "topics/actuator_controls_3.h"
-ORB_DEFINE(actuator_controls_3, struct actuator_controls_3_s);
-//Virtual control groups, used for VTOL operation
-#include "topics/actuator_controls_virtual_mc.h"
-ORB_DEFINE(actuator_controls_virtual_mc, struct actuator_controls_virtual_mc_s);
-#include "topics/actuator_controls_virtual_fw.h"
-ORB_DEFINE(actuator_controls_virtual_fw, struct actuator_controls_virtual_fw_s);
-
-#include "topics/actuator_armed.h"
-ORB_DEFINE(actuator_armed, struct actuator_armed_s);
-
-#include "topics/actuator_outputs.h"
-ORB_DEFINE(actuator_outputs, struct actuator_outputs_s);
-
-#include "topics/actuator_direct.h"
-ORB_DEFINE(actuator_direct, struct actuator_direct_s);
-
-#include "topics/multirotor_motor_limits.h"
-ORB_DEFINE(multirotor_motor_limits, struct multirotor_motor_limits_s);
-
-#include "topics/telemetry_status.h"
-ORB_DEFINE(telemetry_status, struct telemetry_status_s);
-
-#include "topics/test_motor.h"
-ORB_DEFINE(test_motor, struct test_motor_s);
-
-#include "topics/debug_key_value.h"
-ORB_DEFINE(debug_key_value, struct debug_key_value_s);
-
-#include "topics/fw_pos_ctrl_status.h"
-ORB_DEFINE(fw_pos_ctrl_status, struct fw_pos_ctrl_status_s);
-
-#include "topics/esc_status.h"
-ORB_DEFINE(esc_status, struct esc_status_s);
-
-#include "topics/esc_report.h"
-ORB_DEFINE(esc_report, struct esc_report_s);
-
-#include "topics/encoders.h"
-ORB_DEFINE(encoders, struct encoders_s);
-
-#include "topics/estimator_status.h"
-ORB_DEFINE(estimator_status, struct estimator_status_s);
-
-#include "topics/vision_position_estimate.h"
-ORB_DEFINE(vision_position_estimate, struct vision_position_estimate_s);
-
-#include "topics/vehicle_force_setpoint.h"
-ORB_DEFINE(vehicle_force_setpoint, struct vehicle_force_setpoint_s);
-
-#include "topics/tecs_status.h"
-ORB_DEFINE(tecs_status, struct tecs_status_s);
-
-#include "topics/wind_estimate.h"
-ORB_DEFINE(wind_estimate, struct wind_estimate_s);
-
-#include "topics/rc_parameter_map.h"
-ORB_DEFINE(rc_parameter_map, struct rc_parameter_map_s);
-
-#include "topics/time_offset.h"
-ORB_DEFINE(time_offset, struct time_offset_s);
-
-#include "topics/mc_att_ctrl_status.h"
-ORB_DEFINE(mc_att_ctrl_status, struct mc_att_ctrl_status_s);
-
-#include "topics/distance_sensor.h"
-ORB_DEFINE(distance_sensor, struct distance_sensor_s);
-
-#include "topics/camera_trigger.h"
-ORB_DEFINE(camera_trigger, struct camera_trigger_s);
-
-#include "topics/vehicle_command_ack.h"
-ORB_DEFINE(vehicle_command_ack, struct vehicle_command_ack_s);
-
-#include "topics/ekf2_innovations.h"
-ORB_DEFINE(ekf2_innovations, struct ekf2_innovations_s);
-
-#include "topics/ekf2_replay.h"
-ORB_DEFINE(ekf2_replay, struct ekf2_replay_s);
-
-#include "topics/qshell_req.h"
-ORB_DEFINE(qshell_req, struct qshell_req_s);
-
-#include "topics/mavlink_log.h"
-ORB_DEFINE(mavlink_log, struct mavlink_log_s);
-
-#include "topics/follow_target.h"
-ORB_DEFINE(follow_target, struct follow_target_s);
-
-#include "topics/commander_state.h"
-ORB_DEFINE(commander_state, struct commander_state_s);
-
-#include "topics/transponder_report.h"
-ORB_DEFINE(transponder_report, struct transponder_report_s);
-
-#include "topics/gps_inject_data.h"
-ORB_DEFINE(gps_inject_data, struct gps_inject_data_s);
-
-#include "topics/adc_report.h"
-ORB_DEFINE(adc_report, struct adc_report_s);
-
-#include "topics/cpuload.h"
-ORB_DEFINE(cpuload, struct cpuload_s);
diff --git a/src/modules/uORB/uORB.h b/src/modules/uORB/uORB.h
index 807da7b75b..d1027d3b0e 100644
--- a/src/modules/uORB/uORB.h
+++ b/src/modules/uORB/uORB.h
@@ -52,6 +52,7 @@
 struct orb_metadata {
 	const char *o_name;		/**< unique object name */
 	const size_t o_size;		/**< object size */
+	const char *o_fields;		/**< semicolon separated list of fields */
 };
 
 typedef const struct orb_metadata *orb_id_t;
@@ -110,11 +111,13 @@ enum ORB_PRIO {
  *
  * @param _name		The name of the topic.
  * @param _struct	The structure the topic provides.
+ * @param _fields	All fields in a semicolon separated list e.g: "float[3] position;bool armed"
  */
-#define ORB_DEFINE(_name, _struct)			\
+#define ORB_DEFINE(_name, _struct, _fields)			\
 	const struct orb_metadata __orb_##_name = {	\
 		#_name,					\
-		sizeof(_struct)				\
+		sizeof(_struct),		\
+		_fields				\
 	}; struct hack
 
 __BEGIN_DECLS
diff --git a/src/modules/uORB/uORBTopics.h b/src/modules/uORB/uORBTopics.h
new file mode 100644
index 0000000000..f7c3a3c865
--- /dev/null
+++ b/src/modules/uORB/uORBTopics.h
@@ -0,0 +1,50 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2012-2015 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+#ifndef MODULES_UORB_UORBTOPICS_H_
+#define MODULES_UORB_UORBTOPICS_H_
+
+#include <uORB/uORB.h>
+
+/*
+ * Returns count of all declared topics.
+ * It is equal to size of array from orb_get_topics()
+ */
+extern size_t orb_topics_count() __EXPORT;
+
+/*
+ * Returns array of topics metadata
+ */
+extern const struct orb_metadata **orb_get_topics() __EXPORT;
+
+#endif /* MODULES_UORB_UORBTOPICS_H_ */
diff --git a/src/modules/uORB/uORB_tests/uORBTest_UnitTest.hpp b/src/modules/uORB/uORB_tests/uORBTest_UnitTest.hpp
index 5722b164b3..032b25a544 100644
--- a/src/modules/uORB/uORB_tests/uORBTest_UnitTest.hpp
+++ b/src/modules/uORB/uORB_tests/uORBTest_UnitTest.hpp
@@ -41,23 +41,26 @@ struct orb_test {
 	int val;
 	hrt_abstime time;
 };
-ORB_DEFINE(orb_test, struct orb_test);
-ORB_DEFINE(orb_multitest, struct orb_test);
+ORB_DEFINE(orb_test, struct orb_test, "ORB_TEST:int val;hrt_abstime time;");
+ORB_DEFINE(orb_multitest, struct orb_test, "ORB_MULTITEST:int val;hrt_abstime time;");
+
 
 struct orb_test_medium {
 	int val;
 	hrt_abstime time;
 	char junk[64];
 };
-ORB_DEFINE(orb_test_medium, struct orb_test_medium);
-ORB_DEFINE(orb_test_medium_multi, struct orb_test_medium);
+ORB_DEFINE(orb_test_medium, struct orb_test_medium,  "ORB_TEST_MEDIUM:int val;hrt_abstime time;char[64] junk;");
+ORB_DEFINE(orb_test_medium_multi, struct orb_test_medium,
+	   "ORB_TEST_MEDIUM_MULTI:int val;hrt_abstime time;char[64] junk;");
+
 
 struct orb_test_large {
 	int val;
 	hrt_abstime time;
 	char junk[512];
 };
-ORB_DEFINE(orb_test_large, struct orb_test_large);
+ORB_DEFINE(orb_test_large, struct orb_test_large, "ORB_TEST_LARGE:int val;hrt_abstime time;char[512] junk;");
 
 
 namespace uORBTest
diff --git a/src/modules/vtol_att_control/vtol_att_control_main.h b/src/modules/vtol_att_control/vtol_att_control_main.h
index cf072af71d..e2a63993d7 100644
--- a/src/modules/vtol_att_control/vtol_att_control_main.h
+++ b/src/modules/vtol_att_control/vtol_att_control_main.h
@@ -69,8 +69,6 @@
 #include <uORB/topics/fw_virtual_attitude_setpoint.h>
 #include <uORB/topics/manual_control_setpoint.h>
 #include <uORB/topics/actuator_controls.h>
-#include <uORB/topics/actuator_controls_virtual_mc.h>
-#include <uORB/topics/actuator_controls_virtual_fw.h>
 #include <uORB/topics/vehicle_rates_setpoint.h>
 #include <uORB/topics/mc_virtual_rates_setpoint.h>
 #include <uORB/topics/fw_virtual_rates_setpoint.h>
diff --git a/src/platforms/posix/drivers/airspeedsim/airspeedsim.cpp b/src/platforms/posix/drivers/airspeedsim/airspeedsim.cpp
index 58f6aa9929..25bc4672fb 100644
--- a/src/platforms/posix/drivers/airspeedsim/airspeedsim.cpp
+++ b/src/platforms/posix/drivers/airspeedsim/airspeedsim.cpp
@@ -371,12 +371,11 @@ AirspeedSim::update_status()
 {
 	if (_sensor_ok != _last_published_sensor_ok) {
 		/* notify about state change */
-		struct subsystem_info_s info = {
-			true,
-			true,
-			_sensor_ok,
-			subsystem_info_s::SUBSYSTEM_TYPE_DIFFPRESSURE
-		};
+		struct subsystem_info_s info = {};
+		info.present = true;
+		info.enabled = true;
+		info.ok = _sensor_ok;
+		info.subsystem_type = subsystem_info_s::SUBSYSTEM_TYPE_DIFFPRESSURE;
 
 		if (_subsys_pub != nullptr) {
 			orb_publish(ORB_ID(subsystem_info), _subsys_pub, &info);
diff --git a/src/platforms/px4_includes.h b/src/platforms/px4_includes.h
index a38b1e7cb3..434bd5e78b 100644
--- a/src/platforms/px4_includes.h
+++ b/src/platforms/px4_includes.h
@@ -53,8 +53,6 @@
 #include <px4_vehicle_attitude_setpoint.h>
 #include <px4_manual_control_setpoint.h>
 #include <px4_actuator_controls.h>
-#include <px4_actuator_controls_0.h>
-#include <px4_actuator_controls_virtual_mc.h>
 #include <px4_vehicle_rates_setpoint.h>
 #include <px4_mc_virtual_rates_setpoint.h>
 #include <px4_vehicle_attitude.h>
@@ -82,10 +80,6 @@
 #include <platforms/nuttx/px4_messages/px4_vehicle_attitude_setpoint.h>
 #include <platforms/nuttx/px4_messages/px4_manual_control_setpoint.h>
 #include <platforms/nuttx/px4_messages/px4_actuator_controls.h>
-#include <platforms/nuttx/px4_messages/px4_actuator_controls_0.h>
-#include <platforms/nuttx/px4_messages/px4_actuator_controls_1.h>
-#include <platforms/nuttx/px4_messages/px4_actuator_controls_2.h>
-#include <platforms/nuttx/px4_messages/px4_actuator_controls_3.h>
 #include <platforms/nuttx/px4_messages/px4_vehicle_rates_setpoint.h>
 #include <platforms/nuttx/px4_messages/px4_vehicle_attitude.h>
 #include <platforms/nuttx/px4_messages/px4_control_state.h>
@@ -117,10 +111,6 @@
 #include <platforms/posix/px4_messages/px4_vehicle_attitude_setpoint.h>
 #include <platforms/posix/px4_messages/px4_manual_control_setpoint.h>
 #include <platforms/posix/px4_messages/px4_actuator_controls.h>
-#include <platforms/posix/px4_messages/px4_actuator_controls_0.h>
-#include <platforms/posix/px4_messages/px4_actuator_controls_1.h>
-#include <platforms/posix/px4_messages/px4_actuator_controls_2.h>
-#include <platforms/posix/px4_messages/px4_actuator_controls_3.h>
 #include <platforms/posix/px4_messages/px4_vehicle_rates_setpoint.h>
 #include <platforms/posix/px4_messages/px4_vehicle_attitude.h>
 #include <platforms/posix/px4_messages/px4_control_state.h>
@@ -148,10 +138,6 @@
 #include <platforms/qurt/px4_messages/px4_vehicle_attitude_setpoint.h>
 #include <platforms/qurt/px4_messages/px4_manual_control_setpoint.h>
 #include <platforms/qurt/px4_messages/px4_actuator_controls.h>
-#include <platforms/qurt/px4_messages/px4_actuator_controls_0.h>
-#include <platforms/qurt/px4_messages/px4_actuator_controls_1.h>
-#include <platforms/qurt/px4_messages/px4_actuator_controls_2.h>
-#include <platforms/qurt/px4_messages/px4_actuator_controls_3.h>
 #include <platforms/qurt/px4_messages/px4_vehicle_rates_setpoint.h>
 #include <platforms/qurt/px4_messages/px4_vehicle_attitude.h>
 #include <platforms/qurt/px4_messages/px4_control_state.h>
diff --git a/src/systemcmds/esc_calib/esc_calib.c b/src/systemcmds/esc_calib/esc_calib.c
index 86939ff686..161fe7923b 100644
--- a/src/systemcmds/esc_calib/esc_calib.c
+++ b/src/systemcmds/esc_calib/esc_calib.c
@@ -60,10 +60,6 @@
 #include "drivers/drv_pwm_output.h"
 
 #include <uORB/topics/actuator_controls.h>
-#include <uORB/topics/actuator_controls_0.h>
-#include <uORB/topics/actuator_controls_1.h>
-#include <uORB/topics/actuator_controls_2.h>
-#include <uORB/topics/actuator_controls_3.h>
 
 static void	usage(const char *reason);
 __EXPORT int	esc_calib_main(int argc, char *argv[]);
diff --git a/unittests/uorb_unittests/uORBCommunicator_gtests.cpp b/unittests/uorb_unittests/uORBCommunicator_gtests.cpp
index 1ea12b7a96..35822f016f 100644
--- a/unittests/uorb_unittests/uORBCommunicator_gtests.cpp
+++ b/unittests/uorb_unittests/uORBCommunicator_gtests.cpp
@@ -206,7 +206,7 @@ namespace uORB_test
      ASSERT_EQ( c._send_messageCount, 0 );
 
      //step 1.
-     ORB_DEFINE( topicA_sndmsg, struct orb_topic_A );
+     ORB_DEFINE( topicA_sndmsg, struct orb_topic_A, "TOPICA_SNDMSG:int16_t val;" );
      _topicA.val = 1;
      _pub_ptr = orb_advertise(ORB_ID(topicA_sndmsg ), &_topicA );
      ASSERT_TRUE( ( _pub_ptr != nullptr ) ) << "Failed to advertize uORB Topic topicA_sndmsg: errno: " << errno;
diff --git a/unittests/uorb_unittests/uORBGtestTopics.hpp b/unittests/uorb_unittests/uORBGtestTopics.hpp
index 0616afe83e..0cea8d55fc 100644
--- a/unittests/uorb_unittests/uORBGtestTopics.hpp
+++ b/unittests/uorb_unittests/uORBGtestTopics.hpp
@@ -48,12 +48,11 @@ namespace uORB_test
      int16_t val;
    };
 
+   ORB_DEFINE( topicA, struct orb_topic_A, "TOPICA:int16 val;" );
+   ORB_DEFINE( topicB, struct orb_topic_B, "TOPICB:int16 val;" );
 
-   ORB_DEFINE( topicA, struct orb_topic_A );
-   ORB_DEFINE( topicB, struct orb_topic_B );
-
-   ORB_DEFINE( topicA_clone, struct orb_topic_A );
-   ORB_DEFINE( topicB_clone, struct orb_topic_B );
+   ORB_DEFINE( topicA_clone, struct orb_topic_A, "TOPICA_CLONE:int16 val;" );
+   ORB_DEFINE( topicB_clone, struct orb_topic_B, "TOPICB_CLONE:int16 val;"  );
 }
 
 #endif // _UnitTest_uORBTopics_hpp_

From eb29b33620aefcc8e9eecf7d270037f0bd7e1429 Mon Sep 17 00:00:00 2001
From: Daniel Agar <daniel@agar.ca>
Date: Wed, 27 Apr 2016 20:44:15 -0400
Subject: [PATCH 0129/1230] use gcc attributes to align and pack

---
 msg/templates/uorb/msg.h.template               |  5 +++--
 src/drivers/device/integrator.cpp               |  8 ++++----
 src/drivers/device/integrator.h                 |  6 +++---
 src/drivers/l3gd20/l3gd20.cpp                   |  2 +-
 src/drivers/lsm303d/lsm303d.cpp                 |  2 +-
 src/drivers/mpu6000/mpu6000.cpp                 |  4 ++--
 src/drivers/mpu6500/mpu6500.cpp                 |  4 ++--
 src/drivers/mpu9250/mpu9250.cpp                 |  4 ++--
 src/drivers/px4flow/px4flow.cpp                 |  4 ++--
 src/lib/conversion/rotation.cpp                 | 10 ++++++++++
 src/lib/conversion/rotation.h                   |  5 +++++
 src/modules/mavlink/mavlink_receiver.cpp        |  7 +++----
 src/modules/uORB/uORB.h                         |  7 +++++++
 src/platforms/posix/drivers/gyrosim/gyrosim.cpp |  4 ++--
 14 files changed, 47 insertions(+), 25 deletions(-)

diff --git a/msg/templates/uorb/msg.h.template b/msg/templates/uorb/msg.h.template
index 7fe48e593d..564657c2e9 100644
--- a/msg/templates/uorb/msg.h.template
+++ b/msg/templates/uorb/msg.h.template
@@ -173,7 +173,8 @@ def print_parsed_fields():
 
 }@
 
-// #pragma pack(push, 1) badly breaks alignment everywhere!
+// #pragma pack(push, 1)
+ORBPACKED(
 #ifdef __cplusplus
 @#class @(uorb_struct) {
 struct __EXPORT @(uorb_struct) {
@@ -198,7 +199,7 @@ for constant in spec.constants:
   print('\tstatic const %s %s = %s;'%(type_px4, constant.name, int(constant.val)))
 }
 #endif
-};
+});
 //#pragma pack(pop)
 
 /* register this as object request broker structure */
diff --git a/src/drivers/device/integrator.cpp b/src/drivers/device/integrator.cpp
index 072e3096a2..86a9b53505 100644
--- a/src/drivers/device/integrator.cpp
+++ b/src/drivers/device/integrator.cpp
@@ -60,7 +60,7 @@ Integrator::~Integrator()
 }
 
 bool
-Integrator::put(uint64_t timestamp, math::Vector<3> &val, math::Vector<3> &integral, uint64_t &integral_dt)
+Integrator::put(uint64_t timestamp, math::Vector<3> &val, math::Vector<3> &integral, uint64_t *integral_dt)
 {
 	if (_last_integration_time == 0) {
 		/* this is the first item in the integrator */
@@ -132,7 +132,7 @@ Integrator::put_with_interval(unsigned interval_us, math::Vector<3> &val, math::
 }
 
 math::Vector<3>
-Integrator::get(bool reset, uint64_t &integral_dt)
+Integrator::get(bool reset, uint64_t *integral_dt)
 {
 	math::Vector<3> val = _integral;
 
@@ -158,12 +158,12 @@ Integrator::get_and_filtered(bool reset, uint64_t &integral_dt, math::Vector<3>
 }
 
 void
-Integrator::_reset(uint64_t &integral_dt)
+Integrator::_reset(uint64_t *integral_dt)
 {
 	_integral(0) = 0.0f;
 	_integral(1) = 0.0f;
 	_integral(2) = 0.0f;
 
-	integral_dt = (_last_integration_time - _last_reset_time);
+	*integral_dt = (_last_integration_time - _last_reset_time);
 	_last_reset_time = _last_integration_time;
 }
diff --git a/src/drivers/device/integrator.h b/src/drivers/device/integrator.h
index 48a232066f..86817323e2 100644
--- a/src/drivers/device/integrator.h
+++ b/src/drivers/device/integrator.h
@@ -60,7 +60,7 @@ public:
 	 * @return		true if putting the item triggered an integral reset and the integral should be
 	 *			published.
 	 */
-	bool put(uint64_t timestamp, math::Vector<3> &val, math::Vector<3> &integral, uint64_t &integral_dt);
+	bool put(uint64_t timestamp, math::Vector<3> &val, math::Vector<3> &integral, uint64_t *integral_dt);
 
 	/**
 	 * Put an item into the integral but provide an interval instead of a timestamp.
@@ -84,7 +84,7 @@ public:
 	 * @param integral_dt	Get the dt in us of the current integration (only if reset).
 	 * @return		the integral since the last read-reset
 	 */
-	math::Vector<3>		get(bool reset, uint64_t &integral_dt);
+	math::Vector<3>		get(bool reset, uint64_t *integral_dt);
 
 	/**
 	 * Get the current integral and reset the integrator if needed. Additionally give the
@@ -115,5 +115,5 @@ private:
 	 *
 	 * @param integral_dt	Get the dt in us of the current integration.
 	 */
-	void _reset(uint64_t &integral_dt);
+	void _reset(uint64_t *integral_dt);
 };
diff --git a/src/drivers/l3gd20/l3gd20.cpp b/src/drivers/l3gd20/l3gd20.cpp
index 61345628b1..e0b2c39c80 100644
--- a/src/drivers/l3gd20/l3gd20.cpp
+++ b/src/drivers/l3gd20/l3gd20.cpp
@@ -1082,7 +1082,7 @@ L3GD20::measure()
 	math::Vector<3> gval(xin, yin, zin);
 	math::Vector<3> gval_integrated;
 
-	bool gyro_notify = _gyro_int.put(report.timestamp, gval, gval_integrated, report.integral_dt);
+	bool gyro_notify = _gyro_int.put(report.timestamp, gval, gval_integrated, &report.integral_dt);
 	report.x_integral = gval_integrated(0);
 	report.y_integral = gval_integrated(1);
 	report.z_integral = gval_integrated(2);
diff --git a/src/drivers/lsm303d/lsm303d.cpp b/src/drivers/lsm303d/lsm303d.cpp
index 71796cfe8c..6563282f4e 100644
--- a/src/drivers/lsm303d/lsm303d.cpp
+++ b/src/drivers/lsm303d/lsm303d.cpp
@@ -1649,7 +1649,7 @@ LSM303D::measure()
 	math::Vector<3> aval(x_in_new, y_in_new, z_in_new);
 	math::Vector<3> aval_integrated;
 
-	bool accel_notify = _accel_int.put(accel_report.timestamp, aval, aval_integrated, accel_report.integral_dt);
+	bool accel_notify = _accel_int.put(accel_report.timestamp, aval, aval_integrated, &accel_report.integral_dt);
 	accel_report.x_integral = aval_integrated(0);
 	accel_report.y_integral = aval_integrated(1);
 	accel_report.z_integral = aval_integrated(2);
diff --git a/src/drivers/mpu6000/mpu6000.cpp b/src/drivers/mpu6000/mpu6000.cpp
index 4bf6384b3f..a6b2b2e6f1 100644
--- a/src/drivers/mpu6000/mpu6000.cpp
+++ b/src/drivers/mpu6000/mpu6000.cpp
@@ -1905,7 +1905,7 @@ MPU6000::measure()
 	math::Vector<3> aval(x_in_new, y_in_new, z_in_new);
 	math::Vector<3> aval_integrated;
 
-	bool accel_notify = _accel_int.put(arb.timestamp, aval, aval_integrated, arb.integral_dt);
+	bool accel_notify = _accel_int.put(arb.timestamp, aval, aval_integrated, &arb.integral_dt);
 	arb.x_integral = aval_integrated(0);
 	arb.y_integral = aval_integrated(1);
 	arb.z_integral = aval_integrated(2);
@@ -1945,7 +1945,7 @@ MPU6000::measure()
 	math::Vector<3> gval(x_gyro_in_new, y_gyro_in_new, z_gyro_in_new);
 	math::Vector<3> gval_integrated;
 
-	bool gyro_notify = _gyro_int.put(arb.timestamp, gval, gval_integrated, grb.integral_dt);
+	bool gyro_notify = _gyro_int.put(arb.timestamp, gval, gval_integrated, &grb.integral_dt);
 	grb.x_integral = gval_integrated(0);
 	grb.y_integral = gval_integrated(1);
 	grb.z_integral = gval_integrated(2);
diff --git a/src/drivers/mpu6500/mpu6500.cpp b/src/drivers/mpu6500/mpu6500.cpp
index c3457c5b8b..20fcfb6dbe 100644
--- a/src/drivers/mpu6500/mpu6500.cpp
+++ b/src/drivers/mpu6500/mpu6500.cpp
@@ -1846,7 +1846,7 @@ MPU6500::measure()
 	math::Vector<3> aval(x_in_new, y_in_new, z_in_new);
 	math::Vector<3> aval_integrated;
 
-	bool accel_notify = _accel_int.put(arb.timestamp, aval, aval_integrated, arb.integral_dt);
+	bool accel_notify = _accel_int.put(arb.timestamp, aval, aval_integrated, &arb.integral_dt);
 	arb.x_integral = aval_integrated(0);
 	arb.y_integral = aval_integrated(1);
 	arb.z_integral = aval_integrated(2);
@@ -1881,7 +1881,7 @@ MPU6500::measure()
 	math::Vector<3> gval(x_gyro_in_new, y_gyro_in_new, z_gyro_in_new);
 	math::Vector<3> gval_integrated;
 
-	bool gyro_notify = _gyro_int.put(arb.timestamp, gval, gval_integrated, grb.integral_dt);
+	bool gyro_notify = _gyro_int.put(arb.timestamp, gval, gval_integrated, &grb.integral_dt);
 	grb.x_integral = gval_integrated(0);
 	grb.y_integral = gval_integrated(1);
 	grb.z_integral = gval_integrated(2);
diff --git a/src/drivers/mpu9250/mpu9250.cpp b/src/drivers/mpu9250/mpu9250.cpp
index 761c8f1008..b2dd1b3549 100644
--- a/src/drivers/mpu9250/mpu9250.cpp
+++ b/src/drivers/mpu9250/mpu9250.cpp
@@ -1418,7 +1418,7 @@ MPU9250::measure()
 	math::Vector<3> aval(x_in_new, y_in_new, z_in_new);
 	math::Vector<3> aval_integrated;
 
-	bool accel_notify = _accel_int.put(arb.timestamp, aval, aval_integrated, arb.integral_dt);
+	bool accel_notify = _accel_int.put(arb.timestamp, aval, aval_integrated, &arb.integral_dt);
 	arb.x_integral = aval_integrated(0);
 	arb.y_integral = aval_integrated(1);
 	arb.z_integral = aval_integrated(2);
@@ -1453,7 +1453,7 @@ MPU9250::measure()
 	math::Vector<3> gval(x_gyro_in_new, y_gyro_in_new, z_gyro_in_new);
 	math::Vector<3> gval_integrated;
 
-	bool gyro_notify = _gyro_int.put(arb.timestamp, gval, gval_integrated, grb.integral_dt);
+	bool gyro_notify = _gyro_int.put(arb.timestamp, gval, gval_integrated, &grb.integral_dt);
 	grb.x_integral = gval_integrated(0);
 	grb.y_integral = gval_integrated(1);
 	grb.z_integral = gval_integrated(2);
diff --git a/src/drivers/px4flow/px4flow.cpp b/src/drivers/px4flow/px4flow.cpp
index 095d70e38b..647ae1590c 100644
--- a/src/drivers/px4flow/px4flow.cpp
+++ b/src/drivers/px4flow/px4flow.cpp
@@ -541,9 +541,9 @@ PX4FLOW::collect()
 	/* rotate measurements according to parameter */
 	float zeroval = 0.0f;
 
-	rotate_3f(_sensor_rotation, report.pixel_flow_x_integral, report.pixel_flow_y_integral, zeroval);
+	rotate_3f(_sensor_rotation, &report.pixel_flow_x_integral, &report.pixel_flow_y_integral, &zeroval);
 
-	rotate_3f(_sensor_rotation, report.gyro_x_rate_integral, report.gyro_y_rate_integral, report.gyro_z_rate_integral);
+	rotate_3f(_sensor_rotation, &report.gyro_x_rate_integral, &report.gyro_y_rate_integral, &report.gyro_z_rate_integral);
 
 	if (_px4flow_topic == nullptr) {
 		_px4flow_topic = orb_advertise(ORB_ID(optical_flow), &report);
diff --git a/src/lib/conversion/rotation.cpp b/src/lib/conversion/rotation.cpp
index 26bfc4d9ef..971be328ee 100644
--- a/src/lib/conversion/rotation.cpp
+++ b/src/lib/conversion/rotation.cpp
@@ -52,6 +52,16 @@ get_rot_matrix(enum Rotation rot, math::Matrix<3, 3> *rot_matrix)
 
 #define HALF_SQRT_2 0.70710678118654757f
 
+__EXPORT void
+rotate_3f(enum Rotation rot, float *x, float *y, float *z)
+{
+	float &x2 = *x;
+	float &y2 = *y;
+	float &z2 = *z;
+
+	rotate_3f(rot, x2, y2, z2);
+}
+
 __EXPORT void
 rotate_3f(enum Rotation rot, float &x, float &y, float &z)
 {
diff --git a/src/lib/conversion/rotation.h b/src/lib/conversion/rotation.h
index 1e1cdce268..87ac9064d5 100644
--- a/src/lib/conversion/rotation.h
+++ b/src/lib/conversion/rotation.h
@@ -130,6 +130,11 @@ const rot_lookup_t rot_lookup[] = {
 __EXPORT void
 get_rot_matrix(enum Rotation rot, math::Matrix<3, 3> *rot_matrix);
 
+/**
+ * rotate a 3 element float vector in-place
+ */
+__EXPORT void
+rotate_3f(enum Rotation rot, float *x, float *y, float *z);
 
 /**
  * rotate a 3 element float vector in-place
diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp
index 48a08c312f..f450b2e9fc 100644
--- a/src/modules/mavlink/mavlink_receiver.cpp
+++ b/src/modules/mavlink/mavlink_receiver.cpp
@@ -490,8 +490,7 @@ MavlinkReceiver::handle_message_optical_flow_rad(mavlink_message_t *msg)
 	enum Rotation flow_rot;
 	param_get(param_find("SENS_FLOW_ROT"), &flow_rot);
 
-	struct optical_flow_s f;
-	memset(&f, 0, sizeof(f));
+	struct optical_flow_s f = {};
 
 	f.timestamp = flow.time_usec;
 	f.integration_timespan = flow.integration_time_us;
@@ -508,8 +507,8 @@ MavlinkReceiver::handle_message_optical_flow_rad(mavlink_message_t *msg)
 
 	/* rotate measurements according to parameter */
 	float zeroval = 0.0f;
-	rotate_3f(flow_rot, f.pixel_flow_x_integral, f.pixel_flow_y_integral, zeroval);
-	rotate_3f(flow_rot, f.gyro_x_rate_integral, f.gyro_y_rate_integral, f.gyro_z_rate_integral);
+	rotate_3f(flow_rot, &f.pixel_flow_x_integral, &f.pixel_flow_y_integral, &zeroval);
+	rotate_3f(flow_rot, &f.gyro_x_rate_integral, &f.gyro_y_rate_integral, &f.gyro_z_rate_integral);
 
 	if (_flow_pub == nullptr) {
 		_flow_pub = orb_advertise(ORB_ID(optical_flow), &f);
diff --git a/src/modules/uORB/uORB.h b/src/modules/uORB/uORB.h
index d1027d3b0e..f58e093090 100644
--- a/src/modules/uORB/uORB.h
+++ b/src/modules/uORB/uORB.h
@@ -46,6 +46,13 @@
 // Hack until everything is using this header
 #include <systemlib/visibility.h>
 
+// Macro to define packed structures
+#ifdef __GNUC__
+#define ORBPACKED( __Declaration__ ) __Declaration__ __attribute__((aligned(4), packed))
+#else
+#define ORBPACKED( __Declaration__ ) __pragma( pack(push, 1) ) __Declaration__ __pragma( pack(pop) )
+#endif
+
 /**
  * Object metadata.
  */
diff --git a/src/platforms/posix/drivers/gyrosim/gyrosim.cpp b/src/platforms/posix/drivers/gyrosim/gyrosim.cpp
index 012db15942..d7084fffcf 100644
--- a/src/platforms/posix/drivers/gyrosim/gyrosim.cpp
+++ b/src/platforms/posix/drivers/gyrosim/gyrosim.cpp
@@ -1103,7 +1103,7 @@ GYROSIM::_measure()
 	math::Vector<3> aval(mpu_report.accel_x, mpu_report.accel_y, mpu_report.accel_z);
 	math::Vector<3> aval_integrated;
 
-	bool accel_notify = _accel_int.put(arb.timestamp, aval, aval_integrated, arb.integral_dt);
+	bool accel_notify = _accel_int.put(arb.timestamp, aval, aval_integrated, &arb.integral_dt);
 	arb.x_integral = aval_integrated(0);
 	arb.y_integral = aval_integrated(1);
 	arb.z_integral = aval_integrated(2);
@@ -1125,7 +1125,7 @@ GYROSIM::_measure()
 	math::Vector<3> gval(mpu_report.gyro_x, mpu_report.gyro_y, mpu_report.gyro_z);
 	math::Vector<3> gval_integrated;
 
-	bool gyro_notify = _gyro_int.put(grb.timestamp, gval, gval_integrated, grb.integral_dt);
+	bool gyro_notify = _gyro_int.put(grb.timestamp, gval, gval_integrated, &grb.integral_dt);
 	grb.x_integral = gval_integrated(0);
 	grb.y_integral = gval_integrated(1);
 	grb.z_integral = gval_integrated(2);

From 4e0129275d6e0e474b19742375bf3042e038ee6f Mon Sep 17 00:00:00 2001
From: Daniel Agar <daniel@agar.ca>
Date: Mon, 25 Apr 2016 17:36:13 -0400
Subject: [PATCH 0130/1230] new logger

---
 ROMFS/px4fmu_common/init.d/rc.logging         |  40 +
 ROMFS/px4fmu_common/init.d/rcS                |  31 +-
 Tools/check_code_style_all.sh                 |   1 +
 cmake/configs/nuttx_mindpx-v2_default.cmake   |   1 +
 cmake/configs/nuttx_px4fmu-v1_default.cmake   |   1 +
 cmake/configs/nuttx_px4fmu-v2_default.cmake   |   1 +
 cmake/configs/nuttx_px4fmu-v2_ekf2.cmake      |   1 +
 cmake/configs/nuttx_px4fmu-v4_default.cmake   |   1 +
 cmake/configs/posix_eagle_hil.cmake           |   1 +
 .../posix_eagle_legacy_driver_default.cmake   |   1 +
 cmake/configs/posix_rpi2_default.cmake        |   1 +
 cmake/configs/posix_rpi2_release.cmake        |   1 +
 cmake/configs/posix_sdflight_default.cmake    |   1 +
 cmake/configs/posix_sitl_broadcast.cmake      |   1 +
 cmake/configs/posix_sitl_default.cmake        |   2 +-
 cmake/configs/posix_sitl_ekf2.cmake           |   1 +
 cmake/configs/posix_sitl_replay.cmake         |   1 +
 cmake/configs/posix_sitl_test.cmake           |   1 +
 cmake/ros-CMakeLists.txt                      |  37 +-
 src/modules/logger/CMakeLists.txt             |  52 ++
 src/modules/logger/array.h                    | 127 +++
 src/modules/logger/log_writer.cpp             | 237 ++++++
 src/modules/logger/log_writer.h               |  74 ++
 src/modules/logger/logger.cpp                 | 760 ++++++++++++++++++
 src/modules/logger/logger.h                   |  96 +++
 src/modules/systemlib/system_params.c         |  12 +
 26 files changed, 1437 insertions(+), 46 deletions(-)
 create mode 100644 ROMFS/px4fmu_common/init.d/rc.logging
 create mode 100644 src/modules/logger/CMakeLists.txt
 create mode 100644 src/modules/logger/array.h
 create mode 100644 src/modules/logger/log_writer.cpp
 create mode 100644 src/modules/logger/log_writer.h
 create mode 100644 src/modules/logger/logger.cpp
 create mode 100644 src/modules/logger/logger.h

diff --git a/ROMFS/px4fmu_common/init.d/rc.logging b/ROMFS/px4fmu_common/init.d/rc.logging
new file mode 100644
index 0000000000..f46d94dbc7
--- /dev/null
+++ b/ROMFS/px4fmu_common/init.d/rc.logging
@@ -0,0 +1,40 @@
+#!nsh
+#
+# Initialize logging services.
+#
+
+if [ -d /fs/microsd ]
+then
+	if ver hwcmp PX4FMU_V1
+	then
+		if sdlog2 start -r 30 -a -b 2 -t
+		then
+		fi
+	else
+		# check if we should increase logging rate for ekf2 replay message logging
+		if param greater EKF2_REC_RPL 0
+		then
+			if param compare SYS_LOGGER 0
+			then
+				if sdlog2 start -r 500 -e -b 20 -t
+				then
+				fi
+			else
+				if logger start -r 500
+				then
+				fi
+			fi
+		else
+			if param compare SYS_LOGGER 0
+			then
+				if sdlog2 start -r 100 -a -b 12 -t
+				then
+				fi
+			else
+				if logger start -b 32
+				then
+				fi
+			fi
+		fi
+	fi
+fi
diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS
index 1e2bc91bb9..cfcdfa5397 100644
--- a/ROMFS/px4fmu_common/init.d/rcS
+++ b/ROMFS/px4fmu_common/init.d/rcS
@@ -617,32 +617,6 @@ then
 		mavlink start -r 800000 -d /dev/ttyACM0 -m config -x
 	fi
 
-
-	#
-	# Logging
-	#
-	if [ -d /fs/microsd ]
-	then
-		if ver hwcmp PX4FMU_V1
-		then
-			if sdlog2 start -r 30 -a -b 2 -t
-			then
-			fi
-		else
-			# check if we should increase logging rate for ekf2 replay message logging
-			if param greater EKF2_REC_RPL 0
-			then
-				if sdlog2 start -r 500 -e -b 20 -t
-				then
-				fi
-			else
-				if sdlog2 start -r 100 -a -b 12 -t
-				then
-				fi
-			fi
-		fi
-	fi
-
 	#
 	# Start up ARDrone Motor interface
 	#
@@ -863,6 +837,11 @@ then
 		echo "[i] No autostart ID found"
 	fi
 
+	#
+	# Logging
+	#
+	sh /etc/init.d/rc.logging
+
 	# Start any custom addons
 	set FEXTRAS /fs/microsd/etc/extras.txt
 	if [ -f $FEXTRAS ]
diff --git a/Tools/check_code_style_all.sh b/Tools/check_code_style_all.sh
index eba8648f25..3dde2105dd 100755
--- a/Tools/check_code_style_all.sh
+++ b/Tools/check_code_style_all.sh
@@ -26,6 +26,7 @@ find \
     src/modules/gpio_led \
     src/modules/land_detector \
     src/modules/local_position_estimator \
+    src/modules/logger \
     src/modules/mavlink/mavlink_tests \
     src/modules/muorb \
     src/modules/param \
diff --git a/cmake/configs/nuttx_mindpx-v2_default.cmake b/cmake/configs/nuttx_mindpx-v2_default.cmake
index 9f0626e5b7..e09339029f 100644
--- a/cmake/configs/nuttx_mindpx-v2_default.cmake
+++ b/cmake/configs/nuttx_mindpx-v2_default.cmake
@@ -105,6 +105,7 @@ set(config_module_list
 	# Logging
 	#
 	modules/sdlog2
+	modules/logger
 
 	#
 	# Library modules
diff --git a/cmake/configs/nuttx_px4fmu-v1_default.cmake b/cmake/configs/nuttx_px4fmu-v1_default.cmake
index dfb672a5a8..75f8d054ed 100644
--- a/cmake/configs/nuttx_px4fmu-v1_default.cmake
+++ b/cmake/configs/nuttx_px4fmu-v1_default.cmake
@@ -91,6 +91,7 @@ set(config_module_list
 	# Logging
 	#
 	modules/sdlog2
+	modules/logger
 
 	#
 	# Library modules
diff --git a/cmake/configs/nuttx_px4fmu-v2_default.cmake b/cmake/configs/nuttx_px4fmu-v2_default.cmake
index a30cd9b381..ee1263bad6 100644
--- a/cmake/configs/nuttx_px4fmu-v2_default.cmake
+++ b/cmake/configs/nuttx_px4fmu-v2_default.cmake
@@ -100,6 +100,7 @@ set(config_module_list
 	#
 	# Logging
 	#
+	modules/logger
 	modules/sdlog2
 
 	#
diff --git a/cmake/configs/nuttx_px4fmu-v2_ekf2.cmake b/cmake/configs/nuttx_px4fmu-v2_ekf2.cmake
index 33daab5f77..289eb4098a 100644
--- a/cmake/configs/nuttx_px4fmu-v2_ekf2.cmake
+++ b/cmake/configs/nuttx_px4fmu-v2_ekf2.cmake
@@ -99,6 +99,7 @@ set(config_module_list
 	# Logging
 	#
 	modules/sdlog2
+	modules/logger
 
 	#
 	# Library modules
diff --git a/cmake/configs/nuttx_px4fmu-v4_default.cmake b/cmake/configs/nuttx_px4fmu-v4_default.cmake
index eb4713eb50..8a3d8f39f3 100644
--- a/cmake/configs/nuttx_px4fmu-v4_default.cmake
+++ b/cmake/configs/nuttx_px4fmu-v4_default.cmake
@@ -104,6 +104,7 @@ set(config_module_list
 	# Logging
 	#
 	modules/sdlog2
+	modules/logger
 
 	#
 	# Library modules
diff --git a/cmake/configs/posix_eagle_hil.cmake b/cmake/configs/posix_eagle_hil.cmake
index 3e6274956f..63ca567be7 100644
--- a/cmake/configs/posix_eagle_hil.cmake
+++ b/cmake/configs/posix_eagle_hil.cmake
@@ -27,6 +27,7 @@ set(config_module_list
 	modules/sensors
 	modules/dataman
 	modules/sdlog2
+	modules/logger
 	modules/simulator
 	modules/commander
 	modules/load_mon
diff --git a/cmake/configs/posix_eagle_legacy_driver_default.cmake b/cmake/configs/posix_eagle_legacy_driver_default.cmake
index 8b82f52613..ed6cd83c16 100644
--- a/cmake/configs/posix_eagle_legacy_driver_default.cmake
+++ b/cmake/configs/posix_eagle_legacy_driver_default.cmake
@@ -45,6 +45,7 @@ set(config_module_list
 	modules/sensors
 	modules/dataman
 	modules/sdlog2
+	modules/logger
 	modules/simulator
 	modules/commander
 
diff --git a/cmake/configs/posix_rpi2_default.cmake b/cmake/configs/posix_rpi2_default.cmake
index c67d241f9f..ca677261be 100644
--- a/cmake/configs/posix_rpi2_default.cmake
+++ b/cmake/configs/posix_rpi2_default.cmake
@@ -31,6 +31,7 @@ set(config_module_list
 	modules/fw_pos_control_l1
 	modules/dataman
 	modules/sdlog2
+	modules/logger
 	modules/commander
 	modules/load_mon
 	lib/controllib
diff --git a/cmake/configs/posix_rpi2_release.cmake b/cmake/configs/posix_rpi2_release.cmake
index e4272e6ef4..b10914951a 100644
--- a/cmake/configs/posix_rpi2_release.cmake
+++ b/cmake/configs/posix_rpi2_release.cmake
@@ -40,6 +40,7 @@ set(config_module_list
 	modules/fw_pos_control_l1
 	modules/dataman
 	modules/sdlog2
+	modules/logger
 	modules/commander
 	modules/load_mon
 	lib/controllib
diff --git a/cmake/configs/posix_sdflight_default.cmake b/cmake/configs/posix_sdflight_default.cmake
index ef1aef2e2b..b6c686c9d4 100644
--- a/cmake/configs/posix_sdflight_default.cmake
+++ b/cmake/configs/posix_sdflight_default.cmake
@@ -38,6 +38,7 @@ set(config_module_list
 	modules/sensors
 	modules/dataman
 	modules/sdlog2
+	modules/logger
 	modules/simulator
 	modules/commander
 	modules/navigator
diff --git a/cmake/configs/posix_sitl_broadcast.cmake b/cmake/configs/posix_sitl_broadcast.cmake
index 2e92a2f08d..9a9029f1c0 100644
--- a/cmake/configs/posix_sitl_broadcast.cmake
+++ b/cmake/configs/posix_sitl_broadcast.cmake
@@ -49,6 +49,7 @@ set(config_module_list
 	modules/fw_pos_control_l1
 	modules/dataman
 	modules/sdlog2
+	modules/logger
 	modules/commander
 	lib/controllib
 	lib/mathlib
diff --git a/cmake/configs/posix_sitl_default.cmake b/cmake/configs/posix_sitl_default.cmake
index 5d3f364be8..e97d62e3a2 100644
--- a/cmake/configs/posix_sitl_default.cmake
+++ b/cmake/configs/posix_sitl_default.cmake
@@ -40,6 +40,7 @@ set(config_module_list
 	modules/fw_pos_control_l1
 	modules/land_detector
 	modules/load_mon
+	modules/logger
 	modules/mavlink
 	modules/mc_att_control
 	modules/mc_att_control_multiplatform
@@ -56,7 +57,6 @@ set(config_module_list
 	modules/systemlib/mixer
 	modules/uORB
 	modules/vtol_att_control
-
 	lib/controllib
 	lib/conversion
 	lib/DriverFramework/framework
diff --git a/cmake/configs/posix_sitl_ekf2.cmake b/cmake/configs/posix_sitl_ekf2.cmake
index 7da1e16d4a..d92ed793a0 100644
--- a/cmake/configs/posix_sitl_ekf2.cmake
+++ b/cmake/configs/posix_sitl_ekf2.cmake
@@ -48,6 +48,7 @@ set(config_module_list
 	modules/fw_pos_control_l1
 	modules/dataman
 	modules/sdlog2
+	modules/logger
 	modules/commander
 	modules/load_mon
 	lib/controllib
diff --git a/cmake/configs/posix_sitl_replay.cmake b/cmake/configs/posix_sitl_replay.cmake
index 959e389bdb..3c99cba7a7 100644
--- a/cmake/configs/posix_sitl_replay.cmake
+++ b/cmake/configs/posix_sitl_replay.cmake
@@ -17,6 +17,7 @@ set(config_module_list
 	modules/ekf2
 	modules/ekf2_replay
 	modules/sdlog2
+	modules/logger
 	lib/controllib
 	lib/mathlib
 	lib/mathlib/math/filter
diff --git a/cmake/configs/posix_sitl_test.cmake b/cmake/configs/posix_sitl_test.cmake
index 5a7d9cd9bd..22e59e9425 100644
--- a/cmake/configs/posix_sitl_test.cmake
+++ b/cmake/configs/posix_sitl_test.cmake
@@ -40,6 +40,7 @@ set(config_module_list
 	modules/fw_pos_control_l1
 	modules/land_detector
 	modules/load_mon
+	modules/logger
 	modules/mavlink
 	modules/mc_att_control
 	modules/mc_att_control_multiplatform
diff --git a/cmake/ros-CMakeLists.txt b/cmake/ros-CMakeLists.txt
index a6df11a3f6..10cb673bfe 100644
--- a/cmake/ros-CMakeLists.txt
+++ b/cmake/ros-CMakeLists.txt
@@ -59,30 +59,29 @@ find_package(Eigen REQUIRED)
 ## Generate messages in the 'msg' folder
 add_message_files(
   FILES
-  rc_channels.msg
-  vehicle_attitude.msg
-  vehicle_attitude_setpoint.msg
-  manual_control_setpoint.msg
-  actuator_controls.msg
-  actuator_controls_0.msg
-  actuator_controls_virtual_mc.msg
-  vehicle_rates_setpoint.msg
-  mc_virtual_rates_setpoint.msg
-  vehicle_attitude.msg
-  vehicle_control_mode.msg
   actuator_armed.msg
-  parameter_update.msg
-  vehicle_status.msg
+  actuator_controls.msg
   commander_state.msg
-  vehicle_local_position.msg
+  control_state.msg
+  distance_sensor.msg
+  manual_control_setpoint.msg
+  mc_virtual_rates_setpoint.msg
+  offboard_control_mode.msg
+  parameter_update.msg
   position_setpoint.msg
   position_setpoint_triplet.msg
-  vehicle_local_position_setpoint.msg
-  vehicle_global_velocity_setpoint.msg
-  offboard_control_mode.msg
+  rc_channels.msg
+  ros/actuator_controls_0.msg
+  ros/actuator_controls_virtual_mc.msg
+  vehicle_attitude.msg
+  vehicle_attitude_setpoint.msg
+  vehicle_control_mode.msg
   vehicle_force_setpoint.msg
-  distance_sensor.msg
-  control_state.msg
+  vehicle_global_velocity_setpoint.msg
+  vehicle_local_position.msg
+  vehicle_local_position_setpoint.msg
+  vehicle_rates_setpoint.msg
+  vehicle_status.msg
 )
 
 ## Generate services in the 'srv' folder
diff --git a/src/modules/logger/CMakeLists.txt b/src/modules/logger/CMakeLists.txt
new file mode 100644
index 0000000000..577f9f1be0
--- /dev/null
+++ b/src/modules/logger/CMakeLists.txt
@@ -0,0 +1,52 @@
+############################################################################
+#
+#   Copyright (c) 2015 PX4 Development Team. All rights reserved.
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions
+# are met:
+#
+# 1. Redistributions of source code must retain the above copyright
+#    notice, this list of conditions and the following disclaimer.
+# 2. Redistributions in binary form must reproduce the above copyright
+#    notice, this list of conditions and the following disclaimer in
+#    the documentation and/or other materials provided with the
+#    distribution.
+# 3. Neither the name PX4 nor the names of its contributors may be
+#    used to endorse or promote products derived from this software
+#    without specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.
+#
+############################################################################
+if (${OS} STREQUAL "nuttx")
+	list(APPEND MODULE_CFLAGS -Wframe-larger-than=2200)
+endif()
+
+px4_add_module(
+	MODULE modules__logger
+	MAIN logger
+	PRIORITY "SCHED_PRIORITY_MAX-30"
+	STACK_MAIN 2200
+	COMPILE_FLAGS
+		${MODULE_CFLAGS}
+		-Os
+	SRCS
+		logger.cpp
+		log_writer.cpp
+	DEPENDS
+		platforms__common
+		modules__uORB
+	)
+# vim: set noet ft=cmake fenc=utf-8 ff=unix : 
diff --git a/src/modules/logger/array.h b/src/modules/logger/array.h
new file mode 100644
index 0000000000..78862ff072
--- /dev/null
+++ b/src/modules/logger/array.h
@@ -0,0 +1,127 @@
+#pragma once
+
+#include <stdlib.h>
+
+namespace px4
+{
+
+template <typename TYPE, size_t N>
+class Array
+{
+	typedef TYPE &reference;
+	typedef const TYPE &const_reference;
+	typedef TYPE *iterator;
+	typedef const TYPE *const_iterator;
+
+public:
+	Array()
+		: _size(0), _overflow(false)
+	{}
+
+	bool push_back(const TYPE &x)
+	{
+		if (_size == N) {
+			_overflow = true;
+			return false;
+
+		} else {
+			_items[_size] = x;
+			++_size;
+			return true;
+		}
+	}
+
+	void remove(unsigned idx)
+	{
+		if (idx < _size) {
+			--_size;
+
+			for (unsigned i = idx; i < _size; ++i) {
+				_items[i] = _items[i + 1];
+			}
+		}
+	}
+
+	reference operator[](size_t n)
+	{
+		return _items[n];
+	}
+
+	const_reference operator[](size_t n) const
+	{
+		return _items[n];
+	}
+
+	reference at(size_t n)
+	{
+		return _items[n];
+	}
+
+	const_reference at(size_t n) const
+	{
+		return _items[n];
+	}
+
+	size_t size() const
+	{
+		return _size;
+	}
+
+	size_t max_size() const
+	{
+		return N;
+	}
+
+	size_t capacity() const
+	{
+		return N;
+	}
+
+	bool empty() const
+	{
+		return _size == 0;
+	}
+
+	bool is_overflowed()
+	{
+		return _overflow;
+	}
+
+	iterator begin()
+	{
+		return &_items[0];
+	}
+
+	iterator end()
+	{
+		return &_items[_size];
+	}
+
+	const_iterator begin() const
+	{
+		return &_items[0];
+	}
+
+	const_iterator end() const
+	{
+		return &_items[_size];
+	}
+
+	void erase(iterator item)
+	{
+		if (item - _items < static_cast<int>(_size)) {
+			--_size;
+
+			for (iterator it = item; it != &_items[_size]; ++it) {
+				*it = *(it + 1);
+			}
+		}
+	}
+
+private:
+	TYPE        _items[N];
+	size_t      _size;
+	bool        _overflow;
+};
+
+}
diff --git a/src/modules/logger/log_writer.cpp b/src/modules/logger/log_writer.cpp
new file mode 100644
index 0000000000..249180167a
--- /dev/null
+++ b/src/modules/logger/log_writer.cpp
@@ -0,0 +1,237 @@
+#include "log_writer.h"
+#include <fcntl.h>
+#include <string.h>
+
+namespace px4
+{
+namespace logger
+{
+
+LogWriter::LogWriter(uint8_t *buffer, size_t buffer_size) :
+	_buffer(buffer),
+	_buffer_size(buffer_size),
+	_min_write_chunk(4096)
+{
+	pthread_mutex_init(&_mtx, nullptr);
+	pthread_cond_init(&_cv, nullptr);
+	/* allocate write performance counters */
+	perf_write = perf_alloc(PC_ELAPSED, "sd write");
+	perf_fsync = perf_alloc(PC_ELAPSED, "sd fsync");
+}
+
+void LogWriter::start_log(const char *filename)
+{
+	::strncpy(_filename, filename, sizeof(_filename));
+	// Clear buffer and counters
+	_head = 0;
+	_count = 0;
+	_total_written = 0;
+	_should_run = true;
+	notify();
+}
+
+void LogWriter::stop_log()
+{
+	_should_run = false;
+	notify();
+}
+
+pthread_t LogWriter::thread_start()
+{
+	pthread_attr_t thr_attr;
+	pthread_attr_init(&thr_attr);
+
+	sched_param param;
+	/* low priority, as this is expensive disk I/O */
+	param.sched_priority = SCHED_PRIORITY_DEFAULT - 40;
+	(void)pthread_attr_setschedparam(&thr_attr, &param);
+
+	pthread_attr_setstacksize(&thr_attr, 1024);
+
+	pthread_t thr;
+
+	if (0 != pthread_create(&thr, &thr_attr, &LogWriter::run_helper, this)) {
+		PX4_WARN("error creating logwriter thread");
+	}
+
+	pthread_attr_destroy(&thr_attr);
+
+	return thr;
+}
+
+void LogWriter::thread_stop()
+{
+	// this will terminate the main loop of the writer thread
+	_exit_thread = true;
+	_should_run = false;
+}
+
+void *LogWriter::run_helper(void *context)
+{
+	px4_prctl(PR_SET_NAME, "log_writer", px4_getpid());
+
+	reinterpret_cast<LogWriter *>(context)->run();
+	return nullptr;
+}
+
+void LogWriter::run()
+{
+	// Wait for _should_run flag
+	while (!_exit_thread) {
+		bool start = false;
+		pthread_mutex_lock(&_mtx);
+		pthread_cond_wait(&_cv, &_mtx);
+		start = _should_run;
+		pthread_mutex_unlock(&_mtx);
+
+		if (start) {
+			break;
+		}
+	}
+
+	while (!_exit_thread) {
+		// Outer endless loop, start new file each time
+		// _filename must be set before setting _should_run = true
+
+
+		_fd = ::open(_filename, O_CREAT | O_WRONLY, PX4_O_MODE_666);
+
+		if (_fd < 0) {
+			PX4_WARN("can't open log file %s", _filename);
+			_should_run = false;
+			continue;
+		}
+
+		PX4_WARN("started, log file: %s", _filename);
+
+		_should_run = true;
+
+		int poll_count = 0;
+		int written = 0;
+
+		while (true) {
+			size_t available = 0;
+			void *read_ptr = nullptr;
+			bool is_part = false;
+
+			/* lock _buffer
+			 * wait for sufficient data, cycle on notify()
+			 */
+			pthread_mutex_lock(&_mtx);
+
+			while (true) {
+				available = get_read_ptr(&read_ptr, &is_part);
+
+				/* if sufficient data available or partial read or terminating, exit this wait loop */
+				if ((available > _min_write_chunk) || is_part || !_should_run) {
+					/* GOTO end of block */
+					break;
+				}
+
+				/* wait for a call to notify()
+				 * this call unlocks the mutex while waiting, and returns with the mutex locked
+				 */
+				pthread_cond_wait(&_cv, &_mtx);
+			}
+
+			pthread_mutex_unlock(&_mtx);
+
+			if (available > 0) {
+				perf_begin(perf_write);
+				written = ::write(_fd, read_ptr, available);
+				perf_end(perf_write);
+
+				/* call fsync periodically to minimize potential loss of data */
+				if (++poll_count >= 100) {
+					perf_begin(perf_fsync);
+					::fsync(_fd);
+					perf_end(perf_fsync);
+					poll_count = 0;
+				}
+
+				if (written < 0) {
+					PX4_WARN("error writing log file");
+					_should_run = false;
+					/* GOTO end of block */
+					break;
+				}
+
+				pthread_mutex_lock(&_mtx);
+				/* subtract bytes written from number in _buffer (_count -= written) */
+				mark_read(written);
+				pthread_mutex_unlock(&_mtx);
+
+				_total_written += written;
+			}
+
+			if (!_should_run && written == static_cast<int>(available) && !is_part) {
+				// Stop only when all data written
+				break;
+			}
+		}
+
+		_head = 0;
+		_count = 0;
+
+		int res = ::close(_fd);
+
+		if (res) {
+			PX4_WARN("error closing log file");
+		}
+
+		PX4_WARN("stopped, bytes written: %zu", _total_written);
+	}
+}
+
+bool LogWriter::write(void *ptr, size_t size)
+{
+
+	// Bytes available to write
+	size_t available = _buffer_size - _count;
+
+	if (size > available) {
+		// buffer overflow
+		return false;
+	}
+
+	size_t n = _buffer_size - _head;	// bytes to end of the buffer
+
+	uint8_t *buffer_c = reinterpret_cast<uint8_t *>(ptr);
+
+	if (size > n) {
+		// Message goes over the end of the buffer
+		memcpy(&(_buffer[_head]), buffer_c, n);
+		_head = 0;
+
+	} else {
+		n = 0;
+	}
+
+	// now: n = bytes already written
+	size_t p = size - n;	// number of bytes to write
+	memcpy(&(_buffer[_head]), &(buffer_c[n]), p);
+	_head = (_head + p) % _buffer_size;
+	_count += size;
+	return true;
+}
+
+size_t LogWriter::get_read_ptr(void **ptr, bool *is_part)
+{
+	// bytes available to read
+	int read_ptr = _head - _count;
+
+	if (read_ptr < 0) {
+		read_ptr += _buffer_size;
+		*ptr = &_buffer[read_ptr];
+		*is_part = true;
+		return _buffer_size - read_ptr;
+
+	} else {
+		*ptr = &_buffer[read_ptr];
+		*is_part = false;
+		return _count;
+	}
+}
+
+}
+}
diff --git a/src/modules/logger/log_writer.h b/src/modules/logger/log_writer.h
new file mode 100644
index 0000000000..f6fd35c4de
--- /dev/null
+++ b/src/modules/logger/log_writer.h
@@ -0,0 +1,74 @@
+#pragma once
+
+#include <px4.h>
+#include <stdint.h>
+#include <pthread.h>
+#include <drivers/drv_hrt.h>
+#include <systemlib/perf_counter.h>
+
+namespace px4
+{
+namespace logger
+{
+
+class LogWriter
+{
+	friend class Logger;
+public:
+	LogWriter(uint8_t *buffer, size_t buffer_size);
+
+	pthread_t thread_start();
+
+	void thread_stop();
+
+	void start_log(const char *filename);
+
+	void stop_log();
+
+	bool write(void *ptr, size_t size);
+
+	void lock()
+	{
+		pthread_mutex_lock(&_mtx);
+	}
+
+	void unlock()
+	{
+		pthread_mutex_unlock(&_mtx);
+	}
+
+	void notify()
+	{
+		pthread_cond_broadcast(&_cv);
+	}
+
+private:
+	static void *run_helper(void *);
+
+	void run();
+
+	size_t get_read_ptr(void **ptr, bool *is_part);
+
+	void mark_read(size_t n)
+	{
+		_count -= n;
+	}
+
+	char		_filename[64];
+	int			_fd;
+	uint8_t 	*_buffer;
+	const size_t	_buffer_size;
+	const size_t	_min_write_chunk;	/* 512 didn't seem to work properly, 4096 should match the FAT cluster size */
+	size_t			_head = 0;
+	size_t			_count = 0;
+	size_t		_total_written = 0;
+	bool		_should_run = false;
+	bool 		_exit_thread = false;
+	pthread_mutex_t		_mtx;
+	pthread_cond_t		_cv;
+	perf_counter_t perf_write;
+	perf_counter_t perf_fsync;
+};
+
+}
+}
diff --git a/src/modules/logger/logger.cpp b/src/modules/logger/logger.cpp
new file mode 100644
index 0000000000..3005690ce2
--- /dev/null
+++ b/src/modules/logger/logger.cpp
@@ -0,0 +1,760 @@
+#include "logger.h"
+
+#include <sys/stat.h>
+#include <errno.h>
+#include <string.h>
+
+#include <uORB/uORBTopics.h>
+#include <px4_getopt.h>
+
+#define DBGPRINT
+
+#if defined(DBGPRINT)
+// needed for mallinfo
+#if defined(__PX4_POSIX) && !defined(__PX4_DARWIN)
+#include <malloc.h>
+#endif /* __PX4_POSIX */
+
+// struct mallinfo not available on OSX?
+#if defined(__PX4_DARWIN)
+#undef DBGPRINT
+#endif /* defined(__PX4_DARWIN) */
+#endif /* defined(DBGPRINT) */
+
+using namespace px4::logger;
+
+int logger_main(int argc, char *argv[])
+{
+	if (argc < 2) {
+		PX4_INFO("usage: logger {start|stop|status}");
+		return 1;
+	}
+
+	if (!strcmp(argv[1], "start")) {
+
+		if (logger_ptr != nullptr) {
+			PX4_INFO("already running");
+			return 1;
+		}
+
+		if (OK != Logger::start((char *const *)argv)) {
+			PX4_WARN("start failed");
+			return 1;
+		}
+
+		return 0;
+	}
+
+	if (!strcmp(argv[1], "stop")) {
+		if (logger_ptr == nullptr) {
+			PX4_INFO("not running");
+			return 1;
+		}
+
+		delete logger_ptr;
+		logger_ptr = nullptr;
+		return 0;
+	}
+
+	if (!strcmp(argv[1], "status")) {
+		if (logger_ptr) {
+			logger_ptr->status();
+			return 0;
+
+		} else {
+			PX4_INFO("not running");
+			return 1;
+		}
+	}
+
+	Logger::usage("unrecognized command");
+	return 1;
+}
+
+namespace px4
+{
+namespace logger
+{
+
+void Logger::usage(const char *reason)
+{
+	if (reason) {
+		PX4_WARN("%s\n", reason);
+	}
+
+	PX4_INFO("usage: logger {start|stop|status} [-r <log rate>] [-b <buffer size>] -e -a -t -x\n"
+		 "\t-r\tLog rate in Hz, 0 means unlimited rate\n"
+		 "\t-b\tLog buffer size in KiB, default is 8\n"
+		 "\t-e\tEnable logging by default (if not, can be started by command)\n"
+		 "\t-a\tLog only when armed (can be still overriden by command)\n"
+		 "\t-t\tUse date/time for naming log directories and files\n"
+		 "\t-x\tExtended logging");
+}
+
+int Logger::start(char *const *argv)
+{
+	ASSERT(logger_task == -1);
+
+	/* start the task */
+	logger_task = px4_task_spawn_cmd("logger",
+					 SCHED_DEFAULT,
+					 SCHED_PRIORITY_MAX - 5,
+					 3100,
+					 (px4_main_t)&Logger::run_trampoline,
+					 (char *const *)argv);
+
+	if (logger_task < 0) {
+		PX4_WARN("task start failed");
+		return -errno;
+	}
+
+	return OK;
+}
+
+void Logger::status()
+{
+	if (!_enabled) {
+		PX4_INFO("running, but not logging");
+
+	} else {
+		PX4_INFO("running");
+
+//		float kibibytes = _writer.get_total_written() / 1024.0f;
+//		float mebibytes = kibibytes / 1024.0f;
+//		float seconds = ((float)(hrt_absolute_time() - _start_time)) / 1000000.0f;
+//
+//		PX4_WARN("wrote %lu msgs, %4.2f MiB (average %5.3f KiB/s), skipped %lu msgs", log_msgs_written, (double)mebibytes, (double)(kibibytes / seconds), log_msgs_skipped);
+//		mavlink_log_info(&mavlink_log_pub, "[blackbox] wrote %lu msgs, skipped %lu msgs", log_msgs_written, log_msgs_skipped);
+	}
+}
+
+void Logger::run_trampoline(int argc, char *argv[])
+{
+	unsigned log_interval = 3500;
+	int log_buffer_size = 12 * 1024;
+	bool log_on_start = false;
+	bool error_flag = false;
+
+	int myoptind = 1;
+	int ch;
+	const char *myoptarg = NULL;
+
+	while ((ch = px4_getopt(argc, argv, "r:b:eatx", &myoptind, &myoptarg)) != EOF) {
+		switch (ch) {
+		case 'r': {
+				unsigned long r = strtoul(myoptarg, NULL, 10);
+
+				if (r <= 0) {
+					r = 1;
+				}
+
+				log_interval = 1e6 / r;
+			}
+			break;
+
+		case 'e':
+			log_on_start = true;
+			break;
+
+		case 'b': {
+				unsigned long s = strtoul(myoptarg, NULL, 10);
+
+				if (s < 1) {
+					s = 1;
+				}
+
+				log_buffer_size = 1024 * s;
+			}
+			break;
+
+		case '?':
+			error_flag = true;
+			break;
+
+		default:
+			PX4_WARN("unrecognized flag");
+			error_flag = true;
+			break;
+		}
+	}
+
+	if (error_flag) {
+		logger_task = -1;
+		return;
+	}
+
+	logger_ptr = new Logger(log_buffer_size, log_interval, log_on_start);
+
+	if (logger_ptr == nullptr) {
+		PX4_WARN("alloc failed");
+
+	} else {
+		logger_ptr->run();
+	}
+}
+
+enum class MessageType : uint8_t {
+	FORMAT = 'F',
+	DATA = 'D',
+	INFO = 'I',
+	PARAMETER = 'P',
+};
+
+/* declare message data structs with byte alignment (no padding) */
+#pragma pack(push, 1)
+struct message_format_s {
+	uint8_t msg_type = static_cast<uint8_t>(MessageType::FORMAT);
+	uint16_t msg_size;
+
+	uint8_t msg_id;
+	uint16_t format_len;
+	char format[2096];
+};
+
+struct message_data_header_s {
+	uint8_t msg_type = static_cast<uint8_t>(MessageType::DATA);
+	uint16_t msg_size;
+
+	uint8_t msg_id;
+	uint8_t multi_id;
+};
+
+// currently unused
+struct message_info_header_s {
+	uint8_t msg_type = static_cast<uint8_t>(MessageType::INFO);
+	uint16_t msg_size;
+
+	uint8_t key_len;
+	char key[255];
+};
+
+struct message_parameter_header_s {
+	uint8_t msg_type = static_cast<uint8_t>(MessageType::PARAMETER);
+	uint16_t msg_size;
+
+	uint8_t key_len;
+	char key[255];
+};
+#pragma pack(pop)
+
+
+static constexpr size_t MAX_DATA_SIZE = 740;
+
+Logger::Logger(size_t buffer_size, unsigned log_interval, bool log_on_start) :
+	_log_on_start(log_on_start),
+	_writer((_log_buffer = new uint8_t[buffer_size]), buffer_size),
+	_log_interval(log_interval)
+{
+}
+
+Logger::~Logger()
+{
+	if (logger_task != -1) {
+		/* task wakes up every 100ms or so at the longest */
+		_task_should_exit = true;
+
+		/* wait for a second for the task to quit at our request */
+		unsigned int i = 0;
+
+		do {
+			/* wait 20ms */
+			usleep(20000);
+
+			/* if we have given up, kill it */
+			if (++i > 200) {
+				px4_task_delete(logger_task);
+				logger_task = -1;
+				break;
+			}
+		} while (logger_task != -1);
+	}
+
+	delete [] _log_buffer;
+	logger_ptr = nullptr;
+}
+
+int Logger::add_topic(const orb_metadata *topic)
+{
+	int fd = -1;
+
+	if (topic->o_size > MAX_DATA_SIZE) {
+		PX4_WARN("skip topic %s, data size is too large: %zu (max is %zu)", topic->o_name, topic->o_size, MAX_DATA_SIZE);
+
+	} else {
+		size_t fields_len = strlen(topic->o_fields);
+
+		if (fields_len > sizeof(message_format_s::format)) {
+			PX4_WARN("skip topic %s, format string is too large: %zu (max is %zu)", topic->o_name, fields_len,
+				 sizeof(message_format_s::format));
+
+		} else {
+			fd = orb_subscribe(topic);
+			_subscriptions.push_back(LoggerSubscription(fd, topic));
+		}
+	}
+
+	return fd;
+}
+
+int Logger::add_topic(const char *name, unsigned interval = 0)
+{
+	const orb_metadata **topics = orb_get_topics();
+	int fd = -1;
+
+	for (size_t i = 0; i < orb_topics_count(); i++) {
+		if (strcmp(name, topics[i]->o_name) == 0) {
+			fd = add_topic(topics[i]);
+			printf("logging topic: %zu, %s\n", i, topics[i]->o_name);
+			break;
+		}
+	}
+
+	if ((fd > 0) && (interval != 0)) {
+		orb_set_interval(fd, interval);
+	}
+
+	return fd;
+}
+
+bool Logger::copy_if_updated_multi(orb_id_t topic, int multi_instance, int *handle, void *buffer,
+				   uint64_t *time_last_checked)
+{
+	bool updated = false;
+
+	// only try to subscribe to topic if this is the first time
+	// after that just check after a certain interval to avoid high cpu usage
+	if (*handle < 0 && (*time_last_checked == 0 || hrt_elapsed_time(time_last_checked) > TRY_SUBSCRIBE_INTERVAL)) {
+		//if (multi_instance == 1) warnx("checking instance 1 of topic %s", topic->o_name);
+		*time_last_checked = hrt_absolute_time();
+
+		if (OK == orb_exists(topic, multi_instance)) {
+			*handle = orb_subscribe_multi(topic, multi_instance);
+
+			//warnx("subscribed to instance %d of topic %s", multi_instance, topic->o_name);
+
+			/* copy first data */
+			if (*handle >= 0) {
+				orb_copy(topic, *handle, buffer);
+				updated = true;
+			}
+		}
+
+	} else if (*handle >= 0) {
+		orb_check(*handle, &updated);
+
+		if (updated) {
+			orb_copy(topic, *handle, buffer);
+		}
+	}
+
+	return updated;
+}
+
+void Logger::run()
+{
+#ifdef DBGPRINT
+	struct mallinfo alloc_info = {};
+#endif /* DBGPRINT */
+
+	PX4_WARN("started");
+
+	int mkdir_ret = mkdir(LOG_ROOT, S_IRWXU | S_IRWXG | S_IRWXO);
+
+	if (mkdir_ret == 0) {
+		PX4_WARN("log root dir created: %s", LOG_ROOT);
+
+	} else if (errno != EEXIST) {
+		PX4_WARN("failed creating log root dir: %s", LOG_ROOT);
+		return;
+	}
+
+	add_topic("manual_control_setpoint", 10);
+	add_topic("vehicle_rates_setpoint", 10);
+	add_topic("vehicle_attitude_setpoint", 10);
+	add_topic("vehicle_attitude", 10);
+	add_topic("actuator_outputs", 50);
+	add_topic("battery_status", 100);
+	add_topic("vehicle_command", 100);
+	add_topic("actuator_controls", 10);
+	add_topic("vehicle_local_position_setpoint", 30);
+	add_topic("rc_channels", 100);
+	add_topic("ekf2_innovations", 20);
+	add_topic("commander_state", 100);
+	add_topic("vehicle_local_position", 10);
+	add_topic("vehicle_global_position", 10);
+	add_topic("system_power", 100);
+	add_topic("servorail_status", 100);
+	add_topic("mc_att_ctrl_status", 50);
+	add_topic("control_state");
+	add_topic("estimator_status");
+	add_topic("vehicle_status", 20);
+
+	_writer_thread = _writer.thread_start();
+
+	_task_should_exit = false;
+
+#ifdef DBGPRINT
+	hrt_abstime	dropout_start = 0;
+	hrt_abstime	timer_start = 0;
+	uint32_t	total_bytes = 0;
+	uint16_t	dropout_count = 0;
+	size_t 		highWater = 0;
+	size_t		available = 0;
+	double		max_drop_len = 0;
+#endif /* DBGPRINT */
+
+	// we start logging immediately
+	// the case where we wait with logging until vehicle is armed is handled below
+	if (_log_on_start) {
+		start_log();
+	}
+
+	/* every log_interval usec, check for orb updates */
+	while (!_task_should_exit) {
+
+		// Start/stop logging when system arm/disarm
+		if (_vehicle_status_sub.check_updated()) {
+			_vehicle_status_sub.update();
+			bool armed = (_vehicle_status_sub.get().arming_state == vehicle_status_s::ARMING_STATE_ARMED) ||
+				     (_vehicle_status_sub.get().arming_state == vehicle_status_s::ARMING_STATE_ARMED_ERROR);
+
+			if (_enabled != armed && !_log_on_start) {
+				if (armed) {
+					start_log();
+
+#ifdef DBGPRINT
+					timer_start = hrt_absolute_time();
+					total_bytes = 0;
+#endif /* DBGPRINT */
+
+				} else {
+					stop_log();
+				}
+			}
+		}
+
+		if (_enabled) {
+			/* wait for lock on log buffer */
+			_writer.lock();
+
+			bool data_written = false;
+
+			// Write data messages for normal subscriptions
+			int msg_id = 0;
+
+			for (LoggerSubscription &sub : _subscriptions) {
+				/* each message consists of a header followed by an orb data object
+				 * The size of the data object is given by orb_metadata.o_size
+				 */
+				size_t msg_size = sizeof(message_data_header_s) + sub.metadata->o_size;
+				uint8_t buffer[msg_size];
+
+				/* if this topic has been updated, copy the new data into the message buffer
+				 * and write a message to the log
+				 */
+				//orb_check(sub.fd, &updated);	// check whether a non-multi topic has been updated
+				/* this works for both single and multi-instances */
+				for (uint8_t instance = 0; instance < ORB_MULTI_MAX_INSTANCES; instance++) {
+					if (copy_if_updated_multi(sub.metadata, instance, &sub.fd[instance], buffer + sizeof(message_data_header_s),
+								  &sub.time_tried_subscribe)) {
+
+						//uint64_t timestamp;
+						//memcpy(&timestamp, buffer + sizeof(message_data_header_s), sizeof(timestamp));
+						//warnx("topic: %s, instance: %d, timestamp: %llu",
+						//		sub.metadata->o_name, instance, timestamp);
+
+						/* copy the current topic data into the buffer after the header */
+						//orb_copy(sub.metadata, sub.fd, buffer + sizeof(message_data_header_s));
+
+						/* fill the message header struct in-place at the front of the buffer,
+						 * accessing the unaligned (packed) structure properly
+						 */
+						message_data_header_s *header = reinterpret_cast<message_data_header_s *>(buffer);
+						header->msg_type = static_cast<uint8_t>(MessageType::DATA);
+						/* the ORB topic data object has 2 unused trailing bytes? */
+						header->msg_size = static_cast<uint16_t>(msg_size - 2);
+						header->msg_id = msg_id;
+						header->multi_id = 0x80 + instance;	// Non multi, active
+
+#ifdef DBGPRINT
+						//warnx("subscription %s updated: %d, size: %d", sub.metadata->o_name, updated, msg_size);
+						hrt_abstime trytime = hrt_absolute_time();
+
+						if (_writer._count > highWater) {
+							highWater = _writer._count;
+						}
+
+#endif /* DBGPRINT */
+
+						if (_writer.write(buffer, msg_size)) {
+
+#ifdef DBGPRINT
+
+							// successful write: note end of dropout if dropout_start != 0
+							if (dropout_start != 0) {
+								double drop_len = (double)(trytime - dropout_start)  * 1e-6;
+
+								if (drop_len > max_drop_len) { max_drop_len = drop_len; }
+
+								PX4_WARN("dropout length: %5.3f seconds", drop_len);
+								dropout_start = 0;
+								highWater = 0;
+							}
+
+							total_bytes += msg_size;
+#endif /* DBGPRINT */
+
+							data_written = true;
+
+						} else {
+
+#ifdef DBGPRINT
+
+							if (dropout_start == 0)	{
+								available = _writer._count;
+								PX4_WARN("dropout, available: %d/%d", available, _writer._buffer_size);
+								dropout_start = trytime;
+								dropout_count++;
+							}
+
+#endif /* DBGPRINT */
+
+							break;	// Write buffer overflow, skip this record
+						}
+					}
+				}
+
+				msg_id++;
+			}
+
+			/* release the log buffer */
+			_writer.unlock();
+
+			/* notify the writer thread if data is available */
+			if (data_written) {
+				_writer.notify();
+			}
+
+#ifdef DBGPRINT
+			double deltat = (double)(hrt_absolute_time() - timer_start)  * 1e-6;
+
+			if (deltat > 4.0) {
+				alloc_info = mallinfo();
+				double throughput = total_bytes / deltat;
+				PX4_INFO("%8.1e Kbytes/sec, %d highWater,  %d dropouts, %5.3f sec max, free heap: %d",
+					 throughput / 1e3, highWater, dropout_count, max_drop_len, alloc_info.fordblks);
+
+				total_bytes = 0;
+				highWater = 0,
+				dropout_count = 0;
+				max_drop_len = 0;
+				timer_start = hrt_absolute_time();
+			}
+
+#endif /* DBGPRINT */
+
+		}
+
+		usleep(_log_interval);
+	}
+
+	// stop the writer thread
+	_writer.thread_stop();
+
+	_writer.notify();
+
+	// wait for thread to complete
+	int ret = pthread_join(_writer_thread, NULL);
+
+	if (ret) {
+		PX4_WARN("join failed: %d", ret);
+	}
+
+	logger_task = -1;
+}
+
+int Logger::create_log_dir()
+{
+	/* create dir on sdcard if needed */
+	uint16_t dir_number = 1; // start with dir sess001
+	int mkdir_ret;
+
+	/* look for the next dir that does not exist */
+	while (dir_number <= MAX_NO_LOGFOLDER) {
+		/* format log dir: e.g. /fs/microsd/sess001 */
+		sprintf(_log_dir, "%s/sess%03u", LOG_ROOT, dir_number);
+		mkdir_ret = mkdir(_log_dir, S_IRWXU | S_IRWXG | S_IRWXO);
+
+		if (mkdir_ret == 0) {
+			PX4_INFO("log dir created: %s", _log_dir);
+			break;
+
+		} else if (errno != EEXIST) {
+			PX4_WARN("failed creating new dir: %s", _log_dir);
+			return -1;
+		}
+
+		/* dir exists already */
+		dir_number++;
+		continue;
+	}
+
+	if (dir_number >= MAX_NO_LOGFOLDER) {
+		/* we should not end up here, either we have more than MAX_NO_LOGFOLDER on the SD card, or another problem */
+		PX4_WARN("all %d possible dirs exist already", MAX_NO_LOGFOLDER);
+		return -1;
+	}
+
+	/* print logging path, important to find log file later */
+	//mavlink_and_console_log_info(mavlink_fd, "[sdlog2] log dir: %s", log_dir);
+	return 0;
+}
+
+bool Logger::file_exist(const char *filename)
+{
+	struct stat buffer;
+	return stat(filename, &buffer) == 0;
+}
+
+int Logger::get_log_file_name(char *file_name, size_t file_name_size)
+{
+	uint16_t file_number = 1; // start with file log001
+
+	/* look for the next file that does not exist */
+	while (file_number <= MAX_NO_LOGFILE) {
+		/* format log file path: e.g. /fs/microsd/sess001/log001.ulg */
+		snprintf(file_name, file_name_size, "%s/log%03u.ulg", _log_dir, file_number);
+
+		if (!file_exist(file_name)) {
+			break;
+		}
+
+		file_number++;
+	}
+
+	if (file_number > MAX_NO_LOGFILE) {
+		/* we should not end up here, either we have more than MAX_NO_LOGFILE on the SD card, or another problem */
+		//mavlink_and_console_log_critical(mavlink_fd, "[sdlog2] ERR: max files %d", MAX_NO_LOGFILE);
+		return -1;
+	}
+
+	return 0;
+}
+
+void Logger::start_log()
+{
+	PX4_WARN("start log");
+
+	if (create_log_dir()) {
+		return;
+	}
+
+	char file_name[64] = "";
+
+	if (get_log_file_name(file_name, sizeof(file_name))) {
+		return;
+	}
+
+	_writer.start_log(file_name);
+	write_formats();
+	write_parameters();
+	_enabled = true;
+}
+
+void Logger::stop_log()
+{
+	_enabled = false;
+	_writer.stop_log();
+}
+
+void Logger::write_formats()
+{
+	_writer.lock();
+	message_format_s msg;
+
+	int msg_id = 0;
+
+	for (LoggerSubscription &sub : _subscriptions) {
+		msg.msg_id = msg_id;
+		msg.format_len = snprintf(msg.format, sizeof(msg.format), "%s", sub.metadata->o_fields);
+		size_t msg_size = sizeof(msg) - sizeof(msg.format) + msg.format_len;
+		msg.msg_size = msg_size - 2;
+
+		while (!_writer.write(&msg, msg_size)) {
+			_writer.unlock();
+			_writer.notify();
+			usleep(_log_interval);	// Wait if buffer is full, don't skip FORMAT messages
+			_writer.lock();
+		}
+
+		msg_id++;
+	}
+
+	_writer.unlock();
+	_writer.notify();
+}
+
+void Logger::write_parameters()
+{
+	_writer.lock();
+	uint8_t buffer[sizeof(message_parameter_header_s) + sizeof(param_value_u)];
+	message_parameter_header_s *msg = reinterpret_cast<message_parameter_header_s *>(buffer);
+
+	msg->msg_type = static_cast<uint8_t>(MessageType::PARAMETER);
+	int param_idx = 0;
+	param_t param = 0;
+
+	do {
+		do {
+			param = param_for_index(param_idx);
+			++param_idx;
+		} while (param != PARAM_INVALID && !param_used(param));
+
+		if (param != PARAM_INVALID) {
+			/* get parameter type and size */
+			const char *type_str;
+			param_type_t type = param_type(param);
+			size_t value_size = 0;
+
+			switch (type) {
+			case PARAM_TYPE_INT32:
+				type_str = "int32_t";
+				value_size = sizeof(int32_t);
+				break;
+
+			case PARAM_TYPE_FLOAT:
+				type_str = "float";
+				value_size = sizeof(float);
+				break;
+
+			default:
+				continue;
+			}
+
+			/* format parameter key (type and name) */
+			msg->key_len = snprintf(msg->key, sizeof(msg->key), "%s %s", type_str, param_name(param));
+			size_t msg_size = sizeof(*msg) - sizeof(msg->key) + msg->key_len;
+
+			/* copy parameter value directly to buffer */
+			param_get(param, &buffer[msg_size]);
+			msg_size += value_size;
+
+			msg->msg_size = msg_size - 2;
+
+			/* write message */
+			while (!_writer.write(buffer, msg_size)) {
+				/* wait if buffer is full, don't skip PARAMETER messages */
+				_writer.unlock();
+				_writer.notify();
+				usleep(_log_interval);
+				_writer.lock();
+			}
+		}
+	} while ((param != PARAM_INVALID) && (param_idx < (int) param_count()));
+
+	_writer.unlock();
+	_writer.notify();
+}
+
+}
+}
diff --git a/src/modules/logger/logger.h b/src/modules/logger/logger.h
new file mode 100644
index 0000000000..f7a30c6f0f
--- /dev/null
+++ b/src/modules/logger/logger.h
@@ -0,0 +1,96 @@
+#pragma once
+
+#include "log_writer.h"
+#include "array.h"
+#include <px4.h>
+#include <drivers/drv_hrt.h>
+#include <uORB/Subscription.hpp>
+#include <uORB/topics/vehicle_status.h>
+
+extern "C" __EXPORT int logger_main(int argc, char *argv[]);
+
+#define TRY_SUBSCRIBE_INTERVAL 1000*1000	// interval in microseconds at which we try to subscribe to a topic
+// if we haven't succeeded before
+
+namespace px4
+{
+namespace logger
+{
+
+struct LoggerSubscription {
+	int fd[ORB_MULTI_MAX_INSTANCES];
+	uint64_t time_tried_subscribe;	// captures the time at which we checked last time if this instance existed
+	const orb_metadata *metadata = nullptr;
+
+	LoggerSubscription() {}
+
+	LoggerSubscription(int fd_, const orb_metadata *metadata_) :
+		metadata(metadata_)
+	{
+		fd[0] = fd_;
+		time_tried_subscribe = 0;
+
+		for (int i = 1; i < ORB_MULTI_MAX_INSTANCES; i++) { fd[i] = -1; }
+	}
+};
+
+class Logger
+{
+public:
+	Logger(size_t buffer_size, unsigned log_interval, bool log_on_start);
+
+	~Logger();
+
+	int add_topic(const orb_metadata *topic);
+
+	int add_topic(const char *name, unsigned interval);
+
+	static int start(char *const *argv);
+
+	static void usage(const char *reason);
+
+	void status();
+
+private:
+	static void run_trampoline(int argc, char *argv[]);
+
+	void run();
+
+	int create_log_dir();
+
+	static bool file_exist(const char *filename);
+
+	int get_log_file_name(char *file_name, size_t file_name_size);
+
+	void start_log();
+
+	void stop_log();
+
+	void write_formats();
+
+	void write_parameters();
+
+	bool copy_if_updated_multi(orb_id_t topic, int multi_instance, int *handle, void *buffer, uint64_t *time_last_checked);
+
+	static constexpr size_t 	MAX_TOPICS_NUM = 128;
+	static constexpr unsigned	MAX_NO_LOGFOLDER = 999;	/**< Maximum number of log dirs */
+	static constexpr unsigned	MAX_NO_LOGFILE = 999;	/**< Maximum number of log files */
+	static constexpr const char 		*LOG_ROOT = PX4_ROOTFSDIR"/fs/microsd/log";
+
+	bool						_task_should_exit = true;
+	uint8_t 					*_log_buffer;
+	char 						_log_dir[64];
+	uORB::Subscription<vehicle_status_s>	_vehicle_status_sub {ORB_ID(vehicle_status)};
+	bool						_enabled = false;
+	bool 						_log_on_start;
+	Array<LoggerSubscription, MAX_TOPICS_NUM>	_subscriptions;
+	LogWriter					_writer;
+	uint32_t					_log_interval;
+};
+
+Logger *logger_ptr;
+int		logger_task = -1;
+pthread_t _writer_thread;
+
+}
+}
diff --git a/src/modules/systemlib/system_params.c b/src/modules/systemlib/system_params.c
index dae66295aa..692f919126 100644
--- a/src/modules/systemlib/system_params.c
+++ b/src/modules/systemlib/system_params.c
@@ -139,3 +139,15 @@ PARAM_DEFINE_INT32(SYS_COMPANION, 157600);
  * @group System
  */
 PARAM_DEFINE_INT32(SYS_PARAM_VER, 1);
+
+/**
+ * SD logger
+ *
+ * @value 0 sdlog2 (default)
+ * @value 1 new logger (experimental)
+ * @min 0
+ * @max 1
+ * @reboot_required true
+ * @group System
+ */
+PARAM_DEFINE_INT32(SYS_LOGGER, 0);

From 672042e051fb44d55f72eda096f39eee787b99f7 Mon Sep 17 00:00:00 2001
From: Mark Whitehorn <kd0aij@gmail.com>
Date: Mon, 25 Apr 2016 21:25:22 -0600
Subject: [PATCH 0131/1230] not enough RAM for 32K buffer: reduce to 20K

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

diff --git a/ROMFS/px4fmu_common/init.d/rc.logging b/ROMFS/px4fmu_common/init.d/rc.logging
index f46d94dbc7..84e3cbb5e7 100644
--- a/ROMFS/px4fmu_common/init.d/rc.logging
+++ b/ROMFS/px4fmu_common/init.d/rc.logging
@@ -31,7 +31,7 @@ then
 				then
 				fi
 			else
-				if logger start -b 32
+				if logger start -b 20
 				then
 				fi
 			fi

From 728de5f87b4a5d3c0e86d33de2bffe4ab9daedf4 Mon Sep 17 00:00:00 2001
From: Mark Whitehorn <kd0aij@gmail.com>
Date: Wed, 27 Apr 2016 08:28:39 -0600
Subject: [PATCH 0132/1230] clean up file open/close logic

---
 src/modules/logger/log_writer.cpp | 55 ++++++++++++++++---------------
 src/modules/logger/log_writer.h   |  1 +
 src/modules/logger/logger.cpp     |  2 +-
 3 files changed, 30 insertions(+), 28 deletions(-)

diff --git a/src/modules/logger/log_writer.cpp b/src/modules/logger/log_writer.cpp
index 249180167a..3752706182 100644
--- a/src/modules/logger/log_writer.cpp
+++ b/src/modules/logger/log_writer.cpp
@@ -22,11 +22,23 @@ LogWriter::LogWriter(uint8_t *buffer, size_t buffer_size) :
 void LogWriter::start_log(const char *filename)
 {
 	::strncpy(_filename, filename, sizeof(_filename));
+	_fd = ::open(_filename, O_CREAT | O_WRONLY, PX4_O_MODE_666);
+
+	if (_fd < 0) {
+		PX4_WARN("can't open log file %s", _filename);
+		_should_run = false;
+		return;
+
+	} else {
+		PX4_WARN("opened log file: %s", _filename);
+		_should_run = true;
+		_running = true;
+	}
+
 	// Clear buffer and counters
 	_head = 0;
 	_count = 0;
 	_total_written = 0;
-	_should_run = true;
 	notify();
 }
 
@@ -90,21 +102,7 @@ void LogWriter::run()
 	}
 
 	while (!_exit_thread) {
-		// Outer endless loop, start new file each time
-		// _filename must be set before setting _should_run = true
-
-
-		_fd = ::open(_filename, O_CREAT | O_WRONLY, PX4_O_MODE_666);
-
-		if (_fd < 0) {
-			PX4_WARN("can't open log file %s", _filename);
-			_should_run = false;
-			continue;
-		}
-
-		PX4_WARN("started, log file: %s", _filename);
-
-		_should_run = true;
+		// Outer endless loop
 
 		int poll_count = 0;
 		int written = 0;
@@ -164,22 +162,25 @@ void LogWriter::run()
 				_total_written += written;
 			}
 
-			if (!_should_run && written == static_cast<int>(available) && !is_part) {
+			if (_running && !_should_run && written == static_cast<int>(available) && !is_part) {
 				// Stop only when all data written
+				_running = false;
+				_head = 0;
+				_count = 0;
+
+				int res = ::close(_fd);
+
+				if (res) {
+					PX4_WARN("error closing log file");
+
+				} else {
+					PX4_WARN("closed logfile: %s, bytes written: %zu", _filename, _total_written);
+				}
+
 				break;
 			}
 		}
 
-		_head = 0;
-		_count = 0;
-
-		int res = ::close(_fd);
-
-		if (res) {
-			PX4_WARN("error closing log file");
-		}
-
-		PX4_WARN("stopped, bytes written: %zu", _total_written);
 	}
 }
 
diff --git a/src/modules/logger/log_writer.h b/src/modules/logger/log_writer.h
index f6fd35c4de..2c1b19f083 100644
--- a/src/modules/logger/log_writer.h
+++ b/src/modules/logger/log_writer.h
@@ -63,6 +63,7 @@ private:
 	size_t			_count = 0;
 	size_t		_total_written = 0;
 	bool		_should_run = false;
+	bool		_running = false;
 	bool 		_exit_thread = false;
 	pthread_mutex_t		_mtx;
 	pthread_cond_t		_cv;
diff --git a/src/modules/logger/logger.cpp b/src/modules/logger/logger.cpp
index 3005690ce2..2a53dbb1c8 100644
--- a/src/modules/logger/logger.cpp
+++ b/src/modules/logger/logger.cpp
@@ -356,7 +356,7 @@ void Logger::run()
 	struct mallinfo alloc_info = {};
 #endif /* DBGPRINT */
 
-	PX4_WARN("started");
+	PX4_WARN("logger started");
 
 	int mkdir_ret = mkdir(LOG_ROOT, S_IRWXU | S_IRWXG | S_IRWXO);
 

From 9a0e962cbfbfbe03c0ec0e309a23ee157468aa1b Mon Sep 17 00:00:00 2001
From: Daniel Agar <daniel@agar.ca>
Date: Mon, 25 Apr 2016 17:36:13 -0400
Subject: [PATCH 0133/1230] uorb autogeneration

---
 msg/templates/uorb/msg.cpp.template | 12 +++-----
 msg/templates/uorb/msg.h.template   | 46 +++++++++++++++++++++++++----
 src/modules/uORB/uORB.h             |  2 +-
 3 files changed, 45 insertions(+), 15 deletions(-)

diff --git a/msg/templates/uorb/msg.cpp.template b/msg/templates/uorb/msg.cpp.template
index ff5afb297e..54313dc5ec 100644
--- a/msg/templates/uorb/msg.cpp.template
+++ b/msg/templates/uorb/msg.cpp.template
@@ -57,8 +57,7 @@ uorb_struct = '%s_s'%spec.short_name
 uorb_pack_func = 'pack_%s'%spec.short_name
 topic_name = spec.short_name
 
-type_map = {
-  'int8': 'int8_t',
+type_map = {'int8': 'int8_t',
   'int16': 'int16_t',
   'int32': 'int32_t',
   'int64': 'int64_t',
@@ -72,11 +71,9 @@ type_map = {
   'char': 'char',
   'fence_vertex': 'fence_vertex',
   'position_setpoint': 'position_setpoint',
-  'esc_report': 'esc_report'
-}
+  'esc_report': 'esc_report'}
 
-msgtype_size_map = {
-  'int8': 8,
+msgtype_size_map = {'int8': 8,
   'int16': 16,
   'int32': 32,
   'int64': 64,
@@ -90,8 +87,7 @@ msgtype_size_map = {
   'char': 1,
   'fence_vertex': 8,
   'position_setpoint': 104,
-  'esc_report': 36
-}
+  'esc_report': 36}
 
 def convert_type(spec_type):
     bare_type = spec_type
diff --git a/msg/templates/uorb/msg.h.template b/msg/templates/uorb/msg.h.template
index 564657c2e9..8ad98c61d5 100644
--- a/msg/templates/uorb/msg.h.template
+++ b/msg/templates/uorb/msg.h.template
@@ -63,6 +63,11 @@ topic_name = spec.short_name
 @# Generic Includes
 @##############################
 #include <stdint.h>
+#ifdef __cplusplus
+#include <cstring>
+#else
+#include <string.h>
+#endif
 #include <uORB/uORB.h>
 
 @##############################
@@ -88,8 +93,7 @@ for field in spec.parsed_fields():
 @# Main struct of message
 @##############################
 @{
-type_map = {
-  'int8': 'int8_t',
+type_map = {'int8': 'int8_t',
   'int16': 'int16_t',
   'int32': 'int32_t',
   'int64': 'int64_t',
@@ -124,6 +128,33 @@ msgtype_size_map = {
   'esc_report': 36
 }
 
+def bare_name(msg_type):
+    bare = msg_type
+    if '/' in msg_type:
+        # removing prefix
+        bare = (msg_type.split('/'))[1]
+    # removing suffix
+    return bare.split('[')[0]
+
+def sizeof_field_type(field):
+    return msgtype_size_map[bare_name(field.type)]
+
+msgtype_size_map = {'int8': 8,
+  'int16': 16,
+  'int32': 32,
+  'int64': 64,
+  'uint8': 8,
+  'uint16': 16,
+  'uint32': 32,
+  'uint64': 64,
+  'float32': 32,
+  'float64': 64,
+  'bool': 1,
+  'char': 1,
+  'fence_vertex': 8,
+  'position_setpoint': 104,
+  'esc_report': 36}
+
 def bare_name(msg_type):
     bare = msg_type
     if '/' in msg_type:
@@ -173,8 +204,7 @@ def print_parsed_fields():
 
 }@
 
-// #pragma pack(push, 1)
-ORBPACKED(
+#pragma pack(push, 1)
 #ifdef __cplusplus
 @#class @(uorb_struct) {
 struct __EXPORT @(uorb_struct) {
@@ -199,8 +229,12 @@ for constant in spec.constants:
   print('\tstatic const %s %s = %s;'%(type_px4, constant.name, int(constant.val)))
 }
 #endif
-});
-//#pragma pack(pop)
+};
+#pragma pack(pop)
+
+/**
+ * @@}
+ */
 
 /* register this as object request broker structure */
 @[for multi_topic in topics]@
diff --git a/src/modules/uORB/uORB.h b/src/modules/uORB/uORB.h
index f58e093090..d9e57c9436 100644
--- a/src/modules/uORB/uORB.h
+++ b/src/modules/uORB/uORB.h
@@ -118,7 +118,7 @@ enum ORB_PRIO {
  *
  * @param _name		The name of the topic.
  * @param _struct	The structure the topic provides.
- * @param _fields	All fields in a semicolon separated list e.g: "float[3] position;bool armed"
+ * @param _func		The pointer to a function that packs topic
  */
 #define ORB_DEFINE(_name, _struct, _fields)			\
 	const struct orb_metadata __orb_##_name = {	\

From dcdeefd5ea4f44de659e805484254fa5030d8dac Mon Sep 17 00:00:00 2001
From: Daniel Agar <daniel@agar.ca>
Date: Mon, 25 Apr 2016 17:36:13 -0400
Subject: [PATCH 0134/1230] new logger

---
 ROMFS/px4fmu_common/init.d/rc.logging |  2 +-
 src/modules/logger/log_writer.cpp     | 55 +++++++++++++--------------
 src/modules/logger/log_writer.h       |  1 -
 src/modules/logger/logger.cpp         |  2 +-
 4 files changed, 29 insertions(+), 31 deletions(-)

diff --git a/ROMFS/px4fmu_common/init.d/rc.logging b/ROMFS/px4fmu_common/init.d/rc.logging
index 84e3cbb5e7..f46d94dbc7 100644
--- a/ROMFS/px4fmu_common/init.d/rc.logging
+++ b/ROMFS/px4fmu_common/init.d/rc.logging
@@ -31,7 +31,7 @@ then
 				then
 				fi
 			else
-				if logger start -b 20
+				if logger start -b 32
 				then
 				fi
 			fi
diff --git a/src/modules/logger/log_writer.cpp b/src/modules/logger/log_writer.cpp
index 3752706182..249180167a 100644
--- a/src/modules/logger/log_writer.cpp
+++ b/src/modules/logger/log_writer.cpp
@@ -22,23 +22,11 @@ LogWriter::LogWriter(uint8_t *buffer, size_t buffer_size) :
 void LogWriter::start_log(const char *filename)
 {
 	::strncpy(_filename, filename, sizeof(_filename));
-	_fd = ::open(_filename, O_CREAT | O_WRONLY, PX4_O_MODE_666);
-
-	if (_fd < 0) {
-		PX4_WARN("can't open log file %s", _filename);
-		_should_run = false;
-		return;
-
-	} else {
-		PX4_WARN("opened log file: %s", _filename);
-		_should_run = true;
-		_running = true;
-	}
-
 	// Clear buffer and counters
 	_head = 0;
 	_count = 0;
 	_total_written = 0;
+	_should_run = true;
 	notify();
 }
 
@@ -102,7 +90,21 @@ void LogWriter::run()
 	}
 
 	while (!_exit_thread) {
-		// Outer endless loop
+		// Outer endless loop, start new file each time
+		// _filename must be set before setting _should_run = true
+
+
+		_fd = ::open(_filename, O_CREAT | O_WRONLY, PX4_O_MODE_666);
+
+		if (_fd < 0) {
+			PX4_WARN("can't open log file %s", _filename);
+			_should_run = false;
+			continue;
+		}
+
+		PX4_WARN("started, log file: %s", _filename);
+
+		_should_run = true;
 
 		int poll_count = 0;
 		int written = 0;
@@ -162,25 +164,22 @@ void LogWriter::run()
 				_total_written += written;
 			}
 
-			if (_running && !_should_run && written == static_cast<int>(available) && !is_part) {
+			if (!_should_run && written == static_cast<int>(available) && !is_part) {
 				// Stop only when all data written
-				_running = false;
-				_head = 0;
-				_count = 0;
-
-				int res = ::close(_fd);
-
-				if (res) {
-					PX4_WARN("error closing log file");
-
-				} else {
-					PX4_WARN("closed logfile: %s, bytes written: %zu", _filename, _total_written);
-				}
-
 				break;
 			}
 		}
 
+		_head = 0;
+		_count = 0;
+
+		int res = ::close(_fd);
+
+		if (res) {
+			PX4_WARN("error closing log file");
+		}
+
+		PX4_WARN("stopped, bytes written: %zu", _total_written);
 	}
 }
 
diff --git a/src/modules/logger/log_writer.h b/src/modules/logger/log_writer.h
index 2c1b19f083..f6fd35c4de 100644
--- a/src/modules/logger/log_writer.h
+++ b/src/modules/logger/log_writer.h
@@ -63,7 +63,6 @@ private:
 	size_t			_count = 0;
 	size_t		_total_written = 0;
 	bool		_should_run = false;
-	bool		_running = false;
 	bool 		_exit_thread = false;
 	pthread_mutex_t		_mtx;
 	pthread_cond_t		_cv;
diff --git a/src/modules/logger/logger.cpp b/src/modules/logger/logger.cpp
index 2a53dbb1c8..3005690ce2 100644
--- a/src/modules/logger/logger.cpp
+++ b/src/modules/logger/logger.cpp
@@ -356,7 +356,7 @@ void Logger::run()
 	struct mallinfo alloc_info = {};
 #endif /* DBGPRINT */
 
-	PX4_WARN("logger started");
+	PX4_WARN("started");
 
 	int mkdir_ret = mkdir(LOG_ROOT, S_IRWXU | S_IRWXG | S_IRWXO);
 

From 56cc9bd3771ee4e586a60186efa04a759867446d Mon Sep 17 00:00:00 2001
From: Mark Whitehorn <kd0aij@gmail.com>
Date: Tue, 26 Apr 2016 16:49:37 -0600
Subject: [PATCH 0135/1230] my topics

---
 src/modules/logger/logger.cpp | 9 +++++----
 1 file changed, 5 insertions(+), 4 deletions(-)

diff --git a/src/modules/logger/logger.cpp b/src/modules/logger/logger.cpp
index 3005690ce2..d027da31a3 100644
--- a/src/modules/logger/logger.cpp
+++ b/src/modules/logger/logger.cpp
@@ -368,7 +368,8 @@ void Logger::run()
 		return;
 	}
 
-	add_topic("manual_control_setpoint", 10);
+	add_topic("sensor_gyro", 10);
+	add_topic("sensor_accel", 10);
 	add_topic("vehicle_rates_setpoint", 10);
 	add_topic("vehicle_attitude_setpoint", 10);
 	add_topic("vehicle_attitude", 10);
@@ -378,7 +379,7 @@ void Logger::run()
 	add_topic("actuator_controls", 10);
 	add_topic("vehicle_local_position_setpoint", 30);
 	add_topic("rc_channels", 100);
-	add_topic("ekf2_innovations", 20);
+//	add_topic("ekf2_innovations", 20);
 	add_topic("commander_state", 100);
 	add_topic("vehicle_local_position", 10);
 	add_topic("vehicle_global_position", 10);
@@ -386,8 +387,8 @@ void Logger::run()
 	add_topic("servorail_status", 100);
 	add_topic("mc_att_ctrl_status", 50);
 	add_topic("control_state");
-	add_topic("estimator_status");
-	add_topic("vehicle_status", 20);
+//	add_topic("estimator_status");
+	add_topic("vehicle_status", 100);
 
 	_writer_thread = _writer.thread_start();
 

From f07c93651f6001c4b5b6706aa2d0d3ff0e7be9d5 Mon Sep 17 00:00:00 2001
From: Mark Whitehorn <kd0aij@gmail.com>
Date: Wed, 27 Apr 2016 08:28:39 -0600
Subject: [PATCH 0136/1230] clean up file open/close logic

---
 src/modules/logger/log_writer.cpp | 52 +++++++++++++++----------------
 src/modules/logger/log_writer.h   |  1 +
 src/modules/logger/logger.cpp     |  2 +-
 3 files changed, 27 insertions(+), 28 deletions(-)

diff --git a/src/modules/logger/log_writer.cpp b/src/modules/logger/log_writer.cpp
index 249180167a..965e313038 100644
--- a/src/modules/logger/log_writer.cpp
+++ b/src/modules/logger/log_writer.cpp
@@ -22,11 +22,22 @@ LogWriter::LogWriter(uint8_t *buffer, size_t buffer_size) :
 void LogWriter::start_log(const char *filename)
 {
 	::strncpy(_filename, filename, sizeof(_filename));
+	_fd = ::open(_filename, O_CREAT | O_WRONLY, PX4_O_MODE_666);
+
+	if (_fd < 0) {
+		PX4_WARN("can't open log file %s", _filename);
+		_should_run = false;
+		return;
+	} else {
+		PX4_WARN("opened log file: %s", _filename);
+		_should_run = true;
+		_running = true;
+	}
+
 	// Clear buffer and counters
 	_head = 0;
 	_count = 0;
 	_total_written = 0;
-	_should_run = true;
 	notify();
 }
 
@@ -90,21 +101,7 @@ void LogWriter::run()
 	}
 
 	while (!_exit_thread) {
-		// Outer endless loop, start new file each time
-		// _filename must be set before setting _should_run = true
-
-
-		_fd = ::open(_filename, O_CREAT | O_WRONLY, PX4_O_MODE_666);
-
-		if (_fd < 0) {
-			PX4_WARN("can't open log file %s", _filename);
-			_should_run = false;
-			continue;
-		}
-
-		PX4_WARN("started, log file: %s", _filename);
-
-		_should_run = true;
+		// Outer endless loop
 
 		int poll_count = 0;
 		int written = 0;
@@ -164,22 +161,23 @@ void LogWriter::run()
 				_total_written += written;
 			}
 
-			if (!_should_run && written == static_cast<int>(available) && !is_part) {
+			if (_running && !_should_run && written == static_cast<int>(available) && !is_part) {
 				// Stop only when all data written
+				_running = false;
+				_head = 0;
+				_count = 0;
+
+				int res = ::close(_fd);
+
+				if (res) {
+					PX4_WARN("error closing log file");
+				} else {
+					PX4_WARN("closed logfile: %s, bytes written: %zu", _filename, _total_written);
+				}
 				break;
 			}
 		}
 
-		_head = 0;
-		_count = 0;
-
-		int res = ::close(_fd);
-
-		if (res) {
-			PX4_WARN("error closing log file");
-		}
-
-		PX4_WARN("stopped, bytes written: %zu", _total_written);
 	}
 }
 
diff --git a/src/modules/logger/log_writer.h b/src/modules/logger/log_writer.h
index f6fd35c4de..2c1b19f083 100644
--- a/src/modules/logger/log_writer.h
+++ b/src/modules/logger/log_writer.h
@@ -63,6 +63,7 @@ private:
 	size_t			_count = 0;
 	size_t		_total_written = 0;
 	bool		_should_run = false;
+	bool		_running = false;
 	bool 		_exit_thread = false;
 	pthread_mutex_t		_mtx;
 	pthread_cond_t		_cv;
diff --git a/src/modules/logger/logger.cpp b/src/modules/logger/logger.cpp
index d027da31a3..f61480ec49 100644
--- a/src/modules/logger/logger.cpp
+++ b/src/modules/logger/logger.cpp
@@ -356,7 +356,7 @@ void Logger::run()
 	struct mallinfo alloc_info = {};
 #endif /* DBGPRINT */
 
-	PX4_WARN("started");
+	PX4_WARN("logger started");
 
 	int mkdir_ret = mkdir(LOG_ROOT, S_IRWXU | S_IRWXG | S_IRWXO);
 

From 1b483bcc2af1c12e1960aa1ff7dd517d758d6527 Mon Sep 17 00:00:00 2001
From: Mark Whitehorn <kd0aij@gmail.com>
Date: Fri, 29 Apr 2016 06:54:58 -0600
Subject: [PATCH 0137/1230] correctly report failure to allocat log_buffer

---
 src/modules/logger/logger.cpp | 16 ++++++++++++++--
 1 file changed, 14 insertions(+), 2 deletions(-)

diff --git a/src/modules/logger/logger.cpp b/src/modules/logger/logger.cpp
index f61480ec49..dd0585887f 100644
--- a/src/modules/logger/logger.cpp
+++ b/src/modules/logger/logger.cpp
@@ -183,12 +183,24 @@ void Logger::run_trampoline(int argc, char *argv[])
 		return;
 	}
 
+#ifdef DBGPRINT
+	struct mallinfo alloc_info = mallinfo();
+	warnx("largest free chunk: %d bytes", alloc_info.mxordblk);
+	warnx("allocating %d bytes for log_buffer", log_buffer_size);
+#endif /* DBGPRINT */
+
 	logger_ptr = new Logger(log_buffer_size, log_interval, log_on_start);
 
-	if (logger_ptr == nullptr) {
-		PX4_WARN("alloc failed");
+	if (logger_ptr->_log_buffer == nullptr) {
+		PX4_WARN("log buffer malloc failed");
 
 	} else {
+
+#ifdef DBGPRINT
+		alloc_info = mallinfo();
+		warnx("remaining free heap: %d bytes", alloc_info.fordblks);
+#endif /* DBGPRINT */
+
 		logger_ptr->run();
 	}
 }

From db858a853affdbaef438a90a48a9141f53f9d647 Mon Sep 17 00:00:00 2001
From: Mark Whitehorn <kd0aij@gmail.com>
Date: Fri, 29 Apr 2016 06:56:27 -0600
Subject: [PATCH 0138/1230] run astyle

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

diff --git a/src/modules/logger/log_writer.cpp b/src/modules/logger/log_writer.cpp
index 965e313038..3752706182 100644
--- a/src/modules/logger/log_writer.cpp
+++ b/src/modules/logger/log_writer.cpp
@@ -28,6 +28,7 @@ void LogWriter::start_log(const char *filename)
 		PX4_WARN("can't open log file %s", _filename);
 		_should_run = false;
 		return;
+
 	} else {
 		PX4_WARN("opened log file: %s", _filename);
 		_should_run = true;
@@ -171,9 +172,11 @@ void LogWriter::run()
 
 				if (res) {
 					PX4_WARN("error closing log file");
+
 				} else {
 					PX4_WARN("closed logfile: %s, bytes written: %zu", _filename, _total_written);
 				}
+
 				break;
 			}
 		}

From 8a77fec9c815fe924a16eadb4339989849cf4416 Mon Sep 17 00:00:00 2001
From: Mark Whitehorn <kd0aij@gmail.com>
Date: Fri, 29 Apr 2016 06:57:27 -0600
Subject: [PATCH 0139/1230] still need this workaround to avoid hardfault

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

diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp
index ba304dd229..bb743239b9 100644
--- a/src/modules/commander/commander.cpp
+++ b/src/modules/commander/commander.cpp
@@ -2295,8 +2295,9 @@ int commander_thread_main(int argc, char *argv[])
 		 * rejection. Back off 2 seconds to not overlay
 		 * home tune.
 		 */
+		uint64_t ts = _home.timestamp;
 		if (status_flags.condition_home_position_valid &&
-			(hrt_elapsed_time(&_home.timestamp) > 2000000) &&
+			(hrt_elapsed_time(&ts) > 2000000) &&
 			_last_mission_instance != mission_result.instance_count) {
 			if (!mission_result.valid) {
 				/* the mission is invalid */

From 89294317a15638ad8917571846bd6257757a29da Mon Sep 17 00:00:00 2001
From: Mark Whitehorn <kd0aij@gmail.com>
Date: Fri, 29 Apr 2016 08:20:52 -0600
Subject: [PATCH 0140/1230] reduce log buffer to 24K

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

diff --git a/ROMFS/px4fmu_common/init.d/rc.logging b/ROMFS/px4fmu_common/init.d/rc.logging
index f46d94dbc7..b88f46ae86 100644
--- a/ROMFS/px4fmu_common/init.d/rc.logging
+++ b/ROMFS/px4fmu_common/init.d/rc.logging
@@ -31,7 +31,7 @@ then
 				then
 				fi
 			else
-				if logger start -b 32
+				if logger start -b 24
 				then
 				fi
 			fi

From 02b6d2541451df47ef2c35d2e29f622ae5880235 Mon Sep 17 00:00:00 2001
From: Mark Whitehorn <kd0aij@gmail.com>
Date: Fri, 29 Apr 2016 09:46:02 -0600
Subject: [PATCH 0141/1230] non-posix NuttX feature?

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

diff --git a/src/modules/logger/logger.cpp b/src/modules/logger/logger.cpp
index dd0585887f..02aa3ae6ad 100644
--- a/src/modules/logger/logger.cpp
+++ b/src/modules/logger/logger.cpp
@@ -183,7 +183,7 @@ void Logger::run_trampoline(int argc, char *argv[])
 		return;
 	}
 
-#ifdef DBGPRINT
+#if defined(DBGPRINT) && defined(__PX4_NUTTX)
 	struct mallinfo alloc_info = mallinfo();
 	warnx("largest free chunk: %d bytes", alloc_info.mxordblk);
 	warnx("allocating %d bytes for log_buffer", log_buffer_size);

From 90ce04654d3682b671c81052ad0976e2c3c5fec1 Mon Sep 17 00:00:00 2001
From: Mark Whitehorn <kd0aij@gmail.com>
Date: Fri, 29 Apr 2016 10:35:37 -0600
Subject: [PATCH 0142/1230] fix posix build

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

diff --git a/src/modules/logger/logger.cpp b/src/modules/logger/logger.cpp
index 02aa3ae6ad..c2be5ce68f 100644
--- a/src/modules/logger/logger.cpp
+++ b/src/modules/logger/logger.cpp
@@ -196,7 +196,7 @@ void Logger::run_trampoline(int argc, char *argv[])
 
 	} else {
 
-#ifdef DBGPRINT
+#if defined(DBGPRINT) && defined(__PX4_NUTTX)
 		alloc_info = mallinfo();
 		warnx("remaining free heap: %d bytes", alloc_info.fordblks);
 #endif /* DBGPRINT */

From fb4d72df86fc40a7e6d167a749f4bef5f4600be6 Mon Sep 17 00:00:00 2001
From: Mark Whitehorn <kd0aij@gmail.com>
Date: Sat, 30 Apr 2016 09:11:21 -0600
Subject: [PATCH 0143/1230] write git and hw version records to log

---
 src/modules/logger/logger.cpp | 39 +++++++++++++++++++++++++++++++++++
 src/modules/logger/logger.h   |  6 ++++++
 2 files changed, 45 insertions(+)

diff --git a/src/modules/logger/logger.cpp b/src/modules/logger/logger.cpp
index c2be5ce68f..3c7577217d 100644
--- a/src/modules/logger/logger.cpp
+++ b/src/modules/logger/logger.cpp
@@ -669,6 +669,7 @@ void Logger::start_log()
 	}
 
 	_writer.start_log(file_name);
+	write_version();
 	write_formats();
 	write_parameters();
 	_enabled = true;
@@ -707,6 +708,44 @@ void Logger::write_formats()
 	_writer.notify();
 }
 
+/* write info message */
+void Logger::write_info(const char *name, const char *value)
+{
+	_writer.lock();
+	uint8_t buffer[sizeof(message_info_header_s)];
+	message_info_header_s *msg = reinterpret_cast<message_info_header_s *>(buffer);
+	msg->msg_type = static_cast<uint8_t>(MessageType::INFO);
+
+	/* construct format key (type and name) */
+	size_t vlen = strlen(value);
+	msg->key_len = snprintf(msg->key, sizeof(msg->key), "char[%d] %s", vlen, name);
+	size_t msg_size = sizeof(*msg) - sizeof(msg->key) + msg->key_len;
+
+	/* copy string value directly to buffer */
+	if (vlen < (sizeof(*msg) - msg_size)) {
+		memcpy(&buffer[msg_size], value, vlen);
+		msg_size += vlen;
+
+		msg->msg_size = msg_size - 2;
+
+		/* write message */
+		while (!_writer.write(buffer, msg_size)) {
+			/* wait if buffer is full, don't skip INFO messages */
+			_writer.unlock();
+			_writer.notify();
+			usleep(_log_interval);
+			_writer.lock();
+		}
+	}
+}
+
+/* write version info messages */
+void Logger::write_version()
+{
+	write_info("ver_sw", PX4_GIT_VERSION_STR);
+	write_info("ver_hw", HW_ARCH);
+}
+
 void Logger::write_parameters()
 {
 	_writer.lock();
diff --git a/src/modules/logger/logger.h b/src/modules/logger/logger.h
index f7a30c6f0f..67823309ff 100644
--- a/src/modules/logger/logger.h
+++ b/src/modules/logger/logger.h
@@ -6,6 +6,8 @@
 #include <drivers/drv_hrt.h>
 #include <uORB/Subscription.hpp>
 #include <uORB/topics/vehicle_status.h>
+#include <version/version.h>
+#include <systemlib/git_version.h>
 
 extern "C" __EXPORT int logger_main(int argc, char *argv[]);
 
@@ -68,6 +70,10 @@ private:
 
 	void write_formats();
 
+	void write_version();
+
+	void write_info(const char *name, const char *value);
+
 	void write_parameters();
 
 	bool copy_if_updated_multi(orb_id_t topic, int multi_instance, int *handle, void *buffer, uint64_t *time_last_checked);

From e5e523aa9e2d191fc8eb819af4f02ffdafb1c8dc Mon Sep 17 00:00:00 2001
From: Mark Whitehorn <kd0aij@gmail.com>
Date: Sat, 30 Apr 2016 10:16:16 -0600
Subject: [PATCH 0144/1230] size_t is different in posix build

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

diff --git a/src/modules/logger/logger.cpp b/src/modules/logger/logger.cpp
index 3c7577217d..7b664173e7 100644
--- a/src/modules/logger/logger.cpp
+++ b/src/modules/logger/logger.cpp
@@ -718,7 +718,7 @@ void Logger::write_info(const char *name, const char *value)
 
 	/* construct format key (type and name) */
 	size_t vlen = strlen(value);
-	msg->key_len = snprintf(msg->key, sizeof(msg->key), "char[%d] %s", vlen, name);
+	msg->key_len = snprintf(msg->key, sizeof(msg->key), "char[%u] %s", (unsigned)vlen, name);
 	size_t msg_size = sizeof(*msg) - sizeof(msg->key) + msg->key_len;
 
 	/* copy string value directly to buffer */

From 9dbbe8cd8d1cb061d907a9dd62ba0dd47dcc3662 Mon Sep 17 00:00:00 2001
From: Mark Whitehorn <kd0aij@gmail.com>
Date: Sat, 30 Apr 2016 17:36:38 -0600
Subject: [PATCH 0145/1230] log changes to parameters

---
 src/modules/logger/logger.cpp | 76 +++++++++++++++++++++++++++++++++++
 src/modules/logger/logger.h   |  3 ++
 2 files changed, 79 insertions(+)

diff --git a/src/modules/logger/logger.cpp b/src/modules/logger/logger.cpp
index 7b664173e7..927448b3bf 100644
--- a/src/modules/logger/logger.cpp
+++ b/src/modules/logger/logger.cpp
@@ -452,6 +452,14 @@ void Logger::run()
 
 			bool data_written = false;
 
+			/* Check if parameters have changed */
+			// this needs to change to a timestamped record to record a history of parameter changes
+			if (_parameter_update_sub.check_updated()) {
+				warnx("parameter update");
+				_parameter_update_sub.update();
+				write_changed_parameters();
+			}
+
 			// Write data messages for normal subscriptions
 			int msg_id = 0;
 
@@ -757,11 +765,13 @@ void Logger::write_parameters()
 	param_t param = 0;
 
 	do {
+		// get next parameter which is invalid OR used
 		do {
 			param = param_for_index(param_idx);
 			++param_idx;
 		} while (param != PARAM_INVALID && !param_used(param));
 
+		// save parameters which are valid AND used
 		if (param != PARAM_INVALID) {
 			/* get parameter type and size */
 			const char *type_str;
@@ -808,5 +818,71 @@ void Logger::write_parameters()
 	_writer.notify();
 }
 
+void Logger::write_changed_parameters()
+{
+	_writer.lock();
+	uint8_t buffer[sizeof(message_parameter_header_s) + sizeof(param_value_u)];
+	message_parameter_header_s *msg = reinterpret_cast<message_parameter_header_s *>(buffer);
+
+	msg->msg_type = static_cast<uint8_t>(MessageType::PARAMETER);
+	int param_idx = 0;
+	param_t param = 0;
+
+	do {
+		// get next parameter which is invalid OR used
+		do {
+			param = param_for_index(param_idx);
+			++param_idx;
+		} while (param != PARAM_INVALID && !param_used(param));
+
+		// log parameters which are valid AND used AND unsaved
+		if ((param != PARAM_INVALID) && param_value_unsaved(param)) {
+			warnx("logging change to parameter %s", param_name(param));
+
+			/* get parameter type and size */
+			const char *type_str;
+			param_type_t type = param_type(param);
+			size_t value_size = 0;
+
+			switch (type) {
+			case PARAM_TYPE_INT32:
+				type_str = "int32_t";
+				value_size = sizeof(int32_t);
+				break;
+
+			case PARAM_TYPE_FLOAT:
+				type_str = "float";
+				value_size = sizeof(float);
+				break;
+
+			default:
+				continue;
+			}
+
+			/* format parameter key (type and name) */
+			msg->key_len = snprintf(msg->key, sizeof(msg->key), "%s %s", type_str, param_name(param));
+			size_t msg_size = sizeof(*msg) - sizeof(msg->key) + msg->key_len;
+
+			/* copy parameter value directly to buffer */
+			param_get(param, &buffer[msg_size]);
+			msg_size += value_size;
+
+			msg->msg_size = msg_size - 2;
+
+			/* write message */
+			while (!_writer.write(buffer, msg_size)) {
+				/* wait if buffer is full, don't skip PARAMETER messages */
+				_writer.unlock();
+				_writer.notify();
+				usleep(_log_interval);
+				_writer.lock();
+			}
+		}
+	} while ((param != PARAM_INVALID) && (param_idx < (int) param_count()));
+
+	_writer.unlock();
+	_writer.notify();
+}
+
 }
 }
diff --git a/src/modules/logger/logger.h b/src/modules/logger/logger.h
index 67823309ff..f19de0ccb7 100644
--- a/src/modules/logger/logger.h
+++ b/src/modules/logger/logger.h
@@ -76,6 +76,8 @@ private:
 
 	void write_parameters();
 
+	void write_changed_parameters();
+
 	bool copy_if_updated_multi(orb_id_t topic, int multi_instance, int *handle, void *buffer, uint64_t *time_last_checked);
 
 	static constexpr size_t 	MAX_TOPICS_NUM = 128;
@@ -87,6 +89,7 @@ private:
 	uint8_t 					*_log_buffer;
 	char 						_log_dir[64];
 	uORB::Subscription<vehicle_status_s>	_vehicle_status_sub {ORB_ID(vehicle_status)};
+	uORB::Subscription<parameter_update_s>	_parameter_update_sub {ORB_ID(parameter_update)};
 	bool						_enabled = false;
 	bool 						_log_on_start;
 	Array<LoggerSubscription, MAX_TOPICS_NUM>	_subscriptions;

From 72263eaa97f6e93393a961a2dafa2c45bb7e91e4 Mon Sep 17 00:00:00 2001
From: Mark Whitehorn <kd0aij@gmail.com>
Date: Sat, 30 Apr 2016 22:18:19 -0600
Subject: [PATCH 0146/1230] correct msg_size offset to 3 for all records (must
 be same)

---
 src/modules/logger/logger.cpp | 16 ++++++++--------
 1 file changed, 8 insertions(+), 8 deletions(-)

diff --git a/src/modules/logger/logger.cpp b/src/modules/logger/logger.cpp
index 927448b3bf..f00c8d051f 100644
--- a/src/modules/logger/logger.cpp
+++ b/src/modules/logger/logger.cpp
@@ -492,8 +492,7 @@ void Logger::run()
 						 */
 						message_data_header_s *header = reinterpret_cast<message_data_header_s *>(buffer);
 						header->msg_type = static_cast<uint8_t>(MessageType::DATA);
-						/* the ORB topic data object has 2 unused trailing bytes? */
-						header->msg_size = static_cast<uint16_t>(msg_size - 2);
+						header->msg_size = static_cast<uint16_t>(msg_size - 3);
 						header->msg_id = msg_id;
 						header->multi_id = 0x80 + instance;	// Non multi, active
 
@@ -700,7 +699,7 @@ void Logger::write_formats()
 		msg.msg_id = msg_id;
 		msg.format_len = snprintf(msg.format, sizeof(msg.format), "%s", sub.metadata->o_fields);
 		size_t msg_size = sizeof(msg) - sizeof(msg.format) + msg.format_len;
-		msg.msg_size = msg_size - 2;
+		msg.msg_size = msg_size - 3;
 
 		while (!_writer.write(&msg, msg_size)) {
 			_writer.unlock();
@@ -734,7 +733,7 @@ void Logger::write_info(const char *name, const char *value)
 		memcpy(&buffer[msg_size], value, vlen);
 		msg_size += vlen;
 
-		msg->msg_size = msg_size - 2;
+		msg->msg_size = msg_size - 3;
 
 		/* write message */
 		while (!_writer.write(buffer, msg_size)) {
@@ -801,7 +800,7 @@ void Logger::write_parameters()
 			param_get(param, &buffer[msg_size]);
 			msg_size += value_size;
 
-			msg->msg_size = msg_size - 2;
+			msg->msg_size = msg_size - 3;
 
 			/* write message */
 			while (!_writer.write(buffer, msg_size)) {
@@ -859,15 +858,16 @@ void Logger::write_changed_parameters()
 				continue;
 			}
 
-			/* format parameter key (type and name) */
-			msg->key_len = snprintf(msg->key, sizeof(msg->key), "%s %s", type_str, param_name(param));
+			/* format parameter key (type and name and timestamp) */
+			msg->key_len = snprintf(msg->key, sizeof(msg->key), "%s %s ", type_str, param_name(param));
 			size_t msg_size = sizeof(*msg) - sizeof(msg->key) + msg->key_len;
 
 			/* copy parameter value directly to buffer */
 			param_get(param, &buffer[msg_size]);
 			msg_size += value_size;
 
-			msg->msg_size = msg_size - 2;
+			/* msg_size is now 1 (msg_type) + 2 (msg_size) + 1 (key_len) + key_len + value_size */
+			msg->msg_size = msg_size - 3;
 
 			/* write message */
 			while (!_writer.write(buffer, msg_size)) {

From 84015e5c0118df15915c2b8bac5511af2df0a7ae Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Beat=20K=C3=BCng?= <beat-kueng@gmx.net>
Date: Mon, 2 May 2016 14:44:45 +0200
Subject: [PATCH 0147/1230] logger: proper error handling if writer thread
 creation fails

---
 src/modules/logger/log_writer.cpp | 11 +++--------
 src/modules/logger/log_writer.h   |  7 ++++++-
 src/modules/logger/logger.cpp     |  9 +++++++--
 3 files changed, 16 insertions(+), 11 deletions(-)

diff --git a/src/modules/logger/log_writer.cpp b/src/modules/logger/log_writer.cpp
index 3752706182..8a75dc9102 100644
--- a/src/modules/logger/log_writer.cpp
+++ b/src/modules/logger/log_writer.cpp
@@ -48,7 +48,7 @@ void LogWriter::stop_log()
 	notify();
 }
 
-pthread_t LogWriter::thread_start()
+int LogWriter::thread_start(pthread_t &thread)
 {
 	pthread_attr_t thr_attr;
 	pthread_attr_init(&thr_attr);
@@ -60,15 +60,10 @@ pthread_t LogWriter::thread_start()
 
 	pthread_attr_setstacksize(&thr_attr, 1024);
 
-	pthread_t thr;
-
-	if (0 != pthread_create(&thr, &thr_attr, &LogWriter::run_helper, this)) {
-		PX4_WARN("error creating logwriter thread");
-	}
-
+	int ret = pthread_create(&thread, &thr_attr, &LogWriter::run_helper, this);
 	pthread_attr_destroy(&thr_attr);
 
-	return thr;
+	return ret;
 }
 
 void LogWriter::thread_stop()
diff --git a/src/modules/logger/log_writer.h b/src/modules/logger/log_writer.h
index 2c1b19f083..e49ab4efe4 100644
--- a/src/modules/logger/log_writer.h
+++ b/src/modules/logger/log_writer.h
@@ -17,7 +17,12 @@ class LogWriter
 public:
 	LogWriter(uint8_t *buffer, size_t buffer_size);
 
-	pthread_t thread_start();
+	/**
+	 * start the thread
+	 * @param thread will be set to the created thread on success
+	 * @return 0 on success, error number otherwise (@see pthread_create)
+	 */
+	int thread_start(pthread_t &thread);
 
 	void thread_stop();
 
diff --git a/src/modules/logger/logger.cpp b/src/modules/logger/logger.cpp
index f00c8d051f..60315321e5 100644
--- a/src/modules/logger/logger.cpp
+++ b/src/modules/logger/logger.cpp
@@ -402,7 +402,12 @@ void Logger::run()
 //	add_topic("estimator_status");
 	add_topic("vehicle_status", 100);
 
-	_writer_thread = _writer.thread_start();
+	int ret = _writer.thread_start(_writer_thread);
+
+	if (ret) {
+		PX4_ERR("logger: failed to create writer thread (%i)", ret);
+		return;
+	}
 
 	_task_should_exit = false;
 
@@ -584,7 +589,7 @@ void Logger::run()
 	_writer.notify();
 
 	// wait for thread to complete
-	int ret = pthread_join(_writer_thread, NULL);
+	ret = pthread_join(_writer_thread, NULL);
 
 	if (ret) {
 		PX4_WARN("join failed: %d", ret);

From e9f257c15ff227273170a2d487645a5504e39664 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Beat=20K=C3=BCng?= <beat-kueng@gmx.net>
Date: Mon, 2 May 2016 14:45:57 +0200
Subject: [PATCH 0148/1230] logger: fix wrong default buffer size in usage
 string

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

diff --git a/src/modules/logger/logger.cpp b/src/modules/logger/logger.cpp
index 60315321e5..551e7d9d33 100644
--- a/src/modules/logger/logger.cpp
+++ b/src/modules/logger/logger.cpp
@@ -84,7 +84,7 @@ void Logger::usage(const char *reason)
 
 	PX4_INFO("usage: logger {start|stop|status} [-r <log rate>] [-b <buffer size>] -e -a -t -x\n"
 		 "\t-r\tLog rate in Hz, 0 means unlimited rate\n"
-		 "\t-b\tLog buffer size in KiB, default is 8\n"
+		 "\t-b\tLog buffer size in KiB, default is 12\n"
 		 "\t-e\tEnable logging by default (if not, can be started by command)\n"
 		 "\t-a\tLog only when armed (can be still overriden by command)\n"
 		 "\t-t\tUse date/time for naming log directories and files\n"

From dd22445768bafdca2b584d754268bd79018c140e Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Beat=20K=C3=BCng?= <beat-kueng@gmx.net>
Date: Mon, 2 May 2016 14:47:58 +0200
Subject: [PATCH 0149/1230] Logger::add_topic: error handling if orb_subscribe
 or _subscriptions.push_back fail

---
 src/modules/logger/logger.cpp | 30 +++++++++++++++++++++---------
 1 file changed, 21 insertions(+), 9 deletions(-)

diff --git a/src/modules/logger/logger.cpp b/src/modules/logger/logger.cpp
index 551e7d9d33..c709145c46 100644
--- a/src/modules/logger/logger.cpp
+++ b/src/modules/logger/logger.cpp
@@ -291,18 +291,30 @@ int Logger::add_topic(const orb_metadata *topic)
 
 	if (topic->o_size > MAX_DATA_SIZE) {
 		PX4_WARN("skip topic %s, data size is too large: %zu (max is %zu)", topic->o_name, topic->o_size, MAX_DATA_SIZE);
+		return -1;
 
-	} else {
-		size_t fields_len = strlen(topic->o_fields);
+	}
 
-		if (fields_len > sizeof(message_format_s::format)) {
-			PX4_WARN("skip topic %s, format string is too large: %zu (max is %zu)", topic->o_name, fields_len,
-				 sizeof(message_format_s::format));
+	size_t fields_len = strlen(topic->o_fields);
 
-		} else {
-			fd = orb_subscribe(topic);
-			_subscriptions.push_back(LoggerSubscription(fd, topic));
-		}
+	if (fields_len > sizeof(message_format_s::format)) {
+		PX4_WARN("skip topic %s, format string is too large: %zu (max is %zu)", topic->o_name, fields_len,
+			 sizeof(message_format_s::format));
+
+		return -1;
+	}
+
+	fd = orb_subscribe(topic);
+
+	if (fd < 0) {
+		PX4_WARN("logger: orb_subscribe failed");
+		return -1;
+	}
+
+	if (!_subscriptions.push_back(LoggerSubscription(fd, topic))) {
+		PX4_WARN("logger: failed to add topic. Too many subscriptions");
+		orb_unsubscribe(fd);
+		fd = -1;
 	}
 
 	return fd;

From d0d2664efad95a7ea45a60643d8ba77f6025efe0 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Beat=20K=C3=BCng?= <beat-kueng@gmx.net>
Date: Mon, 2 May 2016 14:50:43 +0200
Subject: [PATCH 0150/1230] logger: use %zu to print type size_t, use PX4_INFO
 instead of printf

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

diff --git a/src/modules/logger/logger.cpp b/src/modules/logger/logger.cpp
index c709145c46..53b8d68c31 100644
--- a/src/modules/logger/logger.cpp
+++ b/src/modules/logger/logger.cpp
@@ -328,7 +328,7 @@ int Logger::add_topic(const char *name, unsigned interval = 0)
 	for (size_t i = 0; i < orb_topics_count(); i++) {
 		if (strcmp(name, topics[i]->o_name) == 0) {
 			fd = add_topic(topics[i]);
-			printf("logging topic: %zu, %s\n", i, topics[i]->o_name);
+			PX4_INFO("logging topic: %zu, %s\n", i, topics[i]->o_name);
 			break;
 		}
 	}
@@ -549,7 +549,7 @@ void Logger::run()
 
 							if (dropout_start == 0)	{
 								available = _writer._count;
-								PX4_WARN("dropout, available: %d/%d", available, _writer._buffer_size);
+								PX4_WARN("dropout, available: %zu/%zu", available, _writer._buffer_size);
 								dropout_start = trytime;
 								dropout_count++;
 							}
@@ -578,7 +578,7 @@ void Logger::run()
 			if (deltat > 4.0) {
 				alloc_info = mallinfo();
 				double throughput = total_bytes / deltat;
-				PX4_INFO("%8.1e Kbytes/sec, %d highWater,  %d dropouts, %5.3f sec max, free heap: %d",
+				PX4_INFO("%8.1e Kbytes/sec, %zu highWater,  %d dropouts, %5.3f sec max, free heap: %d",
 					 throughput / 1e3, highWater, dropout_count, max_drop_len, alloc_info.fordblks);
 
 				total_bytes = 0;

From 04f38b9197abd1a5a15706c68b848b419372f56a Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Beat=20K=C3=BCng?= <beat-kueng@gmx.net>
Date: Mon, 2 May 2016 14:52:16 +0200
Subject: [PATCH 0151/1230] fix Logger::add_topic: 0 is a valid file descriptor

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

diff --git a/src/modules/logger/logger.cpp b/src/modules/logger/logger.cpp
index 53b8d68c31..9d547adf52 100644
--- a/src/modules/logger/logger.cpp
+++ b/src/modules/logger/logger.cpp
@@ -333,7 +333,7 @@ int Logger::add_topic(const char *name, unsigned interval = 0)
 		}
 	}
 
-	if ((fd > 0) && (interval != 0)) {
+	if (fd >= 0 && interval != 0) {
 		orb_set_interval(fd, interval);
 	}
 

From 8b5a32564494b7838ea36a66d45625f2ba5aebd5 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Beat=20K=C3=BCng?= <beat-kueng@gmx.net>
Date: Mon, 2 May 2016 15:07:15 +0200
Subject: [PATCH 0152/1230] logger: remove _log_buffer from Logger, initialize
 it in the writer instead

it's not used in the logger, so don't store it there. It is accessed via
LogWriter::write.
This also makes sure the buffer size is >= _min_write_chunk and handles
allocation failure properly.
---
 src/modules/logger/log_writer.cpp | 26 ++++++++++++++++++++++----
 src/modules/logger/log_writer.h   | 15 ++++++++++-----
 src/modules/logger/logger.cpp     | 10 ++++++----
 src/modules/logger/logger.h       |  3 +--
 4 files changed, 39 insertions(+), 15 deletions(-)

diff --git a/src/modules/logger/log_writer.cpp b/src/modules/logger/log_writer.cpp
index 8a75dc9102..cb1c321a07 100644
--- a/src/modules/logger/log_writer.cpp
+++ b/src/modules/logger/log_writer.cpp
@@ -2,15 +2,15 @@
 #include <fcntl.h>
 #include <string.h>
 
+#include <mathlib/mathlib.h>
+
 namespace px4
 {
 namespace logger
 {
 
-LogWriter::LogWriter(uint8_t *buffer, size_t buffer_size) :
-	_buffer(buffer),
-	_buffer_size(buffer_size),
-	_min_write_chunk(4096)
+LogWriter::LogWriter(size_t buffer_size) :
+	_buffer_size(math::max(buffer_size, _min_write_chunk))
 {
 	pthread_mutex_init(&_mtx, nullptr);
 	pthread_cond_init(&_cv, nullptr);
@@ -19,6 +19,24 @@ LogWriter::LogWriter(uint8_t *buffer, size_t buffer_size) :
 	perf_fsync = perf_alloc(PC_ELAPSED, "sd fsync");
 }
 
+bool LogWriter::init()
+{
+	if (_buffer) {
+		return true;
+	}
+
+	_buffer = new uint8_t[_buffer_size];
+
+	return _buffer;
+}
+
+LogWriter::~LogWriter()
+{
+	if (_buffer) {
+		delete[] _buffer;
+	}
+}
+
 void LogWriter::start_log(const char *filename)
 {
 	::strncpy(_filename, filename, sizeof(_filename));
diff --git a/src/modules/logger/log_writer.h b/src/modules/logger/log_writer.h
index e49ab4efe4..2aef0344f2 100644
--- a/src/modules/logger/log_writer.h
+++ b/src/modules/logger/log_writer.h
@@ -15,7 +15,10 @@ class LogWriter
 {
 	friend class Logger;
 public:
-	LogWriter(uint8_t *buffer, size_t buffer_size);
+	LogWriter(size_t buffer_size);
+	~LogWriter();
+
+	bool init();
 
 	/**
 	 * start the thread
@@ -59,13 +62,15 @@ private:
 		_count -= n;
 	}
 
+	/* 512 didn't seem to work properly, 4096 should match the FAT cluster size */
+	static const size_t	_min_write_chunk = 4096;
+
 	char		_filename[64];
 	int			_fd;
-	uint8_t 	*_buffer;
+	uint8_t 	*_buffer = nullptr;
 	const size_t	_buffer_size;
-	const size_t	_min_write_chunk;	/* 512 didn't seem to work properly, 4096 should match the FAT cluster size */
-	size_t			_head = 0;
-	size_t			_count = 0;
+	size_t			_head = 0; ///< next position to write to
+	size_t			_count = 0; ///< number of bytes in _buffer to be written
 	size_t		_total_written = 0;
 	bool		_should_run = false;
 	bool		_running = false;
diff --git a/src/modules/logger/logger.cpp b/src/modules/logger/logger.cpp
index 9d547adf52..dd23e1521b 100644
--- a/src/modules/logger/logger.cpp
+++ b/src/modules/logger/logger.cpp
@@ -254,7 +254,7 @@ static constexpr size_t MAX_DATA_SIZE = 740;
 
 Logger::Logger(size_t buffer_size, unsigned log_interval, bool log_on_start) :
 	_log_on_start(log_on_start),
-	_writer((_log_buffer = new uint8_t[buffer_size]), buffer_size),
+	_writer(buffer_size),
 	_log_interval(log_interval)
 {
 }
@@ -280,9 +280,6 @@ Logger::~Logger()
 			}
 		} while (logger_task != -1);
 	}
-
-	delete [] _log_buffer;
-	logger_ptr = nullptr;
 }
 
 int Logger::add_topic(const orb_metadata *topic)
@@ -414,6 +411,11 @@ void Logger::run()
 //	add_topic("estimator_status");
 	add_topic("vehicle_status", 100);
 
+	if (!_writer.init()) {
+		PX4_ERR("logger: init of writer failed");
+		return;
+	}
+
 	int ret = _writer.thread_start(_writer_thread);
 
 	if (ret) {
diff --git a/src/modules/logger/logger.h b/src/modules/logger/logger.h
index f19de0ccb7..c25e7171e6 100644
--- a/src/modules/logger/logger.h
+++ b/src/modules/logger/logger.h
@@ -80,13 +80,12 @@ private:
 
 	bool copy_if_updated_multi(orb_id_t topic, int multi_instance, int *handle, void *buffer, uint64_t *time_last_checked);
 
-	static constexpr size_t 	MAX_TOPICS_NUM = 128;
+	static constexpr size_t 	MAX_TOPICS_NUM = 128; /**< Maximum number of logged topics */
 	static constexpr unsigned	MAX_NO_LOGFOLDER = 999;	/**< Maximum number of log dirs */
 	static constexpr unsigned	MAX_NO_LOGFILE = 999;	/**< Maximum number of log files */
 	static constexpr const char 		*LOG_ROOT = PX4_ROOTFSDIR"/fs/microsd/log";
 
 	bool						_task_should_exit = true;
-	uint8_t 					*_log_buffer;
 	char 						_log_dir[64];
 	uORB::Subscription<vehicle_status_s>	_vehicle_status_sub {ORB_ID(vehicle_status)};
 	uORB::Subscription<parameter_update_s>	_parameter_update_sub {ORB_ID(parameter_update)};

From 3dade23e39147786213a2f1681f300bae92d05fe Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Beat=20K=C3=BCng?= <beat-kueng@gmx.net>
Date: Mon, 2 May 2016 15:09:11 +0200
Subject: [PATCH 0153/1230] logger: fix _min_write_chunk comparison and set
 file descriptor after closing it

---
 src/modules/logger/log_writer.cpp | 3 ++-
 1 file changed, 2 insertions(+), 1 deletion(-)

diff --git a/src/modules/logger/log_writer.cpp b/src/modules/logger/log_writer.cpp
index cb1c321a07..eb91031155 100644
--- a/src/modules/logger/log_writer.cpp
+++ b/src/modules/logger/log_writer.cpp
@@ -134,7 +134,7 @@ void LogWriter::run()
 				available = get_read_ptr(&read_ptr, &is_part);
 
 				/* if sufficient data available or partial read or terminating, exit this wait loop */
-				if ((available > _min_write_chunk) || is_part || !_should_run) {
+				if ((available >= _min_write_chunk) || is_part || !_should_run) {
 					/* GOTO end of block */
 					break;
 				}
@@ -182,6 +182,7 @@ void LogWriter::run()
 				_count = 0;
 
 				int res = ::close(_fd);
+				_fd = -1;
 
 				if (res) {
 					PX4_WARN("error closing log file");

From 7055d3c9299496a95ac927b73ed9d7e415eda763 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Beat=20K=C3=BCng?= <beat-kueng@gmx.net>
Date: Mon, 2 May 2016 16:01:27 +0200
Subject: [PATCH 0154/1230] fix tecs.cpp: use UINT64_C for uint64_t constant
 instead of UUL

---
 src/lib/external_lgpl/tecs/tecs.cpp | 6 +++---
 src/lib/mathlib/math/Limits.hpp     | 8 ++++++++
 2 files changed, 11 insertions(+), 3 deletions(-)

diff --git a/src/lib/external_lgpl/tecs/tecs.cpp b/src/lib/external_lgpl/tecs/tecs.cpp
index 8e8e03efc0..94181dc22b 100644
--- a/src/lib/external_lgpl/tecs/tecs.cpp
+++ b/src/lib/external_lgpl/tecs/tecs.cpp
@@ -40,7 +40,7 @@ void TECS::update_state(float baro_altitude, float airspeed, const math::Matrix<
 
 	// Calculate time in seconds since last update
 	uint64_t now = ecl_absolute_time();
-	float DT = max((now - _update_50hz_last_usec), 0ULL) * 1.0e-6f;
+	float DT = max((now - _update_50hz_last_usec), UINT64_C(0)) * 1.0e-6f;
 
 	// printf("dt: %10.6f baro alt: %6.2f eas: %6.2f R(0,0): %6.2f, R(1,1): %6.2f\naccel body: %6.2f %6.2f %6.2f\naccel earth: %6.2f %6.2f %6.2f\n",
 	// 	DT, baro_altitude, airspeed, rotMat(0, 0), rotMat(1, 1), accel_body(0), accel_body(1), accel_body(2),
@@ -122,7 +122,7 @@ void TECS::_update_speed(float airspeed_demand, float indicated_airspeed,
 {
 	// Calculate time in seconds since last update
 	uint64_t now = ecl_absolute_time();
-	float DT = max((now - _update_speed_last_usec), 0ULL) * 1.0e-6f;
+	float DT = max((now - _update_speed_last_usec), UINT64_C(0)) * 1.0e-6f;
 
 	// Convert equivalent airspeeds to true airspeeds
 
@@ -550,7 +550,7 @@ void TECS::update_pitch_throttle(const math::Matrix<3,3> &rotMat, float pitch, f
 
 	// Calculate time in seconds since last update
 	uint64_t now = ecl_absolute_time();
-	_DT = max((now - _update_pitch_throttle_last_usec), 0ULL) * 1.0e-6f;
+	_DT = max((now - _update_pitch_throttle_last_usec), UINT64_C(0)) * 1.0e-6f;
 
 	// printf("tecs in: dt:%10.6f pitch: %6.2f baro_alt: %6.2f alt sp: %6.2f\neas sp: %6.2f eas: %6.2f, eas2tas: %6.2f\n %s pitch min C0: %6.2f thr min: %6.2f, thr max: %6.2f thr cruis: %6.2f pt min: %6.2f, pt max: %6.2f\n",
 	// 	_DT, pitch, baro_altitude, hgt_dem, EAS_dem, indicated_airspeed, EAS2TAS, (climbOutDem) ? "climb" : "level", ptchMinCO, throttle_min, throttle_max, throttle_cruise, pitch_limit_min, pitch_limit_max);
diff --git a/src/lib/mathlib/math/Limits.hpp b/src/lib/mathlib/math/Limits.hpp
index f04d725078..7560406e05 100644
--- a/src/lib/mathlib/math/Limits.hpp
+++ b/src/lib/mathlib/math/Limits.hpp
@@ -42,6 +42,14 @@
 #include <platforms/px4_defines.h>
 #include <stdint.h>
 
+//this should be defined in stdint.h, but seems to be missing in the ARM toolchain (5.2.0)
+#ifndef UINT64_C
+# if __WORDSIZE == 64
+#  define UINT64_C(c)	c ## UL
+# else
+#  define UINT64_C(c)	c ## ULL
+# endif
+#endif
 namespace math
 {
 

From dae12f12386b47b60e53865665e979021390e517 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Beat=20K=C3=BCng?= <beat-kueng@gmx.net>
Date: Mon, 2 May 2016 16:02:41 +0200
Subject: [PATCH 0155/1230] mathlib: replace math::{min,max,constrain} with
 template methods

---
 src/lib/mathlib/math/Limits.cpp | 76 ---------------------------------
 src/lib/mathlib/math/Limits.hpp | 46 ++++++++------------
 2 files changed, 17 insertions(+), 105 deletions(-)

diff --git a/src/lib/mathlib/math/Limits.cpp b/src/lib/mathlib/math/Limits.cpp
index 4b1273c7a1..114929ba17 100644
--- a/src/lib/mathlib/math/Limits.cpp
+++ b/src/lib/mathlib/math/Limits.cpp
@@ -51,82 +51,6 @@ namespace math
 #define M_PI_F 3.14159265358979323846f
 #endif
 
-float __EXPORT min(float val1, float val2)
-{
-	return (val1 < val2) ? val1 : val2;
-}
-
-int __EXPORT min(int val1, int val2)
-{
-	return (val1 < val2) ? val1 : val2;
-}
-
-unsigned __EXPORT min(unsigned val1, unsigned val2)
-{
-	return (val1 < val2) ? val1 : val2;
-}
-
-uint64_t __EXPORT min(uint64_t val1, uint64_t val2)
-{
-	return (val1 < val2) ? val1 : val2;
-}
-
-double __EXPORT min(double val1, double val2)
-{
-	return (val1 < val2) ? val1 : val2;
-}
-
-float __EXPORT max(float val1, float val2)
-{
-	return (val1 > val2) ? val1 : val2;
-}
-
-int __EXPORT max(int val1, int val2)
-{
-	return (val1 > val2) ? val1 : val2;
-}
-
-unsigned __EXPORT max(unsigned val1, unsigned val2)
-{
-	return (val1 > val2) ? val1 : val2;
-}
-
-uint64_t __EXPORT max(uint64_t val1, uint64_t val2)
-{
-	return (val1 > val2) ? val1 : val2;
-}
-
-double __EXPORT max(double val1, double val2)
-{
-	return (val1 > val2) ? val1 : val2;
-}
-
-
-float __EXPORT constrain(float val, float min, float max)
-{
-	return (val < min) ? min : ((val > max) ? max : val);
-}
-
-int __EXPORT constrain(int val, int min, int max)
-{
-	return (val < min) ? min : ((val > max) ? max : val);
-}
-
-unsigned __EXPORT constrain(unsigned val, unsigned min, unsigned max)
-{
-	return (val < min) ? min : ((val > max) ? max : val);
-}
-
-uint64_t __EXPORT constrain(uint64_t val, uint64_t min, uint64_t max)
-{
-	return (val < min) ? min : ((val > max) ? max : val);
-}
-
-double __EXPORT constrain(double val, double min, double max)
-{
-	return (val < min) ? min : ((val > max) ? max : val);
-}
-
 float __EXPORT radians(float degrees)
 {
 	return (degrees / 180.0f) * M_PI_F;
diff --git a/src/lib/mathlib/math/Limits.hpp b/src/lib/mathlib/math/Limits.hpp
index 7560406e05..6215b8f955 100644
--- a/src/lib/mathlib/math/Limits.hpp
+++ b/src/lib/mathlib/math/Limits.hpp
@@ -50,40 +50,28 @@
 #  define UINT64_C(c)	c ## ULL
 # endif
 #endif
+
+
 namespace math
 {
 
+template<typename _Tp>
+inline const _Tp &min(const _Tp &a, const _Tp &b)
+{
+	return (a < b) ? a : b;
+}
 
-float __EXPORT min(float val1, float val2);
+template<typename _Tp>
+inline const _Tp &max(const _Tp &a, const _Tp &b)
+{
+	return (a > b) ? a : b;
+}
 
-int __EXPORT min(int val1, int val2);
-
-unsigned __EXPORT min(unsigned val1, unsigned val2);
-
-uint64_t __EXPORT min(uint64_t val1, uint64_t val2);
-
-double __EXPORT min(double val1, double val2);
-
-float __EXPORT max(float val1, float val2);
-
-int __EXPORT max(int val1, int val2);
-
-unsigned __EXPORT max(unsigned val1, unsigned val2);
-
-uint64_t __EXPORT max(uint64_t val1, uint64_t val2);
-
-double __EXPORT max(double val1, double val2);
-
-
-float __EXPORT constrain(float val, float min, float max);
-
-int __EXPORT constrain(int val, int min, int max);
-
-unsigned __EXPORT constrain(unsigned val, unsigned min, unsigned max);
-
-uint64_t __EXPORT constrain(uint64_t val, uint64_t min, uint64_t max);
-
-double __EXPORT constrain(double val, double min, double max);
+template<typename _Tp>
+inline const _Tp &constrain(const _Tp &val, const _Tp &min_val, const _Tp &max_val)
+{
+	return (val < min_val) ? min_val : ((val > max_val) ? max_val : val);
+}
 
 float __EXPORT radians(float degrees);
 

From f40afac448ab18471e6cf8a6f0ebf8431e1da77e Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Beat=20K=C3=BCng?= <beat-kueng@gmx.net>
Date: Mon, 2 May 2016 16:39:07 +0200
Subject: [PATCH 0156/1230] logger: fix 'Undefined symbols for architecture
 x86_64' on clang for _min_write_chunk

---
 src/modules/logger/log_writer.cpp | 2 ++
 src/modules/logger/log_writer.h   | 2 +-
 2 files changed, 3 insertions(+), 1 deletion(-)

diff --git a/src/modules/logger/log_writer.cpp b/src/modules/logger/log_writer.cpp
index eb91031155..f7763a8dfc 100644
--- a/src/modules/logger/log_writer.cpp
+++ b/src/modules/logger/log_writer.cpp
@@ -8,6 +8,8 @@ namespace px4
 {
 namespace logger
 {
+constexpr size_t LogWriter::_min_write_chunk;
+
 
 LogWriter::LogWriter(size_t buffer_size) :
 	_buffer_size(math::max(buffer_size, _min_write_chunk))
diff --git a/src/modules/logger/log_writer.h b/src/modules/logger/log_writer.h
index 2aef0344f2..30d4818a69 100644
--- a/src/modules/logger/log_writer.h
+++ b/src/modules/logger/log_writer.h
@@ -63,7 +63,7 @@ private:
 	}
 
 	/* 512 didn't seem to work properly, 4096 should match the FAT cluster size */
-	static const size_t	_min_write_chunk = 4096;
+	static constexpr size_t	_min_write_chunk = 4096;
 
 	char		_filename[64];
 	int			_fd;

From 26596dbe157bf71e380b3c57f5371c5483902a2f Mon Sep 17 00:00:00 2001
From: Mark Whitehorn <kd0aij@gmail.com>
Date: Mon, 2 May 2016 09:28:15 -0600
Subject: [PATCH 0157/1230] fix infinite loop when not logging

---
 src/modules/logger/log_writer.cpp | 26 ++++++++++++--------------
 1 file changed, 12 insertions(+), 14 deletions(-)

diff --git a/src/modules/logger/log_writer.cpp b/src/modules/logger/log_writer.cpp
index f7763a8dfc..5c96a9518a 100644
--- a/src/modules/logger/log_writer.cpp
+++ b/src/modules/logger/log_writer.cpp
@@ -103,21 +103,20 @@ void *LogWriter::run_helper(void *context)
 
 void LogWriter::run()
 {
-	// Wait for _should_run flag
-	while (!_exit_thread) {
-		bool start = false;
-		pthread_mutex_lock(&_mtx);
-		pthread_cond_wait(&_cv, &_mtx);
-		start = _should_run;
-		pthread_mutex_unlock(&_mtx);
-
-		if (start) {
-			break;
-		}
-	}
-
 	while (!_exit_thread) {
 		// Outer endless loop
+		// Wait for _should_run flag
+		while (!_exit_thread) {
+			bool start = false;
+			pthread_mutex_lock(&_mtx);
+			pthread_cond_wait(&_cv, &_mtx);
+			start = _should_run;
+			pthread_mutex_unlock(&_mtx);
+
+			if (start) {
+				break;
+			}
+		}
 
 		int poll_count = 0;
 		int written = 0;
@@ -196,7 +195,6 @@ void LogWriter::run()
 				break;
 			}
 		}
-
 	}
 }
 

From 4e50f271d6f925da3a95defa53bb3523c3a14e8d Mon Sep 17 00:00:00 2001
From: Mark Whitehorn <kd0aij@gmail.com>
Date: Mon, 2 May 2016 09:29:20 -0600
Subject: [PATCH 0158/1230] use C99 print format for size_t

---
 src/modules/logger/logger.cpp | 24 ++++++++++++------------
 1 file changed, 12 insertions(+), 12 deletions(-)

diff --git a/src/modules/logger/logger.cpp b/src/modules/logger/logger.cpp
index dd23e1521b..a66fe47daa 100644
--- a/src/modules/logger/logger.cpp
+++ b/src/modules/logger/logger.cpp
@@ -389,27 +389,27 @@ void Logger::run()
 		return;
 	}
 
-	add_topic("sensor_gyro", 10);
-	add_topic("sensor_accel", 10);
+	add_topic("sensor_gyro", 0);
+	add_topic("sensor_accel", 0);
 	add_topic("vehicle_rates_setpoint", 10);
 	add_topic("vehicle_attitude_setpoint", 10);
-	add_topic("vehicle_attitude", 10);
+	add_topic("vehicle_attitude", 0);
 	add_topic("actuator_outputs", 50);
 	add_topic("battery_status", 100);
 	add_topic("vehicle_command", 100);
 	add_topic("actuator_controls", 10);
-	add_topic("vehicle_local_position_setpoint", 30);
-	add_topic("rc_channels", 100);
+	add_topic("vehicle_local_position_setpoint", 200);
+	add_topic("rc_channels", 20);
 //	add_topic("ekf2_innovations", 20);
 	add_topic("commander_state", 100);
-	add_topic("vehicle_local_position", 10);
-	add_topic("vehicle_global_position", 10);
+	add_topic("vehicle_local_position", 200);
+	add_topic("vehicle_global_position", 200);
 	add_topic("system_power", 100);
-	add_topic("servorail_status", 100);
+	add_topic("servorail_status", 200);
 	add_topic("mc_att_ctrl_status", 50);
-	add_topic("control_state");
+//	add_topic("control_state");
 //	add_topic("estimator_status");
-	add_topic("vehicle_status", 100);
+	add_topic("vehicle_status", 200);
 
 	if (!_writer.init()) {
 		PX4_ERR("logger: init of writer failed");
@@ -744,7 +744,7 @@ void Logger::write_info(const char *name, const char *value)
 
 	/* construct format key (type and name) */
 	size_t vlen = strlen(value);
-	msg->key_len = snprintf(msg->key, sizeof(msg->key), "char[%u] %s", (unsigned)vlen, name);
+	msg->key_len = snprintf(msg->key, sizeof(msg->key), "char[%zu] %s", vlen, name);
 	size_t msg_size = sizeof(*msg) - sizeof(msg->key) + msg->key_len;
 
 	/* copy string value directly to buffer */
@@ -877,7 +877,7 @@ void Logger::write_changed_parameters()
 				continue;
 			}
 
-			/* format parameter key (type and name and timestamp) */
+			/* format parameter key (type and name) */
 			msg->key_len = snprintf(msg->key, sizeof(msg->key), "%s %s ", type_str, param_name(param));
 			size_t msg_size = sizeof(*msg) - sizeof(msg->key) + msg->key_len;
 

From c7e7026f4713993d9c2eacefdf16b7253aea6d96 Mon Sep 17 00:00:00 2001
From: Mark Whitehorn <kd0aij@gmail.com>
Date: Mon, 2 May 2016 09:59:12 -0600
Subject: [PATCH 0159/1230] remove code obsoleted by move of log buffer

---
 src/modules/logger/logger.cpp | 14 ++++----------
 1 file changed, 4 insertions(+), 10 deletions(-)

diff --git a/src/modules/logger/logger.cpp b/src/modules/logger/logger.cpp
index a66fe47daa..e6f0748acf 100644
--- a/src/modules/logger/logger.cpp
+++ b/src/modules/logger/logger.cpp
@@ -191,18 +191,12 @@ void Logger::run_trampoline(int argc, char *argv[])
 
 	logger_ptr = new Logger(log_buffer_size, log_interval, log_on_start);
 
-	if (logger_ptr->_log_buffer == nullptr) {
-		PX4_WARN("log buffer malloc failed");
-
-	} else {
-
 #if defined(DBGPRINT) && defined(__PX4_NUTTX)
-		alloc_info = mallinfo();
-		warnx("remaining free heap: %d bytes", alloc_info.fordblks);
+	alloc_info = mallinfo();
+	warnx("remaining free heap: %d bytes", alloc_info.fordblks);
 #endif /* DBGPRINT */
 
-		logger_ptr->run();
-	}
+	logger_ptr->run();
 }
 
 enum class MessageType : uint8_t {
@@ -325,7 +319,7 @@ int Logger::add_topic(const char *name, unsigned interval = 0)
 	for (size_t i = 0; i < orb_topics_count(); i++) {
 		if (strcmp(name, topics[i]->o_name) == 0) {
 			fd = add_topic(topics[i]);
-			PX4_INFO("logging topic: %zu, %s\n", i, topics[i]->o_name);
+			PX4_INFO("logging topic: %zu, %s", i, topics[i]->o_name);
 			break;
 		}
 	}

From 768e5ab66f058e33eb79459264ea710eaae8c278 Mon Sep 17 00:00:00 2001
From: Daniel Agar <daniel@agar.ca>
Date: Tue, 3 May 2016 17:10:41 -0400
Subject: [PATCH 0160/1230] adc_report remove timestamp

---
 msg/adc_report.msg | 1 -
 1 file changed, 1 deletion(-)

diff --git a/msg/adc_report.msg b/msg/adc_report.msg
index bfa093a202..a93ff2ac76 100644
--- a/msg/adc_report.msg
+++ b/msg/adc_report.msg
@@ -1,3 +1,2 @@
-uint64 timestamp		# Timestamp in microseconds since boot
 int16[8] channel_id		# ADC channel IDs, negative for non-existent
 float32[8] channel_value	# ADC channel value in volt, valid if channel ID is positive

From 63bd2cebf95223311ab8aa0b0b15e476853b0834 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Beat=20K=C3=BCng?= <beat-kueng@gmx.net>
Date: Wed, 4 May 2016 11:22:05 +0200
Subject: [PATCH 0161/1230] refactor logger: add a write_wait method to avoid
 code duplication

---
 src/modules/logger/log_writer.h |  4 +++
 src/modules/logger/logger.cpp   | 46 ++++++++++++---------------------
 src/modules/logger/logger.h     |  6 +++++
 3 files changed, 26 insertions(+), 30 deletions(-)

diff --git a/src/modules/logger/log_writer.h b/src/modules/logger/log_writer.h
index 30d4818a69..45106b9ddf 100644
--- a/src/modules/logger/log_writer.h
+++ b/src/modules/logger/log_writer.h
@@ -33,6 +33,10 @@ public:
 
 	void stop_log();
 
+	/**
+	 * Write data to be logged. The caller must call lock() before calling this.
+	 * @return true on success, false if not enough space in the buffer left
+	 */
 	bool write(void *ptr, size_t size);
 
 	void lock()
diff --git a/src/modules/logger/logger.cpp b/src/modules/logger/logger.cpp
index e6f0748acf..5bb3f89732 100644
--- a/src/modules/logger/logger.cpp
+++ b/src/modules/logger/logger.cpp
@@ -701,6 +701,18 @@ void Logger::stop_log()
 	_writer.stop_log();
 }
 
+bool Logger::write_wait(void *ptr, size_t size)
+{
+	while (!_writer.write(ptr, size)) {
+		_writer.unlock();
+		_writer.notify();
+		usleep(_log_interval);
+		_writer.lock();
+	}
+
+	return true;
+}
+
 void Logger::write_formats()
 {
 	_writer.lock();
@@ -714,12 +726,7 @@ void Logger::write_formats()
 		size_t msg_size = sizeof(msg) - sizeof(msg.format) + msg.format_len;
 		msg.msg_size = msg_size - 3;
 
-		while (!_writer.write(&msg, msg_size)) {
-			_writer.unlock();
-			_writer.notify();
-			usleep(_log_interval);	// Wait if buffer is full, don't skip FORMAT messages
-			_writer.lock();
-		}
+		write_wait(&msg, msg_size);
 
 		msg_id++;
 	}
@@ -748,14 +755,7 @@ void Logger::write_info(const char *name, const char *value)
 
 		msg->msg_size = msg_size - 3;
 
-		/* write message */
-		while (!_writer.write(buffer, msg_size)) {
-			/* wait if buffer is full, don't skip INFO messages */
-			_writer.unlock();
-			_writer.notify();
-			usleep(_log_interval);
-			_writer.lock();
-		}
+		write_wait(buffer, msg_size);
 	}
 }
 
@@ -815,14 +815,7 @@ void Logger::write_parameters()
 
 			msg->msg_size = msg_size - 3;
 
-			/* write message */
-			while (!_writer.write(buffer, msg_size)) {
-				/* wait if buffer is full, don't skip PARAMETER messages */
-				_writer.unlock();
-				_writer.notify();
-				usleep(_log_interval);
-				_writer.lock();
-			}
+			write_wait(buffer, msg_size);
 		}
 	} while ((param != PARAM_INVALID) && (param_idx < (int) param_count()));
 
@@ -882,14 +875,7 @@ void Logger::write_changed_parameters()
 			/* msg_size is now 1 (msg_type) + 2 (msg_size) + 1 (key_len) + key_len + value_size */
 			msg->msg_size = msg_size - 3;
 
-			/* write message */
-			while (!_writer.write(buffer, msg_size)) {
-				/* wait if buffer is full, don't skip PARAMETER messages */
-				_writer.unlock();
-				_writer.notify();
-				usleep(_log_interval);
-				_writer.lock();
-			}
+			write_wait(buffer, msg_size);
 		}
 	} while ((param != PARAM_INVALID) && (param_idx < (int) param_count()));
 
diff --git a/src/modules/logger/logger.h b/src/modules/logger/logger.h
index c25e7171e6..4a0dc59d04 100644
--- a/src/modules/logger/logger.h
+++ b/src/modules/logger/logger.h
@@ -80,6 +80,12 @@ private:
 
 	bool copy_if_updated_multi(orb_id_t topic, int multi_instance, int *handle, void *buffer, uint64_t *time_last_checked);
 
+	/**
+	 * Write data to the logger. Waits if buffer is full until all data is written.
+	 * Must be called with _writer.lock() held.
+	 */
+	bool write_wait(void *ptr, size_t size);
+
 	static constexpr size_t 	MAX_TOPICS_NUM = 128; /**< Maximum number of logged topics */
 	static constexpr unsigned	MAX_NO_LOGFOLDER = 999;	/**< Maximum number of log dirs */
 	static constexpr unsigned	MAX_NO_LOGFILE = 999;	/**< Maximum number of log files */

From 4edc0d9ea962824c8562b107a9b392237f2b3873 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Beat=20K=C3=BCng?= <beat-kueng@gmx.net>
Date: Wed, 4 May 2016 11:23:51 +0200
Subject: [PATCH 0162/1230] fix logger: add forgotten unlock in
 Logger::write_info

---
 src/modules/logger/logger.cpp | 3 ++-
 1 file changed, 2 insertions(+), 1 deletion(-)

diff --git a/src/modules/logger/logger.cpp b/src/modules/logger/logger.cpp
index 5bb3f89732..f2a2134e1a 100644
--- a/src/modules/logger/logger.cpp
+++ b/src/modules/logger/logger.cpp
@@ -692,6 +692,7 @@ void Logger::start_log()
 	write_version();
 	write_formats();
 	write_parameters();
+	_writer.notify();
 	_enabled = true;
 }
 
@@ -732,7 +733,6 @@ void Logger::write_formats()
 	}
 
 	_writer.unlock();
-	_writer.notify();
 }
 
 /* write info message */
@@ -757,6 +757,7 @@ void Logger::write_info(const char *name, const char *value)
 
 		write_wait(buffer, msg_size);
 	}
+	_writer.unlock();
 }
 
 /* write version info messages */

From 8f5cb4084d2c276ad165a274b63ce78e84a86a6a Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Beat=20K=C3=BCng?= <beat-kueng@gmx.net>
Date: Wed, 4 May 2016 12:26:08 +0200
Subject: [PATCH 0163/1230] logger: use non-scientific format for status output

---
 src/modules/logger/logger.cpp | 5 +++--
 1 file changed, 3 insertions(+), 2 deletions(-)

diff --git a/src/modules/logger/logger.cpp b/src/modules/logger/logger.cpp
index f2a2134e1a..a2345a86f6 100644
--- a/src/modules/logger/logger.cpp
+++ b/src/modules/logger/logger.cpp
@@ -574,8 +574,8 @@ void Logger::run()
 			if (deltat > 4.0) {
 				alloc_info = mallinfo();
 				double throughput = total_bytes / deltat;
-				PX4_INFO("%8.1e Kbytes/sec, %zu highWater,  %d dropouts, %5.3f sec max, free heap: %d",
-					 throughput / 1e3, highWater, dropout_count, max_drop_len, alloc_info.fordblks);
+				PX4_INFO("%8.1lf KB/s, %zu highWater,  %d dropouts, %5.3f sec max, free heap: %d",
+					 throughput / 1.e3, highWater, dropout_count, max_drop_len, alloc_info.fordblks);
 
 				total_bytes = 0;
 				highWater = 0,
@@ -757,6 +757,7 @@ void Logger::write_info(const char *name, const char *value)
 
 		write_wait(buffer, msg_size);
 	}
+
 	_writer.unlock();
 }
 

From 69c1ce1714b902a6d67c10718919dfaf49b26796 Mon Sep 17 00:00:00 2001
From: Daniel Agar <daniel@agar.ca>
Date: Tue, 3 May 2016 16:50:12 -0400
Subject: [PATCH 0164/1230] WIP logger serialization

---
 cmake/configs/nuttx_px4fmu-v2_debug.cmake     | 189 ------------------
 cmake/configs/nuttx_px4fmu-v2_default.cmake   |   2 +-
 msg/templates/uorb/msg.cpp.template           |  70 +++++--
 msg/templates/uorb/msg.h.template             |  59 ++----
 src/drivers/device/integrator.cpp             |   8 +-
 src/drivers/device/integrator.h               |   6 +-
 src/drivers/l3gd20/l3gd20.cpp                 |   2 +-
 src/drivers/lsm303d/lsm303d.cpp               |   2 +-
 src/drivers/mpu6000/mpu6000.cpp               |   4 +-
 src/drivers/mpu6500/mpu6500.cpp               |   4 +-
 src/drivers/mpu9250/mpu9250.cpp               |   4 +-
 src/drivers/px4flow/px4flow.cpp               |   4 +-
 src/lib/conversion/rotation.cpp               |  10 -
 src/lib/conversion/rotation.h                 |   5 -
 src/modules/commander/commander.cpp           |   3 +-
 src/modules/logger/logger.cpp                 |  43 +++-
 src/modules/logger/logger.h                   |   4 +-
 src/modules/mavlink/mavlink_receiver.cpp      |   4 +-
 src/modules/uORB/uORB.h                       |  21 +-
 .../uORB/uORB_tests/uORBTest_UnitTest.hpp     |  10 +-
 .../posix/drivers/gyrosim/gyrosim.cpp         |   4 +-
 .../uORBCommunicator_gtests.cpp               |   2 +-
 unittests/uorb_unittests/uORBGtestTopics.hpp  |   8 +-
 23 files changed, 154 insertions(+), 314 deletions(-)
 delete mode 100644 cmake/configs/nuttx_px4fmu-v2_debug.cmake

diff --git a/cmake/configs/nuttx_px4fmu-v2_debug.cmake b/cmake/configs/nuttx_px4fmu-v2_debug.cmake
deleted file mode 100644
index e0fedbb7e6..0000000000
--- a/cmake/configs/nuttx_px4fmu-v2_debug.cmake
+++ /dev/null
@@ -1,189 +0,0 @@
-set(CMAKE_BUILD_TYPE "Debug" CACHE STRING "build type")
-
-
-include(nuttx/px4_impl_nuttx)
-
-set(CMAKE_TOOLCHAIN_FILE ${CMAKE_SOURCE_DIR}/cmake/toolchains/Toolchain-arm-none-eabi.cmake)
-
-set(config_module_list
-	#
-	# Board support modules
-	#
-	drivers/device
-	drivers/stm32
-	drivers/stm32/adc
-	drivers/stm32/tone_alarm
-	drivers/led
-	drivers/px4fmu
-	drivers/px4io
-	drivers/boards/px4fmu-v2
-	drivers/rgbled
-	drivers/mpu6000
-	#drivers/mpu9250
-	drivers/lsm303d
-	drivers/l3gd20
-	#drivers/hmc5883
-	drivers/ms5611
-	#drivers/mb12xx
-	#drivers/srf02
-	#drivers/sf0x
-	#drivers/ll40ls
-	drivers/trone
-	drivers/gps
-	drivers/pwm_out_sim
-	#drivers/hott
-	#drivers/hott/hott_telemetry
-	#drivers/hott/hott_sensors
-	#drivers/blinkm
-	drivers/airspeed
-	drivers/ets_airspeed
-	drivers/meas_airspeed
-	#drivers/frsky_telemetry
-	modules/sensors
-	#drivers/mkblctrl
-	#drivers/px4flow
-	#drivers/oreoled
-	drivers/gimbal
-	drivers/pwm_input
-	drivers/camera_trigger
-	drivers/bst
-	#drivers/snapdragon_rc_pwm
-	#drivers/lis3mdl
-
-	#
-	# System commands
-	#
-	systemcmds/bl_update
-	systemcmds/mixer
-	systemcmds/param
-	systemcmds/perf
-	systemcmds/pwm
-	systemcmds/esc_calib
-	systemcmds/reboot
-	#systemcmds/topic_listener
-	systemcmds/top
-	systemcmds/config
-	systemcmds/nshterm
-	systemcmds/mtd
-	systemcmds/dumpfile
-	systemcmds/ver
-
-	#
-	# General system control
-	#
-	modules/commander
-	modules/navigator
-	modules/mavlink
-	modules/gpio_led
-	#modules/uavcan
-	modules/land_detector
-
-	#
-	# Estimation modules (EKF/ SO3 / other filters)
-	#
-	# Too high RAM usage due to static allocations
-	# modules/attitude_estimator_ekf
-	modules/attitude_estimator_q
-	modules/ekf_att_pos_estimator
-	modules/position_estimator_inav
-
-	#
-	# Vehicle Control
-	#
-	# modules/segway # XXX Needs GCC 4.7 fix
-	modules/fw_pos_control_l1
-	modules/fw_att_control
-	modules/mc_att_control
-	modules/mc_pos_control
-	modules/vtol_att_control
-
-	#
-	# Logging
-	#
-	modules/sdlog2
-
-	#
-	# Library modules
-	#
-	modules/param
-	modules/systemlib
-	modules/systemlib/mixer
-	modules/uORB
-	modules/dataman
-
-	#
-	# Libraries
-	#
-	lib/controllib
-	lib/mathlib
-	lib/mathlib/math/filter
-	lib/ecl
-	lib/external_lgpl
-	lib/geo
-	lib/geo_lookup
-	lib/conversion
-	lib/launchdetection
-	lib/terrain_estimation
-	lib/runway_takeoff
-	lib/tailsitter_recovery
-	platforms/nuttx
-
-	# had to add for cmake, not sure why wasn't in original config
-	platforms/common 
-	platforms/nuttx/px4_layer
-
-	#
-	# OBC challenge
-	#
-	#modules/bottle_drop
-
-	#
-	# Rover apps
-	#
-	#examples/rover_steering_control
-
-	#
-	# Demo apps
-	#
-	#examples/math_demo
-	# Tutorial code from
-	# https://px4.io/dev/px4_simple_app
-	#examples/px4_simple_app
-
-	# Tutorial code from
-	# https://px4.io/dev/daemon
-	#examples/px4_daemon_app
-
-	# Tutorial code from
-	# https://px4.io/dev/debug_values
-	#examples/px4_mavlink_debug
-
-	# Tutorial code from
-	# https://px4.io/dev/example_fixedwing_control
-	#examples/fixedwing_control
-
-	# Hardware test
-	#examples/hwtest
-)
-
-set(config_extra_builtin_cmds
-	serdis
-	sercon
-	)
-
-set(config_io_board
-	px4io-v2
-	)
-
-set(config_io_extra_libs
-	)
-
-add_custom_target(sercon)
-set_target_properties(sercon PROPERTIES
-	PRIORITY "SCHED_PRIORITY_DEFAULT"
-	MAIN "sercon" STACK_MAIN "2048")
-
-add_custom_target(serdis)
-set_target_properties(serdis PROPERTIES
-	PRIORITY "SCHED_PRIORITY_DEFAULT"
-	MAIN "serdis" STACK_MAIN "2048")
diff --git a/cmake/configs/nuttx_px4fmu-v2_default.cmake b/cmake/configs/nuttx_px4fmu-v2_default.cmake
index ee1263bad6..71cdc9463a 100644
--- a/cmake/configs/nuttx_px4fmu-v2_default.cmake
+++ b/cmake/configs/nuttx_px4fmu-v2_default.cmake
@@ -85,7 +85,7 @@ set(config_module_list
 	modules/attitude_estimator_q
 	modules/ekf_att_pos_estimator
 	modules/position_estimator_inav
-	modules/local_position_estimator
+	#modules/local_position_estimator
 
 	#
 	# Vehicle Control
diff --git a/msg/templates/uorb/msg.cpp.template b/msg/templates/uorb/msg.cpp.template
index 54313dc5ec..589d05dca4 100644
--- a/msg/templates/uorb/msg.cpp.template
+++ b/msg/templates/uorb/msg.cpp.template
@@ -57,7 +57,8 @@ uorb_struct = '%s_s'%spec.short_name
 uorb_pack_func = 'pack_%s'%spec.short_name
 topic_name = spec.short_name
 
-type_map = {'int8': 'int8_t',
+type_map = {
+  'int8': 'int8_t',
   'int16': 'int16_t',
   'int32': 'int32_t',
   'int64': 'int64_t',
@@ -71,23 +72,28 @@ type_map = {'int8': 'int8_t',
   'char': 'char',
   'fence_vertex': 'fence_vertex',
   'position_setpoint': 'position_setpoint',
-  'esc_report': 'esc_report'}
+  'esc_report': 'esc_report'
+}
 
-msgtype_size_map = {'int8': 8,
-  'int16': 16,
-  'int32': 32,
-  'int64': 64,
-  'uint8': 8,
-  'uint16': 16,
-  'uint32': 32,
-  'uint64': 64,
-  'float32': 32,
-  'float64': 64,
+msgtype_size_map = {
+  'int8': 1,
+  'int16': 2,
+  'int32': 4,
+  'int64': 8,
+  'uint8': 1,
+  'uint16': 2,
+  'uint32': 4,
+  'uint64': 8,
+  'float32': 4,
+  'float64': 8,
   'bool': 1,
   'char': 1,
   'fence_vertex': 8,
   'position_setpoint': 104,
-  'esc_report': 36}
+  'esc_report': 36
+}
+
+msgtypes_composite = ['position_setpoint', 'esc_report', 'fence_vertex']
 
 def convert_type(spec_type):
     bare_type = spec_type
@@ -112,7 +118,7 @@ def bare_name(msg_type):
 def sizeof_field_type(field):
     return msgtype_size_map[bare_name(field.type)]
 
-sorted_fields = sorted(spec.parsed_fields(), key = sizeof_field_type, reverse=True)
+sorted_fields = sorted(spec.parsed_fields(), key=sizeof_field_type, reverse=True)
 topic_fields = ["%s %s" % (convert_type(field.type), field.name) for field in sorted_fields]
 }@
 
@@ -120,9 +126,43 @@ topic_fields = ["%s %s" % (convert_type(field.type), field.name) for field in so
 #include <drivers/drv_orb_dev.h>
 #include <uORB/topics/@(topic_name).h>
 
+
+void serialize_@(topic_name)(void *in, struct orb_output_buffer *buffer)
+{
+	struct @(uorb_struct) *topic = static_cast<struct @(uorb_struct) *>(in);
+		
+@{
+
+serialize_expr = "	serialize_{type}(&topic->{field}, buffer);"
+serialize_array_expr = "	serialize_{type}(&topic->{field}[{index}], buffer);"
+
+buffer_memcpy_expr = "	memcpy(((char *)buffer->data) + buffer->next, &topic->{field}, sizeof(topic->{field}));"
+buffer_next_expr = "	buffer->next += sizeof(topic->{field});"
+
+for each_field in sorted_fields:
+  if not each_field.is_header:
+  
+    # call appropriate serialize functions for fields that are orb messages
+    if bare_name(each_field.type) in msgtypes_composite:
+      
+      if each_field.is_array:
+        for i in range(each_field.array_len):
+          print(serialize_array_expr.format(type=type_map[bare_name(each_field.type)], field=each_field.name, index=i))
+
+      else:
+    	print(serialize_expr.format(type=type_map[bare_name(each_field.type)], field=each_field.name))
+
+	# copy primitive fields
+    else:
+    	print(buffer_memcpy_expr.format(field=each_field.name))
+    	print(buffer_next_expr.format(field=each_field.name))
+
+}@
+}
+
 @# join all msg files in one line e.g: "float[3] position;float[3] velocity;bool armed"
 const char *__orb_@(topic_name)_fields = "@( topic_name.upper() ):@( ";".join(topic_fields) );";
 
 @[for multi_topic in topics]@
-ORB_DEFINE(@multi_topic, struct @uorb_struct, __orb_@(topic_name)_fields);
+ORB_DEFINE(@multi_topic, struct @uorb_struct, &serialize_@(topic_name), __orb_@(topic_name)_fields);
 @[end for]
diff --git a/msg/templates/uorb/msg.h.template b/msg/templates/uorb/msg.h.template
index 8ad98c61d5..9196edbc4d 100644
--- a/msg/templates/uorb/msg.h.template
+++ b/msg/templates/uorb/msg.h.template
@@ -68,6 +68,7 @@ topic_name = spec.short_name
 #else
 #include <string.h>
 #endif
+
 #include <uORB/uORB.h>
 
 @##############################
@@ -93,7 +94,8 @@ for field in spec.parsed_fields():
 @# Main struct of message
 @##############################
 @{
-type_map = {'int8': 'int8_t',
+type_map = {
+  'int8': 'int8_t',
   'int16': 'int16_t',
   'int32': 'int32_t',
   'int64': 'int64_t',
@@ -111,16 +113,16 @@ type_map = {'int8': 'int8_t',
 }
 
 msgtype_size_map = {
-  'int8': 8,
-  'int16': 16,
-  'int32': 32,
-  'int64': 64,
-  'uint8': 8,
-  'uint16': 16,
-  'uint32': 32,
-  'uint64': 64,
-  'float32': 32,
-  'float64': 64,
+  'int8': 1,
+  'int16': 2,
+  'int32': 4,
+  'int64': 8,
+  'uint8': 1,
+  'uint16': 2,
+  'uint32': 4,
+  'uint64': 8,
+  'float32': 4,
+  'float64': 8,
   'bool': 1,
   'char': 1,
   'fence_vertex': 8,
@@ -128,33 +130,6 @@ msgtype_size_map = {
   'esc_report': 36
 }
 
-def bare_name(msg_type):
-    bare = msg_type
-    if '/' in msg_type:
-        # removing prefix
-        bare = (msg_type.split('/'))[1]
-    # removing suffix
-    return bare.split('[')[0]
-
-def sizeof_field_type(field):
-    return msgtype_size_map[bare_name(field.type)]
-
-msgtype_size_map = {'int8': 8,
-  'int16': 16,
-  'int32': 32,
-  'int64': 64,
-  'uint8': 8,
-  'uint16': 16,
-  'uint32': 32,
-  'uint64': 64,
-  'float32': 32,
-  'float64': 64,
-  'bool': 1,
-  'char': 1,
-  'fence_vertex': 8,
-  'position_setpoint': 104,
-  'esc_report': 36}
-
 def bare_name(msg_type):
     bare = msg_type
     if '/' in msg_type:
@@ -196,7 +171,7 @@ def print_field_def(field):
 
 def print_parsed_fields():
   # sort fields
-  sorted_fields = sorted(spec.parsed_fields(), key = sizeof_field_type, reverse=True)
+  sorted_fields = sorted(spec.parsed_fields(), key=sizeof_field_type, reverse=True)
   # loop over all fields and print the type and name
   for field in sorted_fields:
     if (not field.is_header):
@@ -204,7 +179,6 @@ def print_parsed_fields():
 
 }@
 
-#pragma pack(push, 1)
 #ifdef __cplusplus
 @#class @(uorb_struct) {
 struct __EXPORT @(uorb_struct) {
@@ -230,11 +204,8 @@ for constant in spec.constants:
 }
 #endif
 };
-#pragma pack(pop)
 
-/**
- * @@}
- */
+void serialize_@(topic_name)(void *in, struct orb_output_buffer *buffer);
 
 /* register this as object request broker structure */
 @[for multi_topic in topics]@
diff --git a/src/drivers/device/integrator.cpp b/src/drivers/device/integrator.cpp
index 86a9b53505..072e3096a2 100644
--- a/src/drivers/device/integrator.cpp
+++ b/src/drivers/device/integrator.cpp
@@ -60,7 +60,7 @@ Integrator::~Integrator()
 }
 
 bool
-Integrator::put(uint64_t timestamp, math::Vector<3> &val, math::Vector<3> &integral, uint64_t *integral_dt)
+Integrator::put(uint64_t timestamp, math::Vector<3> &val, math::Vector<3> &integral, uint64_t &integral_dt)
 {
 	if (_last_integration_time == 0) {
 		/* this is the first item in the integrator */
@@ -132,7 +132,7 @@ Integrator::put_with_interval(unsigned interval_us, math::Vector<3> &val, math::
 }
 
 math::Vector<3>
-Integrator::get(bool reset, uint64_t *integral_dt)
+Integrator::get(bool reset, uint64_t &integral_dt)
 {
 	math::Vector<3> val = _integral;
 
@@ -158,12 +158,12 @@ Integrator::get_and_filtered(bool reset, uint64_t &integral_dt, math::Vector<3>
 }
 
 void
-Integrator::_reset(uint64_t *integral_dt)
+Integrator::_reset(uint64_t &integral_dt)
 {
 	_integral(0) = 0.0f;
 	_integral(1) = 0.0f;
 	_integral(2) = 0.0f;
 
-	*integral_dt = (_last_integration_time - _last_reset_time);
+	integral_dt = (_last_integration_time - _last_reset_time);
 	_last_reset_time = _last_integration_time;
 }
diff --git a/src/drivers/device/integrator.h b/src/drivers/device/integrator.h
index 86817323e2..48a232066f 100644
--- a/src/drivers/device/integrator.h
+++ b/src/drivers/device/integrator.h
@@ -60,7 +60,7 @@ public:
 	 * @return		true if putting the item triggered an integral reset and the integral should be
 	 *			published.
 	 */
-	bool put(uint64_t timestamp, math::Vector<3> &val, math::Vector<3> &integral, uint64_t *integral_dt);
+	bool put(uint64_t timestamp, math::Vector<3> &val, math::Vector<3> &integral, uint64_t &integral_dt);
 
 	/**
 	 * Put an item into the integral but provide an interval instead of a timestamp.
@@ -84,7 +84,7 @@ public:
 	 * @param integral_dt	Get the dt in us of the current integration (only if reset).
 	 * @return		the integral since the last read-reset
 	 */
-	math::Vector<3>		get(bool reset, uint64_t *integral_dt);
+	math::Vector<3>		get(bool reset, uint64_t &integral_dt);
 
 	/**
 	 * Get the current integral and reset the integrator if needed. Additionally give the
@@ -115,5 +115,5 @@ private:
 	 *
 	 * @param integral_dt	Get the dt in us of the current integration.
 	 */
-	void _reset(uint64_t *integral_dt);
+	void _reset(uint64_t &integral_dt);
 };
diff --git a/src/drivers/l3gd20/l3gd20.cpp b/src/drivers/l3gd20/l3gd20.cpp
index e0b2c39c80..61345628b1 100644
--- a/src/drivers/l3gd20/l3gd20.cpp
+++ b/src/drivers/l3gd20/l3gd20.cpp
@@ -1082,7 +1082,7 @@ L3GD20::measure()
 	math::Vector<3> gval(xin, yin, zin);
 	math::Vector<3> gval_integrated;
 
-	bool gyro_notify = _gyro_int.put(report.timestamp, gval, gval_integrated, &report.integral_dt);
+	bool gyro_notify = _gyro_int.put(report.timestamp, gval, gval_integrated, report.integral_dt);
 	report.x_integral = gval_integrated(0);
 	report.y_integral = gval_integrated(1);
 	report.z_integral = gval_integrated(2);
diff --git a/src/drivers/lsm303d/lsm303d.cpp b/src/drivers/lsm303d/lsm303d.cpp
index 6563282f4e..71796cfe8c 100644
--- a/src/drivers/lsm303d/lsm303d.cpp
+++ b/src/drivers/lsm303d/lsm303d.cpp
@@ -1649,7 +1649,7 @@ LSM303D::measure()
 	math::Vector<3> aval(x_in_new, y_in_new, z_in_new);
 	math::Vector<3> aval_integrated;
 
-	bool accel_notify = _accel_int.put(accel_report.timestamp, aval, aval_integrated, &accel_report.integral_dt);
+	bool accel_notify = _accel_int.put(accel_report.timestamp, aval, aval_integrated, accel_report.integral_dt);
 	accel_report.x_integral = aval_integrated(0);
 	accel_report.y_integral = aval_integrated(1);
 	accel_report.z_integral = aval_integrated(2);
diff --git a/src/drivers/mpu6000/mpu6000.cpp b/src/drivers/mpu6000/mpu6000.cpp
index a6b2b2e6f1..4bf6384b3f 100644
--- a/src/drivers/mpu6000/mpu6000.cpp
+++ b/src/drivers/mpu6000/mpu6000.cpp
@@ -1905,7 +1905,7 @@ MPU6000::measure()
 	math::Vector<3> aval(x_in_new, y_in_new, z_in_new);
 	math::Vector<3> aval_integrated;
 
-	bool accel_notify = _accel_int.put(arb.timestamp, aval, aval_integrated, &arb.integral_dt);
+	bool accel_notify = _accel_int.put(arb.timestamp, aval, aval_integrated, arb.integral_dt);
 	arb.x_integral = aval_integrated(0);
 	arb.y_integral = aval_integrated(1);
 	arb.z_integral = aval_integrated(2);
@@ -1945,7 +1945,7 @@ MPU6000::measure()
 	math::Vector<3> gval(x_gyro_in_new, y_gyro_in_new, z_gyro_in_new);
 	math::Vector<3> gval_integrated;
 
-	bool gyro_notify = _gyro_int.put(arb.timestamp, gval, gval_integrated, &grb.integral_dt);
+	bool gyro_notify = _gyro_int.put(arb.timestamp, gval, gval_integrated, grb.integral_dt);
 	grb.x_integral = gval_integrated(0);
 	grb.y_integral = gval_integrated(1);
 	grb.z_integral = gval_integrated(2);
diff --git a/src/drivers/mpu6500/mpu6500.cpp b/src/drivers/mpu6500/mpu6500.cpp
index 20fcfb6dbe..c3457c5b8b 100644
--- a/src/drivers/mpu6500/mpu6500.cpp
+++ b/src/drivers/mpu6500/mpu6500.cpp
@@ -1846,7 +1846,7 @@ MPU6500::measure()
 	math::Vector<3> aval(x_in_new, y_in_new, z_in_new);
 	math::Vector<3> aval_integrated;
 
-	bool accel_notify = _accel_int.put(arb.timestamp, aval, aval_integrated, &arb.integral_dt);
+	bool accel_notify = _accel_int.put(arb.timestamp, aval, aval_integrated, arb.integral_dt);
 	arb.x_integral = aval_integrated(0);
 	arb.y_integral = aval_integrated(1);
 	arb.z_integral = aval_integrated(2);
@@ -1881,7 +1881,7 @@ MPU6500::measure()
 	math::Vector<3> gval(x_gyro_in_new, y_gyro_in_new, z_gyro_in_new);
 	math::Vector<3> gval_integrated;
 
-	bool gyro_notify = _gyro_int.put(arb.timestamp, gval, gval_integrated, &grb.integral_dt);
+	bool gyro_notify = _gyro_int.put(arb.timestamp, gval, gval_integrated, grb.integral_dt);
 	grb.x_integral = gval_integrated(0);
 	grb.y_integral = gval_integrated(1);
 	grb.z_integral = gval_integrated(2);
diff --git a/src/drivers/mpu9250/mpu9250.cpp b/src/drivers/mpu9250/mpu9250.cpp
index b2dd1b3549..761c8f1008 100644
--- a/src/drivers/mpu9250/mpu9250.cpp
+++ b/src/drivers/mpu9250/mpu9250.cpp
@@ -1418,7 +1418,7 @@ MPU9250::measure()
 	math::Vector<3> aval(x_in_new, y_in_new, z_in_new);
 	math::Vector<3> aval_integrated;
 
-	bool accel_notify = _accel_int.put(arb.timestamp, aval, aval_integrated, &arb.integral_dt);
+	bool accel_notify = _accel_int.put(arb.timestamp, aval, aval_integrated, arb.integral_dt);
 	arb.x_integral = aval_integrated(0);
 	arb.y_integral = aval_integrated(1);
 	arb.z_integral = aval_integrated(2);
@@ -1453,7 +1453,7 @@ MPU9250::measure()
 	math::Vector<3> gval(x_gyro_in_new, y_gyro_in_new, z_gyro_in_new);
 	math::Vector<3> gval_integrated;
 
-	bool gyro_notify = _gyro_int.put(arb.timestamp, gval, gval_integrated, &grb.integral_dt);
+	bool gyro_notify = _gyro_int.put(arb.timestamp, gval, gval_integrated, grb.integral_dt);
 	grb.x_integral = gval_integrated(0);
 	grb.y_integral = gval_integrated(1);
 	grb.z_integral = gval_integrated(2);
diff --git a/src/drivers/px4flow/px4flow.cpp b/src/drivers/px4flow/px4flow.cpp
index 647ae1590c..095d70e38b 100644
--- a/src/drivers/px4flow/px4flow.cpp
+++ b/src/drivers/px4flow/px4flow.cpp
@@ -541,9 +541,9 @@ PX4FLOW::collect()
 	/* rotate measurements according to parameter */
 	float zeroval = 0.0f;
 
-	rotate_3f(_sensor_rotation, &report.pixel_flow_x_integral, &report.pixel_flow_y_integral, &zeroval);
+	rotate_3f(_sensor_rotation, report.pixel_flow_x_integral, report.pixel_flow_y_integral, zeroval);
 
-	rotate_3f(_sensor_rotation, &report.gyro_x_rate_integral, &report.gyro_y_rate_integral, &report.gyro_z_rate_integral);
+	rotate_3f(_sensor_rotation, report.gyro_x_rate_integral, report.gyro_y_rate_integral, report.gyro_z_rate_integral);
 
 	if (_px4flow_topic == nullptr) {
 		_px4flow_topic = orb_advertise(ORB_ID(optical_flow), &report);
diff --git a/src/lib/conversion/rotation.cpp b/src/lib/conversion/rotation.cpp
index 971be328ee..26bfc4d9ef 100644
--- a/src/lib/conversion/rotation.cpp
+++ b/src/lib/conversion/rotation.cpp
@@ -52,16 +52,6 @@ get_rot_matrix(enum Rotation rot, math::Matrix<3, 3> *rot_matrix)
 
 #define HALF_SQRT_2 0.70710678118654757f
 
-__EXPORT void
-rotate_3f(enum Rotation rot, float *x, float *y, float *z)
-{
-	float &x2 = *x;
-	float &y2 = *y;
-	float &z2 = *z;
-
-	rotate_3f(rot, x2, y2, z2);
-}
-
 __EXPORT void
 rotate_3f(enum Rotation rot, float &x, float &y, float &z)
 {
diff --git a/src/lib/conversion/rotation.h b/src/lib/conversion/rotation.h
index 87ac9064d5..1e1cdce268 100644
--- a/src/lib/conversion/rotation.h
+++ b/src/lib/conversion/rotation.h
@@ -130,11 +130,6 @@ const rot_lookup_t rot_lookup[] = {
 __EXPORT void
 get_rot_matrix(enum Rotation rot, math::Matrix<3, 3> *rot_matrix);
 
-/**
- * rotate a 3 element float vector in-place
- */
-__EXPORT void
-rotate_3f(enum Rotation rot, float *x, float *y, float *z);
 
 /**
  * rotate a 3 element float vector in-place
diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp
index bb743239b9..ba304dd229 100644
--- a/src/modules/commander/commander.cpp
+++ b/src/modules/commander/commander.cpp
@@ -2295,9 +2295,8 @@ int commander_thread_main(int argc, char *argv[])
 		 * rejection. Back off 2 seconds to not overlay
 		 * home tune.
 		 */
-		uint64_t ts = _home.timestamp;
 		if (status_flags.condition_home_position_valid &&
-			(hrt_elapsed_time(&ts) > 2000000) &&
+			(hrt_elapsed_time(&_home.timestamp) > 2000000) &&
 			_last_mission_instance != mission_result.instance_count) {
 			if (!mission_result.valid) {
 				/* the mission is invalid */
diff --git a/src/modules/logger/logger.cpp b/src/modules/logger/logger.cpp
index a2345a86f6..a323b8407b 100644
--- a/src/modules/logger/logger.cpp
+++ b/src/modules/logger/logger.cpp
@@ -4,8 +4,11 @@
 #include <errno.h>
 #include <string.h>
 
+#include <uORB/uORB.h>
 #include <uORB/uORBTopics.h>
+#include <px4_includes.h>
 #include <px4_getopt.h>
+#include <px4_log.h>
 
 #define DBGPRINT
 
@@ -25,6 +28,14 @@ using namespace px4::logger;
 
 int logger_main(int argc, char *argv[])
 {
+	// logger currently assumes little endian
+	int num = 1;
+
+	if (*(char *)&num != 1) {
+		PX4_ERR("Logger only works on little endian!\n");
+		return 1;
+	}
+
 	if (argc < 2) {
 		PX4_INFO("usage: logger {start|stop|status}");
 		return 1;
@@ -196,7 +207,12 @@ void Logger::run_trampoline(int argc, char *argv[])
 	warnx("remaining free heap: %d bytes", alloc_info.fordblks);
 #endif /* DBGPRINT */
 
-	logger_ptr->run();
+	if (logger_ptr == nullptr) {
+		PX4_WARN("alloc failed");
+
+	} else {
+		logger_ptr->run();
+	}
 }
 
 enum class MessageType : uint8_t {
@@ -480,8 +496,8 @@ void Logger::run()
 				/* each message consists of a header followed by an orb data object
 				 * The size of the data object is given by orb_metadata.o_size
 				 */
-				size_t msg_size = sizeof(message_data_header_s) + sub.metadata->o_size;
-				uint8_t buffer[msg_size];
+				size_t padded_msg_size = sub.metadata->o_size;
+				uint8_t orb_msg_padded[padded_msg_size];
 
 				/* if this topic has been updated, copy the new data into the message buffer
 				 * and write a message to the log
@@ -489,8 +505,15 @@ void Logger::run()
 				//orb_check(sub.fd, &updated);	// check whether a non-multi topic has been updated
 				/* this works for both single and multi-instances */
 				for (uint8_t instance = 0; instance < ORB_MULTI_MAX_INSTANCES; instance++) {
-					if (copy_if_updated_multi(sub.metadata, instance, &sub.fd[instance], buffer + sizeof(message_data_header_s),
-								  &sub.time_tried_subscribe)) {
+					if (copy_if_updated_multi(sub.metadata, instance, &sub.fd[instance], &orb_msg_padded, &sub.time_tried_subscribe)) {
+
+						struct orb_output_buffer output_buffer = {};
+						uint8_t buffer[sizeof(message_data_header_s) + padded_msg_size];
+						output_buffer.data = &buffer;
+						output_buffer.next = sizeof(message_data_header_s);
+
+						sub.metadata->serialize(&orb_msg_padded, &output_buffer);
+						size_t msg_size = output_buffer.next;
 
 						//uint64_t timestamp;
 						//memcpy(&timestamp, buffer + sizeof(message_data_header_s), sizeof(timestamp));
@@ -505,10 +528,14 @@ void Logger::run()
 						 */
 						message_data_header_s *header = reinterpret_cast<message_data_header_s *>(buffer);
 						header->msg_type = static_cast<uint8_t>(MessageType::DATA);
-						header->msg_size = static_cast<uint16_t>(msg_size - 3);
+						/* the ORB topic data object has 2 unused trailing bytes? */
+						//header->msg_size = static_cast<uint16_t>(msg_size - 3);
+						header->msg_size = msg_size;
 						header->msg_id = msg_id;
 						header->multi_id = 0x80 + instance;	// Non multi, active
 
+						//PX4_INFO("topic: %s, size = %zu, out_size = %zu", sub.metadata->o_name, sub.metadata->o_size, msg_size);
+
 #ifdef DBGPRINT
 						//warnx("subscription %s updated: %d, size: %d", sub.metadata->o_name, updated, msg_size);
 						hrt_abstime trytime = hrt_absolute_time();
@@ -527,7 +554,9 @@ void Logger::run()
 							if (dropout_start != 0) {
 								double drop_len = (double)(trytime - dropout_start)  * 1e-6;
 
-								if (drop_len > max_drop_len) { max_drop_len = drop_len; }
+								if (drop_len > max_drop_len) {
+									max_drop_len = drop_len;
+								}
 
 								PX4_WARN("dropout length: %5.3f seconds", drop_len);
 								dropout_start = 0;
diff --git a/src/modules/logger/logger.h b/src/modules/logger/logger.h
index 4a0dc59d04..3309ebcb1d 100644
--- a/src/modules/logger/logger.h
+++ b/src/modules/logger/logger.h
@@ -32,7 +32,9 @@ struct LoggerSubscription {
 		fd[0] = fd_;
 		time_tried_subscribe = 0;
 
-		for (int i = 1; i < ORB_MULTI_MAX_INSTANCES; i++) { fd[i] = -1; }
+		for (int i = 1; i < ORB_MULTI_MAX_INSTANCES; i++) {
+			fd[i] = -1;
+		}
 	}
 };
 
diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp
index f450b2e9fc..7a72c24ded 100644
--- a/src/modules/mavlink/mavlink_receiver.cpp
+++ b/src/modules/mavlink/mavlink_receiver.cpp
@@ -507,8 +507,8 @@ MavlinkReceiver::handle_message_optical_flow_rad(mavlink_message_t *msg)
 
 	/* rotate measurements according to parameter */
 	float zeroval = 0.0f;
-	rotate_3f(flow_rot, &f.pixel_flow_x_integral, &f.pixel_flow_y_integral, &zeroval);
-	rotate_3f(flow_rot, &f.gyro_x_rate_integral, &f.gyro_y_rate_integral, &f.gyro_z_rate_integral);
+	rotate_3f(flow_rot, f.pixel_flow_x_integral, f.pixel_flow_y_integral, zeroval);
+	rotate_3f(flow_rot, f.gyro_x_rate_integral, f.gyro_y_rate_integral, f.gyro_z_rate_integral);
 
 	if (_flow_pub == nullptr) {
 		_flow_pub = orb_advertise(ORB_ID(optical_flow), &f);
diff --git a/src/modules/uORB/uORB.h b/src/modules/uORB/uORB.h
index d9e57c9436..1b06a2ad89 100644
--- a/src/modules/uORB/uORB.h
+++ b/src/modules/uORB/uORB.h
@@ -46,12 +46,12 @@
 // Hack until everything is using this header
 #include <systemlib/visibility.h>
 
-// Macro to define packed structures
-#ifdef __GNUC__
-#define ORBPACKED( __Declaration__ ) __Declaration__ __attribute__((aligned(4), packed))
-#else
-#define ORBPACKED( __Declaration__ ) __pragma( pack(push, 1) ) __Declaration__ __pragma( pack(pop) )
-#endif
+struct orb_output_buffer {
+	void *data;
+	size_t next;
+};
+
+typedef void (*func_ptr)(void *in, struct orb_output_buffer *out);
 
 /**
  * Object metadata.
@@ -59,6 +59,7 @@
 struct orb_metadata {
 	const char *o_name;		/**< unique object name */
 	const size_t o_size;		/**< object size */
+	func_ptr serialize;			/**< serialization function for this orb topic */
 	const char *o_fields;		/**< semicolon separated list of fields */
 };
 
@@ -118,13 +119,15 @@ enum ORB_PRIO {
  *
  * @param _name		The name of the topic.
  * @param _struct	The structure the topic provides.
- * @param _func		The pointer to a function that packs topic
+ * @param _func		The pointer to a function that serializes this topic
+ * @param _fields	All fields in a semicolon separated list e.g: "float[3] position;bool armed"
  */
-#define ORB_DEFINE(_name, _struct, _fields)			\
+#define ORB_DEFINE(_name, _struct, _func, _fields)			\
 	const struct orb_metadata __orb_##_name = {	\
 		#_name,					\
 		sizeof(_struct),		\
-		_fields				\
+		_func,					\
+		_fields					\
 	}; struct hack
 
 __BEGIN_DECLS
diff --git a/src/modules/uORB/uORB_tests/uORBTest_UnitTest.hpp b/src/modules/uORB/uORB_tests/uORBTest_UnitTest.hpp
index 032b25a544..6f5012cec3 100644
--- a/src/modules/uORB/uORB_tests/uORBTest_UnitTest.hpp
+++ b/src/modules/uORB/uORB_tests/uORBTest_UnitTest.hpp
@@ -41,8 +41,8 @@ struct orb_test {
 	int val;
 	hrt_abstime time;
 };
-ORB_DEFINE(orb_test, struct orb_test, "ORB_TEST:int val;hrt_abstime time;");
-ORB_DEFINE(orb_multitest, struct orb_test, "ORB_MULTITEST:int val;hrt_abstime time;");
+ORB_DEFINE(orb_test, struct orb_test, nullptr, "ORB_TEST:int val;hrt_abstime time;");
+ORB_DEFINE(orb_multitest, struct orb_test, nullptr, "ORB_MULTITEST:int val;hrt_abstime time;");
 
 
 struct orb_test_medium {
@@ -50,8 +50,8 @@ struct orb_test_medium {
 	hrt_abstime time;
 	char junk[64];
 };
-ORB_DEFINE(orb_test_medium, struct orb_test_medium,  "ORB_TEST_MEDIUM:int val;hrt_abstime time;char[64] junk;");
-ORB_DEFINE(orb_test_medium_multi, struct orb_test_medium,
+ORB_DEFINE(orb_test_medium, struct orb_test_medium, nullptr, "ORB_TEST_MEDIUM:int val;hrt_abstime time;char[64] junk;");
+ORB_DEFINE(orb_test_medium_multi, struct orb_test_medium, nullptr,
 	   "ORB_TEST_MEDIUM_MULTI:int val;hrt_abstime time;char[64] junk;");
 
 
@@ -60,7 +60,7 @@ struct orb_test_large {
 	hrt_abstime time;
 	char junk[512];
 };
-ORB_DEFINE(orb_test_large, struct orb_test_large, "ORB_TEST_LARGE:int val;hrt_abstime time;char[512] junk;");
+ORB_DEFINE(orb_test_large, struct orb_test_large, nullptr, "ORB_TEST_LARGE:int val;hrt_abstime time;char[512] junk;");
 
 
 namespace uORBTest
diff --git a/src/platforms/posix/drivers/gyrosim/gyrosim.cpp b/src/platforms/posix/drivers/gyrosim/gyrosim.cpp
index d7084fffcf..012db15942 100644
--- a/src/platforms/posix/drivers/gyrosim/gyrosim.cpp
+++ b/src/platforms/posix/drivers/gyrosim/gyrosim.cpp
@@ -1103,7 +1103,7 @@ GYROSIM::_measure()
 	math::Vector<3> aval(mpu_report.accel_x, mpu_report.accel_y, mpu_report.accel_z);
 	math::Vector<3> aval_integrated;
 
-	bool accel_notify = _accel_int.put(arb.timestamp, aval, aval_integrated, &arb.integral_dt);
+	bool accel_notify = _accel_int.put(arb.timestamp, aval, aval_integrated, arb.integral_dt);
 	arb.x_integral = aval_integrated(0);
 	arb.y_integral = aval_integrated(1);
 	arb.z_integral = aval_integrated(2);
@@ -1125,7 +1125,7 @@ GYROSIM::_measure()
 	math::Vector<3> gval(mpu_report.gyro_x, mpu_report.gyro_y, mpu_report.gyro_z);
 	math::Vector<3> gval_integrated;
 
-	bool gyro_notify = _gyro_int.put(grb.timestamp, gval, gval_integrated, &grb.integral_dt);
+	bool gyro_notify = _gyro_int.put(grb.timestamp, gval, gval_integrated, grb.integral_dt);
 	grb.x_integral = gval_integrated(0);
 	grb.y_integral = gval_integrated(1);
 	grb.z_integral = gval_integrated(2);
diff --git a/unittests/uorb_unittests/uORBCommunicator_gtests.cpp b/unittests/uorb_unittests/uORBCommunicator_gtests.cpp
index 35822f016f..a0b61a250f 100644
--- a/unittests/uorb_unittests/uORBCommunicator_gtests.cpp
+++ b/unittests/uorb_unittests/uORBCommunicator_gtests.cpp
@@ -206,7 +206,7 @@ namespace uORB_test
      ASSERT_EQ( c._send_messageCount, 0 );
 
      //step 1.
-     ORB_DEFINE( topicA_sndmsg, struct orb_topic_A, "TOPICA_SNDMSG:int16_t val;" );
+     ORB_DEFINE( topicA_sndmsg, struct orb_topic_A, nullptr, "TOPICA_SNDMSG:int16_t val;" );
      _topicA.val = 1;
      _pub_ptr = orb_advertise(ORB_ID(topicA_sndmsg ), &_topicA );
      ASSERT_TRUE( ( _pub_ptr != nullptr ) ) << "Failed to advertize uORB Topic topicA_sndmsg: errno: " << errno;
diff --git a/unittests/uorb_unittests/uORBGtestTopics.hpp b/unittests/uorb_unittests/uORBGtestTopics.hpp
index 0cea8d55fc..ca8572efac 100644
--- a/unittests/uorb_unittests/uORBGtestTopics.hpp
+++ b/unittests/uorb_unittests/uORBGtestTopics.hpp
@@ -48,11 +48,11 @@ namespace uORB_test
      int16_t val;
    };
 
-   ORB_DEFINE( topicA, struct orb_topic_A, "TOPICA:int16 val;" );
-   ORB_DEFINE( topicB, struct orb_topic_B, "TOPICB:int16 val;" );
+   ORB_DEFINE(topicA, struct orb_topic_A, nullptr, "TOPICA:int16 val;");
+   ORB_DEFINE(topicB, struct orb_topic_B, nullptr, "TOPICB:int16 val;");
 
-   ORB_DEFINE( topicA_clone, struct orb_topic_A, "TOPICA_CLONE:int16 val;" );
-   ORB_DEFINE( topicB_clone, struct orb_topic_B, "TOPICB_CLONE:int16 val;"  );
+   ORB_DEFINE(topicA_clone, struct orb_topic_A, nullptr, "TOPICA_CLONE:int16 val;";
+   ORB_DEFINE(topicB_clone, struct orb_topic_B, nullptr, "TOPICB_CLONE:int16 val;");
 }
 
 #endif // _UnitTest_uORBTopics_hpp_

From 7d4c4c0401fd9f4e488f22bbbac797234116c40a Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Beat=20K=C3=BCng?= <beat-kueng@gmx.net>
Date: Thu, 5 May 2016 14:05:42 +0200
Subject: [PATCH 0165/1230] px_generate_uorb_topic_sources.py: add search_path
 to the environment

---
 Tools/px_generate_uorb_topic_sources.py | 11 +++++++++--
 1 file changed, 9 insertions(+), 2 deletions(-)

diff --git a/Tools/px_generate_uorb_topic_sources.py b/Tools/px_generate_uorb_topic_sources.py
index ccba860214..b3071832c3 100755
--- a/Tools/px_generate_uorb_topic_sources.py
+++ b/Tools/px_generate_uorb_topic_sources.py
@@ -77,6 +77,7 @@ __email__ = "againagainst@gmail.com, thomasgubler@gmail.com"
 TOPIC_TEMPLATE_FILE = 'msg.cpp.template'
 TOPICS_LIST_TEMPLATE_FILE = 'uORBTopics.cpp.template'
 OUTPUT_FILE_EXT = '.cpp'
+INCL_DEFAULT = ['std_msgs:./msg/std_msgs']
 PACKAGE = 'px4'
 TOPICS_TOKEN = '# TOPICS '
 
@@ -104,7 +105,7 @@ def get_msgs_list(msgdir):
     return [fn for fn in os.listdir(msgdir) if fn.endswith(".msg")]
 
 
-def generate_source_from_file(filename, outputdir, template_file, quiet=False):
+def generate_source_from_file(filename, outputdir, template_file, includepath, quiet=False):
         """
         Converts a single .msg file to a uorb source file
         """
@@ -113,10 +114,15 @@ def generate_source_from_file(filename, outputdir, template_file, quiet=False):
         full_type_name = genmsg.gentools.compute_full_type_name(PACKAGE, os.path.basename(filename))
         spec = genmsg.msg_loader.load_msg_from_file(msg_context, filename, full_type_name)
         topics = get_multi_topics(filename)
+        if includepath:
+                search_path = genmsg.command_line.includepath_to_dict(includepath)
+        else:
+                search_path = {}
         if len(topics) == 0:
             topics.append(spec.short_name)
         em_globals = {
             "file_name_in": filename,
+            "search_path": search_path,
             "spec": spec,
             "topics": topics
         }
@@ -156,6 +162,7 @@ def convert_dir(msgdir, outputdir, templatedir, quiet=False):
                 os.makedirs(outputdir)
         template_file = os.path.join(templatedir, TOPIC_TEMPLATE_FILE)
 
+        includepath = INCL_DEFAULT + [':'.join([PACKAGE, msgdir])]
         for f in os.listdir(msgdir):
                 # Ignore hidden files
                 if f.startswith("."):
@@ -164,7 +171,7 @@ def convert_dir(msgdir, outputdir, templatedir, quiet=False):
                 # Only look at actual files
                 if not os.path.isfile(fn):
                         continue
-                generate_source_from_file(fn, outputdir, template_file, quiet)
+                generate_source_from_file(fn, outputdir, template_file, includepath, quiet)
 
         # generate cpp file with topics list
         tl_globals = {"msgs" : get_msgs_list(msgdir)}

From eabc43d78c78754ce85dcac6c3d57e8f7bf876e4 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Beat=20K=C3=BCng?= <beat-kueng@gmx.net>
Date: Thu, 5 May 2016 14:10:23 +0200
Subject: [PATCH 0166/1230] orb structs: add padding bytes to align the structs
 where necessary

This is required for the logger, we just manually add the padding bytes
what would otherwise be done by the compiler. Additionally we reorder
the fields by type, so that padding is only necessary for nested types.
---
 msg/templates/uorb/msg.cpp.template           | 111 ++++++++++--------
 msg/templates/uorb/msg.h.template             |  82 ++++++++++---
 src/modules/logger/logger.cpp                 |  34 +-----
 src/modules/uORB/uORB.h                       |  17 +--
 .../uORB/uORB_tests/uORBTest_UnitTest.hpp     |  14 ++-
 5 files changed, 152 insertions(+), 106 deletions(-)

diff --git a/msg/templates/uorb/msg.cpp.template b/msg/templates/uorb/msg.cpp.template
index 589d05dca4..c9bf11bb85 100644
--- a/msg/templates/uorb/msg.cpp.template
+++ b/msg/templates/uorb/msg.cpp.template
@@ -13,10 +13,12 @@
 @#  - file_name_in (String) Source file
 @#  - spec (msggen.MsgSpec) Parsed specification of the .msg file
 @#  - md5sum (String) MD5Sum of the .msg specification
+@#  - search_path (dict) search paths for genmsg
+@#  - topics (List of String) multi-topic names
 @###############################################
 /****************************************************************************
  *
- *   Copyright (C) 2013-2015 PX4 Development Team. All rights reserved.
+ *   Copyright (C) 2013-2016 PX4 Development Team. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -54,7 +56,6 @@ import genmsg.msgs
 import gencpp
 
 uorb_struct = '%s_s'%spec.short_name
-uorb_pack_func = 'pack_%s'%spec.short_name
 topic_name = spec.short_name
 
 type_map = {
@@ -70,9 +71,6 @@ type_map = {
   'float64': 'double',
   'bool': 'bool',
   'char': 'char',
-  'fence_vertex': 'fence_vertex',
-  'position_setpoint': 'position_setpoint',
-  'esc_report': 'esc_report'
 }
 
 msgtype_size_map = {
@@ -88,13 +86,8 @@ msgtype_size_map = {
   'float64': 8,
   'bool': 1,
   'char': 1,
-  'fence_vertex': 8,
-  'position_setpoint': 104,
-  'esc_report': 36
 }
 
-msgtypes_composite = ['position_setpoint', 'esc_report', 'fence_vertex']
-
 def convert_type(spec_type):
     bare_type = spec_type
     if '/' in spec_type:
@@ -102,7 +95,9 @@ def convert_type(spec_type):
         bare_type = (spec_type.split('/'))[1]
 
     msg_type, is_array, array_length = genmsg.msgs.parse_type(bare_type)
-    c_type = type_map[msg_type]
+    c_type = msg_type
+    if msg_type in type_map:
+        c_type = type_map[msg_type]
     if is_array:
         return c_type + "[" + str(array_length) + "]"
     return c_type
@@ -116,9 +111,65 @@ def bare_name(msg_type):
     return bare.split('[')[0]
 
 def sizeof_field_type(field):
-    return msgtype_size_map[bare_name(field.type)]
+    bare_name_str = bare_name(field.type)
+    if bare_name_str in msgtype_size_map:
+        return msgtype_size_map[bare_name_str]
+    return 0 # this is for non-builtin types: sort them at the end
+
+def get_children_fields(base_type):
+    (package, name) = genmsg.names.package_resource_name(base_type)
+    tmp_msg_context = genmsg.msg_loader.MsgContext.create_default()
+    spec_temp = genmsg.msg_loader.load_msg_by_type(tmp_msg_context, '%s/%s' %(package, name), search_path)  
+    sorted_fields = sorted(spec_temp.parsed_fields(), key=sizeof_field_type, reverse=True)
+    return sorted_fields
+
+# Add padding fields before the embedded types, at the end and calculate the
+# struct size
+def add_padding_bytes(fields):
+  struct_size = 8 # account for the timestamp
+  align_to = 8 # this is always 8, because of the 64bit timestamp
+  i = 0
+  padding_idx = 0
+  while i < len(fields):
+    field = fields[i]
+    if not field.is_header:
+        a_pos = field.type.find('[')
+        array_size = 1
+        if field.is_array:
+            array_size = field.array_len
+        if field.is_builtin:
+            field.sizeof_field_type = sizeof_field_type(field)
+        else:
+            # embedded type: may need to add padding
+            num_padding_bytes = align_to - (struct_size % align_to)
+            if num_padding_bytes != align_to:
+                padding_field = genmsg.Field('_padding'+str(padding_idx),
+                    'uint8['+str(num_padding_bytes)+']')
+                padding_idx += 1
+                padding_field.sizeof_field_type = 1
+                struct_size += num_padding_bytes
+                fields.insert(i, padding_field)
+                i += 1
+            children_fields = get_children_fields(field.base_type)
+            field.sizeof_field_type, unused = add_padding_bytes(children_fields)
+        struct_size += field.sizeof_field_type * array_size
+    i += 1
+
+  # add padding at the end (necessary for embedded types)
+  num_padding_bytes = align_to - (struct_size % align_to)
+  if num_padding_bytes == align_to:
+      num_padding_bytes = 0
+  else:
+      padding_field = genmsg.Field('_padding'+str(padding_idx),
+          'uint8['+str(num_padding_bytes)+']')
+      padding_idx += 1
+      padding_field.sizeof_field_type = 1
+      struct_size += num_padding_bytes
+      fields.append(padding_field)
+  return (struct_size, num_padding_bytes)
 
 sorted_fields = sorted(spec.parsed_fields(), key=sizeof_field_type, reverse=True)
+struct_size, padding_end_size = add_padding_bytes(sorted_fields)
 topic_fields = ["%s %s" % (convert_type(field.type), field.name) for field in sorted_fields]
 }@
 
@@ -127,42 +178,10 @@ topic_fields = ["%s %s" % (convert_type(field.type), field.name) for field in so
 #include <uORB/topics/@(topic_name).h>
 
 
-void serialize_@(topic_name)(void *in, struct orb_output_buffer *buffer)
-{
-	struct @(uorb_struct) *topic = static_cast<struct @(uorb_struct) *>(in);
-		
-@{
-
-serialize_expr = "	serialize_{type}(&topic->{field}, buffer);"
-serialize_array_expr = "	serialize_{type}(&topic->{field}[{index}], buffer);"
-
-buffer_memcpy_expr = "	memcpy(((char *)buffer->data) + buffer->next, &topic->{field}, sizeof(topic->{field}));"
-buffer_next_expr = "	buffer->next += sizeof(topic->{field});"
-
-for each_field in sorted_fields:
-  if not each_field.is_header:
-  
-    # call appropriate serialize functions for fields that are orb messages
-    if bare_name(each_field.type) in msgtypes_composite:
-      
-      if each_field.is_array:
-        for i in range(each_field.array_len):
-          print(serialize_array_expr.format(type=type_map[bare_name(each_field.type)], field=each_field.name, index=i))
-
-      else:
-    	print(serialize_expr.format(type=type_map[bare_name(each_field.type)], field=each_field.name))
-
-	# copy primitive fields
-    else:
-    	print(buffer_memcpy_expr.format(field=each_field.name))
-    	print(buffer_next_expr.format(field=each_field.name))
-
-}@
-}
-
 @# join all msg files in one line e.g: "float[3] position;float[3] velocity;bool armed"
 const char *__orb_@(topic_name)_fields = "@( topic_name.upper() ):@( ";".join(topic_fields) );";
 
 @[for multi_topic in topics]@
-ORB_DEFINE(@multi_topic, struct @uorb_struct, &serialize_@(topic_name), __orb_@(topic_name)_fields);
+ORB_DEFINE(@multi_topic, struct @uorb_struct, @(struct_size-padding_end_size),
+    __orb_@(topic_name)_fields);
 @[end for]
diff --git a/msg/templates/uorb/msg.h.template b/msg/templates/uorb/msg.h.template
index 9196edbc4d..a165f51ad5 100644
--- a/msg/templates/uorb/msg.h.template
+++ b/msg/templates/uorb/msg.h.template
@@ -13,10 +13,12 @@
 @#  - file_name_in (String) Source file
 @#  - spec (msggen.MsgSpec) Parsed specification of the .msg file
 @#  - md5sum (String) MD5Sum of the .msg specification
+@#  - search_path (dict) search paths for genmsg
+@#  - topics (List of String) multi-topic names
 @###############################################
 /****************************************************************************
  *
- *   Copyright (C) 2013-2015 PX4 Development Team. All rights reserved.
+ *   Copyright (C) 2013-2016 PX4 Development Team. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -107,9 +109,6 @@ type_map = {
   'float64': 'double',
   'bool': 'bool',
   'char': 'char',
-  'fence_vertex': 'fence_vertex',
-  'position_setpoint': 'position_setpoint',
-  'esc_report': 'esc_report'
 }
 
 msgtype_size_map = {
@@ -125,9 +124,6 @@ msgtype_size_map = {
   'float64': 8,
   'bool': 1,
   'char': 1,
-  'fence_vertex': 8,
-  'position_setpoint': 104,
-  'esc_report': 36
 }
 
 def bare_name(msg_type):
@@ -139,7 +135,62 @@ def bare_name(msg_type):
     return bare.split('[')[0]
 
 def sizeof_field_type(field):
-    return msgtype_size_map[bare_name(field.type)]
+    bare_name_str = bare_name(field.type)
+    if bare_name_str in msgtype_size_map:
+        return msgtype_size_map[bare_name_str]
+    return 0 # this is for non-builtin types: sort them at the end
+
+def get_children_fields(base_type):
+    (package, name) = genmsg.names.package_resource_name(base_type)
+    tmp_msg_context = genmsg.msg_loader.MsgContext.create_default()
+    spec_temp = genmsg.msg_loader.load_msg_by_type(tmp_msg_context, '%s/%s' %(package, name), search_path)  
+    sorted_fields = sorted(spec_temp.parsed_fields(), key=sizeof_field_type, reverse=True)
+    return sorted_fields
+
+# Add padding fields before the embedded types, at the end and calculate the
+# struct size
+def add_padding_bytes(fields):
+  struct_size = 8 # account for the timestamp
+  align_to = 8 # this is always 8, because of the 64bit timestamp
+  i = 0
+  padding_idx = 0
+  while i < len(fields):
+    field = fields[i]
+    if not field.is_header:
+        a_pos = field.type.find('[')
+        array_size = 1
+        if field.is_array:
+            array_size = field.array_len
+        if field.is_builtin:
+            field.sizeof_field_type = sizeof_field_type(field)
+        else:
+            # embedded type: may need to add padding
+            num_padding_bytes = align_to - (struct_size % align_to)
+            if num_padding_bytes != align_to:
+                padding_field = genmsg.Field('_padding'+str(padding_idx),
+                    'uint8['+str(num_padding_bytes)+']')
+                padding_idx += 1
+                padding_field.sizeof_field_type = 1
+                struct_size += num_padding_bytes
+                fields.insert(i, padding_field)
+                i += 1
+            children_fields = get_children_fields(field.base_type)
+            field.sizeof_field_type, unused = add_padding_bytes(children_fields)
+        struct_size += field.sizeof_field_type * array_size
+    i += 1
+
+  # add padding at the end (necessary for embedded types)
+  num_padding_bytes = align_to - (struct_size % align_to)
+  if num_padding_bytes == align_to:
+      num_padding_bytes = 0
+  else:
+      padding_field = genmsg.Field('_padding'+str(padding_idx),
+          'uint8['+str(num_padding_bytes)+']')
+      padding_idx += 1
+      padding_field.sizeof_field_type = 1
+      struct_size += num_padding_bytes
+      fields.append(padding_field)
+  return (struct_size, num_padding_bytes)
 
 # Function to print a standard ros type
 def print_field_def(field):
@@ -165,18 +216,23 @@ def print_field_def(field):
     # need to add _t: int8 --> int8_t
     type_px4 = type_map[type_name]
   else:
-    raise Exception("Type {0} not supported, add to to template file!".format(type_name))
+    type_px4 = type_name
 
-  print('\t%s%s%s %s%s;'%(type_prefix, type_px4, type_appendix, field.name, array_size))
+  comment = ''
+  if field.name.startswith('_padding'):
+      comment = ' // required for logger'
+
+  print('\t%s%s%s %s%s;%s'%(type_prefix, type_px4, type_appendix, field.name,
+      array_size, comment))
 
 def print_parsed_fields():
-  # sort fields
+  # sort fields (using a stable sort)
   sorted_fields = sorted(spec.parsed_fields(), key=sizeof_field_type, reverse=True)
+  struct_size, padding_end_size = add_padding_bytes(sorted_fields)
   # loop over all fields and print the type and name
   for field in sorted_fields:
     if (not field.is_header):
       print_field_def(field)
-
 }@
 
 #ifdef __cplusplus
@@ -205,8 +261,6 @@ for constant in spec.constants:
 #endif
 };
 
-void serialize_@(topic_name)(void *in, struct orb_output_buffer *buffer);
-
 /* register this as object request broker structure */
 @[for multi_topic in topics]@
 ORB_DECLARE(@multi_topic);
diff --git a/src/modules/logger/logger.cpp b/src/modules/logger/logger.cpp
index a323b8407b..9ba2543ead 100644
--- a/src/modules/logger/logger.cpp
+++ b/src/modules/logger/logger.cpp
@@ -494,43 +494,21 @@ void Logger::run()
 
 			for (LoggerSubscription &sub : _subscriptions) {
 				/* each message consists of a header followed by an orb data object
-				 * The size of the data object is given by orb_metadata.o_size
 				 */
-				size_t padded_msg_size = sub.metadata->o_size;
-				uint8_t orb_msg_padded[padded_msg_size];
+				size_t msg_size = sizeof(message_data_header_s) + sub.metadata->o_size;
+				//TODO: use sub.metadata->o_size_no_padding
+				uint8_t buffer[msg_size];
 
 				/* if this topic has been updated, copy the new data into the message buffer
 				 * and write a message to the log
 				 */
-				//orb_check(sub.fd, &updated);	// check whether a non-multi topic has been updated
-				/* this works for both single and multi-instances */
 				for (uint8_t instance = 0; instance < ORB_MULTI_MAX_INSTANCES; instance++) {
-					if (copy_if_updated_multi(sub.metadata, instance, &sub.fd[instance], &orb_msg_padded, &sub.time_tried_subscribe)) {
+					if (copy_if_updated_multi(sub.metadata, instance, &sub.fd[instance], buffer + sizeof(message_data_header_s),
+							&sub.time_tried_subscribe)) {
 
-						struct orb_output_buffer output_buffer = {};
-						uint8_t buffer[sizeof(message_data_header_s) + padded_msg_size];
-						output_buffer.data = &buffer;
-						output_buffer.next = sizeof(message_data_header_s);
-
-						sub.metadata->serialize(&orb_msg_padded, &output_buffer);
-						size_t msg_size = output_buffer.next;
-
-						//uint64_t timestamp;
-						//memcpy(&timestamp, buffer + sizeof(message_data_header_s), sizeof(timestamp));
-						//warnx("topic: %s, instance: %d, timestamp: %llu",
-						//		sub.metadata->o_name, instance, timestamp);
-
-						/* copy the current topic data into the buffer after the header */
-						//orb_copy(sub.metadata, sub.fd, buffer + sizeof(message_data_header_s));
-
-						/* fill the message header struct in-place at the front of the buffer,
-						 * accessing the unaligned (packed) structure properly
-						 */
 						message_data_header_s *header = reinterpret_cast<message_data_header_s *>(buffer);
 						header->msg_type = static_cast<uint8_t>(MessageType::DATA);
-						/* the ORB topic data object has 2 unused trailing bytes? */
-						//header->msg_size = static_cast<uint16_t>(msg_size - 3);
-						header->msg_size = msg_size;
+						header->msg_size = static_cast<uint16_t>(msg_size - 3);
 						header->msg_id = msg_id;
 						header->multi_id = 0x80 + instance;	// Non multi, active
 
diff --git a/src/modules/uORB/uORB.h b/src/modules/uORB/uORB.h
index 1b06a2ad89..fba37a57af 100644
--- a/src/modules/uORB/uORB.h
+++ b/src/modules/uORB/uORB.h
@@ -46,21 +46,14 @@
 // Hack until everything is using this header
 #include <systemlib/visibility.h>
 
-struct orb_output_buffer {
-	void *data;
-	size_t next;
-};
-
-typedef void (*func_ptr)(void *in, struct orb_output_buffer *out);
-
 /**
  * Object metadata.
  */
 struct orb_metadata {
 	const char *o_name;		/**< unique object name */
 	const size_t o_size;		/**< object size */
-	func_ptr serialize;			/**< serialization function for this orb topic */
-	const char *o_fields;		/**< semicolon separated list of fields */
+	const size_t o_size_no_padding;	/**< object size w/o padding at the end (for logger) */
+	const char *o_fields;		/**< semicolon separated list of fields (with type) */
 };
 
 typedef const struct orb_metadata *orb_id_t;
@@ -119,14 +112,14 @@ enum ORB_PRIO {
  *
  * @param _name		The name of the topic.
  * @param _struct	The structure the topic provides.
- * @param _func		The pointer to a function that serializes this topic
+ * @param _size_no_padding	Struct size w/o padding at the end
  * @param _fields	All fields in a semicolon separated list e.g: "float[3] position;bool armed"
  */
-#define ORB_DEFINE(_name, _struct, _func, _fields)			\
+#define ORB_DEFINE(_name, _struct, _size_no_padding, _fields)		\
 	const struct orb_metadata __orb_##_name = {	\
 		#_name,					\
 		sizeof(_struct),		\
-		_func,					\
+		_size_no_padding,			\
 		_fields					\
 	}; struct hack
 
diff --git a/src/modules/uORB/uORB_tests/uORBTest_UnitTest.hpp b/src/modules/uORB/uORB_tests/uORBTest_UnitTest.hpp
index 6f5012cec3..5962b62d09 100644
--- a/src/modules/uORB/uORB_tests/uORBTest_UnitTest.hpp
+++ b/src/modules/uORB/uORB_tests/uORBTest_UnitTest.hpp
@@ -41,8 +41,8 @@ struct orb_test {
 	int val;
 	hrt_abstime time;
 };
-ORB_DEFINE(orb_test, struct orb_test, nullptr, "ORB_TEST:int val;hrt_abstime time;");
-ORB_DEFINE(orb_multitest, struct orb_test, nullptr, "ORB_MULTITEST:int val;hrt_abstime time;");
+ORB_DEFINE(orb_test, struct orb_test, sizeof(orb_test), "ORB_TEST:int val;hrt_abstime time;");
+ORB_DEFINE(orb_multitest, struct orb_test, sizeof(orb_test), "ORB_MULTITEST:int val;hrt_abstime time;");
 
 
 struct orb_test_medium {
@@ -50,9 +50,10 @@ struct orb_test_medium {
 	hrt_abstime time;
 	char junk[64];
 };
-ORB_DEFINE(orb_test_medium, struct orb_test_medium, nullptr, "ORB_TEST_MEDIUM:int val;hrt_abstime time;char[64] junk;");
-ORB_DEFINE(orb_test_medium_multi, struct orb_test_medium, nullptr,
-	   "ORB_TEST_MEDIUM_MULTI:int val;hrt_abstime time;char[64] junk;");
+ORB_DEFINE(orb_test_medium, struct orb_test_medium, sizeof(orb_test_medium),
+		"ORB_TEST_MEDIUM:int val;hrt_abstime time;char[64] junk;");
+ORB_DEFINE(orb_test_medium_multi, struct orb_test_medium, sizeof(orb_test_medium),
+		"ORB_TEST_MEDIUM_MULTI:int val;hrt_abstime time;char[64] junk;");
 
 
 struct orb_test_large {
@@ -60,7 +61,8 @@ struct orb_test_large {
 	hrt_abstime time;
 	char junk[512];
 };
-ORB_DEFINE(orb_test_large, struct orb_test_large, nullptr, "ORB_TEST_LARGE:int val;hrt_abstime time;char[512] junk;");
+ORB_DEFINE(orb_test_large, struct orb_test_large, sizeof(orb_test_large),
+		"ORB_TEST_LARGE:int val;hrt_abstime time;char[512] junk;");
 
 
 namespace uORBTest

From 1edf03767a5a2c2d6da5439f4546297da554ed9e Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Beat=20K=C3=BCng?= <beat-kueng@gmx.net>
Date: Thu, 5 May 2016 14:10:40 +0200
Subject: [PATCH 0167/1230] logger: KB/s -> kB/s

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

diff --git a/src/modules/logger/logger.cpp b/src/modules/logger/logger.cpp
index 9ba2543ead..9eb8ce68b0 100644
--- a/src/modules/logger/logger.cpp
+++ b/src/modules/logger/logger.cpp
@@ -581,7 +581,7 @@ void Logger::run()
 			if (deltat > 4.0) {
 				alloc_info = mallinfo();
 				double throughput = total_bytes / deltat;
-				PX4_INFO("%8.1lf KB/s, %zu highWater,  %d dropouts, %5.3f sec max, free heap: %d",
+				PX4_INFO("%8.1lf kB/s, %zu highWater,  %d dropouts, %5.3f sec max, free heap: %d",
 					 throughput / 1.e3, highWater, dropout_count, max_drop_len, alloc_info.fordblks);
 
 				total_bytes = 0;

From 3de7fbb0a92616c657ac2e29d9b55f2de26971e6 Mon Sep 17 00:00:00 2001
From: Daniel Agar <daniel@agar.ca>
Date: Thu, 5 May 2016 10:17:40 -0400
Subject: [PATCH 0168/1230] logger and uORBTest_UnitTest astyle

---
 src/modules/logger/logger.cpp                     | 2 +-
 src/modules/uORB/uORB_tests/uORBTest_UnitTest.hpp | 6 +++---
 2 files changed, 4 insertions(+), 4 deletions(-)

diff --git a/src/modules/logger/logger.cpp b/src/modules/logger/logger.cpp
index 9eb8ce68b0..f2da479d4b 100644
--- a/src/modules/logger/logger.cpp
+++ b/src/modules/logger/logger.cpp
@@ -504,7 +504,7 @@ void Logger::run()
 				 */
 				for (uint8_t instance = 0; instance < ORB_MULTI_MAX_INSTANCES; instance++) {
 					if (copy_if_updated_multi(sub.metadata, instance, &sub.fd[instance], buffer + sizeof(message_data_header_s),
-							&sub.time_tried_subscribe)) {
+								  &sub.time_tried_subscribe)) {
 
 						message_data_header_s *header = reinterpret_cast<message_data_header_s *>(buffer);
 						header->msg_type = static_cast<uint8_t>(MessageType::DATA);
diff --git a/src/modules/uORB/uORB_tests/uORBTest_UnitTest.hpp b/src/modules/uORB/uORB_tests/uORBTest_UnitTest.hpp
index 5962b62d09..cf81d36e23 100644
--- a/src/modules/uORB/uORB_tests/uORBTest_UnitTest.hpp
+++ b/src/modules/uORB/uORB_tests/uORBTest_UnitTest.hpp
@@ -51,9 +51,9 @@ struct orb_test_medium {
 	char junk[64];
 };
 ORB_DEFINE(orb_test_medium, struct orb_test_medium, sizeof(orb_test_medium),
-		"ORB_TEST_MEDIUM:int val;hrt_abstime time;char[64] junk;");
+	   "ORB_TEST_MEDIUM:int val;hrt_abstime time;char[64] junk;");
 ORB_DEFINE(orb_test_medium_multi, struct orb_test_medium, sizeof(orb_test_medium),
-		"ORB_TEST_MEDIUM_MULTI:int val;hrt_abstime time;char[64] junk;");
+	   "ORB_TEST_MEDIUM_MULTI:int val;hrt_abstime time;char[64] junk;");
 
 
 struct orb_test_large {
@@ -62,7 +62,7 @@ struct orb_test_large {
 	char junk[512];
 };
 ORB_DEFINE(orb_test_large, struct orb_test_large, sizeof(orb_test_large),
-		"ORB_TEST_LARGE:int val;hrt_abstime time;char[512] junk;");
+	   "ORB_TEST_LARGE:int val;hrt_abstime time;char[512] junk;");
 
 
 namespace uORBTest

From 4ce658ab99992c3988422f04a1c7c56f0ea84e0a Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Beat=20K=C3=BCng?= <beat-kueng@gmx.net>
Date: Fri, 6 May 2016 09:31:15 +0200
Subject: [PATCH 0169/1230] logger: move _writer.lock() call after
 write_changed_parameters()

write_changed_parameters() also takes the lock and thus would deadlock
otherwise.
---
 src/modules/logger/logger.cpp | 5 +++--
 1 file changed, 3 insertions(+), 2 deletions(-)

diff --git a/src/modules/logger/logger.cpp b/src/modules/logger/logger.cpp
index f2da479d4b..7e4366b876 100644
--- a/src/modules/logger/logger.cpp
+++ b/src/modules/logger/logger.cpp
@@ -476,8 +476,6 @@ void Logger::run()
 		}
 
 		if (_enabled) {
-			/* wait for lock on log buffer */
-			_writer.lock();
 
 			bool data_written = false;
 
@@ -492,6 +490,9 @@ void Logger::run()
 			// Write data messages for normal subscriptions
 			int msg_id = 0;
 
+			/* wait for lock on log buffer */
+			_writer.lock();
+
 			for (LoggerSubscription &sub : _subscriptions) {
 				/* each message consists of a header followed by an orb data object
 				 */

From 9a02dbdd66a1554c6e1e3243f0d89124c0a57920 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Beat=20K=C3=BCng?= <beat-kueng@gmx.net>
Date: Fri, 6 May 2016 11:37:29 +0200
Subject: [PATCH 0170/1230] logger: extend status ouput, disable DBGPRINT for
 now

---
 src/modules/logger/log_writer.h |  5 ++
 src/modules/logger/logger.cpp   | 97 +++++++++++++--------------------
 src/modules/logger/logger.h     |  8 +++
 3 files changed, 51 insertions(+), 59 deletions(-)

diff --git a/src/modules/logger/log_writer.h b/src/modules/logger/log_writer.h
index 45106b9ddf..1b6204fe69 100644
--- a/src/modules/logger/log_writer.h
+++ b/src/modules/logger/log_writer.h
@@ -54,6 +54,11 @@ public:
 		pthread_cond_broadcast(&_cv);
 	}
 
+	size_t get_total_written() const
+	{
+		return _total_written;
+	}
+
 private:
 	static void *run_helper(void *);
 
diff --git a/src/modules/logger/logger.cpp b/src/modules/logger/logger.cpp
index 7e4366b876..558271f2fe 100644
--- a/src/modules/logger/logger.cpp
+++ b/src/modules/logger/logger.cpp
@@ -10,7 +10,7 @@
 #include <px4_getopt.h>
 #include <px4_log.h>
 
-#define DBGPRINT
+//#define DBGPRINT //write status output every few seconds
 
 #if defined(DBGPRINT)
 // needed for mallinfo
@@ -125,17 +125,20 @@ int Logger::start(char *const *argv)
 void Logger::status()
 {
 	if (!_enabled) {
-		PX4_INFO("running, but not logging");
+		PX4_INFO("Running, but not logging");
 
 	} else {
-		PX4_INFO("running");
+		PX4_INFO("Running");
 
-//		float kibibytes = _writer.get_total_written() / 1024.0f;
-//		float mebibytes = kibibytes / 1024.0f;
-//		float seconds = ((float)(hrt_absolute_time() - _start_time)) / 1000000.0f;
-//
-//		PX4_WARN("wrote %lu msgs, %4.2f MiB (average %5.3f KiB/s), skipped %lu msgs", log_msgs_written, (double)mebibytes, (double)(kibibytes / seconds), log_msgs_skipped);
-//		mavlink_log_info(&mavlink_log_pub, "[blackbox] wrote %lu msgs, skipped %lu msgs", log_msgs_written, log_msgs_skipped);
+		float kibibytes = _writer.get_total_written() / 1024.0f;
+		float mebibytes = kibibytes / 1024.0f;
+		float seconds = ((float)(hrt_absolute_time() - _start_time)) / 1000000.0f;
+
+		PX4_INFO("Wrote %4.2f MiB (avg %5.2f KiB/s)", (double)mebibytes, (double)(kibibytes / seconds));
+		PX4_INFO("Dropouts: %zu (max len: %.3f s), max used buffer: %zu / %zu B",
+			 _write_dropouts, (double)_max_dropout_duration, _high_water, _writer._buffer_size);
+		_high_water = 0;
+		_max_dropout_duration = 0.f;
 	}
 }
 
@@ -194,16 +197,11 @@ void Logger::run_trampoline(int argc, char *argv[])
 		return;
 	}
 
-#if defined(DBGPRINT) && defined(__PX4_NUTTX)
-	struct mallinfo alloc_info = mallinfo();
-	warnx("largest free chunk: %d bytes", alloc_info.mxordblk);
-	warnx("allocating %d bytes for log_buffer", log_buffer_size);
-#endif /* DBGPRINT */
-
 	logger_ptr = new Logger(log_buffer_size, log_interval, log_on_start);
 
 #if defined(DBGPRINT) && defined(__PX4_NUTTX)
-	alloc_info = mallinfo();
+	struct mallinfo alloc_info = mallinfo();
+	warnx("largest free chunk: %d bytes", alloc_info.mxordblk);
 	warnx("remaining free heap: %d bytes", alloc_info.fordblks);
 #endif /* DBGPRINT */
 
@@ -436,13 +434,8 @@ void Logger::run()
 	_task_should_exit = false;
 
 #ifdef DBGPRINT
-	hrt_abstime	dropout_start = 0;
 	hrt_abstime	timer_start = 0;
 	uint32_t	total_bytes = 0;
-	uint16_t	dropout_count = 0;
-	size_t 		highWater = 0;
-	size_t		available = 0;
-	double		max_drop_len = 0;
 #endif /* DBGPRINT */
 
 	// we start logging immediately
@@ -515,51 +508,32 @@ void Logger::run()
 
 						//PX4_INFO("topic: %s, size = %zu, out_size = %zu", sub.metadata->o_name, sub.metadata->o_size, msg_size);
 
-#ifdef DBGPRINT
-						//warnx("subscription %s updated: %d, size: %d", sub.metadata->o_name, updated, msg_size);
-						hrt_abstime trytime = hrt_absolute_time();
-
-						if (_writer._count > highWater) {
-							highWater = _writer._count;
-						}
-
-#endif /* DBGPRINT */
-
 						if (_writer.write(buffer, msg_size)) {
 
 #ifdef DBGPRINT
-
-							// successful write: note end of dropout if dropout_start != 0
-							if (dropout_start != 0) {
-								double drop_len = (double)(trytime - dropout_start)  * 1e-6;
-
-								if (drop_len > max_drop_len) {
-									max_drop_len = drop_len;
-								}
-
-								PX4_WARN("dropout length: %5.3f seconds", drop_len);
-								dropout_start = 0;
-								highWater = 0;
-							}
-
 							total_bytes += msg_size;
 #endif /* DBGPRINT */
 
+							if (_dropout_start) {
+								float dropout_duration = (float)(hrt_elapsed_time(&_dropout_start) / 1000) / 1.e3f;
+
+								if (dropout_duration > _max_dropout_duration) {
+									_max_dropout_duration = dropout_duration;
+								}
+
+								_dropout_start = 0;
+							}
+
 							data_written = true;
 
 						} else {
 
-#ifdef DBGPRINT
-
-							if (dropout_start == 0)	{
-								available = _writer._count;
-								PX4_WARN("dropout, available: %zu/%zu", available, _writer._buffer_size);
-								dropout_start = trytime;
-								dropout_count++;
+							if (!_dropout_start) {
+								_dropout_start = hrt_absolute_time();
+								++_write_dropouts;
+								_high_water = 0;
 							}
 
-#endif /* DBGPRINT */
-
 							break;	// Write buffer overflow, skip this record
 						}
 					}
@@ -568,6 +542,10 @@ void Logger::run()
 				msg_id++;
 			}
 
+			if (!_dropout_start && _writer._count > _high_water) {
+				_high_water = _writer._count;
+			}
+
 			/* release the log buffer */
 			_writer.unlock();
 
@@ -582,13 +560,13 @@ void Logger::run()
 			if (deltat > 4.0) {
 				alloc_info = mallinfo();
 				double throughput = total_bytes / deltat;
-				PX4_INFO("%8.1lf kB/s, %zu highWater,  %d dropouts, %5.3f sec max, free heap: %d",
-					 throughput / 1.e3, highWater, dropout_count, max_drop_len, alloc_info.fordblks);
+				PX4_INFO("%8.1f kB/s, %zu highWater,  %d dropouts, %5.3f sec max, free heap: %d",
+					 throughput / 1.e3, _high_water, _write_dropouts, (double)_max_dropout_duration,
+					 alloc_info.fordblks);
 
+				_high_water = 0;
+				_max_dropout_duration = 0.f;
 				total_bytes = 0;
-				highWater = 0,
-				dropout_count = 0;
-				max_drop_len = 0;
 				timer_start = hrt_absolute_time();
 			}
 
@@ -702,6 +680,7 @@ void Logger::start_log()
 	write_parameters();
 	_writer.notify();
 	_enabled = true;
+	_start_time = hrt_absolute_time();
 }
 
 void Logger::stop_log()
diff --git a/src/modules/logger/logger.h b/src/modules/logger/logger.h
index 3309ebcb1d..f37ea4401d 100644
--- a/src/modules/logger/logger.h
+++ b/src/modules/logger/logger.h
@@ -98,6 +98,14 @@ private:
 	uORB::Subscription<vehicle_status_s>	_vehicle_status_sub {ORB_ID(vehicle_status)};
 	uORB::Subscription<parameter_update_s>	_parameter_update_sub {ORB_ID(parameter_update)};
 	bool						_enabled = false;
+
+	// statistics
+	hrt_abstime					_start_time; ///< Time when logging started (not the logger thread)
+	hrt_abstime					_dropout_start = 0; ///< start of current dropout (0 = no dropout)
+	float						_max_dropout_duration = 0.f; ///< max duration of dropout [s]
+	size_t						_write_dropouts = 0; ///< failed buffer writes due to buffer overflow
+	size_t						_high_water = 0; ///< maximum used write buffer
+
 	bool 						_log_on_start;
 	Array<LoggerSubscription, MAX_TOPICS_NUM>	_subscriptions;
 	LogWriter					_writer;

From 7d42a648f0900535484f2eaffddf42e169b987d7 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Beat=20K=C3=BCng?= <beat-kueng@gmx.net>
Date: Fri, 6 May 2016 11:47:13 +0200
Subject: [PATCH 0171/1230] logger: make sure the buffer is at least 300B
 larger than _min_write_chunk

We always write larger chunks (orb messages) to the buffer, so the buffer
needs to be larger than the minimum write chunk
---
 src/modules/logger/log_writer.cpp | 4 +++-
 1 file changed, 3 insertions(+), 1 deletion(-)

diff --git a/src/modules/logger/log_writer.cpp b/src/modules/logger/log_writer.cpp
index 5c96a9518a..7804195d48 100644
--- a/src/modules/logger/log_writer.cpp
+++ b/src/modules/logger/log_writer.cpp
@@ -12,7 +12,9 @@ constexpr size_t LogWriter::_min_write_chunk;
 
 
 LogWriter::LogWriter(size_t buffer_size) :
-	_buffer_size(math::max(buffer_size, _min_write_chunk))
+	//We always write larger chunks (orb messages) to the buffer, so the buffer
+	//needs to be larger than the minimum write chunk (300 is somewhat arbitrary)
+	_buffer_size(math::max(buffer_size, _min_write_chunk + 300))
 {
 	pthread_mutex_init(&_mtx, nullptr);
 	pthread_cond_init(&_cv, nullptr);

From dde96dd4d7915246cfc6c67e07b1c4f09b31947f Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Beat=20K=C3=BCng?= <beat-kueng@gmx.net>
Date: Fri, 6 May 2016 13:12:50 +0200
Subject: [PATCH 0172/1230] logger: allocate _vehicle_status_sub &
 _parameter_update_sub on the logger thread

This makes sure the file descriptors are closed in the right thread.
Before on NuttX, when stopping the logger, orb unsubscribe failed due to
this.
---
 src/modules/logger/logger.cpp | 34 +++++++++++++++++++++++++---------
 src/modules/logger/logger.h   |  4 ++--
 2 files changed, 27 insertions(+), 11 deletions(-)

diff --git a/src/modules/logger/logger.cpp b/src/modules/logger/logger.cpp
index 558271f2fe..c7ee20a138 100644
--- a/src/modules/logger/logger.cpp
+++ b/src/modules/logger/logger.cpp
@@ -385,18 +385,31 @@ void Logger::run()
 	struct mallinfo alloc_info = {};
 #endif /* DBGPRINT */
 
-	PX4_WARN("logger started");
+	PX4_INFO("logger started");
 
 	int mkdir_ret = mkdir(LOG_ROOT, S_IRWXU | S_IRWXG | S_IRWXO);
 
 	if (mkdir_ret == 0) {
-		PX4_WARN("log root dir created: %s", LOG_ROOT);
+		PX4_INFO("log root dir created: %s", LOG_ROOT);
 
 	} else if (errno != EEXIST) {
-		PX4_WARN("failed creating log root dir: %s", LOG_ROOT);
+		PX4_ERR("failed creating log root dir: %s", LOG_ROOT);
 		return;
 	}
 
+	if (!(_vehicle_status_sub = new uORB::Subscription<vehicle_status_s>(ORB_ID(vehicle_status)))) {
+		PX4_ERR("Failed to allocate subscription");
+		return;
+	}
+
+	if (!(_parameter_update_sub = new uORB::Subscription<parameter_update_s>(ORB_ID(parameter_update)))) {
+		delete _vehicle_status_sub;
+		_vehicle_status_sub = nullptr;
+		PX4_ERR("Failed to allocate subscription");
+		return;
+	}
+
+
 	add_topic("sensor_gyro", 0);
 	add_topic("sensor_accel", 0);
 	add_topic("vehicle_rates_setpoint", 10);
@@ -448,10 +461,10 @@ void Logger::run()
 	while (!_task_should_exit) {
 
 		// Start/stop logging when system arm/disarm
-		if (_vehicle_status_sub.check_updated()) {
-			_vehicle_status_sub.update();
-			bool armed = (_vehicle_status_sub.get().arming_state == vehicle_status_s::ARMING_STATE_ARMED) ||
-				     (_vehicle_status_sub.get().arming_state == vehicle_status_s::ARMING_STATE_ARMED_ERROR);
+		if (_vehicle_status_sub->check_updated()) {
+			_vehicle_status_sub->update();
+			bool armed = (_vehicle_status_sub->get().arming_state == vehicle_status_s::ARMING_STATE_ARMED) ||
+				     (_vehicle_status_sub->get().arming_state == vehicle_status_s::ARMING_STATE_ARMED_ERROR);
 
 			if (_enabled != armed && !_log_on_start) {
 				if (armed) {
@@ -474,9 +487,9 @@ void Logger::run()
 
 			/* Check if parameters have changed */
 			// this needs to change to a timestamped record to record a history of parameter changes
-			if (_parameter_update_sub.check_updated()) {
+			if (_parameter_update_sub->check_updated()) {
 				warnx("parameter update");
-				_parameter_update_sub.update();
+				_parameter_update_sub->update();
 				write_changed_parameters();
 			}
 
@@ -577,6 +590,9 @@ void Logger::run()
 		usleep(_log_interval);
 	}
 
+	delete _vehicle_status_sub;
+	delete _parameter_update_sub;
+
 	// stop the writer thread
 	_writer.thread_stop();
 
diff --git a/src/modules/logger/logger.h b/src/modules/logger/logger.h
index f37ea4401d..0fc557a715 100644
--- a/src/modules/logger/logger.h
+++ b/src/modules/logger/logger.h
@@ -95,8 +95,8 @@ private:
 
 	bool						_task_should_exit = true;
 	char 						_log_dir[64];
-	uORB::Subscription<vehicle_status_s>	_vehicle_status_sub {ORB_ID(vehicle_status)};
-	uORB::Subscription<parameter_update_s>	_parameter_update_sub {ORB_ID(parameter_update)};
+	uORB::Subscription<vehicle_status_s>	*_vehicle_status_sub = nullptr;
+	uORB::Subscription<parameter_update_s>	*_parameter_update_sub = nullptr;
 	bool						_enabled = false;
 
 	// statistics

From 04f301619f68aab400556132fb1cb6ed6331c8d8 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Beat=20K=C3=BCng?= <beat-kueng@gmx.net>
Date: Fri, 6 May 2016 13:18:38 +0200
Subject: [PATCH 0173/1230] refactor LogWriter: remove friend class Logger and
 use the public interface

---
 src/modules/logger/log_writer.h | 11 ++++++++++-
 src/modules/logger/logger.cpp   |  6 +++---
 2 files changed, 13 insertions(+), 4 deletions(-)

diff --git a/src/modules/logger/log_writer.h b/src/modules/logger/log_writer.h
index 1b6204fe69..82be6374e7 100644
--- a/src/modules/logger/log_writer.h
+++ b/src/modules/logger/log_writer.h
@@ -13,7 +13,6 @@ namespace logger
 
 class LogWriter
 {
-	friend class Logger;
 public:
 	LogWriter(size_t buffer_size);
 	~LogWriter();
@@ -59,6 +58,16 @@ public:
 		return _total_written;
 	}
 
+	size_t get_buffer_size() const
+	{
+		return _buffer_size;
+	}
+
+	size_t get_buffer_fill_count() const
+	{
+		return _count;
+	}
+
 private:
 	static void *run_helper(void *);
 
diff --git a/src/modules/logger/logger.cpp b/src/modules/logger/logger.cpp
index c7ee20a138..4b09adadcb 100644
--- a/src/modules/logger/logger.cpp
+++ b/src/modules/logger/logger.cpp
@@ -136,7 +136,7 @@ void Logger::status()
 
 		PX4_INFO("Wrote %4.2f MiB (avg %5.2f KiB/s)", (double)mebibytes, (double)(kibibytes / seconds));
 		PX4_INFO("Dropouts: %zu (max len: %.3f s), max used buffer: %zu / %zu B",
-			 _write_dropouts, (double)_max_dropout_duration, _high_water, _writer._buffer_size);
+			 _write_dropouts, (double)_max_dropout_duration, _high_water, _writer.get_buffer_size());
 		_high_water = 0;
 		_max_dropout_duration = 0.f;
 	}
@@ -555,8 +555,8 @@ void Logger::run()
 				msg_id++;
 			}
 
-			if (!_dropout_start && _writer._count > _high_water) {
-				_high_water = _writer._count;
+			if (!_dropout_start && _writer.get_buffer_fill_count() > _high_water) {
+				_high_water = _writer.get_buffer_fill_count();
 			}
 
 			/* release the log buffer */

From d5dcbf01d0d4f8d3434ff4745c50a485ca948e2e Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Beat=20K=C3=BCng?= <beat-kueng@gmx.net>
Date: Fri, 6 May 2016 13:36:42 +0200
Subject: [PATCH 0174/1230] logger: add copyright to files

---
 src/modules/logger/CMakeLists.txt |  2 +-
 src/modules/logger/array.h        | 33 +++++++++++++++++++++++++++++++
 src/modules/logger/log_writer.cpp | 33 +++++++++++++++++++++++++++++++
 src/modules/logger/log_writer.h   | 33 +++++++++++++++++++++++++++++++
 src/modules/logger/logger.cpp     | 33 +++++++++++++++++++++++++++++++
 src/modules/logger/logger.h       | 33 +++++++++++++++++++++++++++++++
 6 files changed, 166 insertions(+), 1 deletion(-)

diff --git a/src/modules/logger/CMakeLists.txt b/src/modules/logger/CMakeLists.txt
index 577f9f1be0..31e3f05884 100644
--- a/src/modules/logger/CMakeLists.txt
+++ b/src/modules/logger/CMakeLists.txt
@@ -1,6 +1,6 @@
 ############################################################################
 #
-#   Copyright (c) 2015 PX4 Development Team. All rights reserved.
+#   Copyright (c) 2016 PX4 Development Team. All rights reserved.
 #
 # Redistribution and use in source and binary forms, with or without
 # modification, are permitted provided that the following conditions
diff --git a/src/modules/logger/array.h b/src/modules/logger/array.h
index 78862ff072..9ac1d00062 100644
--- a/src/modules/logger/array.h
+++ b/src/modules/logger/array.h
@@ -1,3 +1,36 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2016 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
 #pragma once
 
 #include <stdlib.h>
diff --git a/src/modules/logger/log_writer.cpp b/src/modules/logger/log_writer.cpp
index 7804195d48..f4ade4b1b7 100644
--- a/src/modules/logger/log_writer.cpp
+++ b/src/modules/logger/log_writer.cpp
@@ -1,3 +1,36 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2016 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
 #include "log_writer.h"
 #include <fcntl.h>
 #include <string.h>
diff --git a/src/modules/logger/log_writer.h b/src/modules/logger/log_writer.h
index 82be6374e7..7ddac85d98 100644
--- a/src/modules/logger/log_writer.h
+++ b/src/modules/logger/log_writer.h
@@ -1,3 +1,36 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2016 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
 #pragma once
 
 #include <px4.h>
diff --git a/src/modules/logger/logger.cpp b/src/modules/logger/logger.cpp
index 4b09adadcb..93e1603a40 100644
--- a/src/modules/logger/logger.cpp
+++ b/src/modules/logger/logger.cpp
@@ -1,3 +1,36 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2016 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
 #include "logger.h"
 
 #include <sys/stat.h>
diff --git a/src/modules/logger/logger.h b/src/modules/logger/logger.h
index 0fc557a715..e8470dfad6 100644
--- a/src/modules/logger/logger.h
+++ b/src/modules/logger/logger.h
@@ -1,3 +1,36 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2016 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
 #pragma once
 
 #include "log_writer.h"

From 797d0f24d6597c5695a3610db35d9803d390657b Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Beat=20K=C3=BCng?= <beat-kueng@gmx.net>
Date: Fri, 6 May 2016 13:48:12 +0200
Subject: [PATCH 0175/1230] reformat orb message templates

---
 msg/templates/uorb/msg.cpp.template | 128 ++++++++--------
 msg/templates/uorb/msg.h.template   | 218 ++++++++++++++--------------
 2 files changed, 173 insertions(+), 173 deletions(-)

diff --git a/msg/templates/uorb/msg.cpp.template b/msg/templates/uorb/msg.cpp.template
index c9bf11bb85..922ce3122d 100644
--- a/msg/templates/uorb/msg.cpp.template
+++ b/msg/templates/uorb/msg.cpp.template
@@ -59,33 +59,33 @@ uorb_struct = '%s_s'%spec.short_name
 topic_name = spec.short_name
 
 type_map = {
-  'int8': 'int8_t',
-  'int16': 'int16_t',
-  'int32': 'int32_t',
-  'int64': 'int64_t',
-  'uint8': 'uint8_t',
-  'uint16': 'uint16_t',
-  'uint32': 'uint32_t',
-  'uint64': 'uint64_t',
-  'float32': 'float',
-  'float64': 'double',
-  'bool': 'bool',
-  'char': 'char',
+    'int8': 'int8_t',
+    'int16': 'int16_t',
+    'int32': 'int32_t',
+    'int64': 'int64_t',
+    'uint8': 'uint8_t',
+    'uint16': 'uint16_t',
+    'uint32': 'uint32_t',
+    'uint64': 'uint64_t',
+    'float32': 'float',
+    'float64': 'double',
+    'bool': 'bool',
+    'char': 'char',
 }
 
 msgtype_size_map = {
-  'int8': 1,
-  'int16': 2,
-  'int32': 4,
-  'int64': 8,
-  'uint8': 1,
-  'uint16': 2,
-  'uint32': 4,
-  'uint64': 8,
-  'float32': 4,
-  'float64': 8,
-  'bool': 1,
-  'char': 1,
+    'int8': 1,
+    'int16': 2,
+    'int32': 4,
+    'int64': 8,
+    'uint8': 1,
+    'uint16': 2,
+    'uint32': 4,
+    'uint64': 8,
+    'float32': 4,
+    'float64': 8,
+    'bool': 1,
+    'char': 1,
 }
 
 def convert_type(spec_type):
@@ -126,47 +126,47 @@ def get_children_fields(base_type):
 # Add padding fields before the embedded types, at the end and calculate the
 # struct size
 def add_padding_bytes(fields):
-  struct_size = 8 # account for the timestamp
-  align_to = 8 # this is always 8, because of the 64bit timestamp
-  i = 0
-  padding_idx = 0
-  while i < len(fields):
-    field = fields[i]
-    if not field.is_header:
-        a_pos = field.type.find('[')
-        array_size = 1
-        if field.is_array:
-            array_size = field.array_len
-        if field.is_builtin:
-            field.sizeof_field_type = sizeof_field_type(field)
-        else:
-            # embedded type: may need to add padding
-            num_padding_bytes = align_to - (struct_size % align_to)
-            if num_padding_bytes != align_to:
-                padding_field = genmsg.Field('_padding'+str(padding_idx),
-                    'uint8['+str(num_padding_bytes)+']')
-                padding_idx += 1
-                padding_field.sizeof_field_type = 1
-                struct_size += num_padding_bytes
-                fields.insert(i, padding_field)
-                i += 1
-            children_fields = get_children_fields(field.base_type)
-            field.sizeof_field_type, unused = add_padding_bytes(children_fields)
-        struct_size += field.sizeof_field_type * array_size
-    i += 1
+    struct_size = 8 # account for the timestamp
+    align_to = 8 # this is always 8, because of the 64bit timestamp
+    i = 0
+    padding_idx = 0
+    while i < len(fields):
+        field = fields[i]
+        if not field.is_header:
+            a_pos = field.type.find('[')
+            array_size = 1
+            if field.is_array:
+                array_size = field.array_len
+            if field.is_builtin:
+                field.sizeof_field_type = sizeof_field_type(field)
+            else:
+                # embedded type: may need to add padding
+                num_padding_bytes = align_to - (struct_size % align_to)
+                if num_padding_bytes != align_to:
+                    padding_field = genmsg.Field('_padding'+str(padding_idx),
+                        'uint8['+str(num_padding_bytes)+']')
+                    padding_idx += 1
+                    padding_field.sizeof_field_type = 1
+                    struct_size += num_padding_bytes
+                    fields.insert(i, padding_field)
+                    i += 1
+                children_fields = get_children_fields(field.base_type)
+                field.sizeof_field_type, unused = add_padding_bytes(children_fields)
+            struct_size += field.sizeof_field_type * array_size
+        i += 1
 
-  # add padding at the end (necessary for embedded types)
-  num_padding_bytes = align_to - (struct_size % align_to)
-  if num_padding_bytes == align_to:
-      num_padding_bytes = 0
-  else:
-      padding_field = genmsg.Field('_padding'+str(padding_idx),
-          'uint8['+str(num_padding_bytes)+']')
-      padding_idx += 1
-      padding_field.sizeof_field_type = 1
-      struct_size += num_padding_bytes
-      fields.append(padding_field)
-  return (struct_size, num_padding_bytes)
+    # add padding at the end (necessary for embedded types)
+    num_padding_bytes = align_to - (struct_size % align_to)
+    if num_padding_bytes == align_to:
+        num_padding_bytes = 0
+    else:
+        padding_field = genmsg.Field('_padding'+str(padding_idx),
+                'uint8['+str(num_padding_bytes)+']')
+        padding_idx += 1
+        padding_field.sizeof_field_type = 1
+        struct_size += num_padding_bytes
+        fields.append(padding_field)
+    return (struct_size, num_padding_bytes)
 
 sorted_fields = sorted(spec.parsed_fields(), key=sizeof_field_type, reverse=True)
 struct_size, padding_end_size = add_padding_bytes(sorted_fields)
diff --git a/msg/templates/uorb/msg.h.template b/msg/templates/uorb/msg.h.template
index a165f51ad5..f625ce56b7 100644
--- a/msg/templates/uorb/msg.h.template
+++ b/msg/templates/uorb/msg.h.template
@@ -78,11 +78,11 @@ topic_name = spec.short_name
 @##############################
 @{
 for field in spec.parsed_fields():
-  if (not field.is_builtin):
-    if (not field.is_header):
-      (package, name) = genmsg.names.package_resource_name(field.base_type)
-      package = package or spec.package # convert '' to package
-      print('#include <uORB/topics/%s.h>'%(name))
+    if (not field.is_builtin):
+        if (not field.is_header):
+            (package, name) = genmsg.names.package_resource_name(field.base_type)
+            package = package or spec.package # convert '' to package
+            print('#include <uORB/topics/%s.h>'%(name))
 }@
 
 @# Constants c style
@@ -97,33 +97,33 @@ for field in spec.parsed_fields():
 @##############################
 @{
 type_map = {
-  'int8': 'int8_t',
-  'int16': 'int16_t',
-  'int32': 'int32_t',
-  'int64': 'int64_t',
-  'uint8': 'uint8_t',
-  'uint16': 'uint16_t',
-  'uint32': 'uint32_t',
-  'uint64': 'uint64_t',
-  'float32': 'float',
-  'float64': 'double',
-  'bool': 'bool',
-  'char': 'char',
+    'int8': 'int8_t',
+    'int16': 'int16_t',
+    'int32': 'int32_t',
+    'int64': 'int64_t',
+    'uint8': 'uint8_t',
+    'uint16': 'uint16_t',
+    'uint32': 'uint32_t',
+    'uint64': 'uint64_t',
+    'float32': 'float',
+    'float64': 'double',
+    'bool': 'bool',
+    'char': 'char',
 }
 
 msgtype_size_map = {
-  'int8': 1,
-  'int16': 2,
-  'int32': 4,
-  'int64': 8,
-  'uint8': 1,
-  'uint16': 2,
-  'uint32': 4,
-  'uint64': 8,
-  'float32': 4,
-  'float64': 8,
-  'bool': 1,
-  'char': 1,
+    'int8': 1,
+    'int16': 2,
+    'int32': 4,
+    'int64': 8,
+    'uint8': 1,
+    'uint16': 2,
+    'uint32': 4,
+    'uint64': 8,
+    'float32': 4,
+    'float64': 8,
+    'bool': 1,
+    'char': 1,
 }
 
 def bare_name(msg_type):
@@ -150,89 +150,89 @@ def get_children_fields(base_type):
 # Add padding fields before the embedded types, at the end and calculate the
 # struct size
 def add_padding_bytes(fields):
-  struct_size = 8 # account for the timestamp
-  align_to = 8 # this is always 8, because of the 64bit timestamp
-  i = 0
-  padding_idx = 0
-  while i < len(fields):
-    field = fields[i]
-    if not field.is_header:
-        a_pos = field.type.find('[')
-        array_size = 1
-        if field.is_array:
-            array_size = field.array_len
-        if field.is_builtin:
-            field.sizeof_field_type = sizeof_field_type(field)
-        else:
-            # embedded type: may need to add padding
-            num_padding_bytes = align_to - (struct_size % align_to)
-            if num_padding_bytes != align_to:
-                padding_field = genmsg.Field('_padding'+str(padding_idx),
-                    'uint8['+str(num_padding_bytes)+']')
-                padding_idx += 1
-                padding_field.sizeof_field_type = 1
-                struct_size += num_padding_bytes
-                fields.insert(i, padding_field)
-                i += 1
-            children_fields = get_children_fields(field.base_type)
-            field.sizeof_field_type, unused = add_padding_bytes(children_fields)
-        struct_size += field.sizeof_field_type * array_size
-    i += 1
+    struct_size = 8 # account for the timestamp
+    align_to = 8 # this is always 8, because of the 64bit timestamp
+    i = 0
+    padding_idx = 0
+    while i < len(fields):
+        field = fields[i]
+        if not field.is_header:
+            a_pos = field.type.find('[')
+            array_size = 1
+            if field.is_array:
+                array_size = field.array_len
+            if field.is_builtin:
+                field.sizeof_field_type = sizeof_field_type(field)
+            else:
+                # embedded type: may need to add padding
+                num_padding_bytes = align_to - (struct_size % align_to)
+                if num_padding_bytes != align_to:
+                    padding_field = genmsg.Field('_padding'+str(padding_idx),
+                        'uint8['+str(num_padding_bytes)+']')
+                    padding_idx += 1
+                    padding_field.sizeof_field_type = 1
+                    struct_size += num_padding_bytes
+                    fields.insert(i, padding_field)
+                    i += 1
+                children_fields = get_children_fields(field.base_type)
+                field.sizeof_field_type, unused = add_padding_bytes(children_fields)
+            struct_size += field.sizeof_field_type * array_size
+        i += 1
 
-  # add padding at the end (necessary for embedded types)
-  num_padding_bytes = align_to - (struct_size % align_to)
-  if num_padding_bytes == align_to:
-      num_padding_bytes = 0
-  else:
-      padding_field = genmsg.Field('_padding'+str(padding_idx),
-          'uint8['+str(num_padding_bytes)+']')
-      padding_idx += 1
-      padding_field.sizeof_field_type = 1
-      struct_size += num_padding_bytes
-      fields.append(padding_field)
-  return (struct_size, num_padding_bytes)
+    # add padding at the end (necessary for embedded types)
+    num_padding_bytes = align_to - (struct_size % align_to)
+    if num_padding_bytes == align_to:
+        num_padding_bytes = 0
+    else:
+        padding_field = genmsg.Field('_padding'+str(padding_idx),
+                'uint8['+str(num_padding_bytes)+']')
+        padding_idx += 1
+        padding_field.sizeof_field_type = 1
+        struct_size += num_padding_bytes
+        fields.append(padding_field)
+    return (struct_size, num_padding_bytes)
 
 # Function to print a standard ros type
 def print_field_def(field):
-  type_name = field.type
-  # detect embedded types
-  sl_pos = type_name.find('/')
-  type_appendix = ''
-  type_prefix = ''
-  if (sl_pos >= 0):
-    type_name = type_name[sl_pos + 1:]
-    type_prefix = 'struct '
-    type_appendix = '_s'
+    type_name = field.type
+    # detect embedded types
+    sl_pos = type_name.find('/')
+    type_appendix = ''
+    type_prefix = ''
+    if (sl_pos >= 0):
+        type_name = type_name[sl_pos + 1:]
+        type_prefix = 'struct '
+        type_appendix = '_s'
 
-  # detect arrays
-  a_pos = type_name.find('[')
-  array_size = ''
-  if (a_pos >= 0):
-    # field is array
-    array_size = type_name[a_pos:]
-    type_name = type_name[:a_pos]
+    # detect arrays
+    a_pos = type_name.find('[')
+    array_size = ''
+    if (a_pos >= 0):
+        # field is array
+        array_size = type_name[a_pos:]
+        type_name = type_name[:a_pos]
 
-  if type_name in type_map:
-    # need to add _t: int8 --> int8_t
-    type_px4 = type_map[type_name]
-  else:
-    type_px4 = type_name
+    if type_name in type_map:
+        # need to add _t: int8 --> int8_t
+        type_px4 = type_map[type_name]
+    else:
+        type_px4 = type_name
 
-  comment = ''
-  if field.name.startswith('_padding'):
-      comment = ' // required for logger'
+    comment = ''
+    if field.name.startswith('_padding'):
+        comment = ' // required for logger'
 
-  print('\t%s%s%s %s%s;%s'%(type_prefix, type_px4, type_appendix, field.name,
-      array_size, comment))
+    print('\t%s%s%s %s%s;%s'%(type_prefix, type_px4, type_appendix, field.name,
+                array_size, comment))
 
 def print_parsed_fields():
-  # sort fields (using a stable sort)
-  sorted_fields = sorted(spec.parsed_fields(), key=sizeof_field_type, reverse=True)
-  struct_size, padding_end_size = add_padding_bytes(sorted_fields)
-  # loop over all fields and print the type and name
-  for field in sorted_fields:
-    if (not field.is_header):
-      print_field_def(field)
+    # sort fields (using a stable sort)
+    sorted_fields = sorted(spec.parsed_fields(), key=sizeof_field_type, reverse=True)
+    struct_size, padding_end_size = add_padding_bytes(sorted_fields)
+    # loop over all fields and print the type and name
+    for field in sorted_fields:
+        if (not field.is_header):
+            print_field_def(field)
 }@
 
 #ifdef __cplusplus
@@ -249,14 +249,14 @@ struct @(uorb_struct) {
 @# Constants again c++-ified
 @{
 for constant in spec.constants:
-  type_name = constant.type
-  if type_name in type_map:
-    # need to add _t: int8 --> int8_t
-    type_px4 = type_map[type_name]
-  else:
-    raise Exception("Type {0} not supported, add to to template file!".format(type_name))
+    type_name = constant.type
+    if type_name in type_map:
+        # need to add _t: int8 --> int8_t
+        type_px4 = type_map[type_name]
+    else:
+        raise Exception("Type {0} not supported, add to to template file!".format(type_name))
 
-  print('\tstatic const %s %s = %s;'%(type_px4, constant.name, int(constant.val)))
+    print('\tstatic const %s %s = %s;'%(type_px4, constant.name, int(constant.val)))
 }
 #endif
 };

From 093eece29b039697ac5aa20ed1199876a1de902a Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Beat=20K=C3=BCng?= <beat-kueng@gmx.net>
Date: Fri, 6 May 2016 14:15:13 +0200
Subject: [PATCH 0176/1230] orb message templates: move common code into
 px_generate_uorb_topic_helper.py

This also greatly speeds up the generators.
---
 Tools/px_generate_uorb_topic_helper.py | 171 +++++++++++++++++++++++++
 msg/templates/uorb/msg.cpp.template    | 113 +---------------
 msg/templates/uorb/msg.h.template      | 131 +------------------
 3 files changed, 175 insertions(+), 240 deletions(-)
 create mode 100644 Tools/px_generate_uorb_topic_helper.py

diff --git a/Tools/px_generate_uorb_topic_helper.py b/Tools/px_generate_uorb_topic_helper.py
new file mode 100644
index 0000000000..9a28b8a2eb
--- /dev/null
+++ b/Tools/px_generate_uorb_topic_helper.py
@@ -0,0 +1,171 @@
+
+# helper methods & common code for the uorb message templates msg.{cpp,h}.template
+
+# Another positive effect of having the code here, is that this file will get
+# precompiled and thus message generation will be much faster
+
+
+import genmsg.msgs
+import gencpp
+
+type_map = {
+    'int8': 'int8_t',
+    'int16': 'int16_t',
+    'int32': 'int32_t',
+    'int64': 'int64_t',
+    'uint8': 'uint8_t',
+    'uint16': 'uint16_t',
+    'uint32': 'uint32_t',
+    'uint64': 'uint64_t',
+    'float32': 'float',
+    'float64': 'double',
+    'bool': 'bool',
+    'char': 'char',
+}
+
+msgtype_size_map = {
+    'int8': 1,
+    'int16': 2,
+    'int32': 4,
+    'int64': 8,
+    'uint8': 1,
+    'uint16': 2,
+    'uint32': 4,
+    'uint64': 8,
+    'float32': 4,
+    'float64': 8,
+    'bool': 1,
+    'char': 1,
+}
+
+
+def bare_name(msg_type):
+    """
+    Get bare_name from <dir>/<bare_name>[x] format
+    """
+    bare = msg_type
+    if '/' in msg_type:
+        # removing prefix
+        bare = (msg_type.split('/'))[1]
+    # removing suffix
+    return bare.split('[')[0]
+
+
+def sizeof_field_type(field):
+    """
+    Get size of a field, used for sorting
+    """
+    bare_name_str = bare_name(field.type)
+    if bare_name_str in msgtype_size_map:
+        return msgtype_size_map[bare_name_str]
+    return 0 # this is for non-builtin types: sort them at the end
+
+def get_children_fields(base_type, search_path):
+    (package, name) = genmsg.names.package_resource_name(base_type)
+    tmp_msg_context = genmsg.msg_loader.MsgContext.create_default()
+    spec_temp = genmsg.msg_loader.load_msg_by_type(tmp_msg_context, '%s/%s' %(package, name), search_path)  
+    sorted_fields = sorted(spec_temp.parsed_fields(), key=sizeof_field_type, reverse=True)
+    return sorted_fields
+
+def add_padding_bytes(fields, search_path):
+    """
+    Add padding fields before the embedded types, at the end and calculate the
+    struct size
+    returns a tuple with the struct size and padding at the end
+    """
+    struct_size = 8 # account for the timestamp
+    align_to = 8 # this is always 8, because of the 64bit timestamp
+    i = 0
+    padding_idx = 0
+    while i < len(fields):
+        field = fields[i]
+        if not field.is_header:
+            a_pos = field.type.find('[')
+            array_size = 1
+            if field.is_array:
+                array_size = field.array_len
+            if field.is_builtin:
+                field.sizeof_field_type = sizeof_field_type(field)
+            else:
+                # embedded type: may need to add padding
+                num_padding_bytes = align_to - (struct_size % align_to)
+                if num_padding_bytes != align_to:
+                    padding_field = genmsg.Field('_padding'+str(padding_idx),
+                        'uint8['+str(num_padding_bytes)+']')
+                    padding_idx += 1
+                    padding_field.sizeof_field_type = 1
+                    struct_size += num_padding_bytes
+                    fields.insert(i, padding_field)
+                    i += 1
+                children_fields = get_children_fields(field.base_type, search_path)
+                field.sizeof_field_type, unused = add_padding_bytes(children_fields,
+                        search_path)
+            struct_size += field.sizeof_field_type * array_size
+        i += 1
+
+    # add padding at the end (necessary for embedded types)
+    num_padding_bytes = align_to - (struct_size % align_to)
+    if num_padding_bytes == align_to:
+        num_padding_bytes = 0
+    else:
+        padding_field = genmsg.Field('_padding'+str(padding_idx),
+                'uint8['+str(num_padding_bytes)+']')
+        padding_idx += 1
+        padding_field.sizeof_field_type = 1
+        struct_size += num_padding_bytes
+        fields.append(padding_field)
+    return (struct_size, num_padding_bytes)
+
+
+def convert_type(spec_type):
+    """
+    Convert from msg type to C type
+    """
+    bare_type = spec_type
+    if '/' in spec_type:
+        # removing prefix
+        bare_type = (spec_type.split('/'))[1]
+
+    msg_type, is_array, array_length = genmsg.msgs.parse_type(bare_type)
+    c_type = msg_type
+    if msg_type in type_map:
+        c_type = type_map[msg_type]
+    if is_array:
+        return c_type + "[" + str(array_length) + "]"
+    return c_type
+
+
+def print_field_def(field):
+    """
+    Print the C type from a field
+    """
+    type_name = field.type
+    # detect embedded types
+    sl_pos = type_name.find('/')
+    type_appendix = ''
+    type_prefix = ''
+    if (sl_pos >= 0):
+        type_name = type_name[sl_pos + 1:]
+        type_prefix = 'struct '
+        type_appendix = '_s'
+
+    # detect arrays
+    a_pos = type_name.find('[')
+    array_size = ''
+    if (a_pos >= 0):
+        # field is array
+        array_size = type_name[a_pos:]
+        type_name = type_name[:a_pos]
+
+    if type_name in type_map:
+        # need to add _t: int8 --> int8_t
+        type_px4 = type_map[type_name]
+    else:
+        type_px4 = type_name
+
+    comment = ''
+    if field.name.startswith('_padding'):
+        comment = ' // required for logger'
+
+    print('\t%s%s%s %s%s;%s'%(type_prefix, type_px4, type_appendix, field.name,
+                array_size, comment))
diff --git a/msg/templates/uorb/msg.cpp.template b/msg/templates/uorb/msg.cpp.template
index 922ce3122d..9244846c7f 100644
--- a/msg/templates/uorb/msg.cpp.template
+++ b/msg/templates/uorb/msg.cpp.template
@@ -54,122 +54,13 @@
 @{
 import genmsg.msgs
 import gencpp
+from px_generate_uorb_topic_helper import * # this is in Tools/
 
 uorb_struct = '%s_s'%spec.short_name
 topic_name = spec.short_name
 
-type_map = {
-    'int8': 'int8_t',
-    'int16': 'int16_t',
-    'int32': 'int32_t',
-    'int64': 'int64_t',
-    'uint8': 'uint8_t',
-    'uint16': 'uint16_t',
-    'uint32': 'uint32_t',
-    'uint64': 'uint64_t',
-    'float32': 'float',
-    'float64': 'double',
-    'bool': 'bool',
-    'char': 'char',
-}
-
-msgtype_size_map = {
-    'int8': 1,
-    'int16': 2,
-    'int32': 4,
-    'int64': 8,
-    'uint8': 1,
-    'uint16': 2,
-    'uint32': 4,
-    'uint64': 8,
-    'float32': 4,
-    'float64': 8,
-    'bool': 1,
-    'char': 1,
-}
-
-def convert_type(spec_type):
-    bare_type = spec_type
-    if '/' in spec_type:
-        # removing prefix
-        bare_type = (spec_type.split('/'))[1]
-
-    msg_type, is_array, array_length = genmsg.msgs.parse_type(bare_type)
-    c_type = msg_type
-    if msg_type in type_map:
-        c_type = type_map[msg_type]
-    if is_array:
-        return c_type + "[" + str(array_length) + "]"
-    return c_type
-
-def bare_name(msg_type):
-    bare = msg_type
-    if '/' in msg_type:
-        # removing prefix
-        bare = (msg_type.split('/'))[1]
-    # removing suffix
-    return bare.split('[')[0]
-
-def sizeof_field_type(field):
-    bare_name_str = bare_name(field.type)
-    if bare_name_str in msgtype_size_map:
-        return msgtype_size_map[bare_name_str]
-    return 0 # this is for non-builtin types: sort them at the end
-
-def get_children_fields(base_type):
-    (package, name) = genmsg.names.package_resource_name(base_type)
-    tmp_msg_context = genmsg.msg_loader.MsgContext.create_default()
-    spec_temp = genmsg.msg_loader.load_msg_by_type(tmp_msg_context, '%s/%s' %(package, name), search_path)  
-    sorted_fields = sorted(spec_temp.parsed_fields(), key=sizeof_field_type, reverse=True)
-    return sorted_fields
-
-# Add padding fields before the embedded types, at the end and calculate the
-# struct size
-def add_padding_bytes(fields):
-    struct_size = 8 # account for the timestamp
-    align_to = 8 # this is always 8, because of the 64bit timestamp
-    i = 0
-    padding_idx = 0
-    while i < len(fields):
-        field = fields[i]
-        if not field.is_header:
-            a_pos = field.type.find('[')
-            array_size = 1
-            if field.is_array:
-                array_size = field.array_len
-            if field.is_builtin:
-                field.sizeof_field_type = sizeof_field_type(field)
-            else:
-                # embedded type: may need to add padding
-                num_padding_bytes = align_to - (struct_size % align_to)
-                if num_padding_bytes != align_to:
-                    padding_field = genmsg.Field('_padding'+str(padding_idx),
-                        'uint8['+str(num_padding_bytes)+']')
-                    padding_idx += 1
-                    padding_field.sizeof_field_type = 1
-                    struct_size += num_padding_bytes
-                    fields.insert(i, padding_field)
-                    i += 1
-                children_fields = get_children_fields(field.base_type)
-                field.sizeof_field_type, unused = add_padding_bytes(children_fields)
-            struct_size += field.sizeof_field_type * array_size
-        i += 1
-
-    # add padding at the end (necessary for embedded types)
-    num_padding_bytes = align_to - (struct_size % align_to)
-    if num_padding_bytes == align_to:
-        num_padding_bytes = 0
-    else:
-        padding_field = genmsg.Field('_padding'+str(padding_idx),
-                'uint8['+str(num_padding_bytes)+']')
-        padding_idx += 1
-        padding_field.sizeof_field_type = 1
-        struct_size += num_padding_bytes
-        fields.append(padding_field)
-    return (struct_size, num_padding_bytes)
-
 sorted_fields = sorted(spec.parsed_fields(), key=sizeof_field_type, reverse=True)
-struct_size, padding_end_size = add_padding_bytes(sorted_fields)
+struct_size, padding_end_size = add_padding_bytes(sorted_fields, search_path)
 topic_fields = ["%s %s" % (convert_type(field.type), field.name) for field in sorted_fields]
 }@
 
diff --git a/msg/templates/uorb/msg.h.template b/msg/templates/uorb/msg.h.template
index f625ce56b7..b890bf88b5 100644
--- a/msg/templates/uorb/msg.h.template
+++ b/msg/templates/uorb/msg.h.template
@@ -54,6 +54,7 @@
 @{
 import genmsg.msgs
 import gencpp
+from px_generate_uorb_topic_helper import * # this is in Tools/
 
 uorb_struct = '%s_s'%spec.short_name
 topic_name = spec.short_name
@@ -96,139 +97,11 @@ for field in spec.parsed_fields():
 @# Main struct of message
 @##############################
 @{
-type_map = {
-    'int8': 'int8_t',
-    'int16': 'int16_t',
-    'int32': 'int32_t',
-    'int64': 'int64_t',
-    'uint8': 'uint8_t',
-    'uint16': 'uint16_t',
-    'uint32': 'uint32_t',
-    'uint64': 'uint64_t',
-    'float32': 'float',
-    'float64': 'double',
-    'bool': 'bool',
-    'char': 'char',
-}
-
-msgtype_size_map = {
-    'int8': 1,
-    'int16': 2,
-    'int32': 4,
-    'int64': 8,
-    'uint8': 1,
-    'uint16': 2,
-    'uint32': 4,
-    'uint64': 8,
-    'float32': 4,
-    'float64': 8,
-    'bool': 1,
-    'char': 1,
-}
-
-def bare_name(msg_type):
-    bare = msg_type
-    if '/' in msg_type:
-        # removing prefix
-        bare = (msg_type.split('/'))[1]
-    # removing suffix
-    return bare.split('[')[0]
-
-def sizeof_field_type(field):
-    bare_name_str = bare_name(field.type)
-    if bare_name_str in msgtype_size_map:
-        return msgtype_size_map[bare_name_str]
-    return 0 # this is for non-builtin types: sort them at the end
-
-def get_children_fields(base_type):
-    (package, name) = genmsg.names.package_resource_name(base_type)
-    tmp_msg_context = genmsg.msg_loader.MsgContext.create_default()
-    spec_temp = genmsg.msg_loader.load_msg_by_type(tmp_msg_context, '%s/%s' %(package, name), search_path)  
-    sorted_fields = sorted(spec_temp.parsed_fields(), key=sizeof_field_type, reverse=True)
-    return sorted_fields
-
-# Add padding fields before the embedded types, at the end and calculate the
-# struct size
-def add_padding_bytes(fields):
-    struct_size = 8 # account for the timestamp
-    align_to = 8 # this is always 8, because of the 64bit timestamp
-    i = 0
-    padding_idx = 0
-    while i < len(fields):
-        field = fields[i]
-        if not field.is_header:
-            a_pos = field.type.find('[')
-            array_size = 1
-            if field.is_array:
-                array_size = field.array_len
-            if field.is_builtin:
-                field.sizeof_field_type = sizeof_field_type(field)
-            else:
-                # embedded type: may need to add padding
-                num_padding_bytes = align_to - (struct_size % align_to)
-                if num_padding_bytes != align_to:
-                    padding_field = genmsg.Field('_padding'+str(padding_idx),
-                        'uint8['+str(num_padding_bytes)+']')
-                    padding_idx += 1
-                    padding_field.sizeof_field_type = 1
-                    struct_size += num_padding_bytes
-                    fields.insert(i, padding_field)
-                    i += 1
-                children_fields = get_children_fields(field.base_type)
-                field.sizeof_field_type, unused = add_padding_bytes(children_fields)
-            struct_size += field.sizeof_field_type * array_size
-        i += 1
-
-    # add padding at the end (necessary for embedded types)
-    num_padding_bytes = align_to - (struct_size % align_to)
-    if num_padding_bytes == align_to:
-        num_padding_bytes = 0
-    else:
-        padding_field = genmsg.Field('_padding'+str(padding_idx),
-                'uint8['+str(num_padding_bytes)+']')
-        padding_idx += 1
-        padding_field.sizeof_field_type = 1
-        struct_size += num_padding_bytes
-        fields.append(padding_field)
-    return (struct_size, num_padding_bytes)
-
-# Function to print a standard ros type
-def print_field_def(field):
-    type_name = field.type
-    # detect embedded types
-    sl_pos = type_name.find('/')
-    type_appendix = ''
-    type_prefix = ''
-    if (sl_pos >= 0):
-        type_name = type_name[sl_pos + 1:]
-        type_prefix = 'struct '
-        type_appendix = '_s'
-
-    # detect arrays
-    a_pos = type_name.find('[')
-    array_size = ''
-    if (a_pos >= 0):
-        # field is array
-        array_size = type_name[a_pos:]
-        type_name = type_name[:a_pos]
-
-    if type_name in type_map:
-        # need to add _t: int8 --> int8_t
-        type_px4 = type_map[type_name]
-    else:
-        type_px4 = type_name
-
-    comment = ''
-    if field.name.startswith('_padding'):
-        comment = ' // required for logger'
-
-    print('\t%s%s%s %s%s;%s'%(type_prefix, type_px4, type_appendix, field.name,
-                array_size, comment))
 
 def print_parsed_fields():
     # sort fields (using a stable sort)
     sorted_fields = sorted(spec.parsed_fields(), key=sizeof_field_type, reverse=True)
-    struct_size, padding_end_size = add_padding_bytes(sorted_fields)
+    struct_size, padding_end_size = add_padding_bytes(sorted_fields, search_path)
     # loop over all fields and print the type and name
     for field in sorted_fields:
         if (not field.is_header):

From 6e7c605279f77ebc66b5da17a296f0741cc326a5 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Beat=20K=C3=BCng?= <beat-kueng@gmx.net>
Date: Fri, 6 May 2016 16:42:35 +0200
Subject: [PATCH 0177/1230] Tools/px_generate_uorb_topic*: combine the src &
 header generators into one script

---
 ...ers.py => px_generate_uorb_topic_files.py} |  83 ++++++--
 Tools/px_generate_uorb_topic_sources.py       | 199 ------------------
 cmake/common/px4_base.cmake                   |  14 +-
 cmake/ros-CMakeLists.txt                      |   4 +-
 4 files changed, 71 insertions(+), 229 deletions(-)
 rename Tools/{px_generate_uorb_topic_headers.py => px_generate_uorb_topic_files.py} (75%)
 delete mode 100755 Tools/px_generate_uorb_topic_sources.py

diff --git a/Tools/px_generate_uorb_topic_headers.py b/Tools/px_generate_uorb_topic_files.py
similarity index 75%
rename from Tools/px_generate_uorb_topic_headers.py
rename to Tools/px_generate_uorb_topic_files.py
index 0dba2468f3..7ca22995b5 100755
--- a/Tools/px_generate_uorb_topic_headers.py
+++ b/Tools/px_generate_uorb_topic_files.py
@@ -1,7 +1,7 @@
 #!/usr/bin/env python
 #############################################################################
 #
-#   Copyright (C) 2013-2015 PX4 Development Team. All rights reserved.
+#   Copyright (C) 2013-2016 PX4 Development Team. All rights reserved.
 #
 # Redistribution and use in source and binary forms, with or without
 # modification, are permitted provided that the following conditions
@@ -33,8 +33,8 @@
 #############################################################################
 
 """
-px_generate_uorb_topic_headers.py
-Generates c/cpp header files for uorb topics from .msg (ROS syntax)
+px_generate_uorb_topics.py
+Generates c/cpp header/source files for uorb topics from .msg (ROS syntax)
 message files
 """
 from __future__ import print_function
@@ -69,14 +69,15 @@ On Windows please run:
 ''')
         exit(1)
 
-__author__ = "Thomas Gubler"
-__copyright__ = "Copyright (C) 2013-2014 PX4 Development Team."
+__author__ = "Sergey Belash, Thomas Gubler, Beat Kueng"
+__copyright__ = "Copyright (C) 2013-2016 PX4 Development Team."
 __license__ = "BSD"
 __email__ = "thomasgubler@gmail.com"
 
 
-TEMPLATE_FILE = 'msg.h.template'
-OUTPUT_FILE_EXT = '.h'
+TEMPLATE_FILE = ['msg.h.template', 'msg.cpp.template']
+TOPICS_LIST_TEMPLATE_FILE = 'uORBTopics.cpp.template'
+OUTPUT_FILE_EXT = ['.h', '.cpp']
 INCL_DEFAULT = ['std_msgs:./msg/std_msgs']
 PACKAGE = 'px4'
 TOPICS_TOKEN = '# TOPICS '
@@ -97,10 +98,16 @@ def get_multi_topics(filename):
         ofile.close()
         return result
 
+def get_msgs_list(msgdir):
+    """
+    Makes list of msg files in the given directory
+    """
+    return [fn for fn in os.listdir(msgdir) if fn.endswith(".msg")]
 
-def generate_header_from_file(filename, outputdir, templatedir, includepath):
+
+def generate_output_from_file(format_idx, filename, outputdir, templatedir, includepath):
         """
-        Converts a single .msg file to a uorb header file
+        Converts a single .msg file to an uorb header/source file
         """
         msg_context = genmsg.msg_loader.MsgContext.create_default()
         full_type_name = genmsg.gentools.compute_full_type_name(PACKAGE, os.path.basename(filename))
@@ -127,28 +134,34 @@ def generate_header_from_file(filename, outputdir, templatedir, includepath):
         if not os.path.isdir(outputdir):
                 os.makedirs(outputdir)
 
-        template_file = os.path.join(templatedir, TEMPLATE_FILE)
-        output_file = os.path.join(outputdir, spec.short_name + OUTPUT_FILE_EXT)
+        template_file = os.path.join(templatedir, TEMPLATE_FILE[format_idx])
+        output_file = os.path.join(outputdir, spec.short_name +
+                OUTPUT_FILE_EXT[format_idx])
 
-        if os.path.isfile(output_file):
-                return False
+        return generate_by_template(output_file, template_file, em_globals)
 
+
+def generate_by_template(output_file, template_file, em_globals):
+        """
+        Invokes empy intepreter to geneate output_file by the
+        given template_file and predefined em_globals dict
+        """
         ofile = open(output_file, 'w')
         # todo, reuse interpreter
         interpreter = em.Interpreter(output=ofile, globals=em_globals, options={em.RAW_OPT:True,em.BUFFERED_OPT:True})
         if not os.path.isfile(template_file):
                 ofile.close()
                 os.remove(output_file)
-                raise RuntimeError("Template file %s not found in template dir %s" % (template_file, templatedir))
+                raise RuntimeError("Template file %s not found" % (template_file))
         interpreter.file(open(template_file)) #todo try
         interpreter.shutdown()
         ofile.close()
         return True
 
 
-def convert_dir(inputdir, outputdir, templatedir):
+def convert_dir(format_idx, inputdir, outputdir, templatedir):
         """
-        Converts all .msg files in inputdir to uORB header files
+        Converts all .msg files in inputdir to uORB header/source files
         """
 
         # Find the most recent modification time in input dir
@@ -185,7 +198,7 @@ def convert_dir(inputdir, outputdir, templatedir):
                 if not os.path.isfile(fn):
                         continue
 
-                generate_header_from_file(fn, outputdir, templatedir, includepath)
+                generate_output_from_file(format_idx, fn, outputdir, templatedir, includepath)
         return True
 
 
@@ -223,19 +236,32 @@ def copy_changed(inputdir, outputdir, prefix='', quiet=False):
                             print("{0}: unchanged".format(input_file))
 
 
-def convert_dir_save(inputdir, outputdir, templatedir, temporarydir, prefix, quiet=False):
+def convert_dir_save(format_idx, inputdir, outputdir, templatedir, temporarydir, prefix, quiet=False):
         """
         Converts all .msg files in inputdir to uORB header files
         Unchanged existing files are not overwritten.
         """
         # Create new headers in temporary output directory
-        convert_dir(inputdir, temporarydir, templatedir)
+        convert_dir(format_idx, inputdir, temporarydir, templatedir)
+        if generate_idx == 1:
+            generate_topics_list_file(inputdir, temporarydir, templatedir)
         # Copy changed headers from temporary dir to output dir
         copy_changed(temporarydir, outputdir, prefix, quiet)
 
+def generate_topics_list_file(msgdir, outputdir, templatedir):
+        # generate cpp file with topics list
+        tl_globals = {"msgs" : get_msgs_list(msgdir)}
+        tl_template_file = os.path.join(templatedir, TOPICS_LIST_TEMPLATE_FILE)
+        tl_out_file = os.path.join(outputdir, TOPICS_LIST_TEMPLATE_FILE.replace(".template", ""))
+        generate_by_template(tl_out_file, tl_template_file, tl_globals)
+
 if __name__ == "__main__":
         parser = argparse.ArgumentParser(
-            description='Convert msg files to uorb headers')
+            description='Convert msg files to uorb headers/sources')
+        parser.add_argument('--headers', help='Generate header files',
+            action='store_true')
+        parser.add_argument('--sources', help='Generate source files',
+            action='store_true')
         parser.add_argument('-d', dest='dir', help='directory with msg files')
         parser.add_argument('-f', dest='file',
                             help="files to convert (use only without -d)",
@@ -254,11 +280,22 @@ if __name__ == "__main__":
                             ' name when converting directories')
         args = parser.parse_args()
 
+        if args.headers:
+            generate_idx = 0
+        elif args.sources:
+            generate_idx = 1
+        else:
+            print('Error: either --headers or --sources must be specified')
+            exit(-1)
+
         if args.file is not None:
-                for f in args.file:
-                        generate_header_from_file(f, args.outputdir, args.templatedir, INCL_DEFAULT)
+            for f in args.file:
+                generate_output_from_file(generate_idx, f, args.outputdir, args.templatedir, INCL_DEFAULT)
+            if generate_idx == 1:
+                generate_topics_list_file(args.dir, args.outputdir, args.templatedir)
         elif args.dir is not None:
-                convert_dir_save(
+            convert_dir_save(
+                    generate_idx,
                     args.dir,
                     args.outputdir,
                     args.templatedir,
diff --git a/Tools/px_generate_uorb_topic_sources.py b/Tools/px_generate_uorb_topic_sources.py
deleted file mode 100755
index b3071832c3..0000000000
--- a/Tools/px_generate_uorb_topic_sources.py
+++ /dev/null
@@ -1,199 +0,0 @@
-#!/usr/bin/env python
-#############################################################################
-#
-#   Copyright (C) 2013-2015 PX4 Development Team. All rights reserved.
-#
-# Redistribution and use in source and binary forms, with or without
-# modification, are permitted provided that the following conditions
-# are met:
-#
-# 1. Redistributions of source code must retain the above copyright
-#    notice, this list of conditions and the following disclaimer.
-# 2. Redistributions in binary form must reproduce the above copyright
-#    notice, this list of conditions and the following disclaimer in
-#    the documentation and/or other materials provided with the
-#    distribution.
-# 3. Neither the name PX4 nor the names of its contributors may be
-#    used to endorse or promote products derived from this software
-#    without specific prior written permission.
-#
-# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
-# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
-# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
-# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
-# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
-# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
-# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
-# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
-# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
-# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
-# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
-# POSSIBILITY OF SUCH DAMAGE.
-#
-#############################################################################
-
-"""
-px_generate_uorb_topic_sources.py
-Generates cpp source files for uorb topics from .msg (ROS syntax)
-message files
-"""
-from __future__ import print_function
-import os
-import shutil
-import filecmp
-import argparse
-
-import sys
-px4_tools_dir = os.path.dirname(os.path.abspath(__file__))
-sys.path.append(px4_tools_dir + "/genmsg/src")
-sys.path.append(px4_tools_dir + "/gencpp/src")
-
-try:
-        import em
-        import genmsg.template_tools
-except ImportError as e:
-        print("python import error: ", e)
-        print('''
-Required python packages not installed.
-
-On a Debian/Ubuntu system please run:
-
-  sudo apt-get install python-empy
-  sudo pip install catkin_pkg
-
-On MacOS please run:
-  sudo pip install empy catkin_pkg
-
-On Windows please run:
-  easy_install empy catkin_pkg
-''')
-        exit(1)
-
-__author__ = "Sergey Belash, Thomas Gubler"
-__copyright__ = "Copyright (C) 2013-2014 PX4 Development Team."
-__license__ = "BSD"
-__email__ = "againagainst@gmail.com, thomasgubler@gmail.com"
-
-TOPIC_TEMPLATE_FILE = 'msg.cpp.template'
-TOPICS_LIST_TEMPLATE_FILE = 'uORBTopics.cpp.template'
-OUTPUT_FILE_EXT = '.cpp'
-INCL_DEFAULT = ['std_msgs:./msg/std_msgs']
-PACKAGE = 'px4'
-TOPICS_TOKEN = '# TOPICS '
-
-
-def get_multi_topics(filename):
-        """
-        Get TOPICS names from a "# TOPICS" line
-        """
-        ofile = open(filename, 'r')
-        text = ofile.read()
-        result = []
-        for each_line in text.split('\n'):
-                if each_line.startswith (TOPICS_TOKEN):
-                        topic_names_str = each_line.strip()
-                        topic_names_str = topic_names_str.replace(TOPICS_TOKEN, "")
-                        result.extend(topic_names_str.split(" "))
-        ofile.close()
-        return result
-
-
-def get_msgs_list(msgdir):
-    """
-    Makes list of msg files in the given directory
-    """
-    return [fn for fn in os.listdir(msgdir) if fn.endswith(".msg")]
-
-
-def generate_source_from_file(filename, outputdir, template_file, includepath, quiet=False):
-        """
-        Converts a single .msg file to a uorb source file
-        """
-        # print("Generating sources from {0}".format(filename))
-        msg_context = genmsg.msg_loader.MsgContext.create_default()
-        full_type_name = genmsg.gentools.compute_full_type_name(PACKAGE, os.path.basename(filename))
-        spec = genmsg.msg_loader.load_msg_from_file(msg_context, filename, full_type_name)
-        topics = get_multi_topics(filename)
-        if includepath:
-                search_path = genmsg.command_line.includepath_to_dict(includepath)
-        else:
-                search_path = {}
-        if len(topics) == 0:
-            topics.append(spec.short_name)
-        em_globals = {
-            "file_name_in": filename,
-            "search_path": search_path,
-            "spec": spec,
-            "topics": topics
-        }
-
-        output_file = os.path.join(outputdir, spec.short_name + OUTPUT_FILE_EXT)
-        if os.path.isfile(output_file):
-            return False
-        generate_by_template(output_file, template_file, em_globals)
-        if not quiet:
-            print("{0}: new source file".format(output_file))
-        return True
-
-
-def generate_by_template(output_file, template_file, em_globals):
-        """
-        Invokes empy intepreter to geneate output_file by the
-        given template_file and predefined em_globals dict
-        """
-        ofile = open(output_file, 'w')
-        # todo, reuse interpreter
-        interpreter = em.Interpreter(output=ofile, globals=em_globals, options={em.RAW_OPT:True,em.BUFFERED_OPT:True})
-        if not os.path.isfile(template_file):
-            ofile.close()
-            os.remove(output_file)
-            raise RuntimeError("Template file %s not found" % (template_file))
-        interpreter.file(open(template_file)) #todo try
-        interpreter.shutdown()
-        ofile.close()
-
-
-def convert_dir(msgdir, outputdir, templatedir, quiet=False):
-        """
-        Converts all .msg files in msgdir to uORB sources
-        """
-        # Make sure output directory exists:
-        if not os.path.isdir(outputdir):
-                os.makedirs(outputdir)
-        template_file = os.path.join(templatedir, TOPIC_TEMPLATE_FILE)
-
-        includepath = INCL_DEFAULT + [':'.join([PACKAGE, msgdir])]
-        for f in os.listdir(msgdir):
-                # Ignore hidden files
-                if f.startswith("."):
-                        continue
-                fn = os.path.join(msgdir, f)
-                # Only look at actual files
-                if not os.path.isfile(fn):
-                        continue
-                generate_source_from_file(fn, outputdir, template_file, includepath, quiet)
-
-        # generate cpp file with topics list
-        tl_globals = {"msgs" : get_msgs_list(msgdir)}
-        tl_template_file = os.path.join(templatedir, TOPICS_LIST_TEMPLATE_FILE)
-        tl_out_file = os.path.join(outputdir, TOPICS_LIST_TEMPLATE_FILE.replace(".template", ""))
-        generate_by_template(tl_out_file, tl_template_file, tl_globals)
-        return True
-
-
-if __name__ == "__main__":
-        parser = argparse.ArgumentParser(description='Convert msg files to uorb sources')
-        parser.add_argument('-d', dest='dir', help='directory with msg files')
-        parser.add_argument('-f', dest='file', help="files to convert (use only without -d)", nargs="+")
-        parser.add_argument('-e', dest='templatedir', help='directory with template files',)
-        parser.add_argument('-o', dest='outputdir', help='output directory for source files')
-        parser.add_argument('-q', dest='quiet', default=False, action='store_true',
-                            help='string added as prefix to the output file '
-                            ' name when converting directories')
-        args = parser.parse_args()
-
-        if args.file is not None:
-                for f in args.file:
-                        generate_source_from_file(f, args.outputdir, args.templatedir, args.quiet)
-        elif args.dir is not None:
-                convert_dir(args.dir, args.outputdir, args.templatedir, args.quiet)
diff --git a/cmake/common/px4_base.cmake b/cmake/common/px4_base.cmake
index 41de8a94c4..84383fc670 100644
--- a/cmake/common/px4_base.cmake
+++ b/cmake/common/px4_base.cmake
@@ -380,12 +380,13 @@ function(px4_generate_messages)
 	endforeach()
 	add_custom_command(OUTPUT ${msg_files_out}
 		COMMAND ${PYTHON_EXECUTABLE}
-			Tools/px_generate_uorb_topic_headers.py
+			Tools/px_generate_uorb_topic_files.py
+			--headers
 			${QUIET}
 			-d msg
 			-o ${msg_out_path}
 			-e msg/templates/uorb
-			-t ${CMAKE_BINARY_DIR}/topics_temporary
+			-t ${CMAKE_BINARY_DIR}/topics_temporary_header
 		DEPENDS ${DEPENDS} ${MSG_FILES}
 		WORKING_DIRECTORY ${CMAKE_SOURCE_DIR}
 		COMMENT "Generating uORB topic headers"
@@ -400,11 +401,13 @@ function(px4_generate_messages)
 	endforeach()
 	add_custom_command(OUTPUT ${msg_source_files_out}
 		COMMAND ${PYTHON_EXECUTABLE} 
-			Tools/px_generate_uorb_topic_sources.py
+			Tools/px_generate_uorb_topic_files.py
+			--sources
 			${QUIET}
 			-d msg
-			-e msg/templates/uorb
 			-o ${msg_source_out_path}
+			-e msg/templates/uorb
+			-t ${CMAKE_BINARY_DIR}/topics_temporary_sources
 		DEPENDS ${DEPENDS} ${MSG_FILES}
 		WORKING_DIRECTORY ${CMAKE_SOURCE_DIR}
 		COMMENT "Generating uORB topic sources"
@@ -421,7 +424,8 @@ function(px4_generate_messages)
 	endforeach()
 	add_custom_command(OUTPUT ${msg_multi_files_out}
 		COMMAND ${PYTHON_EXECUTABLE}
-			Tools/px_generate_uorb_topic_headers.py
+			Tools/px_generate_uorb_topic_files.py
+			--headers
 			${QUIET}
 			-d msg
 			-o ${msg_multi_out_path}
diff --git a/cmake/ros-CMakeLists.txt b/cmake/ros-CMakeLists.txt
index 10cb673bfe..67870b1e2c 100644
--- a/cmake/ros-CMakeLists.txt
+++ b/cmake/ros-CMakeLists.txt
@@ -147,8 +147,8 @@ set(MULTIPLATFORM_HEADER_DIR ${CMAKE_CURRENT_SOURCE_DIR}/src/platforms/ros/px4_m
 set(MULTIPLATFORM_TEMPLATE_DIR ${CMAKE_CURRENT_SOURCE_DIR}/msg/templates/px4/ros)
 set(TOPICHEADER_TEMP_DIR ${CMAKE_BINARY_DIR}/topics_temporary)
 set(MULTIPLATFORM_PREFIX px4_)
-add_custom_target(multiplatform_message_headers ALL ${PYTHON_EXECUTABLE} ${CMAKE_CURRENT_SOURCE_DIR}/Tools/px_generate_uorb_topic_headers.py
-	-d ${CMAKE_CURRENT_SOURCE_DIR}/msg -o ${MULTIPLATFORM_HEADER_DIR} -e ${MULTIPLATFORM_TEMPLATE_DIR}
+add_custom_target(multiplatform_message_headers ALL ${PYTHON_EXECUTABLE} ${CMAKE_CURRENT_SOURCE_DIR}/Tools/px_generate_uorb_topic_files.py
+	--headers -d ${CMAKE_CURRENT_SOURCE_DIR}/msg -o ${MULTIPLATFORM_HEADER_DIR} -e ${MULTIPLATFORM_TEMPLATE_DIR}
 	-t ${TOPICHEADER_TEMP_DIR} -p ${MULTIPLATFORM_PREFIX})
 
 ## Declare a cpp library

From 4f0573d6127170e0078d7f59ce9b501822277938 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Beat=20K=C3=BCng?= <beat-kueng@gmx.net>
Date: Sat, 7 May 2016 08:44:27 +0200
Subject: [PATCH 0178/1230] logger: reset _write_dropouts after status output

---
 src/modules/logger/logger.cpp | 3 ++-
 1 file changed, 2 insertions(+), 1 deletion(-)

diff --git a/src/modules/logger/logger.cpp b/src/modules/logger/logger.cpp
index 93e1603a40..20680ae543 100644
--- a/src/modules/logger/logger.cpp
+++ b/src/modules/logger/logger.cpp
@@ -168,9 +168,10 @@ void Logger::status()
 		float seconds = ((float)(hrt_absolute_time() - _start_time)) / 1000000.0f;
 
 		PX4_INFO("Wrote %4.2f MiB (avg %5.2f KiB/s)", (double)mebibytes, (double)(kibibytes / seconds));
-		PX4_INFO("Dropouts: %zu (max len: %.3f s), max used buffer: %zu / %zu B",
+		PX4_INFO("Since last status: dropouts: %zu (max len: %.3f s), max used buffer: %zu / %zu B",
 			 _write_dropouts, (double)_max_dropout_duration, _high_water, _writer.get_buffer_size());
 		_high_water = 0;
+		_write_dropouts = 0;
 		_max_dropout_duration = 0.f;
 	}
 }

From d7f0808316b06fef81c188d242ed28ef72465fad Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Beat=20K=C3=BCng?= <beat-kueng@gmx.net>
Date: Mon, 9 May 2016 09:47:35 +0200
Subject: [PATCH 0179/1230] logger: create _vehicle_status_sub &
 _parameter_update_sub on stack

Since it's only used in run().
---
 src/modules/logger/logger.cpp | 30 +++++++++---------------------
 src/modules/logger/logger.h   |  2 --
 2 files changed, 9 insertions(+), 23 deletions(-)

diff --git a/src/modules/logger/logger.cpp b/src/modules/logger/logger.cpp
index 20680ae543..850335875f 100644
--- a/src/modules/logger/logger.cpp
+++ b/src/modules/logger/logger.cpp
@@ -143,7 +143,7 @@ int Logger::start(char *const *argv)
 	logger_task = px4_task_spawn_cmd("logger",
 					 SCHED_DEFAULT,
 					 SCHED_PRIORITY_MAX - 5,
-					 3100,
+					 3200,
 					 (px4_main_t)&Logger::run_trampoline,
 					 (char *const *)argv);
 
@@ -431,17 +431,8 @@ void Logger::run()
 		return;
 	}
 
-	if (!(_vehicle_status_sub = new uORB::Subscription<vehicle_status_s>(ORB_ID(vehicle_status)))) {
-		PX4_ERR("Failed to allocate subscription");
-		return;
-	}
-
-	if (!(_parameter_update_sub = new uORB::Subscription<parameter_update_s>(ORB_ID(parameter_update)))) {
-		delete _vehicle_status_sub;
-		_vehicle_status_sub = nullptr;
-		PX4_ERR("Failed to allocate subscription");
-		return;
-	}
+	uORB::Subscription<vehicle_status_s> vehicle_status_sub(ORB_ID(vehicle_status));
+	uORB::Subscription<parameter_update_s> parameter_update_sub(ORB_ID(parameter_update));
 
 
 	add_topic("sensor_gyro", 0);
@@ -495,10 +486,10 @@ void Logger::run()
 	while (!_task_should_exit) {
 
 		// Start/stop logging when system arm/disarm
-		if (_vehicle_status_sub->check_updated()) {
-			_vehicle_status_sub->update();
-			bool armed = (_vehicle_status_sub->get().arming_state == vehicle_status_s::ARMING_STATE_ARMED) ||
-				     (_vehicle_status_sub->get().arming_state == vehicle_status_s::ARMING_STATE_ARMED_ERROR);
+		if (vehicle_status_sub.check_updated()) {
+			vehicle_status_sub.update();
+			bool armed = (vehicle_status_sub.get().arming_state == vehicle_status_s::ARMING_STATE_ARMED) ||
+				     (vehicle_status_sub.get().arming_state == vehicle_status_s::ARMING_STATE_ARMED_ERROR);
 
 			if (_enabled != armed && !_log_on_start) {
 				if (armed) {
@@ -521,9 +512,9 @@ void Logger::run()
 
 			/* Check if parameters have changed */
 			// this needs to change to a timestamped record to record a history of parameter changes
-			if (_parameter_update_sub->check_updated()) {
+			if (parameter_update_sub.check_updated()) {
 				warnx("parameter update");
-				_parameter_update_sub->update();
+				parameter_update_sub.update();
 				write_changed_parameters();
 			}
 
@@ -624,9 +615,6 @@ void Logger::run()
 		usleep(_log_interval);
 	}
 
-	delete _vehicle_status_sub;
-	delete _parameter_update_sub;
-
 	// stop the writer thread
 	_writer.thread_stop();
 
diff --git a/src/modules/logger/logger.h b/src/modules/logger/logger.h
index e8470dfad6..b247fd292f 100644
--- a/src/modules/logger/logger.h
+++ b/src/modules/logger/logger.h
@@ -128,8 +128,6 @@ private:
 
 	bool						_task_should_exit = true;
 	char 						_log_dir[64];
-	uORB::Subscription<vehicle_status_s>	*_vehicle_status_sub = nullptr;
-	uORB::Subscription<parameter_update_s>	*_parameter_update_sub = nullptr;
 	bool						_enabled = false;
 
 	// statistics

From cfa491467e1a00cbd2f9c4b468618380acf4e988 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Beat=20K=C3=BCng?= <beat-kueng@gmx.net>
Date: Mon, 9 May 2016 11:08:45 +0200
Subject: [PATCH 0180/1230] logger: fix 'logger stop' when nothing has been
 logged yet

when executing 'logger stop' and the logger did not log yet, _running was
false, so log_writer thread would never exit.
---
 src/modules/logger/log_writer.cpp | 17 ++++++++++-------
 src/modules/logger/log_writer.h   |  2 +-
 2 files changed, 11 insertions(+), 8 deletions(-)

diff --git a/src/modules/logger/log_writer.cpp b/src/modules/logger/log_writer.cpp
index f4ade4b1b7..2630e5238a 100644
--- a/src/modules/logger/log_writer.cpp
+++ b/src/modules/logger/log_writer.cpp
@@ -182,6 +182,7 @@ void LogWriter::run()
 			}
 
 			pthread_mutex_unlock(&_mtx);
+			written = 0;
 
 			if (available > 0) {
 				perf_begin(perf_write);
@@ -211,20 +212,22 @@ void LogWriter::run()
 				_total_written += written;
 			}
 
-			if (_running && !_should_run && written == static_cast<int>(available) && !is_part) {
+			if (!_should_run && written == static_cast<int>(available) && !is_part) {
 				// Stop only when all data written
 				_running = false;
 				_head = 0;
 				_count = 0;
 
-				int res = ::close(_fd);
-				_fd = -1;
+				if (_fd >= 0) {
+					int res = ::close(_fd);
+					_fd = -1;
 
-				if (res) {
-					PX4_WARN("error closing log file");
+					if (res) {
+						PX4_WARN("error closing log file");
 
-				} else {
-					PX4_WARN("closed logfile: %s, bytes written: %zu", _filename, _total_written);
+					} else {
+						PX4_WARN("closed logfile: %s, bytes written: %zu", _filename, _total_written);
+					}
 				}
 
 				break;
diff --git a/src/modules/logger/log_writer.h b/src/modules/logger/log_writer.h
index 7ddac85d98..5014169f20 100644
--- a/src/modules/logger/log_writer.h
+++ b/src/modules/logger/log_writer.h
@@ -117,7 +117,7 @@ private:
 	static constexpr size_t	_min_write_chunk = 4096;
 
 	char		_filename[64];
-	int			_fd;
+	int			_fd = -1;
 	uint8_t 	*_buffer = nullptr;
 	const size_t	_buffer_size;
 	size_t			_head = 0; ///< next position to write to

From 693703de1e7255db9f97edb5a8596ca3f39b23bb Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Beat=20K=C3=BCng?= <beat-kueng@gmx.net>
Date: Mon, 9 May 2016 11:09:14 +0200
Subject: [PATCH 0181/1230] logger: initialize logger_ptr with null

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

diff --git a/src/modules/logger/logger.h b/src/modules/logger/logger.h
index b247fd292f..c0516180a6 100644
--- a/src/modules/logger/logger.h
+++ b/src/modules/logger/logger.h
@@ -143,7 +143,7 @@ private:
 	uint32_t					_log_interval;
 };
 
-Logger *logger_ptr;
+Logger *logger_ptr = nullptr;
 int		logger_task = -1;
 pthread_t _writer_thread;
 

From a0beef32046c308dbc34a6b3e118048825d5b2fd Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Beat=20K=C3=BCng?= <beat-kueng@gmx.net>
Date: Mon, 9 May 2016 14:16:30 +0200
Subject: [PATCH 0182/1230] logger: output error on failed to get log file name

---
 src/modules/logger/logger.cpp | 3 ++-
 1 file changed, 2 insertions(+), 1 deletion(-)

diff --git a/src/modules/logger/logger.cpp b/src/modules/logger/logger.cpp
index 850335875f..ed4d7db2db 100644
--- a/src/modules/logger/logger.cpp
+++ b/src/modules/logger/logger.cpp
@@ -700,7 +700,7 @@ int Logger::get_log_file_name(char *file_name, size_t file_name_size)
 
 void Logger::start_log()
 {
-	PX4_WARN("start log");
+	PX4_INFO("start log");
 
 	if (create_log_dir()) {
 		return;
@@ -709,6 +709,7 @@ void Logger::start_log()
 	char file_name[64] = "";
 
 	if (get_log_file_name(file_name, sizeof(file_name))) {
+		PX4_ERR("logger: failed to get log file name");
 		return;
 	}
 

From d3a9930b500f0adcfedb5926980d69d90a0a5132 Mon Sep 17 00:00:00 2001
From: Julian Oes <julian@oes.ch>
Date: Mon, 9 May 2016 14:28:06 +0200
Subject: [PATCH 0183/1230] logger: get paths working on Snapdragon

---
 src/modules/logger/logger.h | 6 +++++-
 src/platforms/px4_defines.h | 1 -
 2 files changed, 5 insertions(+), 2 deletions(-)

diff --git a/src/modules/logger/logger.h b/src/modules/logger/logger.h
index c0516180a6..99ddaf89c5 100644
--- a/src/modules/logger/logger.h
+++ b/src/modules/logger/logger.h
@@ -124,7 +124,11 @@ private:
 	static constexpr size_t 	MAX_TOPICS_NUM = 128; /**< Maximum number of logged topics */
 	static constexpr unsigned	MAX_NO_LOGFOLDER = 999;	/**< Maximum number of log dirs */
 	static constexpr unsigned	MAX_NO_LOGFILE = 999;	/**< Maximum number of log files */
-	static constexpr const char 		*LOG_ROOT = PX4_ROOTFSDIR"/fs/microsd/log";
+#ifdef __PX4_POSIX_EAGLE
+	static constexpr const char	*LOG_ROOT = PX4_ROOTFSDIR"/log";
+#else
+	static constexpr const char 	*LOG_ROOT = PX4_ROOTFSDIR"/fs/microsd/log";
+#endif
 
 	bool						_task_should_exit = true;
 	char 						_log_dir[64];
diff --git a/src/platforms/px4_defines.h b/src/platforms/px4_defines.h
index fd87b78a28..ee721b8221 100644
--- a/src/platforms/px4_defines.h
+++ b/src/platforms/px4_defines.h
@@ -229,7 +229,6 @@ __END_DECLS
 
 #if defined(__PX4_QURT)
 
-#define PX4_ROOTFSDIR
 #define DEFAULT_PARAM_FILE "/fs/eeprom/parameters"
 
 #define SIOCDEVPRIVATE 999999

From da1e63eaf3549c9b991f19a091439b086f3baceb Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Beat=20K=C3=BCng?= <beat-kueng@gmx.net>
Date: Mon, 9 May 2016 16:34:18 +0200
Subject: [PATCH 0184/1230] logger: fix resource leaks in LogWriter

---
 src/modules/logger/log_writer.cpp | 17 +++++++++++------
 src/modules/logger/log_writer.h   |  4 ++--
 2 files changed, 13 insertions(+), 8 deletions(-)

diff --git a/src/modules/logger/log_writer.cpp b/src/modules/logger/log_writer.cpp
index 2630e5238a..a720ff8b8c 100644
--- a/src/modules/logger/log_writer.cpp
+++ b/src/modules/logger/log_writer.cpp
@@ -52,8 +52,8 @@ LogWriter::LogWriter(size_t buffer_size) :
 	pthread_mutex_init(&_mtx, nullptr);
 	pthread_cond_init(&_cv, nullptr);
 	/* allocate write performance counters */
-	perf_write = perf_alloc(PC_ELAPSED, "sd write");
-	perf_fsync = perf_alloc(PC_ELAPSED, "sd fsync");
+	_perf_write = perf_alloc(PC_ELAPSED, "sd write");
+	_perf_fsync = perf_alloc(PC_ELAPSED, "sd fsync");
 }
 
 bool LogWriter::init()
@@ -69,6 +69,11 @@ bool LogWriter::init()
 
 LogWriter::~LogWriter()
 {
+	pthread_mutex_destroy(&_mtx);
+	pthread_cond_destroy(&_cv);
+	perf_free(_perf_write);
+	perf_free(_perf_fsync);
+
 	if (_buffer) {
 		delete[] _buffer;
 	}
@@ -185,15 +190,15 @@ void LogWriter::run()
 			written = 0;
 
 			if (available > 0) {
-				perf_begin(perf_write);
+				perf_begin(_perf_write);
 				written = ::write(_fd, read_ptr, available);
-				perf_end(perf_write);
+				perf_end(_perf_write);
 
 				/* call fsync periodically to minimize potential loss of data */
 				if (++poll_count >= 100) {
-					perf_begin(perf_fsync);
+					perf_begin(_perf_fsync);
 					::fsync(_fd);
-					perf_end(perf_fsync);
+					perf_end(_perf_fsync);
 					poll_count = 0;
 				}
 
diff --git a/src/modules/logger/log_writer.h b/src/modules/logger/log_writer.h
index 5014169f20..115f4e03ff 100644
--- a/src/modules/logger/log_writer.h
+++ b/src/modules/logger/log_writer.h
@@ -128,8 +128,8 @@ private:
 	bool 		_exit_thread = false;
 	pthread_mutex_t		_mtx;
 	pthread_cond_t		_cv;
-	perf_counter_t perf_write;
-	perf_counter_t perf_fsync;
+	perf_counter_t _perf_write;
+	perf_counter_t _perf_fsync;
 };
 
 }

From ec6c53eb6093fbd4f2f1c2511491b20e57467964 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Beat=20K=C3=BCng?= <beat-kueng@gmx.net>
Date: Mon, 9 May 2016 17:24:54 +0200
Subject: [PATCH 0185/1230] logger: correct cleanup in case 'logger start'
 fails

---
 src/modules/logger/logger.cpp | 11 +++++++++--
 1 file changed, 9 insertions(+), 2 deletions(-)

diff --git a/src/modules/logger/logger.cpp b/src/modules/logger/logger.cpp
index ed4d7db2db..f1ae7e518f 100644
--- a/src/modules/logger/logger.cpp
+++ b/src/modules/logger/logger.cpp
@@ -74,6 +74,12 @@ int logger_main(int argc, char *argv[])
 		return 1;
 	}
 
+	//Check if thread exited, but the object has not been destroyed yet (happens in case of an error)
+	if (logger_task == -1 && logger_ptr) {
+		delete logger_ptr;
+		logger_ptr = nullptr;
+	}
+
 	if (!strcmp(argv[1], "start")) {
 
 		if (logger_ptr != nullptr) {
@@ -148,6 +154,7 @@ int Logger::start(char *const *argv)
 					 (char *const *)argv);
 
 	if (logger_task < 0) {
+		logger_task = -1;
 		PX4_WARN("task start failed");
 		return -errno;
 	}
@@ -245,6 +252,8 @@ void Logger::run_trampoline(int argc, char *argv[])
 	} else {
 		logger_ptr->run();
 	}
+
+	logger_task = -1;
 }
 
 enum class MessageType : uint8_t {
@@ -626,8 +635,6 @@ void Logger::run()
 	if (ret) {
 		PX4_WARN("join failed: %d", ret);
 	}
-
-	logger_task = -1;
 }
 
 int Logger::create_log_dir()

From d9ced9730b91422198d705b3c58fb9ff25c07e9b Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Beat=20K=C3=BCng?= <beat-kueng@gmx.net>
Date: Tue, 10 May 2016 08:10:21 +0200
Subject: [PATCH 0186/1230] logger: remove unnecessary MODULE_CFLAGS from
 CMakeLists.txt

---
 src/modules/logger/CMakeLists.txt | 3 ---
 1 file changed, 3 deletions(-)

diff --git a/src/modules/logger/CMakeLists.txt b/src/modules/logger/CMakeLists.txt
index 31e3f05884..cb88ca6eaf 100644
--- a/src/modules/logger/CMakeLists.txt
+++ b/src/modules/logger/CMakeLists.txt
@@ -30,9 +30,6 @@
 # POSSIBILITY OF SUCH DAMAGE.
 #
 ############################################################################
-if (${OS} STREQUAL "nuttx")
-	list(APPEND MODULE_CFLAGS -Wframe-larger-than=2200)
-endif()
 
 px4_add_module(
 	MODULE modules__logger

From 9da2eac3d33c4639fca92b8700db7e4285f4606e Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Beat=20K=C3=BCng?= <beat-kueng@gmx.net>
Date: Tue, 10 May 2016 16:11:12 +0200
Subject: [PATCH 0187/1230] logger: remove unneeded start parameters (-x & -a)

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

diff --git a/src/modules/logger/logger.cpp b/src/modules/logger/logger.cpp
index f1ae7e518f..1037e7fec5 100644
--- a/src/modules/logger/logger.cpp
+++ b/src/modules/logger/logger.cpp
@@ -135,10 +135,8 @@ void Logger::usage(const char *reason)
 	PX4_INFO("usage: logger {start|stop|status} [-r <log rate>] [-b <buffer size>] -e -a -t -x\n"
 		 "\t-r\tLog rate in Hz, 0 means unlimited rate\n"
 		 "\t-b\tLog buffer size in KiB, default is 12\n"
-		 "\t-e\tEnable logging by default (if not, can be started by command)\n"
-		 "\t-a\tLog only when armed (can be still overriden by command)\n"
-		 "\t-t\tUse date/time for naming log directories and files\n"
-		 "\t-x\tExtended logging");
+		 "\t-e\tEnable logging right after start (otherwise only when armed)\n"
+		 "\t-t\tUse date/time for naming log directories and files");
 }
 
 int Logger::start(char *const *argv)
@@ -194,7 +192,7 @@ void Logger::run_trampoline(int argc, char *argv[])
 	int ch;
 	const char *myoptarg = NULL;
 
-	while ((ch = px4_getopt(argc, argv, "r:b:eatx", &myoptind, &myoptarg)) != EOF) {
+	while ((ch = px4_getopt(argc, argv, "r:b:et", &myoptind, &myoptarg)) != EOF) {
 		switch (ch) {
 		case 'r': {
 				unsigned long r = strtoul(myoptarg, NULL, 10);
@@ -282,7 +280,6 @@ struct message_data_header_s {
 	uint8_t multi_id;
 };
 
-// currently unused
 struct message_info_header_s {
 	uint8_t msg_type = static_cast<uint8_t>(MessageType::INFO);
 	uint16_t msg_size;

From fe2b80ffb829b015fd5b104e1014c4b7bba82df7 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Beat=20K=C3=BCng?= <beat-kueng@gmx.net>
Date: Wed, 11 May 2016 10:08:25 +0200
Subject: [PATCH 0188/1230] logger: add '-t' option to use GPS date/time for
 file and dir name

---
 src/modules/logger/logger.cpp | 170 +++++++++++++++++++++++++---------
 src/modules/logger/logger.h   |  27 +++++-
 2 files changed, 151 insertions(+), 46 deletions(-)

diff --git a/src/modules/logger/logger.cpp b/src/modules/logger/logger.cpp
index 1037e7fec5..403dab2134 100644
--- a/src/modules/logger/logger.cpp
+++ b/src/modules/logger/logger.cpp
@@ -36,13 +36,20 @@
 #include <sys/stat.h>
 #include <errno.h>
 #include <string.h>
+#include <time.h>
 
 #include <uORB/uORB.h>
 #include <uORB/uORBTopics.h>
+#include <uORB/topics/parameter_update.h>
+#include <uORB/topics/vehicle_status.h>
+#include <uORB/topics/vehicle_gps_position.h>
+
 #include <px4_includes.h>
 #include <px4_getopt.h>
 #include <px4_log.h>
 
+#define GPS_EPOCH_SECS ((time_t)1234567890ULL)
+
 //#define DBGPRINT //write status output every few seconds
 
 #if defined(DBGPRINT)
@@ -187,6 +194,7 @@ void Logger::run_trampoline(int argc, char *argv[])
 	int log_buffer_size = 12 * 1024;
 	bool log_on_start = false;
 	bool error_flag = false;
+	bool log_name_timestamp = false;
 
 	int myoptind = 1;
 	int ch;
@@ -220,6 +228,10 @@ void Logger::run_trampoline(int argc, char *argv[])
 			}
 			break;
 
+		case 't':
+			log_name_timestamp = true;
+			break;
+
 		case '?':
 			error_flag = true;
 			break;
@@ -236,7 +248,7 @@ void Logger::run_trampoline(int argc, char *argv[])
 		return;
 	}
 
-	logger_ptr = new Logger(log_buffer_size, log_interval, log_on_start);
+	logger_ptr = new Logger(log_buffer_size, log_interval, log_on_start, log_name_timestamp);
 
 #if defined(DBGPRINT) && defined(__PX4_NUTTX)
 	struct mallinfo alloc_info = mallinfo();
@@ -300,11 +312,14 @@ struct message_parameter_header_s {
 
 static constexpr size_t MAX_DATA_SIZE = 740;
 
-Logger::Logger(size_t buffer_size, unsigned log_interval, bool log_on_start) :
+Logger::Logger(size_t buffer_size, unsigned log_interval, bool log_on_start,
+	       bool log_name_timestamp) :
 	_log_on_start(log_on_start),
+	_log_name_timestamp(log_name_timestamp),
 	_writer(buffer_size),
 	_log_interval(log_interval)
 {
+	_log_utc_offset = param_find("SDLOG_UTC_OFFSET");
 }
 
 Logger::~Logger()
@@ -634,40 +649,52 @@ void Logger::run()
 	}
 }
 
-int Logger::create_log_dir()
+int Logger::create_log_dir(tm *tt)
 {
 	/* create dir on sdcard if needed */
-	uint16_t dir_number = 1; // start with dir sess001
 	int mkdir_ret;
 
-	/* look for the next dir that does not exist */
-	while (dir_number <= MAX_NO_LOGFOLDER) {
-		/* format log dir: e.g. /fs/microsd/sess001 */
-		sprintf(_log_dir, "%s/sess%03u", LOG_ROOT, dir_number);
+	if (tt) {
+		int n = snprintf(_log_dir, sizeof(_log_dir), "%s/", LOG_ROOT);
+		strftime(_log_dir + n, sizeof(_log_dir) - n, "%Y-%m-%d", tt);
 		mkdir_ret = mkdir(_log_dir, S_IRWXU | S_IRWXG | S_IRWXO);
 
-		if (mkdir_ret == 0) {
-			PX4_INFO("log dir created: %s", _log_dir);
-			break;
-
-		} else if (errno != EEXIST) {
-			PX4_WARN("failed creating new dir: %s", _log_dir);
+		if (mkdir_ret != OK && errno != EEXIST) {
+			PX4_ERR("failed creating new dir: %s", _log_dir);
 			return -1;
 		}
 
-		/* dir exists already */
-		dir_number++;
-		continue;
+	} else {
+		uint16_t dir_number = 1; // start with dir sess001
+
+		/* look for the next dir that does not exist */
+		while (!_has_log_dir && dir_number <= MAX_NO_LOGFOLDER) {
+			/* format log dir: e.g. /fs/microsd/sess001 */
+			sprintf(_log_dir, "%s/sess%03u", LOG_ROOT, dir_number);
+			mkdir_ret = mkdir(_log_dir, S_IRWXU | S_IRWXG | S_IRWXO);
+
+			if (mkdir_ret == 0) {
+				PX4_DEBUG("log dir created: %s", _log_dir);
+				_has_log_dir = true;
+
+			} else if (errno != EEXIST) {
+				PX4_ERR("failed creating new dir: %s (%i)", _log_dir, errno);
+				return -1;
+			}
+
+			/* dir exists already */
+			dir_number++;
+		}
+
+		if (dir_number >= MAX_NO_LOGFOLDER) {
+			/* we should not end up here, either we have more than MAX_NO_LOGFOLDER on the SD card, or another problem */
+			PX4_ERR("All %d possible dirs exist already", MAX_NO_LOGFOLDER);
+			return -1;
+		}
+
+		_has_log_dir = true;
 	}
 
-	if (dir_number >= MAX_NO_LOGFOLDER) {
-		/* we should not end up here, either we have more than MAX_NO_LOGFOLDER on the SD card, or another problem */
-		PX4_WARN("all %d possible dirs exist already", MAX_NO_LOGFOLDER);
-		return -1;
-	}
-
-	/* print logging path, important to find log file later */
-	//mavlink_and_console_log_info(mavlink_fd, "[sdlog2] log dir: %s", log_dir);
 	return 0;
 }
 
@@ -679,37 +706,96 @@ bool Logger::file_exist(const char *filename)
 
 int Logger::get_log_file_name(char *file_name, size_t file_name_size)
 {
-	uint16_t file_number = 1; // start with file log001
+	tm tt;
+	bool time_ok = false;
 
-	/* look for the next file that does not exist */
-	while (file_number <= MAX_NO_LOGFILE) {
-		/* format log file path: e.g. /fs/microsd/sess001/log001.ulg */
-		snprintf(file_name, file_name_size, "%s/log%03u.ulg", _log_dir, file_number);
+	if (_log_name_timestamp) {
+		/* use RTC time for log file naming, e.g. /fs/microsd/2014-01-19/19_37_52.ulg */
+		time_ok = get_log_time(&tt, false);
+	}
 
-		if (!file_exist(file_name)) {
-			break;
+	if (time_ok) {
+		if (create_log_dir(&tt)) {
+			return -1;
 		}
 
-		file_number++;
+		char log_file_name[64] = "";
+		strftime(log_file_name, sizeof(log_file_name), "%H_%M_%S.ulg", &tt);
+		snprintf(file_name, file_name_size, "%s/%s", _log_dir, log_file_name);
+
+	} else {
+		if (create_log_dir(nullptr)) {
+			return -1;
+		}
+
+		uint16_t file_number = 1; // start with file log001
+
+		/* look for the next file that does not exist */
+		while (file_number <= MAX_NO_LOGFILE) {
+			/* format log file path: e.g. /fs/microsd/sess001/log001.ulg */
+			snprintf(file_name, file_name_size, "%s/log%03u.ulg", _log_dir, file_number);
+
+			if (!file_exist(file_name)) {
+				break;
+			}
+
+			file_number++;
+		}
+
+		if (file_number > MAX_NO_LOGFILE) {
+			/* we should not end up here, either we have more than MAX_NO_LOGFILE on the SD card, or another problem */
+			return -1;
+		}
 	}
 
-	if (file_number > MAX_NO_LOGFILE) {
-		/* we should not end up here, either we have more than MAX_NO_LOGFILE on the SD card, or another problem */
-		//mavlink_and_console_log_critical(mavlink_fd, "[sdlog2] ERR: max files %d", MAX_NO_LOGFILE);
-		return -1;
-	}
 
 	return 0;
 }
 
+bool Logger::get_log_time(struct tm *tt, bool boot_time)
+{
+	int vehicle_gps_position_sub = orb_subscribe(ORB_ID(vehicle_gps_position));
+
+	if (vehicle_gps_position_sub < 0) {
+		return false;
+	}
+
+	/* Get the latest GPS publication */
+	vehicle_gps_position_s gps_pos;
+
+	if (orb_copy(ORB_ID(vehicle_gps_position), vehicle_gps_position_sub, &gps_pos) < 0) {
+		orb_unsubscribe(vehicle_gps_position_sub);
+		return false;
+	}
+
+	orb_unsubscribe(vehicle_gps_position_sub);
+	time_t utc_time_sec = gps_pos.time_utc_usec / 1e6;
+
+	if (gps_pos.fix_type < 2 || utc_time_sec < GPS_EPOCH_SECS) {
+		return false;
+	}
+
+	/* strip the time elapsed since boot */
+	if (boot_time) {
+		utc_time_sec -= hrt_absolute_time() / 1e6;
+	}
+
+	int32_t utc_offset = 0;
+
+	if (_log_utc_offset != PARAM_INVALID) {
+		param_get(_log_utc_offset, &utc_offset);
+	}
+
+	/* apply utc offset */
+	utc_time_sec += utc_offset * 60;
+
+	return gmtime_r(&utc_time_sec, tt) != nullptr;
+}
+
 void Logger::start_log()
 {
 	PX4_INFO("start log");
 
-	if (create_log_dir()) {
-		return;
-	}
-
 	char file_name[64] = "";
 
 	if (get_log_file_name(file_name, sizeof(file_name))) {
diff --git a/src/modules/logger/logger.h b/src/modules/logger/logger.h
index 99ddaf89c5..7ce35bf816 100644
--- a/src/modules/logger/logger.h
+++ b/src/modules/logger/logger.h
@@ -38,9 +38,9 @@
 #include <px4.h>
 #include <drivers/drv_hrt.h>
 #include <uORB/Subscription.hpp>
-#include <uORB/topics/vehicle_status.h>
 #include <version/version.h>
 #include <systemlib/git_version.h>
+#include <systemlib/param/param.h>
 
 extern "C" __EXPORT int logger_main(int argc, char *argv[]);
 
@@ -74,7 +74,7 @@ struct LoggerSubscription {
 class Logger
 {
 public:
-	Logger(size_t buffer_size, unsigned log_interval, bool log_on_start);
+	Logger(size_t buffer_size, unsigned log_interval, bool log_on_start, bool log_name_timestamp);
 
 	~Logger();
 
@@ -93,10 +93,18 @@ private:
 
 	void run();
 
-	int create_log_dir();
+	/**
+	 * Create logging directory
+	 * @param tt if not null, use it for the directory name
+	 * @return 0 on success
+	 */
+	int create_log_dir(tm *tt);
 
 	static bool file_exist(const char *filename);
 
+	/**
+	 * Get log file name with directory (create it if necessary)
+	 */
 	int get_log_file_name(char *file_name, size_t file_name_size);
 
 	void start_log();
@@ -121,6 +129,14 @@ private:
 	 */
 	bool write_wait(void *ptr, size_t size);
 
+	/**
+	 * Get the time for log file name
+	 * @param tt returned time
+	 * @param boot_time use time when booted instead of current time
+	 * @return true on success, false otherwise (eg. if no gps)
+	 */
+	bool get_log_time(struct tm *tt, bool boot_time = false);
+
 	static constexpr size_t 	MAX_TOPICS_NUM = 128; /**< Maximum number of logged topics */
 	static constexpr unsigned	MAX_NO_LOGFOLDER = 999;	/**< Maximum number of log dirs */
 	static constexpr unsigned	MAX_NO_LOGFILE = 999;	/**< Maximum number of log files */
@@ -132,6 +148,7 @@ private:
 
 	bool						_task_should_exit = true;
 	char 						_log_dir[64];
+	bool						_has_log_dir = false;
 	bool						_enabled = false;
 
 	// statistics
@@ -141,10 +158,12 @@ private:
 	size_t						_write_dropouts = 0; ///< failed buffer writes due to buffer overflow
 	size_t						_high_water = 0; ///< maximum used write buffer
 
-	bool 						_log_on_start;
+	const bool 					_log_on_start;
+	const bool					_log_name_timestamp;
 	Array<LoggerSubscription, MAX_TOPICS_NUM>	_subscriptions;
 	LogWriter					_writer;
 	uint32_t					_log_interval;
+	param_t						_log_utc_offset;
 };
 
 Logger *logger_ptr = nullptr;

From 501544520ff7f6687e1c2abb96357292e5ef1224 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Beat=20K=C3=BCng?= <beat-kueng@gmx.net>
Date: Wed, 11 May 2016 10:10:14 +0200
Subject: [PATCH 0189/1230] logger: change some PX4_WARN to PX4_INFO and
 PX4_ERR

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

diff --git a/src/modules/logger/log_writer.cpp b/src/modules/logger/log_writer.cpp
index a720ff8b8c..2d99cf6b69 100644
--- a/src/modules/logger/log_writer.cpp
+++ b/src/modules/logger/log_writer.cpp
@@ -85,12 +85,12 @@ void LogWriter::start_log(const char *filename)
 	_fd = ::open(_filename, O_CREAT | O_WRONLY, PX4_O_MODE_666);
 
 	if (_fd < 0) {
-		PX4_WARN("can't open log file %s", _filename);
+		PX4_ERR("Can't open log file %s", _filename);
 		_should_run = false;
 		return;
 
 	} else {
-		PX4_WARN("opened log file: %s", _filename);
+		PX4_INFO("Opened log file: %s", _filename);
 		_should_run = true;
 		_running = true;
 	}
@@ -231,7 +231,7 @@ void LogWriter::run()
 						PX4_WARN("error closing log file");
 
 					} else {
-						PX4_WARN("closed logfile: %s, bytes written: %zu", _filename, _total_written);
+						PX4_INFO("closed logfile: %s, bytes written: %zu", _filename, _total_written);
 					}
 				}
 

From fc51f81bf5e5782dcb2027b1a5b7f88dfbff9e44 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Beat=20K=C3=BCng?= <beat-kueng@gmx.net>
Date: Wed, 11 May 2016 11:05:05 +0200
Subject: [PATCH 0190/1230] logger: add free space check (need at least 50MB to
 start)

---
 src/modules/logger/logger.cpp | 40 +++++++++++++++++++++++++++++++++++
 src/modules/logger/logger.h   |  7 ++++++
 2 files changed, 47 insertions(+)

diff --git a/src/modules/logger/logger.cpp b/src/modules/logger/logger.cpp
index 403dab2134..0df97f474e 100644
--- a/src/modules/logger/logger.cpp
+++ b/src/modules/logger/logger.cpp
@@ -47,6 +47,14 @@
 #include <px4_includes.h>
 #include <px4_getopt.h>
 #include <px4_log.h>
+#include <systemlib/mavlink_log.h>
+
+#ifdef __PX4_DARWIN
+#include <sys/param.h>
+#include <sys/mount.h>
+#else
+#include <sys/statfs.h>
+#endif
 
 #define GPS_EPOCH_SECS ((time_t)1234567890ULL)
 
@@ -452,6 +460,10 @@ void Logger::run()
 		return;
 	}
 
+	if (check_free_space() == 1) {
+		return;
+	}
+
 	uORB::Subscription<vehicle_status_s> vehicle_status_sub(ORB_ID(vehicle_status));
 	uORB::Subscription<parameter_update_s> parameter_update_sub(ORB_ID(parameter_update));
 
@@ -647,6 +659,11 @@ void Logger::run()
 	if (ret) {
 		PX4_WARN("join failed: %d", ret);
 	}
+
+	if (_mavlink_log_pub) {
+		orb_unadvertise(_mavlink_log_pub);
+		_mavlink_log_pub = nullptr;
+	}
 }
 
 int Logger::create_log_dir(tm *tt)
@@ -803,6 +820,9 @@ void Logger::start_log()
 		return;
 	}
 
+	/* print logging path, important to find log file later */
+	mavlink_log_info(&_mavlink_log_pub, "[logger] file: %s", file_name);
+
 	_writer.start_log(file_name);
 	write_version();
 	write_formats();
@@ -1001,5 +1021,25 @@ void Logger::write_changed_parameters()
 	_writer.notify();
 }
 
+int Logger::check_free_space()
+{
+	/* use statfs to determine the number of blocks left */
+	struct statfs statfs_buf;
+
+	if (statfs(LOG_ROOT, &statfs_buf) != 0) {
+		return PX4_ERROR;
+	}
+
+	/* use a threshold of 50 MiB */
+	if (statfs_buf.f_bavail < (px4_statfs_buf_f_bavail_t)(50 * 1024 * 1024 / statfs_buf.f_bsize)) {
+		mavlink_and_console_log_critical(&_mavlink_log_pub,
+						 "[logger] Not logging; SD almost full: %u MiB",
+						 (unsigned int)(statfs_buf.f_bavail / 1024U * statfs_buf.f_bsize / 1024U));
+		return 1;
+	}
+
+	return PX4_OK;
+}
+
 }
 }
diff --git a/src/modules/logger/logger.h b/src/modules/logger/logger.h
index 7ce35bf816..0022b05e06 100644
--- a/src/modules/logger/logger.h
+++ b/src/modules/logger/logger.h
@@ -107,6 +107,12 @@ private:
 	 */
 	int get_log_file_name(char *file_name, size_t file_name_size);
 
+	/**
+	 * Check if there is enough free space left on the SD Card
+	 * @return 0 on success, 1 if not enough space, <0 on error
+	 */
+	int check_free_space();
+
 	void start_log();
 
 	void stop_log();
@@ -164,6 +170,7 @@ private:
 	LogWriter					_writer;
 	uint32_t					_log_interval;
 	param_t						_log_utc_offset;
+	orb_advert_t					_mavlink_log_pub = nullptr;
 };
 
 Logger *logger_ptr = nullptr;

From fcf7e8b78c873fa9e64712a53c228a25f29cd1f6 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Beat=20K=C3=BCng?= <beat-kueng@gmx.net>
Date: Wed, 11 May 2016 15:32:29 +0200
Subject: [PATCH 0191/1230] logger: -e option only logs until disarm, add -f
 option to log until shutdown

---
 src/modules/logger/logger.cpp | 27 ++++++++++++++++++++++-----
 src/modules/logger/logger.h   |  4 +++-
 2 files changed, 25 insertions(+), 6 deletions(-)

diff --git a/src/modules/logger/logger.cpp b/src/modules/logger/logger.cpp
index 0df97f474e..31d1ead523 100644
--- a/src/modules/logger/logger.cpp
+++ b/src/modules/logger/logger.cpp
@@ -150,7 +150,8 @@ void Logger::usage(const char *reason)
 	PX4_INFO("usage: logger {start|stop|status} [-r <log rate>] [-b <buffer size>] -e -a -t -x\n"
 		 "\t-r\tLog rate in Hz, 0 means unlimited rate\n"
 		 "\t-b\tLog buffer size in KiB, default is 12\n"
-		 "\t-e\tEnable logging right after start (otherwise only when armed)\n"
+		 "\t-e\tEnable logging right after start until disarm (otherwise only when armed)\n"
+		 "\t-f\tLog until shutdown (implies -e)\n"
 		 "\t-t\tUse date/time for naming log directories and files");
 }
 
@@ -201,6 +202,7 @@ void Logger::run_trampoline(int argc, char *argv[])
 	unsigned log_interval = 3500;
 	int log_buffer_size = 12 * 1024;
 	bool log_on_start = false;
+	bool log_until_shutdown = false;
 	bool error_flag = false;
 	bool log_name_timestamp = false;
 
@@ -208,7 +210,7 @@ void Logger::run_trampoline(int argc, char *argv[])
 	int ch;
 	const char *myoptarg = NULL;
 
-	while ((ch = px4_getopt(argc, argv, "r:b:et", &myoptind, &myoptarg)) != EOF) {
+	while ((ch = px4_getopt(argc, argv, "r:b:etf", &myoptind, &myoptarg)) != EOF) {
 		switch (ch) {
 		case 'r': {
 				unsigned long r = strtoul(myoptarg, NULL, 10);
@@ -240,6 +242,11 @@ void Logger::run_trampoline(int argc, char *argv[])
 			log_name_timestamp = true;
 			break;
 
+		case 'f':
+			log_on_start = true;
+			log_until_shutdown = true;
+			break;
+
 		case '?':
 			error_flag = true;
 			break;
@@ -256,7 +263,8 @@ void Logger::run_trampoline(int argc, char *argv[])
 		return;
 	}
 
-	logger_ptr = new Logger(log_buffer_size, log_interval, log_on_start, log_name_timestamp);
+	logger_ptr = new Logger(log_buffer_size, log_interval, log_on_start,
+				log_until_shutdown, log_name_timestamp);
 
 #if defined(DBGPRINT) && defined(__PX4_NUTTX)
 	struct mallinfo alloc_info = mallinfo();
@@ -321,8 +329,9 @@ struct message_parameter_header_s {
 static constexpr size_t MAX_DATA_SIZE = 740;
 
 Logger::Logger(size_t buffer_size, unsigned log_interval, bool log_on_start,
-	       bool log_name_timestamp) :
+	       bool log_until_shutdown, bool log_name_timestamp) :
 	_log_on_start(log_on_start),
+	_log_until_shutdown(log_until_shutdown),
 	_log_name_timestamp(log_name_timestamp),
 	_writer(buffer_size),
 	_log_interval(log_interval)
@@ -524,7 +533,7 @@ void Logger::run()
 			bool armed = (vehicle_status_sub.get().arming_state == vehicle_status_s::ARMING_STATE_ARMED) ||
 				     (vehicle_status_sub.get().arming_state == vehicle_status_s::ARMING_STATE_ARMED_ERROR);
 
-			if (_enabled != armed && !_log_on_start) {
+			if (_enabled != armed && !_log_until_shutdown) {
 				if (armed) {
 					start_log();
 
@@ -811,6 +820,10 @@ bool Logger::get_log_time(struct tm *tt, bool boot_time)
 
 void Logger::start_log()
 {
+	if (_enabled) {
+		return;
+	}
+
 	PX4_INFO("start log");
 
 	char file_name[64] = "";
@@ -834,6 +847,10 @@ void Logger::start_log()
 
 void Logger::stop_log()
 {
+	if (!_enabled) {
+		return;
+	}
+
 	_enabled = false;
 	_writer.stop_log();
 }
diff --git a/src/modules/logger/logger.h b/src/modules/logger/logger.h
index 0022b05e06..9f92f86672 100644
--- a/src/modules/logger/logger.h
+++ b/src/modules/logger/logger.h
@@ -74,7 +74,8 @@ struct LoggerSubscription {
 class Logger
 {
 public:
-	Logger(size_t buffer_size, unsigned log_interval, bool log_on_start, bool log_name_timestamp);
+	Logger(size_t buffer_size, unsigned log_interval, bool log_on_start,
+	       bool log_until_shutdown, bool log_name_timestamp);
 
 	~Logger();
 
@@ -165,6 +166,7 @@ private:
 	size_t						_high_water = 0; ///< maximum used write buffer
 
 	const bool 					_log_on_start;
+	const bool 					_log_until_shutdown;
 	const bool					_log_name_timestamp;
 	Array<LoggerSubscription, MAX_TOPICS_NUM>	_subscriptions;
 	LogWriter					_writer;

From 197b37fc17cf95c39ce1e17fdecb14283c4cfecc Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Beat=20K=C3=BCng?= <beat-kueng@gmx.net>
Date: Fri, 13 May 2016 15:01:33 +0200
Subject: [PATCH 0192/1230] fix cpuload.msg: remove timestamp (cleanup after
 rebase)

---
 msg/cpuload.msg | 1 -
 1 file changed, 1 deletion(-)

diff --git a/msg/cpuload.msg b/msg/cpuload.msg
index a8d84dd53c..ef85fa58a3 100644
--- a/msg/cpuload.msg
+++ b/msg/cpuload.msg
@@ -1,2 +1 @@
-uint64 timestamp                # in microseconds since system start, is set whenever the writing thread stores new data
 float32 load                    # processor load from 0 to 1

From 408f299dbb6aa0588e05f7bd1e4fb4788c2d882d Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Beat=20K=C3=BCng?= <beat-kueng@gmx.net>
Date: Fri, 13 May 2016 15:06:57 +0200
Subject: [PATCH 0193/1230] cmake px4fmu-v2_test: disable sdlog2 (avoids flash
 overflow)

---
 cmake/configs/nuttx_px4fmu-v2_test.cmake | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/cmake/configs/nuttx_px4fmu-v2_test.cmake b/cmake/configs/nuttx_px4fmu-v2_test.cmake
index b1fc89ccc2..f65c74617c 100644
--- a/cmake/configs/nuttx_px4fmu-v2_test.cmake
+++ b/cmake/configs/nuttx_px4fmu-v2_test.cmake
@@ -111,7 +111,7 @@ set(config_module_list
 	#
 	# Logging
 	#
-	modules/sdlog2
+	#modules/sdlog2
 
 	#
 	# Library modules

From 0f30bfa0aced15a0c081c7468399d27defbc0b57 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Beat=20K=C3=BCng?= <beat-kueng@gmx.net>
Date: Fri, 13 May 2016 15:52:15 +0200
Subject: [PATCH 0194/1230] logger: fix -e parameter (logger immediately
 stopped again after start)

---
 src/modules/logger/logger.cpp | 3 ++-
 src/modules/logger/logger.h   | 1 +
 2 files changed, 3 insertions(+), 1 deletion(-)

diff --git a/src/modules/logger/logger.cpp b/src/modules/logger/logger.cpp
index 31d1ead523..fb5b3866f6 100644
--- a/src/modules/logger/logger.cpp
+++ b/src/modules/logger/logger.cpp
@@ -533,7 +533,8 @@ void Logger::run()
 			bool armed = (vehicle_status_sub.get().arming_state == vehicle_status_s::ARMING_STATE_ARMED) ||
 				     (vehicle_status_sub.get().arming_state == vehicle_status_s::ARMING_STATE_ARMED_ERROR);
 
-			if (_enabled != armed && !_log_until_shutdown) {
+			if (_was_armed != armed && !_log_until_shutdown) {
+				_was_armed = armed;
 				if (armed) {
 					start_log();
 
diff --git a/src/modules/logger/logger.h b/src/modules/logger/logger.h
index 9f92f86672..36b4bae958 100644
--- a/src/modules/logger/logger.h
+++ b/src/modules/logger/logger.h
@@ -157,6 +157,7 @@ private:
 	char 						_log_dir[64];
 	bool						_has_log_dir = false;
 	bool						_enabled = false;
+	bool						_was_armed = false;
 
 	// statistics
 	hrt_abstime					_start_time; ///< Time when logging started (not the logger thread)

From 4b8152465d9ad7c27c4a5956bbf6b71f5ca5a2bb Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Beat=20K=C3=BCng?= <beat-kueng@gmx.net>
Date: Fri, 13 May 2016 16:23:34 +0200
Subject: [PATCH 0195/1230] logger: unsubscribe from all topics when logger
 exits

---
 src/modules/logger/logger.cpp | 10 ++++++++++
 1 file changed, 10 insertions(+)

diff --git a/src/modules/logger/logger.cpp b/src/modules/logger/logger.cpp
index fb5b3866f6..c763c40f3f 100644
--- a/src/modules/logger/logger.cpp
+++ b/src/modules/logger/logger.cpp
@@ -670,6 +670,16 @@ void Logger::run()
 		PX4_WARN("join failed: %d", ret);
 	}
 
+	//unsubscribe
+	for (LoggerSubscription &sub : _subscriptions) {
+		for (uint8_t instance = 0; instance < ORB_MULTI_MAX_INSTANCES; instance++) {
+			if (sub.fd[instance] != -1) {
+				orb_unsubscribe(sub.fd[instance]);
+				sub.fd[instance] = -1;
+			}
+		}
+	}
+
 	if (_mavlink_log_pub) {
 		orb_unadvertise(_mavlink_log_pub);
 		_mavlink_log_pub = nullptr;

From d082060429dfed5651c31a281779c3d1a3377af7 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Beat=20K=C3=BCng?= <beat-kueng@gmx.net>
Date: Fri, 13 May 2016 20:00:56 +0200
Subject: [PATCH 0196/1230] commander: fix wrong #ifdef header guard in
 state_machine_helper_test.h

---
 .../commander/commander_tests/state_machine_helper_test.h     | 4 +---
 1 file changed, 1 insertion(+), 3 deletions(-)

diff --git a/src/modules/commander/commander_tests/state_machine_helper_test.h b/src/modules/commander/commander_tests/state_machine_helper_test.h
index cf67190955..ad502618ca 100644
--- a/src/modules/commander/commander_tests/state_machine_helper_test.h
+++ b/src/modules/commander/commander_tests/state_machine_helper_test.h
@@ -36,9 +36,7 @@
  * @file state_machine_helper_test.h
  */
 
-#ifndef STATE_MACHINE_HELPER_TEST_H_
-#define STATE_MACHINE_HELPER_TEST_
+#pragma once
 
 bool stateMachineHelperTest(void);
 
-#endif /* STATE_MACHINE_HELPER_TEST_H_ */

From cf667dedb8b6f4f314860523627bbc5c796387ef Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Beat=20K=C3=BCng?= <beat-kueng@gmx.net>
Date: Fri, 13 May 2016 20:04:31 +0200
Subject: [PATCH 0197/1230] tests: increase stack size from 8000 to 9000

clang failed with:
../src/systemcmds/tests/test_mathlib.cpp:56:5: fatal error: stack frame
size of 7400 bytes in function 'test_mathlib' [-Wframe-larger-than=]
int test_mathlib(int argc, char *argv[])
---
 src/systemcmds/tests/CMakeLists.txt | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/src/systemcmds/tests/CMakeLists.txt b/src/systemcmds/tests/CMakeLists.txt
index 460c46ac73..60f9cc885d 100644
--- a/src/systemcmds/tests/CMakeLists.txt
+++ b/src/systemcmds/tests/CMakeLists.txt
@@ -69,9 +69,9 @@ endif()
 px4_add_module(
 	MODULE systemcmds__tests
 	MAIN tests
-	STACK_MAIN 8000
+	STACK_MAIN 9000
 	COMPILE_FLAGS
-		-Wframe-larger-than=6000
+		-Wframe-larger-than=8000
 		-Wno-float-equal
 		-O0
 	SRCS ${srcs}

From 9c32792017e5d63779c4e3ab5d3ecb0150d19ac9 Mon Sep 17 00:00:00 2001
From: Daniel Agar <daniel@agar.ca>
Date: Fri, 13 May 2016 17:14:44 -0400
Subject: [PATCH 0198/1230] param_test link libmsg_gen

---
 src/modules/systemlib/param/param.c | 4 +++-
 unittests/CMakeLists.txt            | 1 +
 2 files changed, 4 insertions(+), 1 deletion(-)

diff --git a/src/modules/systemlib/param/param.c b/src/modules/systemlib/param/param.c
index 5958399252..1973704738 100644
--- a/src/modules/systemlib/param/param.c
+++ b/src/modules/systemlib/param/param.c
@@ -235,7 +235,9 @@ param_find_changed(param_t param)
 static void
 param_notify_changes(bool is_saved)
 {
-	struct parameter_update_s pup = { .timestamp = hrt_absolute_time(), .saved = is_saved};
+	struct parameter_update_s pup;
+	pup.timestamp = hrt_absolute_time();
+	pup.saved = is_saved;
 
 	/*
 	 * If we don't have a handle to our topic, create one now; otherwise
diff --git a/unittests/CMakeLists.txt b/unittests/CMakeLists.txt
index edaaa60423..5d39cc4bb7 100644
--- a/unittests/CMakeLists.txt
+++ b/unittests/CMakeLists.txt
@@ -170,6 +170,7 @@ add_gtest(sf0x_test)
 add_executable(param_test param_test.cpp uorb_stub.cpp
 						${PX4_SRC}/modules/systemlib/bson/tinybson.c
 						${PX4_SRC}/modules/systemlib/param/param.c)
+target_link_libraries(param_test ${PX4_SITL_BUILD}/libmsg_gen.a)
 add_gtest(param_test)
 
 # param_shmem_test

From c57bc26d5bc865a42081055e086b2043260cab2c Mon Sep 17 00:00:00 2001
From: Daniel Agar <daniel@agar.ca>
Date: Fri, 13 May 2016 17:36:21 -0400
Subject: [PATCH 0199/1230] fully restore px4fmu-v2_lpe

---
 Makefile                                |   2 +-
 cmake/configs/nuttx_px4fmu-v2_lpe.cmake | 196 ++++++++++++++++++++++++
 2 files changed, 197 insertions(+), 1 deletion(-)
 create mode 100644 cmake/configs/nuttx_px4fmu-v2_lpe.cmake

diff --git a/Makefile b/Makefile
index 3f9922144f..85fb00742a 100644
--- a/Makefile
+++ b/Makefile
@@ -240,7 +240,7 @@ ifeq ($(VECTORCONTROL),1)
 	@(rm -rf vectorcontrol && git clone --quiet --depth 1 https://github.com/thiemar/vectorcontrol.git && cd vectorcontrol && BOARD=s2740vc_1_0 make --silent --no-print-directory && BOARD=px4esc_1_6 make --silent --no-print-directory && ../Tools/uavcan_copy.sh)
 endif
 
-check: check_px4fmu-v1_default check_px4fmu-v2_default check_px4fmu-v2_test check_px4fmu-v2_ekf2 check_px4fmu-v4_default_and_uavcan check_mindpx-v2_default check_px4-stm32f4discovery_default check_posix_sitl_default check_posix_sitl_test check_unittest check_format
+check: check_px4fmu-v1_default check_px4fmu-v2_default check_px4fmu-v2_test check_px4fmu-v2_lpe check_px4fmu-v2_ekf2 check_px4fmu-v4_default_and_uavcan check_mindpx-v2_default check_px4-stm32f4discovery_default check_posix_sitl_default check_posix_sitl_test check_unittest check_format
 
 check_format:
 	$(call colorecho,"Checking formatting with astyle")
diff --git a/cmake/configs/nuttx_px4fmu-v2_lpe.cmake b/cmake/configs/nuttx_px4fmu-v2_lpe.cmake
new file mode 100644
index 0000000000..dce99d090c
--- /dev/null
+++ b/cmake/configs/nuttx_px4fmu-v2_lpe.cmake
@@ -0,0 +1,196 @@
+include(nuttx/px4_impl_nuttx)
+
+set(CMAKE_TOOLCHAIN_FILE ${CMAKE_SOURCE_DIR}/cmake/toolchains/Toolchain-arm-none-eabi.cmake)
+
+set(config_uavcan_num_ifaces 2)
+
+set(config_module_list
+	#
+	# Board support modules
+	#
+	drivers/device
+	drivers/stm32
+	drivers/stm32/adc
+	drivers/stm32/tone_alarm
+	drivers/led
+	drivers/px4fmu
+	drivers/px4io
+	drivers/boards/px4fmu-v2
+	drivers/rgbled
+	drivers/mpu6000
+	drivers/mpu9250
+	drivers/lsm303d
+	drivers/l3gd20
+	drivers/hmc5883
+	drivers/ms5611
+	#drivers/mb12xx
+	drivers/srf02
+	drivers/sf0x
+	drivers/ll40ls
+	drivers/trone
+	drivers/gps
+	drivers/pwm_out_sim
+	#drivers/hott
+	#drivers/hott/hott_telemetry
+	#drivers/hott/hott_sensors
+	drivers/blinkm
+	drivers/airspeed
+	drivers/ets_airspeed
+	drivers/meas_airspeed
+	drivers/frsky_telemetry
+	modules/sensors
+	#drivers/mkblctrl
+	drivers/px4flow
+	#drivers/oreoled
+	drivers/gimbal
+	drivers/pwm_input
+	drivers/camera_trigger
+	drivers/bst
+	drivers/snapdragon_rc_pwm
+	drivers/lis3mdl
+	drivers/bmi160
+
+	#
+	# System commands
+	#
+	systemcmds/bl_update
+	systemcmds/mixer
+	systemcmds/param
+	systemcmds/perf
+	systemcmds/pwm
+	systemcmds/esc_calib
+	systemcmds/reboot
+	#systemcmds/topic_listener
+	systemcmds/top
+	systemcmds/config
+	systemcmds/nshterm
+	systemcmds/mtd
+	systemcmds/dumpfile
+	systemcmds/ver
+
+	#
+	# General system control
+	#
+	modules/commander
+	modules/load_mon
+	modules/navigator
+	modules/mavlink
+	modules/gpio_led
+	modules/uavcan
+	modules/land_detector
+
+	#
+	# Estimation modules (EKF/ SO3 / other filters)
+	#
+	modules/attitude_estimator_q
+	#modules/ekf_att_pos_estimator
+	#modules/position_estimator_inav
+	modules/local_position_estimator
+
+	#
+	# Vehicle Control
+	#
+	# modules/segway # XXX Needs GCC 4.7 fix
+	modules/fw_pos_control_l1
+	modules/fw_att_control
+	modules/mc_att_control
+	modules/mc_pos_control
+	modules/vtol_att_control
+
+	#
+	# Logging
+	#
+	modules/logger
+	modules/sdlog2
+
+	#
+	# Library modules
+	#
+	modules/param
+	modules/systemlib
+	modules/systemlib/mixer
+	modules/uORB
+	modules/dataman
+
+	#
+	# Libraries
+	#
+	lib/controllib
+	lib/mathlib
+	lib/mathlib/math/filter
+	lib/ecl
+	lib/external_lgpl
+	lib/geo
+	lib/geo_lookup
+	lib/conversion
+	lib/launchdetection
+	lib/terrain_estimation
+	lib/runway_takeoff
+	lib/tailsitter_recovery
+	lib/DriverFramework/framework
+	platforms/nuttx
+
+	# had to add for cmake, not sure why wasn't in original config
+	platforms/common 
+	platforms/nuttx/px4_layer
+
+	#
+	# OBC challenge
+	#
+	#modules/bottle_drop
+
+	#
+	# Rover apps
+	#
+	#examples/rover_steering_control
+
+	#
+	# Demo apps
+	#
+	#examples/math_demo
+	# Tutorial code from
+	# https://px4.io/dev/px4_simple_app
+	#examples/px4_simple_app
+
+	# Tutorial code from
+	# https://px4.io/dev/daemon
+	#examples/px4_daemon_app
+
+	# Tutorial code from
+	# https://px4.io/dev/debug_values
+	#examples/px4_mavlink_debug
+
+	# Tutorial code from
+	# https://px4.io/dev/example_fixedwing_control
+	#examples/fixedwing_control
+
+	# Hardware test
+	#examples/hwtest
+)
+
+set(config_extra_builtin_cmds
+	serdis
+	sercon
+	)
+
+set(config_io_board
+	px4io-v2
+	)
+
+set(config_extra_libs
+	uavcan
+	uavcan_stm32_driver
+	)
+
+set(config_io_extra_libs
+	)
+
+add_custom_target(sercon)
+set_target_properties(sercon PROPERTIES
+	PRIORITY "SCHED_PRIORITY_DEFAULT"
+	MAIN "sercon" STACK_MAIN "2048")
+
+add_custom_target(serdis)
+set_target_properties(serdis PROPERTIES
+	PRIORITY "SCHED_PRIORITY_DEFAULT"
+	MAIN "serdis" STACK_MAIN "2048")

From 97bcea292e866500f07e5fc24f1c8d9989c0e08e Mon Sep 17 00:00:00 2001
From: Daniel Agar <daniel@agar.ca>
Date: Fri, 13 May 2016 17:36:50 -0400
Subject: [PATCH 0200/1230] logger.cpp fix style

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

diff --git a/src/modules/logger/logger.cpp b/src/modules/logger/logger.cpp
index c763c40f3f..90c311a58f 100644
--- a/src/modules/logger/logger.cpp
+++ b/src/modules/logger/logger.cpp
@@ -535,6 +535,7 @@ void Logger::run()
 
 			if (_was_armed != armed && !_log_until_shutdown) {
 				_was_armed = armed;
+
 				if (armed) {
 					start_log();
 

From 6a07d6167105b20b094c0199407eb011ebe923a8 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Beat=20K=C3=BCng?= <beat-kueng@gmx.net>
Date: Thu, 12 May 2016 16:11:26 +0200
Subject: [PATCH 0201/1230] parameter source parser: validate length of
 parameter name (limited to 16)

---
 Tools/px4params/srcparser.py | 3 +++
 1 file changed, 3 insertions(+)

diff --git a/Tools/px4params/srcparser.py b/Tools/px4params/srcparser.py
index c4f1b54e43..b4ad401a9d 100644
--- a/Tools/px4params/srcparser.py
+++ b/Tools/px4params/srcparser.py
@@ -284,6 +284,9 @@ class SourceParser(object):
         for group in self.GetParamGroups():
             for param in group.GetParams():
                 name  = param.GetName()
+                if len(name) > 16:
+                    sys.stderr.write("Parameter Name {0} is too long (Limit is 16)\n".format(name))
+                    return False
                 board = param.GetFieldValue("board")
                 # Check for duplicates
                 name_plus_board = name + "+" + board

From ee33f21303cb66d6ed85a68a33d37ec2fef76aaa Mon Sep 17 00:00:00 2001
From: CarlOlsson <carlolsson.co@gmail.com>
Date: Thu, 12 May 2016 14:43:14 +0200
Subject: [PATCH 0202/1230] added airspeed to ekf2 replay

---
 src/modules/ekf2_replay/ekf2_replay_main.cpp | 35 +++++++++++++++++++-
 src/modules/sdlog2/sdlog2.c                  |  2 +-
 src/modules/sdlog2/sdlog2_messages.h         |  2 +-
 3 files changed, 36 insertions(+), 3 deletions(-)

diff --git a/src/modules/ekf2_replay/ekf2_replay_main.cpp b/src/modules/ekf2_replay/ekf2_replay_main.cpp
index 30f22620f5..bb212935d6 100644
--- a/src/modules/ekf2_replay/ekf2_replay_main.cpp
+++ b/src/modules/ekf2_replay/ekf2_replay_main.cpp
@@ -69,6 +69,7 @@
 #include <uORB/topics/vehicle_land_detected.h>
 #include <uORB/topics/optical_flow.h>
 #include <uORB/topics/distance_sensor.h>
+#include <uORB/topics/airspeed.h>
 
 #include <sdlog2/sdlog2_messages.h>
 
@@ -133,6 +134,7 @@ private:
 	orb_advert_t _landed_pub;
 	orb_advert_t _flow_pub;
 	orb_advert_t _range_pub;
+	orb_advert_t _airspeed_pub;
 
 	int _att_sub;
 	int _estimator_status_sub;
@@ -148,12 +150,14 @@ private:
 	struct vehicle_land_detected_s _land_detected;
 	struct optical_flow_s _flow;
 	struct distance_sensor_s _range;
+	struct airspeed_s _airspeed;
 
 	unsigned _message_counter; // counter which will increase with every message read from the log
 	unsigned _part1_counter_ref;		// this is the value of _message_counter when the part1 of the replay message is read (imu data)
 	bool _read_part2;				// indicates if part 2 of replay message has been read
 	bool _read_part3;
 	bool _read_part4;
+	bool _read_part6;
 
 	int _write_fd = -1;
 	px4_pollfd_struct_t _fds[1];
@@ -201,6 +205,7 @@ Ekf2Replay::Ekf2Replay(char *logfile) :
 	_landed_pub(nullptr),
 	_flow_pub(nullptr),
 	_range_pub(nullptr),
+	_airspeed_pub(nullptr),
 	_att_sub(-1),
 	_estimator_status_sub(-1),
 	_innov_sub(-1),
@@ -217,6 +222,7 @@ Ekf2Replay::Ekf2Replay(char *logfile) :
 	_read_part2(false),
 	_read_part3(false),
 	_read_part4(false),
+	_read_part6(false),
 	_write_fd(-1)
 {
 	// build the path to the log
@@ -270,6 +276,14 @@ void Ekf2Replay::publishEstimatorInput()
 	} else if (_sensors_pub != nullptr) {
 		orb_publish(ORB_ID(sensor_combined), _sensors_pub, &_sensors);
 	}
+
+	if (_airspeed_pub == nullptr && _read_part6) {
+		_airspeed_pub = orb_advertise(ORB_ID(airspeed), &_airspeed);
+	} else if (_airspeed_pub != nullptr) {
+		orb_publish(ORB_ID(airspeed), _airspeed_pub, &_airspeed);
+	}
+
+	_read_part6 = false;
 }
 
 void Ekf2Replay::parseMessage(uint8_t *source, uint8_t *destination, uint8_t type)
@@ -332,6 +346,7 @@ void Ekf2Replay::setEstimatorInput(uint8_t *data, uint8_t type)
 	struct log_RPL2_s replay_part2 = {};
 	struct log_RPL3_s replay_part3 = {};
 	struct log_RPL4_s replay_part4 = {};
+	struct log_RPL6_s replay_part6 = {};
 	struct log_LAND_s vehicle_landed = {};
 
 	if (type == LOG_RPL1_MSG) {
@@ -394,7 +409,22 @@ void Ekf2Replay::setEstimatorInput(uint8_t *data, uint8_t type)
 		_range.current_distance = replay_part4.range_to_ground;
 		_read_part4 = true;
 
-	} else if (type == LOG_LAND_MSG) {
+	} 
+
+	else if (type == LOG_RPL6_MSG){
+		uint8_t *dest_ptr = (uint8_t *)&replay_part6.time_airs_usec;
+		parseMessage(data, dest_ptr, type);
+		_airspeed.timestamp = replay_part6.time_airs_usec;
+		_airspeed.indicated_airspeed_m_s = replay_part6.indicated_airspeed_m_s;
+		_airspeed.true_airspeed_m_s = replay_part6.true_airspeed_m_s;
+		_airspeed.true_airspeed_unfiltered_m_s = replay_part6.true_airspeed_unfiltered_m_s;
+		_airspeed.air_temperature_celsius = replay_part6.air_temperature_celsius;
+		_airspeed.confidence = replay_part6.confidence;
+		_read_part6 = true;
+
+	}
+
+	else if (type == LOG_LAND_MSG) {
 		uint8_t *dest_ptr = (uint8_t *)&vehicle_landed.landed;
 		parseMessage(data, dest_ptr, type);
 		_land_detected.landed =  vehicle_landed.landed;
@@ -582,6 +612,9 @@ void Ekf2Replay::logIfUpdated()
 
 		log_message.body.innov2.s[6] = innov.heading_innov;
 		log_message.body.innov2.s[7] = innov.heading_innov_var;
+		log_message.body.innov2.s[8] = innov.airspeed_innov;
+		log_message.body.innov2.s[9] = innov.airspeed_innov_var;
+
 		writeMessage(_write_fd, (void *)&log_message.head1, _formats[LOG_EST5_MSG].length);
 
 		// optical flow innovations and innovation variances
diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c
index 0193d90600..351ad1da59 100644
--- a/src/modules/sdlog2/sdlog2.c
+++ b/src/modules/sdlog2/sdlog2.c
@@ -1590,7 +1590,7 @@ int sdlog2_thread_main(int argc, char *argv[])
 
 				if (buf.replay.asp_timestamp > 0) {
 					log_msg.msg_type = LOG_RPL6_MSG;
-					log_msg.body.log_RPL6.timestamp = buf.replay.asp_timestamp;
+					log_msg.body.log_RPL6.time_airs_usec = buf.replay.asp_timestamp;
 					log_msg.body.log_RPL6.indicated_airspeed_m_s = buf.replay.indicated_airspeed_m_s;
 					log_msg.body.log_RPL6.true_airspeed_m_s = buf.replay.true_airspeed_m_s;
 					log_msg.body.log_RPL6.true_airspeed_unfiltered_m_s = buf.replay.true_airspeed_unfiltered_m_s;
diff --git a/src/modules/sdlog2/sdlog2_messages.h b/src/modules/sdlog2/sdlog2_messages.h
index 900f02a472..12942fde79 100644
--- a/src/modules/sdlog2/sdlog2_messages.h
+++ b/src/modules/sdlog2/sdlog2_messages.h
@@ -579,7 +579,7 @@ struct log_RPL4_s {
 /* --- EKF2 REPLAY Part 4 --- */
 #define LOG_RPL6_MSG 59
 struct log_RPL6_s {
-	uint64_t timestamp;
+	uint64_t time_airs_usec;
 	float indicated_airspeed_m_s;
 	float true_airspeed_m_s;
 	float true_airspeed_unfiltered_m_s;

From b8b05b1b4b36848f59bab34e2e4ef626a4ea3150 Mon Sep 17 00:00:00 2001
From: Paul Riseborough <p_riseborough@live.com.au>
Date: Sat, 14 May 2016 21:31:19 +1000
Subject: [PATCH 0203/1230] ecl: update submodule reference

Fixes error in calculation of observation variance used by terrain estimator.
---
 src/lib/ecl | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/src/lib/ecl b/src/lib/ecl
index c7e225124c..172f4be594 160000
--- a/src/lib/ecl
+++ b/src/lib/ecl
@@ -1 +1 @@
-Subproject commit c7e225124c4303238da1e8e4431d23a8ff81aec0
+Subproject commit 172f4be594a0b33997a07f5b46899c141a6cee86

From b2a223eaabf6527018a89dfa8cbf81b84b2d7afc Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Sat, 14 May 2016 17:21:46 +0200
Subject: [PATCH 0204/1230] Move logging to main rcS to save RAM

---
 ROMFS/px4fmu_common/init.d/rc.logging | 40 ---------------------------
 ROMFS/px4fmu_common/init.d/rcS        | 33 +++++++++++++++++++++-
 2 files changed, 32 insertions(+), 41 deletions(-)
 delete mode 100644 ROMFS/px4fmu_common/init.d/rc.logging

diff --git a/ROMFS/px4fmu_common/init.d/rc.logging b/ROMFS/px4fmu_common/init.d/rc.logging
deleted file mode 100644
index b88f46ae86..0000000000
--- a/ROMFS/px4fmu_common/init.d/rc.logging
+++ /dev/null
@@ -1,40 +0,0 @@
-#!nsh
-#
-# Initialize logging services.
-#
-
-if [ -d /fs/microsd ]
-then
-	if ver hwcmp PX4FMU_V1
-	then
-		if sdlog2 start -r 30 -a -b 2 -t
-		then
-		fi
-	else
-		# check if we should increase logging rate for ekf2 replay message logging
-		if param greater EKF2_REC_RPL 0
-		then
-			if param compare SYS_LOGGER 0
-			then
-				if sdlog2 start -r 500 -e -b 20 -t
-				then
-				fi
-			else
-				if logger start -r 500
-				then
-				fi
-			fi
-		else
-			if param compare SYS_LOGGER 0
-			then
-				if sdlog2 start -r 100 -a -b 12 -t
-				then
-				fi
-			else
-				if logger start -b 24
-				then
-				fi
-			fi
-		fi
-	fi
-fi
diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS
index cfcdfa5397..98f9e688a3 100644
--- a/ROMFS/px4fmu_common/init.d/rcS
+++ b/ROMFS/px4fmu_common/init.d/rcS
@@ -840,7 +840,38 @@ then
 	#
 	# Logging
 	#
-	sh /etc/init.d/rc.logging
+	if ver hwcmp PX4FMU_V1
+	then
+		if sdlog2 start -r 30 -a -b 2 -t
+		then
+		fi
+	else
+		# check if we should increase logging rate for ekf2 replay message logging
+		if param greater EKF2_REC_RPL 0
+		then
+			if param compare SYS_LOGGER 0
+			then
+				if sdlog2 start -r 500 -e -b 20 -t
+				then
+				fi
+			else
+				if logger start -r 500
+				then
+				fi
+			fi
+		else
+			if param compare SYS_LOGGER 0
+			then
+				if sdlog2 start -r 100 -a -b 12 -t
+				then
+				fi
+			else
+				if logger start -b 24
+				then
+				fi
+			fi
+		fi
+	fi
 
 	# Start any custom addons
 	set FEXTRAS /fs/microsd/etc/extras.txt

From 91142f0c2079da20a30c2dac426dbf6b4313c5aa Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Sat, 14 May 2016 18:05:14 +0200
Subject: [PATCH 0205/1230] PWM cmd: better reporting

---
 src/systemcmds/pwm/pwm.c | 14 +++++++-------
 1 file changed, 7 insertions(+), 7 deletions(-)

diff --git a/src/systemcmds/pwm/pwm.c b/src/systemcmds/pwm/pwm.c
index a119952963..1457e7fe2d 100644
--- a/src/systemcmds/pwm/pwm.c
+++ b/src/systemcmds/pwm/pwm.c
@@ -350,11 +350,11 @@ pwm_main(int argc, char *argv[])
 	} else if (!strcmp(argv[1], "min")) {
 
 		if (set_mask == 0) {
-			usage("no channels set");
+			usage("min: no channels set");
 		}
 
 		if (pwm_value == 0) {
-			usage("no PWM value provided");
+			usage("min: no PWM value provided");
 		}
 
 		struct pwm_output_values pwm_values;
@@ -381,7 +381,7 @@ pwm_main(int argc, char *argv[])
 		}
 
 		if (pwm_values.channel_count == 0) {
-			usage("no PWM values added");
+			usage("min: no channels provided");
 
 		} else {
 
@@ -428,7 +428,7 @@ pwm_main(int argc, char *argv[])
 		}
 
 		if (pwm_values.channel_count == 0) {
-			usage("no PWM values added");
+			usage("max: no PWM channels");
 
 		} else {
 
@@ -475,7 +475,7 @@ pwm_main(int argc, char *argv[])
 		}
 
 		if (pwm_values.channel_count == 0) {
-			usage("no PWM values added");
+			usage("disarmed: no PWM channels");
 
 		} else {
 
@@ -495,7 +495,7 @@ pwm_main(int argc, char *argv[])
 		}
 
 		if (pwm_value == 0) {
-			usage("no PWM provided");
+			usage("failsafe: no PWM provided");
 		}
 
 		struct pwm_output_values pwm_values;
@@ -522,7 +522,7 @@ pwm_main(int argc, char *argv[])
 		}
 
 		if (pwm_values.channel_count == 0) {
-			usage("no PWM values added");
+			usage("failsafe: no PWM channels");
 
 		} else {
 

From 06fec2bce08fba0682a0b392ffc12c6f56871e21 Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Sat, 14 May 2016 18:05:24 +0200
Subject: [PATCH 0206/1230] IO driver: Fix PWM load

---
 src/drivers/px4io/px4io.cpp | 58 ++++++++++++++++++++++---------------
 1 file changed, 35 insertions(+), 23 deletions(-)

diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp
index a1e1525514..01e8d46ff1 100644
--- a/src/drivers/px4io/px4io.cpp
+++ b/src/drivers/px4io/px4io.cpp
@@ -683,7 +683,7 @@ PX4IO::init()
 	_max_transfer  = io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_MAX_TRANSFER) - 2;
 	_max_rc_input  = io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_RC_INPUT_COUNT);
 
-	if ((_max_actuators < 1) || (_max_actuators > 255) ||
+	if ((_max_actuators < 1) || (_max_actuators > 16) ||
 	    (_max_relays > 32)   ||
 	    (_max_transfer < 16) || (_max_transfer > 255)  ||
 	    (_max_rc_input < 1)  || (_max_rc_input > 255)) {
@@ -2471,16 +2471,19 @@ PX4IO::ioctl(file *filep, int cmd, unsigned long arg)
 			break;
 		}
 
-	case PWM_SERVO_GET_FAILSAFE_PWM:
+	case PWM_SERVO_GET_FAILSAFE_PWM: {
+			struct pwm_output_values *pwm = (struct pwm_output_values *)arg;
+			pwm->channel_count = _max_actuators;
 
-		ret = io_reg_get(PX4IO_PAGE_FAILSAFE_PWM, 0, (uint16_t *)arg, _max_actuators);
+			ret = io_reg_get(PX4IO_PAGE_FAILSAFE_PWM, 0, pwm->values, _max_actuators);
 
-		if (ret != OK) {
-			ret = -EIO;
+			if (ret != OK) {
+				ret = -EIO;
+			}
+
+			break;
 		}
 
-		break;
-
 	case PWM_SERVO_SET_DISARMED_PWM: {
 			struct pwm_output_values *pwm = (struct pwm_output_values *)arg;
 
@@ -2495,16 +2498,19 @@ PX4IO::ioctl(file *filep, int cmd, unsigned long arg)
 			break;
 		}
 
-	case PWM_SERVO_GET_DISARMED_PWM:
+	case PWM_SERVO_GET_DISARMED_PWM: {
+			struct pwm_output_values *pwm = (struct pwm_output_values *)arg;
+			pwm->channel_count = _max_actuators;
 
-		ret = io_reg_get(PX4IO_PAGE_DISARMED_PWM, 0, (uint16_t *)arg, _max_actuators);
+			ret = io_reg_get(PX4IO_PAGE_DISARMED_PWM, 0, pwm->values, _max_actuators);
 
-		if (ret != OK) {
-			ret = -EIO;
+			if (ret != OK) {
+				ret = -EIO;
+			}
+
+			break;
 		}
 
-		break;
-
 	case PWM_SERVO_SET_MIN_PWM: {
 			struct pwm_output_values *pwm = (struct pwm_output_values *)arg;
 
@@ -2519,16 +2525,19 @@ PX4IO::ioctl(file *filep, int cmd, unsigned long arg)
 			break;
 		}
 
-	case PWM_SERVO_GET_MIN_PWM:
+	case PWM_SERVO_GET_MIN_PWM: {
+			struct pwm_output_values *pwm = (struct pwm_output_values *)arg;
+			pwm->channel_count = _max_actuators;
 
-		ret = io_reg_get(PX4IO_PAGE_CONTROL_MIN_PWM, 0, (uint16_t *)arg, _max_actuators);
+			ret = io_reg_get(PX4IO_PAGE_CONTROL_MIN_PWM, 0, pwm->values, _max_actuators);
 
-		if (ret != OK) {
-			ret = -EIO;
+			if (ret != OK) {
+				ret = -EIO;
+			}
+
+			break;
 		}
 
-		break;
-
 	case PWM_SERVO_SET_MAX_PWM: {
 			struct pwm_output_values *pwm = (struct pwm_output_values *)arg;
 
@@ -2543,12 +2552,15 @@ PX4IO::ioctl(file *filep, int cmd, unsigned long arg)
 			break;
 		}
 
-	case PWM_SERVO_GET_MAX_PWM:
+	case PWM_SERVO_GET_MAX_PWM: {
+			struct pwm_output_values *pwm = (struct pwm_output_values *)arg;
+			pwm->channel_count = _max_actuators;
 
-		ret = io_reg_get(PX4IO_PAGE_CONTROL_MAX_PWM, 0, (uint16_t *)arg, _max_actuators);
+			ret = io_reg_get(PX4IO_PAGE_CONTROL_MAX_PWM, 0, pwm->values, _max_actuators);
 
-		if (ret != OK) {
-			ret = -EIO;
+			if (ret != OK) {
+				ret = -EIO;
+			}
 		}
 
 		break;

From 237bdfdb61ea335df7c2baca0588fd7fa3e26c6d Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Sat, 14 May 2016 18:22:34 +0200
Subject: [PATCH 0207/1230] EKF: Be less verbose, avoid floating ng point
 printing stack smashing

---
 .../ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp       | 3 ---
 1 file changed, 3 deletions(-)

diff --git a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp
index e96ca53966..9432c5d2de 100644
--- a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp
+++ b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp
@@ -699,8 +699,6 @@ void AttitudePositionEstimatorEKF::task_main()
 
 					_filter_ref_offset = -_baro.altitude;
 
-					PX4_INFO("filter ref off: baro_alt: %8.4f", (double)_filter_ref_offset);
-
 				} else {
 
 					if (!_gps_initialized && _gpsIsGood) {
@@ -785,7 +783,6 @@ void AttitudePositionEstimatorEKF::initReferencePosition(hrt_abstime timestamp,
 		_local_pos.ref_timestamp = timestamp;
 
 		map_projection_init(&_pos_ref, lat, lon);
-		mavlink_and_console_log_info(&_mavlink_log_pub, "[ekf] ref: LA %.4f,LO %.4f,ALT %.2f", lat, lon, (double)gps_alt);
 	}
 }
 

From 69f702fb6ae819f800470e036861577a2e55e41e Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lorenz@px4.io>
Date: Sun, 15 May 2016 14:27:59 +0200
Subject: [PATCH 0208/1230] MAVLink: Remove unused function definitions

---
 src/modules/mavlink/mavlink_parameters.h | 27 ------------------------
 1 file changed, 27 deletions(-)

diff --git a/src/modules/mavlink/mavlink_parameters.h b/src/modules/mavlink/mavlink_parameters.h
index d258f3b240..1dcd073203 100644
--- a/src/modules/mavlink/mavlink_parameters.h
+++ b/src/modules/mavlink/mavlink_parameters.h
@@ -75,33 +75,6 @@ public:
 
 	void handle_message(const mavlink_message_t *msg);
 
-	/**
-	 * Send one parameter identified by index.
-	 *
-	 * @param index		The index of the parameter to send.
-	 * @return		zero on success, nonzero else.
-	 */
-	void		start_send_one(int index);
-
-
-	/**
-	 * Send one parameter identified by name.
-	 *
-	 * @param name		The index of the parameter to send.
-	 * @return		zero on success, nonzero else.
-	 */
-	int			start_send_for_name(const char *name);
-
-	/**
-	 * Start sending the parameter queue.
-	 *
-	 * This function will not directly send parameters, but instead
-	 * activate the sending of one parameter on each call of
-	 * mavlink_pm_queued_send().
-	 * @see 		mavlink_pm_queued_send()
-	 */
-	void		start_send_all();
-
 private:
 	int		_send_all_index;
 

From 65e079f8cd8ab32efc3fa3f239672a6b1a78f01c Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lorenz@px4.io>
Date: Sun, 15 May 2016 14:28:19 +0200
Subject: [PATCH 0209/1230] Startup: Boot system with sdlog starting sooner

---
 ROMFS/px4fmu_common/init.d/rcS | 72 +++++++++++++++++-----------------
 1 file changed, 36 insertions(+), 36 deletions(-)

diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS
index 98f9e688a3..8e64b9f4c0 100644
--- a/ROMFS/px4fmu_common/init.d/rcS
+++ b/ROMFS/px4fmu_common/init.d/rcS
@@ -617,6 +617,42 @@ then
 		mavlink start -r 800000 -d /dev/ttyACM0 -m config -x
 	fi
 
+	#
+	# Logging
+	#
+	if ver hwcmp PX4FMU_V1
+	then
+		if sdlog2 start -r 30 -a -b 2 -t
+		then
+		fi
+	else
+		# check if we should increase logging rate for ekf2 replay message logging
+		if param greater EKF2_REC_RPL 0
+		then
+			if param compare SYS_LOGGER 0
+			then
+				if sdlog2 start -r 500 -e -b 20 -t
+				then
+				fi
+			else
+				if logger start -r 500
+				then
+				fi
+			fi
+		else
+			if param compare SYS_LOGGER 0
+			then
+				if sdlog2 start -r 100 -a -b 12 -t
+				then
+				fi
+			else
+				if logger start -b 24
+				then
+				fi
+			fi
+		fi
+	fi
+
 	#
 	# Start up ARDrone Motor interface
 	#
@@ -837,42 +873,6 @@ then
 		echo "[i] No autostart ID found"
 	fi
 
-	#
-	# Logging
-	#
-	if ver hwcmp PX4FMU_V1
-	then
-		if sdlog2 start -r 30 -a -b 2 -t
-		then
-		fi
-	else
-		# check if we should increase logging rate for ekf2 replay message logging
-		if param greater EKF2_REC_RPL 0
-		then
-			if param compare SYS_LOGGER 0
-			then
-				if sdlog2 start -r 500 -e -b 20 -t
-				then
-				fi
-			else
-				if logger start -r 500
-				then
-				fi
-			fi
-		else
-			if param compare SYS_LOGGER 0
-			then
-				if sdlog2 start -r 100 -a -b 12 -t
-				then
-				fi
-			else
-				if logger start -b 24
-				then
-				fi
-			fi
-		fi
-	fi
-
 	# Start any custom addons
 	set FEXTRAS /fs/microsd/etc/extras.txt
 	if [ -f $FEXTRAS ]

From 41b127d40526b2c507dd19ec5fcbdf34d61f3fc9 Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Tue, 10 May 2016 18:59:23 +0200
Subject: [PATCH 0210/1230] Make IO RSSI handling as robust and informative as
 on FMU

---
 src/drivers/px4io/px4io.cpp | 33 ++++++++++++++++++++++++++++++++-
 1 file changed, 32 insertions(+), 1 deletion(-)

diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp
index 01e8d46ff1..61335a3eeb 100644
--- a/src/drivers/px4io/px4io.cpp
+++ b/src/drivers/px4io/px4io.cpp
@@ -311,6 +311,8 @@ private:
 	int32_t			_rssi_pwm_chan; ///< RSSI PWM input channel
 	int32_t			_rssi_pwm_max; ///< max RSSI input on PWM channel
 	int32_t			_rssi_pwm_min; ///< min RSSI input on PWM channel
+	bool			_analog_rc_rssi_stable; ///< true when analog RSSI input is stable
+	float			_analog_rc_rssi_volt; ///< analog RSSI voltage
 
 	float			_last_throttle; ///< last throttle value for battery calculation
 
@@ -545,6 +547,8 @@ PX4IO::PX4IO(device::Device *interface) :
 	_rssi_pwm_chan(0),
 	_rssi_pwm_max(0),
 	_rssi_pwm_min(0),
+	_analog_rc_rssi_stable(false),
+	_analog_rc_rssi_volt(-1.0f),
 	_last_throttle(0.0f)
 #ifdef CONFIG_ARCH_BOARD_PX4FMU_V1
 	, _dsm_vcc_ctl(false)
@@ -1706,6 +1710,16 @@ PX4IO::io_handle_vservo(uint16_t vservo, uint16_t vrssi)
 	_servorail_status.voltage_v = vservo * 0.001f;
 	_servorail_status.rssi_v    = vrssi * 0.001f;
 
+	if (_analog_rc_rssi_volt < 0.0f) {
+		_analog_rc_rssi_volt = _servorail_status.rssi_v;
+	}
+
+	_analog_rc_rssi_volt = _analog_rc_rssi_volt * 0.99f + _servorail_status.rssi_v * 0.01f;
+
+	if (_analog_rc_rssi_volt > 2.5f) {
+		_analog_rc_rssi_stable = true;
+	}
+
 	/* lazily publish the servorail voltages */
 	if (_to_servorail != nullptr) {
 		orb_publish(ORB_ID(servorail_status), _to_servorail, &_servorail_status);
@@ -1784,7 +1798,24 @@ PX4IO::io_get_raw_rc_input(rc_input_values &input_rc)
 	input_rc.timestamp_publication = hrt_absolute_time();
 
 	input_rc.rc_ppm_frame_length = regs[PX4IO_P_RAW_RC_DATA];
-	input_rc.rssi = regs[PX4IO_P_RAW_RC_NRSSI];
+
+	if (!_analog_rc_rssi_stable) {
+		input_rc.rssi = 255;// we do not actually get digital RSSI regs[PX4IO_P_RAW_RC_NRSSI];
+
+	} else {
+		float rssi_analog = ((_analog_rc_rssi_volt - 0.2f) / 3.0f) * 100.0f;
+
+		if (rssi_analog > 100.0f) {
+			rssi_analog = 100.0f;
+		}
+
+		if (rssi_analog < 0.0f) {
+			rssi_analog = 0.0f;
+		}
+
+		input_rc.rssi = rssi_analog;
+	}
+
 	input_rc.rc_failsafe = (regs[PX4IO_P_RAW_RC_FLAGS] & PX4IO_P_RAW_RC_FLAGS_FAILSAFE);
 	input_rc.rc_lost = !(regs[PX4IO_P_RAW_RC_FLAGS] & PX4IO_P_RAW_RC_FLAGS_RC_OK);
 	input_rc.rc_lost_frame_count = regs[PX4IO_P_RAW_LOST_FRAME_COUNT];

From 7633797190f0b26769af452d9ca92769c0066803 Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lorenz@px4.io>
Date: Sun, 15 May 2016 15:37:46 +0200
Subject: [PATCH 0211/1230] Battery status: Add valid flag

---
 msg/battery_status.msg | 1 +
 1 file changed, 1 insertion(+)

diff --git a/msg/battery_status.msg b/msg/battery_status.msg
index 7dbdc7b1ce..351b31d035 100644
--- a/msg/battery_status.msg
+++ b/msg/battery_status.msg
@@ -5,6 +5,7 @@ float32 current_filtered_a	# Battery current in amperes, filtered, 0 if unknown
 float32 discharged_mah		# Discharged amount in mAh, -1 if unknown
 float32 remaining		# From 1 to 0, -1 if unknown
 int32 cell_count		# Number of cells
+bool connected			# Wether or not a battery is connected
 #bool is_powering_off		# Power off event imminent indication, false if unknown
 
 

From 55c879a0abc2ec03d9641d72a61b7a2e6939da91 Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lorenz@px4.io>
Date: Sun, 15 May 2016 15:38:01 +0200
Subject: [PATCH 0212/1230] MAVLink: Use valid flag to initialize fields

---
 src/modules/mavlink/mavlink_messages.cpp      | 23 +++++++++----------
 .../mavlink/mavlink_orb_subscription.cpp      |  2 +-
 2 files changed, 12 insertions(+), 13 deletions(-)

diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp
index b0e7a4ecca..e88188a8f3 100644
--- a/src/modules/mavlink/mavlink_messages.cpp
+++ b/src/modules/mavlink/mavlink_messages.cpp
@@ -592,9 +592,9 @@ protected:
 
 	void send(const hrt_abstime t)
 	{
-		struct vehicle_status_s status;
-		struct cpuload_s cpuload;
-		struct battery_status_s battery_status;
+		struct vehicle_status_s status = {};
+		struct cpuload_s cpuload = {};
+		struct battery_status_s battery_status = {};
 
 		const bool updated_status = _status_sub->update(&status);
 		const bool updated_cpuload = _cpuload_sub->update(&cpuload);
@@ -611,14 +611,13 @@ protected:
 		if (updated_status || updated_battery || updated_cpuload) {
 			mavlink_sys_status_t msg;
 
-
 			msg.onboard_control_sensors_present = status.onboard_control_sensors_present;
 			msg.onboard_control_sensors_enabled = status.onboard_control_sensors_enabled;
 			msg.onboard_control_sensors_health = status.onboard_control_sensors_health;
 			msg.load = cpuload.load * 1000.0f;
-			msg.voltage_battery = battery_status.voltage_filtered_v * 1000.0f;
-			msg.current_battery = battery_status.current_filtered_a * 100.0f;
-			msg.battery_remaining = battery_status.remaining >= 0 ? battery_status.remaining * 100.0f : -1;
+			msg.voltage_battery = (battery_status.connected) ? battery_status.voltage_filtered_v * 1000.0f : UINT16_MAX;
+			msg.current_battery = (battery_status.connected) ? battery_status.current_filtered_a * 100.0f : -1;
+			msg.battery_remaining = (battery_status.connected) ? battery_status.remaining * 100.0f : -1;
 			// TODO: fill in something useful in the fields below
 			msg.drop_rate_comm = 0;
 			msg.errors_comm = 0;
@@ -634,13 +633,13 @@ protected:
 			bat_msg.id = 0;
 			bat_msg.battery_function = MAV_BATTERY_FUNCTION_ALL;
 			bat_msg.type = MAV_BATTERY_TYPE_LIPO;
-			bat_msg.current_consumed = battery_status.discharged_mah;
+			bat_msg.current_consumed = (battery_status.connected) ? battery_status.discharged_mah : -1;
 			bat_msg.energy_consumed = -1;
-			bat_msg.current_battery = battery_status.current_a * 100;
-			bat_msg.battery_remaining = battery_status.remaining >= 0 ? battery_status.remaining * 100.0f : -1;
+			bat_msg.current_battery = (battery_status.connected) ? battery_status.current_filtered_a * 100 : -1;
+			bat_msg.battery_remaining = (battery_status.connected) ? battery_status.remaining * 100.0f : -1;
 			bat_msg.temperature = INT16_MAX;
 			for (unsigned int i = 0; i < (sizeof(bat_msg.voltages) / sizeof(bat_msg.voltages[0])); i++) {
-				if ((int)i < battery_status.cell_count) {
+				if ((int)i < battery_status.cell_count && battery_status.connected) {
 					bat_msg.voltages[i] = (battery_status.voltage_v / battery_status.cell_count) * 1000.0f;
 				} else {
 					bat_msg.voltages[i] = UINT16_MAX;
@@ -2686,7 +2685,7 @@ protected:
 
 	void send(const hrt_abstime t)
 	{
-		struct vehicle_status_s status;
+		struct vehicle_status_s status = {};
 		(void)_status_sub->update(&status);
 
 		mavlink_command_long_t msg;
diff --git a/src/modules/mavlink/mavlink_orb_subscription.cpp b/src/modules/mavlink/mavlink_orb_subscription.cpp
index 4567f4f410..631461a454 100644
--- a/src/modules/mavlink/mavlink_orb_subscription.cpp
+++ b/src/modules/mavlink/mavlink_orb_subscription.cpp
@@ -86,7 +86,7 @@ MavlinkOrbSubscription::update(uint64_t *time, void* data)
 	}
 
 	if (orb_copy(_topic, _fd, data)) {
-		if (data) {
+		if (data != nullptr) {
 			/* error copying topic data */
 			memset(data, 0, _topic->o_size);
 		}

From f3ee22b22c75593620d13af8d97fbbe1a35d3cff Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lorenz@px4.io>
Date: Sun, 15 May 2016 15:38:20 +0200
Subject: [PATCH 0213/1230] Battery lib: Set valid flag

---
 src/modules/systemlib/battery.cpp | 3 ++-
 1 file changed, 2 insertions(+), 1 deletion(-)

diff --git a/src/modules/systemlib/battery.cpp b/src/modules/systemlib/battery.cpp
index b971b10d87..0b7d17ad18 100644
--- a/src/modules/systemlib/battery.cpp
+++ b/src/modules/systemlib/battery.cpp
@@ -75,6 +75,7 @@ Battery::reset(battery_status_s *battery_status)
 	battery_status->cell_count = _param_n_cells.get();
 	// TODO: check if it is sane to reset warning to NONE
 	battery_status->warning = battery_status_s::BATTERY_WARNING_NONE;
+	battery_status->connected = false;
 }
 
 void
@@ -97,7 +98,7 @@ Battery::updateBatteryStatus(hrt_abstime timestamp, float voltage_v, float curre
 		battery_status->discharged_mah = _discharged_mah;
 		battery_status->warning = _warning;
 		battery_status->remaining = _remaining;
-
+		battery_status->connected = true;
 	}
 }
 

From 94f3c50f830a69dc5a97c6877bdfffd0ab596d06 Mon Sep 17 00:00:00 2001
From: Jimmy Johnson <catch22@fastmail.net>
Date: Mon, 11 Apr 2016 21:30:05 -0700
Subject: [PATCH 0214/1230] follow target safety updates

---
 src/modules/navigator/follow_target.cpp | 140 ++++++++++++++++--------
 src/modules/navigator/follow_target.h   |  37 ++++++-
 src/modules/navigator/navigation.h      |   3 +-
 3 files changed, 130 insertions(+), 50 deletions(-)

diff --git a/src/modules/navigator/follow_target.cpp b/src/modules/navigator/follow_target.cpp
index ff23802efd..0e259a683c 100644
--- a/src/modules/navigator/follow_target.cpp
+++ b/src/modules/navigator/follow_target.cpp
@@ -45,7 +45,9 @@
 #include <stdbool.h>
 #include <math.h>
 #include <fcntl.h>
+#include <float.h>
 
+#include <mavlink/mavlink_log.h>
 #include <systemlib/err.h>
 
 #include <uORB/uORB.h>
@@ -58,20 +60,24 @@
 FollowTarget::FollowTarget(Navigator *navigator, const char *name) :
 	MissionBlock(navigator, name),
 	_navigator(navigator),
-	_param_min_alt(this, "MIS_TAKEOFF_ALT", false),
+	_param_min_alt(this, "NAV_MIN_FT_HT", false),
 	_follow_target_state(WAIT_FOR_TARGET_POSITION),
+	_follow_target_position(FOLLOW_FROM_BEHIND),
 	_follow_target_sub(-1),
 	_step_time_in_ms(0.0f),
 	_target_updates(0),
 	_last_update_time(0),
 	_current_target_motion( {}),
-			_previous_target_motion({})
+	_previous_target_motion({})
 {
 	updateParams();
 	_current_vel.zero();
 	_step_vel.zero();
 	_target_vel.zero();
 	_target_distance.zero();
+	_target_position_offset.zero();
+
+	_rot_matrix = (_follow_position_matricies[_follow_target_position]);
 }
 
 FollowTarget::~FollowTarget()
@@ -94,11 +100,13 @@ void FollowTarget::on_active()
 {
 	struct map_projection_reference_s target_ref;
 	math::Vector<3> target_position(0, 0, 0);
+	follow_target_s target_motion = {};
 	uint64_t current_time = hrt_absolute_time();
 	bool _radius_entered = false;
 	bool _radius_exited = false;
 	bool updated = false;
 	float dt_ms = 0;
+	float yaw_angle = NAN;
 
 	orb_check(_follow_target_sub, &updated);
 
@@ -112,43 +120,11 @@ void FollowTarget::on_active()
 
 		orb_copy(ORB_ID(follow_target), _follow_target_sub, &_current_target_motion);
 
-	} else if (((current_time - _previous_target_motion.timestamp) / 1000 / 1000) > TARGET_TIMEOUT_S
+	} else if (((_current_target_motion.timestamp - _previous_target_motion.timestamp) / 1000 ) > TARGET_TIMEOUT_MS
 		   && target_velocity_valid()) {
 		reset_target_validity();
 	}
 
-	// update target velocity
-
-	if (target_velocity_valid() && updated) {
-
-		dt_ms = ((_current_target_motion.timestamp - _previous_target_motion.timestamp) / 1000);
-
-		// get last gps known reference for target
-
-		map_projection_init(&target_ref, _previous_target_motion.lat, _previous_target_motion.lon);
-
-		// calculate distance the target has moved
-
-		map_projection_project(&target_ref, _current_target_motion.lat, _current_target_motion.lon, &(target_position(0)),
-				       &(target_position(1)));
-
-		// update the average velocity of the target based on the position
-
-		_target_vel = target_position / (dt_ms / 1000.0f);
-
-		// to keep the velocity increase/decrease smooth
-		// calculate how many velocity increments/decrements
-		// it will take to reach the targets velocity
-		// with the given amount of steps also add a feed forward input that adjusts the
-		// velocity as the position gap increases since
-		// just traveling at the exact velocity of the target will not
-		// get any closer to the target
-
-		_step_vel = (_target_vel - _current_vel) + _target_distance * FF_K;
-		_step_vel /= (dt_ms / 1000.0f * (float) INTERPOLATION_PNTS);
-		_step_time_in_ms = dt_ms / (float) INTERPOLATION_PNTS;
-	}
-
 	// update distance to target
 
 	if (target_position_valid()) {
@@ -159,12 +135,84 @@ void FollowTarget::on_active()
 		map_projection_project(&target_ref, _current_target_motion.lat, _current_target_motion.lon, &_target_distance(0),
 				       &_target_distance(1));
 
+		target_motion = _current_target_motion;
+
+		// use target offset
+
+		map_projection_init(&target_ref, _current_target_motion.lat, _current_target_motion.lon);
+		map_projection_reproject(&target_ref, _target_position_offset(0), _target_position_offset(1), &target_motion.lat, &target_motion.lon);
+
 		// are we within the target acceptance radius?
 		// give a buffer to exit/enter the radius to give the velocity controller
 		// a chance to catch up
 
-		_radius_exited = (_target_distance.length() > (float) TARGET_ACCEPTANCE_RADIUS_M * 1.5f);
-		_radius_entered = (_target_distance.length() < (float) TARGET_ACCEPTANCE_RADIUS_M);
+		_radius_exited =  ((_target_position_offset + _target_distance).length() > (float) TARGET_ACCEPTANCE_RADIUS_M * 1.5f);
+		_radius_entered = ((_target_position_offset + _target_distance).length() < (float) TARGET_ACCEPTANCE_RADIUS_M);
+
+	}
+
+	// update target velocity
+
+	if (target_velocity_valid() && updated) {
+
+		dt_ms = ((_current_target_motion.timestamp - _previous_target_motion.timestamp) / 1000);
+
+		// protect from to small of a dt
+
+		if (dt_ms > 10) {
+
+			// get last gps known reference for target
+
+			map_projection_init(&target_ref, _previous_target_motion.lat, _previous_target_motion.lon);
+
+			// calculate distance the target has moved
+
+			map_projection_project(&target_ref, _current_target_motion.lat, _current_target_motion.lon, &(target_position(0)),  &(target_position(1)));
+
+			// only calculate new velocity if target has moved
+
+			if(target_position.length() > 0.0F) {
+
+				// track to the left, right, behind, or front
+
+				_target_position_offset = _rot_matrix * (target_position.normalized() * OFFSET_M);
+
+				// update the average velocity of the target based on the position
+
+				_target_vel = target_position / (dt_ms / 1000.0f);
+
+				// to keep the velocity increase/decrease smooth
+				// calculate how many velocity increments/decrements
+				// it will take to reach the targets velocity
+				// with the given amount of steps also add a feed forward input that adjusts the
+				// velocity as the position gap increases since
+				// just traveling at the exact velocity of the target will not
+				// get any closer to the target
+
+				_step_vel = (_target_vel - _current_vel) + (_target_position_offset + _target_distance) * FF_K;
+				_step_vel /= (dt_ms / 1000.0F * (float) INTERPOLATION_PNTS);
+				_step_time_in_ms = dt_ms / (float) INTERPOLATION_PNTS;
+			}
+		}
+	}
+
+	// always point towards target
+
+	if (target_position_valid() && updated) {
+
+		yaw_angle = get_bearing_to_next_waypoint(_navigator->get_global_position()->lat, _navigator->get_global_position()->lon,
+				_current_target_motion.lat, _current_target_motion.lon);
+
+//			warnx(" lat %f (%f) lon %f (%f), dist x %f y %f (%f) yaw = %f mode = %d",
+//					target_motion.lat,
+//					(double )_navigator->get_global_position()->lat,
+//					target_motion.lon,
+//					(double ) _navigator->get_global_position()->lon,
+//					(double ) _target_distance(0),
+//					(double ) _target_distance(1),
+//					(double ) _target_distance.length(),
+//					(double) yaw_angle,
+//					_follow_target_state);
 	}
 
 	// update state machine
@@ -177,10 +225,12 @@ void FollowTarget::on_active()
 				_follow_target_state = TRACK_VELOCITY;
 
 			} else if (target_velocity_valid()) {
-				set_follow_target_item(&_mission_item, _param_min_alt.get(), _current_target_motion, NAN);
+				set_follow_target_item(&_mission_item, _param_min_alt.get(), target_motion, yaw_angle);
 				// keep the current velocity updated with the target velocity for when it's needed
 				_current_vel = _target_vel;
 				update_position_sp(true, true);
+			} else {
+				_follow_target_state = WAIT_FOR_TARGET_POSITION;
 			}
 
 			break;
@@ -192,7 +242,7 @@ void FollowTarget::on_active()
 				_follow_target_state = TRACK_POSITION;
 
 			} else if (target_velocity_valid()) {
-				set_follow_target_item(&_mission_item, _param_min_alt.get(), _current_target_motion, NAN);
+				set_follow_target_item(&_mission_item, _param_min_alt.get(), target_motion, yaw_angle);
 
 				if ((current_time - _last_update_time) / 1000 >= _step_time_in_ms) {
 					_current_vel += _step_vel;
@@ -200,6 +250,8 @@ void FollowTarget::on_active()
 				}
 
 				update_position_sp(true, false);
+			} else {
+				_follow_target_state = WAIT_FOR_TARGET_POSITION;
 			}
 
 			break;
@@ -248,7 +300,6 @@ void FollowTarget::update_position_sp(bool use_velocity, bool use_position)
 	pos_sp_triplet->current.vx = _current_vel(0);
 	pos_sp_triplet->current.vy = _current_vel(1);
 	pos_sp_triplet->next.valid = false;
-
 	_navigator->set_position_setpoint_triplet_updated();
 }
 
@@ -261,18 +312,19 @@ void FollowTarget::reset_target_validity()
 	_step_vel.zero();
 	_target_vel.zero();
 	_target_distance.zero();
+	_target_position_offset.zero();
 	reset_mission_item_reached();
 	_follow_target_state = WAIT_FOR_TARGET_POSITION;
 }
 
 bool FollowTarget::target_velocity_valid()
 {
-	// need at least 2 data points for velocity estimate
-	return (_target_updates >= 2);
+	// need at least 5 continuous data points for velocity estimate
+	return (_target_updates >= 5);
 }
 
 bool FollowTarget::target_position_valid()
 {
-	// need at least 1 data point for position estimate
-	return (_target_updates >= 1);
+	// need at least 2 continuous data points for position estimate
+	return (_target_updates >= 2);
 }
diff --git a/src/modules/navigator/follow_target.h b/src/modules/navigator/follow_target.h
index b018bc58ee..019c93c293 100644
--- a/src/modules/navigator/follow_target.h
+++ b/src/modules/navigator/follow_target.h
@@ -43,7 +43,7 @@
 #include <controllib/blocks.hpp>
 #include <controllib/block/BlockParam.hpp>
 #include <lib/mathlib/math/Vector.hpp>
-
+#include <lib/mathlib/math/Matrix.hpp>
 #include "navigator_mode.h"
 #include "mission_block.h"
 
@@ -60,10 +60,11 @@ public:
 
 private:
 
-	static constexpr int TARGET_TIMEOUT_S = 5;
+	static constexpr int TARGET_TIMEOUT_MS = 1500;
 	static constexpr int TARGET_ACCEPTANCE_RADIUS_M = 5;
 	static constexpr int INTERPOLATION_PNTS = 20;
-	static constexpr float FF_K = .15f;
+	static constexpr float FF_K = .40F;
+	static constexpr float OFFSET_M = 8;
 
 	enum FollowTargetState {
 		TRACK_POSITION,
@@ -71,9 +72,36 @@ private:
 		WAIT_FOR_TARGET_POSITION
 	};
 
+	enum FollowTargetPosition {
+		FOLLOW_FROM_RIGHT,
+		FOLLOW_FROM_BEHIND,
+		FOLLOW_FROM_FRONT,
+		FOLLOW_FROM_LEFT
+	};
+
+	float _follow_position_matricies[4][9] = {
+				 	 	 	 	 	 {1.0F,  -1.0F, 0.0F,
+						 	 	 	  1.0F,   1.0F, 0.0F,
+									  0.0F,   0.0F, 1.0F}, // follow right
+
+									 {-1.0F,  0.0F, 0.0F,
+						 	 	 	   0.0F, -1.0F, 0.0F,
+									   0.0F,  0.0F, 1.0F}, // follow behind
+
+									 {1.0F,   0.0F, 0.0F,
+							 	 	  0.0F,   1.0F, 0.0F,
+									  0.0F,   0.0F, 1.0F}, // follow front
+
+									 {1.0F,   1.0F, 0.0F,
+							 	 	 -1.0F,   1.0F, 0.0F,
+									  0.0F,   0.0F, 1.0F}}; // follow left side
+
+
 	Navigator *_navigator;
 	control::BlockParamFloat _param_min_alt;
 	FollowTargetState _follow_target_state;
+	FollowTargetPosition _follow_target_position;
+
 	int _follow_target_sub;
 	float _step_time_in_ms;
 
@@ -85,10 +113,11 @@ private:
 	math::Vector<3> _step_vel;
 	math::Vector<3> _target_vel;
 	math::Vector<3> _target_distance;
+	math::Vector<3> _target_position_offset;
 
 	follow_target_s _current_target_motion;
 	follow_target_s _previous_target_motion;
-
+	math::Matrix<3,3> _rot_matrix;
 	void track_target_position();
 	void track_target_velocity();
 	bool target_velocity_valid();
diff --git a/src/modules/navigator/navigation.h b/src/modules/navigator/navigation.h
index a7da600330..7fadabbd2c 100644
--- a/src/modules/navigator/navigation.h
+++ b/src/modules/navigator/navigation.h
@@ -61,14 +61,13 @@ enum NAV_CMD {
 	NAV_CMD_TAKEOFF = 22,
 	NAV_CMD_ROI = 80,
 	NAV_CMD_PATHPLANNING = 81,
-	NAV_CMD_FOLLOW_TARGET = 194, // temporary placeholder
-	NAV_CMD_GOTO_TAREGT = 195,
 	NAV_CMD_VTOL_TAKEOFF = 84,
 	NAV_CMD_VTOL_LAND = 85,
 	NAV_CMD_DO_JUMP = 177,
 	NAV_CMD_DO_CHANGE_SPEED = 178,
 	NAV_CMD_DO_SET_SERVO=183,
 	NAV_CMD_DO_REPEAT_SERVO=184,
+	NAV_CMD_FOLLOW_TARGET = 194, // temporary placeholder
 	NAV_CMD_DO_DIGICAM_CONTROL=203,
 	NAV_CMD_DO_SET_CAM_TRIGG_DIST=206,
 	NAV_CMD_DO_VTOL_TRANSITION=3000,

From f5c90a2d640e945aa37c26a7b813d476eadeb653 Mon Sep 17 00:00:00 2001
From: Jimmy Johnson <catch22@fastmail.net>
Date: Mon, 11 Apr 2016 21:33:26 -0700
Subject: [PATCH 0215/1230] adding new follow target parameter

---
 src/modules/navigator/follow_target_params.c | 55 ++++++++++++++++++++
 1 file changed, 55 insertions(+)
 create mode 100644 src/modules/navigator/follow_target_params.c

diff --git a/src/modules/navigator/follow_target_params.c b/src/modules/navigator/follow_target_params.c
new file mode 100644
index 0000000000..3796f638e4
--- /dev/null
+++ b/src/modules/navigator/follow_target_params.c
@@ -0,0 +1,55 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2016 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * AS IS AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file follow_target_params.c
+ *
+ * Parameters for follow target mode
+ *
+ * @author Jimmy Johnson <catch22@fastmail.net>
+ */
+
+/*
+ * Follow target parameters
+ */
+
+/**
+ * Minimum follow target altitude
+ *
+ * The minimum height in meters relative to home for following a target
+ *
+ * @unit meters
+ * @min 8.0
+ * @group Follow target
+ */
+PARAM_DEFINE_FLOAT(NAV_MIN_FT_HT, 8.0f);

From 55f023b77111d440c61c096026077ff3d1685ca8 Mon Sep 17 00:00:00 2001
From: Jimmy Johnson <catch22@fastmail.net>
Date: Mon, 11 Apr 2016 23:52:22 -0700
Subject: [PATCH 0216/1230] adding params for offset, and side to track, fixing
 velocity tracking bug

---
 src/modules/navigator/follow_target.cpp      | 39 +++++++++++++-------
 src/modules/navigator/follow_target.h        |  4 ++
 src/modules/navigator/follow_target_params.c | 24 ++++++++++++
 3 files changed, 54 insertions(+), 13 deletions(-)

diff --git a/src/modules/navigator/follow_target.cpp b/src/modules/navigator/follow_target.cpp
index 0e259a683c..e3e7a4fed8 100644
--- a/src/modules/navigator/follow_target.cpp
+++ b/src/modules/navigator/follow_target.cpp
@@ -47,7 +47,6 @@
 #include <fcntl.h>
 #include <float.h>
 
-#include <mavlink/mavlink_log.h>
 #include <systemlib/err.h>
 
 #include <uORB/uORB.h>
@@ -61,13 +60,16 @@ FollowTarget::FollowTarget(Navigator *navigator, const char *name) :
 	MissionBlock(navigator, name),
 	_navigator(navigator),
 	_param_min_alt(this, "NAV_MIN_FT_HT", false),
+	_param_tracking_dist(this,"NAV_FT_DST", false),
+	_param_tracking_side(this,"NAV_FT_FS", false),
 	_follow_target_state(WAIT_FOR_TARGET_POSITION),
 	_follow_target_position(FOLLOW_FROM_BEHIND),
 	_follow_target_sub(-1),
 	_step_time_in_ms(0.0f),
+	_follow_offset(OFFSET_M),
 	_target_updates(0),
 	_last_update_time(0),
-	_current_target_motion( {}),
+	_current_target_motion({}),
 	_previous_target_motion({})
 {
 	updateParams();
@@ -91,6 +93,18 @@ void FollowTarget::on_inactive()
 
 void FollowTarget::on_activation()
 {
+	updateParams();
+
+	_follow_offset = _param_tracking_dist.get() < 1.0F ? 1.0F : _param_tracking_dist.get();
+
+	_follow_target_position = (FollowTargetPosition) _param_tracking_side.get();
+
+	if((_follow_target_position > 3) || (_follow_target_position < 0)) {
+		_follow_target_position = FOLLOW_FROM_BEHIND;
+	}
+
+	_rot_matrix = (_follow_position_matricies[_follow_target_position]);
+
 	if (_follow_target_sub < 0) {
 		_follow_target_sub = orb_subscribe(ORB_ID(follow_target));
 	}
@@ -120,8 +134,7 @@ void FollowTarget::on_active()
 
 		orb_copy(ORB_ID(follow_target), _follow_target_sub, &_current_target_motion);
 
-	} else if (((_current_target_motion.timestamp - _previous_target_motion.timestamp) / 1000 ) > TARGET_TIMEOUT_MS
-		   && target_velocity_valid()) {
+	} else if (((current_time - _current_target_motion.timestamp) / 1000 ) > TARGET_TIMEOUT_MS && target_velocity_valid()) {
 		reset_target_validity();
 	}
 
@@ -157,9 +170,9 @@ void FollowTarget::on_active()
 
 		dt_ms = ((_current_target_motion.timestamp - _previous_target_motion.timestamp) / 1000);
 
-		// protect from to small of a dt
+		// ignore a small dt
 
-		if (dt_ms > 10) {
+		if (dt_ms > 10.0F) {
 
 			// get last gps known reference for target
 
@@ -169,14 +182,14 @@ void FollowTarget::on_active()
 
 			map_projection_project(&target_ref, _current_target_motion.lat, _current_target_motion.lon, &(target_position(0)),  &(target_position(1)));
 
-			// only calculate new velocity if target has moved
+			// only calculate offset rotation if target has moved
 
 			if(target_position.length() > 0.0F) {
 
 				// track to the left, right, behind, or front
 
-				_target_position_offset = _rot_matrix * (target_position.normalized() * OFFSET_M);
-
+				_target_position_offset = _rot_matrix * (target_position.normalized() * _follow_offset);
+			}
 				// update the average velocity of the target based on the position
 
 				_target_vel = target_position / (dt_ms / 1000.0f);
@@ -192,7 +205,6 @@ void FollowTarget::on_active()
 				_step_vel = (_target_vel - _current_vel) + (_target_position_offset + _target_distance) * FF_K;
 				_step_vel /= (dt_ms / 1000.0F * (float) INTERPOLATION_PNTS);
 				_step_time_in_ms = dt_ms / (float) INTERPOLATION_PNTS;
-			}
 		}
 	}
 
@@ -200,8 +212,10 @@ void FollowTarget::on_active()
 
 	if (target_position_valid() && updated) {
 
-		yaw_angle = get_bearing_to_next_waypoint(_navigator->get_global_position()->lat, _navigator->get_global_position()->lon,
-				_current_target_motion.lat, _current_target_motion.lon);
+		yaw_angle = get_bearing_to_next_waypoint(_navigator->get_global_position()->lat,
+				_navigator->get_global_position()->lon,
+				_current_target_motion.lat,
+				_current_target_motion.lon);
 
 //			warnx(" lat %f (%f) lon %f (%f), dist x %f y %f (%f) yaw = %f mode = %d",
 //					target_motion.lat,
@@ -258,7 +272,6 @@ void FollowTarget::on_active()
 		}
 
 	case WAIT_FOR_TARGET_POSITION: {
-
 			// Climb to the minimum altitude
 			// and wait until a position is received
 
diff --git a/src/modules/navigator/follow_target.h b/src/modules/navigator/follow_target.h
index 019c93c293..b00136b5ce 100644
--- a/src/modules/navigator/follow_target.h
+++ b/src/modules/navigator/follow_target.h
@@ -99,11 +99,15 @@ private:
 
 	Navigator *_navigator;
 	control::BlockParamFloat _param_min_alt;
+	control::BlockParamFloat _param_tracking_dist;
+	control::BlockParamInt _param_tracking_side;
+
 	FollowTargetState _follow_target_state;
 	FollowTargetPosition _follow_target_position;
 
 	int _follow_target_sub;
 	float _step_time_in_ms;
+	float _follow_offset;
 
 	uint64_t _target_updates;
 
diff --git a/src/modules/navigator/follow_target_params.c b/src/modules/navigator/follow_target_params.c
index 3796f638e4..27ad5645e1 100644
--- a/src/modules/navigator/follow_target_params.c
+++ b/src/modules/navigator/follow_target_params.c
@@ -53,3 +53,27 @@
  * @group Follow target
  */
 PARAM_DEFINE_FLOAT(NAV_MIN_FT_HT, 8.0f);
+
+/**
+ * Distance to follow target from
+ *
+ * The distance in meters to follow the target at
+ *
+ * @unit meters
+ * @min 1.0
+ * @group Follow target
+ */
+PARAM_DEFINE_FLOAT(NAV_FT_DST, 8.0f);
+
+/**
+ * Side to follow target from
+ *
+ * The side to follow the target from (front right = 0, behind = 1, front = 2, front left = 3)
+ *
+ * @unit n/a
+ * @min 0
+ * @max 3
+ * @group Follow target
+ */
+PARAM_DEFINE_INT32(NAV_FT_FS, 1);
+

From 42e04d4c11c7d425c7fb0baf936849c7134ea8e5 Mon Sep 17 00:00:00 2001
From: Jimmy Johnson <catch22@fastmail.net>
Date: Tue, 12 Apr 2016 00:04:26 -0700
Subject: [PATCH 0217/1230] fixing travis CI build

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

diff --git a/src/modules/navigator/follow_target.cpp b/src/modules/navigator/follow_target.cpp
index e3e7a4fed8..060149cb43 100644
--- a/src/modules/navigator/follow_target.cpp
+++ b/src/modules/navigator/follow_target.cpp
@@ -99,7 +99,7 @@ void FollowTarget::on_activation()
 
 	_follow_target_position = (FollowTargetPosition) _param_tracking_side.get();
 
-	if((_follow_target_position > 3) || (_follow_target_position < 0)) {
+	if((_follow_target_position > FOLLOW_FROM_LEFT) || (_follow_target_position < FOLLOW_FROM_RIGHT)) {
 		_follow_target_position = FOLLOW_FROM_BEHIND;
 	}
 

From 38b42789984db8bacbd9c4f7db5967873da4af77 Mon Sep 17 00:00:00 2001
From: Jimmy Johnson <catch22@fastmail.net>
Date: Tue, 12 Apr 2016 13:25:19 -0700
Subject: [PATCH 0218/1230] minimum follow target alt of 8 m added, protecting
 against nan values in pos controller, fixing ci build error

---
 src/modules/mc_pos_control/mc_pos_control_main.cpp | 10 ++++++++--
 src/modules/navigator/follow_target.cpp            |  5 +----
 src/modules/navigator/follow_target.h              |  4 ++--
 src/modules/navigator/mission_block.cpp            |  2 +-
 4 files changed, 12 insertions(+), 9 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 8dd6672071..d2431989fd 100644
--- a/src/modules/mc_pos_control/mc_pos_control_main.cpp
+++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp
@@ -1417,10 +1417,16 @@ MulticopterPositionControl::task_main()
 					_vel_sp(1) = (_pos_sp(1) - _pos(1)) * _params.pos_p(1);
 				}
 
+				// guard against any bad velocity values
+
+				bool velocity_valid = PX4_ISFINITE(_pos_sp_triplet.current.vx) &&
+									  PX4_ISFINITE(_pos_sp_triplet.current.vy) &&
+									  _pos_sp_triplet.current.velocity_valid;
+
 				// do not go slower than the follow target velocity when position tracking is active (set to valid)
 
 				if(_pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_FOLLOW_TARGET &&
-					_pos_sp_triplet.current.velocity_valid &&
+					velocity_valid &&
 					_pos_sp_triplet.current.position_valid) {
 
 					math::Vector<3> ft_vel(_pos_sp_triplet.current.vx, _pos_sp_triplet.current.vy, 0);
@@ -1444,7 +1450,7 @@ MulticopterPositionControl::task_main()
 					// track target using velocity only
 
 				} else if (_pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_FOLLOW_TARGET &&
-					_pos_sp_triplet.current.velocity_valid) {
+						velocity_valid) {
 
 					_vel_sp(0) = _pos_sp_triplet.current.vx;
 					_vel_sp(1) = _pos_sp_triplet.current.vy;
diff --git a/src/modules/navigator/follow_target.cpp b/src/modules/navigator/follow_target.cpp
index 060149cb43..7a3955bd4d 100644
--- a/src/modules/navigator/follow_target.cpp
+++ b/src/modules/navigator/follow_target.cpp
@@ -45,7 +45,6 @@
 #include <stdbool.h>
 #include <math.h>
 #include <fcntl.h>
-#include <float.h>
 
 #include <systemlib/err.h>
 
@@ -78,8 +77,6 @@ FollowTarget::FollowTarget(Navigator *navigator, const char *name) :
 	_target_vel.zero();
 	_target_distance.zero();
 	_target_position_offset.zero();
-
-	_rot_matrix = (_follow_position_matricies[_follow_target_position]);
 }
 
 FollowTarget::~FollowTarget()
@@ -97,7 +94,7 @@ void FollowTarget::on_activation()
 
 	_follow_offset = _param_tracking_dist.get() < 1.0F ? 1.0F : _param_tracking_dist.get();
 
-	_follow_target_position = (FollowTargetPosition) _param_tracking_side.get();
+	_follow_target_position = _param_tracking_side.get();
 
 	if((_follow_target_position > FOLLOW_FROM_LEFT) || (_follow_target_position < FOLLOW_FROM_RIGHT)) {
 		_follow_target_position = FOLLOW_FROM_BEHIND;
diff --git a/src/modules/navigator/follow_target.h b/src/modules/navigator/follow_target.h
index b00136b5ce..7395accd58 100644
--- a/src/modules/navigator/follow_target.h
+++ b/src/modules/navigator/follow_target.h
@@ -72,7 +72,7 @@ private:
 		WAIT_FOR_TARGET_POSITION
 	};
 
-	enum FollowTargetPosition {
+	enum {
 		FOLLOW_FROM_RIGHT,
 		FOLLOW_FROM_BEHIND,
 		FOLLOW_FROM_FRONT,
@@ -103,7 +103,7 @@ private:
 	control::BlockParamInt _param_tracking_side;
 
 	FollowTargetState _follow_target_state;
-	FollowTargetPosition _follow_target_position;
+	int _follow_target_position;
 
 	int _follow_target_sub;
 	float _step_time_in_ms;
diff --git a/src/modules/navigator/mission_block.cpp b/src/modules/navigator/mission_block.cpp
index 74dbd59f74..8c2c460c63 100644
--- a/src/modules/navigator/mission_block.cpp
+++ b/src/modules/navigator/mission_block.cpp
@@ -469,7 +469,7 @@ MissionBlock::set_follow_target_item(struct mission_item_s *item, float min_clea
 		item->lon = target.lon;
 		item->altitude = _navigator->get_home_position()->alt;
 
-		if (min_clearance > 0.0f) {
+		if (min_clearance > 8.0f) {
 			item->altitude += min_clearance;
 		} else {
 			item->altitude += 8.0f; // if min clearance is bad set it to 8.0 meters (well above the average height of a person)

From bb79d14cb19abe8aa47bcd893c8c2f0d5bb78e4d Mon Sep 17 00:00:00 2001
From: Jimmy Johnson <catch22@fastmail.net>
Date: Mon, 9 May 2016 07:11:38 -0700
Subject: [PATCH 0219/1230] adding lpf based on confidence of linear movement

---
 msg/follow_target.msg                   |   6 +-
 src/modules/navigator/follow_target.cpp | 209 +++++++++++++++---------
 src/modules/navigator/follow_target.h   |  31 +++-
 3 files changed, 163 insertions(+), 83 deletions(-)

diff --git a/msg/follow_target.msg b/msg/follow_target.msg
index c93f6fc719..75f124e2ee 100644
--- a/msg/follow_target.msg
+++ b/msg/follow_target.msg
@@ -1,3 +1,7 @@
 float64 lat			# target position (deg * 1e7)
 float64 lon			# target position (deg * 1e7) 
-float32 alt			# target position 
+float32 alt			# target position
+float32 vy			# target vel in y
+float32 vx			# target vel in x
+float32 vz			# target vel in z 
+uint8 est_cap # target reporting capabilities
diff --git a/src/modules/navigator/follow_target.cpp b/src/modules/navigator/follow_target.cpp
index 7a3955bd4d..3fc64d3c15 100644
--- a/src/modules/navigator/follow_target.cpp
+++ b/src/modules/navigator/follow_target.cpp
@@ -61,7 +61,7 @@ FollowTarget::FollowTarget(Navigator *navigator, const char *name) :
 	_param_min_alt(this, "NAV_MIN_FT_HT", false),
 	_param_tracking_dist(this,"NAV_FT_DST", false),
 	_param_tracking_side(this,"NAV_FT_FS", false),
-	_follow_target_state(WAIT_FOR_TARGET_POSITION),
+	_follow_target_state(SET_WAIT_FOR_TARGET_POSITION),
 	_follow_target_position(FOLLOW_FROM_BEHIND),
 	_follow_target_sub(-1),
 	_step_time_in_ms(0.0f),
@@ -69,14 +69,19 @@ FollowTarget::FollowTarget(Navigator *navigator, const char *name) :
 	_target_updates(0),
 	_last_update_time(0),
 	_current_target_motion({}),
-	_previous_target_motion({})
+	_previous_target_motion({}),
+	_confidence(0.0F),
+	_confidence_ratio(0.0F),
+	_yaw_rate(0.0F)
 {
+	_filtered_target_lat = _filtered_target_lon = 0.0F;
 	updateParams();
 	_current_vel.zero();
 	_step_vel.zero();
-	_target_vel.zero();
+	_est_target_vel.zero();
 	_target_distance.zero();
 	_target_position_offset.zero();
+	_target_position_delta.zero();
 }
 
 FollowTarget::~FollowTarget()
@@ -110,7 +115,7 @@ void FollowTarget::on_activation()
 void FollowTarget::on_active()
 {
 	struct map_projection_reference_s target_ref;
-	math::Vector<3> target_position(0, 0, 0);
+	math::Vector<3> target_reported_velocity(0, 0, 0);
 	follow_target_s target_motion = {};
 	uint64_t current_time = hrt_absolute_time();
 	bool _radius_entered = false;
@@ -131,6 +136,9 @@ void FollowTarget::on_active()
 
 		orb_copy(ORB_ID(follow_target), _follow_target_sub, &_current_target_motion);
 
+		target_reported_velocity(0) = _current_target_motion.vx;
+		target_reported_velocity(1) = _current_target_motion.vy;
+
 	} else if (((current_time - _current_target_motion.timestamp) / 1000 ) > TARGET_TIMEOUT_MS && target_velocity_valid()) {
 		reset_target_validity();
 	}
@@ -142,8 +150,7 @@ void FollowTarget::on_active()
 		// get distance to target
 
 		map_projection_init(&target_ref, _navigator->get_global_position()->lat, _navigator->get_global_position()->lon);
-		map_projection_project(&target_ref, _current_target_motion.lat, _current_target_motion.lon, &_target_distance(0),
-				       &_target_distance(1));
+		map_projection_project(&target_ref, _filtered_target_lat, _filtered_target_lon, &_target_distance(0), &_target_distance(1));
 
 		target_motion = _current_target_motion;
 
@@ -152,13 +159,6 @@ void FollowTarget::on_active()
 		map_projection_init(&target_ref, _current_target_motion.lat, _current_target_motion.lon);
 		map_projection_reproject(&target_ref, _target_position_offset(0), _target_position_offset(1), &target_motion.lat, &target_motion.lon);
 
-		// are we within the target acceptance radius?
-		// give a buffer to exit/enter the radius to give the velocity controller
-		// a chance to catch up
-
-		_radius_exited =  ((_target_position_offset + _target_distance).length() > (float) TARGET_ACCEPTANCE_RADIUS_M * 1.5f);
-		_radius_entered = ((_target_position_offset + _target_distance).length() < (float) TARGET_ACCEPTANCE_RADIUS_M);
-
 	}
 
 	// update target velocity
@@ -171,59 +171,99 @@ void FollowTarget::on_active()
 
 		if (dt_ms > 10.0F) {
 
+			math::Vector<3> prev_position_delta = _target_position_delta;
+
 			// get last gps known reference for target
 
 			map_projection_init(&target_ref, _previous_target_motion.lat, _previous_target_motion.lon);
 
 			// calculate distance the target has moved
 
-			map_projection_project(&target_ref, _current_target_motion.lat, _current_target_motion.lon, &(target_position(0)),  &(target_position(1)));
+			map_projection_project(&target_ref, _current_target_motion.lat, _current_target_motion.lon, &(_target_position_delta(0)), &(_target_position_delta(1)));
 
-			// only calculate offset rotation if target has moved
+			// filter out gps noise to figure out if we are actually moving
 
-			if(target_position.length() > 0.0F) {
+			if (_target_position_delta.length() > 0.1F && prev_position_delta.length() > 0.1F) {
+
+				float cos_ratio = (_target_position_delta * prev_position_delta)/(_target_position_delta.length() * prev_position_delta.length());
+
+				if(_confidence >= RESPONSIVENESS) {
+					_confidence = 0.0F; // reset confidence level to 50/50
+				}
+
+				_confidence += cos_ratio;
+
+				if (_confidence < -1.0F * RESPONSIVENESS) {
+					_confidence = -1.0F * RESPONSIVENESS;
+				}
+
+				if (_confidence > RESPONSIVENESS) {
+					_confidence = RESPONSIVENESS;
+				}
+
+				_confidence_ratio = (_confidence + RESPONSIVENESS) / (RESPONSIVENESS * 2.0F);
 
 				// track to the left, right, behind, or front
 
-				_target_position_offset = _rot_matrix * (target_position.normalized() * _follow_offset);
+				_filtered_target_position_delta = (_target_position_delta*_confidence_ratio) + _filtered_target_position_delta*(1.0F - _confidence_ratio);
+
+				// only track from a set side if we are 100% sure
+				// UAV is moving in a straight line
+
+				if(_confidence_ratio  >= 1.0F) {
+					_target_position_offset = _rot_matrix * (_filtered_target_position_delta.normalized() * _follow_offset);
+				}
+			} else  {
+				_filtered_target_position_delta.zero();
+				_confidence_ratio = _confidence = 0.0F;
 			}
-				// update the average velocity of the target based on the position
 
-				_target_vel = target_position / (dt_ms / 1000.0f);
+			yaw_angle = get_bearing_to_next_waypoint(_navigator->get_global_position()->lat,
+													_navigator->get_global_position()->lon,
+													_current_target_motion.lat,
+													_current_target_motion.lon);
 
-				// to keep the velocity increase/decrease smooth
-				// calculate how many velocity increments/decrements
-				// it will take to reach the targets velocity
-				// with the given amount of steps also add a feed forward input that adjusts the
-				// velocity as the position gap increases since
-				// just traveling at the exact velocity of the target will not
-				// get any closer to the target
+			_yaw_rate = (yaw_angle - _navigator->get_global_position()->yaw)/(dt_ms/1000.0F);
 
-				_step_vel = (_target_vel - _current_vel) + (_target_position_offset + _target_distance) * FF_K;
-				_step_vel /= (dt_ms / 1000.0F * (float) INTERPOLATION_PNTS);
-				_step_time_in_ms = dt_ms / (float) INTERPOLATION_PNTS;
+			_yaw_rate = _wrap_pi(_yaw_rate);
+
+			// update the average velocity of the target based on the position
+
+			_est_target_vel = _filtered_target_position_delta / (dt_ms / 1000.0f);
+
+			_filtered_target_lat = (_current_target_motion.lat*(double)_confidence_ratio) + _filtered_target_lat*(double)(1 - _confidence_ratio);
+			_filtered_target_lon = (_current_target_motion.lon*(double)_confidence_ratio) + _filtered_target_lon*(double)(1 - _confidence_ratio);
+
+			// are we within the target acceptance radius?
+			// give a buffer to exit/enter the radius to give the velocity controller
+			// a chance to catch up
+
+			_radius_exited =  ((_target_position_offset + _target_distance).length() > (float) TARGET_ACCEPTANCE_RADIUS_M * 1.5f);
+			_radius_entered = ((_target_position_offset + _target_distance).length() < (float) TARGET_ACCEPTANCE_RADIUS_M);
+
+			// to keep the velocity increase/decrease smooth
+			// calculate how many velocity increments/decrements
+			// it will take to reach the targets velocity
+			// with the given amount of steps also add a feed forward input that adjusts the
+			// velocity as the position gap increases since
+			// just traveling at the exact velocity of the target will not
+			// get any closer or farther from the target
+
+			_step_vel = (_est_target_vel - _current_vel) + (_target_position_offset + _target_distance) * FF_K;
+			_step_vel /= (dt_ms / 1000.0F * (float) INTERPOLATION_PNTS);
+			_step_time_in_ms = (dt_ms / (float) INTERPOLATION_PNTS);
 		}
-	}
 
-	// always point towards target
-
-	if (target_position_valid() && updated) {
-
-		yaw_angle = get_bearing_to_next_waypoint(_navigator->get_global_position()->lat,
-				_navigator->get_global_position()->lon,
-				_current_target_motion.lat,
-				_current_target_motion.lon);
-
-//			warnx(" lat %f (%f) lon %f (%f), dist x %f y %f (%f) yaw = %f mode = %d",
-//					target_motion.lat,
-//					(double )_navigator->get_global_position()->lat,
-//					target_motion.lon,
-//					(double ) _navigator->get_global_position()->lon,
-//					(double ) _target_distance(0),
-//					(double ) _target_distance(1),
-//					(double ) _target_distance.length(),
-//					(double) yaw_angle,
-//					_follow_target_state);
+		warnx(" _step_vel x %3.6f y %3.6f cur vel %3.6f %3.6f tar vel %3.6f %3.6f dist = %3.6f mode = %d con ratio = %3.6f con = %3.6f",
+				(double) _step_vel(0),
+				(double) _step_vel(1),
+				(double) _current_vel(0),
+				(double) _current_vel(1),
+				(double) _est_target_vel(0),
+				(double) _est_target_vel(1),
+				(double) _target_distance.length(),
+				_follow_target_state,
+				(double)_confidence_ratio, (double) _confidence);
 	}
 
 	// update state machine
@@ -234,14 +274,16 @@ void FollowTarget::on_active()
 
 			if (_radius_entered == true) {
 				_follow_target_state = TRACK_VELOCITY;
-
 			} else if (target_velocity_valid()) {
 				set_follow_target_item(&_mission_item, _param_min_alt.get(), target_motion, yaw_angle);
 				// keep the current velocity updated with the target velocity for when it's needed
-				_current_vel = _target_vel;
-				update_position_sp(true, true);
+				_current_vel = _est_target_vel;
+				_filtered_target_lat = _current_target_motion.lat;
+				_filtered_target_lon = _current_target_motion.lon;
+				// TODO: make max cruise speed less if very close to target
+				update_position_sp(true, true, _yaw_rate);
 			} else {
-				_follow_target_state = WAIT_FOR_TARGET_POSITION;
+				_follow_target_state = SET_WAIT_FOR_TARGET_POSITION;
 			}
 
 			break;
@@ -251,7 +293,6 @@ void FollowTarget::on_active()
 
 			if (_radius_exited == true) {
 				_follow_target_state = TRACK_POSITION;
-
 			} else if (target_velocity_valid()) {
 				set_follow_target_item(&_mission_item, _param_min_alt.get(), target_motion, yaw_angle);
 
@@ -260,40 +301,50 @@ void FollowTarget::on_active()
 					_last_update_time = current_time;
 				}
 
-				update_position_sp(true, false);
+				update_position_sp(true, false, _yaw_rate);
 			} else {
-				_follow_target_state = WAIT_FOR_TARGET_POSITION;
+				_follow_target_state = SET_WAIT_FOR_TARGET_POSITION;
 			}
 
 			break;
 		}
+	case SET_WAIT_FOR_TARGET_POSITION: {
 
+		// Climb to the minimum altitude
+		// and wait until a position is received
+
+		follow_target_s target = { };
+
+		// for now set the target at the minimum height above the uav
+
+		target.lat = _navigator->get_global_position()->lat;
+		target.lon = _navigator->get_global_position()->lon;
+		target.alt = 0.0F;
+
+		set_follow_target_item(&_mission_item, _param_min_alt.get(), target, yaw_angle);
+
+		update_position_sp(false, false, _yaw_rate);
+
+		_follow_target_state = WAIT_FOR_TARGET_POSITION;
+	}
 	case WAIT_FOR_TARGET_POSITION: {
-			// Climb to the minimum altitude
-			// and wait until a position is received
 
-			follow_target_s target = { };
+		if(target_position_valid()) {
+			_filtered_target_lat = _current_target_motion.lat;
+			_filtered_target_lon = _current_target_motion.lon;
+		}
 
-			// for now set the target at the minimum height above the uav
-
-			target.lat = _navigator->get_global_position()->lat;
-			target.lon = _navigator->get_global_position()->lon;
-			target.alt = 0.0F;
-
-			set_follow_target_item(&_mission_item, _param_min_alt.get(), target, NAN);
-
-			update_position_sp(false, false);
-
-			if (is_mission_item_reached() && target_velocity_valid()) {
-				_follow_target_state = TRACK_POSITION;
-			}
+		if (is_mission_item_reached() && target_velocity_valid()) {
+			_target_position_offset(0) = _follow_offset;
+			_follow_target_state = TRACK_POSITION;
+		}
 
 			break;
 		}
 	}
 }
 
-void FollowTarget::update_position_sp(bool use_velocity, bool use_position)
+void FollowTarget::update_position_sp(bool use_velocity, bool use_position, float yaw_rate)
 {
 	// convert mission item to current setpoint
 
@@ -310,31 +361,35 @@ void FollowTarget::update_position_sp(bool use_velocity, bool use_position)
 	pos_sp_triplet->current.vx = _current_vel(0);
 	pos_sp_triplet->current.vy = _current_vel(1);
 	pos_sp_triplet->next.valid = false;
+	pos_sp_triplet->current.yawspeed_valid = true;
+	pos_sp_triplet->current.yawspeed = yaw_rate;
 	_navigator->set_position_setpoint_triplet_updated();
 }
 
 void FollowTarget::reset_target_validity()
 {
+	_confidence = -1*RESPONSIVENESS;
+	_confidence_ratio = 0.0F;
 	_previous_target_motion = {};
 	_current_target_motion = {};
 	_target_updates = 0;
 	_current_vel.zero();
 	_step_vel.zero();
-	_target_vel.zero();
+	_est_target_vel.zero();
 	_target_distance.zero();
 	_target_position_offset.zero();
 	reset_mission_item_reached();
-	_follow_target_state = WAIT_FOR_TARGET_POSITION;
+	_follow_target_state = SET_WAIT_FOR_TARGET_POSITION;
 }
 
 bool FollowTarget::target_velocity_valid()
 {
 	// need at least 5 continuous data points for velocity estimate
-	return (_target_updates >= 5);
+	return (_target_updates >= 2);
 }
 
 bool FollowTarget::target_position_valid()
 {
 	// need at least 2 continuous data points for position estimate
-	return (_target_updates >= 2);
+	return (_target_updates >= 1);
 }
diff --git a/src/modules/navigator/follow_target.h b/src/modules/navigator/follow_target.h
index 7395accd58..25f77b162d 100644
--- a/src/modules/navigator/follow_target.h
+++ b/src/modules/navigator/follow_target.h
@@ -60,15 +60,20 @@ public:
 
 private:
 
-	static constexpr int TARGET_TIMEOUT_MS = 1500;
+	static constexpr int TARGET_TIMEOUT_MS = 5000;
 	static constexpr int TARGET_ACCEPTANCE_RADIUS_M = 5;
 	static constexpr int INTERPOLATION_PNTS = 20;
-	static constexpr float FF_K = .40F;
+	static constexpr float FF_K = .1F;
 	static constexpr float OFFSET_M = 8;
 
+	// higher numbers slow down the time it takes to decide whether a target is moving
+
+	static constexpr float RESPONSIVENESS = 8.0F;
+
 	enum FollowTargetState {
 		TRACK_POSITION,
 		TRACK_VELOCITY,
+		SET_WAIT_FOR_TARGET_POSITION,
 		WAIT_FOR_TARGET_POSITION
 	};
 
@@ -110,24 +115,40 @@ private:
 	float _follow_offset;
 
 	uint64_t _target_updates;
-
 	uint64_t _last_update_time;
 
 	math::Vector<3> _current_vel;
 	math::Vector<3> _step_vel;
-	math::Vector<3> _target_vel;
+	math::Vector<3> _est_target_vel;
 	math::Vector<3> _target_distance;
 	math::Vector<3> _target_position_offset;
+	math::Vector<3> _target_position_delta;
+	math::Vector<3> _filtered_target_position_delta;
 
 	follow_target_s _current_target_motion;
 	follow_target_s _previous_target_motion;
+	float _confidence;
+	float _confidence_ratio;
+	double _filtered_target_lat;
+	double _filtered_target_lon;
+	float _yaw_rate;
+
+	// Mavlink defined motion reporting capabilities
+
+    enum {
+        POS = 0,
+        VEL = 1,
+        ACCEL = 2,
+        ATT_RATES = 3
+    };
+
 	math::Matrix<3,3> _rot_matrix;
 	void track_target_position();
 	void track_target_velocity();
 	bool target_velocity_valid();
 	bool target_position_valid();
 	void reset_target_validity();
-	void update_position_sp(bool velocity_valid, bool position_valid);
+	void update_position_sp(bool velocity_valid, bool position_valid, float yaw_rate);
 	void update_target_motion();
 	void update_target_velocity();
 };

From 59296e0a494eb89dcf9bf8f7322d3cde383afe49 Mon Sep 17 00:00:00 2001
From: Jimmy Johnson <catch22@fastmail.net>
Date: Sun, 15 May 2016 16:14:22 -0700
Subject: [PATCH 0220/1230] adding user parameters, simplifying dynamic gps
 filter, adding yaw smoothing

---
 .../mc_pos_control/mc_pos_control_main.cpp    |   5 +-
 src/modules/navigator/follow_target.cpp       | 105 ++++++++++--------
 src/modules/navigator/follow_target.h         |  22 ++--
 src/modules/navigator/follow_target_params.c  |  13 +++
 4 files changed, 88 insertions(+), 57 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 d2431989fd..40f008fede 100644
--- a/src/modules/mc_pos_control/mc_pos_control_main.cpp
+++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp
@@ -1149,7 +1149,10 @@ void MulticopterPositionControl::control_auto(float dt)
 		_pos_sp = pos_sp_s.edivide(scale);
 
 		/* update yaw setpoint if needed */
-		if (PX4_ISFINITE(_pos_sp_triplet.current.yaw)) {
+
+		if (_pos_sp_triplet.current.yawspeed_valid && _pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_FOLLOW_TARGET) {
+			_att_sp.yaw_body = _att_sp.yaw_body + _pos_sp_triplet.current.yawspeed * dt;
+		} else if (PX4_ISFINITE(_pos_sp_triplet.current.yaw)) {
 			_att_sp.yaw_body = _pos_sp_triplet.current.yaw;
 		}
 
diff --git a/src/modules/navigator/follow_target.cpp b/src/modules/navigator/follow_target.cpp
index 3fc64d3c15..8bff1033a3 100644
--- a/src/modules/navigator/follow_target.cpp
+++ b/src/modules/navigator/follow_target.cpp
@@ -52,6 +52,7 @@
 #include <uORB/topics/position_setpoint_triplet.h>
 #include <uORB/topics/follow_target.h>
 #include <lib/geo/geo.h>
+#include <lib/mathlib/math/Limits.hpp>
 
 #include "navigator.h"
 
@@ -61,6 +62,8 @@ FollowTarget::FollowTarget(Navigator *navigator, const char *name) :
 	_param_min_alt(this, "NAV_MIN_FT_HT", false),
 	_param_tracking_dist(this,"NAV_FT_DST", false),
 	_param_tracking_side(this,"NAV_FT_FS", false),
+	_param_tracking_resp(this,"NAV_FT_RS", false),
+	_param_yaw_auto_max(this,"MC_YAWRAUTO_MAX", false),
 	_follow_target_state(SET_WAIT_FOR_TARGET_POSITION),
 	_follow_target_position(FOLLOW_FROM_BEHIND),
 	_follow_target_sub(-1),
@@ -70,10 +73,11 @@ FollowTarget::FollowTarget(Navigator *navigator, const char *name) :
 	_last_update_time(0),
 	_current_target_motion({}),
 	_previous_target_motion({}),
-	_confidence(0.0F),
-	_confidence_ratio(0.0F),
-	_yaw_rate(0.0F)
+	_avg_cos_ratio(0.0F),
+	_yaw_rate(0.0F),
+	_responsiveness(0.0F)
 {
+	_avg_cos_ratio = 0.0F;
 	_filtered_target_lat = _filtered_target_lon = 0.0F;
 	updateParams();
 	_current_vel.zero();
@@ -99,6 +103,10 @@ void FollowTarget::on_activation()
 
 	_follow_offset = _param_tracking_dist.get() < 1.0F ? 1.0F : _param_tracking_dist.get();
 
+	_responsiveness = math::constrain((float) _param_tracking_resp.get(), .1F, 1.0F);
+
+	_yaw_auto_max = _param_yaw_auto_max.get();
+
 	_follow_target_position = _param_tracking_side.get();
 
 	if((_follow_target_position > FOLLOW_FROM_LEFT) || (_follow_target_position < FOLLOW_FROM_RIGHT)) {
@@ -187,52 +195,33 @@ void FollowTarget::on_active()
 
 				float cos_ratio = (_target_position_delta * prev_position_delta)/(_target_position_delta.length() * prev_position_delta.length());
 
-				if(_confidence >= RESPONSIVENESS) {
-					_confidence = 0.0F; // reset confidence level to 50/50
+				_avg_cos_ratio = _responsiveness*_avg_cos_ratio + (1 - _responsiveness) * cos_ratio;
+
+				if(_avg_cos_ratio < 0) {
+					_avg_cos_ratio = 0.0F;
 				}
 
-				_confidence += cos_ratio;
-
-				if (_confidence < -1.0F * RESPONSIVENESS) {
-					_confidence = -1.0F * RESPONSIVENESS;
+				if (_avg_cos_ratio > 0.0F) {
+					_filtered_target_position_delta = _target_position_delta*_avg_cos_ratio + _filtered_target_position_delta*(1.0F - _avg_cos_ratio);
+				} else {
+					_filtered_target_position_delta.zero();
 				}
 
-				if (_confidence > RESPONSIVENESS) {
-					_confidence = RESPONSIVENESS;
-				}
-
-				_confidence_ratio = (_confidence + RESPONSIVENESS) / (RESPONSIVENESS * 2.0F);
-
-				// track to the left, right, behind, or front
-
-				_filtered_target_position_delta = (_target_position_delta*_confidence_ratio) + _filtered_target_position_delta*(1.0F - _confidence_ratio);
-
-				// only track from a set side if we are 100% sure
-				// UAV is moving in a straight line
-
-				if(_confidence_ratio  >= 1.0F) {
+				if(_avg_cos_ratio  >= .50F) {
 					_target_position_offset = _rot_matrix * (_filtered_target_position_delta.normalized() * _follow_offset);
 				}
+
 			} else  {
 				_filtered_target_position_delta.zero();
-				_confidence_ratio = _confidence = 0.0F;
+				_avg_cos_ratio = 0.0F;
 			}
 
-			yaw_angle = get_bearing_to_next_waypoint(_navigator->get_global_position()->lat,
-													_navigator->get_global_position()->lon,
-													_current_target_motion.lat,
-													_current_target_motion.lon);
-
-			_yaw_rate = (yaw_angle - _navigator->get_global_position()->yaw)/(dt_ms/1000.0F);
-
-			_yaw_rate = _wrap_pi(_yaw_rate);
-
 			// update the average velocity of the target based on the position
 
 			_est_target_vel = _filtered_target_position_delta / (dt_ms / 1000.0f);
 
-			_filtered_target_lat = (_current_target_motion.lat*(double)_confidence_ratio) + _filtered_target_lat*(double)(1 - _confidence_ratio);
-			_filtered_target_lon = (_current_target_motion.lon*(double)_confidence_ratio) + _filtered_target_lon*(double)(1 - _confidence_ratio);
+			_filtered_target_lat = (_current_target_motion.lat*(double)_avg_cos_ratio) + _filtered_target_lat*(double)(1 - _avg_cos_ratio);
+			_filtered_target_lon = (_current_target_motion.lon*(double)_avg_cos_ratio) + _filtered_target_lon*(double)(1 - _avg_cos_ratio);
 
 			// are we within the target acceptance radius?
 			// give a buffer to exit/enter the radius to give the velocity controller
@@ -252,9 +241,35 @@ void FollowTarget::on_active()
 			_step_vel = (_est_target_vel - _current_vel) + (_target_position_offset + _target_distance) * FF_K;
 			_step_vel /= (dt_ms / 1000.0F * (float) INTERPOLATION_PNTS);
 			_step_time_in_ms = (dt_ms / (float) INTERPOLATION_PNTS);
+
+			// if we are less than 3 meters from the target don't worry about trying to yaw
+			// just lock the yaw until we are a distance that makes sense
+
+			if(_target_distance.length() > 3.0F) {
+
+				// smooth yaw
+				// this really needs to control the yaw rate directly in the attitude pid controller
+				// but seems to work ok for now since that cannot be controlled directly in auto mode
+				// right now
+
+				yaw_angle = get_bearing_to_next_waypoint(_navigator->get_global_position()->lat,
+						_navigator->get_global_position()->lon,
+						_current_target_motion.lat,
+						_current_target_motion.lon);
+
+				_yaw_rate = (yaw_angle - _navigator->get_global_position()->yaw) / (dt_ms / 1000.0F);
+
+				_yaw_rate = _wrap_pi(_yaw_rate);
+
+				_yaw_rate = math::constrain(_yaw_rate, -1.0F*_yaw_auto_max, _yaw_auto_max)*.50F;
+
+			} else {
+				yaw_angle = _yaw_rate = NAN;
+			}
 		}
 
-		warnx(" _step_vel x %3.6f y %3.6f cur vel %3.6f %3.6f tar vel %3.6f %3.6f dist = %3.6f mode = %d con ratio = %3.6f con = %3.6f",
+
+		warnx(" _step_vel x %3.6f y %3.6f cur vel %3.6f %3.6f tar vel %3.6f %3.6f dist = %3.6f mode = %d con ratio = %3.6f yaw rate = %3.6f",
 				(double) _step_vel(0),
 				(double) _step_vel(1),
 				(double) _current_vel(0),
@@ -263,7 +278,7 @@ void FollowTarget::on_active()
 				(double) _est_target_vel(1),
 				(double) _target_distance.length(),
 				_follow_target_state,
-				(double)_confidence_ratio, (double) _confidence);
+				(double)_avg_cos_ratio, (double) _yaw_rate);
 	}
 
 	// update state machine
@@ -280,8 +295,7 @@ void FollowTarget::on_active()
 				_current_vel = _est_target_vel;
 				_filtered_target_lat = _current_target_motion.lat;
 				_filtered_target_lon = _current_target_motion.lon;
-				// TODO: make max cruise speed less if very close to target
-				update_position_sp(true, true, _yaw_rate);
+				update_position_sp(true, true, NAN);
 			} else {
 				_follow_target_state = SET_WAIT_FOR_TARGET_POSITION;
 			}
@@ -294,13 +308,14 @@ void FollowTarget::on_active()
 			if (_radius_exited == true) {
 				_follow_target_state = TRACK_POSITION;
 			} else if (target_velocity_valid()) {
-				set_follow_target_item(&_mission_item, _param_min_alt.get(), target_motion, yaw_angle);
 
 				if ((current_time - _last_update_time) / 1000 >= _step_time_in_ms) {
 					_current_vel += _step_vel;
 					_last_update_time = current_time;
 				}
 
+				set_follow_target_item(&_mission_item, _param_min_alt.get(), target_motion, yaw_angle);
+
 				update_position_sp(true, false, _yaw_rate);
 			} else {
 				_follow_target_state = SET_WAIT_FOR_TARGET_POSITION;
@@ -361,15 +376,15 @@ void FollowTarget::update_position_sp(bool use_velocity, bool use_position, floa
 	pos_sp_triplet->current.vx = _current_vel(0);
 	pos_sp_triplet->current.vy = _current_vel(1);
 	pos_sp_triplet->next.valid = false;
-	pos_sp_triplet->current.yawspeed_valid = true;
+	pos_sp_triplet->current.yawspeed_valid = PX4_ISFINITE(yaw_rate);
 	pos_sp_triplet->current.yawspeed = yaw_rate;
 	_navigator->set_position_setpoint_triplet_updated();
 }
 
 void FollowTarget::reset_target_validity()
 {
-	_confidence = -1*RESPONSIVENESS;
-	_confidence_ratio = 0.0F;
+	_yaw_rate = NAN;
+	_avg_cos_ratio = 0.0F;
 	_previous_target_motion = {};
 	_current_target_motion = {};
 	_target_updates = 0;
@@ -384,12 +399,12 @@ void FollowTarget::reset_target_validity()
 
 bool FollowTarget::target_velocity_valid()
 {
-	// need at least 5 continuous data points for velocity estimate
+	// need at least 2 continuous data points for velocity estimate
 	return (_target_updates >= 2);
 }
 
 bool FollowTarget::target_position_valid()
 {
-	// need at least 2 continuous data points for position estimate
+	// need at least 1 continuous data points for position estimate
 	return (_target_updates >= 1);
 }
diff --git a/src/modules/navigator/follow_target.h b/src/modules/navigator/follow_target.h
index 25f77b162d..b58e45b032 100644
--- a/src/modules/navigator/follow_target.h
+++ b/src/modules/navigator/follow_target.h
@@ -60,16 +60,12 @@ public:
 
 private:
 
-	static constexpr int TARGET_TIMEOUT_MS = 5000;
+	static constexpr int TARGET_TIMEOUT_MS = 2500;
 	static constexpr int TARGET_ACCEPTANCE_RADIUS_M = 5;
 	static constexpr int INTERPOLATION_PNTS = 20;
-	static constexpr float FF_K = .1F;
+	static constexpr float FF_K = .25F;
 	static constexpr float OFFSET_M = 8;
 
-	// higher numbers slow down the time it takes to decide whether a target is moving
-
-	static constexpr float RESPONSIVENESS = 8.0F;
-
 	enum FollowTargetState {
 		TRACK_POSITION,
 		TRACK_VELOCITY,
@@ -103,9 +99,12 @@ private:
 
 
 	Navigator *_navigator;
-	control::BlockParamFloat _param_min_alt;
-	control::BlockParamFloat _param_tracking_dist;
-	control::BlockParamInt _param_tracking_side;
+	control::BlockParamFloat	_param_min_alt;
+	control::BlockParamFloat 	_param_tracking_dist;
+	control::BlockParamInt 		_param_tracking_side;
+	control::BlockParamFloat 	_param_tracking_resp;
+	control::BlockParamFloat 	_param_yaw_auto_max;
+
 
 	FollowTargetState _follow_target_state;
 	int _follow_target_position;
@@ -127,11 +126,12 @@ private:
 
 	follow_target_s _current_target_motion;
 	follow_target_s _previous_target_motion;
-	float _confidence;
-	float _confidence_ratio;
+	float _avg_cos_ratio;
 	double _filtered_target_lat;
 	double _filtered_target_lon;
 	float _yaw_rate;
+	float _responsiveness;
+	float _yaw_auto_max;
 
 	// Mavlink defined motion reporting capabilities
 
diff --git a/src/modules/navigator/follow_target_params.c b/src/modules/navigator/follow_target_params.c
index 27ad5645e1..dd18461186 100644
--- a/src/modules/navigator/follow_target_params.c
+++ b/src/modules/navigator/follow_target_params.c
@@ -77,3 +77,16 @@ PARAM_DEFINE_FLOAT(NAV_FT_DST, 8.0f);
  */
 PARAM_DEFINE_INT32(NAV_FT_FS, 1);
 
+/**
+ * Dynamic filtering algorithm responsiveness to target movement
+ * lower numbers increase the responsiveness to changing long lat
+ * but also ignore less noise
+ *
+ * @unit n/a
+ * @min 0.0
+ * @max 1.0
+ * @decimal 2
+ * @group Follow target
+ */
+PARAM_DEFINE_FLOAT(NAV_FT_RS, 0.8f);
+

From 8e3720ca9a78d449bea2ef95991299b4afa46382 Mon Sep 17 00:00:00 2001
From: Jimmy Johnson <catch22@fastmail.net>
Date: Mon, 16 May 2016 14:53:37 -0700
Subject: [PATCH 0221/1230] adding clamp for yaw smoothing

---
 src/modules/navigator/follow_target.cpp | 73 +++++++++++++++----------
 src/modules/navigator/follow_target.h   |  1 +
 2 files changed, 44 insertions(+), 30 deletions(-)

diff --git a/src/modules/navigator/follow_target.cpp b/src/modules/navigator/follow_target.cpp
index 8bff1033a3..974b463e9b 100644
--- a/src/modules/navigator/follow_target.cpp
+++ b/src/modules/navigator/follow_target.cpp
@@ -74,11 +74,13 @@ FollowTarget::FollowTarget(Navigator *navigator, const char *name) :
 	_current_target_motion({}),
 	_previous_target_motion({}),
 	_avg_cos_ratio(0.0F),
+	_filtered_target_lat(0.0F),
+	_filtered_target_lon(0.0F),
 	_yaw_rate(0.0F),
-	_responsiveness(0.0F)
+	_responsiveness(0.0F),
+	_yaw_auto_max(0.0F),
+	_yaw_angle(0.0F)
 {
-	_avg_cos_ratio = 0.0F;
-	_filtered_target_lat = _filtered_target_lon = 0.0F;
 	updateParams();
 	_current_vel.zero();
 	_step_vel.zero();
@@ -105,7 +107,7 @@ void FollowTarget::on_activation()
 
 	_responsiveness = math::constrain((float) _param_tracking_resp.get(), .1F, 1.0F);
 
-	_yaw_auto_max = _param_yaw_auto_max.get();
+	_yaw_auto_max = math::radians(_param_yaw_auto_max.get());
 
 	_follow_target_position = _param_tracking_side.get();
 
@@ -130,7 +132,6 @@ void FollowTarget::on_active()
 	bool _radius_exited = false;
 	bool updated = false;
 	float dt_ms = 0;
-	float yaw_angle = NAN;
 
 	orb_check(_follow_target_sub, &updated);
 
@@ -164,7 +165,7 @@ void FollowTarget::on_active()
 
 		// use target offset
 
-		map_projection_init(&target_ref, _current_target_motion.lat, _current_target_motion.lon);
+		map_projection_init(&target_ref,  _filtered_target_lat, _filtered_target_lon);
 		map_projection_reproject(&target_ref, _target_position_offset(0), _target_position_offset(1), &target_motion.lat, &target_motion.lon);
 
 	}
@@ -203,11 +204,11 @@ void FollowTarget::on_active()
 
 				if (_avg_cos_ratio > 0.0F) {
 					_filtered_target_position_delta = _target_position_delta*_avg_cos_ratio + _filtered_target_position_delta*(1.0F - _avg_cos_ratio);
-				} else {
-					_filtered_target_position_delta.zero();
 				}
 
-				if(_avg_cos_ratio  >= .50F) {
+				// if ratio is high enough, track target from a side
+
+				if(_avg_cos_ratio  > .70F) {
 					_target_position_offset = _rot_matrix * (_filtered_target_position_delta.normalized() * _follow_offset);
 				}
 
@@ -243,42 +244,54 @@ void FollowTarget::on_active()
 			_step_time_in_ms = (dt_ms / (float) INTERPOLATION_PNTS);
 
 			// if we are less than 3 meters from the target don't worry about trying to yaw
-			// just lock the yaw until we are a distance that makes sense
+			// lock the yaw until we are a distance that makes sense
 
-			if(_target_distance.length() > 3.0F) {
+			if((_target_distance).length() > 3.0F) {
+
+				// yaw smoothing
 
-				// smooth yaw
 				// this really needs to control the yaw rate directly in the attitude pid controller
 				// but seems to work ok for now since that cannot be controlled directly in auto mode
 				// right now
 
-				yaw_angle = get_bearing_to_next_waypoint(_navigator->get_global_position()->lat,
+				_yaw_angle = get_bearing_to_next_waypoint(_navigator->get_global_position()->lat,
 						_navigator->get_global_position()->lon,
 						_current_target_motion.lat,
 						_current_target_motion.lon);
 
-				_yaw_rate = (yaw_angle - _navigator->get_global_position()->yaw) / (dt_ms / 1000.0F);
+				_yaw_rate = (_yaw_angle - _navigator->get_global_position()->yaw) / (dt_ms / 1000.0F);
 
 				_yaw_rate = _wrap_pi(_yaw_rate);
 
-				_yaw_rate = math::constrain(_yaw_rate, -1.0F*_yaw_auto_max, _yaw_auto_max)*.50F;
+				_yaw_rate = math::constrain(_yaw_rate, -1.0F*_yaw_auto_max, _yaw_auto_max);//*.80F;
 
 			} else {
-				yaw_angle = _yaw_rate = NAN;
+				_yaw_angle = _yaw_rate = NAN;
 			}
 		}
 
+//		warnx(" _step_vel x %3.6f y %3.6f cur vel %3.6f %3.6f tar vel %3.6f %3.6f dist = %3.6f (%3.6f) mode = %d con ratio = %3.6f yaw rate = %3.6f",
+//				(double) _step_vel(0),
+//				(double) _step_vel(1),
+//				(double) _current_vel(0),
+//				(double) _current_vel(1),
+//				(double) _est_target_vel(0),
+//				(double) _est_target_vel(1),
+//				(double) (_target_distance).length(),
+//				(double) (_target_position_offset + _target_distance).length(),
+//				_follow_target_state,
+//				(double)_avg_cos_ratio, (double) _yaw_rate);
+	}
 
-		warnx(" _step_vel x %3.6f y %3.6f cur vel %3.6f %3.6f tar vel %3.6f %3.6f dist = %3.6f mode = %d con ratio = %3.6f yaw rate = %3.6f",
-				(double) _step_vel(0),
-				(double) _step_vel(1),
-				(double) _current_vel(0),
-				(double) _current_vel(1),
-				(double) _est_target_vel(0),
-				(double) _est_target_vel(1),
-				(double) _target_distance.length(),
-				_follow_target_state,
-				(double)_avg_cos_ratio, (double) _yaw_rate);
+	// prevent yaw rate smoothing from over shooting target
+	// uses modulus of two pi to get diff
+	// by converting float to int
+
+	int angle_diff = (int) ((fabsf(_yaw_angle) - fabsf(_navigator->get_global_position()->yaw)) * 1e4F);
+	float mod_diff = ((float)(angle_diff % ((int) (M_PI_F * 2.0F * 1e4F))))/1e4F;
+
+	if (fabsf(mod_diff) < math::radians(5.0F)) {
+		_yaw_angle = _yaw_rate = NAN;
 	}
 
 	// update state machine
@@ -290,7 +303,7 @@ void FollowTarget::on_active()
 			if (_radius_entered == true) {
 				_follow_target_state = TRACK_VELOCITY;
 			} else if (target_velocity_valid()) {
-				set_follow_target_item(&_mission_item, _param_min_alt.get(), target_motion, yaw_angle);
+				set_follow_target_item(&_mission_item, _param_min_alt.get(), target_motion, _yaw_angle);
 				// keep the current velocity updated with the target velocity for when it's needed
 				_current_vel = _est_target_vel;
 				_filtered_target_lat = _current_target_motion.lat;
@@ -314,7 +327,7 @@ void FollowTarget::on_active()
 					_last_update_time = current_time;
 				}
 
-				set_follow_target_item(&_mission_item, _param_min_alt.get(), target_motion, yaw_angle);
+				set_follow_target_item(&_mission_item, _param_min_alt.get(), target_motion, _yaw_angle);
 
 				update_position_sp(true, false, _yaw_rate);
 			} else {
@@ -328,7 +341,7 @@ void FollowTarget::on_active()
 		// Climb to the minimum altitude
 		// and wait until a position is received
 
-		follow_target_s target = { };
+		follow_target_s target = {};
 
 		// for now set the target at the minimum height above the uav
 
@@ -336,7 +349,7 @@ void FollowTarget::on_active()
 		target.lon = _navigator->get_global_position()->lon;
 		target.alt = 0.0F;
 
-		set_follow_target_item(&_mission_item, _param_min_alt.get(), target, yaw_angle);
+		set_follow_target_item(&_mission_item, _param_min_alt.get(), target, _yaw_angle);
 
 		update_position_sp(false, false, _yaw_rate);
 
diff --git a/src/modules/navigator/follow_target.h b/src/modules/navigator/follow_target.h
index b58e45b032..b54e2d5f30 100644
--- a/src/modules/navigator/follow_target.h
+++ b/src/modules/navigator/follow_target.h
@@ -132,6 +132,7 @@ private:
 	float _yaw_rate;
 	float _responsiveness;
 	float _yaw_auto_max;
+	float _yaw_angle;
 
 	// Mavlink defined motion reporting capabilities
 

From bf6dff3e99cb64a952e1f1ea942926f94e9d227e Mon Sep 17 00:00:00 2001
From: Jimmy Johnson <catch22@fastmail.net>
Date: Mon, 16 May 2016 15:10:43 -0700
Subject: [PATCH 0222/1230] raising follow target filter responsiveness to
 tested value

---
 src/modules/navigator/follow_target_params.c | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/src/modules/navigator/follow_target_params.c b/src/modules/navigator/follow_target_params.c
index dd18461186..23c206b8f3 100644
--- a/src/modules/navigator/follow_target_params.c
+++ b/src/modules/navigator/follow_target_params.c
@@ -88,5 +88,5 @@ PARAM_DEFINE_INT32(NAV_FT_FS, 1);
  * @decimal 2
  * @group Follow target
  */
-PARAM_DEFINE_FLOAT(NAV_FT_RS, 0.8f);
+PARAM_DEFINE_FLOAT(NAV_FT_RS, 0.5f);
 

From a268845f1dd60adda130695d2f4ebb2cc170f777 Mon Sep 17 00:00:00 2001
From: Pavel Kirienko <pavel.kirienko@gmail.com>
Date: Mon, 16 May 2016 21:21:11 +0300
Subject: [PATCH 0223/1230] Cleaned up UAVCAN ioctl codes

---
 src/modules/uavcan/uavcan_main.cpp   |  2 +-
 src/modules/uavcan/uavcan_main.hpp   |  4 ----
 src/modules/uavcan/uavcan_module.hpp | 13 ++++++++++++-
 3 files changed, 13 insertions(+), 6 deletions(-)

diff --git a/src/modules/uavcan/uavcan_main.cpp b/src/modules/uavcan/uavcan_main.cpp
index fb9223a6a2..a7b92ee7d8 100644
--- a/src/modules/uavcan/uavcan_main.cpp
+++ b/src/modules/uavcan/uavcan_main.cpp
@@ -1115,7 +1115,7 @@ UavcanNode::ioctl(file *filp, int cmd, unsigned long arg)
 		break;
 
 
-	case UAVCANIOC_HARDPOINT_SET: {
+	case UAVCAN_IOCS_HARDPOINT_SET: {
 			const auto &hp_cmd = *reinterpret_cast<uavcan::equipment::hardpoint::Command *>(arg);
 			_hardpoint_controller.set_command(hp_cmd.hardpoint_id, hp_cmd.command);
 		}
diff --git a/src/modules/uavcan/uavcan_main.hpp b/src/modules/uavcan/uavcan_main.hpp
index 5c7d764602..dd508b1abc 100644
--- a/src/modules/uavcan/uavcan_main.hpp
+++ b/src/modules/uavcan/uavcan_main.hpp
@@ -73,10 +73,6 @@
 // we add two to allow for actuator_direct and busevent
 #define UAVCAN_NUM_POLL_FDS (NUM_ACTUATOR_CONTROL_GROUPS_UAVCAN+2)
 
-// IOCTL control codes
-static constexpr unsigned UAVCANIOCBASE = 0x7800;
-static constexpr unsigned UAVCANIOC_HARDPOINT_SET = _PX4_IOC(UAVCANIOCBASE, 0x10);
-
 /**
  * A UAVCAN node.
  */
diff --git a/src/modules/uavcan/uavcan_module.hpp b/src/modules/uavcan/uavcan_module.hpp
index 49c5437e62..7e54f1785f 100644
--- a/src/modules/uavcan/uavcan_module.hpp
+++ b/src/modules/uavcan/uavcan_module.hpp
@@ -65,7 +65,18 @@
 // ioctl interface
 #define _UAVCAN_IOC(_n)               (_IOC(_UAVCAN_IOCBASE, _n))
 #define _UAVCAN_IOCBASE               (0x4000)                        // IOCTL base for module UAVCAN
-#define UAVCAN_IOCG_NODEID_INPROGRESS _UAVCAN_IOC(1)                  // query if node identification is in progress
+/*
+ * Query if node identification is in progress. Returns:
+ *      EINVAL - not applicable in the current operating mode
+ *      ETIME  - network discovery complete
+ *      OK (0) - network discovery in progress
+ */
+#define UAVCAN_IOCG_NODEID_INPROGRESS _UAVCAN_IOC(1)
+/*
+ * Set hardpoint command. Accepts a pointer to uavcan::equipment::hardpoint::Command; returns nothing.
+ * The pointer may be invalidated once the call returns.
+ */
+#define UAVCAN_IOCS_HARDPOINT_SET       _UAVCAN_IOC(10)
 
 // public prototypes
 extern "C" __EXPORT int uavcan_main(int argc, char *argv[]);

From f6f47e3362e1be9f8c65b12d5961ce4f148718ae Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Beat=20K=C3=BCng?= <beat-kueng@gmx.net>
Date: Mon, 16 May 2016 07:51:57 +0200
Subject: [PATCH 0224/1230] cmake topic_listener: make topic_listener.cpp
 depend on msg_gen

This makes sure the .cpp file is regenerated on .msg file changes
---
 src/systemcmds/topic_listener/CMakeLists.txt | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/src/systemcmds/topic_listener/CMakeLists.txt b/src/systemcmds/topic_listener/CMakeLists.txt
index 087ce616a1..0984e546e3 100644
--- a/src/systemcmds/topic_listener/CMakeLists.txt
+++ b/src/systemcmds/topic_listener/CMakeLists.txt
@@ -33,6 +33,7 @@
 
 add_custom_command(OUTPUT topic_listener.cpp
                 COMMAND ${PYTHON_EXECUTABLE} ${CMAKE_SOURCE_DIR}/Tools/generate_listener.py ${CMAKE_SOURCE_DIR} > topic_listener.cpp
+                DEPENDS msg_gen
                 )
 
 add_custom_target(generate_topic_listener
@@ -51,6 +52,5 @@ px4_add_module(
 	DEPENDS
 		platforms__common
 		generate_topic_listener
-		msg_gen
 	)
 # vim: set noet ft=cmake fenc=utf-8 ff=unix : 

From 7f65e01d07ea0c1527ea1369750b8f4e53e86094 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Beat=20K=C3=BCng?= <beat-kueng@gmx.net>
Date: Mon, 16 May 2016 09:22:34 +0200
Subject: [PATCH 0225/1230] cmake: avoid GLOB for *.msg files and use an
 explicit .msg file enumeration

This makes sure that adding & removing of .msg files is handled properly
by the build system.
---
 CMakeLists.txt                        |   2 +-
 Tools/px_generate_uorb_topic_files.py |   3 +
 cmake/common/px4_base.cmake           |   7 ++
 msg/CMakeLists.txt                    | 130 ++++++++++++++++++++++++++
 4 files changed, 141 insertions(+), 1 deletion(-)
 create mode 100644 msg/CMakeLists.txt

diff --git a/CMakeLists.txt b/CMakeLists.txt
index 30ded0df5a..f9e11ee448 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -295,7 +295,7 @@ add_definitions(${definitions})
 #=============================================================================
 # source code generation
 #
-file(GLOB msg_files msg/*.msg)
+add_subdirectory(msg)
 px4_generate_messages(TARGET msg_gen
 	MSG_FILES ${msg_files}
 	OS ${OS}
diff --git a/Tools/px_generate_uorb_topic_files.py b/Tools/px_generate_uorb_topic_files.py
index 7ca22995b5..4bf1df6014 100755
--- a/Tools/px_generate_uorb_topic_files.py
+++ b/Tools/px_generate_uorb_topic_files.py
@@ -198,6 +198,9 @@ def convert_dir(format_idx, inputdir, outputdir, templatedir):
                 if not os.path.isfile(fn):
                         continue
 
+                if fn[-4:].lower() != '.msg':
+                        continue
+
                 generate_output_from_file(format_idx, fn, outputdir, templatedir, includepath)
         return True
 
diff --git a/cmake/common/px4_base.cmake b/cmake/common/px4_base.cmake
index 84383fc670..3bdcfe1333 100644
--- a/cmake/common/px4_base.cmake
+++ b/cmake/common/px4_base.cmake
@@ -415,6 +415,13 @@ function(px4_generate_messages)
 		)
 	set_source_files_properties(${msg_source_files_out} PROPERTIES GENERATED TRUE)
 
+	# We remove uORBTopics.cpp to make sure the generator is re-run, which is
+	# necessary when a .msg file is removed and because uORBTopics.cpp depends
+	# on all topics.
+	execute_process(COMMAND rm uORBTopics.cpp
+		WORKING_DIRECTORY ${msg_source_out_path}
+		ERROR_QUIET)
+
 	# multi messages for target OS
 	set(msg_multi_out_path
 		${CMAKE_BINARY_DIR}/src/platforms/${OS}/px4_messages)
diff --git a/msg/CMakeLists.txt b/msg/CMakeLists.txt
new file mode 100644
index 0000000000..dedd4983ac
--- /dev/null
+++ b/msg/CMakeLists.txt
@@ -0,0 +1,130 @@
+############################################################################
+#
+#   Copyright (c) 2016 PX4 Development Team. All rights reserved.
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions
+# are met:
+#
+# 1. Redistributions of source code must retain the above copyright
+#    notice, this list of conditions and the following disclaimer.
+# 2. Redistributions in binary form must reproduce the above copyright
+#    notice, this list of conditions and the following disclaimer in
+#    the documentation and/or other materials provided with the
+#    distribution.
+# 3. Neither the name PX4 nor the names of its contributors may be
+#    used to endorse or promote products derived from this software
+#    without specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.
+#
+############################################################################
+
+set(msg_file_names
+	actuator_armed.msg
+	actuator_controls.msg
+	actuator_direct.msg
+	actuator_outputs.msg
+	adc_report.msg
+	airspeed.msg
+	att_pos_mocap.msg
+	battery_status.msg
+	camera_trigger.msg
+	commander_state.msg
+	control_state.msg
+	cpuload.msg
+	debug_key_value.msg
+	differential_pressure.msg
+	distance_sensor.msg
+	ekf2_innovations.msg
+	ekf2_replay.msg
+	encoders.msg
+	esc_report.msg
+	esc_status.msg
+	estimator_status.msg
+	fence.msg
+	fence_vertex.msg
+	filtered_bottom_flow.msg
+	follow_target.msg
+	fw_pos_ctrl_status.msg
+	fw_virtual_attitude_setpoint.msg
+	fw_virtual_rates_setpoint.msg
+	geofence_result.msg
+	gps_inject_data.msg
+	hil_sensor.msg
+	home_position.msg
+	input_rc.msg
+	manual_control_setpoint.msg
+	mavlink_log.msg
+	mc_att_ctrl_status.msg
+	mc_virtual_attitude_setpoint.msg
+	mc_virtual_rates_setpoint.msg
+	mission.msg
+	mission_result.msg
+	multirotor_motor_limits.msg
+	offboard_control_mode.msg
+	optical_flow.msg
+	output_pwm.msg
+	parameter_update.msg
+	position_setpoint.msg
+	position_setpoint_triplet.msg
+	pwm_input.msg
+	qshell_req.msg
+	rc_channels.msg
+	rc_parameter_map.msg
+	safety.msg
+	satellite_info.msg
+	sensor_accel.msg
+	sensor_baro.msg
+	sensor_combined.msg
+	sensor_gyro.msg
+	sensor_mag.msg
+	servorail_status.msg
+	subsystem_info.msg
+	system_power.msg
+	tecs_status.msg
+	telemetry_status.msg
+	test_motor.msg
+	time_offset.msg
+	transponder_report.msg
+	uavcan_parameter_request.msg
+	uavcan_parameter_value.msg
+	vehicle_attitude.msg
+	vehicle_attitude_setpoint.msg
+	vehicle_command_ack.msg
+	vehicle_command.msg
+	vehicle_control_mode.msg
+	vehicle_force_setpoint.msg
+	vehicle_global_position.msg
+	vehicle_global_velocity_setpoint.msg
+	vehicle_gps_position.msg
+	vehicle_land_detected.msg
+	vehicle_local_position.msg
+	vehicle_local_position_setpoint.msg
+	vehicle_rates_setpoint.msg
+	vehicle_status.msg
+	vision_position_estimate.msg
+	vtol_vehicle_status.msg
+	wind_estimate.msg
+	)
+
+# Get absolute paths
+set(msg_files)
+foreach(msg_file ${msg_file_names})
+	list(APPEND msg_files ${CMAKE_CURRENT_SOURCE_DIR}/${msg_file})
+endforeach()
+
+set(msg_files ${msg_files} PARENT_SCOPE)
+
+# vim: set noet ft=cmake fenc=utf-8 ff=unix :

From ec991ab1a87979aeece0c38e11c767192f838679 Mon Sep 17 00:00:00 2001
From: Paul Riseborough <p_riseborough@live.com.au>
Date: Tue, 17 May 2016 11:17:54 +1000
Subject: [PATCH 0226/1230] ecl: update submodule reference

---
 src/lib/ecl | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/src/lib/ecl b/src/lib/ecl
index 172f4be594..88860d0307 160000
--- a/src/lib/ecl
+++ b/src/lib/ecl
@@ -1 +1 @@
-Subproject commit 172f4be594a0b33997a07f5b46899c141a6cee86
+Subproject commit 88860d0307ffcf014a3adb1236dd7a7fe6df0558

From 7d2d15643db37f6183a4adbb144d9ee36b36e5d2 Mon Sep 17 00:00:00 2001
From: Paul Riseborough <p_riseborough@live.com.au>
Date: Tue, 17 May 2016 11:14:12 +1000
Subject: [PATCH 0227/1230] ekf2: Add tuning for IMU switch-on bias errors

---
 src/modules/ekf2/ekf2_main.cpp |  8 +++++++-
 src/modules/ekf2/ekf2_params.c | 22 ++++++++++++++++++++++
 2 files changed, 29 insertions(+), 1 deletion(-)

diff --git a/src/modules/ekf2/ekf2_main.cpp b/src/modules/ekf2/ekf2_main.cpp
index de351376b0..f71f320230 100644
--- a/src/modules/ekf2/ekf2_main.cpp
+++ b/src/modules/ekf2/ekf2_main.cpp
@@ -248,6 +248,10 @@ private:
 	control::BlockParamFloat _tau_vel;	// time constant used by the output velocity complementary filter (s)
 	control::BlockParamFloat _tau_pos;	// time constant used by the output position complementary filter (s)
 
+	// IMU switch on bias paameters
+	control::BlockParamFloat _gyr_bias_init;	// 1-sigma gyro bias uncertainty at switch-on (rad/sec)
+	control::BlockParamFloat _acc_bias_init;	// 1-sigma accelerometer bias uncertainty at switch-on (m/s**2)
+
 	int update_subscriptions();
 
 };
@@ -332,7 +336,9 @@ Ekf2::Ekf2():
 	_flow_pos_y(this, "EKF2_OF_POS_Y", false, &_params->flow_pos_body(1)),
 	_flow_pos_z(this, "EKF2_OF_POS_Z", false, &_params->flow_pos_body(2)),
 	_tau_vel(this, "EKF2_TAU_VEL", false, &_params->vel_Tau),
-	_tau_pos(this, "EKF2_TAU_POS", false, &_params->pos_Tau)
+	_tau_pos(this, "EKF2_TAU_POS", false, &_params->pos_Tau),
+	_gyr_bias_init(this, "EKF2_GBIAS_INIT", false, &_params->switch_on_gyro_bias),
+	_acc_bias_init(this, "EKF2_ABIAS_INIT", false, &_params->switch_on_accel_bias)
 {
 
 }
diff --git a/src/modules/ekf2/ekf2_params.c b/src/modules/ekf2/ekf2_params.c
index 84ceebdf0d..5e670c1450 100644
--- a/src/modules/ekf2/ekf2_params.c
+++ b/src/modules/ekf2/ekf2_params.c
@@ -721,3 +721,25 @@ PARAM_DEFINE_FLOAT(EKF2_TAU_VEL, 0.5f);
  * @decimal 2
  */
 PARAM_DEFINE_FLOAT(EKF2_TAU_POS, 0.25f);
+
+/**
+ * 1-sigma IMU gyro switch-on bias
+ *
+ * @group EKF2
+ * @min 0.0
+ * @max 0.2
+ * @unit rad/sec
+ * @decimal 2
+ */
+PARAM_DEFINE_FLOAT(EKF2_GBIAS_INIT, 0.1f);
+
+/**
+ * 1-sigma IMU accelerometer switch-on bias
+ *
+ * @group EKF2
+ * @min 0.0
+ * @max 0.5
+ * @unit m/s/s
+ * @decimal 2
+ */
+PARAM_DEFINE_FLOAT(EKF2_ABIAS_INIT, 0.2f);

From 71db04c02c17a3f9b2a66dad485e7b7a8a4e043b Mon Sep 17 00:00:00 2001
From: Paul Riseborough <p_riseborough@live.com.au>
Date: Tue, 17 May 2016 13:15:26 +1000
Subject: [PATCH 0228/1230] ecl: update submodule reference

adds ability to set initial tilt alignment uncertainty
---
 src/lib/ecl | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/src/lib/ecl b/src/lib/ecl
index 88860d0307..57b2a256f7 160000
--- a/src/lib/ecl
+++ b/src/lib/ecl
@@ -1 +1 @@
-Subproject commit 88860d0307ffcf014a3adb1236dd7a7fe6df0558
+Subproject commit 57b2a256f783c9041a843dcc9876be5b4a9933ce

From 83cc9ef496d68ec0e3de0160c9532fb0690ae60b Mon Sep 17 00:00:00 2001
From: Paul Riseborough <p_riseborough@live.com.au>
Date: Tue, 17 May 2016 13:15:57 +1000
Subject: [PATCH 0229/1230] ekf2: Enable tuning of initial tilt alignment
 uncertainty

---
 src/modules/ekf2/ekf2_main.cpp |  4 +++-
 src/modules/ekf2/ekf2_params.c | 11 +++++++++++
 2 files changed, 14 insertions(+), 1 deletion(-)

diff --git a/src/modules/ekf2/ekf2_main.cpp b/src/modules/ekf2/ekf2_main.cpp
index f71f320230..45f77d4222 100644
--- a/src/modules/ekf2/ekf2_main.cpp
+++ b/src/modules/ekf2/ekf2_main.cpp
@@ -251,6 +251,7 @@ private:
 	// IMU switch on bias paameters
 	control::BlockParamFloat _gyr_bias_init;	// 1-sigma gyro bias uncertainty at switch-on (rad/sec)
 	control::BlockParamFloat _acc_bias_init;	// 1-sigma accelerometer bias uncertainty at switch-on (m/s**2)
+	control::BlockParamFloat _ang_err_init;		// 1-sigma uncertainty in tilt angle after gravity vector alignment (rad)
 
 	int update_subscriptions();
 
@@ -338,7 +339,8 @@ Ekf2::Ekf2():
 	_tau_vel(this, "EKF2_TAU_VEL", false, &_params->vel_Tau),
 	_tau_pos(this, "EKF2_TAU_POS", false, &_params->pos_Tau),
 	_gyr_bias_init(this, "EKF2_GBIAS_INIT", false, &_params->switch_on_gyro_bias),
-	_acc_bias_init(this, "EKF2_ABIAS_INIT", false, &_params->switch_on_accel_bias)
+	_acc_bias_init(this, "EKF2_ABIAS_INIT", false, &_params->switch_on_accel_bias),
+	_ang_err_init(this, "EKF2_ANGERR_INIT", false, &_params->initial_tilt_err)
 {
 
 }
diff --git a/src/modules/ekf2/ekf2_params.c b/src/modules/ekf2/ekf2_params.c
index 5e670c1450..4837d7df89 100644
--- a/src/modules/ekf2/ekf2_params.c
+++ b/src/modules/ekf2/ekf2_params.c
@@ -743,3 +743,14 @@ PARAM_DEFINE_FLOAT(EKF2_GBIAS_INIT, 0.1f);
  * @decimal 2
  */
 PARAM_DEFINE_FLOAT(EKF2_ABIAS_INIT, 0.2f);
+
+/**
+ * 1-sigma tilt angle uncertainty after gravity vector alignment
+ *
+ * @group EKF2
+ * @min 0.0
+ * @max 0.5
+ * @unit rad
+ * @decimal 3
+ */
+PARAM_DEFINE_FLOAT(EKF2_ANGERR_INIT, 0.1f);

From 1e155776fdfac1827cf3738aac904c7a1762c022 Mon Sep 17 00:00:00 2001
From: Julian Oes <julian@oes.ch>
Date: Tue, 17 May 2016 09:08:33 -0400
Subject: [PATCH 0230/1230] Revert "Switch from EKF2 to LPE since SITL s is not
 any more a bearable workflow with EKF2 init lag"

This reverts commit 503966165bf0322a521bebee505b13ec90d25ec4.
---
 posix-configs/SITL/init/rcS_jmavsim_iris | 3 +--
 1 file changed, 1 insertion(+), 2 deletions(-)

diff --git a/posix-configs/SITL/init/rcS_jmavsim_iris b/posix-configs/SITL/init/rcS_jmavsim_iris
index a80931b016..ab0e045afc 100644
--- a/posix-configs/SITL/init/rcS_jmavsim_iris
+++ b/posix-configs/SITL/init/rcS_jmavsim_iris
@@ -54,8 +54,7 @@ sensors start
 commander start
 land_detector start multicopter
 navigator start
-attitude_estimator_q start
-local_position_estimator start
+ekf2 start
 mc_pos_control start
 mc_att_control start
 mixer load /dev/pwm_output0 ../../../../ROMFS/px4fmu_common/mixers/quad_x.main.mix

From 6ddffb71ea6e187c1e172ce46943ebf16065208f Mon Sep 17 00:00:00 2001
From: Julian Oes <julian@oes.ch>
Date: Tue, 17 May 2016 09:14:59 -0400
Subject: [PATCH 0231/1230] SITL init: set EKF2 params for SITL

The sensors in the SITL environment are near ideal, so the
initialization in ekf2 can happen quicker.
---
 posix-configs/SITL/init/rcS_gazebo_iris  | 2 ++
 posix-configs/SITL/init/rcS_jmavsim_iris | 2 ++
 2 files changed, 4 insertions(+)

diff --git a/posix-configs/SITL/init/rcS_gazebo_iris b/posix-configs/SITL/init/rcS_gazebo_iris
index dd27b3f4c2..650c7212f4 100644
--- a/posix-configs/SITL/init/rcS_gazebo_iris
+++ b/posix-configs/SITL/init/rcS_gazebo_iris
@@ -33,6 +33,8 @@ param set MC_PITCH_P 5
 param set MC_ROLL_P 5
 param set MPC_Z_VEL_P 0.8
 param set MPC_Z_VEL_I 0.15
+param set EKF2_GBIAS_INIT 0.01
+param set EKF2_ANGERR_INIT 0.01
 simulator start -s
 rgbledsim start
 tone_alarm start
diff --git a/posix-configs/SITL/init/rcS_jmavsim_iris b/posix-configs/SITL/init/rcS_jmavsim_iris
index ab0e045afc..3f13970349 100644
--- a/posix-configs/SITL/init/rcS_jmavsim_iris
+++ b/posix-configs/SITL/init/rcS_jmavsim_iris
@@ -40,6 +40,8 @@ param set MPC_HOLD_MAX_Z 2.0
 param set MPC_HOLD_XY_DZ 0.1
 param set MPC_Z_VEL_MAX 2.0
 param set MPC_Z_VEL_P 0.4
+param set EKF2_GBIAS_INIT 0.01
+param set EKF2_ANGERR_INIT 0.01
 simulator start -s
 rgbledsim start
 tone_alarm start

From f5a1569f4940cbaaee7ac806f95aaa3e3f5597f0 Mon Sep 17 00:00:00 2001
From: Paul Riseborough <p_riseborough@live.com.au>
Date: Wed, 18 May 2016 12:19:01 +1000
Subject: [PATCH 0232/1230] ecl: update library reference

fixes bug that can cause alignment to fail for pitch angles near +-90 deg
---
 src/lib/ecl | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/src/lib/ecl b/src/lib/ecl
index 57b2a256f7..50ba26a434 160000
--- a/src/lib/ecl
+++ b/src/lib/ecl
@@ -1 +1 @@
-Subproject commit 57b2a256f783c9041a843dcc9876be5b4a9933ce
+Subproject commit 50ba26a43414be21a9f2ca0616663a3c94ad37ff

From 567e10eb6633be1e3eb0d36a89fe5e0f07445057 Mon Sep 17 00:00:00 2001
From: Paul Riseborough <p_riseborough@live.com.au>
Date: Wed, 18 May 2016 21:24:34 +1000
Subject: [PATCH 0233/1230] ecl: update library reference

Further updates to improve angular alignment consistency
---
 src/lib/ecl | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/src/lib/ecl b/src/lib/ecl
index 50ba26a434..867bc8c601 160000
--- a/src/lib/ecl
+++ b/src/lib/ecl
@@ -1 +1 @@
-Subproject commit 50ba26a43414be21a9f2ca0616663a3c94ad37ff
+Subproject commit 867bc8c60155ed48c5e8532b1adf2b1a822f796f

From fee5e87d8784974c125dc02aa46bf98ec3e43c90 Mon Sep 17 00:00:00 2001
From: Paul Riseborough <p_riseborough@live.com.au>
Date: Thu, 19 May 2016 12:58:58 +1000
Subject: [PATCH 0234/1230] ecl: update submodule reference

Incorporates big fixes for use of external vision data
---
 src/lib/ecl | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/src/lib/ecl b/src/lib/ecl
index 867bc8c601..e3b9800cac 160000
--- a/src/lib/ecl
+++ b/src/lib/ecl
@@ -1 +1 @@
-Subproject commit 867bc8c60155ed48c5e8532b1adf2b1a822f796f
+Subproject commit e3b9800cac459e7c279395c18191a872f465f848

From fd0f52bebd0a2c05a860768bfcf51cd1ca078701 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Beat=20K=C3=BCng?= <beat-kueng@gmx.net>
Date: Thu, 19 May 2016 11:08:37 +0200
Subject: [PATCH 0235/1230] orb macros: cleanup some unused code (#4576)

---
 msg/mission.msg                    | 2 --
 src/drivers/drv_orb_dev.h          | 3 ---
 src/modules/navigator/navigation.h | 3 ---
 src/modules/uORB/uORB.h            | 4 +---
 src/modules/uORB/uORBManager.hpp   | 3 ---
 5 files changed, 1 insertion(+), 14 deletions(-)

diff --git a/msg/mission.msg b/msg/mission.msg
index e208d36929..b25940c07a 100644
--- a/msg/mission.msg
+++ b/msg/mission.msg
@@ -2,6 +2,4 @@ int32 dataman_id	# default 0, there are two offboard storage places in the datam
 uint32 count		# count of the missions stored in the dataman
 int32 current_seq	# default -1, start at the one changed latest
 
-# fixme: there is no mission definition in objects_common.cpp
-# but it's required for systemcmds/topic_listener/topic_listener
 # TOPICS mission offboard_mission onboard_mission
diff --git a/src/drivers/drv_orb_dev.h b/src/drivers/drv_orb_dev.h
index 37e9bdb7cf..79a2895fb5 100644
--- a/src/drivers/drv_orb_dev.h
+++ b/src/drivers/drv_orb_dev.h
@@ -45,9 +45,6 @@
 #include <sys/ioctl.h>
 #include <stdint.h>
 
-/* XXX for ORB_DECLARE used in many drivers */
-#include "../modules/uORB/uORB.h"
-
 /*
  * ioctl() definitions
  */
diff --git a/src/modules/navigator/navigation.h b/src/modules/navigator/navigation.h
index 7fadabbd2c..3497b8563e 100644
--- a/src/modules/navigator/navigation.h
+++ b/src/modules/navigator/navigation.h
@@ -119,8 +119,5 @@ struct mission_item_s {
  * @}
  */
 
-/* register this as object request broker structure */
-ORB_DECLARE(offboard_mission);
-ORB_DECLARE(onboard_mission);
 
 #endif
diff --git a/src/modules/uORB/uORB.h b/src/modules/uORB/uORB.h
index fba37a57af..57e12f9236 100644
--- a/src/modules/uORB/uORB.h
+++ b/src/modules/uORB/uORB.h
@@ -89,16 +89,14 @@ enum ORB_PRIO {
 #define ORB_ID(_name)		&__orb_##_name
 
 /**
- * Declare (prototype) the uORB metadata for a topic.
+ * Declare (prototype) the uORB metadata for a topic (used by code generators).
  *
  * @param _name		The name of the topic.
  */
 #if defined(__cplusplus)
 # define ORB_DECLARE(_name)		extern "C" const struct orb_metadata __orb_##_name __EXPORT
-# define ORB_DECLARE_OPTIONAL(_name)	extern "C" const struct orb_metadata __orb_##_name __EXPORT
 #else
 # define ORB_DECLARE(_name)		extern const struct orb_metadata __orb_##_name __EXPORT
-# define ORB_DECLARE_OPTIONAL(_name)	extern const struct orb_metadata __orb_##_name __EXPORT
 #endif
 
 /**
diff --git a/src/modules/uORB/uORBManager.hpp b/src/modules/uORB/uORBManager.hpp
index eb02a42b39..ca5aee846e 100644
--- a/src/modules/uORB/uORBManager.hpp
+++ b/src/modules/uORB/uORBManager.hpp
@@ -187,9 +187,6 @@ public:
 	 *      for the topic.
 	 * @return    ERROR on error, otherwise returns a handle
 	 *      that can be used to read and update the topic.
-	 *      If the topic in question is not known (due to an
-	 *      ORB_DEFINE_OPTIONAL with no corresponding ORB_DECLARE)
-	 *      this function will return -1 and set errno to ENOENT.
 	 */
 	int  orb_subscribe(const struct orb_metadata *meta) ;
 

From 02ba3317e908723e2bb37fdddc62f1133e2aea13 Mon Sep 17 00:00:00 2001
From: Roman Bapst <bapstr@ethz.ch>
Date: Thu, 19 May 2016 15:28:04 +0200
Subject: [PATCH 0236/1230] WIP: Manual attitude setpoint for large heading
 errors (#4564)

* mc pos control: in manual mode calculate attitude setpoint
such that it reflects the users intuition of roll and pitch
for any given heading error

* added some comments on the new manual attitude setpoint generation

* make calculation shorter
---
 .../mc_pos_control/mc_pos_control_main.cpp    | 36 +++++++++++++++++--
 1 file changed, 34 insertions(+), 2 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 40f008fede..5d9d6ec1ae 100644
--- a/src/modules/mc_pos_control/mc_pos_control_main.cpp
+++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp
@@ -2002,8 +2002,40 @@ MulticopterPositionControl::task_main()
 
 			math::Matrix<3, 3> R_sp;
 
-			/* construct attitude setpoint rotation matrix */
-			R_sp.from_euler(_att_sp.roll_body, _att_sp.pitch_body, _att_sp.yaw_body);
+			// construct attitude setpoint rotation matrix. modify the setpoints for roll
+			// and pitch such that they reflect the user's intention even if a yaw error
+			// (yaw_sp - yaw) is present. In the presence of a yaw error constructing a rotation matrix
+			// from the pure euler angle setpoints will lead to unexpected attitude behaviour from
+			// the user's view as the euler angle sequence uses the  yaw setpoint and not the current
+			// heading of the vehicle.
+
+			// calculate our current yaw error
+			float yaw_error = _wrap_pi(_att_sp.yaw_body - _yaw);
+
+			// compute the vector obtained by rotating a z unit vector by the rotation
+			// given by the roll and pitch commands of the user
+			math::Vector<3> zB = {0, 0, 1};
+			math::Matrix<3,3> R_sp_roll_pitch;
+			R_sp_roll_pitch.from_euler(_att_sp.roll_body, _att_sp.pitch_body, 0);
+			math::Vector<3> z_roll_pitch_sp = R_sp_roll_pitch * zB;
+
+
+			// transform the vector into a new frame which is rotated around the z axis
+			// by the current yaw error. this vector defines the desired tilt when we look
+			// into the direction of the desired heading
+			math::Matrix<3,3> R_yaw_correction;
+			R_yaw_correction.from_euler(0.0f, 0.0f, -yaw_error);
+			z_roll_pitch_sp = R_yaw_correction * z_roll_pitch_sp;
+
+			// use the formula z_roll_pitch_sp = R_tilt * [0;0;1]
+			// to calculate the new desired roll and pitch angles
+			// R_tilt can be written as a function of the new desired roll and pitch
+			// angles. we get three equations and have to solve for 2 unknowns
+			float pitch_new = asinf(z_roll_pitch_sp(0));
+			float roll_new = -atan2f(z_roll_pitch_sp(1), z_roll_pitch_sp(2));
+
+			R_sp.from_euler(roll_new, pitch_new, _att_sp.yaw_body);
+
 			memcpy(&_att_sp.R_body[0], R_sp.data, sizeof(_att_sp.R_body));
 
 			/* reset the acceleration set point for all non-attitude flight modes */

From 529460d5736287e90f3a4970d5431b672140b5c4 Mon Sep 17 00:00:00 2001
From: Kabir Mohammed <mhkabir98@gmail.com>
Date: Fri, 29 Apr 2016 09:39:23 +0530
Subject: [PATCH 0237/1230] Fix RPi2 build system

---
 Tools/scp_upload.sh                           | 24 +++++
 cmake/common/px4_base.cmake                   | 18 ++++
 cmake/configs/posix_rpi2_release.cmake        | 98 ++++++++++++++-----
 ...olchain-arm-linux-gnueabihf-raspbian.cmake | 86 ++++++++++++++++
 posix-configs/rpi2/init/rcS_navio             |  0
 posix-configs/rpi2/mainapp.config             | 17 ++++
 src/firmware/posix/CMakeLists.txt             | 24 +++++
 .../df_bmp280_wrapper/df_bmp280_wrapper.cpp   |  4 +-
 8 files changed, 245 insertions(+), 26 deletions(-)
 create mode 100755 Tools/scp_upload.sh
 create mode 100644 cmake/toolchains/Toolchain-arm-linux-gnueabihf-raspbian.cmake
 delete mode 100644 posix-configs/rpi2/init/rcS_navio
 create mode 100644 posix-configs/rpi2/mainapp.config

diff --git a/Tools/scp_upload.sh b/Tools/scp_upload.sh
new file mode 100755
index 0000000000..37b2ce9898
--- /dev/null
+++ b/Tools/scp_upload.sh
@@ -0,0 +1,24 @@
+#!/bin/bash
+
+if [[ "$#" < 2 ]]; then
+	echo "usage: scp_upload.sh SRC1 [SRC2 ...] DEST"
+	exit
+fi
+
+echo "Uploading..."
+
+# Get last argument
+for last; do true; done
+
+# Go through source files and push them one by one.
+i=0
+for arg
+do
+	if [[ $((i+1)) == "$#" ]]; then
+		break
+	fi
+	# echo "Pushing $arg to $last"
+	#adb push $arg $last
+	scp $arg pi@px4autopilot:$last
+	((i+=1))
+done
diff --git a/cmake/common/px4_base.cmake b/cmake/common/px4_base.cmake
index 3bdcfe1333..2098996188 100644
--- a/cmake/common/px4_base.cmake
+++ b/cmake/common/px4_base.cmake
@@ -528,6 +528,24 @@ function(px4_add_adb_push)
 		)
 endfunction()
 
+function(px4_add_scp_push)
+	px4_parse_function_args(
+		NAME px4_add_upload
+		ONE_VALUE OS BOARD OUT DEST
+		MULTI_VALUE FILES DEPENDS
+		REQUIRED OS BOARD OUT FILES DEPENDS DEST
+		ARGN ${ARGN})
+
+	add_custom_target(${OUT}
+		COMMAND ${CMAKE_SOURCE_DIR}/Tools/scp_upload.sh ${FILES} ${DEST}
+		DEPENDS ${DEPENDS}
+		WORKING_DIRECTORY ${CMAKE_BINARY_DIR}
+		COMMENT "uploading ${BUNDLE}"
+		VERBATIM
+		USES_TERMINAL
+		)
+endfunction()
+
 
 #=============================================================================
 #
diff --git a/cmake/configs/posix_rpi2_release.cmake b/cmake/configs/posix_rpi2_release.cmake
index b10914951a..64d20d2fbb 100644
--- a/cmake/configs/posix_rpi2_release.cmake
+++ b/cmake/configs/posix_rpi2_release.cmake
@@ -1,59 +1,109 @@
 include(posix/px4_impl_posix)
 
-if ("${RPI_TOOLCHAIN_DIR}" STREQUAL "")
-	set(RPI_TOOLCHAIN_DIR /opt/rpi_toolchain)
-endif()
+set(CMAKE_TOOLCHAIN_FILE ${CMAKE_SOURCE_DIR}/cmake/toolchains/Toolchain-arm-linux-gnueabihf-raspbian.cmake)
 
 set(CMAKE_PROGRAM_PATH
 	"${RPI_TOOLCHAIN_DIR}/gcc-linaro-arm-linux-gnueabihf-raspbian/bin"
 	${CMAKE_PROGRAM_PATH}
 	)
 
-set(CMAKE_TOOLCHAIN_FILE ${CMAKE_SOURCE_DIR}/cmake/toolchains/Toolchain-arm-linux-gnueabihf.cmake)
+# This definition allows to differentiate if this just the usual POSIX build
+# or if it is for the RPi.
+add_definitions(
+	-D__PX4_POSIX_RPI2
+	)
 
 set(config_module_list
+	#
+	# Board support modules
+	#
 	drivers/device
-	platforms/common
-	platforms/posix/px4_layer
-	platforms/posix/work_queue
+	modules/sensors
+	platforms/posix/drivers/df_mpu9250_wrapper
+	platforms/posix/drivers/df_bmp280_wrapper
+	platforms/posix/drivers/df_hmc5883_wrapper
+	platforms/posix/drivers/df_trone_wrapper
+	platforms/posix/drivers/df_isl29501_wrapper
+
+	#
+	# System commands
+	#
 	systemcmds/param
 	systemcmds/mixer
 	systemcmds/ver
 	systemcmds/esc_calib
-	systemcmds/reboot
 	systemcmds/topic_listener
 	systemcmds/perf
-	modules/uORB
-	modules/param
-	modules/systemlib
-	modules/systemlib/mixer
-	modules/sensors
-	modules/mavlink
+
+	#
+	# Estimation modules (EKF/ SO3 / other filters)
+	#
+	#modules/attitude_estimator_ekf
+	modules/ekf_att_pos_estimator
 	modules/attitude_estimator_q
 	modules/position_estimator_inav
-	modules/navigator
-	modules/vtol_att_control
-	modules/mc_pos_control
+	modules/local_position_estimator
+	modules/ekf2
+
+	#
+	# Vehicle Control
+	#
 	modules/mc_att_control
-	modules/land_detector
+	modules/mc_pos_control
 	modules/fw_att_control
-	modules/fw_pos_control_l1
-	modules/dataman
+	modules/fw_pos_control_l1	
+	modules/vtol_att_control
+
+	#
+	# Library modules
+	#
 	modules/sdlog2
 	modules/logger
 	modules/commander
 	modules/load_mon
+	modules/param
+	modules/systemlib
+	modules/systemlib/mixer
+	modules/uORB
+	modules/dataman
+	modules/land_detector
+	modules/navigator
+	modules/mavlink
+
+	#
+	# PX4 drivers
+	#
+
+	#
+	# Libraries
+	#
 	lib/controllib
 	lib/mathlib
 	lib/mathlib/math/filter
-	lib/conversion
-	lib/ecl
-	lib/external_lgpl
 	lib/geo
+	lib/ecl
 	lib/geo_lookup
 	lib/launchdetection
+	lib/external_lgpl
+	lib/conversion
 	lib/terrain_estimation
 	lib/runway_takeoff
 	lib/tailsitter_recovery
 	lib/DriverFramework/framework
-)
+
+	#
+	# POSIX
+	#
+	platforms/common
+	platforms/posix/px4_layer
+	platforms/posix/work_queue
+
+	)
+
+set(config_df_driver_list
+	mpu9250
+	bmp280
+	hmc5883
+	trone
+	isl29501
+	)
diff --git a/cmake/toolchains/Toolchain-arm-linux-gnueabihf-raspbian.cmake b/cmake/toolchains/Toolchain-arm-linux-gnueabihf-raspbian.cmake
new file mode 100644
index 0000000000..bfd68043da
--- /dev/null
+++ b/cmake/toolchains/Toolchain-arm-linux-gnueabihf-raspbian.cmake
@@ -0,0 +1,86 @@
+# defines:
+#
+# NM
+# OBJCOPY
+# LD
+# CXX_COMPILER
+# C_COMPILER
+# CMAKE_SYSTEM_NAME
+# CMAKE_SYSTEM_VERSION
+# GENROMFS
+# LINKER_FLAGS
+# CMAKE_EXE_LINKER_FLAGS
+# CMAKE_FIND_ROOT_PATH
+# CMAKE_FIND_ROOT_PATH_MODE_PROGRAM
+# CMAKE_FIND_ROOT_PATH_MODE_LIBRARY
+# CMAKE_FIND_ROOT_PATH_MODE_INCLUDE
+
+include(CMakeForceCompiler)
+
+if ("$ENV{RPI_TOOLCHAIN_DIR}" STREQUAL "")
+        message(FATAL_ERROR "RPI_TOOLCHAIN_DIR not set")
+else()
+        set(RPI_TOOLCHAIN_DIR $ENV{RPI_TOOLCHAIN_DIR})
+endif()
+
+# this one is important
+set(CMAKE_SYSTEM_NAME Generic)
+
+#this one not so much
+set(CMAKE_SYSTEM_VERSION 1)
+
+# specify the cross compiler
+find_program(C_COMPILER arm-linux-gnueabihf-gcc
+	PATHS ${RPI_TOOLCHAIN_DIR}/gcc-linaro-arm-linux-gnueabihf-raspbian/bin
+	NO_DEFAULT_PATH
+	)
+
+if(NOT C_COMPILER)
+	message(FATAL_ERROR "could not find arm-linux-gnueabihf-gcc compiler")
+endif()
+cmake_force_c_compiler(${C_COMPILER} GNU)
+
+find_program(CXX_COMPILER arm-linux-gnueabihf-g++
+	PATHS ${RPI_TOOLCHAIN_DIR}/gcc-linaro-arm-linux-gnueabihf-raspbian/bin
+	NO_DEFAULT_PATH
+	)
+
+if(NOT CXX_COMPILER)
+	message(FATAL_ERROR "could not find arm-linux-gnueabihf-g++ compiler")
+endif()
+cmake_force_cxx_compiler(${CXX_COMPILER} GNU)
+
+# compiler tools
+foreach(tool objcopy nm ld)
+	string(TOUPPER ${tool} TOOL)
+	find_program(${TOOL} arm-linux-gnueabihf-${tool}
+		PATHS ${RPI_TOOLCHAIN_DIR}/gcc-linaro-arm-linux-gnueabihf-raspbian/bin
+		NO_DEFAULT_PATH
+		)
+	if(NOT ${TOOL})
+		message(FATAL_ERROR "could not find arm-linux-gnueabihf-${tool}")
+	endif()
+endforeach()
+
+# os tools
+foreach(tool echo patch grep rm mkdir nm genromfs cp touch make unzip)
+	string(TOUPPER ${tool} TOOL)
+	find_program(${TOOL} ${tool})
+	if(NOT ${TOOL})
+		message(FATAL_ERROR "could not find ${TOOL}")
+	endif()
+endforeach()
+
+set(LINKER_FLAGS "-Wl,-gc-sections")
+set(CMAKE_EXE_LINKER_FLAGS ${LINKER_FLAGS})
+set(CMAKE_C_FLAGS ${C_FLAGS})
+set(CMAKE_CXX_LINKER_FLAGS ${C_FLAGS})
+
+# where is the target environment
+set(CMAKE_FIND_ROOT_PATH  get_file_component(${C_COMPILER} PATH))
+
+# search for programs in the build host directories
+set(CMAKE_FIND_ROOT_PATH_MODE_PROGRAM NEVER)
+# for libraries and headers in the target directories
+set(CMAKE_FIND_ROOT_PATH_MODE_LIBRARY ONLY)
+set(CMAKE_FIND_ROOT_PATH_MODE_INCLUDE ONLY)
diff --git a/posix-configs/rpi2/init/rcS_navio b/posix-configs/rpi2/init/rcS_navio
deleted file mode 100644
index e69de29bb2..0000000000
diff --git a/posix-configs/rpi2/mainapp.config b/posix-configs/rpi2/mainapp.config
new file mode 100644
index 0000000000..28fbb2a67c
--- /dev/null
+++ b/posix-configs/rpi2/mainapp.config
@@ -0,0 +1,17 @@
+uorb start
+param set SYS_AUTOSTART 4001
+sleep 1
+param set MAV_TYPE 2
+df_mpu9250_wrapper start
+df_hmc5883_wrapper start
+sensors start
+commander start
+ekf2 start
+land_detector start multicopter
+mc_pos_control start
+mc_att_control start
+mavlink start -u 14556 -r 1000000
+sleep 1
+mavlink stream -u 14556 -s HIGHRES_IMU -r 50
+mavlink stream -u 14556 -s ATTITUDE -r 50
+mavlink boot_complete
diff --git a/src/firmware/posix/CMakeLists.txt b/src/firmware/posix/CMakeLists.txt
index 030ad0793c..ce2b469961 100644
--- a/src/firmware/posix/CMakeLists.txt
+++ b/src/firmware/posix/CMakeLists.txt
@@ -36,6 +36,30 @@ if ("${BOARD}" STREQUAL "eagle" OR ("${BOARD}" STREQUAL "excelsior"))
 				 ${CMAKE_SOURCE_DIR}/posix-configs/eagle/flight/mainapp.config
 			 DEPENDS mainapp
 			 DEST /home/linaro)
+			 
+elseif ("${BOARD}" STREQUAL "rpi2")
+
+	add_executable(mainapp
+		${CMAKE_SOURCE_DIR}/src/platforms/posix/main.cpp
+		apps.h
+		)
+		
+	target_link_libraries(mainapp
+		-Wl,--start-group
+		${module_libraries}
+		df_driver_framework
+		${df_driver_libs}
+		pthread m rt
+		-Wl,--end-group
+		)
+
+	px4_add_scp_push(OUT upload
+			 OS ${OS}
+			 BOARD ${BOARD}
+			 FILES ${CMAKE_CURRENT_BINARY_DIR}/mainapp
+				 ${CMAKE_SOURCE_DIR}/posix-configs/rpi2/mainapp.config
+			 DEPENDS mainapp
+			 DEST /home/pi)
 
 else()
 	add_executable(mainapp
diff --git a/src/platforms/posix/drivers/df_bmp280_wrapper/df_bmp280_wrapper.cpp b/src/platforms/posix/drivers/df_bmp280_wrapper/df_bmp280_wrapper.cpp
index 3f69985cf6..0383a3ca13 100644
--- a/src/platforms/posix/drivers/df_bmp280_wrapper/df_bmp280_wrapper.cpp
+++ b/src/platforms/posix/drivers/df_bmp280_wrapper/df_bmp280_wrapper.cpp
@@ -178,10 +178,10 @@ int DfBmp280Wrapper::_publish(struct baro_sensor_data &data)
 	const double R  = 287.05;	/* ideal gas constant in J/kg/K */
 
 	/* current pressure at MSL in kPa */
-	double p1 = MSL_PRESSURE / 1000.0;
+	double p1 = MSL_PRESSURE / 1000.0f;
 
 	/* measured pressure in kPa */
-	double p = data.pressure_pa / 1000.0;
+	double p = data.pressure_pa / 1000.0f;
 
 	/*
 	 * Solve:

From 1bd4dca266ec1ecb10f59dd5a6784d32c036defa Mon Sep 17 00:00:00 2001
From: Kabir Mohammed <mhkabir98@gmail.com>
Date: Fri, 29 Apr 2016 14:32:38 +0530
Subject: [PATCH 0238/1230] Fix RPI2 defines

---
 src/modules/sensors/sensors.cpp      | 10 +++++-----
 src/modules/sensors/sensors_init.cpp |  2 +-
 2 files changed, 6 insertions(+), 6 deletions(-)

diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp
index a555884db9..35db3614af 100644
--- a/src/modules/sensors/sensors.cpp
+++ b/src/modules/sensors/sensors.cpp
@@ -953,7 +953,7 @@ Sensors::parameters_update()
 	DevHandle h_baro;
 	DevMgr::getHandle(BARO0_DEVICE_PATH, h_baro);
 
-#if !defined(__PX4_QURT) && !defined(__RPI2)
+#if !defined(__PX4_QURT) && !defined(__PX4_POSIX_RPI2)
 
 	// TODO: this needs fixing for QURT and Raspberry Pi
 	if (!h_baro.isValid()) {
@@ -1521,7 +1521,7 @@ Sensors::parameter_update_poll(bool forced)
 bool
 Sensors::apply_gyro_calibration(DevHandle &h, const struct gyro_calibration_s *gcal, const int device_id)
 {
-#if !defined(__PX4_QURT) && !defined(__RPI2)
+#if !defined(__PX4_QURT) && !defined(__PX4_POSIX_RPI2)
 
 	/* On most systems, we can just use the IOCTL call to set the calibration params. */
 	const int res = h.ioctl(GYROIOCSSCALE, (long unsigned int)gcal);
@@ -1542,7 +1542,7 @@ Sensors::apply_gyro_calibration(DevHandle &h, const struct gyro_calibration_s *g
 bool
 Sensors::apply_accel_calibration(DevHandle &h, const struct accel_calibration_s *acal, const int device_id)
 {
-#if !defined(__PX4_QURT) && !defined(__RPI2)
+#if !defined(__PX4_QURT) && !defined(__PX4_POSIX_RPI2)
 
 	/* On most systems, we can just use the IOCTL call to set the calibration params. */
 	const int res = h.ioctl(ACCELIOCSSCALE, (long unsigned int)acal);
@@ -1563,7 +1563,7 @@ Sensors::apply_accel_calibration(DevHandle &h, const struct accel_calibration_s
 bool
 Sensors::apply_mag_calibration(DevHandle &h, const struct mag_calibration_s *mcal, const int device_id)
 {
-#if !defined(__PX4_QURT) && !defined(__RPI2)
+#if !defined(__PX4_QURT) && !defined(__PX4_POSIX_RPI2)
 
 	/* On most systems, we can just use the IOCTL call to set the calibration params. */
 	const int res = h.ioctl(MAGIOCSSCALE, (long unsigned int)mcal);
@@ -2065,7 +2065,7 @@ Sensors::task_main()
 	/* This calls a sensors_init which can have different implementations on NuttX, POSIX, QURT. */
 	ret = sensors_init();
 
-#if !defined(__PX4_QURT) && !defined(__RPI2)
+#if !defined(__PX4_QURT) && !defined(__PX4_POSIX_RPI2)
 	// TODO: move adc_init into the sensors_init call.
 	ret = ret || adc_init();
 #endif
diff --git a/src/modules/sensors/sensors_init.cpp b/src/modules/sensors/sensors_init.cpp
index e13142c754..447d569d50 100644
--- a/src/modules/sensors/sensors_init.cpp
+++ b/src/modules/sensors/sensors_init.cpp
@@ -52,7 +52,7 @@
 
 using namespace DriverFramework;
 
-#if defined(__PX4_QURT) || defined(__RPI2)
+#if defined(__PX4_QURT) || defined(__PX4_POSIX_RPI2)
 
 // Sensor initialization is performed automatically when the QuRT sensor drivers
 // are loaded.

From bcaa990220a4e87ed824d2ed84a5c9db73dc7e6f Mon Sep 17 00:00:00 2001
From: Kabir Mohammed <mhkabir98@gmail.com>
Date: Fri, 29 Apr 2016 17:50:28 +0530
Subject: [PATCH 0239/1230] Updated DF submodule

---
 src/lib/DriverFramework | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/src/lib/DriverFramework b/src/lib/DriverFramework
index 62449aef6d..28faa74015 160000
--- a/src/lib/DriverFramework
+++ b/src/lib/DriverFramework
@@ -1 +1 @@
-Subproject commit 62449aef6decd2faa75bc2abd509333df5a675c4
+Subproject commit 28faa740155c06287340b903cabd5947f0c59d39

From e185352f56facaca3b5be9384619f2e3f54bfec7 Mon Sep 17 00:00:00 2001
From: Kabir Mohammed <mhkabir98@gmail.com>
Date: Fri, 29 Apr 2016 21:03:31 +0530
Subject: [PATCH 0240/1230] Add RPI2 definitions for DF

---
 cmake/toolchains/Toolchain-arm-linux-gnueabihf-raspbian.cmake | 4 ++++
 1 file changed, 4 insertions(+)

diff --git a/cmake/toolchains/Toolchain-arm-linux-gnueabihf-raspbian.cmake b/cmake/toolchains/Toolchain-arm-linux-gnueabihf-raspbian.cmake
index bfd68043da..3b17c83558 100644
--- a/cmake/toolchains/Toolchain-arm-linux-gnueabihf-raspbian.cmake
+++ b/cmake/toolchains/Toolchain-arm-linux-gnueabihf-raspbian.cmake
@@ -71,6 +71,10 @@ foreach(tool echo patch grep rm mkdir nm genromfs cp touch make unzip)
 	endif()
 endforeach()
 
+add_definitions(
+	-D __RPI2
+	)
+
 set(LINKER_FLAGS "-Wl,-gc-sections")
 set(CMAKE_EXE_LINKER_FLAGS ${LINKER_FLAGS})
 set(CMAKE_C_FLAGS ${C_FLAGS})

From 9027ff437211dca836c01ff283e460f25579976a Mon Sep 17 00:00:00 2001
From: Michael Schaeuble <schaeuble.michael@gmail.com>
Date: Thu, 19 May 2016 14:22:54 +0200
Subject: [PATCH 0241/1230] DriverFramework: update submodule reference

---
 src/lib/DriverFramework | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/src/lib/DriverFramework b/src/lib/DriverFramework
index 28faa74015..62449aef6d 160000
--- a/src/lib/DriverFramework
+++ b/src/lib/DriverFramework
@@ -1 +1 @@
-Subproject commit 28faa740155c06287340b903cabd5947f0c59d39
+Subproject commit 62449aef6decd2faa75bc2abd509333df5a675c4

From c67907ffb22d02bee68252883d6e3f53f6d1157a Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Thu, 19 May 2016 15:44:37 +0200
Subject: [PATCH 0242/1230] BMP280 wrapper: Clean up floating point operations

---
 .../posix/drivers/df_bmp280_wrapper/df_bmp280_wrapper.cpp   | 6 +++---
 1 file changed, 3 insertions(+), 3 deletions(-)

diff --git a/src/platforms/posix/drivers/df_bmp280_wrapper/df_bmp280_wrapper.cpp b/src/platforms/posix/drivers/df_bmp280_wrapper/df_bmp280_wrapper.cpp
index 0383a3ca13..0d580b19f9 100644
--- a/src/platforms/posix/drivers/df_bmp280_wrapper/df_bmp280_wrapper.cpp
+++ b/src/platforms/posix/drivers/df_bmp280_wrapper/df_bmp280_wrapper.cpp
@@ -169,7 +169,7 @@ int DfBmp280Wrapper::_publish(struct baro_sensor_data &data)
 	// TODO: verify this, it's just copied from the MS5611 driver.
 
 	// Constant for now
-	const float MSL_PRESSURE = 101325.0f;
+	const double MSL_PRESSURE_KPA = 101325.0 / 1000.0;
 
 	/* tropospheric properties (0-11km) for standard atmosphere */
 	const double T1 = 15.0 + 273.15;	/* temperature at base height in Kelvin */
@@ -178,10 +178,10 @@ int DfBmp280Wrapper::_publish(struct baro_sensor_data &data)
 	const double R  = 287.05;	/* ideal gas constant in J/kg/K */
 
 	/* current pressure at MSL in kPa */
-	double p1 = MSL_PRESSURE / 1000.0f;
+	double p1 = MSL_PRESSURE_KPA;
 
 	/* measured pressure in kPa */
-	double p = data.pressure_pa / 1000.0f;
+	double p = static_cast<double>(data.pressure_pa) / 1000.0;
 
 	/*
 	 * Solve:

From 6eccfe3d14febbea68d52292fd8099fdf3a806dc Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Fri, 13 May 2016 14:53:47 +0200
Subject: [PATCH 0243/1230] MAVLink 2.0: Take a first stab at integration,
 enable heartbeat packets

---
 src/modules/mavlink/mavlink_bridge_header.h | 11 ++---
 src/modules/mavlink/mavlink_log_handler.h   |  2 +-
 src/modules/mavlink/mavlink_main.cpp        | 47 ++++++++++++++-------
 src/modules/mavlink/mavlink_main.h          |  2 +
 src/modules/mavlink/mavlink_messages.cpp    | 15 +++----
 5 files changed, 44 insertions(+), 33 deletions(-)

diff --git a/src/modules/mavlink/mavlink_bridge_header.h b/src/modules/mavlink/mavlink_bridge_header.h
index 0cd08769e1..97acfb908e 100644
--- a/src/modules/mavlink/mavlink_bridge_header.h
+++ b/src/modules/mavlink/mavlink_bridge_header.h
@@ -44,12 +44,7 @@
 
 __BEGIN_DECLS
 
-/*
- * We are NOT using convenience functions,
- * but instead send messages with a custom function.
- * So we do NOT do this:
- * #define MAVLINK_USE_CONVENIENCE_FUNCTIONS
- */
+#define MAVLINK_USE_CONVENIENCE_FUNCTIONS
 
 /* use efficient approach, see mavlink_helpers.h */
 #define MAVLINK_SEND_UART_BYTES mavlink_send_uart_bytes
@@ -57,7 +52,7 @@ __BEGIN_DECLS
 #define MAVLINK_GET_CHANNEL_BUFFER mavlink_get_channel_buffer
 #define MAVLINK_GET_CHANNEL_STATUS mavlink_get_channel_status
 
-#include <v1.0/mavlink_types.h>
+#include <v2.0/mavlink_types.h>
 #include <unistd.h>
 
 
@@ -84,7 +79,7 @@ void mavlink_send_uart_bytes(mavlink_channel_t chan, const uint8_t *ch, int leng
 extern mavlink_status_t *mavlink_get_channel_status(uint8_t chan);
 extern mavlink_message_t *mavlink_get_channel_buffer(uint8_t chan);
 
-#include <v1.0/common/mavlink.h>
+#include <v2.0/common/mavlink.h>
 
 __END_DECLS
 
diff --git a/src/modules/mavlink/mavlink_log_handler.h b/src/modules/mavlink/mavlink_log_handler.h
index ebbbe116a8..8742d8e2f4 100644
--- a/src/modules/mavlink/mavlink_log_handler.h
+++ b/src/modules/mavlink/mavlink_log_handler.h
@@ -40,7 +40,7 @@
 #include <queue.h>
 #include <time.h>
 #include <stdio.h>
-#include <v1.0/mavlink_types.h>
+#include <v2.0/mavlink_types.h>
 #include "mavlink_stream.h"
 
 class Mavlink;
diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp
index f35541c1e4..6d6da33a3a 100644
--- a/src/modules/mavlink/mavlink_main.cpp
+++ b/src/modules/mavlink/mavlink_main.cpp
@@ -106,8 +106,7 @@ static const int ERROR = -1;
 
 static Mavlink *_mavlink_instances = nullptr;
 
-static const uint8_t mavlink_message_lengths[256] = MAVLINK_MESSAGE_LENGTHS;
-static const uint8_t mavlink_message_crcs[256] = MAVLINK_MESSAGE_CRCS;
+static const mavlink_crc_entry_t mavlink_message_meta[] = MAVLINK_MESSAGE_CRCS;
 
 /**
  * mavlink app start / stop handling function
@@ -118,6 +117,17 @@ extern "C" __EXPORT int mavlink_main(int argc, char *argv[]);
 
 extern mavlink_system_t mavlink_system;
 
+void mavlink_send_uart_bytes(mavlink_channel_t chan, const uint8_t *ch, int length)
+{
+	Mavlink* m = Mavlink::get_instance((unsigned)chan);
+
+	if (m->get_free_tx_buf() < length) {
+		return;
+	}
+
+	m->send_bytes(ch, length);
+}
+
 static void usage(void);
 
 bool Mavlink::_boot_complete = false;
@@ -816,15 +826,7 @@ Mavlink::get_free_tx_buf()
 void
 Mavlink::send_message(const uint8_t msgid, const void *msg, uint8_t component_ID)
 {
-	/* If the wait until transmit flag is on, only transmit after we've received messages.
-	   Otherwise, transmit all the time. */
-	if (!should_transmit()) {
-		return;
-	}
-
-	pthread_mutex_lock(&_send_mutex);
-
-	uint8_t payload_len = mavlink_message_lengths[msgid];
+	uint8_t payload_len = mavlink_message_meta[msgid].msg_len;
 	unsigned packet_len = payload_len + MAVLINK_NUM_NON_PAYLOAD_BYTES;
 
 	_last_write_try_time = hrt_absolute_time();
@@ -845,6 +847,8 @@ Mavlink::send_message(const uint8_t msgid, const void *msg, uint8_t component_ID
 		}
 	}
 
+	mavlink_get_channel_status(_channel)->flags |= MAVLINK_STATUS_FLAG_OUT_MAVLINK1;
+
 	uint8_t buf[MAVLINK_MAX_PACKET_LEN];
 
 	/* header */
@@ -863,11 +867,25 @@ Mavlink::send_message(const uint8_t msgid, const void *msg, uint8_t component_ID
 	uint16_t checksum;
 	crc_init(&checksum);
 	crc_accumulate_buffer(&checksum, (const char *) &buf[1], MAVLINK_CORE_HEADER_LEN + payload_len);
-	crc_accumulate(mavlink_message_crcs[msgid], &checksum);
+	crc_accumulate(mavlink_message_meta[msgid].crc_extra, &checksum);
 
 	buf[MAVLINK_NUM_HEADER_BYTES + payload_len] = (uint8_t)(checksum & 0xFF);
 	buf[MAVLINK_NUM_HEADER_BYTES + payload_len + 1] = (uint8_t)(checksum >> 8);
 
+	send_bytes(&buf[0], packet_len);
+}
+
+void
+Mavlink::send_bytes(const uint8_t *buf, unsigned packet_len)
+{
+	/* If the wait until transmit flag is on, only transmit after we've received messages.
+	   Otherwise, transmit all the time. */
+	if (!should_transmit()) {
+		return;
+	}
+
+	pthread_mutex_lock(&_send_mutex);
+
 	size_t ret = -1;
 
 	/* send message to UART */
@@ -881,11 +899,10 @@ Mavlink::send_message(const uint8_t msgid, const void *msg, uint8_t component_ID
 
 		struct telemetry_status_s &tstatus = get_rx_status();
 
-		/* resend heartbeat via broadcast */
+		/* resend message via broadcast if no valid connection exists */
 		if ((_mode != MAVLINK_MODE_ONBOARD) &&
 			(!get_client_source_initialized()
-			|| (hrt_elapsed_time(&tstatus.heartbeat_time) > 3 * 1000 * 1000))
-			&& (msgid == MAVLINK_MSG_ID_HEARTBEAT)) {
+			|| (hrt_elapsed_time(&tstatus.heartbeat_time) > 3 * 1000 * 1000))) {
 
 			if (!_broadcast_address_found) {
 				find_broadcast_address();
diff --git a/src/modules/mavlink/mavlink_main.h b/src/modules/mavlink/mavlink_main.h
index 9372c53f0f..90110d0f54 100644
--- a/src/modules/mavlink/mavlink_main.h
+++ b/src/modules/mavlink/mavlink_main.h
@@ -226,6 +226,8 @@ public:
 
 	void			send_message(const uint8_t msgid, const void *msg, uint8_t component_ID = 0);
 
+	void			send_bytes(const uint8_t *buf, unsigned packet_len);
+
 	/**
 	 * Resend message as is, don't change sequence number and CRC.
 	 */
diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp
index e88188a8f3..5c2677bc46 100644
--- a/src/modules/mavlink/mavlink_messages.cpp
+++ b/src/modules/mavlink/mavlink_messages.cpp
@@ -333,16 +333,13 @@ protected:
 			memset(&pos_sp_triplet, 0, sizeof(pos_sp_triplet));
 		}
 
-		mavlink_heartbeat_t msg;
+		uint8_t base_mode = 0;
+		uint32_t custom_mode = 0;
+		uint8_t system_status = 0;
+		get_mavlink_mode_state(&status, &pos_sp_triplet, &system_status, &base_mode, &custom_mode);
 
-		msg.base_mode = 0;
-		msg.custom_mode = 0;
-		get_mavlink_mode_state(&status, &pos_sp_triplet, &msg.system_status, &msg.base_mode, &msg.custom_mode);
-		msg.type = _mavlink->get_system_type();
-		msg.autopilot = MAV_AUTOPILOT_PX4;
-		msg.mavlink_version = 3;
-
-		_mavlink->send_message(MAVLINK_MSG_ID_HEARTBEAT, &msg);
+		mavlink_msg_heartbeat_send(_mavlink->get_channel(), _mavlink->get_system_type(), MAV_AUTOPILOT_PX4,
+			base_mode, custom_mode, system_status);
 	}
 };
 

From c6aa1b1efb18578881e78c588c23d0bd71456533 Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Sat, 14 May 2016 14:16:58 +0200
Subject: [PATCH 0244/1230] Update MAVLink version

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

diff --git a/mavlink/include/mavlink/v2.0 b/mavlink/include/mavlink/v2.0
index eec77a089b..a62bc59cd1 160000
--- a/mavlink/include/mavlink/v2.0
+++ b/mavlink/include/mavlink/v2.0
@@ -1 +1 @@
-Subproject commit eec77a089b75216ee28c157dca2993c4d12209fd
+Subproject commit a62bc59cd1b333ffc373cc2ee9f91c80e8322ca8

From 0a597d0d62080a2c8418ba4f41b053b52833d5ba Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Sat, 14 May 2016 14:17:30 +0200
Subject: [PATCH 0245/1230] MAVLink: Update to version 2 compaat API

---
 src/modules/mavlink/mavlink_bridge_header.h |   4 +
 src/modules/mavlink/mavlink_ftp.cpp         |   4 +-
 src/modules/mavlink/mavlink_log_handler.cpp |   6 +-
 src/modules/mavlink/mavlink_main.cpp        | 144 ++++++++++----------
 src/modules/mavlink/mavlink_main.h          |  15 +-
 src/modules/mavlink/mavlink_messages.cpp    |  76 +++++------
 src/modules/mavlink/mavlink_mission.cpp     |  14 +-
 src/modules/mavlink/mavlink_parameters.cpp  |   8 +-
 src/modules/mavlink/mavlink_receiver.cpp    |   6 +-
 9 files changed, 144 insertions(+), 133 deletions(-)

diff --git a/src/modules/mavlink/mavlink_bridge_header.h b/src/modules/mavlink/mavlink_bridge_header.h
index 97acfb908e..799df3da36 100644
--- a/src/modules/mavlink/mavlink_bridge_header.h
+++ b/src/modules/mavlink/mavlink_bridge_header.h
@@ -49,6 +49,8 @@ __BEGIN_DECLS
 /* use efficient approach, see mavlink_helpers.h */
 #define MAVLINK_SEND_UART_BYTES mavlink_send_uart_bytes
 
+#define MAVLINK_END_UART_SEND mavlink_end_uart_send
+
 #define MAVLINK_GET_CHANNEL_BUFFER mavlink_get_channel_buffer
 #define MAVLINK_GET_CHANNEL_STATUS mavlink_get_channel_status
 
@@ -76,6 +78,8 @@ extern mavlink_system_t mavlink_system;
  */
 void mavlink_send_uart_bytes(mavlink_channel_t chan, const uint8_t *ch, int length);
 
+void mavlink_end_uart_send(mavlink_channel_t chan, int length);
+
 extern mavlink_status_t *mavlink_get_channel_status(uint8_t chan);
 extern mavlink_message_t *mavlink_get_channel_buffer(uint8_t chan);
 
diff --git a/src/modules/mavlink/mavlink_ftp.cpp b/src/modules/mavlink/mavlink_ftp.cpp
index e8ea9c89b9..674fe66c83 100644
--- a/src/modules/mavlink/mavlink_ftp.cpp
+++ b/src/modules/mavlink/mavlink_ftp.cpp
@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2014, 2015 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2014-2016 PX4 Development Team. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -292,7 +292,7 @@ MavlinkFTP::_reply(mavlink_file_transfer_protocol_t* ftp_req)
 	// Unit test hook is set, call that instead
 	_utRcvMsgFunc(ftp_req, _worker_data);
 #else
-	_mavlink->send_message(MAVLINK_MSG_ID_FILE_TRANSFER_PROTOCOL, ftp_req);
+	mavlink_msg_file_transfer_protocol_send_struct(_mavlink->get_channel(), ftp_req);
 #endif
 
 }
diff --git a/src/modules/mavlink/mavlink_log_handler.cpp b/src/modules/mavlink/mavlink_log_handler.cpp
index 08197f51fe..a9dddb55ff 100644
--- a/src/modules/mavlink/mavlink_log_handler.cpp
+++ b/src/modules/mavlink/mavlink_log_handler.cpp
@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2014, 2015 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2014-2016 PX4 Development Team. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -257,7 +257,7 @@ MavlinkLogHandler::_log_send_listing()
 	response.id 		= _pLogHandlerHelper->next_entry;
 	response.num_logs	= _pLogHandlerHelper->log_count;
 	response.last_log_num	= _pLogHandlerHelper->last_entry;
-	_mavlink->send_message(MAVLINK_MSG_ID_LOG_ENTRY, &response);
+	mavlink_msg_log_entry_send_struct(_mavlink->get_channel(), &response);
         //-- If we're done listing, flag it.
         if (_pLogHandlerHelper->next_entry == _pLogHandlerHelper->last_entry) {
 		_pLogHandlerHelper->current_status = LogListHelper::LOG_HANDLER_IDLE;
@@ -289,7 +289,7 @@ MavlinkLogHandler::_log_send_data()
 	response.ofs   = _pLogHandlerHelper->current_log_data_offset;
 	response.id    = _pLogHandlerHelper->current_log_index;
 	response.count = read_size;
-	_mavlink->send_message(MAVLINK_MSG_ID_LOG_DATA, &response);
+	mavlink_msg_log_data_send_struct(_mavlink->get_channel(), &response);
 	_pLogHandlerHelper->current_log_data_offset    += read_size;
 	_pLogHandlerHelper->current_log_data_remaining -= read_size;
 	if (read_size < sizeof(response.data) || _pLogHandlerHelper->current_log_data_remaining == 0) {
diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp
index 6d6da33a3a..7b1530e2f9 100644
--- a/src/modules/mavlink/mavlink_main.cpp
+++ b/src/modules/mavlink/mavlink_main.cpp
@@ -120,12 +120,17 @@ extern mavlink_system_t mavlink_system;
 void mavlink_send_uart_bytes(mavlink_channel_t chan, const uint8_t *ch, int length)
 {
 	Mavlink* m = Mavlink::get_instance((unsigned)chan);
-
-	if (m->get_free_tx_buf() < length) {
-		return;
+	if (m != nullptr) {
+		m->send_bytes(ch, length);
 	}
+}
 
-	m->send_bytes(ch, length);
+void mavlink_end_uart_send(mavlink_channel_t chan, int length)
+{
+	Mavlink* m = Mavlink::get_instance((unsigned)chan);
+	if (m != nullptr) {
+		m->send_packet();
+	}
 }
 
 static void usage(void);
@@ -191,6 +196,8 @@ Mavlink::Mavlink() :
 	_bcast_addr{},
 	_src_addr_initialized(false),
 	_broadcast_address_found(false),
+	_network_buf{},
+	_network_buf_len(0),
 #endif
 	_socket_fd(-1),
 	_protocol(SERIAL),
@@ -824,10 +831,61 @@ Mavlink::get_free_tx_buf()
 }
 
 void
-Mavlink::send_message(const uint8_t msgid, const void *msg, uint8_t component_ID)
+Mavlink::send_packet()
 {
-	uint8_t payload_len = mavlink_message_meta[msgid].msg_len;
-	unsigned packet_len = payload_len + MAVLINK_NUM_NON_PAYLOAD_BYTES;
+#ifdef __PX4_POSIX
+
+	int ret = -1;
+
+	if (get_protocol() == UDP) {
+		ret = sendto(_socket_fd, _network_buf, _network_buf_len, 0, (struct sockaddr *)&_src_addr, sizeof(_src_addr));
+
+		struct telemetry_status_s &tstatus = get_rx_status();
+		warnx("UDP count: %d", ret);
+
+		/* resend message via broadcast if no valid connection exists */
+		if ((_mode != MAVLINK_MODE_ONBOARD) &&
+			(!get_client_source_initialized()
+			|| (hrt_elapsed_time(&tstatus.heartbeat_time) > 3 * 1000 * 1000))) {
+
+			if (!_broadcast_address_found) {
+				find_broadcast_address();
+			}
+
+			if (_broadcast_address_found) {
+
+				int bret = sendto(_socket_fd, _network_buf, _network_buf_len, 0, (struct sockaddr *)&_bcast_addr, sizeof(_bcast_addr));
+
+				if (bret <= 0) {
+					PX4_WARN("sending broadcast failed, errno: %d: %s", errno, strerror(errno));
+				} else {
+					warnx("BROADCAST count: %d", bret);
+				}
+			}
+		}
+
+	} else if (get_protocol() == TCP) {
+		/* not implemented, but possible to do so */
+		warnx("TCP transport pending implementation");
+	}
+
+	_network_buf_len = 0;
+
+#endif
+}
+
+void
+Mavlink::send_bytes(const uint8_t *buf, unsigned packet_len)
+{
+	warnx("send bytes");
+	/* If the wait until transmit flag is on, only transmit after we've received messages.
+	   Otherwise, transmit all the time. */
+	if (!should_transmit()) {
+		warnx("not transmitting");
+		return;
+	}
+
+	pthread_mutex_lock(&_send_mutex);
 
 	_last_write_try_time = hrt_absolute_time();
 
@@ -847,45 +905,6 @@ Mavlink::send_message(const uint8_t msgid, const void *msg, uint8_t component_ID
 		}
 	}
 
-	mavlink_get_channel_status(_channel)->flags |= MAVLINK_STATUS_FLAG_OUT_MAVLINK1;
-
-	uint8_t buf[MAVLINK_MAX_PACKET_LEN];
-
-	/* header */
-	buf[0] = MAVLINK_STX;
-	buf[1] = payload_len;
-	/* use mavlink's internal counter for the TX seq */
-	buf[2] = mavlink_get_channel_status(_channel)->current_tx_seq++;
-	buf[3] = mavlink_system.sysid;
-	buf[4] = (component_ID == 0) ? mavlink_system.compid : component_ID;
-	buf[5] = msgid;
-
-	/* payload */
-	memcpy(&buf[MAVLINK_NUM_HEADER_BYTES], msg, payload_len);
-
-	/* checksum */
-	uint16_t checksum;
-	crc_init(&checksum);
-	crc_accumulate_buffer(&checksum, (const char *) &buf[1], MAVLINK_CORE_HEADER_LEN + payload_len);
-	crc_accumulate(mavlink_message_meta[msgid].crc_extra, &checksum);
-
-	buf[MAVLINK_NUM_HEADER_BYTES + payload_len] = (uint8_t)(checksum & 0xFF);
-	buf[MAVLINK_NUM_HEADER_BYTES + payload_len + 1] = (uint8_t)(checksum >> 8);
-
-	send_bytes(&buf[0], packet_len);
-}
-
-void
-Mavlink::send_bytes(const uint8_t *buf, unsigned packet_len)
-{
-	/* If the wait until transmit flag is on, only transmit after we've received messages.
-	   Otherwise, transmit all the time. */
-	if (!should_transmit()) {
-		return;
-	}
-
-	pthread_mutex_lock(&_send_mutex);
-
 	size_t ret = -1;
 
 	/* send message to UART */
@@ -894,33 +913,14 @@ Mavlink::send_bytes(const uint8_t *buf, unsigned packet_len)
 	}
 
 #ifdef __PX4_POSIX
-	if (get_protocol() == UDP) {
-		ret = sendto(_socket_fd, buf, packet_len, 0, (struct sockaddr *)&_src_addr, sizeof(_src_addr));
 
-		struct telemetry_status_s &tstatus = get_rx_status();
+	else {
+		if (_network_buf_len + packet_len < sizeof(_network_buf) / sizeof(_network_buf[0])) {
+			memcpy(&_network_buf[_network_buf_len], buf, packet_len);
+			_network_buf_len += packet_len;
 
-		/* resend message via broadcast if no valid connection exists */
-		if ((_mode != MAVLINK_MODE_ONBOARD) &&
-			(!get_client_source_initialized()
-			|| (hrt_elapsed_time(&tstatus.heartbeat_time) > 3 * 1000 * 1000))) {
-
-			if (!_broadcast_address_found) {
-				find_broadcast_address();
-			}
-
-			if (_broadcast_address_found) {
-
-				int bret = sendto(_socket_fd, buf, packet_len, 0, (struct sockaddr *)&_bcast_addr, sizeof(_bcast_addr));
-
-				if (bret <= 0) {
-					PX4_WARN("sending broadcast failed, errno: %d: %s", errno, strerror(errno));
-				}
-			}
+			ret = packet_len;
 		}
-
-	} else if (get_protocol() == TCP) {
-		/* not implemented, but possible to do so */
-		warnx("TCP transport pending implementation");
 	}
 #endif
 
@@ -1231,7 +1231,7 @@ void Mavlink::send_autopilot_capabilites()
 		mcu_unique_id(uid);
 		msg.uid = (((uint64_t)uid[1]) << 32) | uid[2];
 
-		this->send_message(MAVLINK_MSG_ID_AUTOPILOT_VERSION, &msg);
+		mavlink_msg_autopilot_version_send_struct(get_channel(), &msg);
 	}
 }
 
@@ -2044,7 +2044,7 @@ Mavlink::task_main(int argc, char *argv[])
 			msg.result = command_ack.result;
 			msg.command = command_ack.command;
 
-			send_message(MAVLINK_MSG_ID_COMMAND_ACK, &msg);
+			mavlink_msg_command_ack_send_struct(get_channel(), &msg);
 		}
 
 		struct mavlink_log_s mavlink_log;
diff --git a/src/modules/mavlink/mavlink_main.h b/src/modules/mavlink/mavlink_main.h
index 90110d0f54..b0b682d5cb 100644
--- a/src/modules/mavlink/mavlink_main.h
+++ b/src/modules/mavlink/mavlink_main.h
@@ -224,10 +224,18 @@ public:
 	 */
 	bool			get_manual_input_mode_generation() { return _generate_rc; }
 
-	void			send_message(const uint8_t msgid, const void *msg, uint8_t component_ID = 0);
-
+	/**
+	 * Send bytes out on the link.
+	 *
+	 * On a network port these might actually get buffered to form a packet.
+	 */
 	void			send_bytes(const uint8_t *buf, unsigned packet_len);
 
+	/**
+	 * Flush the transmit buffer and send one MAVLink packet
+	 */
+	void			send_packet();
+
 	/**
 	 * Resend message as is, don't change sequence number and CRC.
 	 */
@@ -451,7 +459,8 @@ private:
 	struct sockaddr_in _bcast_addr;
 	bool _src_addr_initialized;
 	bool _broadcast_address_found;
-
+	uint8_t _network_buf[MAVLINK_MAX_PACKET_LEN];
+	unsigned _network_buf_len;
 #endif
 	int _socket_fd;
 	Protocol	_protocol;
diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp
index 5c2677bc46..5bafb057ce 100644
--- a/src/modules/mavlink/mavlink_messages.cpp
+++ b/src/modules/mavlink/mavlink_messages.cpp
@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2012-2015 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2012-2016 PX4 Development Team. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -409,7 +409,7 @@ protected:
 				strncpy(msg.text, (const char *)mavlink_log.text, sizeof(msg.text));
 				msg.text[sizeof(msg.text) - 1] = '\0';
 
-				_mavlink->send_message(MAVLINK_MSG_ID_STATUSTEXT, &msg);
+				mavlink_msg_statustext_send_struct(_mavlink->get_channel(), &msg);
 
 // TODO: the logging doesn't work on Snapdragon yet because of file paths.
 #ifndef __PX4_POSIX_EAGLE
@@ -537,7 +537,7 @@ protected:
 				msg.param6 = cmd.param6;
 				msg.param7 = cmd.param7;
 
-				_mavlink->send_message(MAVLINK_MSG_ID_COMMAND_LONG, &msg);
+				mavlink_msg_command_long_send_struct(_mavlink->get_channel(), &msg);
 			}
 		}
 	}
@@ -623,7 +623,7 @@ protected:
 			msg.errors_count3 = 0;
 			msg.errors_count4 = 0;
 
-			_mavlink->send_message(MAVLINK_MSG_ID_SYS_STATUS, &msg);
+			mavlink_msg_sys_status_send_struct(_mavlink->get_channel(), &msg);
 
 			/* battery status message with higher resolution */
 			mavlink_battery_status_t bat_msg = {};
@@ -643,7 +643,7 @@ protected:
 				}
 			}
 
-			_mavlink->send_message(MAVLINK_MSG_ID_BATTERY_STATUS, &bat_msg);
+			mavlink_msg_battery_status_send_struct(_mavlink->get_channel(), &bat_msg);
 		}
 	}
 };
@@ -749,7 +749,7 @@ protected:
 			msg.temperature = sensor.baro_temp_celcius[0];
 			msg.fields_updated = fields_updated;
 
-			_mavlink->send_message(MAVLINK_MSG_ID_HIGHRES_IMU, &msg);
+			mavlink_msg_highres_imu_send_struct(_mavlink->get_channel(), &msg);
 		}
 	}
 };
@@ -813,7 +813,7 @@ protected:
 			msg.pitchspeed = att.pitchspeed;
 			msg.yawspeed = att.yawspeed;
 
-			_mavlink->send_message(MAVLINK_MSG_ID_ATTITUDE, &msg);
+			mavlink_msg_attitude_send_struct(_mavlink->get_channel(), &msg);
 		}
 	}
 };
@@ -877,7 +877,7 @@ protected:
 			msg.pitchspeed = att.pitchspeed;
 			msg.yawspeed = att.yawspeed;
 
-			_mavlink->send_message(MAVLINK_MSG_ID_ATTITUDE_QUATERNION, &msg);
+			mavlink_msg_attitude_quaternion_send_struct(_mavlink->get_channel(), &msg);
 		}
 	}
 };
@@ -983,7 +983,7 @@ protected:
 			}
 			msg.climb = -pos.vel_d;
 
-			_mavlink->send_message(MAVLINK_MSG_ID_VFR_HUD, &msg);
+			mavlink_msg_vfr_hud_send_struct(_mavlink->get_channel(), &msg);
 		}
 	}
 };
@@ -1049,7 +1049,7 @@ protected:
 			msg.cog = _wrap_2pi(gps.cog_rad) * M_RAD_TO_DEG_F * 1e2f,
 			msg.satellites_visible = gps.satellites_used;
 
-			_mavlink->send_message(MAVLINK_MSG_ID_GPS_RAW_INT, &msg);
+			mavlink_msg_gps_raw_int_send_struct(_mavlink->get_channel(), &msg);
 		}
 	}
 };
@@ -1095,7 +1095,7 @@ protected:
 		msg.time_boot_ms = hrt_absolute_time() / 1000;
 		msg.time_unix_usec = (uint64_t)tv.tv_sec * 1000000 + tv.tv_nsec / 1000;
 
-		_mavlink->send_message(MAVLINK_MSG_ID_SYSTEM_TIME, &msg);
+		mavlink_msg_system_time_send_struct(_mavlink->get_channel(), &msg);
 	}
 };
 
@@ -1137,7 +1137,7 @@ protected:
 		msg.tc1 = 0;
 		msg.ts1 = hrt_absolute_time() * 1000; // boot time in nanoseconds
 
-		_mavlink->send_message(MAVLINK_MSG_ID_TIMESYNC, &msg);
+		mavlink_msg_timesync_send_struct(_mavlink->get_channel(), &msg);
 	}
 };
 
@@ -1204,7 +1204,7 @@ protected:
 			msg.flags = pos.flags;
 			msg.squawk = pos.squawk;
 
-			_mavlink->send_message(MAVLINK_MSG_ID_ADSB_VEHICLE, &msg);
+			mavlink_msg_adsb_vehicle_send_struct(_mavlink->get_channel(), &msg);
 		}
 	}
 };
@@ -1263,7 +1263,7 @@ protected:
 
 			/* ensure that only active trigger events are sent */
 			if (trigger.timestamp > 0) {
-				_mavlink->send_message(MAVLINK_MSG_ID_CAMERA_TRIGGER, &msg);
+				mavlink_msg_camera_trigger_send_struct(_mavlink->get_channel(), &msg);
 			}
 		}
 	}
@@ -1337,7 +1337,7 @@ protected:
 			msg.vz = pos.vel_d * 100.0f;
 			msg.hdg = _wrap_2pi(pos.yaw) * M_RAD_TO_DEG_F * 100.0f;
 
-			_mavlink->send_message(MAVLINK_MSG_ID_GLOBAL_POSITION_INT, &msg);
+			mavlink_msg_global_position_int_send_struct(_mavlink->get_channel(), &msg);
 		}
 	}
 };
@@ -1401,7 +1401,7 @@ protected:
             vmsg.pitch = rpy(1);
             vmsg.yaw = rpy(2);
 
-            _mavlink->send_message(MAVLINK_MSG_ID_VISION_POSITION_ESTIMATE, &vmsg);
+            mavlink_msg_vision_position_estimate_send_struct(_mavlink->get_channel(), &vmsg);
         }
     }
 };
@@ -1463,7 +1463,7 @@ protected:
 			msg.vy = pos.vy;
 			msg.vz = pos.vz;
 
-			_mavlink->send_message(MAVLINK_MSG_ID_LOCAL_POSITION_NED, &msg);
+			mavlink_msg_local_position_ned_send_struct(_mavlink->get_channel(), &msg);
 		}
 	}
 };
@@ -1536,7 +1536,7 @@ protected:
 			msg.covariance[11] = est.timeout_flags;
 			memcpy(msg.covariance, est.covariances, sizeof(est.covariances));
 
-			_mavlink->send_message(MAVLINK_MSG_ID_LOCAL_POSITION_NED_COV, &msg);
+			mavlink_msg_local_position_ned_cov_send_struct(_mavlink->get_channel(), &msg);
 		}
 	}
 };
@@ -1591,7 +1591,7 @@ protected:
 
 			// To be added and filled
 			// mavlink_estimator_status_t msg = {};
-			//_mavlink->send_message(MAVLINK_MSG_ID_ESTIMATOR_STATUS, &msg);
+			//mavlink_msg_statustext_send_struct(_mavlink->get_channel(), &msg);
 
 			mavlink_vibration_t msg = {};
 
@@ -1599,7 +1599,7 @@ protected:
 			msg.vibration_y = est.vibe[1];
 			msg.vibration_z = est.vibe[2];
 
-			_mavlink->send_message(MAVLINK_MSG_ID_VIBRATION, &msg);
+			mavlink_msg_vibration_send_struct(_mavlink->get_channel(), &msg);
 		}
 	}
 };
@@ -1662,7 +1662,7 @@ protected:
 			msg.y = mocap.y;
 			msg.z = mocap.z;
 
-			_mavlink->send_message(MAVLINK_MSG_ID_ATT_POS_MOCAP, &msg);
+			mavlink_msg_att_pos_mocap_send_struct(_mavlink->get_channel(), &msg);
 		}
 	}
 };
@@ -1735,7 +1735,7 @@ protected:
 				msg.approach_y = 0.0f;
 				msg.approach_z = 0.0f;
 
-				_mavlink->send_message(MAVLINK_MSG_ID_HOME_POSITION, &msg);
+				mavlink_msg_home_position_send_struct(_mavlink->get_channel(), &msg);
 			}
 		}
 	}
@@ -1817,7 +1817,7 @@ protected:
 			msg.servo7_raw = act.output[6];
 			msg.servo8_raw = act.output[7];
 
-			_mavlink->send_message(MAVLINK_MSG_ID_SERVO_OUTPUT_RAW, &msg);
+			mavlink_msg_servo_output_raw_send_struct(_mavlink->get_channel(), &msg);
 		}
 	}
 };
@@ -1910,7 +1910,7 @@ protected:
 				msg.controls[i] = att_ctrl.control[i];
 			}
 
-			_mavlink->send_message(MAVLINK_MSG_ID_ACTUATOR_CONTROL_TARGET, &msg);
+			mavlink_msg_actuator_control_target_send_struct(_mavlink->get_channel(), &msg);
 		}
 	}
 };
@@ -2076,7 +2076,7 @@ protected:
 			msg.mode = mavlink_base_mode;
 			msg.nav_mode = 0;
 
-			_mavlink->send_message(MAVLINK_MSG_ID_HIL_CONTROLS, &msg);
+			mavlink_msg_hil_controls_send_struct(_mavlink->get_channel(), &msg);
 		}
 	}
 };
@@ -2135,7 +2135,7 @@ protected:
 			msg.lon_int = pos_sp_triplet.current.lon * 1e7;
 			msg.alt = pos_sp_triplet.current.alt;
 
-			_mavlink->send_message(MAVLINK_MSG_ID_POSITION_TARGET_GLOBAL_INT, &msg);
+			mavlink_msg_position_target_global_int_send_struct(_mavlink->get_channel(), &msg);
 		}
 	}
 };
@@ -2203,7 +2203,7 @@ protected:
 			msg.afy = pos_sp.acc_y;
 			msg.afz = pos_sp.acc_z;
 
-			_mavlink->send_message(MAVLINK_MSG_ID_POSITION_TARGET_LOCAL_NED, &msg);
+			mavlink_msg_position_target_local_ned_send_struct(_mavlink->get_channel(), &msg);
 		}
 	}
 };
@@ -2275,7 +2275,7 @@ protected:
 
 			msg.thrust = att_sp.thrust;
 
-			_mavlink->send_message(MAVLINK_MSG_ID_ATTITUDE_TARGET, &msg);
+			mavlink_msg_attitude_target_send_struct(_mavlink->get_channel(), &msg);
 		}
 	}
 };
@@ -2355,7 +2355,7 @@ protected:
 
 			msg.rssi = (rc.channel_count > 0) ? rc.rssi : 0;
 
-			_mavlink->send_message(MAVLINK_MSG_ID_RC_CHANNELS, &msg);
+			mavlink_msg_rc_channels_send_struct(_mavlink->get_channel(), &msg);
 
 			/* send override message - harmless if connected to GCS, allows to connect a board to a Linux system */
 			/* http://mavlink.org/messages/common#RC_CHANNELS_OVERRIDE */
@@ -2371,7 +2371,7 @@ protected:
 			over.chan7_raw = msg.chan7_raw;
 			over.chan8_raw = msg.chan8_raw;
 
-			_mavlink->send_message(MAVLINK_MSG_ID_RC_CHANNELS_OVERRIDE, &over);
+			mavlink_msg_rc_channels_override_send_struct(_mavlink->get_channel(), &over);
 		}
 	}
 };
@@ -2440,7 +2440,7 @@ protected:
 			msg.buttons |= (manual.acro_switch << (shift * 4));
 			msg.buttons |= (manual.offboard_switch << (shift * 5));
 
-			_mavlink->send_message(MAVLINK_MSG_ID_MANUAL_CONTROL, &msg);
+			mavlink_msg_manual_control_send_struct(_mavlink->get_channel(), &msg);
 		}
 	}
 };
@@ -2508,7 +2508,7 @@ protected:
 			msg.time_delta_distance_us = flow.time_since_last_sonar_update;
 			msg.temperature = flow.gyro_temperature;
 
-			_mavlink->send_message(MAVLINK_MSG_ID_OPTICAL_FLOW_RAD, &msg);
+			mavlink_msg_optical_flow_rad_send_struct(_mavlink->get_channel(), &msg);
 		}
 	}
 };
@@ -2568,7 +2568,7 @@ protected:
 			msg.name[sizeof(msg.name) - 1] = '\0';
 			msg.value = debug.value;
 
-			_mavlink->send_message(MAVLINK_MSG_ID_NAMED_VALUE_FLOAT, &msg);
+			mavlink_msg_named_value_float_send_struct(_mavlink->get_channel(), &msg);
 		}
 	}
 };
@@ -2635,7 +2635,7 @@ protected:
 			msg.alt_error = _tecs_status.altitude_filtered - _tecs_status.altitudeSp;
 			msg.aspd_error = _tecs_status.airspeed_filtered - _tecs_status.airspeedSp;
 
-			_mavlink->send_message(MAVLINK_MSG_ID_NAV_CONTROLLER_OUTPUT, &msg);
+			mavlink_msg_nav_controller_output_send_struct(_mavlink->get_channel(), &msg);
 		}
 	}
 };
@@ -2700,7 +2700,7 @@ protected:
 		msg.param6 = 0;
 		msg.param7 = 0;
 
-		_mavlink->send_message(MAVLINK_MSG_ID_COMMAND_LONG, &msg);
+		mavlink_msg_command_long_send_struct(_mavlink->get_channel(), &msg);
 	}
 };
 
@@ -2783,7 +2783,7 @@ protected:
 			msg.current_distance = dist_sensor.current_distance * 100.0f; /* m to cm */
 			msg.covariance = dist_sensor.covariance;
 
-			_mavlink->send_message(MAVLINK_MSG_ID_DISTANCE_SENSOR, &msg);
+			mavlink_msg_distance_sensor_send_struct(_mavlink->get_channel(), &msg);
 		}
 	}
 };
@@ -2877,7 +2877,7 @@ protected:
 		}
 
 		if (updated) {
-			_mavlink->send_message(MAVLINK_MSG_ID_EXTENDED_SYS_STATE, &_msg);
+			mavlink_msg_extended_sys_state_send_struct(_mavlink->get_channel(), &_msg);
 		}
 	}
 };
@@ -2971,7 +2971,7 @@ protected:
 				msg.bottom_clearance = NAN;
 			}
 
-			_mavlink->send_message(MAVLINK_MSG_ID_ALTITUDE, &msg);
+			mavlink_msg_altitude_send_struct(_mavlink->get_channel(), &msg);
 		}
 	}
 };
diff --git a/src/modules/mavlink/mavlink_mission.cpp b/src/modules/mavlink/mavlink_mission.cpp
index 929e67a0c1..05c62e02e3 100644
--- a/src/modules/mavlink/mavlink_mission.cpp
+++ b/src/modules/mavlink/mavlink_mission.cpp
@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2012-2015 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2012-2016 PX4 Development Team. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -188,7 +188,7 @@ MavlinkMissionManager::send_mission_ack(uint8_t sysid, uint8_t compid, uint8_t t
 	wpa.target_component = compid;
 	wpa.type = type;
 
-	_mavlink->send_message(MAVLINK_MSG_ID_MISSION_ACK, &wpa);
+	mavlink_msg_mission_ack_send_struct(_mavlink->get_channel(), &wpa);
 
 	if (_verbose) { warnx("WPM: Send MISSION_ACK type %u to ID %u", wpa.type, wpa.target_system); }
 }
@@ -202,7 +202,7 @@ MavlinkMissionManager::send_mission_current(uint16_t seq)
 
 		wpc.seq = seq;
 
-		_mavlink->send_message(MAVLINK_MSG_ID_MISSION_CURRENT, &wpc);
+		mavlink_msg_mission_current_send_struct(_mavlink->get_channel(), &wpc);
 
 	} else if (seq == 0 && _count == 0) {
 		/* don't broadcast if no WPs */
@@ -226,7 +226,7 @@ MavlinkMissionManager::send_mission_count(uint8_t sysid, uint8_t compid, uint16_
 	wpc.target_component = compid;
 	wpc.count = _count;
 
-	_mavlink->send_message(MAVLINK_MSG_ID_MISSION_COUNT, &wpc);
+	mavlink_msg_mission_count_send_struct(_mavlink->get_channel(), &wpc);
 
 	if (_verbose) { warnx("WPM: Send MISSION_COUNT %u to ID %u", wpc.count, wpc.target_system); }
 }
@@ -250,7 +250,7 @@ MavlinkMissionManager::send_mission_item(uint8_t sysid, uint8_t compid, uint16_t
 		wp.seq = seq;
 		wp.current = (_current_seq == seq) ? 1 : 0;
 
-		_mavlink->send_message(MAVLINK_MSG_ID_MISSION_ITEM, &wp);
+		mavlink_msg_mission_item_send_struct(_mavlink->get_channel(), &wp);
 
 		if (_verbose) { warnx("WPM: Send MISSION_ITEM seq %u to ID %u", wp.seq, wp.target_system); }
 
@@ -276,7 +276,7 @@ MavlinkMissionManager::send_mission_request(uint8_t sysid, uint8_t compid, uint1
 		wpr.target_component = compid;
 		wpr.seq = seq;
 
-		_mavlink->send_message(MAVLINK_MSG_ID_MISSION_REQUEST, &wpr);
+		mavlink_msg_mission_request_send_struct(_mavlink->get_channel(), &wpr);
 
 		if (_verbose) { warnx("WPM: Send MISSION_REQUEST seq %u to ID %u", wpr.seq, wpr.target_system); }
 
@@ -295,7 +295,7 @@ MavlinkMissionManager::send_mission_item_reached(uint16_t seq)
 
 	wp_reached.seq = seq;
 
-	_mavlink->send_message(MAVLINK_MSG_ID_MISSION_ITEM_REACHED, &wp_reached);
+	mavlink_msg_mission_item_reached_send_struct(_mavlink->get_channel(), &wp_reached);
 
 	if (_verbose) { warnx("WPM: Send MISSION_ITEM_REACHED reached_seq %u", wp_reached.seq); }
 }
diff --git a/src/modules/mavlink/mavlink_parameters.cpp b/src/modules/mavlink/mavlink_parameters.cpp
index 50fc0429be..99f887a5b9 100644
--- a/src/modules/mavlink/mavlink_parameters.cpp
+++ b/src/modules/mavlink/mavlink_parameters.cpp
@@ -186,7 +186,7 @@ MavlinkParametersManager::handle_message(const mavlink_message_t *msg)
 						strncpy(param_value.param_id, HASH_PARAM, MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN);
 						param_value.param_type = MAV_PARAM_TYPE_UINT32;
 						memcpy(&param_value.param_value, &hash, sizeof(hash));
-						_mavlink->send_message(MAVLINK_MSG_ID_PARAM_VALUE, &param_value);
+						mavlink_msg_param_value_send_struct(_mavlink->get_channel(), &param_value);
 					} else {
 						/* local name buffer to enforce null-terminated string */
 						char name[MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN + 1];
@@ -302,7 +302,7 @@ MavlinkParametersManager::send(const hrt_abstime t)
 			memcpy(&msg.param_value, &val, sizeof(int32_t));
 			msg.param_type = MAVLINK_TYPE_INT32_T;
 		}
-		_mavlink->send_message(MAVLINK_MSG_ID_PARAM_VALUE, &msg, value.node_id);
+		mavlink_msg_param_value_send_struct(_mavlink->get_channel(), &msg);
 	} else if (_send_all_index >= 0 && _mavlink->boot_complete()) {
 		/* send all parameters if requested, but only after the system has booted */
 
@@ -325,7 +325,7 @@ MavlinkParametersManager::send(const hrt_abstime t)
 			strncpy(msg.param_id, HASH_PARAM, MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN);
 			msg.param_type = MAV_PARAM_TYPE_UINT32;
 			memcpy(&msg.param_value, &hash, sizeof(hash));
-			_mavlink->send_message(MAVLINK_MSG_ID_PARAM_VALUE, &msg);
+			mavlink_msg_param_value_send_struct(_mavlink->get_channel(), &msg);
 
 			/* after this we should start sending all params */
 			_send_all_index = 0;
@@ -396,7 +396,7 @@ MavlinkParametersManager::send_param(param_t param)
 		msg.param_type = MAVLINK_TYPE_FLOAT;
 	}
 
-	_mavlink->send_message(MAVLINK_MSG_ID_PARAM_VALUE, &msg);
+	mavlink_msg_param_value_send_struct(_mavlink->get_channel(), &msg);
 
 	return 0;
 }
diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp
index 7a72c24ded..d31b7e0ad1 100644
--- a/src/modules/mavlink/mavlink_receiver.cpp
+++ b/src/modules/mavlink/mavlink_receiver.cpp
@@ -1353,9 +1353,7 @@ MavlinkReceiver::handle_message_ping(mavlink_message_t *msg)
 
 	if ((mavlink_system.sysid == ping.target_system) &&
 	    (mavlink_system.compid == ping.target_component)) {
-		mavlink_message_t msg_out;
-		mavlink_msg_ping_encode(_mavlink->get_system_id(), _mavlink->get_component_id(), &msg_out, &ping);
-		_mavlink->send_message(MAVLINK_MSG_ID_PING, &msg_out);
+		mavlink_msg_ping_send_struct(_mavlink->get_channel(), &ping);
 	}
 }
 
@@ -1423,7 +1421,7 @@ MavlinkReceiver::handle_message_timesync(mavlink_message_t *msg)
 		rsync.tc1 = now_ns;
 		rsync.ts1 = tsync.ts1;
 
-		_mavlink->send_message(MAVLINK_MSG_ID_TIMESYNC, &rsync);
+		mavlink_msg_timesync_send_struct(_mavlink->get_channel(), &rsync);
 
 		return;
 

From 55750a0e7c3c719c5ad821d9dd5d56a06b8dab58 Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Sat, 14 May 2016 14:35:47 +0200
Subject: [PATCH 0246/1230] Fix GCC compile error

---
 src/modules/mavlink/mavlink_log_handler.cpp | 5 ++++-
 1 file changed, 4 insertions(+), 1 deletion(-)

diff --git a/src/modules/mavlink/mavlink_log_handler.cpp b/src/modules/mavlink/mavlink_log_handler.cpp
index a9dddb55ff..ab7c7c048e 100644
--- a/src/modules/mavlink/mavlink_log_handler.cpp
+++ b/src/modules/mavlink/mavlink_log_handler.cpp
@@ -253,7 +253,10 @@ void
 MavlinkLogHandler::_log_send_listing()
 {
 	mavlink_log_entry_t response;
-	_pLogHandlerHelper->get_entry(_pLogHandlerHelper->next_entry, response.size, response.time_utc);
+	uint32_t size, date;
+	_pLogHandlerHelper->get_entry(_pLogHandlerHelper->next_entry, size, date);
+	response.size 		= size;
+	response.time_utc 	= date;
 	response.id 		= _pLogHandlerHelper->next_entry;
 	response.num_logs	= _pLogHandlerHelper->log_count;
 	response.last_log_num	= _pLogHandlerHelper->last_entry;

From acb2e52389f7c0a83af3def96812414b48ace8f2 Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Sat, 14 May 2016 15:17:00 +0200
Subject: [PATCH 0247/1230] Update headers

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

diff --git a/mavlink/include/mavlink/v2.0 b/mavlink/include/mavlink/v2.0
index a62bc59cd1..699a2b0df1 160000
--- a/mavlink/include/mavlink/v2.0
+++ b/mavlink/include/mavlink/v2.0
@@ -1 +1 @@
-Subproject commit a62bc59cd1b333ffc373cc2ee9f91c80e8322ca8
+Subproject commit 699a2b0df1b259a962a01af788e8306ee8d016d8

From 84800dfd87d1eddcd973bb1a653d6212f73415e8 Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Sat, 14 May 2016 15:17:32 +0200
Subject: [PATCH 0248/1230] MAVLink: Update complete app to support MAVLink 1 &
 MAVLink 2. Add MAV_PROTO_VER param to switch between them

---
 src/modules/mavlink/mavlink.c            | 18 ----------
 src/modules/mavlink/mavlink_main.cpp     | 42 ++++++++++++++++++++----
 src/modules/mavlink/mavlink_main.h       | 21 ++++++++++++
 src/modules/mavlink/mavlink_params.c     |  9 +++++
 src/modules/mavlink/mavlink_receiver.cpp |  4 +++
 5 files changed, 70 insertions(+), 24 deletions(-)

diff --git a/src/modules/mavlink/mavlink.c b/src/modules/mavlink/mavlink.c
index bb6dc44789..ec4b71bf4d 100644
--- a/src/modules/mavlink/mavlink.c
+++ b/src/modules/mavlink/mavlink.c
@@ -50,21 +50,3 @@ mavlink_system_t mavlink_system = {
 	1,
 	1
 }; // System ID, 1-255, Component/Subsystem ID, 1-255
-
-/*
- * Internal function to give access to the channel status for each channel
- */
-extern mavlink_status_t *mavlink_get_channel_status(uint8_t channel)
-{
-	static mavlink_status_t m_mavlink_status[MAVLINK_COMM_NUM_BUFFERS];
-	return &m_mavlink_status[channel];
-}
-
-/*
- * Internal function to give access to the channel buffer for each channel
- */
-extern mavlink_message_t *mavlink_get_channel_buffer(uint8_t channel)
-{
-	static mavlink_message_t m_mavlink_buffer[MAVLINK_COMM_NUM_BUFFERS];
-	return &m_mavlink_buffer[channel];
-}
diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp
index 7b1530e2f9..13bb66cba2 100644
--- a/src/modules/mavlink/mavlink_main.cpp
+++ b/src/modules/mavlink/mavlink_main.cpp
@@ -106,8 +106,6 @@ static const int ERROR = -1;
 
 static Mavlink *_mavlink_instances = nullptr;
 
-static const mavlink_crc_entry_t mavlink_message_meta[] = MAVLINK_MESSAGE_CRCS;
-
 /**
  * mavlink app start / stop handling function
  *
@@ -133,10 +131,28 @@ void mavlink_end_uart_send(mavlink_channel_t chan, int length)
 	}
 }
 
+/*
+ * Internal function to give access to the channel status for each channel
+ */
+mavlink_status_t *mavlink_get_channel_status(uint8_t channel)
+{
+	return Mavlink::get_status_for_instance(channel);
+}
+
+/*
+ * Internal function to give access to the channel buffer for each channel
+ */
+mavlink_message_t *mavlink_get_channel_buffer(uint8_t channel)
+{
+	return Mavlink::get_buffer_for_instance(channel);
+}
+
 static void usage(void);
 
 bool Mavlink::_boot_complete = false;
 bool Mavlink::_config_link_on = false;
+mavlink_message_t Mavlink::_mavlink_buffer[MAVLINK_COMM_NUM_BUFFERS] = {};
+mavlink_status_t Mavlink::_mavlink_status[MAVLINK_COMM_NUM_BUFFERS] = {};
 
 Mavlink::Mavlink() :
 	_device_name("/dev/ttyS1"),
@@ -183,6 +199,7 @@ Mavlink::Mavlink() :
 	_last_write_success_time(0),
 	_last_write_try_time(0),
 	_mavlink_start_time(0),
+	_protocol_version(0),
 	_bytes_tx(0),
 	_bytes_txerr(0),
 	_bytes_rx(0),
@@ -297,6 +314,18 @@ Mavlink::~Mavlink()
 	}
 }
 
+void
+Mavlink::set_proto_version(unsigned version)
+{
+	_protocol_version = version;
+
+	if (version == 1 || ((version == 0) && !_received_messages)) {
+		get_status()->flags |= MAVLINK_STATUS_FLAG_OUT_MAVLINK1;
+	} else if (version == 2) {
+		get_status()->flags &= ~(MAVLINK_STATUS_FLAG_OUT_MAVLINK1);
+	}
+}
+
 void
 Mavlink::count_txerr()
 {
@@ -493,6 +522,7 @@ void Mavlink::mavlink_update_system(void)
 	if (!_param_initialized) {
 		_param_system_id = param_find("MAV_SYS_ID");
 		_param_component_id = param_find("MAV_COMP_ID");
+		_param_proto_ver = param_find("MAV_PROTO_VER");
 		_param_radio_id = param_find("MAV_RADIO_ID");
 		_param_system_type = param_find("MAV_TYPE");
 		_param_use_hil_gps = param_find("MAV_USEHILGPS");
@@ -509,6 +539,10 @@ void Mavlink::mavlink_update_system(void)
 	int32_t component_id;
 	param_get(_param_component_id, &component_id);
 
+	int32_t proto;
+	param_get(_param_proto_ver, &proto);
+	set_proto_version(proto);
+
 	param_get(_param_radio_id, &_radio_id);
 
 	/* only allow system ID and component ID updates
@@ -841,7 +875,6 @@ Mavlink::send_packet()
 		ret = sendto(_socket_fd, _network_buf, _network_buf_len, 0, (struct sockaddr *)&_src_addr, sizeof(_src_addr));
 
 		struct telemetry_status_s &tstatus = get_rx_status();
-		warnx("UDP count: %d", ret);
 
 		/* resend message via broadcast if no valid connection exists */
 		if ((_mode != MAVLINK_MODE_ONBOARD) &&
@@ -858,8 +891,6 @@ Mavlink::send_packet()
 
 				if (bret <= 0) {
 					PX4_WARN("sending broadcast failed, errno: %d: %s", errno, strerror(errno));
-				} else {
-					warnx("BROADCAST count: %d", bret);
 				}
 			}
 		}
@@ -877,7 +908,6 @@ Mavlink::send_packet()
 void
 Mavlink::send_bytes(const uint8_t *buf, unsigned packet_len)
 {
-	warnx("send bytes");
 	/* If the wait until transmit flag is on, only transmit after we've received messages.
 	   Otherwise, transmit all the time. */
 	if (!should_transmit()) {
diff --git a/src/modules/mavlink/mavlink_main.h b/src/modules/mavlink/mavlink_main.h
index b0b682d5cb..42ee722c20 100644
--- a/src/modules/mavlink/mavlink_main.h
+++ b/src/modules/mavlink/mavlink_main.h
@@ -114,6 +114,23 @@ public:
 
 	static Mavlink 		*get_instance_for_network_port(unsigned long port);
 
+	static mavlink_message_t *get_buffer_for_instance(unsigned instance) { return &_mavlink_buffer[instance]; }
+
+	mavlink_message_t 	*get_buffer() { return Mavlink::get_buffer_for_instance(_instance_id); }
+
+	static mavlink_status_t *get_status_for_instance(unsigned instance) { return &_mavlink_status[instance]; }
+
+	mavlink_status_t 	*get_status() { return Mavlink::get_status_for_instance(_instance_id); }
+
+	/**
+	 * Set the MAVLink version
+	 *
+	 * Currently supporting v1 and v2
+	 *
+	 * @param version MAVLink version
+	 */
+	void			set_proto_version(unsigned version);
+
 	static int		destroy_all_instances();
 
 	static int		get_status_all_instances();
@@ -385,6 +402,8 @@ private:
 	orb_advert_t		_mavlink_log_pub;
 	bool			_task_running;
 	static bool		_boot_complete;
+	static mavlink_message_t _mavlink_buffer[MAVLINK_COMM_NUM_BUFFERS];
+	static mavlink_status_t _mavlink_status[MAVLINK_COMM_NUM_BUFFERS];
 
 	/* states */
 	bool			_hil_enabled;		/**< Hardware In the Loop mode */
@@ -444,6 +463,7 @@ private:
 	uint64_t		_last_write_success_time;
 	uint64_t		_last_write_try_time;
 	uint64_t		_mavlink_start_time;
+	int32_t			_protocol_version;
 
 	unsigned		_bytes_tx;
 	unsigned		_bytes_txerr;
@@ -486,6 +506,7 @@ private:
 
 	param_t			_param_system_id;
 	param_t			_param_component_id;
+	param_t			_param_proto_ver;
 	param_t			_param_radio_id;
 	param_t			_param_system_type;
 	param_t			_param_use_hil_gps;
diff --git a/src/modules/mavlink/mavlink_params.c b/src/modules/mavlink/mavlink_params.c
index ad7e9712cf..44964b9840 100644
--- a/src/modules/mavlink/mavlink_params.c
+++ b/src/modules/mavlink/mavlink_params.c
@@ -47,6 +47,15 @@ PARAM_DEFINE_INT32(MAV_SYS_ID, 1);
  */
 PARAM_DEFINE_INT32(MAV_COMP_ID, 1);
 
+/**
+ * MAVLink protocol version
+ * @group MAVLink
+ * @value 0 Default to 1, switch to 2 if GCS sends version 2
+ * @value 1 Always use version 1
+ * @value 2 Always use version 2
+ */
+PARAM_DEFINE_INT32(MAV_PROTO_VER, 1);
+
 /**
  * MAVLink Radio ID
  *
diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp
index d31b7e0ad1..f5ec194177 100644
--- a/src/modules/mavlink/mavlink_receiver.cpp
+++ b/src/modules/mavlink/mavlink_receiver.cpp
@@ -2084,6 +2084,10 @@ MavlinkReceiver::receive_thread(void *arg)
 				/* if read failed, this loop won't execute */
 				for (ssize_t i = 0; i < nread; i++) {
 					if (mavlink_parse_char(_mavlink->get_channel(), buf[i], &msg, &status)) {
+
+						/* check if we received version 2 */
+						// XXX todo _mavlink->set_proto_version(2);
+
 						/* handle generic messages and commands */
 						handle_message(&msg);
 

From 1775435ee5c2af661368f70d1476f8275d188a0a Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Sat, 14 May 2016 16:03:34 +0200
Subject: [PATCH 0249/1230] v2 test config: Disable a couple unnecessary parts

---
 cmake/configs/nuttx_px4fmu-v2_test.cmake | 14 +++++++-------
 1 file changed, 7 insertions(+), 7 deletions(-)

diff --git a/cmake/configs/nuttx_px4fmu-v2_test.cmake b/cmake/configs/nuttx_px4fmu-v2_test.cmake
index f65c74617c..4a0d2e1269 100644
--- a/cmake/configs/nuttx_px4fmu-v2_test.cmake
+++ b/cmake/configs/nuttx_px4fmu-v2_test.cmake
@@ -24,12 +24,12 @@ set(config_module_list
 	drivers/hmc5883
 	drivers/ms5611
 	#drivers/mb12xx
-	drivers/srf02
-	drivers/sf0x
-	drivers/ll40ls
-	drivers/trone
+	#drivers/srf02
+	#drivers/sf0x
+	#drivers/ll40ls
+	#drivers/trone
 	drivers/gps
-	drivers/pwm_out_sim
+	#drivers/pwm_out_sim
 	#drivers/hott
 	#drivers/hott/hott_telemetry
 	#drivers/hott/hott_sensors
@@ -45,8 +45,8 @@ set(config_module_list
 	drivers/gimbal
 	drivers/pwm_input
 	drivers/camera_trigger
-	drivers/bst
-	drivers/snapdragon_rc_pwm
+	#drivers/bst
+	#drivers/snapdragon_rc_pwm
 	#drivers/lis3mdl
 	#drivers/bmi160
 

From 56e887c6c7599fe15349540a527a17402f0d786c Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Sat, 14 May 2016 16:37:41 +0200
Subject: [PATCH 0250/1230] MAVLink app: Return result of network operation

---
 src/modules/mavlink/mavlink_main.cpp | 11 ++++++-----
 src/modules/mavlink/mavlink_main.h   |  4 +++-
 2 files changed, 9 insertions(+), 6 deletions(-)

diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp
index 13bb66cba2..417975faab 100644
--- a/src/modules/mavlink/mavlink_main.cpp
+++ b/src/modules/mavlink/mavlink_main.cpp
@@ -127,7 +127,7 @@ void mavlink_end_uart_send(mavlink_channel_t chan, int length)
 {
 	Mavlink* m = Mavlink::get_instance((unsigned)chan);
 	if (m != nullptr) {
-		m->send_packet();
+		(void)m->send_packet();
 	}
 }
 
@@ -864,13 +864,13 @@ Mavlink::get_free_tx_buf()
 	return buf_free;
 }
 
-void
+int
 Mavlink::send_packet()
 {
-#ifdef __PX4_POSIX
-
 	int ret = -1;
 
+#ifdef __PX4_POSIX
+
 	if (get_protocol() == UDP) {
 		ret = sendto(_socket_fd, _network_buf, _network_buf_len, 0, (struct sockaddr *)&_src_addr, sizeof(_src_addr));
 
@@ -901,8 +901,9 @@ Mavlink::send_packet()
 	}
 
 	_network_buf_len = 0;
-
 #endif
+
+	return ret;
 }
 
 void
diff --git a/src/modules/mavlink/mavlink_main.h b/src/modules/mavlink/mavlink_main.h
index 42ee722c20..6026acf793 100644
--- a/src/modules/mavlink/mavlink_main.h
+++ b/src/modules/mavlink/mavlink_main.h
@@ -250,8 +250,10 @@ public:
 
 	/**
 	 * Flush the transmit buffer and send one MAVLink packet
+	 *
+	 * @return the number of bytes sent or -1 in case of error
 	 */
-	void			send_packet();
+	int			send_packet();
 
 	/**
 	 * Resend message as is, don't change sequence number and CRC.

From be92040fe957229825a5337e793beabcda9a0941 Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Thu, 19 May 2016 21:10:27 +0200
Subject: [PATCH 0251/1230] FMUv2: Do not build BMI160

---
 cmake/configs/nuttx_px4fmu-v2_default.cmake | 1 -
 1 file changed, 1 deletion(-)

diff --git a/cmake/configs/nuttx_px4fmu-v2_default.cmake b/cmake/configs/nuttx_px4fmu-v2_default.cmake
index 71cdc9463a..8af384e7ba 100644
--- a/cmake/configs/nuttx_px4fmu-v2_default.cmake
+++ b/cmake/configs/nuttx_px4fmu-v2_default.cmake
@@ -48,7 +48,6 @@ set(config_module_list
 	drivers/bst
 	drivers/snapdragon_rc_pwm
 	drivers/lis3mdl
-	drivers/bmi160
 
 	#
 	# System commands

From 4e26c7fcd4c61cb148a843800d3744839283b6d2 Mon Sep 17 00:00:00 2001
From: David Sidrane <david_s5@nscdg.com>
Date: Wed, 18 May 2016 14:00:51 -1000
Subject: [PATCH 0252/1230] Use the value of nuttx CONFIG_ARMV7M_STACKCHECK to
 configure PX4 build for HW stack checking

---
 cmake/nuttx/px4_impl_nuttx.cmake | 20 ++++++++++++++++++++
 1 file changed, 20 insertions(+)

diff --git a/cmake/nuttx/px4_impl_nuttx.cmake b/cmake/nuttx/px4_impl_nuttx.cmake
index 061e567d33..79b829881e 100644
--- a/cmake/nuttx/px4_impl_nuttx.cmake
+++ b/cmake/nuttx/px4_impl_nuttx.cmake
@@ -202,6 +202,16 @@ function(px4_nuttx_add_export)
 	    add_dependencies(nuttx_patch nuttx_patch_${patch_name})
 	endforeach()
 
+	# Read defconfig to see if CONFIG_ARMV7M_STACKCHECK is yes 
+	# note: CONFIG will be BOARD in the future evaluation of ${hw_stack_check_${CONFIG}
+	file(STRINGS "${CMAKE_SOURCE_DIR}/nuttx-configs/${CONFIG}/${config_nuttx_config}/defconfig"
+		hw_stack_check_${CONFIG}
+		REGEX "CONFIG_ARMV7M_STACKCHECK=y"
+		)
+	if ("${hw_stack_check_${CONFIG}}" STREQUAL "CONFIG_ARMV7M_STACKCHECK=y")
+		set(config_nuttx_hw_stack_check_${CONFIG} y CACHE INTERNAL "" FORCE)
+	endif()
+
 	# copy and export
 	file(GLOB_RECURSE config_files ${CMAKE_SOURCE_DIR}/nuttx-configs/${CONFIG}/*)
 	add_custom_command(OUTPUT ${CMAKE_BINARY_DIR}/${CONFIG}.export
@@ -428,6 +438,16 @@ function(px4_os_add_flags)
 
 	set(added_exe_linker_flags) # none currently
 
+	set(instrument_flags)
+	if ("${config_nuttx_hw_stack_check_${BOARD}}" STREQUAL "y")
+		set(instrument_flags
+			-finstrument-functions
+			-ffixed-r10
+			)
+		list(APPEND c_flags ${instrument_flags})
+		list(APPEND cxx_flags ${instrument_flags})
+	endif()
+
 	set(cpu_flags)
 	if (${BOARD} STREQUAL "px4fmu-v1")
 		set(cpu_flags

From 3d1713f79e898d4f0438eab25a8f541307a2c3b9 Mon Sep 17 00:00:00 2001
From: Daniel Agar <daniel@agar.ca>
Date: Thu, 19 May 2016 00:02:57 -0400
Subject: [PATCH 0253/1230] vtol remove Wno-write-strings

---
 src/modules/vtol_att_control/CMakeLists.txt |  2 --
 src/modules/vtol_att_control/standard.cpp   | 10 +++++++---
 src/modules/vtol_att_control/tiltrotor.cpp  |  6 ++++--
 src/modules/vtol_att_control/vtol_type.cpp  | 20 ++++++++++++++------
 4 files changed, 25 insertions(+), 13 deletions(-)

diff --git a/src/modules/vtol_att_control/CMakeLists.txt b/src/modules/vtol_att_control/CMakeLists.txt
index b7dcd83ed0..7e424e55b6 100644
--- a/src/modules/vtol_att_control/CMakeLists.txt
+++ b/src/modules/vtol_att_control/CMakeLists.txt
@@ -34,8 +34,6 @@ px4_add_module(
 	MODULE modules__vtol_att_control
 	MAIN vtol_att_control
 	COMPILE_FLAGS
-		-Wno-write-strings
-		
 	SRCS
 		vtol_att_control_main.cpp
 		tiltrotor.cpp
diff --git a/src/modules/vtol_att_control/standard.cpp b/src/modules/vtol_att_control/standard.cpp
index ba6c0b7187..e095c46a3f 100644
--- a/src/modules/vtol_att_control/standard.cpp
+++ b/src/modules/vtol_att_control/standard.cpp
@@ -395,10 +395,12 @@ Standard::set_max_mc(unsigned pwm_value)
 {
 	int ret;
 	unsigned servo_count;
-	char *dev = PWM_OUTPUT0_DEVICE_PATH;
+	const char *dev = PWM_OUTPUT0_DEVICE_PATH;
 	int fd = px4_open(dev, 0);
 
-	if (fd < 0) {PX4_WARN("can't open %s", dev);}
+	if (fd < 0) {
+		PX4_WARN("can't open %s", dev);
+	}
 
 	ret = px4_ioctl(fd, PWM_SERVO_GET_COUNT, (unsigned long)&servo_count);
 	struct pwm_output_values pwm_values;
@@ -411,7 +413,9 @@ Standard::set_max_mc(unsigned pwm_value)
 
 	ret = px4_ioctl(fd, PWM_SERVO_SET_MAX_PWM, (long unsigned int)&pwm_values);
 
-	if (ret != OK) {PX4_WARN("failed setting max values");}
+	if (ret != OK) {
+		PX4_WARN("failed setting max values");
+	}
 
 	px4_close(fd);
 }
diff --git a/src/modules/vtol_att_control/tiltrotor.cpp b/src/modules/vtol_att_control/tiltrotor.cpp
index 7cb5ea26e9..65f14b1e7c 100644
--- a/src/modules/vtol_att_control/tiltrotor.cpp
+++ b/src/modules/vtol_att_control/tiltrotor.cpp
@@ -433,10 +433,12 @@ void Tiltrotor::set_rear_motor_state(rear_motor_state state)
 
 	int ret;
 	unsigned servo_count;
-	char *dev = PWM_OUTPUT0_DEVICE_PATH;
+	const char *dev = PWM_OUTPUT0_DEVICE_PATH;
 	int fd = px4_open(dev, 0);
 
-	if (fd < 0) {PX4_WARN("can't open %s", dev);}
+	if (fd < 0) {
+		PX4_WARN("can't open %s", dev);
+	}
 
 	ret = px4_ioctl(fd, PWM_SERVO_GET_COUNT, (unsigned long)&servo_count);
 	struct pwm_output_values pwm_max_values;
diff --git a/src/modules/vtol_att_control/vtol_type.cpp b/src/modules/vtol_att_control/vtol_type.cpp
index 8184d55d2e..d04290d85e 100644
--- a/src/modules/vtol_att_control/vtol_type.cpp
+++ b/src/modules/vtol_att_control/vtol_type.cpp
@@ -86,10 +86,12 @@ void VtolType::set_idle_mc()
 {
 	int ret;
 	unsigned servo_count;
-	char *dev = PWM_OUTPUT0_DEVICE_PATH;
+	const char *dev = PWM_OUTPUT0_DEVICE_PATH;
 	int fd = px4_open(dev, 0);
 
-	if (fd < 0) {PX4_WARN("can't open %s", dev);}
+	if (fd < 0) {
+		PX4_WARN("can't open %s", dev);
+	}
 
 	ret = px4_ioctl(fd, PWM_SERVO_GET_COUNT, (unsigned long)&servo_count);
 	unsigned pwm_value = _params->idle_pwm_mc;
@@ -103,7 +105,9 @@ void VtolType::set_idle_mc()
 
 	ret = px4_ioctl(fd, PWM_SERVO_SET_MIN_PWM, (long unsigned int)&pwm_values);
 
-	if (ret != OK) {PX4_WARN("failed setting min values");}
+	if (ret != OK) {
+		PX4_WARN("failed setting min values");
+	}
 
 	px4_close(fd);
 
@@ -116,10 +120,12 @@ void VtolType::set_idle_mc()
 void VtolType::set_idle_fw()
 {
 	int ret;
-	char *dev = PWM_OUTPUT0_DEVICE_PATH;
+	const char *dev = PWM_OUTPUT0_DEVICE_PATH;
 	int fd = px4_open(dev, 0);
 
-	if (fd < 0) {PX4_WARN("can't open %s", dev);}
+	if (fd < 0) {
+		PX4_WARN("can't open %s", dev);
+	}
 
 	struct pwm_output_values pwm_values;
 	memset(&pwm_values, 0, sizeof(pwm_values));
@@ -132,7 +138,9 @@ void VtolType::set_idle_fw()
 
 	ret = px4_ioctl(fd, PWM_SERVO_SET_MIN_PWM, (long unsigned int)&pwm_values);
 
-	if (ret != OK) {PX4_WARN("failed setting min values");}
+	if (ret != OK) {
+		PX4_WARN("failed setting min values");
+	}
 
 	px4_close(fd);
 }

From f58596bbcdc7f8fed1e5ca44e4b89bad4d28a27a Mon Sep 17 00:00:00 2001
From: Daniel Agar <daniel@agar.ca>
Date: Thu, 19 May 2016 00:05:07 -0400
Subject: [PATCH 0254/1230] vtol_att_control astyle

---
 Tools/check_code_style_all.sh                     |  1 +
 src/modules/vtol_att_control/standard.cpp         | 15 ++++++++-------
 src/modules/vtol_att_control/standard.h           |  2 +-
 src/modules/vtol_att_control/tailsitter.cpp       |  4 ++--
 .../vtol_att_control/vtol_att_control_main.cpp    |  7 ++++---
 src/modules/vtol_att_control/vtol_type.cpp        |  1 +
 6 files changed, 17 insertions(+), 13 deletions(-)

diff --git a/Tools/check_code_style_all.sh b/Tools/check_code_style_all.sh
index 3dde2105dd..f66a039e15 100755
--- a/Tools/check_code_style_all.sh
+++ b/Tools/check_code_style_all.sh
@@ -37,6 +37,7 @@ find \
     src/modules/systemlib \
     src/modules/unit_test \
     src/modules/uORB \
+    src/modules/vtol_att_control \
     src/platforms \
     src/systemcmds \
     -type f \( -name "*.c" -o -name "*.h" -o -name "*.cpp" -o -name "*.hpp" \) \
diff --git a/src/modules/vtol_att_control/standard.cpp b/src/modules/vtol_att_control/standard.cpp
index e095c46a3f..a4a09ed414 100644
--- a/src/modules/vtol_att_control/standard.cpp
+++ b/src/modules/vtol_att_control/standard.cpp
@@ -155,7 +155,7 @@ void Standard::update_vtol_state()
 			// transition to MC mode if transition time has passed
 			// XXX: base this on XY hold velocity of MC
 			if (hrt_elapsed_time(&_vtol_schedule.transition_start) >
-				(_params_standard.back_trans_dur * 1000000.0f)) {
+			    (_params_standard.back_trans_dur * 1000000.0f)) {
 				_vtol_schedule.flight_mode = MC_MODE;
 			}
 		}
@@ -236,7 +236,7 @@ void Standard::update_transition_state()
 		    (float)hrt_elapsed_time(&_vtol_schedule.transition_start) > (_params_standard.front_trans_time_min * 1000000.0f)
 		   ) {
 			float weight = 1.0f - fabsf(_airspeed->indicated_airspeed_m_s - _params_standard.airspeed_blend) /
-					   _airspeed_trans_blend_margin;
+				       _airspeed_trans_blend_margin;
 			_mc_roll_weight = weight;
 			_mc_pitch_weight = weight;
 			_mc_yaw_weight = weight;
@@ -300,18 +300,19 @@ void Standard::update_mc_state()
 
 	// get projection of thrust vector on body x axis. This is used to
 	// determine the desired forward acceleration which we want to achieve with the pusher
-	math::Matrix<3,3> R(&_v_att->R[0]);
-	math::Matrix<3,3> R_sp(&_v_att_sp->R_body[0]);
-	math::Vector<3> thrust_sp_axis(-R_sp(0,2), -R_sp(1,2), -R_sp(2,2));
+	math::Matrix<3, 3> R(&_v_att->R[0]);
+	math::Matrix<3, 3> R_sp(&_v_att_sp->R_body[0]);
+	math::Vector<3> thrust_sp_axis(-R_sp(0, 2), -R_sp(1, 2), -R_sp(2, 2));
 	math::Vector<3> euler = R.to_euler();
 	R.from_euler(0, 0, euler(2));
-	math::Vector<3> body_x_zero_tilt(R(0,0), R(1,0), R(2,0));
+	math::Vector<3> body_x_zero_tilt(R(0, 0), R(1, 0), R(2, 0));
 
 	// we are using a parameter to scale the thrust value in order to compensate for highly over/under-powered motors
 	_pusher_throttle = body_x_zero_tilt * thrust_sp_axis * _v_att_sp->thrust * _params_standard.forward_thurst_scale;
 	_pusher_throttle = _pusher_throttle < 0.0f ? 0.0f : _pusher_throttle;
 
-	float pitch_sp_corrected = _v_att_sp->pitch_body < -_params_standard.down_pitch_max ? -_params_standard.down_pitch_max : _v_att_sp->pitch_body;
+	float pitch_sp_corrected = _v_att_sp->pitch_body < -_params_standard.down_pitch_max ? -_params_standard.down_pitch_max :
+				   _v_att_sp->pitch_body;
 
 	// compute new desired rotation matrix with corrected pitch angle
 	// and copy data to attitude setpoint topic
diff --git a/src/modules/vtol_att_control/standard.h b/src/modules/vtol_att_control/standard.h
index ea578bf2bf..b4ab84d59b 100644
--- a/src/modules/vtol_att_control/standard.h
+++ b/src/modules/vtol_att_control/standard.h
@@ -103,7 +103,7 @@ private:
 	} _vtol_schedule;
 
 	bool _flag_enable_mc_motors;
-	float _pusher_throttle;	
+	float _pusher_throttle;
 	float _airspeed_trans_blend_margin;
 
 	void set_max_mc(unsigned pwm_value);
diff --git a/src/modules/vtol_att_control/tailsitter.cpp b/src/modules/vtol_att_control/tailsitter.cpp
index f46ceb66e6..ec707ce27d 100644
--- a/src/modules/vtol_att_control/tailsitter.cpp
+++ b/src/modules/vtol_att_control/tailsitter.cpp
@@ -250,9 +250,9 @@ void Tailsitter::update_transition_state()
 		/** create time dependant throttle signal higher than  in MC and growing untill  P2 switch speed reached */
 		if (_airspeed->indicated_airspeed_m_s <= _params_tailsitter.airspeed_trans) {
 			_thrust_transition = _thrust_transition_start + (fabsf(THROTTLE_TRANSITION_MAX * _thrust_transition_start) *
-					    (float)hrt_elapsed_time(&_vtol_schedule.transition_start) / (_params_tailsitter.front_trans_dur * 1000000.0f));
+					     (float)hrt_elapsed_time(&_vtol_schedule.transition_start) / (_params_tailsitter.front_trans_dur * 1000000.0f));
 			_thrust_transition = math::constrain(_thrust_transition , _thrust_transition_start ,
-							    (1.0f + THROTTLE_TRANSITION_MAX) * _thrust_transition_start);
+							     (1.0f + THROTTLE_TRANSITION_MAX) * _thrust_transition_start);
 			_v_att_sp->thrust = _thrust_transition;
 		}
 
diff --git a/src/modules/vtol_att_control/vtol_att_control_main.cpp b/src/modules/vtol_att_control/vtol_att_control_main.cpp
index 81f3b86f19..890a09a840 100644
--- a/src/modules/vtol_att_control/vtol_att_control_main.cpp
+++ b/src/modules/vtol_att_control/vtol_att_control_main.cpp
@@ -491,6 +491,7 @@ VtolAttitudeControl::is_fixed_wing_requested()
 	if (_abort_front_transition) {
 		if (to_fw) {
 			to_fw = false;
+
 		} else {
 			// the state changed to mc mode, reset the abort request
 			_abort_front_transition = false;
@@ -507,7 +508,7 @@ VtolAttitudeControl::is_fixed_wing_requested()
 void
 VtolAttitudeControl::abort_front_transition()
 {
-	if(!_abort_front_transition) {
+	if (!_abort_front_transition) {
 		mavlink_log_critical(&_mavlink_log_pub, "Front transition timeout occured, aborting");
 		_abort_front_transition = true;
 		_vtol_vehicle_status.vtol_transition_failsafe = true;
@@ -789,8 +790,8 @@ void VtolAttitudeControl::task_main()
 
 		/* Only publish if the proper mode(s) are enabled */
 		if (_v_control_mode.flag_control_attitude_enabled ||
-			_v_control_mode.flag_control_rates_enabled ||
-			_v_control_mode.flag_control_manual_enabled) {
+		    _v_control_mode.flag_control_rates_enabled ||
+		    _v_control_mode.flag_control_manual_enabled) {
 			if (_actuators_0_pub != nullptr) {
 				orb_publish(ORB_ID(actuator_controls_0), _actuators_0_pub, &_actuators_out_0);
 
diff --git a/src/modules/vtol_att_control/vtol_type.cpp b/src/modules/vtol_att_control/vtol_type.cpp
index d04290d85e..c9e69d7b62 100644
--- a/src/modules/vtol_att_control/vtol_type.cpp
+++ b/src/modules/vtol_att_control/vtol_type.cpp
@@ -128,6 +128,7 @@ void VtolType::set_idle_fw()
 	}
 
 	struct pwm_output_values pwm_values;
+
 	memset(&pwm_values, 0, sizeof(pwm_values));
 
 	for (int i = 0; i < _params->vtol_motor_count; i++) {

From 2487dbfc9254c7411e0c1e5fc449aaacbbf61e78 Mon Sep 17 00:00:00 2001
From: Daniel Agar <daniel@agar.ca>
Date: Thu, 19 May 2016 00:19:09 -0400
Subject: [PATCH 0255/1230] remove Wpacked and cleanup unused warning flags

---
 cmake/common/px4_base.cmake                            | 1 -
 src/drivers/px4flow/CMakeLists.txt                     | 1 -
 src/drivers/snapdragon_rc_pwm/CMakeLists.txt           | 2 --
 src/drivers/uart_esc/CMakeLists.txt                    | 1 -
 src/firmware/posix/CMakeLists.txt                      | 2 +-
 src/modules/commander/CMakeLists.txt                   | 2 --
 src/modules/mavlink/CMakeLists.txt                     | 4 ----
 src/modules/mavlink/mavlink_tests/CMakeLists.txt       | 3 ---
 src/modules/navigator/CMakeLists.txt                   | 1 -
 src/modules/navigator/geofence.h                       | 2 +-
 src/modules/navigator/mission.cpp                      | 2 +-
 src/modules/sensors/CMakeLists.txt                     | 3 +--
 src/modules/sensors/sensors.cpp                        | 2 +-
 src/modules/simulator/CMakeLists.txt                   | 3 ---
 src/modules/systemlib/CMakeLists.txt                   | 2 --
 src/modules/systemlib/param/param.c                    | 2 +-
 src/modules/uavcan/CMakeLists.txt                      | 3 ---
 src/platforms/posix/drivers/accelsim/CMakeLists.txt    | 1 -
 src/platforms/posix/drivers/airspeedsim/CMakeLists.txt | 1 -
 src/platforms/posix/drivers/barosim/CMakeLists.txt     | 1 -
 src/platforms/posix/drivers/gpssim/CMakeLists.txt      | 1 -
 src/platforms/posix/drivers/gyrosim/CMakeLists.txt     | 1 -
 src/platforms/posix/px4_layer/CMakeLists.txt           | 2 --
 src/platforms/qurt/px4_layer/CMakeLists.txt            | 1 -
 src/systemcmds/mtd/CMakeLists.txt                      | 1 -
 src/systemcmds/tests/CMakeLists.txt                    | 4 ++--
 26 files changed, 8 insertions(+), 41 deletions(-)

diff --git a/cmake/common/px4_base.cmake b/cmake/common/px4_base.cmake
index 2098996188..886522763a 100644
--- a/cmake/common/px4_base.cmake
+++ b/cmake/common/px4_base.cmake
@@ -597,7 +597,6 @@ function(px4_add_common_flags)
 		-Wall
 		-Werror
 		-Wextra
-		-Wpacked
 		-Wno-sign-compare
 		-Wshadow
 		-Wfloat-equal
diff --git a/src/drivers/px4flow/CMakeLists.txt b/src/drivers/px4flow/CMakeLists.txt
index 1859ab3534..d8953b1ae1 100644
--- a/src/drivers/px4flow/CMakeLists.txt
+++ b/src/drivers/px4flow/CMakeLists.txt
@@ -35,7 +35,6 @@ px4_add_module(
 	MAIN px4flow
 	STACK_MAIN 1200
 	COMPILE_FLAGS
-		-Wno-attributes
 		-Os
 	SRCS
 		px4flow.cpp
diff --git a/src/drivers/snapdragon_rc_pwm/CMakeLists.txt b/src/drivers/snapdragon_rc_pwm/CMakeLists.txt
index 405b903306..6fae0f3157 100644
--- a/src/drivers/snapdragon_rc_pwm/CMakeLists.txt
+++ b/src/drivers/snapdragon_rc_pwm/CMakeLists.txt
@@ -35,8 +35,6 @@ px4_add_module(
 	MAIN snapdragon_rc_pwm
 	COMPILE_FLAGS
 		-Os
-		-Wno-attributes
-		-Wno-packed
 	SRCS
 		snapdragon_rc_pwm.cpp
 	DEPENDS
diff --git a/src/drivers/uart_esc/CMakeLists.txt b/src/drivers/uart_esc/CMakeLists.txt
index 43c201b26d..cb060c82da 100644
--- a/src/drivers/uart_esc/CMakeLists.txt
+++ b/src/drivers/uart_esc/CMakeLists.txt
@@ -35,7 +35,6 @@ px4_add_module(
 	MAIN uart_esc
 	COMPILE_FLAGS
 		-Os
-		-Wno-packed
 	SRCS
 		uart_esc.cpp
 	DEPENDS
diff --git a/src/firmware/posix/CMakeLists.txt b/src/firmware/posix/CMakeLists.txt
index ce2b469961..8d0232a017 100644
--- a/src/firmware/posix/CMakeLists.txt
+++ b/src/firmware/posix/CMakeLists.txt
@@ -11,7 +11,7 @@ if ("${BOARD}" STREQUAL "eagle" OR ("${BOARD}" STREQUAL "excelsior"))
 
 	FASTRPC_STUB_GEN(../qurt/px4muorb.idl)
 
-	set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -Wno-missing-prototypes -Wno-missing-declarations")
+	set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS}")
 	LINUX_APP(
 		APP_NAME mainapp
 		IDL_NAME px4muorb
diff --git a/src/modules/commander/CMakeLists.txt b/src/modules/commander/CMakeLists.txt
index be0d32dec7..8972791a41 100644
--- a/src/modules/commander/CMakeLists.txt
+++ b/src/modules/commander/CMakeLists.txt
@@ -36,8 +36,6 @@ px4_add_module(
 	STACK_MAIN 4096
 	STACK_MAX 2450
 	COMPILE_FLAGS
-		-Wno-attributes
-		-Wno-packed
 		-Os
 	SRCS
 		commander.cpp
diff --git a/src/modules/mavlink/CMakeLists.txt b/src/modules/mavlink/CMakeLists.txt
index 01a8b5fa5c..8487dad685 100644
--- a/src/modules/mavlink/CMakeLists.txt
+++ b/src/modules/mavlink/CMakeLists.txt
@@ -36,11 +36,7 @@ px4_add_module(
 	STACK_MAIN 1200
 	STACK_MAX 1500
 	COMPILE_FLAGS
-		-Wno-attributes
-		-Wno-packed
 		-DMAVLINK_COMM_NUM_BUFFERS=4
-		-Wno-packed
-		-Wno-tautological-constant-out-of-range-compare
 		-Os
 	SRCS
 		mavlink.c
diff --git a/src/modules/mavlink/mavlink_tests/CMakeLists.txt b/src/modules/mavlink/mavlink_tests/CMakeLists.txt
index 8096965c3d..26b7d0d4ba 100644
--- a/src/modules/mavlink/mavlink_tests/CMakeLists.txt
+++ b/src/modules/mavlink/mavlink_tests/CMakeLists.txt
@@ -36,9 +36,6 @@ px4_add_module(
 	STACK_MAIN 5000
 	COMPILE_FLAGS
 		-DMAVLINK_FTP_UNIT_TEST
-		-Wno-attributes
-		-Wno-packed
-		-Wno-packed
 		-Os
 	SRCS
 		mavlink_tests.cpp
diff --git a/src/modules/navigator/CMakeLists.txt b/src/modules/navigator/CMakeLists.txt
index 4fb4a5bed4..89530ca119 100644
--- a/src/modules/navigator/CMakeLists.txt
+++ b/src/modules/navigator/CMakeLists.txt
@@ -35,7 +35,6 @@ px4_add_module(
 	MAIN navigator
 	STACK_MAIN 1200
 	COMPILE_FLAGS
-		-Wno-sign-compare
 		-Os
 	SRCS
 		navigator_main.cpp
diff --git a/src/modules/navigator/geofence.h b/src/modules/navigator/geofence.h
index f46f849b18..ee8f61e484 100644
--- a/src/modules/navigator/geofence.h
+++ b/src/modules/navigator/geofence.h
@@ -131,7 +131,7 @@ private:
 	control::BlockParamInt _param_max_hor_distance;
 	control::BlockParamInt _param_max_ver_distance;
 
-	unsigned _outside_counter;
+	int _outside_counter;
 
 	bool inside(double lat, double lon, float altitude);
 	bool inside(const struct vehicle_global_position_s &global_position);
diff --git a/src/modules/navigator/mission.cpp b/src/modules/navigator/mission.cpp
index 56fa817845..401fe0899d 100644
--- a/src/modules/navigator/mission.cpp
+++ b/src/modules/navigator/mission.cpp
@@ -1149,7 +1149,7 @@ Mission::reset_offboard_mission(struct mission_s &mission)
 			if (mission.count > 0) {
 				dm_item_t dm_current = DM_KEY_WAYPOINTS_OFFBOARD(mission.dataman_id);
 
-				for (int index = 0; index < mission.count; index++) {
+				for (unsigned index = 0; index < mission.count; index++) {
 					struct mission_item_s item;
 					const ssize_t len = sizeof(struct mission_item_s);
 
diff --git a/src/modules/sensors/CMakeLists.txt b/src/modules/sensors/CMakeLists.txt
index 93f2f59d2f..e26df79246 100644
--- a/src/modules/sensors/CMakeLists.txt
+++ b/src/modules/sensors/CMakeLists.txt
@@ -35,9 +35,8 @@ px4_add_module(
 	MODULE modules__sensors
 	MAIN sensors
 	PRIORITY "SCHED_PRIORITY_MAX-5"
-	STACK_MAIN 1200
+	STACK_MAIN 1300
 	COMPILE_FLAGS
-		-Wno-type-limits
 		-O3
 	SRCS
 		sensors.cpp
diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp
index 35db3614af..cbc3e062ac 100644
--- a/src/modules/sensors/sensors.cpp
+++ b/src/modules/sensors/sensors.cpp
@@ -1945,7 +1945,7 @@ Sensors::rc_poll()
 			if (_parameters.rc_map_flightmode > 0) {
 
 				/* the number of valid slots equals the index of the max marker minus one */
-				const unsigned num_slots = manual_control_setpoint_s::MODE_SLOT_MAX;
+				const int num_slots = manual_control_setpoint_s::MODE_SLOT_MAX;
 
 				/* the half width of the range of a slot is the total range
 				 * divided by the number of slots, again divided by two
diff --git a/src/modules/simulator/CMakeLists.txt b/src/modules/simulator/CMakeLists.txt
index 0a7e221086..98db9d8731 100644
--- a/src/modules/simulator/CMakeLists.txt
+++ b/src/modules/simulator/CMakeLists.txt
@@ -40,9 +40,6 @@ px4_add_module(
 	MODULE modules__simulator
 	MAIN simulator
 	COMPILE_FLAGS
-		-Wno-attributes
-		-Wno-packed
-		-Wno-packed
 	SRCS
 		${SIMULATOR_SRCS}
 	DEPENDS
diff --git a/src/modules/systemlib/CMakeLists.txt b/src/modules/systemlib/CMakeLists.txt
index 3fadc26da3..4507974e85 100644
--- a/src/modules/systemlib/CMakeLists.txt
+++ b/src/modules/systemlib/CMakeLists.txt
@@ -80,8 +80,6 @@ px4_add_module(
 	MODULE modules__systemlib
 	COMPILE_FLAGS
 		-Wno-sign-compare
-		-Wno-attributes
-		-Wno-packed
 		-Os
 	SRCS ${SRCS}
 	DEPENDS
diff --git a/src/modules/systemlib/param/param.c b/src/modules/systemlib/param/param.c
index 1973704738..02e384e0e5 100644
--- a/src/modules/systemlib/param/param.c
+++ b/src/modules/systemlib/param/param.c
@@ -173,7 +173,7 @@ param_assert_locked(void)
 static bool
 handle_in_range(param_t param)
 {
-	int count = get_param_info_count();
+	unsigned count = get_param_info_count();
 	return (count && param < count);
 }
 
diff --git a/src/modules/uavcan/CMakeLists.txt b/src/modules/uavcan/CMakeLists.txt
index c485fc8da6..8aea69743b 100644
--- a/src/modules/uavcan/CMakeLists.txt
+++ b/src/modules/uavcan/CMakeLists.txt
@@ -59,9 +59,6 @@ px4_add_module(
 	STACK_MAIN 3200
 	STACK_MAX 1500
 	COMPILE_FLAGS
-		-Wno-attributes
-		-Wno-packed
-		-Wno-deprecated-declarations
 		-Os
 	SRCS
 		# Main
diff --git a/src/platforms/posix/drivers/accelsim/CMakeLists.txt b/src/platforms/posix/drivers/accelsim/CMakeLists.txt
index d4146b8129..253fd3c62d 100644
--- a/src/platforms/posix/drivers/accelsim/CMakeLists.txt
+++ b/src/platforms/posix/drivers/accelsim/CMakeLists.txt
@@ -34,7 +34,6 @@ px4_add_module(
 	MODULE platforms__posix__drivers__accelsim
 	MAIN accelsim
 	COMPILE_FLAGS
-		-Wno-packed
 	SRCS
 		accelsim.cpp
 	DEPENDS
diff --git a/src/platforms/posix/drivers/airspeedsim/CMakeLists.txt b/src/platforms/posix/drivers/airspeedsim/CMakeLists.txt
index cecefb204e..8c2a410862 100644
--- a/src/platforms/posix/drivers/airspeedsim/CMakeLists.txt
+++ b/src/platforms/posix/drivers/airspeedsim/CMakeLists.txt
@@ -34,7 +34,6 @@ px4_add_module(
 	MODULE platforms__posix__drivers__airspeedsim
 	MAIN measairspeedsim
 	COMPILE_FLAGS
-		-Wno-packed
 		-Os
 	SRCS
 		airspeedsim.cpp
diff --git a/src/platforms/posix/drivers/barosim/CMakeLists.txt b/src/platforms/posix/drivers/barosim/CMakeLists.txt
index 6103737bfb..6e3a105204 100644
--- a/src/platforms/posix/drivers/barosim/CMakeLists.txt
+++ b/src/platforms/posix/drivers/barosim/CMakeLists.txt
@@ -34,7 +34,6 @@ px4_add_module(
 	MODULE platforms__posix__drivers__barosim
 	MAIN barosim
 	COMPILE_FLAGS
-		-Wno-packed
 		-Os
 	SRCS
 		baro.cpp
diff --git a/src/platforms/posix/drivers/gpssim/CMakeLists.txt b/src/platforms/posix/drivers/gpssim/CMakeLists.txt
index 60cd2de7a8..2c580d7757 100644
--- a/src/platforms/posix/drivers/gpssim/CMakeLists.txt
+++ b/src/platforms/posix/drivers/gpssim/CMakeLists.txt
@@ -34,7 +34,6 @@ px4_add_module(
 	MODULE platforms__posix__drivers__gpssim
 	MAIN gpssim
 	COMPILE_FLAGS
-		-Wno-packed
 		-Os
 	SRCS
 		gpssim.cpp
diff --git a/src/platforms/posix/drivers/gyrosim/CMakeLists.txt b/src/platforms/posix/drivers/gyrosim/CMakeLists.txt
index 88571e41b6..bac977afbb 100644
--- a/src/platforms/posix/drivers/gyrosim/CMakeLists.txt
+++ b/src/platforms/posix/drivers/gyrosim/CMakeLists.txt
@@ -35,7 +35,6 @@ px4_add_module(
 	MAIN gyrosim
 	STACK_MAIN 1200
 	COMPILE_FLAGS
-		-Wno-packed
 		-Os
 	SRCS
 		gyrosim.cpp
diff --git a/src/platforms/posix/px4_layer/CMakeLists.txt b/src/platforms/posix/px4_layer/CMakeLists.txt
index abccca4c59..d80b6083ad 100644
--- a/src/platforms/posix/px4_layer/CMakeLists.txt
+++ b/src/platforms/posix/px4_layer/CMakeLists.txt
@@ -43,8 +43,6 @@ px4_add_module(
 	MODULE platforms__posix__px4_layer
 	COMPILE_FLAGS
 		-Os
-		-Wno-attributes
-		-Wno-packed
 	SRCS
 		px4_posix_impl.cpp
 		px4_posix_tasks.cpp
diff --git a/src/platforms/qurt/px4_layer/CMakeLists.txt b/src/platforms/qurt/px4_layer/CMakeLists.txt
index 25ada03442..6196d1d43f 100644
--- a/src/platforms/qurt/px4_layer/CMakeLists.txt
+++ b/src/platforms/qurt/px4_layer/CMakeLists.txt
@@ -55,7 +55,6 @@ px4_add_module(
 	MODULE platforms__qurt__px4_layer
 	COMPILE_FLAGS
 		-Os
-		-Wno-packed
 	SRCS
 		${QURT_LAYER_SRCS}
 		${CONFIG_SRC}
diff --git a/src/systemcmds/mtd/CMakeLists.txt b/src/systemcmds/mtd/CMakeLists.txt
index 7c67196736..2a1f73e401 100644
--- a/src/systemcmds/mtd/CMakeLists.txt
+++ b/src/systemcmds/mtd/CMakeLists.txt
@@ -34,7 +34,6 @@ px4_add_module(
 	MODULE systemcmds__mtd
 	MAIN mtd
 	COMPILE_FLAGS
-		-Wno-error
 		-Os
 	SRCS
 		mtd.c
diff --git a/src/systemcmds/tests/CMakeLists.txt b/src/systemcmds/tests/CMakeLists.txt
index 60f9cc885d..50365c62b3 100644
--- a/src/systemcmds/tests/CMakeLists.txt
+++ b/src/systemcmds/tests/CMakeLists.txt
@@ -70,10 +70,10 @@ px4_add_module(
 	MODULE systemcmds__tests
 	MAIN tests
 	STACK_MAIN 9000
+	STACK_MAX 9000
 	COMPILE_FLAGS
-		-Wframe-larger-than=8000
 		-Wno-float-equal
-		-O0
+		-Os
 	SRCS ${srcs}
 	DEPENDS
 		platforms__common

From 7cf8b3ea1b64de4c2b1aa396b203da2bdaba8ea1 Mon Sep 17 00:00:00 2001
From: Daniel Agar <daniel@agar.ca>
Date: Thu, 19 May 2016 00:19:53 -0400
Subject: [PATCH 0256/1230] sync posix_sitl_default and posix_sitl_test

---
 cmake/configs/posix_sitl_default.cmake | 12 ++++++++++++
 cmake/configs/posix_sitl_test.cmake    |  3 ++-
 src/systemcmds/tests/CMakeLists.txt    |  1 +
 src/systemcmds/tests/test_file.c       | 13 +++++++------
 src/systemcmds/tests/test_mount.c      | 13 +++++++------
 5 files changed, 29 insertions(+), 13 deletions(-)

diff --git a/cmake/configs/posix_sitl_default.cmake b/cmake/configs/posix_sitl_default.cmake
index e97d62e3a2..b5343ff73d 100644
--- a/cmake/configs/posix_sitl_default.cmake
+++ b/cmake/configs/posix_sitl_default.cmake
@@ -57,6 +57,7 @@ set(config_module_list
 	modules/systemlib/mixer
 	modules/uORB
 	modules/vtol_att_control
+
 	lib/controllib
 	lib/conversion
 	lib/DriverFramework/framework
@@ -72,6 +73,17 @@ set(config_module_list
 	lib/terrain_estimation
 
 	examples/px4_simple_app
+
+	#
+	# Testing
+	#
+	modules/commander/commander_tests
+	modules/controllib_test
+	#modules/mavlink/mavlink_tests
+	modules/unit_test
+	modules/uORB/uORB_tests
+	systemcmds/tests
+
 	)
 
 set(config_extra_builtin_cmds
diff --git a/cmake/configs/posix_sitl_test.cmake b/cmake/configs/posix_sitl_test.cmake
index 22e59e9425..3237c7b16e 100644
--- a/cmake/configs/posix_sitl_test.cmake
+++ b/cmake/configs/posix_sitl_test.cmake
@@ -49,6 +49,7 @@ set(config_module_list
 	modules/navigator
 	modules/param
 	modules/position_estimator_inav
+	modules/local_position_estimator
 	modules/sdlog2
 	modules/sensors
 	modules/simulator
@@ -72,7 +73,7 @@ set(config_module_list
 	lib/terrain_estimation
 
 	examples/px4_simple_app
-	
+
 	#
 	# Testing
 	#
diff --git a/src/systemcmds/tests/CMakeLists.txt b/src/systemcmds/tests/CMakeLists.txt
index 50365c62b3..bbd989058c 100644
--- a/src/systemcmds/tests/CMakeLists.txt
+++ b/src/systemcmds/tests/CMakeLists.txt
@@ -72,6 +72,7 @@ px4_add_module(
 	STACK_MAIN 9000
 	STACK_MAX 9000
 	COMPILE_FLAGS
+		-Wno-unused-result
 		-Wno-float-equal
 		-Os
 	SRCS ${srcs}
diff --git a/src/systemcmds/tests/test_file.c b/src/systemcmds/tests/test_file.c
index bdb089e405..fb081e27b3 100644
--- a/src/systemcmds/tests/test_file.c
+++ b/src/systemcmds/tests/test_file.c
@@ -39,13 +39,14 @@
  * @author Lorenz Meier <lm@inf.ethz.ch>
  */
 
+#include <px4_config.h>
+#include <px4_posix.h>
+
 #include <sys/stat.h>
 #include <poll.h>
 #include <dirent.h>
 #include <stdio.h>
 #include <stddef.h>
-#include <unistd.h>
-#include <fcntl.h>
 #include <systemlib/err.h>
 #include <systemlib/perf_counter.h>
 #include <string.h>
@@ -125,7 +126,7 @@ test_file(int argc, char *argv[])
 			uint8_t read_buf[chunk_sizes[c] + alignments] __attribute__((aligned(64)));
 			hrt_abstime start, end;
 
-			int fd = open(PX4_ROOTFSDIR "/fs/microsd/testfile", O_TRUNC | O_WRONLY | O_CREAT);
+			int fd = px4_open(PX4_ROOTFSDIR "/fs/microsd/testfile", O_TRUNC | O_WRONLY | O_CREAT);
 
 			warnx("testing unaligned writes - please wait..");
 
@@ -156,7 +157,7 @@ test_file(int argc, char *argv[])
 
 			warnx("write took %" PRIu64 " us", (end - start));
 
-			close(fd);
+			px4_close(fd);
 			fd = open(PX4_ROOTFSDIR "/fs/microsd/testfile", O_RDONLY);
 
 			/* read back data for validation */
@@ -196,7 +197,7 @@ test_file(int argc, char *argv[])
 
 			close(fd);
 			int ret = unlink(PX4_ROOTFSDIR "/fs/microsd/testfile");
-			fd = open(PX4_ROOTFSDIR "/fs/microsd/testfile", O_TRUNC | O_WRONLY | O_CREAT);
+			fd = px4_open(PX4_ROOTFSDIR "/fs/microsd/testfile", O_TRUNC | O_WRONLY | O_CREAT);
 
 			warnx("testing aligned writes - please wait.. (CTRL^C to abort)");
 
@@ -218,7 +219,7 @@ test_file(int argc, char *argv[])
 
 			warnx("reading data aligned..");
 
-			close(fd);
+			px4_close(fd);
 			fd = open(PX4_ROOTFSDIR "/fs/microsd/testfile", O_RDONLY);
 
 			bool align_read_ok = true;
diff --git a/src/systemcmds/tests/test_mount.c b/src/systemcmds/tests/test_mount.c
index f92c5f6316..364536b973 100644
--- a/src/systemcmds/tests/test_mount.c
+++ b/src/systemcmds/tests/test_mount.c
@@ -39,12 +39,13 @@
  * @author Lorenz Meier <lm@inf.ethz.ch>
  */
 
+#include <px4_config.h>
+#include <px4_posix.h>
+
 #include <sys/stat.h>
 #include <dirent.h>
 #include <stdio.h>
 #include <stddef.h>
-#include <unistd.h>
-#include <fcntl.h>
 #include <systemlib/err.h>
 #include <systemlib/systemlib.h>
 #include <systemlib/perf_counter.h>
@@ -200,7 +201,7 @@ test_mount(int argc, char *argv[])
 
 			uint8_t read_buf[chunk_sizes[c] + alignments] __attribute__((aligned(64)));
 
-			int fd = open(PX4_ROOTFSDIR "/fs/microsd/testfile", O_TRUNC | O_WRONLY | O_CREAT);
+			int fd = px4_open(PX4_ROOTFSDIR "/fs/microsd/testfile", O_TRUNC | O_WRONLY | O_CREAT);
 
 			for (unsigned i = 0; i < iterations; i++) {
 
@@ -236,8 +237,8 @@ test_mount(int argc, char *argv[])
 			fsync(fileno(stderr));
 			usleep(200000);
 
-			close(fd);
-			fd = open(PX4_ROOTFSDIR "/fs/microsd/testfile", O_RDONLY);
+			px4_close(fd);
+			fd = px4_open(PX4_ROOTFSDIR "/fs/microsd/testfile", O_RDONLY);
 
 			/* read back data for validation */
 			for (unsigned i = 0; i < iterations; i++) {
@@ -267,7 +268,7 @@ test_mount(int argc, char *argv[])
 			}
 
 			int ret = unlink(PX4_ROOTFSDIR "/fs/microsd/testfile");
-			close(fd);
+			px4_close(fd);
 
 			if (ret) {
 				PX4_ERR("UNLINKING FILE FAILED");

From b0b2cfaa654f62aa5aa6aff54a0162f7de54b479 Mon Sep 17 00:00:00 2001
From: Daniel Agar <daniel@agar.ca>
Date: Thu, 19 May 2016 00:21:23 -0400
Subject: [PATCH 0257/1230] cmake nuttx stackcheck fix for current master

---
 cmake/nuttx/px4_impl_nuttx.cmake | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/cmake/nuttx/px4_impl_nuttx.cmake b/cmake/nuttx/px4_impl_nuttx.cmake
index 79b829881e..35b6c0b8bf 100644
--- a/cmake/nuttx/px4_impl_nuttx.cmake
+++ b/cmake/nuttx/px4_impl_nuttx.cmake
@@ -204,7 +204,7 @@ function(px4_nuttx_add_export)
 
 	# Read defconfig to see if CONFIG_ARMV7M_STACKCHECK is yes 
 	# note: CONFIG will be BOARD in the future evaluation of ${hw_stack_check_${CONFIG}
-	file(STRINGS "${CMAKE_SOURCE_DIR}/nuttx-configs/${CONFIG}/${config_nuttx_config}/defconfig"
+	file(STRINGS "${CMAKE_SOURCE_DIR}/nuttx-configs/${CONFIG}/nsh/defconfig"
 		hw_stack_check_${CONFIG}
 		REGEX "CONFIG_ARMV7M_STACKCHECK=y"
 		)

From 206d5a7afcca2b0d28d9ec16b5c65c37a0b619e4 Mon Sep 17 00:00:00 2001
From: Daniel Agar <daniel@agar.ca>
Date: Thu, 19 May 2016 11:41:48 -0400
Subject: [PATCH 0258/1230] sync px4fmu-v2_default and px4fmu-v2_test

---
 cmake/configs/nuttx_px4fmu-v2_default.cmake | 2 ++
 cmake/configs/nuttx_px4fmu-v2_test.cmake    | 4 +++-
 2 files changed, 5 insertions(+), 1 deletion(-)

diff --git a/cmake/configs/nuttx_px4fmu-v2_default.cmake b/cmake/configs/nuttx_px4fmu-v2_default.cmake
index 8af384e7ba..cf17ffce66 100644
--- a/cmake/configs/nuttx_px4fmu-v2_default.cmake
+++ b/cmake/configs/nuttx_px4fmu-v2_default.cmake
@@ -66,6 +66,8 @@ set(config_module_list
 	systemcmds/mtd
 	systemcmds/dumpfile
 	systemcmds/ver
+	#systemcmds/sd_bench
+	#systemcmds/tests
 
 	#
 	# General system control
diff --git a/cmake/configs/nuttx_px4fmu-v2_test.cmake b/cmake/configs/nuttx_px4fmu-v2_test.cmake
index 4a0d2e1269..27bdf6ea8a 100644
--- a/cmake/configs/nuttx_px4fmu-v2_test.cmake
+++ b/cmake/configs/nuttx_px4fmu-v2_test.cmake
@@ -67,6 +67,8 @@ set(config_module_list
 	systemcmds/mtd
 	systemcmds/dumpfile
 	systemcmds/ver
+	systemcmds/sd_bench
+	systemcmds/tests
 
 	#
 	# Testing
@@ -77,7 +79,6 @@ set(config_module_list
 	modules/mavlink/mavlink_tests
 	modules/unit_test
 	modules/uORB/uORB_tests
-	systemcmds/tests
 
 	#
 	# General system control
@@ -111,6 +112,7 @@ set(config_module_list
 	#
 	# Logging
 	#
+	#modules/logger
 	#modules/sdlog2
 
 	#

From 1ecdb0f6fb8d2a3d222b31cc3380399c67deee4c Mon Sep 17 00:00:00 2001
From: Daniel Agar <daniel@agar.ca>
Date: Thu, 19 May 2016 11:42:36 -0400
Subject: [PATCH 0259/1230] adjust stack sizes

---
 src/drivers/mpu6000/CMakeLists.txt                 | 2 +-
 src/drivers/ms5611/CMakeLists.txt                  | 1 +
 src/drivers/pwm_out_sim/CMakeLists.txt             | 1 +
 src/drivers/pwm_out_sim/pwm_out_sim.cpp            | 2 +-
 src/modules/fw_att_control/fw_att_control_main.cpp | 2 +-
 src/modules/mavlink/mavlink_main.cpp               | 2 +-
 src/modules/navigator/CMakeLists.txt               | 2 +-
 src/modules/sensors/CMakeLists.txt                 | 2 +-
 src/modules/sensors/sensors.cpp                    | 2 +-
 src/modules/uORB/CMakeLists.txt                    | 2 +-
 10 files changed, 10 insertions(+), 8 deletions(-)

diff --git a/src/drivers/mpu6000/CMakeLists.txt b/src/drivers/mpu6000/CMakeLists.txt
index c4cabd418d..13009f31b0 100644
--- a/src/drivers/mpu6000/CMakeLists.txt
+++ b/src/drivers/mpu6000/CMakeLists.txt
@@ -33,7 +33,7 @@
 px4_add_module(
 	MODULE drivers__mpu6000
 	MAIN mpu6000
-	STACK_MAIN 1200
+	STACK_MAIN 1300
 	COMPILE_FLAGS
 		-Weffc++
 		-Os
diff --git a/src/drivers/ms5611/CMakeLists.txt b/src/drivers/ms5611/CMakeLists.txt
index 7359bd9507..1cfa739a2e 100644
--- a/src/drivers/ms5611/CMakeLists.txt
+++ b/src/drivers/ms5611/CMakeLists.txt
@@ -50,6 +50,7 @@ endif()
 px4_add_module(
 	MODULE drivers__ms5611
 	MAIN ms5611
+	STACK_MAIN 1200
 	COMPILE_FLAGS
 		-Os
 	SRCS ${srcs}
diff --git a/src/drivers/pwm_out_sim/CMakeLists.txt b/src/drivers/pwm_out_sim/CMakeLists.txt
index d12a5fdfcf..573d9a7afb 100644
--- a/src/drivers/pwm_out_sim/CMakeLists.txt
+++ b/src/drivers/pwm_out_sim/CMakeLists.txt
@@ -34,6 +34,7 @@ px4_add_module(
 	MODULE drivers__pwm_out_sim
 	MAIN pwm_out_sim
 	STACK_MAIN 1200
+	STACK_MAX 1200
 	COMPILE_FLAGS
 		-Os
 	SRCS
diff --git a/src/drivers/pwm_out_sim/pwm_out_sim.cpp b/src/drivers/pwm_out_sim/pwm_out_sim.cpp
index 20f9618c98..7be5a82e60 100644
--- a/src/drivers/pwm_out_sim/pwm_out_sim.cpp
+++ b/src/drivers/pwm_out_sim/pwm_out_sim.cpp
@@ -252,7 +252,7 @@ PWMSim::init()
 	_task = px4_task_spawn_cmd("pwm_out_sim",
 				   SCHED_DEFAULT,
 				   SCHED_PRIORITY_DEFAULT,
-				   1000,
+				   1200,
 				   (px4_main_t)&PWMSim::task_main_trampoline,
 				   nullptr);
 
diff --git a/src/modules/fw_att_control/fw_att_control_main.cpp b/src/modules/fw_att_control/fw_att_control_main.cpp
index 16d1376b38..b0f3adf506 100644
--- a/src/modules/fw_att_control/fw_att_control_main.cpp
+++ b/src/modules/fw_att_control/fw_att_control_main.cpp
@@ -1302,7 +1302,7 @@ FixedwingAttitudeControl::start()
 	_control_task = px4_task_spawn_cmd("fw_att_control",
 					   SCHED_DEFAULT,
 					   SCHED_PRIORITY_MAX - 5,
-					   1300,
+					   1400,
 					   (px4_main_t)&FixedwingAttitudeControl::task_main_trampoline,
 					   nullptr);
 
diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp
index 417975faab..349f670059 100644
--- a/src/modules/mavlink/mavlink_main.cpp
+++ b/src/modules/mavlink/mavlink_main.cpp
@@ -2286,7 +2286,7 @@ Mavlink::start(int argc, char *argv[])
 	px4_task_spawn_cmd(buf,
 			   SCHED_DEFAULT,
 			   SCHED_PRIORITY_DEFAULT,
-			   2800,
+			   3000,
 			   (px4_main_t)&Mavlink::start_helper,
 			   (char *const *)argv);
 
diff --git a/src/modules/navigator/CMakeLists.txt b/src/modules/navigator/CMakeLists.txt
index 89530ca119..18d41d79ca 100644
--- a/src/modules/navigator/CMakeLists.txt
+++ b/src/modules/navigator/CMakeLists.txt
@@ -33,7 +33,7 @@
 px4_add_module(
 	MODULE modules__navigator
 	MAIN navigator
-	STACK_MAIN 1200
+	STACK_MAIN 1300
 	COMPILE_FLAGS
 		-Os
 	SRCS
diff --git a/src/modules/sensors/CMakeLists.txt b/src/modules/sensors/CMakeLists.txt
index e26df79246..0b5993a9ea 100644
--- a/src/modules/sensors/CMakeLists.txt
+++ b/src/modules/sensors/CMakeLists.txt
@@ -35,7 +35,7 @@ px4_add_module(
 	MODULE modules__sensors
 	MAIN sensors
 	PRIORITY "SCHED_PRIORITY_MAX-5"
-	STACK_MAIN 1300
+	STACK_MAIN 2000
 	COMPILE_FLAGS
 		-O3
 	SRCS
diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp
index cbc3e062ac..a84552c93b 100644
--- a/src/modules/sensors/sensors.cpp
+++ b/src/modules/sensors/sensors.cpp
@@ -2276,7 +2276,7 @@ Sensors::start()
 	_sensors_task = px4_task_spawn_cmd("sensors",
 					   SCHED_DEFAULT,
 					   SCHED_PRIORITY_MAX - 5,
-					   2000,
+					   2200,
 					   (px4_main_t)&Sensors::task_main_trampoline,
 					   nullptr);
 
diff --git a/src/modules/uORB/CMakeLists.txt b/src/modules/uORB/CMakeLists.txt
index c8180c29f8..508318c7d1 100644
--- a/src/modules/uORB/CMakeLists.txt
+++ b/src/modules/uORB/CMakeLists.txt
@@ -68,7 +68,7 @@ endif()
 px4_add_module(
 	MODULE modules__uORB
 	MAIN uorb
-	STACK_MAIN 2048
+	STACK_MAIN 2100
 	COMPILE_FLAGS
 		-Os
 	SRCS ${SRCS}

From 464e2f14c19f207d5da77dcdae76e095abb2a2e5 Mon Sep 17 00:00:00 2001
From: Daniel Agar <daniel@agar.ca>
Date: Thu, 19 May 2016 12:24:19 -0400
Subject: [PATCH 0260/1230] mavlink remove clang warning/error

---
 src/modules/mavlink/mavlink_receiver.cpp | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp
index f5ec194177..5ee0312fdc 100644
--- a/src/modules/mavlink/mavlink_receiver.cpp
+++ b/src/modules/mavlink/mavlink_receiver.cpp
@@ -1109,7 +1109,7 @@ void
 MavlinkReceiver::handle_message_radio_status(mavlink_message_t *msg)
 {
 	/* telemetry status supported only on first ORB_MULTI_MAX_INSTANCES mavlink channels */
-	if (_mavlink->get_channel() < ORB_MULTI_MAX_INSTANCES) {
+	if (_mavlink->get_channel() < (mavlink_channel_t)ORB_MULTI_MAX_INSTANCES) {
 		mavlink_radio_status_t rstatus;
 		mavlink_msg_radio_status_decode(msg, &rstatus);
 
@@ -1319,7 +1319,7 @@ void
 MavlinkReceiver::handle_message_heartbeat(mavlink_message_t *msg)
 {
 	/* telemetry status supported only on first TELEMETRY_STATUS_ORB_ID_NUM mavlink channels */
-	if (_mavlink->get_channel() < ORB_MULTI_MAX_INSTANCES) {
+	if (_mavlink->get_channel() < (mavlink_channel_t)ORB_MULTI_MAX_INSTANCES) {
 		mavlink_heartbeat_t hb;
 		mavlink_msg_heartbeat_decode(msg, &hb);
 

From 0afcfd9fab3a38d592c7848f5619101aebcff9b5 Mon Sep 17 00:00:00 2001
From: Daniel Agar <daniel@agar.ca>
Date: Thu, 19 May 2016 12:39:58 -0400
Subject: [PATCH 0261/1230] px4fmu-v4 add lpe

---
 cmake/configs/nuttx_px4fmu-v4_default.cmake | 3 +--
 1 file changed, 1 insertion(+), 2 deletions(-)

diff --git a/cmake/configs/nuttx_px4fmu-v4_default.cmake b/cmake/configs/nuttx_px4fmu-v4_default.cmake
index 8a3d8f39f3..f40f25104a 100644
--- a/cmake/configs/nuttx_px4fmu-v4_default.cmake
+++ b/cmake/configs/nuttx_px4fmu-v4_default.cmake
@@ -83,12 +83,11 @@ set(config_module_list
 	#
 	# Estimation modules (EKF/ SO3 / other filters)
 	#
-	# Too high RAM usage due to static allocations
-	# modules/attitude_estimator_ekf
 	modules/attitude_estimator_q
 	modules/ekf_att_pos_estimator
 	modules/position_estimator_inav
 	modules/ekf2
+	modules/local_position_estimator
 
 	#
 	# Vehicle Control

From 5da9e7e65315bf11f259eb82c301bf8ecc1809cf Mon Sep 17 00:00:00 2001
From: Daniel Agar <daniel@agar.ca>
Date: Thu, 5 May 2016 22:30:56 -0400
Subject: [PATCH 0262/1230] cmake nuttx copy with rsync

---
 cmake/nuttx/px4_impl_nuttx.cmake | 15 +++------------
 1 file changed, 3 insertions(+), 12 deletions(-)

diff --git a/cmake/nuttx/px4_impl_nuttx.cmake b/cmake/nuttx/px4_impl_nuttx.cmake
index 35b6c0b8bf..b9112a4c94 100644
--- a/cmake/nuttx/px4_impl_nuttx.cmake
+++ b/cmake/nuttx/px4_impl_nuttx.cmake
@@ -213,11 +213,11 @@ function(px4_nuttx_add_export)
 	endif()
 
 	# copy and export
+	file(RELATIVE_PATH nuttx_cp_src ${CMAKE_BINARY_DIR} ${CMAKE_SOURCE_DIR}/NuttX)
 	file(GLOB_RECURSE config_files ${CMAKE_SOURCE_DIR}/nuttx-configs/${CONFIG}/*)
 	add_custom_command(OUTPUT ${CMAKE_BINARY_DIR}/${CONFIG}.export
 		COMMAND ${MKDIR} -p ${nuttx_src}
-		COMMAND ${CP} -a ${CMAKE_SOURCE_DIR}/NuttX/. ${nuttx_src}/
-		COMMAND ${RM} -rf ${nuttx_src}/.git
+		COMMAND rsync -a --delete --exclude=.git ${nuttx_cp_src}/ ${CONFIG}/NuttX/
 		#COMMAND ${ECHO} Configuring NuttX for ${CONFIG}
 		COMMAND ${MAKE} --no-print-directory -C${nuttx_src}/nuttx -r --quiet distclean
 		COMMAND ${CP} -r ${CMAKE_SOURCE_DIR}/nuttx-configs/PX4_Warnings.mk ${nuttx_src}/nuttx/
@@ -227,6 +227,7 @@ function(px4_nuttx_add_export)
 		COMMAND ${MAKE} --no-print-directory --quiet -C ${nuttx_src}/nuttx -j${THREADS} -r CONFIG_ARCH_BOARD=${CONFIG} export > nuttx_build.log
 		COMMAND ${CP} -r ${nuttx_src}/nuttx/nuttx-export.zip ${CMAKE_BINARY_DIR}/${CONFIG}.export
 		DEPENDS ${config_files} ${DEPENDS}
+		WORKING_DIRECTORY ${CMAKE_BINARY_DIR}
 		COMMENT "Building NuttX for ${CONFIG}")
 
 	# extract
@@ -438,16 +439,6 @@ function(px4_os_add_flags)
 
 	set(added_exe_linker_flags) # none currently
 
-	set(instrument_flags)
-	if ("${config_nuttx_hw_stack_check_${BOARD}}" STREQUAL "y")
-		set(instrument_flags
-			-finstrument-functions
-			-ffixed-r10
-			)
-		list(APPEND c_flags ${instrument_flags})
-		list(APPEND cxx_flags ${instrument_flags})
-	endif()
-
 	set(cpu_flags)
 	if (${BOARD} STREQUAL "px4fmu-v1")
 		set(cpu_flags

From 761d02d4337e248c038d47a93101d1cdfc5b5e35 Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Thu, 19 May 2016 22:01:12 +0200
Subject: [PATCH 0263/1230] Sensors: return stack handler to default stack size

---
 src/modules/sensors/CMakeLists.txt | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/src/modules/sensors/CMakeLists.txt b/src/modules/sensors/CMakeLists.txt
index 0b5993a9ea..b373b18173 100644
--- a/src/modules/sensors/CMakeLists.txt
+++ b/src/modules/sensors/CMakeLists.txt
@@ -35,7 +35,7 @@ px4_add_module(
 	MODULE modules__sensors
 	MAIN sensors
 	PRIORITY "SCHED_PRIORITY_MAX-5"
-	STACK_MAIN 2000
+	STACK_MAIN 1200
 	COMPILE_FLAGS
 		-O3
 	SRCS

From c626f5b0015c0e17b42a0eae7d2c95d2817f98df Mon Sep 17 00:00:00 2001
From: Mark Whitehorn <kd0aij@gmail.com>
Date: Thu, 19 May 2016 11:13:44 -0600
Subject: [PATCH 0264/1230] shorten pwm init and ramp times

---
 src/modules/systemlib/pwm_limit/pwm_limit.h | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/src/modules/systemlib/pwm_limit/pwm_limit.h b/src/modules/systemlib/pwm_limit/pwm_limit.h
index 23adc9ee3a..644059e5a4 100644
--- a/src/modules/systemlib/pwm_limit/pwm_limit.h
+++ b/src/modules/systemlib/pwm_limit/pwm_limit.h
@@ -51,11 +51,11 @@ __BEGIN_DECLS
  * time for the ESCs to initialize
  * (this is not actually needed if PWM is sent right after boot)
  */
-#define INIT_TIME_US 500000
+#define INIT_TIME_US 50000
 /*
  * time to slowly ramp up the ESCs
  */
-#define RAMP_TIME_US 2500000
+#define RAMP_TIME_US 500000
 
 enum pwm_limit_state {
 	PWM_LIMIT_STATE_OFF = 0,

From 87e5499ad2fea2329a9a40ddfcfe49b2b454687d Mon Sep 17 00:00:00 2001
From: Mark Whitehorn <kd0aij@gmail.com>
Date: Thu, 19 May 2016 12:30:19 -0600
Subject: [PATCH 0265/1230] stop spamming console with timeout warnings

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

diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp
index a383af5d38..e097b3572b 100644
--- a/src/modules/navigator/navigator_main.cpp
+++ b/src/modules/navigator/navigator_main.cpp
@@ -339,6 +339,7 @@ Navigator::task_main()
 			/* timed out - periodic check for _task_should_exit, etc. */
 			if (global_pos_available_once) {
 				PX4_WARN("navigator timed out");
+				global_pos_available_once = false;
 			}
 			continue;
 

From 3c22c1c525d541813e747d5c988fffcecd3e794c Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Thu, 19 May 2016 22:17:41 +0200
Subject: [PATCH 0266/1230] Make navigator message a bit more informative

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

diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp
index e097b3572b..812fe49821 100644
--- a/src/modules/navigator/navigator_main.cpp
+++ b/src/modules/navigator/navigator_main.cpp
@@ -338,7 +338,7 @@ Navigator::task_main()
 		if (pret == 0) {
 			/* timed out - periodic check for _task_should_exit, etc. */
 			if (global_pos_available_once) {
-				PX4_WARN("navigator timed out");
+				PX4_WARN("no GPS - navigator timed out");
 				global_pos_available_once = false;
 			}
 			continue;

From 4267b20c9a2f041f0c687f823c103e93b07399c6 Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Fri, 20 May 2016 10:06:04 +0200
Subject: [PATCH 0267/1230] Fix Lama coax config

---
 ROMFS/px4fmu_common/init.d/15001_coax_heli | 7 +++----
 ROMFS/px4fmu_common/init.d/rcS             | 4 ++++
 2 files changed, 7 insertions(+), 4 deletions(-)

diff --git a/ROMFS/px4fmu_common/init.d/15001_coax_heli b/ROMFS/px4fmu_common/init.d/15001_coax_heli
index 97513b7049..fd10afffed 100644
--- a/ROMFS/px4fmu_common/init.d/15001_coax_heli
+++ b/ROMFS/px4fmu_common/init.d/15001_coax_heli
@@ -1,6 +1,6 @@
 #!nsh
 #
-# @name Coaxial Helicopter (such as Esky Lama v4 or Esky Big Lama)
+# @name Esky (Big) Lama v4
 #
 # @type Coaxial Helicopter
 # 
@@ -12,7 +12,8 @@
 # @maintainer Emmanuel Roussel
 #
 
-set VEHICLE_TYPE mc
+sh /etc/init.d/rc.mc_defaults
+set MIXER coax
 
 if [ $AUTOCNF == yes ]
 then
@@ -47,7 +48,6 @@ then
 	param set MC_YAWRATE_FF 0
 fi
 
-set MIXER coax
 # use PWM parameters for throttle channel
 set PWM_OUT 34
 set PWM_RATE 400
@@ -62,4 +62,3 @@ set PWM_AUX_OUT 1234
 set PWM_AUX_DISARMED 1500
 set PWM_AUX_MIN 1000
 set PWM_AUX_MAX 2000
-
diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS
index 8e64b9f4c0..997459183b 100644
--- a/ROMFS/px4fmu_common/init.d/rcS
+++ b/ROMFS/px4fmu_common/init.d/rcS
@@ -736,6 +736,10 @@ then
 			then
 				set MAV_TYPE 14
 			fi
+			if [ $MIXER == coax ]
+			then
+				set MAV_TYPE 3
+			fi
 		fi
 
 		# Still no MAV_TYPE found

From 7badf645b30a9b93655dde7788fdcf70bbc1bf47 Mon Sep 17 00:00:00 2001
From: Daniel Agar <daniel@agar.ca>
Date: Fri, 20 May 2016 21:17:54 -0400
Subject: [PATCH 0268/1230] fix missing stack check instrument_flags (#4593)

---
 cmake/nuttx/px4_impl_nuttx.cmake | 10 ++++++++++
 1 file changed, 10 insertions(+)

diff --git a/cmake/nuttx/px4_impl_nuttx.cmake b/cmake/nuttx/px4_impl_nuttx.cmake
index b9112a4c94..8ceddfe44a 100644
--- a/cmake/nuttx/px4_impl_nuttx.cmake
+++ b/cmake/nuttx/px4_impl_nuttx.cmake
@@ -439,6 +439,16 @@ function(px4_os_add_flags)
 
 	set(added_exe_linker_flags) # none currently
 
+	set(instrument_flags)
+	if ("${config_nuttx_hw_stack_check_${BOARD}}" STREQUAL "y")
+		set(instrument_flags
+			-finstrument-functions
+			-ffixed-r10
+			)
+		list(APPEND c_flags ${instrument_flags})
+		list(APPEND cxx_flags ${instrument_flags})
+	endif()
+
 	set(cpu_flags)
 	if (${BOARD} STREQUAL "px4fmu-v1")
 		set(cpu_flags

From 1b77ec7f8207b3adfb6b9f248012e6c8504f78c0 Mon Sep 17 00:00:00 2001
From: David Sidrane <david_s5@nscdg.com>
Date: Fri, 20 May 2016 10:39:14 -1000
Subject: [PATCH 0269/1230] Decouple board name

---
 src/drivers/boards/aerocore/CMakeLists.txt    |  1 +
 src/drivers/boards/aerocore/board_config.h    |  1 +
 src/drivers/boards/common/board_name.c        | 71 +++++++++++++++++++
 src/drivers/boards/mindpx-v2/CMakeLists.txt   |  1 +
 src/drivers/boards/mindpx-v2/board_config.h   |  2 +
 .../px4-stm32f4discovery/CMakeLists.txt       |  1 +
 .../px4-stm32f4discovery/board_config.h       |  2 +
 src/drivers/boards/px4fmu-v1/CMakeLists.txt   |  1 +
 src/drivers/boards/px4fmu-v1/board_config.h   |  3 +
 src/drivers/boards/px4fmu-v2/CMakeLists.txt   |  1 +
 src/drivers/boards/px4fmu-v2/board_config.h   |  2 +
 src/drivers/boards/px4fmu-v4/CMakeLists.txt   |  1 +
 src/drivers/boards/px4fmu-v4/board_config.h   |  3 +
 src/lib/version/version.h                     | 58 ++++++---------
 14 files changed, 110 insertions(+), 38 deletions(-)
 create mode 100644 src/drivers/boards/common/board_name.c

diff --git a/src/drivers/boards/aerocore/CMakeLists.txt b/src/drivers/boards/aerocore/CMakeLists.txt
index f1fb63a255..23be262b75 100644
--- a/src/drivers/boards/aerocore/CMakeLists.txt
+++ b/src/drivers/boards/aerocore/CMakeLists.txt
@@ -35,6 +35,7 @@ px4_add_module(
 	COMPILE_FLAGS
 		-Os
 	SRCS
+		../common/board_name.c
 		aerocore_init.c
 		aerocore_pwm_servo.c
 		aerocore_spi.c
diff --git a/src/drivers/boards/aerocore/board_config.h b/src/drivers/boards/aerocore/board_config.h
index d89b383c62..3046839fb5 100644
--- a/src/drivers/boards/aerocore/board_config.h
+++ b/src/drivers/boards/aerocore/board_config.h
@@ -172,6 +172,7 @@ __BEGIN_DECLS
 #define GPIO_TONE_ALARM_IDLE	(GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTA|GPIO_PIN0)
 #define GPIO_TONE_ALARM		(GPIO_ALT|GPIO_AF1|GPIO_SPEED_2MHz|GPIO_PUSHPULL|GPIO_PORTA|GPIO_PIN0)
 
+#define BOARD_NAME "AEROCORE"
 
 /****************************************************************************************************
  * Public Types
diff --git a/src/drivers/boards/common/board_name.c b/src/drivers/boards/common/board_name.c
new file mode 100644
index 0000000000..8108cec4aa
--- /dev/null
+++ b/src/drivers/boards/common/board_name.c
@@ -0,0 +1,71 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2016 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file board_name.c
+ *
+ * Provide the board_name interface.
+ */
+
+/************************************************************************************
+ * Included Files
+ ************************************************************************************/
+
+#include <px4_config.h>
+#include "board_config.h"
+#include <lib/version/version.h>
+
+/************************************************************************************
+ * Definitions
+ ************************************************************************************/
+
+/************************************************************************************
+ * Private Functions
+ ************************************************************************************/
+
+/************************************************************************************
+ * Public Functions
+ ************************************************************************************/
+
+/************************************************************************************
+ * Name: board_name
+ *
+ * Description:
+ *   All boards must provide this API to return the board name.
+ *
+ ************************************************************************************/
+
+__EXPORT const char *board_name()
+{
+	return BOARD_NAME;
+}
diff --git a/src/drivers/boards/mindpx-v2/CMakeLists.txt b/src/drivers/boards/mindpx-v2/CMakeLists.txt
index 306c3ab0b0..e64b05469e 100644
--- a/src/drivers/boards/mindpx-v2/CMakeLists.txt
+++ b/src/drivers/boards/mindpx-v2/CMakeLists.txt
@@ -35,6 +35,7 @@ px4_add_module(
 	COMPILE_FLAGS
 		-Os
 	SRCS
+		../common/board_name.c
 		mindpx_can.c
 		mindpx2_init.c
 		mindpx_timer_config.c
diff --git a/src/drivers/boards/mindpx-v2/board_config.h b/src/drivers/boards/mindpx-v2/board_config.h
index b07949e1a6..069b449d60 100644
--- a/src/drivers/boards/mindpx-v2/board_config.h
+++ b/src/drivers/boards/mindpx-v2/board_config.h
@@ -290,6 +290,8 @@ __BEGIN_DECLS
 #define PWMIN_TIMER_CHANNEL	1
 #define GPIO_PWM_IN		GPIO_TIM3_CH1IN_3
 
+#define BOARD_NAME "MINDPX_V2"
+
 /****************************************************************************************************
  * Public Types
  ****************************************************************************************************/
diff --git a/src/drivers/boards/px4-stm32f4discovery/CMakeLists.txt b/src/drivers/boards/px4-stm32f4discovery/CMakeLists.txt
index e4af694e5d..ee75158403 100644
--- a/src/drivers/boards/px4-stm32f4discovery/CMakeLists.txt
+++ b/src/drivers/boards/px4-stm32f4discovery/CMakeLists.txt
@@ -33,6 +33,7 @@
 px4_add_module(
 	MODULE drivers__boards__px4-stm32f4discovery
 	SRCS
+		../common/board_name.c
 		px4discovery_init.c
 		px4discovery_usb.c
 		px4discovery_led.c
diff --git a/src/drivers/boards/px4-stm32f4discovery/board_config.h b/src/drivers/boards/px4-stm32f4discovery/board_config.h
index d06a3a317b..5673ecf6b0 100644
--- a/src/drivers/boards/px4-stm32f4discovery/board_config.h
+++ b/src/drivers/boards/px4-stm32f4discovery/board_config.h
@@ -87,6 +87,8 @@ __BEGIN_DECLS
 #define HRT_TIMER		8	/* use timer8 for the HRT */
 #define HRT_TIMER_CHANNEL	1	/* use capture/compare channel */
 
+#define BOARD_NAME "PX4_STM32F4DISCOVERY"
+
 /****************************************************************************************************
  * Public Types
  ****************************************************************************************************/
diff --git a/src/drivers/boards/px4fmu-v1/CMakeLists.txt b/src/drivers/boards/px4fmu-v1/CMakeLists.txt
index 3402556f4b..c10b3e2b29 100644
--- a/src/drivers/boards/px4fmu-v1/CMakeLists.txt
+++ b/src/drivers/boards/px4fmu-v1/CMakeLists.txt
@@ -35,6 +35,7 @@ px4_add_module(
 	COMPILE_FLAGS
 		-Os
 	SRCS
+		../common/board_name.c
 		px4fmu_can.c
 		px4fmu_init.c
 		px4fmu_timer_config.c
diff --git a/src/drivers/boards/px4fmu-v1/board_config.h b/src/drivers/boards/px4fmu-v1/board_config.h
index d1e42a8c6f..5ebdb57506 100644
--- a/src/drivers/boards/px4fmu-v1/board_config.h
+++ b/src/drivers/boards/px4fmu-v1/board_config.h
@@ -203,6 +203,9 @@ __BEGIN_DECLS
 #define HRT_PPM_CHANNEL		3	/* use capture/compare channel 3 */
 #define GPIO_PPM_IN		(GPIO_ALT|GPIO_AF1|GPIO_PULLUP|GPIO_PORTA|GPIO_PIN10)
 
+
+#define	BOARD_NAME "PX4FMU_V1"
+
 /****************************************************************************************************
  * Public Types
  ****************************************************************************************************/
diff --git a/src/drivers/boards/px4fmu-v2/CMakeLists.txt b/src/drivers/boards/px4fmu-v2/CMakeLists.txt
index 7e20979100..fe43efae75 100644
--- a/src/drivers/boards/px4fmu-v2/CMakeLists.txt
+++ b/src/drivers/boards/px4fmu-v2/CMakeLists.txt
@@ -35,6 +35,7 @@ px4_add_module(
 	COMPILE_FLAGS
 		-Os
 	SRCS
+		../common/board_name.c
 		px4fmu_can.c
 		px4fmu2_init.c
 		px4fmu_timer_config.c
diff --git a/src/drivers/boards/px4fmu-v2/board_config.h b/src/drivers/boards/px4fmu-v2/board_config.h
index d0962f13a4..daad230434 100644
--- a/src/drivers/boards/px4fmu-v2/board_config.h
+++ b/src/drivers/boards/px4fmu-v2/board_config.h
@@ -242,6 +242,8 @@ __BEGIN_DECLS
 #define PWMIN_TIMER_CHANNEL	2
 #define GPIO_PWM_IN		GPIO_TIM4_CH2IN_2
 
+#define BOARD_NAME "PX4FMU_V2"
+
 /****************************************************************************************************
  * Public Types
  ****************************************************************************************************/
diff --git a/src/drivers/boards/px4fmu-v4/CMakeLists.txt b/src/drivers/boards/px4fmu-v4/CMakeLists.txt
index 00b2b41c41..2afe449392 100644
--- a/src/drivers/boards/px4fmu-v4/CMakeLists.txt
+++ b/src/drivers/boards/px4fmu-v4/CMakeLists.txt
@@ -35,6 +35,7 @@ px4_add_module(
 	COMPILE_FLAGS
 		-Os
 	SRCS
+		../common/board_name.c
 		px4fmu_can.c
 		px4fmu_init.c
 		px4fmu_timer_config.c
diff --git a/src/drivers/boards/px4fmu-v4/board_config.h b/src/drivers/boards/px4fmu-v4/board_config.h
index a375609318..e1871185a1 100644
--- a/src/drivers/boards/px4fmu-v4/board_config.h
+++ b/src/drivers/boards/px4fmu-v4/board_config.h
@@ -272,6 +272,9 @@ __BEGIN_DECLS
 #define SPEKTRUM_RX_AS_GPIO()		stm32_configgpio(GPIO_RC_OUT)
 #define SPEKTRUM_RX_HIGH(_s)		stm32_gpiowrite(GPIO_RC_OUT, (_s))
 
+
+#define	BOARD_NAME "PX4FMU_V4"
+
 /****************************************************************************************************
  * Public Types
  ****************************************************************************************************/
diff --git a/src/lib/version/version.h b/src/lib/version/version.h
index e20bbc5c3a..6767f1b453 100644
--- a/src/lib/version/version.h
+++ b/src/lib/version/version.h
@@ -43,44 +43,26 @@
 #ifndef VERSION_H_
 #define VERSION_H_
 
-#ifdef CONFIG_ARCH_BOARD_PX4FMU_V1
-#define	HW_ARCH "PX4FMU_V1"
+/* The preferred method for publishing a board name up is to
+ * provide board_name()
+ *
+ */
+__BEGIN_DECLS
+
+__EXPORT const char *board_name(void);
+
+__END_DECLS
+
+#if defined(CONFIG_ARCH_BOARD_SITL)
+#  define	HW_ARCH "LINUXTEST"
+#elif defined(CONFIG_ARCH_BOARD_EAGLE)
+#  define	HW_ARCH "LINUXTEST"
+#elif defined(CONFIG_ARCH_BOARD_EXCELSIOR)
+#  define HW_ARCH "LINUXTEST"
+#elif defined(CONFIG_ARCH_BOARD_RPI2)
+#  define	HW_ARCH "LINUXTEST"
+#else
+#define HW_ARCH (board_name())
 #endif
 
-#ifdef CONFIG_ARCH_BOARD_PX4FMU_V2
-#define	HW_ARCH "PX4FMU_V2"
-#endif
-
-#ifdef CONFIG_ARCH_BOARD_PX4FMU_V4
-#define	HW_ARCH "PX4FMU_V4"
-#endif
-
-#ifdef CONFIG_ARCH_BOARD_AEROCORE
-#define	HW_ARCH "AEROCORE"
-#endif
-
-#ifdef CONFIG_ARCH_BOARD_MINDPX_V2
-#define HW_ARCH "MINDPX_V2"
-#endif
-
-#ifdef CONFIG_ARCH_BOARD_PX4_STM32F4DISCOVERY
-#define HW_ARCH "PX4_STM32F4DISCOVERY"
-#endif
-
-#ifdef CONFIG_ARCH_BOARD_SITL
-#define	HW_ARCH "LINUXTEST"
-#endif
-
-#ifdef CONFIG_ARCH_BOARD_EAGLE
-#define	HW_ARCH "LINUXTEST"
-#endif
-
-#ifdef CONFIG_ARCH_BOARD_EXCELSIOR
-#define HW_ARCH "LINUXTEST"
-#endif
-
-
-#ifdef CONFIG_ARCH_BOARD_RPI2
-#define	HW_ARCH "LINUXTEST"
-#endif
 #endif /* VERSION_H_ */

From 45cc79fbf77e34b58adff50de886a05d96673a2c Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Sat, 21 May 2016 10:51:38 +0200
Subject: [PATCH 0270/1230] VTOL and fixed wing: Move parameters that belong to
 the navigator into its parameter definition space

---
 .../fw_pos_control_l1_params.c                | 14 ---------
 src/modules/navigator/mission_params.c        | 30 +++++++++++++++++++
 .../vtol_att_control_params.c                 | 16 ----------
 3 files changed, 30 insertions(+), 30 deletions(-)

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 18fd945c01..42af0d36fa 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
@@ -366,20 +366,6 @@ PARAM_DEFINE_FLOAT(FW_LND_AIRSPD_SC, 1.3f);
  */
 PARAM_DEFINE_FLOAT(FW_AIRSPD_MIN, 10.0f);
 
-/**
- * Trim Airspeed
- *
- * The TECS controller tries to fly at this airspeed.
- *
- * @unit m/s
- * @min 0.0
- * @max 40
- * @decimal 1
- * @increment 0.5
- * @group FW TECS
- */
-PARAM_DEFINE_FLOAT(FW_AIRSPD_TRIM, 15.0f);
-
 /**
  * Maximum Airspeed
  *
diff --git a/src/modules/navigator/mission_params.c b/src/modules/navigator/mission_params.c
index 8f99e4258d..3cc58dd953 100644
--- a/src/modules/navigator/mission_params.c
+++ b/src/modules/navigator/mission_params.c
@@ -156,3 +156,33 @@ PARAM_DEFINE_FLOAT(MIS_YAW_TMT, -1.0f);
  * @group Mission
  */
 PARAM_DEFINE_FLOAT(MIS_YAW_ERR, 12.0f);
+
+/**
+ * Weather-vane mode landings for missions
+ *
+ * @boolean
+ * @group Mission
+ */
+PARAM_DEFINE_INT32(VT_WV_LND_EN, 0);
+
+/**
+ * Weather-vane mode for loiter mode
+ *
+ * @boolean
+ * @group Mission
+ */
+PARAM_DEFINE_INT32(VT_WV_LTR_EN, 0);
+
+/**
+ * Cruise Airspeed
+ *
+ * The fixed wing controller tries to fly at this airspeed.
+ *
+ * @unit m/s
+ * @min 0.0
+ * @max 40
+ * @decimal 1
+ * @increment 0.5
+ * @group FW TECS
+ */
+PARAM_DEFINE_FLOAT(FW_AIRSPD_TRIM, 15.0f);
diff --git a/src/modules/vtol_att_control/vtol_att_control_params.c b/src/modules/vtol_att_control/vtol_att_control_params.c
index c9ba6544f0..bfcc712242 100644
--- a/src/modules/vtol_att_control/vtol_att_control_params.c
+++ b/src/modules/vtol_att_control/vtol_att_control_params.c
@@ -256,14 +256,6 @@ PARAM_DEFINE_FLOAT(VT_ARSP_TRANS, 10.0f);
  */
 PARAM_DEFINE_INT32(VT_OPT_RECOV_EN, 0);
 
-/**
- * Weather-vane mode landings for missions
- *
- * @boolean
- * @group VTOL Attitude Control
- */
-PARAM_DEFINE_INT32(VT_WV_LND_EN, 0);
-
 /**
  * Weather-vane yaw rate scale.
  *
@@ -278,14 +270,6 @@ PARAM_DEFINE_INT32(VT_WV_LND_EN, 0);
  */
 PARAM_DEFINE_FLOAT(VT_WV_YAWR_SCL, 0.15f);
 
-/**
- * Weather-vane mode for loiter
- *
- * @boolean
- * @group VTOL Attitude Control
- */
-PARAM_DEFINE_INT32(VT_WV_LTR_EN, 0);
-
 /**
  * Front transition timeout
  *

From 9570d66d9d0522f007a34325b6766909059e078e Mon Sep 17 00:00:00 2001
From: Henry Zhang <zhanghui629@gmail.com>
Date: Fri, 20 May 2016 13:49:28 +0800
Subject: [PATCH 0271/1230] MindPXFMUv2 uses STM32F427 v3, enable 2M flash.

---
 nuttx-configs/mindpx-v2/scripts/ld.script | 3 +--
 1 file changed, 1 insertion(+), 2 deletions(-)

diff --git a/nuttx-configs/mindpx-v2/scripts/ld.script b/nuttx-configs/mindpx-v2/scripts/ld.script
index b04ad89a6c..ab66d2dc61 100644
--- a/nuttx-configs/mindpx-v2/scripts/ld.script
+++ b/nuttx-configs/mindpx-v2/scripts/ld.script
@@ -50,8 +50,7 @@
 
 MEMORY
 {
-    /* disabled due to silicon errata flash (rx)   : ORIGIN = 0x08004000, LENGTH = 2032K */
-    flash (rx)   : ORIGIN = 0x08004000, LENGTH = 1008K
+    flash (rx)   : ORIGIN = 0x08004000, LENGTH = 2032K
     sram (rwx)   : ORIGIN = 0x20000000, LENGTH = 192K
     ccsram (rwx) : ORIGIN = 0x10000000, LENGTH = 64K
 }

From 4495eafbf441e15dba37eed1181021abfeb0e78e Mon Sep 17 00:00:00 2001
From: Henry Zhang <zhanghui629@gmail.com>
Date: Fri, 20 May 2016 13:57:42 +0800
Subject: [PATCH 0272/1230] MindPXFMUv2 add lpe/ekf2/load_mon.

---
 cmake/configs/nuttx_mindpx-v2_default.cmake | 23 ++++++++++-----------
 1 file changed, 11 insertions(+), 12 deletions(-)

diff --git a/cmake/configs/nuttx_mindpx-v2_default.cmake b/cmake/configs/nuttx_mindpx-v2_default.cmake
index e09339029f..18b496fa02 100644
--- a/cmake/configs/nuttx_mindpx-v2_default.cmake
+++ b/cmake/configs/nuttx_mindpx-v2_default.cmake
@@ -15,7 +15,7 @@ set(config_module_list
 	drivers/led
 	drivers/px4fmu
 	#drivers/px4io
-	drivers/test_ppm
+	#drivers/test_ppm
 	drivers/boards/mindpx-v2
 	#drivers/rgbled
 	drivers/rgbled_pwm
@@ -76,6 +76,7 @@ set(config_module_list
 	# General system control
 	#
 	modules/commander
+	modules/load_mon
 	modules/navigator
 	modules/mavlink
 	modules/gpio_led
@@ -85,11 +86,11 @@ set(config_module_list
 	#
 	# Estimation modules (EKF/ SO3 / other filters)
 	#
-	# Too high RAM usage due to static allocations
-	# modules/attitude_estimator_ekf
 	modules/attitude_estimator_q
 	modules/ekf_att_pos_estimator
 	modules/position_estimator_inav
+	modules/local_position_estimator
+	modules/ekf2
 
 	#
 	# Vehicle Control
@@ -135,7 +136,7 @@ set(config_module_list
 	platforms/nuttx
 
 	# had to add for cmake, not sure why wasn't in original config
-	platforms/common 
+	platforms/common
 	platforms/nuttx/px4_layer
 
 	#
@@ -146,7 +147,7 @@ set(config_module_list
 	#
 	# Rover apps
 	#
-	examples/rover_steering_control
+	#examples/rover_steering_control
 
 	#
 	# Demo apps
@@ -154,7 +155,7 @@ set(config_module_list
 	#examples/math_demo
 	# Tutorial code from
 	# https://px4.io/dev/px4_simple_app
-	examples/px4_simple_app
+	#examples/px4_simple_app
 
 	# Tutorial code from
 	# https://px4.io/dev/daemon
@@ -177,10 +178,6 @@ set(config_extra_builtin_cmds
 	sercon
 	)
 
-set(config_io_board
-	px4io-v2
-	)
-
 set(config_extra_libs
 	uavcan
 	uavcan_stm32_driver
@@ -192,9 +189,11 @@ set(config_io_extra_libs
 add_custom_target(sercon)
 set_target_properties(sercon PROPERTIES
 	PRIORITY "SCHED_PRIORITY_DEFAULT"
-	MAIN "sercon" STACK_MAIN "2048")
+	MAIN "sercon"
+	STACK_MAIN "2048")
 
 add_custom_target(serdis)
 set_target_properties(serdis PROPERTIES
 	PRIORITY "SCHED_PRIORITY_DEFAULT"
-	MAIN "serdis" STACK_MAIN "2048")
+	MAIN "serdis"
+	STACK_MAIN "2048")

From 1551d93a53ede41a3db92b7b74c260459f92937e Mon Sep 17 00:00:00 2001
From: Henry Zhang <zhanghui629@gmail.com>
Date: Fri, 20 May 2016 14:43:21 +0800
Subject: [PATCH 0273/1230] MindPXFMUv2 uses a new board id, different with
 PX4FMUv2.

---
 Images/mindpx-v2.prototype | 8 ++++----
 1 file changed, 4 insertions(+), 4 deletions(-)

diff --git a/Images/mindpx-v2.prototype b/Images/mindpx-v2.prototype
index 1a5ebb8f9d..c99c883b89 100644
--- a/Images/mindpx-v2.prototype
+++ b/Images/mindpx-v2.prototype
@@ -1,10 +1,10 @@
 {
-    "board_id": 9, 
-    "magic": "PX4FWv2",
-    "description": "Firmware for the MindPx-V2  board", 
+    "board_id": 88,
+    "magic": "MindPX",
+    "description": "Firmware for the MindPXFMUv2 board",
     "image": "", 
     "build_time": 0, 
-    "summary": "MindPx-v2",
+    "summary": "MindPXFMUv2",
     "version": "2.1",
     "image_size": 0,
     "git_identity": "",

From f71b68a487de394c10f5ef1c710243018c1fef8a Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Sat, 21 May 2016 12:08:20 +0200
Subject: [PATCH 0274/1230] Allow arming in Rattitude mode

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

diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp
index ba304dd229..84fc3c9c50 100644
--- a/src/modules/commander/commander.cpp
+++ b/src/modules/commander/commander.cpp
@@ -2386,6 +2386,7 @@ int commander_thread_main(int argc, char *argv[])
 						&& (internal_state.main_state != commander_state_s::MAIN_STATE_STAB)
 						&& (internal_state.main_state != commander_state_s::MAIN_STATE_ALTCTL)
 						&& (internal_state.main_state != commander_state_s::MAIN_STATE_POSCTL)
+						&& (internal_state.main_state != commander_state_s::MAIN_STATE_RATTITUDE)
 						) {
 						print_reject_arm("NOT ARMING: Switch to a manual mode first.");
 

From fe60a43bba6ebb976513f3db0b7a8aca749fc2a3 Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Sat, 21 May 2016 13:53:14 +0200
Subject: [PATCH 0275/1230] EKF1: Fix stack smashing resulting from
 uninitialized publication

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

diff --git a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp
index 9432c5d2de..cb4f0b8b08 100644
--- a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp
+++ b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp
@@ -221,6 +221,7 @@ AttitudePositionEstimatorEKF::AttitudePositionEstimatorEKF() :
 	_newAdsData(false),
 	_newDataMag(false),
 	_newRangeData(false),
+	_mavlink_log_pub(nullptr),
 
 	_mag_offset_x(this, "MAGB_X"),
 	_mag_offset_y(this, "MAGB_Y"),

From 490cd95cb227390269740dff05eb56188639d5bf Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Sat, 21 May 2016 14:09:59 +0200
Subject: [PATCH 0276/1230] EKF1: Simplify output

---
 .../ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp       | 3 +--
 1 file changed, 1 insertion(+), 2 deletions(-)

diff --git a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp
index cb4f0b8b08..ae452c01d3 100644
--- a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp
+++ b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp
@@ -422,8 +422,7 @@ int AttitudePositionEstimatorEKF::check_filter_state()
 
 		// Do not warn about accel offset if we have no position updates
 		if (!(warn_index == 5 && _ekf->staticMode)) {
-			PX4_WARN("reset: %s", feedback[warn_index]);
-			mavlink_log_critical(&_mavlink_log_pub, "[ekf check] %s", feedback[warn_index]);
+			mavlink_and_console_log_critical(&_mavlink_log_pub, "[ekf check] %s", feedback[warn_index]);
 		}
 	}
 

From 47d2cb8134f25c2e8ca8541c855920b703b3f400 Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Sat, 21 May 2016 14:10:12 +0200
Subject: [PATCH 0277/1230] LPE: Fix stack smashing

---
 .../local_position_estimator/BlockLocalPositionEstimator.cpp    | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp b/src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp
index b7653a4047..cf7f639f9d 100644
--- a/src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp
+++ b/src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp
@@ -4,7 +4,7 @@
 #include <systemlib/err.h>
 #include <matrix/math.hpp>
 
-orb_advert_t mavlink_log_pub;
+orb_advert_t mavlink_log_pub = nullptr;
 
 // timeouts for sensors in microseconds
 static const uint32_t 		EST_SRC_TIMEOUT = 10000; // 0.01 s

From 2dad43549dff3ef345322a8837cd826edea5e8da Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Sat, 21 May 2016 14:10:31 +0200
Subject: [PATCH 0278/1230] Bottle drop: Fix uninitialized member

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

diff --git a/src/modules/bottle_drop/bottle_drop.cpp b/src/modules/bottle_drop/bottle_drop.cpp
index 8cd4ed66eb..0c17fb52be 100644
--- a/src/modules/bottle_drop/bottle_drop.cpp
+++ b/src/modules/bottle_drop/bottle_drop.cpp
@@ -170,6 +170,7 @@ BottleDrop::BottleDrop() :
 
 	_task_should_exit(false),
 	_main_task(-1),
+	_mavlink_log_pub(nullptr),
 	_command_sub(-1),
 	_wind_estimate_sub(-1),
 	_command {},

From d8bcd77290339aaeee8ab34e2c68950b4623987b Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Sat, 21 May 2016 14:10:49 +0200
Subject: [PATCH 0279/1230] EKF2: Remove unused header

---
 src/modules/ekf2/ekf2_main.cpp | 1 -
 1 file changed, 1 deletion(-)

diff --git a/src/modules/ekf2/ekf2_main.cpp b/src/modules/ekf2/ekf2_main.cpp
index 45f77d4222..de40308a0c 100644
--- a/src/modules/ekf2/ekf2_main.cpp
+++ b/src/modules/ekf2/ekf2_main.cpp
@@ -58,7 +58,6 @@
 #include <systemlib/param/param.h>
 #include <systemlib/err.h>
 #include <systemlib/systemlib.h>
-#include <systemlib/mavlink_log.h>
 #include <mathlib/mathlib.h>
 #include <mathlib/math/filter/LowPassFilter2p.hpp>
 #include <platforms/px4_defines.h>

From 1db1a364c16389c52c55426f36045376d32bf5f0 Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lorenz@px4.io>
Date: Sat, 21 May 2016 15:40:05 +0200
Subject: [PATCH 0280/1230] SBUS decoding: Fix channel 18 return value

---
 src/lib/rc/sbus.c | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/src/lib/rc/sbus.c b/src/lib/rc/sbus.c
index ca4edd2208..aeefc8a59d 100644
--- a/src/lib/rc/sbus.c
+++ b/src/lib/rc/sbus.c
@@ -606,9 +606,9 @@ sbus_decode(uint64_t frame_time, uint8_t *frame, uint16_t *values, uint16_t *num
 		chancount = 18;
 
 		/* channel 17 (index 16) */
-		values[16] = (frame[SBUS_FLAGS_BYTE] & (1 << 0)) * 1000 + 998;
+		values[16] = (((frame[SBUS_FLAGS_BYTE] & (1 << 0)) > 0) ? 1 : 0) * 1000 + 998;
 		/* channel 18 (index 17) */
-		values[17] = (frame[SBUS_FLAGS_BYTE] & (1 << 1)) * 1000 + 998;
+		values[17] = (((frame[SBUS_FLAGS_BYTE] & (1 << 1)) > 0) ? 1 : 0) * 1000 + 998;
 	}
 
 	/* note the number of channels decoded */

From e7add076b5b71b530189e14d6688490131bedea6 Mon Sep 17 00:00:00 2001
From: Mark Whitehorn <kd0aij@gmail.com>
Date: Fri, 20 May 2016 07:57:20 -0600
Subject: [PATCH 0281/1230] typo in comment

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

diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp
index 84fc3c9c50..c8429915b4 100644
--- a/src/modules/commander/commander.cpp
+++ b/src/modules/commander/commander.cpp
@@ -131,7 +131,7 @@ static const int ERROR = -1;
 
 static constexpr uint8_t COMMANDER_MAX_GPS_NOISE = 60;		/**< Maximum percentage signal to noise ratio allowed for GPS reception */
 
-/* Decouple update interval and hysteris counters, all depends on intervals */
+/* Decouple update interval and hysteresis counters, all depends on intervals */
 #define COMMANDER_MONITORING_INTERVAL 10000
 #define COMMANDER_MONITORING_LOOPSPERMSEC (1/(COMMANDER_MONITORING_INTERVAL/1000.0f))
 

From 84761a9b8ed97aa6dc0cfeed8c8cc64e8769e874 Mon Sep 17 00:00:00 2001
From: Mark Whitehorn <kd0aij@gmail.com>
Date: Fri, 20 May 2016 09:59:23 -0600
Subject: [PATCH 0282/1230] add parameter for arm/disarm "hysteresis"

---
 src/modules/commander/commander.cpp      | 16 +++++++++++-----
 src/modules/commander/commander_params.c | 11 +++++++++++
 2 files changed, 22 insertions(+), 5 deletions(-)

diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp
index c8429915b4..d61805f9aa 100644
--- a/src/modules/commander/commander.cpp
+++ b/src/modules/commander/commander.cpp
@@ -138,8 +138,6 @@ static constexpr uint8_t COMMANDER_MAX_GPS_NOISE = 60;		/**< Maximum percentage
 #define MAVLINK_OPEN_INTERVAL 50000
 
 #define STICK_ON_OFF_LIMIT 0.9f
-#define STICK_ON_OFF_HYSTERESIS_TIME_MS 1000
-#define STICK_ON_OFF_COUNTER_LIMIT (STICK_ON_OFF_HYSTERESIS_TIME_MS*COMMANDER_MONITORING_LOOPSPERMSEC)
 
 #define POSITION_TIMEOUT		(1 * 1000 * 1000)	/**< consider the local or global position estimate invalid after 1000ms */
 #define FAILSAFE_DEFAULT_TIMEOUT	(3 * 1000 * 1000)	/**< hysteresis time - the failsafe will trigger after 3 seconds in this state */
@@ -1183,6 +1181,7 @@ int commander_thread_main(int argc, char *argv[])
 	param_t _param_autostart_id = param_find("SYS_AUTOSTART");
 	param_t _param_autosave_params = param_find("COM_AUTOS_PAR");
 	param_t _param_rc_in_off = param_find("COM_RC_IN_MODE");
+	param_t _param_rc_arm_hyst = param_find("COM_RC_ARM_HYST");
 	param_t _param_eph = param_find("COM_HOME_H_T");
 	param_t _param_epv = param_find("COM_HOME_V_T");
 	param_t _param_geofence_action = param_find("GF_ACTION");
@@ -1515,13 +1514,18 @@ int commander_thread_main(int argc, char *argv[])
 		status_flags.condition_system_sensors_initialized = false;
 		set_tune_override(TONE_STARTUP_TUNE); //normal boot tune
 	} else {
-			// sensor diagnostics done continiously, not just at boot so don't warn about any issues just yet
+			// sensor diagnostics done continuously, not just at boot so don't warn about any issues just yet
 			status_flags.condition_system_sensors_initialized = Commander::preflightCheck(&mavlink_log_pub, true, true, true, true,
 				checkAirspeed, (status.rc_input_mode == vehicle_status_s::RC_IN_MODE_DEFAULT),
 				!status_flags.circuit_breaker_engaged_gpsfailure_check, false);
 			set_tune_override(TONE_STARTUP_TUNE); //normal boot tune
 	}
 
+	// user adjustable duration required to assert arm/disarm via throttle/rudder stick
+	int32_t rc_arm_hyst = 100;
+	param_get(_param_rc_arm_hyst, &rc_arm_hyst);
+	rc_arm_hyst *= COMMANDER_MONITORING_LOOPSPERMSEC;
+
 	commander_boot_timestamp = hrt_absolute_time();
 
 	transition_result_t arming_ret;
@@ -1616,6 +1620,8 @@ int commander_thread_main(int argc, char *argv[])
 			param_get(_param_rc_loss_timeout, &rc_loss_timeout);
 			param_get(_param_rc_in_off, &rc_in_off);
 			status.rc_input_mode = rc_in_off;
+			param_get(_param_rc_arm_hyst, &rc_arm_hyst);
+			rc_arm_hyst *= COMMANDER_MONITORING_LOOPSPERMSEC;
 			param_get(_param_datalink_regain_timeout, &datalink_regain_timeout);
 			param_get(_param_ef_throttle_thres, &ef_throttle_thres);
 			param_get(_param_ef_current2throttle_thres, &ef_current2throttle_thres);
@@ -2344,7 +2350,7 @@ int commander_thread_main(int argc, char *argv[])
 			    	land_detector.landed) &&
 			    sp_man.r < -STICK_ON_OFF_LIMIT && sp_man.z < 0.1f) {
 
-				if (stick_off_counter > STICK_ON_OFF_COUNTER_LIMIT) {
+				if (stick_off_counter > rc_arm_hyst) {
 					/* disarm to STANDBY if ARMED or to STANDBY_ERROR if ARMED_ERROR */
 					arming_state_t new_arming_state = (status.arming_state == vehicle_status_s::ARMING_STATE_ARMED ? vehicle_status_s::ARMING_STATE_STANDBY :
 									   vehicle_status_s::ARMING_STATE_STANDBY_ERROR);
@@ -2374,7 +2380,7 @@ int commander_thread_main(int argc, char *argv[])
 
 			/* check if left stick is in lower right position and we're in MANUAL mode -> arm */
 			if (sp_man.r > STICK_ON_OFF_LIMIT && sp_man.z < 0.1f && status.rc_input_mode != vehicle_status_s::RC_IN_MODE_OFF ) {
-				if (stick_on_counter > STICK_ON_OFF_COUNTER_LIMIT) {
+				if (stick_on_counter > rc_arm_hyst) {
 
 					/* we check outside of the transition function here because the requirement
 					 * for being in manual mode only applies to manual arming actions.
diff --git a/src/modules/commander/commander_params.c b/src/modules/commander/commander_params.c
index 00427f484d..46acadfe8f 100644
--- a/src/modules/commander/commander_params.c
+++ b/src/modules/commander/commander_params.c
@@ -235,6 +235,17 @@ PARAM_DEFINE_INT32(COM_AUTOS_PAR, 1);
  */
 PARAM_DEFINE_INT32(COM_RC_IN_MODE, 0);
 
+/**
+ * RC input arm/disarm command duration
+ *
+ * The default value of 1000 requires the stick to be held in the arm or disarm position for 1 second.
+ *
+ * @group Commander
+ * @min 100
+ * @max 1500
+ */
+PARAM_DEFINE_INT32(COM_RC_ARM_HYST, 1000);
+
 /**
  * Time-out for auto disarm after landing
  *

From 5be1f3a856eb15c38e9614b2b87daffcf070a725 Mon Sep 17 00:00:00 2001
From: Daniel Agar <daniel@agar.ca>
Date: Sat, 21 May 2016 12:43:02 -0400
Subject: [PATCH 0283/1230] ASan use normal optimization (MEMORY_DEBUG=1)

- see #4530
---
 cmake/common/px4_base.cmake | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/cmake/common/px4_base.cmake b/cmake/common/px4_base.cmake
index 886522763a..210dcbcdf8 100644
--- a/cmake/common/px4_base.cmake
+++ b/cmake/common/px4_base.cmake
@@ -638,7 +638,7 @@ function(px4_add_common_flags)
 	endif()
 
 	if ($ENV{MEMORY_DEBUG} MATCHES "1")
-		set(max_optimization -O0)
+		set(max_optimization -Os)
 
 		set(optimization_flags
 			-fno-strict-aliasing
@@ -646,7 +646,7 @@ function(px4_add_common_flags)
 			-funsafe-math-optimizations
 			-ffunction-sections
 			-fdata-sections
-			-g -fsanitize=address
+			-g3 -fsanitize=address
 			)
 	else()
 		set(max_optimization -Os)

From 556851a5115cc61624601ccaff38b5894ae364e8 Mon Sep 17 00:00:00 2001
From: Andreas Bircher <bircher@gmx.ch>
Date: Mon, 18 Apr 2016 16:30:28 +0200
Subject: [PATCH 0284/1230] applying dual gps patch

resolve transfer errors

reformatting

implementing multi-topics
---
 src/drivers/gps/gps.cpp              | 173 +++++++++++++++++++--------
 src/modules/sdlog2/sdlog2.c          |  26 +++-
 src/modules/sdlog2/sdlog2_messages.h |   2 +
 3 files changed, 151 insertions(+), 50 deletions(-)

diff --git a/src/drivers/gps/gps.cpp b/src/drivers/gps/gps.cpp
index 1b12a9dd09..0e079bbaca 100644
--- a/src/drivers/gps/gps.cpp
+++ b/src/drivers/gps/gps.cpp
@@ -100,7 +100,7 @@ public:
 class GPS
 {
 public:
-	GPS(const char *uart_path, bool fake_gps, bool enable_sat_info);
+	GPS(const char *uart_path, bool fake_gps, bool enable_sat_info, int gps_num);
 	virtual ~GPS();
 
 	virtual int			init();
@@ -124,13 +124,15 @@ private:
 	GPSHelper			*_helper;					///< instance of GPS parser
 	GPS_Sat_Info			*_sat_info;					///< instance of GPS sat info data object
 	struct vehicle_gps_position_s	_report_gps_pos;				///< uORB topic for gps position
-	orb_advert_t			_report_gps_pos_pub;				///< uORB pub for gps position
+	orb_advert_t			_report_gps_pos_pub[2];				///< uORB pub for gps position
+	int					_gps_orb_instance[2];				///< uORB multi-topic instance
 	struct satellite_info_s		*_p_report_sat_info;				///< pointer to uORB topic for satellite info
 	orb_advert_t			_report_sat_info_pub;				///< uORB pub for satellite info
 	float				_rate;						///< position update rate
 	float				_rate_rtcm_injection;				///< RTCM message injection rate
 	unsigned			_last_rate_rtcm_injection_count; 		///< counter for number of RTCM messages
 	bool				_fake_gps;					///< fake gps output
+	int 				_gps_num;					///< number of GPS connected
 
 	static const int _orb_inject_data_fd_count = 4;
 	int _orb_inject_data_fd[_orb_inject_data_fd_count];
@@ -161,6 +163,11 @@ private:
 	 */
 	void cmd_reset();
 
+	/**
+	 * Publish the gps struct
+	 */
+	void 				publish();
+
 	/**
 	 * This is an abstraction for the poll on serial used.
 	 *
@@ -211,20 +218,23 @@ GPS	*g_dev = nullptr;
 
 }
 
-GPS::GPS(const char *uart_path, bool fake_gps, bool enable_sat_info) :
+
+GPS::GPS(const char *uart_path, bool fake_gps, bool enable_sat_info, int gps_num) :
 	_task_should_exit(false),
 	_healthy(false),
 	_mode_changed(false),
 	_mode(GPS_DRIVER_MODE_UBX),
 	_helper(nullptr),
 	_sat_info(nullptr),
-	_report_gps_pos_pub(nullptr),
+	_report_gps_pos_pub{ nullptr, nullptr},
+	_gps_orb_instance{ -1, -1},
 	_p_report_sat_info(nullptr),
 	_report_sat_info_pub(nullptr),
 	_rate(0.0f),
 	_rate_rtcm_injection(0.0f),
 	_last_rate_rtcm_injection_count(0),
-	_fake_gps(fake_gps)
+	_fake_gps(fake_gps),
+	_gps_num(gps_num)
 {
 	/* store port name */
 	strncpy(_port, uart_path, sizeof(_port));
@@ -566,12 +576,7 @@ GPS::task_main()
 			/* no time and satellite information simulated */
 
 
-			if (_report_gps_pos_pub != nullptr) {
-				orb_publish(ORB_ID(vehicle_gps_position), _report_gps_pos_pub, &_report_gps_pos);
-
-			} else {
-				_report_gps_pos_pub = orb_advertise(ORB_ID(vehicle_gps_position), &_report_gps_pos);
-			}
+			publish();
 
 			usleep(2e5);
 
@@ -628,12 +633,7 @@ GPS::task_main()
 					_report_gps_pos.epv = 10000.0f;
 					_report_gps_pos.fix_type = 0;
 
-					if (_report_gps_pos_pub != nullptr) {
-						orb_publish(ORB_ID(vehicle_gps_position), _report_gps_pos_pub, &_report_gps_pos);
-
-					} else {
-						_report_gps_pos_pub = orb_advertise(ORB_ID(vehicle_gps_position), &_report_gps_pos);
-					}
+					publish();
 
 					/* GPS is obviously detected successfully, reset statistics */
 					_helper->resetUpdateRates();
@@ -644,23 +644,13 @@ GPS::task_main()
 				while ((helper_ret = _helper->receive(TIMEOUT_5HZ)) > 0 && !_task_should_exit) {
 
 					if (helper_ret & 1) {
-						if (_report_gps_pos_pub != nullptr) {
-							orb_publish(ORB_ID(vehicle_gps_position), _report_gps_pos_pub, &_report_gps_pos);
-
-						} else {
-							_report_gps_pos_pub = orb_advertise(ORB_ID(vehicle_gps_position), &_report_gps_pos);
-						}
+						publish();
 
 						last_rate_count++;
 					}
 
 					if (_p_report_sat_info && (helper_ret & 2)) {
-						if (_report_sat_info_pub != nullptr) {
-							orb_publish(ORB_ID(satellite_info), _report_sat_info_pub, _p_report_sat_info);
-
-						} else {
-							_report_sat_info_pub = orb_advertise(ORB_ID(satellite_info), _p_report_sat_info);
-						}
+						publish();
 					}
 
 					/* measure update rate every 5 seconds */
@@ -810,6 +800,29 @@ GPS::print_info()
 	usleep(100000);
 }
 
+void
+GPS::publish()
+{
+	if (_gps_num == 1) {
+		if (_report_gps_pos_pub[0] != nullptr) {
+			orb_publish(ORB_ID(vehicle_gps_position), _report_gps_pos_pub[0], &_report_gps_pos);
+
+		} else {
+			_report_gps_pos_pub[0] = orb_advertise_multi(ORB_ID(vehicle_gps_position), &_report_gps_pos, &_gps_orb_instance[0], ORB_PRIO_DEFAULT);
+			orb_publish(ORB_ID(vehicle_gps_position), _report_gps_pos_pub[0], &_report_gps_pos);
+		}
+
+	} else {
+		if (_report_gps_pos_pub[1] != nullptr) {
+			orb_publish(ORB_ID(vehicle_gps_position), _report_gps_pos_pub[1], &_report_gps_pos);
+
+		} else {
+			_report_gps_pos_pub[1] = orb_advertise_multi(ORB_ID(vehicle_gps_position), &_report_gps_pos, &_gps_orb_instance[1], ORB_PRIO_DEFAULT);
+			orb_publish(ORB_ID(vehicle_gps_position), _report_gps_pos_pub[1], &_report_gps_pos);
+		}
+	}
+}
+
 /**
  * Local functions in support of the shell command.
  */
@@ -817,8 +830,9 @@ namespace gps
 {
 
 GPS	*g_dev = nullptr;
+GPS	*g_dev2 = nullptr;
 
-void	start(const char *uart_path, bool fake_gps, bool enable_sat_info);
+void	start(const char *uart_path, bool fake_gps, bool enable_sat_info, int gps_num);
 void	stop();
 void	test();
 void	reset();
@@ -828,34 +842,66 @@ void	info();
  * Start the driver.
  */
 void
-start(const char *uart_path, bool fake_gps, bool enable_sat_info)
+start(const char *uart_path, bool fake_gps, bool enable_sat_info, int gps_num)
 {
-	if (g_dev != nullptr) {
-		PX4_WARN("gps already started");
+	if (gps_num == 1) {
+		if (g_dev != nullptr) {
+			PX4_WARN("GPS 1 already started");
+		}
+
+		/* create the driver */
+		g_dev = new GPS(uart_path, fake_gps, enable_sat_info, gps_num);
+
+		if (g_dev == nullptr) {
+			goto fail1;
+		}
+
+		if (OK != g_dev->init()) {
+			goto fail1;
+		}
+
 		return;
+
+	} else {
+		if (gps_num == 2) {
+			if (g_dev2 != nullptr) {
+				PX4_WARN("GPS 2 already started");
+			}
+
+			/* create the driver */
+			g_dev2 = new GPS(uart_path, fake_gps, enable_sat_info, gps_num);
+
+			if (g_dev2 == nullptr) {
+				goto fail2;
+			}
+
+			if (OK != g_dev2->init()) {
+				goto fail2;
+			}
+
+			return;
+		}
 	}
 
-	/* create the driver */
-	g_dev = new GPS(uart_path, fake_gps, enable_sat_info);
 
-	if (g_dev == nullptr) {
-		goto fail;
-	}
-
-	if (OK != g_dev->init()) {
-		goto fail;
-	}
-
-	return;
-
-fail:
+fail1:
 
 	if (g_dev != nullptr) {
 		delete g_dev;
 		g_dev = nullptr;
 	}
 
-	PX4_ERR("start failed");
+	PX4_ERR("start of GPS 1 failed");
+	return;
+
+fail2:
+
+	if (g_dev2 != nullptr) {
+		delete g_dev2;
+		g_dev2 = nullptr;
+	}
+
+	PX4_ERR("start of GPS 2 failed");
 	return;
 }
 
@@ -867,6 +913,13 @@ stop()
 {
 	delete g_dev;
 	g_dev = nullptr;
+
+	if (g_dev2 != nullptr) {
+		delete g_dev2;
+	}
+	g_dev2 = nullptr;
+
+	px4_task_exit(0);
 }
 
 /**
@@ -903,6 +956,12 @@ info()
 	}
 
 	g_dev->print_info();
+
+	if (g_dev2 != nullptr) {
+		g_dev2->print_info();
+	}
+
+	return;
 }
 
 } // namespace
@@ -913,6 +972,7 @@ gps_main(int argc, char *argv[])
 {
 	/* set to default */
 	const char *device_name = GPS_DEFAULT_UART_PORT;
+	const char *device_name2 = nullptr;
 	bool fake_gps = false;
 	bool enable_sat_info = false;
 
@@ -949,7 +1009,24 @@ gps_main(int argc, char *argv[])
 			}
 		}
 
-		gps::start(device_name, fake_gps, enable_sat_info);
+		/* Allow to use a second gps device */
+		for (int i = 2; i < argc; i++) {
+			if (!strcmp(argv[i], "-dualgps")) {
+				if (argc > i + 1) {
+					device_name2 = argv[i + 1];
+
+				} else {
+					PX4_ERR("Did not get second device address");
+				}
+			}
+		}
+
+		gps::start(device_name, fake_gps, enable_sat_info, 1);
+
+		if (!(device_name2 == nullptr)) {
+			gps::start(device_name2, fake_gps, enable_sat_info, 2);
+		}
+
 	}
 
 	if (!strcmp(argv[1], "stop")) {
diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c
index 351ad1da59..359dbec7b5 100644
--- a/src/modules/sdlog2/sdlog2.c
+++ b/src/modules/sdlog2/sdlog2.c
@@ -1188,6 +1188,7 @@ int sdlog2_thread_main(int argc, char *argv[])
 		struct ekf2_replay_s replay;
 		struct vehicle_land_detected_s land_detected;
 		struct cpuload_s cpuload;
+		struct vehicle_gps_position_s dual_gps_pos;
 	} buf;
 
 	memset(&buf, 0, sizeof(buf));
@@ -1368,7 +1369,7 @@ int sdlog2_thread_main(int argc, char *argv[])
 	if (log_on_start) {
 		/* check GPS topic to get GPS time */
 		if (log_name_timestamp) {
-			if (!orb_copy(ORB_ID(vehicle_gps_position), subs.gps_pos_sub, &buf_gps_pos)) {
+			if (!copy_if_updated_multi(ORB_ID(vehicle_gps_position), 0, &subs.gps_pos_sub, &buf_gps_pos)) {
 				gps_time_sec = buf_gps_pos.time_utc_usec / 1e6;
 			}
 		}
@@ -1482,7 +1483,7 @@ int sdlog2_thread_main(int argc, char *argv[])
 		}
 
 		/* --- GPS POSITION - LOG MANAGEMENT --- */
-		bool gps_pos_updated = copy_if_updated(ORB_ID(vehicle_gps_position), &subs.gps_pos_sub, &buf_gps_pos);
+		bool gps_pos_updated = copy_if_updated_multi(ORB_ID(vehicle_gps_position), 0, &subs.gps_pos_sub, &buf_gps_pos);
 
 		if (gps_pos_updated && log_name_timestamp) {
 			gps_time_sec = buf_gps_pos.time_utc_usec / 1e6;
@@ -1718,6 +1719,27 @@ int sdlog2_thread_main(int argc, char *argv[])
 				LOGBUFFER_WRITE_AND_COUNT(GPS);
 			}
 
+			/* --- GPS POSITION - UNIT #2 --- */
+			if (copy_if_updated_multi(ORB_ID(vehicle_gps_position), 1, &subs.gps_pos_sub, &buf.dual_gps_pos)) {
+				log_msg.msg_type = LOG_GPS_MSG;
+				log_msg.body.log_GPS.gps_time = buf.dual_gps_pos.time_utc_usec;
+				log_msg.body.log_GPS.fix_type = buf.dual_gps_pos.fix_type;
+				log_msg.body.log_GPS.eph = buf.dual_gps_pos.eph;
+				log_msg.body.log_GPS.epv = buf.dual_gps_pos.epv;
+				log_msg.body.log_GPS.lat = buf.dual_gps_pos.lat;
+				log_msg.body.log_GPS.lon = buf.dual_gps_pos.lon;
+				log_msg.body.log_GPS.alt = buf.dual_gps_pos.alt * 0.001f;
+				log_msg.body.log_GPS.vel_n = buf.dual_gps_pos.vel_n_m_s;
+				log_msg.body.log_GPS.vel_e = buf.dual_gps_pos.vel_e_m_s;
+				log_msg.body.log_GPS.vel_d = buf.dual_gps_pos.vel_d_m_s;
+				log_msg.body.log_GPS.cog = buf.dual_gps_pos.cog_rad;
+				log_msg.body.log_GPS.sats = buf.dual_gps_pos.satellites_used;
+				log_msg.body.log_GPS.snr_mean = snr_mean;
+				log_msg.body.log_GPS.noise_per_ms = buf.dual_gps_pos.noise_per_ms;
+				log_msg.body.log_GPS.jamming_indicator = buf.dual_gps_pos.jamming_indicator;
+				LOGBUFFER_WRITE_AND_COUNT(GPS);
+			}
+
 			/* --- SATELLITE INFO - UNIT #1 --- */
 			if (_extended_logging) {
 
diff --git a/src/modules/sdlog2/sdlog2_messages.h b/src/modules/sdlog2/sdlog2_messages.h
index 12942fde79..71913d9041 100644
--- a/src/modules/sdlog2/sdlog2_messages.h
+++ b/src/modules/sdlog2/sdlog2_messages.h
@@ -146,6 +146,7 @@ struct log_LPSP_s {
 
 /* --- GPS - GPS POSITION --- */
 #define LOG_GPS_MSG 8
+#define LOG_DGPS_MSG 58
 struct log_GPS_s {
 	uint64_t gps_time;
 	uint8_t fix_type;
@@ -647,6 +648,7 @@ static const struct log_format_s log_formats[] = {
 	LOG_FORMAT(LPOS, "ffffffffLLfBBff",	"X,Y,Z,Dist,DistR,VX,VY,VZ,RLat,RLon,RAlt,PFlg,GFlg,EPH,EPV"),
 	LOG_FORMAT(LPSP, "ffffffffff",		"X,Y,Z,Yaw,VX,VY,VZ,AX,AY,AZ"),
 	LOG_FORMAT(GPS, "QBffLLfffffBHHH",	"GPSTime,Fix,EPH,EPV,Lat,Lon,Alt,VelN,VelE,VelD,Cog,nSat,SNR,N,J"),
+	LOG_FORMAT_S(DGPS, GPS,	 "QBffLLfffffBHHH",	"GPSTime,Fix,EPH,EPV,Lat,Lon,Alt,VelN,VelE,VelD,Cog,nSat,SNR,N,J"),
 	LOG_FORMAT_S(ATTC, ATTC, "ffff",		"Roll,Pitch,Yaw,Thrust"),
 	LOG_FORMAT_S(ATC1, ATTC, "ffff",		"Roll,Pitch,Yaw,Thrust"),
 	LOG_FORMAT(STAT, "BBBB",		"MainState,NavState,ArmS,Failsafe"),

From c1cdef7e63aaa8fd084b4a5a54b81d0f3996a85f Mon Sep 17 00:00:00 2001
From: Sebastian Verling <sverling@student.ethz.ch>
Date: Tue, 26 Apr 2016 11:11:49 +0200
Subject: [PATCH 0285/1230] fixed multi subscribing and publishing

removed debugging message

changed to orb_publish_auto
---
 src/drivers/gps/gps.cpp     | 27 +++++----------------------
 src/modules/sdlog2/sdlog2.c | 11 ++++++-----
 2 files changed, 11 insertions(+), 27 deletions(-)

diff --git a/src/drivers/gps/gps.cpp b/src/drivers/gps/gps.cpp
index 0e079bbaca..b99406034b 100644
--- a/src/drivers/gps/gps.cpp
+++ b/src/drivers/gps/gps.cpp
@@ -124,8 +124,8 @@ private:
 	GPSHelper			*_helper;					///< instance of GPS parser
 	GPS_Sat_Info			*_sat_info;					///< instance of GPS sat info data object
 	struct vehicle_gps_position_s	_report_gps_pos;				///< uORB topic for gps position
-	orb_advert_t			_report_gps_pos_pub[2];				///< uORB pub for gps position
-	int					_gps_orb_instance[2];				///< uORB multi-topic instance
+	orb_advert_t			_report_gps_pos_pub;				///< uORB pub for gps position
+	int					_gps_orb_instance;				///< uORB multi-topic instance
 	struct satellite_info_s		*_p_report_sat_info;				///< pointer to uORB topic for satellite info
 	orb_advert_t			_report_sat_info_pub;				///< uORB pub for satellite info
 	float				_rate;						///< position update rate
@@ -226,8 +226,8 @@ GPS::GPS(const char *uart_path, bool fake_gps, bool enable_sat_info, int gps_num
 	_mode(GPS_DRIVER_MODE_UBX),
 	_helper(nullptr),
 	_sat_info(nullptr),
-	_report_gps_pos_pub{ nullptr, nullptr},
-	_gps_orb_instance{ -1, -1},
+	_report_gps_pos_pub{nullptr},
+	_gps_orb_instance(-1),
 	_p_report_sat_info(nullptr),
 	_report_sat_info_pub(nullptr),
 	_rate(0.0f),
@@ -803,24 +803,7 @@ GPS::print_info()
 void
 GPS::publish()
 {
-	if (_gps_num == 1) {
-		if (_report_gps_pos_pub[0] != nullptr) {
-			orb_publish(ORB_ID(vehicle_gps_position), _report_gps_pos_pub[0], &_report_gps_pos);
-
-		} else {
-			_report_gps_pos_pub[0] = orb_advertise_multi(ORB_ID(vehicle_gps_position), &_report_gps_pos, &_gps_orb_instance[0], ORB_PRIO_DEFAULT);
-			orb_publish(ORB_ID(vehicle_gps_position), _report_gps_pos_pub[0], &_report_gps_pos);
-		}
-
-	} else {
-		if (_report_gps_pos_pub[1] != nullptr) {
-			orb_publish(ORB_ID(vehicle_gps_position), _report_gps_pos_pub[1], &_report_gps_pos);
-
-		} else {
-			_report_gps_pos_pub[1] = orb_advertise_multi(ORB_ID(vehicle_gps_position), &_report_gps_pos, &_gps_orb_instance[1], ORB_PRIO_DEFAULT);
-			orb_publish(ORB_ID(vehicle_gps_position), _report_gps_pos_pub[1], &_report_gps_pos);
-		}
-	}
+			orb_publish_auto(ORB_ID(vehicle_gps_position), &_report_gps_pos_pub, &_report_gps_pos, &_gps_orb_instance, ORB_PRIO_DEFAULT);
 }
 
 /**
diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c
index 359dbec7b5..1c07400fd8 100644
--- a/src/modules/sdlog2/sdlog2.c
+++ b/src/modules/sdlog2/sdlog2.c
@@ -1272,7 +1272,7 @@ int sdlog2_thread_main(int argc, char *argv[])
 		int local_pos_sp_sub;
 		int global_pos_sub;
 		int triplet_sub;
-		int gps_pos_sub;
+		int gps_pos_sub[2];
 		int sat_info_sub;
 		int att_pos_mocap_sub;
 		int vision_pos_sub;
@@ -1304,7 +1304,8 @@ int sdlog2_thread_main(int argc, char *argv[])
 	subs.cmd_sub = -1;
 	subs.status_sub = -1;
 	subs.vtol_status_sub = -1;
-	subs.gps_pos_sub = -1;
+	subs.gps_pos_sub[0] = -1;
+	subs.gps_pos_sub[1] = -1;
 	subs.sensor_sub = -1;
 	subs.att_sub = -1;
 	subs.att_sp_sub = -1;
@@ -1369,7 +1370,7 @@ int sdlog2_thread_main(int argc, char *argv[])
 	if (log_on_start) {
 		/* check GPS topic to get GPS time */
 		if (log_name_timestamp) {
-			if (!copy_if_updated_multi(ORB_ID(vehicle_gps_position), 0, &subs.gps_pos_sub, &buf_gps_pos)) {
+			if (!copy_if_updated_multi(ORB_ID(vehicle_gps_position), 0, &subs.gps_pos_sub[0], &buf_gps_pos)) {
 				gps_time_sec = buf_gps_pos.time_utc_usec / 1e6;
 			}
 		}
@@ -1483,7 +1484,7 @@ int sdlog2_thread_main(int argc, char *argv[])
 		}
 
 		/* --- GPS POSITION - LOG MANAGEMENT --- */
-		bool gps_pos_updated = copy_if_updated_multi(ORB_ID(vehicle_gps_position), 0, &subs.gps_pos_sub, &buf_gps_pos);
+		bool gps_pos_updated = copy_if_updated_multi(ORB_ID(vehicle_gps_position), 0, &subs.gps_pos_sub[0], &buf_gps_pos);
 
 		if (gps_pos_updated && log_name_timestamp) {
 			gps_time_sec = buf_gps_pos.time_utc_usec / 1e6;
@@ -1720,7 +1721,7 @@ int sdlog2_thread_main(int argc, char *argv[])
 			}
 
 			/* --- GPS POSITION - UNIT #2 --- */
-			if (copy_if_updated_multi(ORB_ID(vehicle_gps_position), 1, &subs.gps_pos_sub, &buf.dual_gps_pos)) {
+			if (copy_if_updated_multi(ORB_ID(vehicle_gps_position), 1, &subs.gps_pos_sub[1], &buf.dual_gps_pos)) {
 				log_msg.msg_type = LOG_GPS_MSG;
 				log_msg.body.log_GPS.gps_time = buf.dual_gps_pos.time_utc_usec;
 				log_msg.body.log_GPS.fix_type = buf.dual_gps_pos.fix_type;

From 3a72e9b49490a13c4cad7747acc5942ffe5137fe Mon Sep 17 00:00:00 2001
From: Sebastian Verling <sverling@student.ethz.ch>
Date: Thu, 28 Apr 2016 08:43:37 +0200
Subject: [PATCH 0286/1230] formatting

---
 src/drivers/gps/gps.cpp | 4 +++-
 1 file changed, 3 insertions(+), 1 deletion(-)

diff --git a/src/drivers/gps/gps.cpp b/src/drivers/gps/gps.cpp
index b99406034b..6cd154d8af 100644
--- a/src/drivers/gps/gps.cpp
+++ b/src/drivers/gps/gps.cpp
@@ -803,7 +803,8 @@ GPS::print_info()
 void
 GPS::publish()
 {
-			orb_publish_auto(ORB_ID(vehicle_gps_position), &_report_gps_pos_pub, &_report_gps_pos, &_gps_orb_instance, ORB_PRIO_DEFAULT);
+	orb_publish_auto(ORB_ID(vehicle_gps_position), &_report_gps_pos_pub, &_report_gps_pos, &_gps_orb_instance,
+			 ORB_PRIO_DEFAULT);
 }
 
 /**
@@ -900,6 +901,7 @@ stop()
 	if (g_dev2 != nullptr) {
 		delete g_dev2;
 	}
+
 	g_dev2 = nullptr;
 
 	px4_task_exit(0);

From 34150ba68860e796b4a6702cc5904a0b4b848708 Mon Sep 17 00:00:00 2001
From: Sebastian Verling <sverling@student.ethz.ch>
Date: Fri, 29 Apr 2016 13:49:45 +0200
Subject: [PATCH 0287/1230] unadvertising gps_pub

---
 src/drivers/gps/gps.cpp | 1 +
 1 file changed, 1 insertion(+)

diff --git a/src/drivers/gps/gps.cpp b/src/drivers/gps/gps.cpp
index 6cd154d8af..7bd49486cc 100644
--- a/src/drivers/gps/gps.cpp
+++ b/src/drivers/gps/gps.cpp
@@ -278,6 +278,7 @@ GPS::~GPS()
 	}
 
 	g_dev = nullptr;
+	orb_unadvertise(_report_gps_pos_pub);
 
 }
 

From 07d1d78a43e9f6ec5d0ccd370ad525b0173b6f01 Mon Sep 17 00:00:00 2001
From: Sebastian Verling <sverling@student.ethz.ch>
Date: Wed, 4 May 2016 09:13:37 +0200
Subject: [PATCH 0288/1230] driver starting correctly

---
 src/drivers/gps/gps.cpp | 33 +++++++++++++++++++++++----------
 1 file changed, 23 insertions(+), 10 deletions(-)

diff --git a/src/drivers/gps/gps.cpp b/src/drivers/gps/gps.cpp
index 7bd49486cc..6a6dd7aa14 100644
--- a/src/drivers/gps/gps.cpp
+++ b/src/drivers/gps/gps.cpp
@@ -146,7 +146,7 @@ private:
 	/**
 	 * Trampoline to the worker task
 	 */
-	static void task_main_trampoline(void *arg);
+	static void task_main_trampoline(int argc, char *argv[]);
 
 	/**
 	 * Worker task: main GPS thread that configures the GPS and parses incoming data, always running
@@ -214,7 +214,7 @@ extern "C" __EXPORT int gps_main(int argc, char *argv[]);
 namespace
 {
 
-GPS	*g_dev = nullptr;
+GPS	*g_dev[2] = {nullptr, nullptr};
 
 }
 
@@ -242,7 +242,7 @@ GPS::GPS(const char *uart_path, bool fake_gps, bool enable_sat_info, int gps_num
 	_port[sizeof(_port) - 1] = '\0';
 
 	/* we need this potentially before it could be set in task_main */
-	g_dev = this;
+	g_dev[gps_num -1] = this;
 	memset(&_report_gps_pos, 0, sizeof(_report_gps_pos));
 
 	/* create satellite info data object if requested */
@@ -277,17 +277,23 @@ GPS::~GPS()
 		delete(_sat_info);
 	}
 
-	g_dev = nullptr;
-	orb_unadvertise(_report_gps_pos_pub);
+	g_dev[_gps_num-1] = nullptr;
 
 }
 
 int GPS::init()
 {
 
+	char gps_num;
+	sprintf(&gps_num, "%d", _gps_num);
+
+	static char *gps_num_ptr;
+	gps_num_ptr = &gps_num;
+
+
 	/* start the GPS driver worker task */
 	_task = px4_task_spawn_cmd("gps", SCHED_DEFAULT,
-				   SCHED_PRIORITY_SLOW_DRIVER, 1200, (px4_main_t)&GPS::task_main_trampoline, nullptr);
+				   SCHED_PRIORITY_SLOW_DRIVER, 1200, (px4_main_t)&GPS::task_main_trampoline, &gps_num_ptr);
 
 	if (_task < 0) {
 		PX4_WARN("task start failed: %d", errno);
@@ -297,9 +303,13 @@ int GPS::init()
 	return OK;
 }
 
-void GPS::task_main_trampoline(void *arg)
+void GPS::task_main_trampoline(int argc, char *argv[])
 {
-	g_dev->task_main();
+	warnx("arg = %i %c %i", argc, *argv[0], *argv[0]);
+	 if (!strcmp(argv[1], "1"))
+	 g_dev[0]->task_main();
+	 else if (!strcmp(argv[1], "2"))
+	 g_dev[1]->task_main();
 }
 
 int GPS::callback(GPSCallbackType type, void *data1, int data2, void *user)
@@ -730,6 +740,8 @@ GPS::task_main()
 
 	::close(_serial_fd);
 
+	orb_unadvertise(_report_gps_pos_pub);
+
 	/* tell the dtor that we are exiting */
 	_task = -1;
 	px4_task_exit(0);
@@ -832,6 +844,7 @@ start(const char *uart_path, bool fake_gps, bool enable_sat_info, int gps_num)
 	if (gps_num == 1) {
 		if (g_dev != nullptr) {
 			PX4_WARN("GPS 1 already started");
+			return;
 		}
 
 		/* create the driver */
@@ -844,13 +857,13 @@ start(const char *uart_path, bool fake_gps, bool enable_sat_info, int gps_num)
 		if (OK != g_dev->init()) {
 			goto fail1;
 		}
-
 		return;
 
 	} else {
 		if (gps_num == 2) {
 			if (g_dev2 != nullptr) {
 				PX4_WARN("GPS 2 already started");
+				return;
 			}
 
 			/* create the driver */
@@ -863,8 +876,8 @@ start(const char *uart_path, bool fake_gps, bool enable_sat_info, int gps_num)
 			if (OK != g_dev2->init()) {
 				goto fail2;
 			}
-
 			return;
+
 		}
 	}
 

From 7c5d10d57ca3b2cab02a601013d7251659bd3838 Mon Sep 17 00:00:00 2001
From: Sebastian Verling <sverling@student.ethz.ch>
Date: Wed, 4 May 2016 09:50:07 +0200
Subject: [PATCH 0289/1230] removed unnecessary pointer and comment

---
 src/drivers/gps/gps.cpp | 50 +++++++++++++++++++----------------------
 1 file changed, 23 insertions(+), 27 deletions(-)

diff --git a/src/drivers/gps/gps.cpp b/src/drivers/gps/gps.cpp
index 6a6dd7aa14..c0c2355b70 100644
--- a/src/drivers/gps/gps.cpp
+++ b/src/drivers/gps/gps.cpp
@@ -242,7 +242,6 @@ GPS::GPS(const char *uart_path, bool fake_gps, bool enable_sat_info, int gps_num
 	_port[sizeof(_port) - 1] = '\0';
 
 	/* we need this potentially before it could be set in task_main */
-	g_dev[gps_num -1] = this;
 	memset(&_report_gps_pos, 0, sizeof(_report_gps_pos));
 
 	/* create satellite info data object if requested */
@@ -277,7 +276,6 @@ GPS::~GPS()
 		delete(_sat_info);
 	}
 
-	g_dev[_gps_num-1] = nullptr;
 
 }
 
@@ -826,8 +824,6 @@ GPS::publish()
 namespace gps
 {
 
-GPS	*g_dev = nullptr;
-GPS	*g_dev2 = nullptr;
 
 void	start(const char *uart_path, bool fake_gps, bool enable_sat_info, int gps_num);
 void	stop();
@@ -842,38 +838,38 @@ void
 start(const char *uart_path, bool fake_gps, bool enable_sat_info, int gps_num)
 {
 	if (gps_num == 1) {
-		if (g_dev != nullptr) {
+		if (g_dev[0] != nullptr) {
 			PX4_WARN("GPS 1 already started");
 			return;
 		}
 
 		/* create the driver */
-		g_dev = new GPS(uart_path, fake_gps, enable_sat_info, gps_num);
+		g_dev[0] = new GPS(uart_path, fake_gps, enable_sat_info, gps_num);
 
-		if (g_dev == nullptr) {
+		if (g_dev[0] == nullptr) {
 			goto fail1;
 		}
 
-		if (OK != g_dev->init()) {
+		if (OK != g_dev[0]->init()) {
 			goto fail1;
 		}
 		return;
 
 	} else {
 		if (gps_num == 2) {
-			if (g_dev2 != nullptr) {
+			if (g_dev[1] != nullptr) {
 				PX4_WARN("GPS 2 already started");
 				return;
 			}
 
 			/* create the driver */
-			g_dev2 = new GPS(uart_path, fake_gps, enable_sat_info, gps_num);
+			g_dev[1] = new GPS(uart_path, fake_gps, enable_sat_info, gps_num);
 
-			if (g_dev2 == nullptr) {
+			if (g_dev[1] == nullptr) {
 				goto fail2;
 			}
 
-			if (OK != g_dev2->init()) {
+			if (OK != g_dev[1]->init()) {
 				goto fail2;
 			}
 			return;
@@ -884,9 +880,9 @@ start(const char *uart_path, bool fake_gps, bool enable_sat_info, int gps_num)
 
 fail1:
 
-	if (g_dev != nullptr) {
-		delete g_dev;
-		g_dev = nullptr;
+	if (g_dev[0] != nullptr) {
+		delete g_dev[0];
+		g_dev[0] = nullptr;
 	}
 
 	PX4_ERR("start of GPS 1 failed");
@@ -894,9 +890,9 @@ fail1:
 
 fail2:
 
-	if (g_dev2 != nullptr) {
-		delete g_dev2;
-		g_dev2 = nullptr;
+	if (g_dev[1] != nullptr) {
+		delete g_dev[1];
+		g_dev[1] = nullptr;
 	}
 
 	PX4_ERR("start of GPS 2 failed");
@@ -909,14 +905,14 @@ fail2:
 void
 stop()
 {
-	delete g_dev;
-	g_dev = nullptr;
+	delete g_dev[0];
+	g_dev[0] = nullptr;
 
-	if (g_dev2 != nullptr) {
-		delete g_dev2;
+	if (g_dev[1] != nullptr) {
+		delete g_dev[1];
 	}
 
-	g_dev2 = nullptr;
+	g_dev[1] = nullptr;
 
 	px4_task_exit(0);
 }
@@ -949,15 +945,15 @@ reset()
 void
 info()
 {
-	if (g_dev == nullptr) {
+	if (g_dev[0] == nullptr) {
 		PX4_ERR("GPS Not running");
 		return;
 	}
 
-	g_dev->print_info();
+	g_dev[0]->print_info();
 
-	if (g_dev2 != nullptr) {
-		g_dev2->print_info();
+	if (g_dev[1] != nullptr) {
+		g_dev[1]->print_info();
 	}
 
 	return;

From 49e1c99deed67f40724ca0494a60d4f54926d543 Mon Sep 17 00:00:00 2001
From: Sebastian Verling <sverling@student.ethz.ch>
Date: Wed, 4 May 2016 13:02:26 +0200
Subject: [PATCH 0290/1230] make sure gps1 always publishes to gps1 topic

---
 src/drivers/gps/gps.cpp | 10 +++++++++-
 1 file changed, 9 insertions(+), 1 deletion(-)

diff --git a/src/drivers/gps/gps.cpp b/src/drivers/gps/gps.cpp
index c0c2355b70..4de1f3e77f 100644
--- a/src/drivers/gps/gps.cpp
+++ b/src/drivers/gps/gps.cpp
@@ -215,7 +215,7 @@ namespace
 {
 
 GPS	*g_dev[2] = {nullptr, nullptr};
-
+volatile bool is_gps1_advertised = false;
 }
 
 
@@ -814,8 +814,16 @@ GPS::print_info()
 void
 GPS::publish()
 {
+	if(_gps_num == 1){
 	orb_publish_auto(ORB_ID(vehicle_gps_position), &_report_gps_pos_pub, &_report_gps_pos, &_gps_orb_instance,
 			 ORB_PRIO_DEFAULT);
+	is_gps1_advertised = true;
+	}
+	else if(is_gps1_advertised){
+			orb_publish_auto(ORB_ID(vehicle_gps_position), &_report_gps_pos_pub, &_report_gps_pos, &_gps_orb_instance,
+			 ORB_PRIO_DEFAULT);
+	}
+
 }
 
 /**

From 62a152227aebfca3542c0e1007161254277c403a Mon Sep 17 00:00:00 2001
From: Sebastian Verling <sverling@student.ethz.ch>
Date: Wed, 4 May 2016 13:15:41 +0200
Subject: [PATCH 0291/1230] removed debugging line

---
 src/drivers/gps/gps.cpp | 1 -
 1 file changed, 1 deletion(-)

diff --git a/src/drivers/gps/gps.cpp b/src/drivers/gps/gps.cpp
index 4de1f3e77f..ac14eb314e 100644
--- a/src/drivers/gps/gps.cpp
+++ b/src/drivers/gps/gps.cpp
@@ -303,7 +303,6 @@ int GPS::init()
 
 void GPS::task_main_trampoline(int argc, char *argv[])
 {
-	warnx("arg = %i %c %i", argc, *argv[0], *argv[0]);
 	 if (!strcmp(argv[1], "1"))
 	 g_dev[0]->task_main();
 	 else if (!strcmp(argv[1], "2"))

From e7ed07cf3c4a1fbc78e0f4eee350fa086a42a23c Mon Sep 17 00:00:00 2001
From: Sebastian Verling <sverling@student.ethz.ch>
Date: Wed, 4 May 2016 13:50:08 +0200
Subject: [PATCH 0292/1230] formatting

---
 src/drivers/gps/gps.cpp | 28 ++++++++++++++++------------
 1 file changed, 16 insertions(+), 12 deletions(-)

diff --git a/src/drivers/gps/gps.cpp b/src/drivers/gps/gps.cpp
index ac14eb314e..c340f18886 100644
--- a/src/drivers/gps/gps.cpp
+++ b/src/drivers/gps/gps.cpp
@@ -303,10 +303,12 @@ int GPS::init()
 
 void GPS::task_main_trampoline(int argc, char *argv[])
 {
-	 if (!strcmp(argv[1], "1"))
-	 g_dev[0]->task_main();
-	 else if (!strcmp(argv[1], "2"))
-	 g_dev[1]->task_main();
+	if (!strcmp(argv[1], "1")) {
+		g_dev[0]->task_main();
+
+	} else if (!strcmp(argv[1], "2")) {
+		g_dev[1]->task_main();
+	}
 }
 
 int GPS::callback(GPSCallbackType type, void *data1, int data2, void *user)
@@ -813,14 +815,14 @@ GPS::print_info()
 void
 GPS::publish()
 {
-	if(_gps_num == 1){
-	orb_publish_auto(ORB_ID(vehicle_gps_position), &_report_gps_pos_pub, &_report_gps_pos, &_gps_orb_instance,
-			 ORB_PRIO_DEFAULT);
-	is_gps1_advertised = true;
-	}
-	else if(is_gps1_advertised){
-			orb_publish_auto(ORB_ID(vehicle_gps_position), &_report_gps_pos_pub, &_report_gps_pos, &_gps_orb_instance,
-			 ORB_PRIO_DEFAULT);
+	if (_gps_num == 1) {
+		orb_publish_auto(ORB_ID(vehicle_gps_position), &_report_gps_pos_pub, &_report_gps_pos, &_gps_orb_instance,
+				 ORB_PRIO_DEFAULT);
+		is_gps1_advertised = true;
+
+	} else if (is_gps1_advertised) {
+		orb_publish_auto(ORB_ID(vehicle_gps_position), &_report_gps_pos_pub, &_report_gps_pos, &_gps_orb_instance,
+				 ORB_PRIO_DEFAULT);
 	}
 
 }
@@ -860,6 +862,7 @@ start(const char *uart_path, bool fake_gps, bool enable_sat_info, int gps_num)
 		if (OK != g_dev[0]->init()) {
 			goto fail1;
 		}
+
 		return;
 
 	} else {
@@ -879,6 +882,7 @@ start(const char *uart_path, bool fake_gps, bool enable_sat_info, int gps_num)
 			if (OK != g_dev[1]->init()) {
 				goto fail2;
 			}
+
 			return;
 
 		}

From c6f25591f716d5a9199413bfd2e67a1477e9b6d6 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Beat=20K=C3=BCng?= <beat-kueng@gmx.net>
Date: Wed, 4 May 2016 16:16:46 +0200
Subject: [PATCH 0293/1230] dual gps: some cleanup, correctly use args argument
 of px4_task_spawn_cmd

---
 src/drivers/gps/gps.cpp | 94 +++++++++--------------------------------
 1 file changed, 20 insertions(+), 74 deletions(-)

diff --git a/src/drivers/gps/gps.cpp b/src/drivers/gps/gps.cpp
index c340f18886..55290218d2 100644
--- a/src/drivers/gps/gps.cpp
+++ b/src/drivers/gps/gps.cpp
@@ -215,7 +215,8 @@ namespace
 {
 
 GPS	*g_dev[2] = {nullptr, nullptr};
-volatile bool is_gps1_advertised = false;
+volatile bool is_gps1_advertised = false; ///< for the second gps we want to make sure that it gets instance 1
+/// and thus we wait until the first one publishes at least one message.
 }
 
 
@@ -282,16 +283,12 @@ GPS::~GPS()
 int GPS::init()
 {
 
-	char gps_num;
-	sprintf(&gps_num, "%d", _gps_num);
-
-	static char *gps_num_ptr;
-	gps_num_ptr = &gps_num;
-
+	char gps_num[2] = {(char)('0' + _gps_num), 0};
+	char *const args[2] = { gps_num, NULL };
 
 	/* start the GPS driver worker task */
 	_task = px4_task_spawn_cmd("gps", SCHED_DEFAULT,
-				   SCHED_PRIORITY_SLOW_DRIVER, 1200, (px4_main_t)&GPS::task_main_trampoline, &gps_num_ptr);
+				   SCHED_PRIORITY_SLOW_DRIVER, 1200, (px4_main_t)&GPS::task_main_trampoline, args);
 
 	if (_task < 0) {
 		PX4_WARN("task start failed: %d", errno);
@@ -303,12 +300,7 @@ int GPS::init()
 
 void GPS::task_main_trampoline(int argc, char *argv[])
 {
-	if (!strcmp(argv[1], "1")) {
-		g_dev[0]->task_main();
-
-	} else if (!strcmp(argv[1], "2")) {
-		g_dev[1]->task_main();
-	}
+	g_dev[argv[1][0] - '1']->task_main();
 }
 
 int GPS::callback(GPSCallbackType type, void *data1, int data2, void *user)
@@ -764,6 +756,8 @@ GPS::cmd_reset()
 void
 GPS::print_info()
 {
+	PX4_WARN("GPS %i:", _gps_num);
+
 	//GPS Mode
 	if (_fake_gps) {
 		PX4_WARN("protocol: SIMULATED");
@@ -846,68 +840,22 @@ void	info();
 void
 start(const char *uart_path, bool fake_gps, bool enable_sat_info, int gps_num)
 {
-	if (gps_num == 1) {
-		if (g_dev[0] != nullptr) {
-			PX4_WARN("GPS 1 already started");
-			return;
-		}
-
-		/* create the driver */
-		g_dev[0] = new GPS(uart_path, fake_gps, enable_sat_info, gps_num);
-
-		if (g_dev[0] == nullptr) {
-			goto fail1;
-		}
-
-		if (OK != g_dev[0]->init()) {
-			goto fail1;
-		}
-
+	if (g_dev[gps_num - 1] != nullptr) {
+		PX4_WARN("GPS %i already started", gps_num);
 		return;
+	}
 
-	} else {
-		if (gps_num == 2) {
-			if (g_dev[1] != nullptr) {
-				PX4_WARN("GPS 2 already started");
-				return;
-			}
-
-			/* create the driver */
-			g_dev[1] = new GPS(uart_path, fake_gps, enable_sat_info, gps_num);
-
-			if (g_dev[1] == nullptr) {
-				goto fail2;
-			}
-
-			if (OK != g_dev[1]->init()) {
-				goto fail2;
-			}
-
-			return;
+	/* create the driver */
+	g_dev[gps_num - 1] = new GPS(uart_path, fake_gps, enable_sat_info, gps_num);
 
+	if (!g_dev[gps_num - 1] || OK != g_dev[gps_num - 1]->init()) {
+		if (g_dev[gps_num - 1] != nullptr) {
+			delete g_dev[gps_num - 1];
+			g_dev[gps_num - 1] = nullptr;
 		}
+
+		PX4_ERR("start of GPS %i failed", gps_num);
 	}
-
-
-fail1:
-
-	if (g_dev[0] != nullptr) {
-		delete g_dev[0];
-		g_dev[0] = nullptr;
-	}
-
-	PX4_ERR("start of GPS 1 failed");
-	return;
-
-fail2:
-
-	if (g_dev[1] != nullptr) {
-		delete g_dev[1];
-		g_dev[1] = nullptr;
-	}
-
-	PX4_ERR("start of GPS 2 failed");
-	return;
 }
 
 /**
@@ -924,8 +872,6 @@ stop()
 	}
 
 	g_dev[1] = nullptr;
-
-	px4_task_exit(0);
 }
 
 /**
@@ -1029,7 +975,7 @@ gps_main(int argc, char *argv[])
 
 		gps::start(device_name, fake_gps, enable_sat_info, 1);
 
-		if (!(device_name2 == nullptr)) {
+		if (device_name2) {
 			gps::start(device_name2, fake_gps, enable_sat_info, 2);
 		}
 

From f600b7fb3779910c384a12f146a6f190fc3a1e6f Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Beat=20K=C3=BCng?= <beat-kueng@gmx.net>
Date: Wed, 4 May 2016 16:17:17 +0200
Subject: [PATCH 0294/1230] fix gps regression: publish satellite info

---
 src/drivers/gps/gps.cpp | 20 +++++++++++++++++++-
 1 file changed, 19 insertions(+), 1 deletion(-)

diff --git a/src/drivers/gps/gps.cpp b/src/drivers/gps/gps.cpp
index 55290218d2..addac9e489 100644
--- a/src/drivers/gps/gps.cpp
+++ b/src/drivers/gps/gps.cpp
@@ -127,6 +127,7 @@ private:
 	orb_advert_t			_report_gps_pos_pub;				///< uORB pub for gps position
 	int					_gps_orb_instance;				///< uORB multi-topic instance
 	struct satellite_info_s		*_p_report_sat_info;				///< pointer to uORB topic for satellite info
+	int					_gps_sat_orb_instance;				///< uORB multi-topic instance for satellite info
 	orb_advert_t			_report_sat_info_pub;				///< uORB pub for satellite info
 	float				_rate;						///< position update rate
 	float				_rate_rtcm_injection;				///< RTCM message injection rate
@@ -168,6 +169,11 @@ private:
 	 */
 	void 				publish();
 
+	/**
+	 * Publish the satellite info
+	 */
+	void 				publishSatelliteInfo();
+
 	/**
 	 * This is an abstraction for the poll on serial used.
 	 *
@@ -652,7 +658,7 @@ GPS::task_main()
 					}
 
 					if (_p_report_sat_info && (helper_ret & 2)) {
-						publish();
+						publishSatelliteInfo();
 					}
 
 					/* measure update rate every 5 seconds */
@@ -819,6 +825,18 @@ GPS::publish()
 				 ORB_PRIO_DEFAULT);
 	}
 
+}
+void
+GPS::publishSatelliteInfo()
+{
+	if (_gps_num == 1) {
+		orb_publish_auto(ORB_ID(satellite_info), &_report_sat_info_pub, _p_report_sat_info, &_gps_sat_orb_instance,
+				 ORB_PRIO_DEFAULT);
+
+	} else {
+		//we don't publish satellite info for the secondary gps
+	}
+
 }
 
 /**

From 9d0c5469a066e51ca699470f169e2073aa181bac Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Beat=20K=C3=BCng?= <beat-kueng@gmx.net>
Date: Wed, 4 May 2016 19:47:46 +0200
Subject: [PATCH 0295/1230] gps: remove unneeded comment

---
 src/drivers/gps/gps.cpp | 1 -
 1 file changed, 1 deletion(-)

diff --git a/src/drivers/gps/gps.cpp b/src/drivers/gps/gps.cpp
index addac9e489..982607855a 100644
--- a/src/drivers/gps/gps.cpp
+++ b/src/drivers/gps/gps.cpp
@@ -248,7 +248,6 @@ GPS::GPS(const char *uart_path, bool fake_gps, bool enable_sat_info, int gps_num
 	/* enforce null termination */
 	_port[sizeof(_port) - 1] = '\0';
 
-	/* we need this potentially before it could be set in task_main */
 	memset(&_report_gps_pos, 0, sizeof(_report_gps_pos));
 
 	/* create satellite info data object if requested */

From 47ed8c1a3cb8e580b4e878c1a7676ac71ec1ef2e Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Beat=20K=C3=BCng?= <beat-kueng@gmx.net>
Date: Wed, 11 May 2016 14:55:19 +0200
Subject: [PATCH 0296/1230] gps: fix thread initialization under posix

under NuttX the argc in task_main_trampoline contains two arguments,
but on linux only one.
---
 src/drivers/gps/gps.cpp | 3 ++-
 1 file changed, 2 insertions(+), 1 deletion(-)

diff --git a/src/drivers/gps/gps.cpp b/src/drivers/gps/gps.cpp
index 982607855a..ff359146cb 100644
--- a/src/drivers/gps/gps.cpp
+++ b/src/drivers/gps/gps.cpp
@@ -297,6 +297,7 @@ int GPS::init()
 
 	if (_task < 0) {
 		PX4_WARN("task start failed: %d", errno);
+		_task = -1;
 		return -errno;
 	}
 
@@ -305,7 +306,7 @@ int GPS::init()
 
 void GPS::task_main_trampoline(int argc, char *argv[])
 {
-	g_dev[argv[1][0] - '1']->task_main();
+	g_dev[argv[argc - 1][0] - '1']->task_main();
 }
 
 int GPS::callback(GPSCallbackType type, void *data1, int data2, void *user)

From aef78b8aeb5bd196d7856899ea60933806500890 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Beat=20K=C3=BCng?= <beat-kueng@gmx.net>
Date: Thu, 12 May 2016 17:31:08 +0200
Subject: [PATCH 0297/1230] gps: add GPS_DUMP_COMM param: if 1, dump all gps
 communication to a file

---
 src/drivers/gps/gps.cpp  | 152 ++++++++++++++++++++++++++++++++++++++-
 src/drivers/gps/params.c |  48 +++++++++++++
 2 files changed, 198 insertions(+), 2 deletions(-)
 create mode 100644 src/drivers/gps/params.c

diff --git a/src/drivers/gps/gps.cpp b/src/drivers/gps/gps.cpp
index ff359146cb..1088360cdd 100644
--- a/src/drivers/gps/gps.cpp
+++ b/src/drivers/gps/gps.cpp
@@ -52,6 +52,7 @@
 
 
 #include <fcntl.h>
+#include <sys/stat.h>
 #include <sys/types.h>
 #include <stdint.h>
 #include <stdio.h>
@@ -71,9 +72,11 @@
 #include <systemlib/systemlib.h>
 #include <systemlib/scheduling_priorities.h>
 #include <systemlib/err.h>
+#include <systemlib/param/param.h>
 #include <drivers/drv_gps.h>
 #include <uORB/uORB.h>
 #include <uORB/topics/vehicle_gps_position.h>
+#include <uORB/topics/vehicle_status.h>
 #include <uORB/topics/satellite_info.h>
 #include <uORB/topics/gps_inject_data.h>
 
@@ -139,6 +142,17 @@ private:
 	int _orb_inject_data_fd[_orb_inject_data_fd_count];
 	int _orb_inject_data_next = 0;
 
+	// dump communication to file
+#ifdef __PX4_POSIX_EAGLE
+	static constexpr const char	*DUMP_ROOT = PX4_ROOTFSDIR;
+#else
+	static constexpr const char 	*DUMP_ROOT = PX4_ROOTFSDIR"/fs/microsd";
+#endif
+	int _dump_to_gps_device_fd = -1;              ///< file descriptors, to safe all device communication to a file
+	int _dump_from_gps_device_fd = -1;
+	int _vehicle_status_sub = -1;
+	bool _is_armed = false;                       ///< current arming state (only updated if communication dump is enabled)
+
 	/**
 	 * Try to configure the GPS, handle outgoing communication to the GPS
 	 */
@@ -209,6 +223,15 @@ private:
 	 * callback from the driver for the platform specific stuff
 	 */
 	static int callback(GPSCallbackType type, void *data1, int data2, void *user);
+
+	/**
+	 * Setup dumping of the GPS device input and output stream to a file.
+	 */
+	void initializeCommunicationDump();
+
+	int getDumpFileName(char *file_name, size_t file_name_size, const char *suffix);
+
+	static bool fileExist(const char *filename);
 };
 
 
@@ -314,10 +337,25 @@ int GPS::callback(GPSCallbackType type, void *data1, int data2, void *user)
 	GPS *gps = (GPS *)user;
 
 	switch (type) {
-	case GPSCallbackType::readDeviceData:
-		return gps->pollOrRead((uint8_t *)data1, data2, *((int *)data1));
+	case GPSCallbackType::readDeviceData: {
+			int num_read = gps->pollOrRead((uint8_t *)data1, data2, *((int *)data1));
+
+			if (num_read > 0 && gps->_dump_from_gps_device_fd >= 0) {
+				if (write(gps->_dump_from_gps_device_fd, data1, (size_t)num_read) != (size_t)num_read) {
+					PX4_WARN("gps dump failed");
+				}
+			}
+
+			return num_read;
+		}
 
 	case GPSCallbackType::writeDeviceData:
+		if (gps->_dump_to_gps_device_fd >= 0) {
+			if (write(gps->_dump_to_gps_device_fd, data1, (size_t)data2) != (size_t)data2) {
+				PX4_WARN("gps dump failed");
+			}
+		}
+
 		return write(gps->_serial_fd, data1, (size_t)data2);
 
 	case GPSCallbackType::setBaudrate:
@@ -529,6 +567,73 @@ int GPS::setBaudrate(unsigned baud)
 	return 0;
 }
 
+bool GPS::fileExist(const char *filename)
+{
+	struct stat buffer;
+	return stat(filename, &buffer) == 0;
+}
+
+void GPS::initializeCommunicationDump()
+{
+	param_t gps_dump_comm_ph = param_find("GPS_DUMP_COMM");
+	int32_t param_dump_comm;
+
+	if (gps_dump_comm_ph == PARAM_INVALID || param_get(gps_dump_comm_ph, &param_dump_comm) != 0) {
+		return;
+	}
+
+	if (param_dump_comm != 1) {
+		return; //dumping disabled
+	}
+
+	char to_device_file_name[128] = "";
+	char from_device_file_name[128] = "";
+
+	if (getDumpFileName(to_device_file_name, sizeof(to_device_file_name), "to")
+	    || getDumpFileName(from_device_file_name, sizeof(from_device_file_name), "from")) {
+		PX4_ERR("Failed to get GPS dump file name");
+		return;
+	}
+
+	//open files
+	if ((_dump_to_gps_device_fd = open(to_device_file_name, O_CREAT | O_WRONLY, PX4_O_MODE_666)) < 0) {
+		return;
+	}
+
+	if ((_dump_from_gps_device_fd = open(from_device_file_name, O_CREAT | O_WRONLY, PX4_O_MODE_666)) < 0) {
+		close(_dump_to_gps_device_fd);
+		_dump_to_gps_device_fd = -1;
+		return;
+	}
+
+	PX4_INFO("Dumping GPS comm to files %s, %s", to_device_file_name, from_device_file_name);
+
+	_vehicle_status_sub = orb_subscribe(ORB_ID(vehicle_status));
+}
+
+int GPS::getDumpFileName(char *file_name, size_t file_name_size, const char *suffix)
+{
+	uint16_t file_number = 1; // start with file gps_dump_<num>_<suffix>_dev_001.dat
+
+	const uint16_t max_num_files = 999;
+
+	while (file_number <= max_num_files) {
+		snprintf(file_name, file_name_size, "%s/gps_dump_%u_%s_dev_%03u.dat",
+			 DUMP_ROOT, _gps_num, suffix, file_number);
+
+		if (!fileExist(file_name)) {
+			break;
+		}
+
+		file_number++;
+	}
+
+	if (file_number > max_num_files) {
+		return -1;
+	}
+
+	return 0;
+}
 
 void
 GPS::task_main()
@@ -555,6 +660,8 @@ GPS::task_main()
 		_orb_inject_data_fd[i] = orb_subscribe_multi(ORB_ID(gps_inject_data), i);
 	}
 
+	initializeCommunicationDump();
+
 	uint64_t last_rate_measurement = hrt_absolute_time();
 	unsigned last_rate_count = 0;
 
@@ -697,6 +804,32 @@ GPS::task_main()
 //						PX4_WARN("module found: %s", mode_str);
 						_healthy = true;
 					}
+
+					//check for disarming
+					if (_vehicle_status_sub != -1 && _dump_from_gps_device_fd != -1) {
+						bool updated;
+						orb_check(_vehicle_status_sub, &updated);
+
+						if (updated) {
+							vehicle_status_s vehicle_status;
+							orb_copy(ORB_ID(vehicle_status), _vehicle_status_sub, &vehicle_status);
+							bool armed = (vehicle_status.arming_state == vehicle_status_s::ARMING_STATE_ARMED) ||
+								     (vehicle_status.arming_state == vehicle_status_s::ARMING_STATE_ARMED_ERROR);
+
+							if (armed) {
+								_is_armed = true;
+
+							} else if (_is_armed) {
+								//disable communication dump when disarming
+								close(_dump_from_gps_device_fd);
+								_dump_from_gps_device_fd = -1;
+								close(_dump_to_gps_device_fd);
+								_dump_to_gps_device_fd = -1;
+								_is_armed = false;
+							}
+
+						}
+					}
 				}
 
 				if (_healthy) {
@@ -735,6 +868,21 @@ GPS::task_main()
 		_orb_inject_data_fd[i] = -1;
 	}
 
+	if (_dump_to_gps_device_fd != -1) {
+		close(_dump_to_gps_device_fd);
+		_dump_to_gps_device_fd = -1;
+	}
+
+	if (_dump_from_gps_device_fd != -1) {
+		close(_dump_from_gps_device_fd);
+		_dump_from_gps_device_fd = -1;
+	}
+
+	if (_vehicle_status_sub != -1) {
+		orb_unsubscribe(_vehicle_status_sub);
+		_vehicle_status_sub = -1;
+	}
+
 	::close(_serial_fd);
 
 	orb_unadvertise(_report_gps_pos_pub);
diff --git a/src/drivers/gps/params.c b/src/drivers/gps/params.c
new file mode 100644
index 0000000000..9baba96507
--- /dev/null
+++ b/src/drivers/gps/params.c
@@ -0,0 +1,48 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2016 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * Dump GPS communication to a file.
+ *
+ * If this is set to 1, all GPS communication data will be written to a
+ * file. Two files will be created, for reading and writing. All
+ * communication from startup until device disarm will be dumped.
+ *
+ * @min 0
+ * @max 1
+ * @value 0 Disable
+ * @value 1 Enable
+ * @group GPS
+ */
+PARAM_DEFINE_INT32(GPS_DUMP_COMM, 0);
+

From 6b8ab4611b6eb1a7826c08d0637badc103ca07c1 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Beat=20K=C3=BCng?= <beat-kueng@gmx.net>
Date: Mon, 16 May 2016 10:49:46 +0200
Subject: [PATCH 0298/1230] gps: workaround for missing stat() on QuRT

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

diff --git a/src/drivers/gps/gps.cpp b/src/drivers/gps/gps.cpp
index 1088360cdd..aab20d204d 100644
--- a/src/drivers/gps/gps.cpp
+++ b/src/drivers/gps/gps.cpp
@@ -569,8 +569,13 @@ int GPS::setBaudrate(unsigned baud)
 
 bool GPS::fileExist(const char *filename)
 {
+#ifdef __PX4_QURT
+	//FIXME: QuRT neither has stat() nor access(). Just pretend file does not exist.
+	return false;
+#else
 	struct stat buffer;
 	return stat(filename, &buffer) == 0;
+#endif
 }
 
 void GPS::initializeCommunicationDump()
@@ -596,11 +601,11 @@ void GPS::initializeCommunicationDump()
 	}
 
 	//open files
-	if ((_dump_to_gps_device_fd = open(to_device_file_name, O_CREAT | O_WRONLY, PX4_O_MODE_666)) < 0) {
+	if ((_dump_to_gps_device_fd = open(to_device_file_name, O_CREAT | O_WRONLY | O_TRUNC, PX4_O_MODE_666)) < 0) {
 		return;
 	}
 
-	if ((_dump_from_gps_device_fd = open(from_device_file_name, O_CREAT | O_WRONLY, PX4_O_MODE_666)) < 0) {
+	if ((_dump_from_gps_device_fd = open(from_device_file_name, O_CREAT | O_WRONLY | O_TRUNC, PX4_O_MODE_666)) < 0) {
 		close(_dump_to_gps_device_fd);
 		_dump_to_gps_device_fd = -1;
 		return;

From a94654e8d4a2eb3609ecb5f5caf7bc5cb0d37970 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Beat=20K=C3=BCng?= <beat-kueng@gmx.net>
Date: Mon, 16 May 2016 11:38:55 +0200
Subject: [PATCH 0299/1230] gps dump communication to file: include received
 RTCM messages

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

diff --git a/src/drivers/gps/gps.cpp b/src/drivers/gps/gps.cpp
index aab20d204d..b981e80ec3 100644
--- a/src/drivers/gps/gps.cpp
+++ b/src/drivers/gps/gps.cpp
@@ -452,6 +452,12 @@ void GPS::handleInjectDataTopic()
 
 bool GPS::injectData(uint8_t *data, size_t len)
 {
+	if (_dump_to_gps_device_fd >= 0) {
+		if (write(_dump_to_gps_device_fd, data, len) != len) {
+			PX4_WARN("gps dump failed");
+		}
+	}
+
 	return ::write(_serial_fd, data, len) == len;
 }
 

From 034bbdf9887bbc1bd37fa0efeaab8460f2a8ad6d Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Sun, 22 May 2016 12:44:07 +0200
Subject: [PATCH 0300/1230] Fix battery topic handling in SMBus battery

---
 src/drivers/batt_smbus/batt_smbus.cpp | 10 +++++-----
 1 file changed, 5 insertions(+), 5 deletions(-)

diff --git a/src/drivers/batt_smbus/batt_smbus.cpp b/src/drivers/batt_smbus/batt_smbus.cpp
index b2faa43e1d..47b963df78 100644
--- a/src/drivers/batt_smbus/batt_smbus.cpp
+++ b/src/drivers/batt_smbus/batt_smbus.cpp
@@ -278,7 +278,7 @@ private:
 	// internal variables
 	bool			_enabled;	///< true if we have successfully connected to battery
 	work_s			_work;		///< work queue for scheduling reads
-	RingBuffer		*_reports;	///< buffer of recorded voltages, currents
+	ringbuffer::RingBuffer	*_reports;	///< buffer of recorded voltages, currents
 	struct battery_status_s _last_report;	///< last published report, used for test()
 	orb_advert_t		_batt_topic;	///< uORB battery topic
 	orb_id_t		_batt_orb_id;	///< uORB battery topic ID
@@ -312,7 +312,7 @@ BATT_SMBUS::BATT_SMBUS(int bus, uint16_t batt_smbus_addr) :
 	_enabled(false),
 	_work{},
 	_reports(nullptr),
-	_batt_topic(-1),
+	_batt_topic(nullptr),
 	_batt_orb_id(nullptr),
 	_start_time(0),
 	_batt_capacity(0),
@@ -365,7 +365,7 @@ BATT_SMBUS::init()
 
 	} else {
 		// allocate basic report buffers
-		_reports = new RingBuffer(2, sizeof(struct battery_status_s));
+		_reports = new ringbuffer::RingBuffer(2, sizeof(struct battery_status_s));
 
 		if (_reports == nullptr) {
 			ret = ENOTTY;
@@ -706,13 +706,13 @@ BATT_SMBUS::cycle()
 
 
 		// publish to orb
-		if (_batt_topic != -1) {
+		if (_batt_topic != nullptr) {
 			orb_publish(_batt_orb_id, _batt_topic, &new_report);
 
 		} else {
 			_batt_topic = orb_advertise(_batt_orb_id, &new_report);
 
-			if (_batt_topic < 0) {
+			if (_batt_topic == nullptr) {
 				errx(1, "ADVERT FAIL");
 			}
 		}

From 8bc550b619e518d269463bd37a05f59aceddd666 Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <andrew@tridgell.net>
Date: Tue, 12 Apr 2016 18:46:01 +1000
Subject: [PATCH 0301/1230] oreoled: align with ArduPilot version

---
 src/drivers/drv_oreoled.h       |   72 +-
 src/drivers/oreoled/oreoled.cpp | 1399 ++++++++++++++++++++++++++++++-
 2 files changed, 1440 insertions(+), 31 deletions(-)

diff --git a/src/drivers/drv_oreoled.h b/src/drivers/drv_oreoled.h
index 00e3683184..8be6eb8d1a 100644
--- a/src/drivers/drv_oreoled.h
+++ b/src/drivers/drv_oreoled.h
@@ -61,6 +61,33 @@
 /** send bytes */
 #define OREOLED_SEND_BYTES		_OREOLEDIOC(3)
 
+/** send reset */
+#define OREOLED_SEND_RESET		_OREOLEDIOC(4)
+
+/** boot ping */
+#define OREOLED_BL_PING			_OREOLEDIOC(5)
+
+/** boot version */
+#define OREOLED_BL_VER			_OREOLEDIOC(6)
+
+/** boot write flash */
+#define OREOLED_BL_FLASH		_OREOLEDIOC(7)
+
+/** boot application version */
+#define OREOLED_BL_APP_VER		_OREOLEDIOC(8)
+
+/** boot application crc */
+#define OREOLED_BL_APP_CRC		_OREOLEDIOC(9)
+
+/** boot startup colour */
+#define OREOLED_BL_SET_COLOUR	_OREOLEDIOC(10)
+
+/** boot application */
+#define OREOLED_BL_BOOT_APP		_OREOLEDIOC(11)
+
+/** force an i2c gencall */
+#define OREOLED_FORCE_SYNC		_OREOLEDIOC(12)
+
 /* Oreo LED driver supports up to 4 leds */
 #define OREOLED_NUM_LEDS		4
 
@@ -68,7 +95,46 @@
 #define OREOLED_ALL_INSTANCES	0xff
 
 /* maximum command length that can be sent to LEDs */
-#define OREOLED_CMD_LENGTH_MAX	24
+#define OREOLED_CMD_LENGTH_MAX	70
+
+/* maximum command length that can be read from LEDs */
+#define OREOLED_CMD_READ_LENGTH_MAX	10
+
+/* maximum number of commands retries */
+#define OEROLED_COMMAND_RETRIES	10
+
+/* magic number used to verify the software reset is valid */
+#define OEROLED_RESET_NONCE				0x2A
+
+/* microseconds to hold-off between write and reads */
+#define OREOLED_WRITE_READ_HOLDOFF_US	500
+
+/* microseconds to hold-off between retries */
+#define OREOLED_RETRY_HOLDOFF_US		200
+
+#define OEROLED_BOOT_COMMAND_RETRIES	25
+#define OREOLED_BOOT_FLASH_WAITMS		10
+
+#define OREOLED_BOOT_SUPPORTED_VER		0x01
+
+#define OREOLED_BOOT_CMD_PING			0x40
+#define OREOLED_BOOT_CMD_BL_VER			0x41
+#define OREOLED_BOOT_CMD_APP_VER		0x42
+#define OREOLED_BOOT_CMD_APP_CRC		0x43
+#define OREOLED_BOOT_CMD_SET_COLOUR		0x44
+
+#define OREOLED_BOOT_CMD_WRITE_FLASH_A	0x50
+#define OREOLED_BOOT_CMD_WRITE_FLASH_B	0x51
+#define OREOLED_BOOT_CMD_FINALISE_FLASH	0x55
+
+#define OREOLED_BOOT_CMD_BOOT_APP		0x60
+
+#define OREOLED_BOOT_CMD_PING_NONCE		0x2A
+#define OREOLED_BOOT_CMD_BOOT_NONCE		0xA2
+
+#define OREOLED_FW_FILE_HEADER_LENGTH	2
+#define OREOLED_FW_FILE_SIZE_LIMIT		6144
+#define OREOLED_FW_FILE					"/etc/firmware/oreoled.bin"
 
 /* enum passed to OREOLED_SET_MODE ioctl()
  *	defined by hardware */
@@ -97,6 +163,8 @@ enum oreoled_param {
 	OREOLED_PARAM_REPEAT = 7,
 	OREOLED_PARAM_PHASEOFFSET = 8,
 	OREOLED_PARAM_MACRO = 9,
+	OREOLED_PARAM_RESET = 10,
+	OREOLED_PARAM_APP_CHECKSUM = 11,
 	OREOLED_PARAM_ENUM_COUNT
 };
 
@@ -114,6 +182,8 @@ enum oreoled_macro {
 	OREOLED_PARAM_MACRO_BLUE = 8,
 	OREOLED_PARAM_MACRO_YELLOW = 9,
 	OREOLED_PARAM_MACRO_WHITE = 10,
+	OREOLED_PARAM_MACRO_AUTOMOBILE = 11,
+	OREOLED_PARAM_MACRO_AVIATION = 12,
 	OREOLED_PARAM_MACRO_ENUM_COUNT
 };
 
diff --git a/src/drivers/oreoled/oreoled.cpp b/src/drivers/oreoled/oreoled.cpp
index 5ee8dc030b..bbf96b98f4 100644
--- a/src/drivers/oreoled/oreoled.cpp
+++ b/src/drivers/oreoled/oreoled.cpp
@@ -35,7 +35,7 @@
 /**
  * @file oreoled.cpp
  *
- * Driver for the onboard RGB LED controller (TCA62724FMG) connected via I2C.
+ * Driver for oreoled ESCs found in solo, connected via I2C.
  *
  */
 
@@ -53,6 +53,7 @@
 #include <unistd.h>
 #include <stdio.h>
 #include <ctype.h>
+#include <sys/stat.h>
 
 #include <nuttx/arch.h>
 #include <nuttx/wqueue.h>
@@ -69,7 +70,8 @@
 
 #define OREOLED_NUM_LEDS		4			///< maximum number of LEDs the oreo led driver can support
 #define OREOLED_BASE_I2C_ADDR	0x68		///< base i2c address (7-bit)
-#define OREOLED_TIMEOUT_MS		10000000U	///< timeout looking for battery 10seconds after startup
+#define OPEOLED_I2C_RETRYCOUNT  2           ///< i2c retry count
+#define OREOLED_TIMEOUT_USEC	2000000U	///< timeout looking for oreoleds 2 seconds after startup
 #define OREOLED_GENERALCALL_US	4000000U	///< general call sent every 4 seconds
 #define OREOLED_GENERALCALL_CMD	0x00		///< general call command sent at regular intervals
 
@@ -81,7 +83,7 @@
 class OREOLED : public device::I2C
 {
 public:
-	OREOLED(int bus, int i2c_addr);
+	OREOLED(int bus, int i2c_addr, bool autoupdate, bool alwaysupdate);
 	virtual ~OREOLED();
 
 	virtual int		init();
@@ -95,6 +97,9 @@ public:
 	/* send cmd to an LEDs (used for testing only) */
 	int				send_cmd(oreoled_cmd_t sb);
 
+	/* returns true once the driver finished bootloading and ready for commands */
+	bool			is_ready();
+
 private:
 
 	/**
@@ -117,13 +122,40 @@ private:
 	 */
 	void			cycle();
 
+	int				bootloader_app_reset(int led_num);
+	int				bootloader_app_ping(int led_num);
+	uint16_t		bootloader_inapp_checksum(int led_num);
+	int				bootloader_ping(int led_num);
+	uint8_t			bootloader_version(int led_num);
+	uint16_t		bootloader_app_version(int led_num);
+	uint16_t		bootloader_app_checksum(int led_num);
+	int				bootloader_set_colour(int led_num, uint8_t red, uint8_t green);
+	int				bootloader_flash(int led_num);
+	int				bootloader_boot(int led_num);
+	uint16_t		bootloader_fw_checksum(void);
+	int				bootloader_coerce_healthy(void);
+
 	/* internal variables */
 	work_s			_work;							///< work queue for scheduling reads
 	bool			_healthy[OREOLED_NUM_LEDS];		///< health of each LED
+	bool			_in_boot[OREOLED_NUM_LEDS];		///< true for each LED that is in bootloader mode
 	uint8_t			_num_healthy;					///< number of healthy LEDs
+	uint8_t			_num_inboot;					///< number of LEDs in bootloader
 	ringbuffer::RingBuffer	*_cmd_queue;					///< buffer of commands to send to LEDs
 	uint64_t		_last_gencall;
 	uint64_t		_start_time;					///< system time we first attempt to communicate with battery
+	bool			_autoupdate;					///< true if the driver should update all LEDs
+	bool			_alwaysupdate;					///< true if the driver should update all LEDs
+	bool			_is_bootloading;				///< true if a bootloading operation is in progress
+	bool			_is_ready;						///< set to true once the driver has completly initialised
+	uint16_t		_fw_checksum;					///< the current 16bit XOR checksum of the built in oreoled firmware binary
+
+	/* performance checking */
+	perf_counter_t      _call_perf;
+	perf_counter_t      _gcall_perf;
+	perf_counter_t      _probe_perf;
+	perf_counter_t      _comms_errors;
+	perf_counter_t      _reply_errors;
 };
 
 /* for now, we only support one OREOLED */
@@ -137,16 +169,30 @@ void oreoled_usage();
 extern "C" __EXPORT int oreoled_main(int argc, char *argv[]);
 
 /* constructor */
-OREOLED::OREOLED(int bus, int i2c_addr) :
+OREOLED::OREOLED(int bus, int i2c_addr, bool autoupdate, bool alwaysupdate) :
 	I2C("oreoled", OREOLED0_DEVICE_PATH, bus, i2c_addr, 100000),
 	_work{},
 	_num_healthy(0),
+	_num_inboot(0),
 	_cmd_queue(nullptr),
-	_last_gencall(0)
+	_last_gencall(0),
+	_autoupdate(autoupdate),
+	_alwaysupdate(alwaysupdate),
+	_is_bootloading(false),
+	_is_ready(false),
+	_fw_checksum(0x0000),
+	_call_perf(perf_alloc(PC_ELAPSED, "oreoled_call")),
+	_gcall_perf(perf_alloc(PC_ELAPSED, "oreoled_gcall")),
+	_probe_perf(perf_alloc(PC_ELAPSED, "oreoled_probe")),
+	_comms_errors(perf_alloc(PC_COUNT, "oreoled_comms_errors")),
+	_reply_errors(perf_alloc(PC_COUNT, "oreoled_reply_errors"))
 {
 	/* initialise to unhealthy */
 	memset(_healthy, 0, sizeof(_healthy));
 
+	/* initialise to in application */
+	memset(_in_boot, 0, sizeof(_in_boot));
+
 	/* capture startup time */
 	_start_time = hrt_absolute_time();
 }
@@ -161,6 +207,13 @@ OREOLED::~OREOLED()
 	if (_cmd_queue != nullptr) {
 		delete _cmd_queue;
 	}
+
+	/* free perf counters */
+	perf_free(_call_perf);
+	perf_free(_gcall_perf);
+	perf_free(_probe_perf);
+	perf_free(_comms_errors);
+	perf_free(_reply_errors);
 }
 
 int
@@ -192,6 +245,9 @@ OREOLED::init()
 int
 OREOLED::probe()
 {
+	/* set retry count */
+	_retries = OPEOLED_I2C_RETRYCOUNT;
+
 	/* always return true */
 	return OK;
 }
@@ -202,13 +258,20 @@ OREOLED::info()
 	/* print health info on each LED */
 	for (uint8_t i = 0; i < OREOLED_NUM_LEDS; i++) {
 		if (!_healthy[i]) {
-			DEVICE_LOG("oreo %u: BAD", (int)i);
+			DEVICE_LOG("oreo %u: BAD", (unsigned)i);
 
 		} else {
-			DEVICE_LOG("oreo %u: OK", (int)i);
+			DEVICE_LOG("oreo %u: OK", (unsigned)i);
 		}
 	}
 
+	/* display perf info */
+	perf_print_counter(_call_perf);
+	perf_print_counter(_gcall_perf);
+	perf_print_counter(_probe_perf);
+	perf_print_counter(_comms_errors);
+	perf_print_counter(_reply_errors);
+
 	return OK;
 }
 
@@ -241,31 +304,67 @@ OREOLED::cycle()
 {
 	/* check time since startup */
 	uint64_t now = hrt_absolute_time();
-	bool startup_timeout = (now - _start_time > OREOLED_TIMEOUT_MS);
+	bool startup_timeout = (now - _start_time > OREOLED_TIMEOUT_USEC);
 
-	/* if not leds found during start-up period, exit without rescheduling */
-	if (startup_timeout && _num_healthy == 0) {
-		warnx("did not find oreoled");
-		return;
-	}
+	/* prepare the response buffer */
+	uint8_t reply[OREOLED_CMD_READ_LENGTH_MAX];
 
 	/* during startup period keep searching for unhealthy LEDs */
 	if (!startup_timeout && _num_healthy < OREOLED_NUM_LEDS) {
-		/* prepare command to turn off LED*/
-		uint8_t msg[] = {OREOLED_PATTERN_OFF};
+		/* prepare command to turn off LED */
+		/* add two bytes of pre-amble to for higher signal to noise ratio */
+		uint8_t msg[] = {0xAA, 0x55, OREOLED_PATTERN_OFF, 0x00};
 
 		/* attempt to contact each unhealthy LED */
 		for (uint8_t i = 0; i < OREOLED_NUM_LEDS; i++) {
 			if (!_healthy[i]) {
+				perf_begin(_probe_perf);
+
 				/* set I2C address */
 				set_address(OREOLED_BASE_I2C_ADDR + i);
 
-				/* send I2C command and record health*/
-				if (transfer(msg, sizeof(msg), nullptr, 0) == OK) {
-					_healthy[i] = true;
-					_num_healthy++;
-					warnx("oreoled %d ok", (unsigned)i);
+				/* Calculate XOR CRC and append to the i2c write data */
+				msg[sizeof(msg) - 1] = OREOLED_BASE_I2C_ADDR + i;
+
+				for (uint8_t j = 0; j < sizeof(msg) - 1; j++) {
+					msg[sizeof(msg) - 1] ^= msg[j];
 				}
+
+				/* send I2C command */
+				if (transfer(msg, sizeof(msg), reply, 3) == OK) {
+					if (reply[1] == OREOLED_BASE_I2C_ADDR + i &&
+					    reply[2] == msg[sizeof(msg) - 1]) {
+						DEVICE_LOG("oreoled %u ok - in bootloader", (unsigned)i);
+						_healthy[i] = true;
+						_num_healthy++;
+
+						/* If slaves are in application record that so we can reset if we need to bootload */
+						/* This additional check is required for LED firmwares below v1.3 and can be
+						   deprecated once all LEDs in the wild have firmware >= v1.3 */
+						if (bootloader_ping(i) == OK) {
+							_in_boot[i] = true;
+							_num_inboot++;
+						}
+
+						/* Check for a reply with a checksum offset of 1,
+						   which indicates a response from firmwares >= 1.3 */
+
+					} else if (reply[1] == OREOLED_BASE_I2C_ADDR + i &&
+						   reply[2] == msg[sizeof(msg) - 1] + 1) {
+						DEVICE_LOG("oreoled %u ok - in application", (unsigned)i);
+						_healthy[i] = true;
+						_num_healthy++;
+
+					} else {
+						DEVICE_LOG("oreo reply errors: %u", (unsigned)_reply_errors);
+						perf_count(_reply_errors);
+					}
+
+				} else {
+					perf_count(_comms_errors);
+				}
+
+				perf_end(_probe_perf);
 			}
 		}
 
@@ -273,6 +372,121 @@ OREOLED::cycle()
 		work_queue(HPWORK, &_work, (worker_t)&OREOLED::cycle_trampoline, this,
 			   USEC2TICK(OREOLED_STARTUP_INTERVAL_US));
 		return;
+
+	} else if (_alwaysupdate) {
+		/* reset each healthy LED */
+		for (uint8_t i = 0; i < OREOLED_NUM_LEDS; i++) {
+			if (_healthy[i] && !_in_boot[i]) {
+				/* reset the LED if it's not in the bootloader */
+				/* (this happens during a pixhawk OTA update, since the LEDs stay powered) */
+				bootloader_app_reset(i);
+			}
+		}
+
+		/* attempt to update each healthy LED */
+		for (uint8_t i = 0; i < OREOLED_NUM_LEDS; i++) {
+			if (_healthy[i] && _in_boot[i]) {
+				/* flash the new firmware */
+				bootloader_flash(i);
+			}
+		}
+
+		/* boot each healthy LED */
+		for (uint8_t i = 0; i < OREOLED_NUM_LEDS; i++) {
+			if (_healthy[i] && _in_boot[i]) {
+				/* boot the application */
+				bootloader_boot(i);
+			}
+		}
+
+		/* coerce LEDs with startup issues to be healthy again */
+		bootloader_coerce_healthy();
+
+		/* mandatory updating has finished */
+		_alwaysupdate = false;
+
+		/* schedule a fresh cycle call when the measurement is done */
+		work_queue(HPWORK, &_work, (worker_t)&OREOLED::cycle_trampoline, this,
+			   USEC2TICK(OREOLED_UPDATE_INTERVAL_US));
+		return;
+
+	} else if (_autoupdate) {
+		/* check booted oreoleds to see if the app can report it's checksum (release versions >= v1.2) */
+		for (uint8_t i = 0; i < OREOLED_NUM_LEDS; i++) {
+			if (_healthy[i] && !_in_boot[i]) {
+				/* put any out of date oreoleds into bootloader mode */
+				/* being in bootloader mode signals to be code below that the will likey need updating */
+				if (bootloader_inapp_checksum(i) != bootloader_fw_checksum()) {
+					bootloader_app_reset(i);
+				}
+			}
+		}
+
+		/* reset all healthy oreoleds if the number of outdated oreoled's is > 0 */
+		/* this is done for consistency, so if only one oreoled is updating, all LEDs show the same behaviour */
+		/* otherwise a single oreoled could appear broken or failed. */
+		if (_num_inboot > 0) {
+			for (uint8_t i = 0; i < OREOLED_NUM_LEDS; i++) {
+				if (_healthy[i] && !_in_boot[i]) {
+					/* reset the LED if it's not in the bootloader */
+					/* (this happens during a pixhawk OTA update, since the LEDs stay powered) */
+					bootloader_app_reset(i);
+				}
+			}
+
+			/* update each outdated and healthy LED in bootloader mode */
+			for (uint8_t i = 0; i < OREOLED_NUM_LEDS; i++) {
+				if (_healthy[i] && _in_boot[i]) {
+					/* only flash LEDs with an old version of the applictioon */
+					if (bootloader_app_checksum(i) != bootloader_fw_checksum()) {
+						/* flash the new firmware */
+						bootloader_flash(i);
+					}
+				}
+			}
+
+			/* boot each healthy LED */
+			for (uint8_t i = 0; i < OREOLED_NUM_LEDS; i++) {
+				if (_healthy[i] && _in_boot[i]) {
+					/* boot the application */
+					bootloader_boot(i);
+				}
+			}
+
+			/* coerce LEDs with startup issues to be healthy again */
+			bootloader_coerce_healthy();
+		}
+
+		/* auto updating has finished */
+		_autoupdate = false;
+
+		/* schedule a fresh cycle call when the measurement is done */
+		work_queue(HPWORK, &_work, (worker_t)&OREOLED::cycle_trampoline, this,
+			   USEC2TICK(OREOLED_UPDATE_INTERVAL_US));
+		return;
+
+	} else if (_num_inboot > 0) {
+		/* boot any LEDs which are in still in bootloader mode */
+		for (uint8_t i = 0; i < OREOLED_NUM_LEDS; i++) {
+			if (_in_boot[i]) {
+				bootloader_boot(i);
+			}
+		}
+
+		/* coerce LEDs with startup issues to be healthy again */
+		bootloader_coerce_healthy();
+
+		/* ensure we don't get stuck in a loop */
+		_num_inboot = 0;
+
+		/* schedule a fresh cycle call when the measurement is done */
+		work_queue(HPWORK, &_work, (worker_t)&OREOLED::cycle_trampoline, this,
+			   USEC2TICK(OREOLED_UPDATE_INTERVAL_US));
+		return;
+
+	} else if (!_is_ready) {
+		/* indicate a ready state since startup has finished */
+		_is_ready = true;
 	}
 
 	/* get next command from queue */
@@ -282,23 +496,856 @@ OREOLED::cycle()
 		/* send valid messages to healthy LEDs */
 		if ((next_cmd.led_num < OREOLED_NUM_LEDS) && _healthy[next_cmd.led_num]
 		    && (next_cmd.num_bytes <= OREOLED_CMD_LENGTH_MAX)) {
+			/* start performance timer */
+			perf_begin(_call_perf);
+
 			/* set I2C address */
 			set_address(OREOLED_BASE_I2C_ADDR + next_cmd.led_num);
-			/* send I2C command */
-			transfer(next_cmd.buff, next_cmd.num_bytes, nullptr, 0);
+
+			/* Calculate XOR CRC and append to the i2c write data */
+			uint8_t next_cmd_xor = OREOLED_BASE_I2C_ADDR + next_cmd.led_num;
+
+			for (uint8_t i = 0; i < next_cmd.num_bytes; i++) {
+				next_cmd_xor ^= next_cmd.buff[i];
+			}
+
+			next_cmd.buff[next_cmd.num_bytes++] = next_cmd_xor;
+
+			/* send I2C command with a retry limit */
+			for (uint8_t retry = OEROLED_COMMAND_RETRIES; retry > 0; retry--) {
+				if (transfer(next_cmd.buff, next_cmd.num_bytes, reply, 3) == OK) {
+					if (reply[1] == (OREOLED_BASE_I2C_ADDR + next_cmd.led_num) && reply[2] == next_cmd_xor) {
+						/* slave returned a valid response */
+						break;
+
+					} else {
+						perf_count(_reply_errors);
+					}
+
+				} else {
+					perf_count(_comms_errors);
+				}
+			}
+
+			perf_end(_call_perf);
 		}
 	}
 
-	/* send general call every 4 seconds*/
-	if ((now - _last_gencall) > OREOLED_GENERALCALL_US) {
+	/* send general call every 4 seconds, if we aren't bootloading*/
+	if (!_is_bootloading && ((now - _last_gencall) > OREOLED_GENERALCALL_US)) {
+		perf_begin(_gcall_perf);
 		send_general_call();
+		perf_end(_gcall_perf);
 	}
 
-	/* schedule a fresh cycle call when the measurement is done */
+	/* schedule a fresh cycle call when the command is sent */
 	work_queue(HPWORK, &_work, (worker_t)&OREOLED::cycle_trampoline, this,
 		   USEC2TICK(OREOLED_UPDATE_INTERVAL_US));
 }
 
+int
+OREOLED::bootloader_app_reset(int led_num)
+{
+	_is_bootloading = true;
+	oreoled_cmd_t boot_cmd;
+	boot_cmd.led_num = led_num;
+
+	int ret = -1;
+
+	/* Set the current address */
+	set_address(OREOLED_BASE_I2C_ADDR + boot_cmd.led_num);
+
+	/* send a reset */
+	boot_cmd.buff[0] = OREOLED_PATTERN_PARAMUPDATE;
+	boot_cmd.buff[1] = OREOLED_PARAM_RESET;
+	boot_cmd.buff[2] = OEROLED_RESET_NONCE;
+	boot_cmd.buff[3] = OREOLED_BASE_I2C_ADDR + boot_cmd.led_num;
+	boot_cmd.num_bytes = 4;
+
+	for (uint8_t j = 0; j < boot_cmd.num_bytes - 1; j++) {
+		boot_cmd.buff[boot_cmd.num_bytes - 1] ^= boot_cmd.buff[j];
+	}
+
+	uint8_t reply[OREOLED_CMD_READ_LENGTH_MAX];
+
+	/* send I2C command with a retry limit */
+	for (uint8_t retry = OEROLED_COMMAND_RETRIES; retry > 0; retry--) {
+		if (transfer(boot_cmd.buff, boot_cmd.num_bytes, reply, 3) == OK) {
+			if (reply[1] == (OREOLED_BASE_I2C_ADDR + boot_cmd.led_num) &&
+			    reply[2] == boot_cmd.buff[boot_cmd.num_bytes - 1]) {
+				/* slave returned a valid response */
+				ret = OK;
+				/* set this LED as being in boot mode now */
+				_in_boot[led_num] = true;
+				_num_inboot++;
+				break;
+			}
+		}
+	}
+
+	/* Allow time for the LED to reboot */
+	usleep(OREOLED_BOOT_FLASH_WAITMS * 1000 * 10);
+	usleep(OREOLED_BOOT_FLASH_WAITMS * 1000 * 10);
+	usleep(OREOLED_BOOT_FLASH_WAITMS * 1000 * 10);
+	usleep(OREOLED_BOOT_FLASH_WAITMS * 1000 * 10);
+
+	_is_bootloading = false;
+	return ret;
+}
+
+int
+OREOLED::bootloader_app_ping(int led_num)
+{
+	oreoled_cmd_t boot_cmd;
+	boot_cmd.led_num = led_num;
+
+	int ret = -1;
+
+	/* Set the current address */
+	set_address(OREOLED_BASE_I2C_ADDR + boot_cmd.led_num);
+
+	/* send a pattern off command */
+	boot_cmd.buff[0] = 0xAA;
+	boot_cmd.buff[1] = 0x55;
+	boot_cmd.buff[2] = OREOLED_PATTERN_OFF;
+	boot_cmd.buff[3] = OREOLED_BASE_I2C_ADDR + boot_cmd.led_num;
+	boot_cmd.num_bytes = 4;
+
+	for (uint8_t j = 0; j < boot_cmd.num_bytes - 1; j++) {
+		boot_cmd.buff[boot_cmd.num_bytes - 1] ^= boot_cmd.buff[j];
+	}
+
+	uint8_t reply[OREOLED_CMD_READ_LENGTH_MAX];
+
+	/* send I2C command with a retry limit */
+	for (uint8_t retry = OEROLED_COMMAND_RETRIES; retry > 0; retry--) {
+		if (transfer(boot_cmd.buff, boot_cmd.num_bytes, reply, 3) == OK) {
+			if (reply[1] == (OREOLED_BASE_I2C_ADDR + boot_cmd.led_num) &&
+			    reply[2] == boot_cmd.buff[boot_cmd.num_bytes - 1]) {
+				/* slave returned a valid response */
+				ret = OK;
+				break;
+			}
+		}
+	}
+
+	return ret;
+}
+
+uint16_t
+OREOLED::bootloader_inapp_checksum(int led_num)
+{
+	_is_bootloading = true;
+	oreoled_cmd_t boot_cmd;
+	boot_cmd.led_num = led_num;
+
+	uint16_t ret = 0x0000;
+
+	/* Set the current address */
+	set_address(OREOLED_BASE_I2C_ADDR + boot_cmd.led_num);
+
+	boot_cmd.buff[0] = OREOLED_PATTERN_PARAMUPDATE;
+	boot_cmd.buff[1] = OREOLED_PARAM_APP_CHECKSUM;
+	boot_cmd.buff[2] = OREOLED_BASE_I2C_ADDR + boot_cmd.led_num;
+	boot_cmd.num_bytes = 3;
+
+	for (uint8_t j = 0; j < boot_cmd.num_bytes - 1; j++) {
+		boot_cmd.buff[boot_cmd.num_bytes - 1] ^= boot_cmd.buff[j];
+	}
+
+	uint8_t reply[OREOLED_CMD_READ_LENGTH_MAX];
+
+	for (uint8_t retry = OEROLED_COMMAND_RETRIES; retry > 0; retry--) {
+		/* Send the I2C Write+Read */
+		memset(reply, 0, sizeof(reply));
+		transfer(boot_cmd.buff, boot_cmd.num_bytes, reply, 6);
+
+		/* Check the response */
+		if (reply[1] == OREOLED_BASE_I2C_ADDR + boot_cmd.led_num &&
+		    reply[2] == OREOLED_PARAM_APP_CHECKSUM &&
+		    reply[5] == boot_cmd.buff[boot_cmd.num_bytes - 1]) {
+			warnx("bl app checksum OK from LED %i", boot_cmd.led_num);
+			warnx("bl app checksum msb: 0x%x", reply[3]);
+			warnx("bl app checksum lsb: 0x%x", reply[4]);
+			ret = ((reply[3] << 8) | reply[4]);
+			break;
+
+		} else {
+			warnx("bl app checksum FAIL from LED %i", boot_cmd.led_num);
+			warnx("bl app checksum response  ADDR: 0x%x", reply[1]);
+			warnx("bl app checksum response   CMD: 0x%x", reply[2]);
+			warnx("bl app checksum response VER H: 0x%x", reply[3]);
+			warnx("bl app checksum response VER L: 0x%x", reply[4]);
+			warnx("bl app checksum response   XOR: 0x%x", reply[5]);
+
+			if (retry > 1) {
+				warnx("bl app checksum retrying LED %i", boot_cmd.led_num);
+
+			} else {
+				warnx("bl app checksum failed on LED %i", boot_cmd.led_num);
+				break;
+			}
+		}
+	}
+
+	_is_bootloading = false;
+	return ret;
+}
+
+int
+OREOLED::bootloader_ping(int led_num)
+{
+	_is_bootloading = true;
+	oreoled_cmd_t boot_cmd;
+	boot_cmd.led_num = led_num;
+
+	int ret = -1;
+
+	/* Set the current address */
+	set_address(OREOLED_BASE_I2C_ADDR + boot_cmd.led_num);
+
+	boot_cmd.buff[0] = OREOLED_BOOT_CMD_PING;
+	boot_cmd.buff[1] = OREOLED_BASE_I2C_ADDR + boot_cmd.led_num;
+	boot_cmd.num_bytes = 2;
+
+	for (uint8_t j = 0; j < boot_cmd.num_bytes - 1; j++) {
+		boot_cmd.buff[boot_cmd.num_bytes - 1] ^= boot_cmd.buff[j];
+	}
+
+	uint8_t reply[OREOLED_CMD_READ_LENGTH_MAX];
+
+	for (uint8_t retry = OEROLED_BOOT_COMMAND_RETRIES; retry > 0; retry--) {
+		/* Send the I2C Write+Read */
+		memset(reply, 0, sizeof(reply));
+		transfer(boot_cmd.buff, boot_cmd.num_bytes, reply, 5);
+
+		/* Check the response */
+		if (reply[1] == OREOLED_BASE_I2C_ADDR + boot_cmd.led_num &&
+		    reply[2] == OREOLED_BOOT_CMD_PING &&
+		    reply[3] == OREOLED_BOOT_CMD_PING_NONCE &&
+		    reply[4] == boot_cmd.buff[boot_cmd.num_bytes - 1]) {
+			warnx("bl ping OK from LED %i", boot_cmd.led_num);
+			ret = OK;
+			break;
+
+		} else {
+			warnx("bl ping FAIL from LED %i", boot_cmd.led_num);
+			warnx("bl ping response  ADDR: 0x%x", reply[1]);
+			warnx("bl ping response   CMD: 0x%x", reply[2]);
+			warnx("bl ping response NONCE: 0x%x", reply[3]);
+			warnx("bl ping response   XOR: 0x%x", reply[4]);
+
+			if (retry > 1) {
+				warnx("bl ping retrying LED %i", boot_cmd.led_num);
+
+			} else {
+				warnx("bl ping failed on LED %i", boot_cmd.led_num);
+				break;
+			}
+		}
+	}
+
+	_is_bootloading = false;
+	return ret;
+}
+
+uint8_t
+OREOLED::bootloader_version(int led_num)
+{
+	_is_bootloading = true;
+	oreoled_cmd_t boot_cmd;
+	boot_cmd.led_num = led_num;
+
+	uint8_t ret = 0x00;
+
+	/* Set the current address */
+	set_address(OREOLED_BASE_I2C_ADDR + boot_cmd.led_num);
+
+	boot_cmd.buff[0] = OREOLED_BOOT_CMD_BL_VER;
+	boot_cmd.buff[1] = OREOLED_BASE_I2C_ADDR + boot_cmd.led_num;
+	boot_cmd.num_bytes = 2;
+
+	for (uint8_t k = 0; k < boot_cmd.num_bytes - 1; k++) {
+		boot_cmd.buff[boot_cmd.num_bytes - 1] ^= boot_cmd.buff[k];
+	}
+
+	uint8_t reply[OREOLED_CMD_READ_LENGTH_MAX];
+
+	for (uint8_t retry = OEROLED_BOOT_COMMAND_RETRIES; retry > 0; retry--) {
+		/* Send the I2C Write+Read */
+		memset(reply, 0, sizeof(reply));
+		transfer(boot_cmd.buff, boot_cmd.num_bytes, reply, 5);
+
+		/* Check the response */
+		if (reply[1] == OREOLED_BASE_I2C_ADDR + boot_cmd.led_num &&
+		    reply[2] == OREOLED_BOOT_CMD_BL_VER &&
+		    reply[3] == OREOLED_BOOT_SUPPORTED_VER &&
+		    reply[4] == boot_cmd.buff[boot_cmd.num_bytes - 1]) {
+			warnx("bl ver from LED %i = %i", boot_cmd.led_num, reply[3]);
+			ret = reply[3];
+			break;
+
+		} else {
+			warnx("bl ver response  ADDR: 0x%x", reply[1]);
+			warnx("bl ver response   CMD: 0x%x", reply[2]);
+			warnx("bl ver response   VER: 0x%x", reply[3]);
+			warnx("bl ver response   XOR: 0x%x", reply[4]);
+
+			if (retry > 1) {
+				warnx("bl ver retrying LED %i", boot_cmd.led_num);
+
+			} else {
+				warnx("bl ver failed on LED %i", boot_cmd.led_num);
+				break;
+			}
+		}
+	}
+
+	_is_bootloading = false;
+	return ret;
+}
+
+uint16_t
+OREOLED::bootloader_app_version(int led_num)
+{
+	_is_bootloading = true;
+	oreoled_cmd_t boot_cmd;
+	boot_cmd.led_num = led_num;
+
+	uint16_t ret = 0x0000;
+
+	/* Set the current address */
+	set_address(OREOLED_BASE_I2C_ADDR + boot_cmd.led_num);
+
+	boot_cmd.buff[0] = OREOLED_BOOT_CMD_APP_VER;
+	boot_cmd.buff[1] = OREOLED_BASE_I2C_ADDR + boot_cmd.led_num;
+	boot_cmd.num_bytes = 2;
+
+	for (uint8_t j = 0; j < boot_cmd.num_bytes - 1; j++) {
+		boot_cmd.buff[boot_cmd.num_bytes - 1] ^= boot_cmd.buff[j];
+	}
+
+	uint8_t reply[OREOLED_CMD_READ_LENGTH_MAX];
+
+	for (uint8_t retry = OEROLED_BOOT_COMMAND_RETRIES; retry > 0; retry--) {
+		/* Send the I2C Write+Read */
+		memset(reply, 0, sizeof(reply));
+		transfer(boot_cmd.buff, boot_cmd.num_bytes, reply, 6);
+
+		/* Check the response */
+		if (reply[1] == OREOLED_BASE_I2C_ADDR + boot_cmd.led_num &&
+		    reply[2] == OREOLED_BOOT_CMD_APP_VER &&
+		    reply[5] == boot_cmd.buff[boot_cmd.num_bytes - 1]) {
+			warnx("bl app version OK from LED %i", boot_cmd.led_num);
+			warnx("bl app version msb: 0x%x", reply[3]);
+			warnx("bl app version lsb: 0x%x", reply[4]);
+			ret = ((reply[3] << 8) | reply[4]);
+			break;
+
+		} else {
+			warnx("bl app version FAIL from LED %i", boot_cmd.led_num);
+			warnx("bl app version response  ADDR: 0x%x", reply[1]);
+			warnx("bl app version response   CMD: 0x%x", reply[2]);
+			warnx("bl app version response VER H: 0x%x", reply[3]);
+			warnx("bl app version response VER L: 0x%x", reply[4]);
+			warnx("bl app version response   XOR: 0x%x", reply[5]);
+
+			if (retry > 1) {
+				warnx("bl app version retrying LED %i", boot_cmd.led_num);
+
+			} else {
+				warnx("bl app version failed on LED %i", boot_cmd.led_num);
+				break;
+			}
+		}
+	}
+
+	_is_bootloading = false;
+	return ret;
+}
+
+uint16_t
+OREOLED::bootloader_app_checksum(int led_num)
+{
+	_is_bootloading = true;
+	oreoled_cmd_t boot_cmd;
+	boot_cmd.led_num = led_num;
+
+	uint16_t ret = 0x0000;
+
+	/* Set the current address */
+	set_address(OREOLED_BASE_I2C_ADDR + boot_cmd.led_num);
+
+	boot_cmd.buff[0] = OREOLED_BOOT_CMD_APP_CRC;
+	boot_cmd.buff[1] = OREOLED_BASE_I2C_ADDR + boot_cmd.led_num;
+	boot_cmd.num_bytes = 2;
+
+	for (uint8_t j = 0; j < boot_cmd.num_bytes - 1; j++) {
+		boot_cmd.buff[boot_cmd.num_bytes - 1] ^= boot_cmd.buff[j];
+	}
+
+	uint8_t reply[OREOLED_CMD_READ_LENGTH_MAX];
+
+	for (uint8_t retry = OEROLED_BOOT_COMMAND_RETRIES; retry > 0; retry--) {
+		/* Send the I2C Write+Read */
+		memset(reply, 0, sizeof(reply));
+		transfer(boot_cmd.buff, boot_cmd.num_bytes, reply, 6);
+
+		/* Check the response */
+		if (reply[1] == OREOLED_BASE_I2C_ADDR + boot_cmd.led_num &&
+		    reply[2] == OREOLED_BOOT_CMD_APP_CRC &&
+		    reply[5] == boot_cmd.buff[boot_cmd.num_bytes - 1]) {
+			warnx("bl app checksum OK from LED %i", boot_cmd.led_num);
+			warnx("bl app checksum msb: 0x%x", reply[3]);
+			warnx("bl app checksum lsb: 0x%x", reply[4]);
+			ret = ((reply[3] << 8) | reply[4]);
+			break;
+
+		} else {
+			warnx("bl app checksum FAIL from LED %i", boot_cmd.led_num);
+			warnx("bl app checksum response  ADDR: 0x%x", reply[1]);
+			warnx("bl app checksum response   CMD: 0x%x", reply[2]);
+			warnx("bl app checksum response VER H: 0x%x", reply[3]);
+			warnx("bl app checksum response VER L: 0x%x", reply[4]);
+			warnx("bl app checksum response   XOR: 0x%x", reply[5]);
+
+			if (retry > 1) {
+				warnx("bl app checksum retrying LED %i", boot_cmd.led_num);
+
+			} else {
+				warnx("bl app checksum failed on LED %i", boot_cmd.led_num);
+				break;
+			}
+		}
+	}
+
+	_is_bootloading = false;
+	return ret;
+}
+
+int
+OREOLED::bootloader_set_colour(int led_num, uint8_t red, uint8_t green)
+{
+	_is_bootloading = true;
+	oreoled_cmd_t boot_cmd;
+	boot_cmd.led_num = led_num;
+
+	int ret = -1;
+
+	/* Set the current address */
+	set_address(OREOLED_BASE_I2C_ADDR + boot_cmd.led_num);
+
+	boot_cmd.buff[0] = OREOLED_BOOT_CMD_SET_COLOUR;
+	boot_cmd.buff[1] = red;
+	boot_cmd.buff[2] = green;
+	boot_cmd.buff[3] = OREOLED_BASE_I2C_ADDR + boot_cmd.led_num;
+	boot_cmd.num_bytes = 4;
+
+	for (uint8_t j = 0; j < boot_cmd.num_bytes - 1; j++) {
+		boot_cmd.buff[boot_cmd.num_bytes - 1] ^= boot_cmd.buff[j];
+	}
+
+	uint8_t reply[OREOLED_CMD_READ_LENGTH_MAX];
+
+	for (uint8_t retry = OEROLED_BOOT_COMMAND_RETRIES; retry > 0; retry--) {
+		/* Send the I2C Write+Read */
+		memset(reply, 0, sizeof(reply));
+		transfer(boot_cmd.buff, boot_cmd.num_bytes, reply, 4);
+
+		/* Check the response */
+		if (reply[1] == OREOLED_BASE_I2C_ADDR + boot_cmd.led_num &&
+		    reply[2] == OREOLED_BOOT_CMD_SET_COLOUR &&
+		    reply[3] == boot_cmd.buff[boot_cmd.num_bytes - 1]) {
+			warnx("bl set colour OK from LED %i", boot_cmd.led_num);
+			ret = OK;
+			break;
+
+		} else {
+			warnx("bl set colour FAIL from LED %i", boot_cmd.led_num);
+			warnx("bl set colour response  ADDR: 0x%x", reply[1]);
+			warnx("bl set colour response   CMD: 0x%x", reply[2]);
+			warnx("bl set colour response   XOR: 0x%x", reply[3]);
+
+			if (retry > 1) {
+				warnx("bl app colour retrying LED %i", boot_cmd.led_num);
+
+			} else {
+				warnx("bl app colour failed on LED %i", boot_cmd.led_num);
+				break;
+			}
+		}
+	}
+
+	_is_bootloading = false;
+	return ret;
+}
+
+int
+OREOLED::bootloader_flash(int led_num)
+{
+	_is_bootloading = true;
+	oreoled_cmd_t boot_cmd;
+	boot_cmd.led_num = led_num;
+
+	/* Open the bootloader file */
+	int fd = ::open(OREOLED_FW_FILE, O_RDONLY);
+
+	/* check for error opening the file */
+	if (fd < 0) {
+		return -1;
+	}
+
+	struct stat s;
+
+	/* attempt to stat the file */
+	if (stat(OREOLED_FW_FILE, &s) != 0) {
+		::close(fd);
+		return -1;
+	}
+
+	uint16_t fw_length = s.st_size - OREOLED_FW_FILE_HEADER_LENGTH;
+
+	/* sanity-check file size */
+	if (fw_length > OREOLED_FW_FILE_SIZE_LIMIT) {
+		::close(fd);
+		return -1;
+	}
+
+	uint8_t *buf = new uint8_t[s.st_size];
+
+	/* check that the buffer has been allocated */
+	if (buf == NULL) {
+		::close(fd);
+		return -1;
+	}
+
+	/* check that the firmware can be read into the buffer */
+	if (::read(fd, buf, s.st_size) != s.st_size) {
+		::close(fd);
+		delete[] buf;
+		return -1;
+	}
+
+	::close(fd);
+
+	/* Grab the version bytes from the binary */
+	uint8_t version_major = buf[0];
+	uint8_t version_minor = buf[1];
+
+	/* calculate flash pages (rounded up to nearest integer) */
+	uint8_t flash_pages = ((fw_length + 64 - 1) / 64);
+
+	/* Set the current address */
+	set_address(OREOLED_BASE_I2C_ADDR + boot_cmd.led_num);
+
+	uint8_t reply[OREOLED_CMD_READ_LENGTH_MAX];
+
+	/* Loop through flash pages */
+	for (uint8_t page_idx = 0; page_idx < flash_pages; page_idx++) {
+
+		/* Send the first half of the 64 byte flash page */
+		memset(boot_cmd.buff, 0, sizeof(boot_cmd.buff));
+		boot_cmd.buff[0] = OREOLED_BOOT_CMD_WRITE_FLASH_A;
+		boot_cmd.buff[1] = page_idx;
+		memcpy(boot_cmd.buff + 2, buf + (page_idx * 64) + OREOLED_FW_FILE_HEADER_LENGTH, 32);
+		boot_cmd.buff[32 + 2] = OREOLED_BASE_I2C_ADDR + boot_cmd.led_num;
+		boot_cmd.num_bytes = 32 + 3;
+
+		for (uint8_t k = 0; k < boot_cmd.num_bytes - 1; k++) {
+			boot_cmd.buff[boot_cmd.num_bytes - 1] ^= boot_cmd.buff[k];
+		}
+
+		for (uint8_t retry = OEROLED_BOOT_COMMAND_RETRIES; retry > 0; retry--) {
+			/* Send the I2C Write+Read */
+			memset(reply, 0, sizeof(reply));
+			transfer(boot_cmd.buff, boot_cmd.num_bytes, reply, 4);
+
+			/* Check the response */
+			if (reply[1] == OREOLED_BASE_I2C_ADDR + boot_cmd.led_num &&
+			    reply[2] == OREOLED_BOOT_CMD_WRITE_FLASH_A &&
+			    reply[3] == boot_cmd.buff[boot_cmd.num_bytes - 1]) {
+				warnx("bl flash %ia OK for LED %i", page_idx, boot_cmd.led_num);
+				break;
+
+			} else {
+				warnx("bl flash %ia FAIL for LED %i", page_idx, boot_cmd.led_num);
+				warnx("bl flash %ia response ADDR: 0x%x", page_idx, reply[1]);
+				warnx("bl flash %ia response  CMD: 0x%x", page_idx, reply[2]);
+				warnx("bl flash %ia response  XOR: 0x%x", page_idx, reply[3]);
+
+				if (retry > 1) {
+					warnx("bl flash %ia retrying LED %i", page_idx, boot_cmd.led_num);
+
+				} else {
+					warnx("bl flash %ia failed on LED %i", page_idx, boot_cmd.led_num);
+					delete[] buf;
+					return -1;
+				}
+			}
+		}
+
+		/* Send the second half of the 64 byte flash page */
+		memset(boot_cmd.buff, 0, sizeof(boot_cmd.buff));
+		boot_cmd.buff[0] = OREOLED_BOOT_CMD_WRITE_FLASH_B;
+		memcpy(boot_cmd.buff + 1, buf + (page_idx * 64) + 32 + OREOLED_FW_FILE_HEADER_LENGTH, 32);
+		boot_cmd.buff[32 + 1] = OREOLED_BASE_I2C_ADDR + boot_cmd.led_num;
+		boot_cmd.num_bytes = 32 + 2;
+
+		for (uint8_t k = 0; k < boot_cmd.num_bytes - 1; k++) {
+			boot_cmd.buff[boot_cmd.num_bytes - 1] ^= boot_cmd.buff[k];
+		}
+
+		for (uint8_t retry = OEROLED_BOOT_COMMAND_RETRIES; retry > 0; retry--) {
+			/* Send the I2C Write+Read */
+			memset(reply, 0, sizeof(reply));
+			transfer(boot_cmd.buff, boot_cmd.num_bytes, reply, 4);
+
+			/* Check the response */
+			if (reply[1] == OREOLED_BASE_I2C_ADDR + boot_cmd.led_num &&
+			    reply[2] == OREOLED_BOOT_CMD_WRITE_FLASH_B &&
+			    reply[3] == boot_cmd.buff[boot_cmd.num_bytes - 1]) {
+				warnx("bl flash %ib OK for LED %i", page_idx, boot_cmd.led_num);
+				break;
+
+			} else {
+				warnx("bl flash %ib FAIL for LED %i", page_idx, boot_cmd.led_num);
+				warnx("bl flash %ib response ADDR: 0x%x", page_idx, reply[1]);
+				warnx("bl flash %ib response  CMD: 0x%x", page_idx, reply[2]);
+				warnx("bl flash %ib response  XOR: 0x%x", page_idx, reply[3]);
+
+				if (retry > 1) {
+					warnx("bl flash %ib retrying LED %i", page_idx, boot_cmd.led_num);
+
+				} else {
+					errx(1, "bl flash %ib failed on LED %i", page_idx, boot_cmd.led_num);
+					delete[] buf;
+					return -1;
+				}
+			}
+		}
+
+		/* Sleep to allow flash to write */
+		/* Wait extra long on the first write, to allow time for EEPROM updates */
+		if (page_idx == 0) {
+			usleep(OREOLED_BOOT_FLASH_WAITMS * 1000 * 10);
+
+		} else {
+			usleep(OREOLED_BOOT_FLASH_WAITMS * 1000);
+		}
+	}
+
+	uint16_t app_checksum = bootloader_fw_checksum();
+
+	/* Flash writes must have succeeded so finalise the flash */
+	boot_cmd.buff[0] = OREOLED_BOOT_CMD_FINALISE_FLASH;
+	boot_cmd.buff[1] = version_major;
+	boot_cmd.buff[2] = version_minor;
+	boot_cmd.buff[3] = (uint8_t)(fw_length >> 8);
+	boot_cmd.buff[4] = (uint8_t)(fw_length & 0xFF);
+	boot_cmd.buff[5] = (uint8_t)(app_checksum >> 8);
+	boot_cmd.buff[6] = (uint8_t)(app_checksum & 0xFF);
+	boot_cmd.buff[7] = OREOLED_BASE_I2C_ADDR + boot_cmd.led_num;
+	boot_cmd.num_bytes = 8;
+
+	for (uint8_t k = 0; k < boot_cmd.num_bytes - 1; k++) {
+		boot_cmd.buff[boot_cmd.num_bytes - 1] ^= boot_cmd.buff[k];
+	}
+
+	/* Try to finalise for twice the amount of normal retries */
+	for (uint8_t retry = OEROLED_BOOT_COMMAND_RETRIES * 2; retry > 0; retry--) {
+		/* Send the I2C Write */
+		memset(reply, 0, sizeof(reply));
+		transfer(boot_cmd.buff, boot_cmd.num_bytes, reply, 4);
+
+		/* Check the response */
+		if (reply[1] == OREOLED_BASE_I2C_ADDR + boot_cmd.led_num &&
+		    reply[2] == OREOLED_BOOT_CMD_FINALISE_FLASH &&
+		    reply[3] == boot_cmd.buff[boot_cmd.num_bytes - 1]) {
+			warnx("bl finalise OK from LED %i", boot_cmd.led_num);
+			break;
+
+		} else {
+			warnx("bl finalise response  ADDR: 0x%x", reply[1]);
+			warnx("bl finalise response   CMD: 0x%x", reply[2]);
+			warnx("bl finalise response   XOR: 0x%x", reply[3]);
+
+			if (retry > 1) {
+				warnx("bl finalise retrying LED %i", boot_cmd.led_num);
+
+			} else {
+				warnx("bl finalise failed on LED %i", boot_cmd.led_num);
+				delete[] buf;
+				return -1;
+			}
+		}
+	}
+
+	/* allow time for flash to finalise */
+	usleep(OREOLED_BOOT_FLASH_WAITMS * 1000 * 10);
+	usleep(OREOLED_BOOT_FLASH_WAITMS * 1000 * 10);
+
+	/* clean up file buffer */
+	delete[] buf;
+
+	_is_bootloading = false;
+	return 1;
+}
+
+int
+OREOLED::bootloader_boot(int led_num)
+{
+	_is_bootloading = true;
+	oreoled_cmd_t boot_cmd;
+	boot_cmd.led_num = led_num;
+
+	int ret = -1;
+
+	/* Set the current address */
+	set_address(OREOLED_BASE_I2C_ADDR + boot_cmd.led_num);
+
+	boot_cmd.buff[0] = OREOLED_BOOT_CMD_BOOT_APP;
+	boot_cmd.buff[1] = OREOLED_BOOT_CMD_BOOT_NONCE;
+	boot_cmd.buff[2] = OREOLED_BASE_I2C_ADDR + boot_cmd.led_num;
+	boot_cmd.num_bytes = 3;
+
+	for (uint8_t k = 0; k < boot_cmd.num_bytes - 1; k++) {
+		boot_cmd.buff[boot_cmd.num_bytes - 1] ^= boot_cmd.buff[k];
+	}
+
+	for (uint8_t retry = OEROLED_BOOT_COMMAND_RETRIES; retry > 0; retry--) {
+		/* Send the I2C Write */
+		uint8_t reply[OREOLED_CMD_READ_LENGTH_MAX];
+		transfer(boot_cmd.buff, boot_cmd.num_bytes, reply, 4);
+
+		/* Check the response */
+		if (reply[1] == OREOLED_BASE_I2C_ADDR + boot_cmd.led_num &&
+		    reply[2] == OREOLED_BOOT_CMD_BOOT_APP &&
+		    reply[3] == boot_cmd.buff[boot_cmd.num_bytes - 1]) {
+			warnx("bl boot OK from LED %i", boot_cmd.led_num);
+			/* decrement the inboot counter so we don't get confused */
+			_in_boot[led_num] = false;
+			_num_inboot--;
+			ret = OK;
+			break;
+
+		} else if (reply[1] == OREOLED_BASE_I2C_ADDR + boot_cmd.led_num &&
+			   reply[2] == OREOLED_BOOT_CMD_BOOT_NONCE &&
+			   reply[3] == boot_cmd.buff[boot_cmd.num_bytes - 1]) {
+			warnx("bl boot error from LED %i: no app", boot_cmd.led_num);
+			break;
+
+		} else {
+			warnx("bl boot response  ADDR: 0x%x", reply[1]);
+			warnx("bl boot response   CMD: 0x%x", reply[2]);
+			warnx("bl boot response   XOR: 0x%x", reply[3]);
+
+			if (retry > 1) {
+				warnx("bl boot retrying LED %i", boot_cmd.led_num);
+
+			} else {
+				warnx("bl boot failed on LED %i", boot_cmd.led_num);
+				break;
+			}
+		}
+	}
+
+	/* allow time for the LEDs to boot */
+	usleep(OREOLED_BOOT_FLASH_WAITMS * 1000 * 10);
+	usleep(OREOLED_BOOT_FLASH_WAITMS * 1000 * 10);
+	usleep(OREOLED_BOOT_FLASH_WAITMS * 1000 * 10);
+	usleep(OREOLED_BOOT_FLASH_WAITMS * 1000 * 10);
+
+	_is_bootloading = false;
+	return ret;
+}
+
+uint16_t
+OREOLED::bootloader_fw_checksum(void)
+{
+	/* Calculate the 16 bit XOR checksum of the firmware on the first call of this function */
+	if (_fw_checksum == 0x0000) {
+		/* Open the bootloader file */
+		int fd = ::open(OREOLED_FW_FILE, O_RDONLY);
+
+		/* check for error opening the file */
+		if (fd < 0) {
+			return -1;
+		}
+
+		struct stat s;
+
+		/* attempt to stat the file */
+		if (stat(OREOLED_FW_FILE, &s) != 0) {
+			::close(fd);
+			return -1;
+		}
+
+		uint16_t fw_length = s.st_size - OREOLED_FW_FILE_HEADER_LENGTH;
+
+		/* sanity-check file size */
+		if (fw_length > OREOLED_FW_FILE_SIZE_LIMIT) {
+			::close(fd);
+			return -1;
+		}
+
+		uint8_t *buf = new uint8_t[s.st_size];
+
+		/* check that the buffer has been allocated */
+		if (buf == NULL) {
+			::close(fd);
+			return -1;
+		}
+
+		/* check that the firmware can be read into the buffer */
+		if (::read(fd, buf, s.st_size) != s.st_size) {
+			::close(fd);
+			delete[] buf;
+			return -1;
+		}
+
+		::close(fd);
+
+		/* Calculate a 16 bit XOR checksum of the flash */
+		/* Skip the first two bytes which are the version information, plus
+		   the next two bytes which are modified by the bootloader */
+		uint16_t app_checksum = 0x0000;
+
+		for (uint16_t j = 2 + OREOLED_FW_FILE_HEADER_LENGTH; j < s.st_size; j += 2) {
+			app_checksum ^= (buf[j] << 8) | buf[j + 1];
+		}
+
+		delete[] buf;
+
+		warnx("fw length = %i", fw_length);
+		warnx("fw checksum = %i", app_checksum);
+
+		/* Store the checksum so it's only calculated once */
+		_fw_checksum = app_checksum;
+	}
+
+	return _fw_checksum;
+}
+
+int
+OREOLED::bootloader_coerce_healthy(void)
+{
+	int ret = -1;
+
+	/* check each unhealthy LED */
+	/* this re-checks "unhealthy" LEDs as they can sometimes power up with the wrong ID, */
+	/*  but will have the correct ID once they jump to the application and be healthy again */
+	for (uint8_t i = 0; i < OREOLED_NUM_LEDS; i++) {
+		if (!_healthy[i] && bootloader_app_ping(i) == OK) {
+			/* mark as healthy */
+			_healthy[i] = true;
+			_num_healthy++;
+			ret = OK;
+		}
+	}
+
+	return ret;
+}
+
 int
 OREOLED::ioctl(struct file *filp, int cmd, unsigned long arg)
 {
@@ -368,6 +1415,100 @@ OREOLED::ioctl(struct file *filp, int cmd, unsigned long arg)
 
 		return ret;
 
+	case OREOLED_SEND_RESET:
+		/* send a reset */
+		new_cmd.led_num = OREOLED_ALL_INSTANCES;
+		new_cmd.buff[0] = OREOLED_PATTERN_PARAMUPDATE;
+		new_cmd.buff[1] = OREOLED_PARAM_RESET;
+		new_cmd.buff[2] = OEROLED_RESET_NONCE;
+		new_cmd.num_bytes = 3;
+
+		for (uint8_t i = 0; i < OREOLED_NUM_LEDS; i++) {
+			/* add command to queue for all healthy leds */
+			if (_healthy[i]) {
+				warnx("sending a reset... to %i", i);
+				new_cmd.led_num = i;
+				_cmd_queue->force(&new_cmd);
+				ret = OK;
+			}
+		}
+
+		return ret;
+
+	case OREOLED_BL_PING:
+		for (uint8_t i = 0; i < OREOLED_NUM_LEDS; i++) {
+			if (_healthy[i]) {
+				bootloader_ping(i);
+				ret = OK;
+			}
+		}
+
+		return ret;
+
+	case OREOLED_BL_VER:
+		for (uint8_t i = 0; i < OREOLED_NUM_LEDS; i++) {
+			if (_healthy[i]) {
+				bootloader_version(i);
+				ret = OK;
+			}
+		}
+
+		return ret;
+
+	case OREOLED_BL_FLASH:
+		for (uint8_t i = 0; i < OREOLED_NUM_LEDS; i++) {
+			if (_healthy[i]) {
+				bootloader_flash(i);
+				ret = OK;
+			}
+		}
+
+		return ret;
+
+	case OREOLED_BL_APP_VER:
+		for (uint8_t i = 0; i < OREOLED_NUM_LEDS; i++) {
+			if (_healthy[i]) {
+				bootloader_app_version(i);
+				ret = OK;
+			}
+		}
+
+		return ret;
+
+	case OREOLED_BL_APP_CRC:
+		for (uint8_t i = 0; i < OREOLED_NUM_LEDS; i++) {
+			if (_healthy[i]) {
+				bootloader_app_checksum(i);
+				ret = OK;
+			}
+		}
+
+		return ret;
+
+	case OREOLED_BL_SET_COLOUR:
+		new_cmd.led_num = OREOLED_ALL_INSTANCES;
+
+		for (uint8_t i = 0; i < OREOLED_NUM_LEDS; i++) {
+			if (_healthy[i]) {
+				bootloader_set_colour(i, ((oreoled_rgbset_t *) arg)->red, ((oreoled_rgbset_t *) arg)->green);
+				ret = OK;
+			}
+		}
+
+		return ret;
+
+	case OREOLED_BL_BOOT_APP:
+		new_cmd.led_num = OREOLED_ALL_INSTANCES;
+
+		for (uint8_t i = 0; i < OREOLED_NUM_LEDS; i++) {
+			if (_healthy[i]) {
+				bootloader_boot(i);
+				ret = OK;
+			}
+		}
+
+		return ret;
+
 	case OREOLED_SEND_BYTES:
 		/* send bytes */
 		new_cmd = *((oreoled_cmd_t *) arg);
@@ -393,6 +1534,10 @@ OREOLED::ioctl(struct file *filp, int cmd, unsigned long arg)
 
 		return ret;
 
+	case OREOLED_FORCE_SYNC:
+		send_general_call();
+		break;
+
 	default:
 		/* see if the parent class can make any use of it */
 		ret = CDev::ioctl(filp, cmd, arg);
@@ -444,10 +1589,18 @@ OREOLED::send_cmd(oreoled_cmd_t new_cmd)
 	return ret;
 }
 
+/* return the internal _is_ready flag indicating if initialisation is complete */
+bool
+OREOLED::is_ready()
+{
+	return _is_ready;
+}
+
 void
 oreoled_usage()
 {
-	warnx("missing command: try 'start', 'test', 'info', 'off', 'stop', 'rgb 30 40 50' 'macro 4' 'gencall' 'bytes <lednum> 7 9 6'");
+	warnx("missing command: try 'start', 'test', 'info', 'off', 'stop', 'reset', 'rgb 30 40 50', 'macro 4', 'gencall', 'bytes <lednum> 7 9 6'");
+	warnx("bootloader commands: try 'blping', 'blver', 'blappver', 'blappcrc', 'blcolour <red> <green>', 'blflash', 'blboot'");
 	warnx("options:");
 	warnx("    -b i2cbus (%d)", PX4_I2C_BUS_LED);
 	warnx("    -a addr (0x%x)", OREOLED_BASE_I2C_ADDR);
@@ -498,8 +1651,21 @@ oreoled_main(int argc, char *argv[])
 			i2cdevice = PX4_I2C_BUS_LED;
 		}
 
+		/* handle update flags */
+		bool autoupdate = false;
+		bool alwaysupdate = false;
+
+		if (argc > 2 && !strcmp(argv[2], "autoupdate")) {
+			warnx("autoupdate enabled");
+			autoupdate = true;
+
+		} else if (argc > 2 && !strcmp(argv[2], "alwaysupdate")) {
+			warnx("alwaysupdate enabled");
+			alwaysupdate = true;
+		}
+
 		/* instantiate driver */
-		g_oreoled = new OREOLED(i2cdevice, i2c_addr);
+		g_oreoled = new OREOLED(i2cdevice, i2c_addr, autoupdate, alwaysupdate);
 
 		/* check if object was created */
 		if (g_oreoled == nullptr) {
@@ -513,6 +1679,15 @@ oreoled_main(int argc, char *argv[])
 			errx(1, "failed to start driver");
 		}
 
+		/* wait for up to 20 seconds for the driver become ready */
+		for (uint8_t i = 0; i < 20; i++) {
+			if (g_oreoled != nullptr && g_oreoled->is_ready()) {
+				break;
+			}
+
+			sleep(1);
+		}
+
 		exit(0);
 	}
 
@@ -648,6 +1823,170 @@ oreoled_main(int argc, char *argv[])
 		exit(ret);
 	}
 
+	/* send reset request to all LEDS */
+	if (!strcmp(verb, "reset")) {
+		if (argc < 2) {
+			errx(1, "Usage: oreoled reset");
+		}
+
+		int fd = open(OREOLED0_DEVICE_PATH, 0);
+
+		if (fd == -1) {
+			errx(1, "Unable to open " OREOLED0_DEVICE_PATH);
+		}
+
+		if ((ret = ioctl(fd, OREOLED_SEND_RESET, 0)) != OK) {
+			errx(1, "failed to run macro");
+		}
+
+		close(fd);
+		exit(ret);
+	}
+
+	/* attempt to flash all LEDS in bootloader mode*/
+	if (!strcmp(verb, "blflash")) {
+		if (argc < 2) {
+			errx(1, "Usage: oreoled blflash");
+		}
+
+		int fd = open(OREOLED0_DEVICE_PATH, 0);
+
+		if (fd == -1) {
+			errx(1, "Unable to open " OREOLED0_DEVICE_PATH);
+		}
+
+		if ((ret = ioctl(fd, OREOLED_BL_FLASH, 0)) != OK) {
+			errx(1, "failed to run flash");
+		}
+
+		close(fd);
+		exit(ret);
+	}
+
+	/* send bootloader boot request to all LEDS */
+	if (!strcmp(verb, "blboot")) {
+		if (argc < 2) {
+			errx(1, "Usage: oreoled blboot");
+		}
+
+		int fd = open(OREOLED0_DEVICE_PATH, 0);
+
+		if (fd == -1) {
+			errx(1, "Unable to open " OREOLED0_DEVICE_PATH);
+		}
+
+		if ((ret = ioctl(fd, OREOLED_BL_BOOT_APP, 0)) != OK) {
+			errx(1, "failed to run boot");
+		}
+
+		close(fd);
+		exit(ret);
+	}
+
+	/* send bootloader ping all LEDs */
+	if (!strcmp(verb, "blping")) {
+		if (argc < 2) {
+			errx(1, "Usage: oreoled blping");
+		}
+
+		int fd = open(OREOLED0_DEVICE_PATH, 0);
+
+		if (fd == -1) {
+			errx(1, "Unable to open " OREOLED0_DEVICE_PATH);
+		}
+
+		if ((ret = ioctl(fd, OREOLED_BL_PING, 0)) != OK) {
+			errx(1, "failed to run blping");
+		}
+
+		close(fd);
+		exit(ret);
+	}
+
+	/* ask all LEDs for their bootloader version */
+	if (!strcmp(verb, "blver")) {
+		if (argc < 2) {
+			errx(1, "Usage: oreoled blver");
+		}
+
+		int fd = open(OREOLED0_DEVICE_PATH, 0);
+
+		if (fd == -1) {
+			errx(1, "Unable to open " OREOLED0_DEVICE_PATH);
+		}
+
+		if ((ret = ioctl(fd, OREOLED_BL_VER, 0)) != OK) {
+			errx(1, "failed to get bootloader version");
+		}
+
+		close(fd);
+		exit(ret);
+	}
+
+	/* ask all LEDs for their application version */
+	if (!strcmp(verb, "blappver")) {
+		if (argc < 2) {
+			errx(1, "Usage: oreoled blappver");
+		}
+
+		int fd = open(OREOLED0_DEVICE_PATH, 0);
+
+		if (fd == -1) {
+			errx(1, "Unable to open " OREOLED0_DEVICE_PATH);
+		}
+
+		if ((ret = ioctl(fd, OREOLED_BL_APP_VER, 0)) != OK) {
+			errx(1, "failed to get boot app version");
+		}
+
+		close(fd);
+		exit(ret);
+	}
+
+	/* ask all LEDs for their application crc */
+	if (!strcmp(verb, "blappcrc")) {
+		if (argc < 2) {
+			errx(1, "Usage: oreoled blappcrc");
+		}
+
+		int fd = open(OREOLED0_DEVICE_PATH, 0);
+
+		if (fd == -1) {
+			errx(1, "Unable to open " OREOLED0_DEVICE_PATH);
+		}
+
+		if ((ret = ioctl(fd, OREOLED_BL_APP_CRC, 0)) != OK) {
+			errx(1, "failed to get boot app crc");
+		}
+
+		close(fd);
+		exit(ret);
+	}
+
+	/* set the default bootloader LED colour on all LEDs */
+	if (!strcmp(verb, "blcolour")) {
+		if (argc < 4) {
+			errx(1, "Usage: oreoled blcolour <red> <green>");
+		}
+
+		int fd = open(OREOLED0_DEVICE_PATH, 0);
+
+		if (fd == -1) {
+			errx(1, "Unable to open " OREOLED0_DEVICE_PATH);
+		}
+
+		uint8_t red = (uint8_t)strtol(argv[2], NULL, 0);
+		uint8_t green = (uint8_t)strtol(argv[3], NULL, 0);
+		oreoled_rgbset_t rgb_set = {OREOLED_ALL_INSTANCES, OREOLED_PATTERN_SOLID, red, green, 0};
+
+		if ((ret = ioctl(fd, OREOLED_BL_SET_COLOUR, (unsigned long)&rgb_set)) != OK) {
+			errx(1, "failed to set boot startup colours");
+		}
+
+		close(fd);
+		exit(ret);
+	}
+
 	/* send general hardware call to all LEDS */
 	if (!strcmp(verb, "gencall")) {
 		ret = g_oreoled->send_general_call();
@@ -670,18 +2009,18 @@ oreoled_main(int argc, char *argv[])
 		}
 
 		/* check led num */
-		sendb.led_num = (uint8_t)strtol(argv[2], NULL, 0);
+		sendb.led_num = (uint8_t)strtol(argv[optind + 1], NULL, 0);
 
 		if (sendb.led_num > 3) {
 			errx(1, "led number must be between 0 ~ 3");
 		}
 
 		/* get bytes */
-		sendb.num_bytes = argc - 3;
+		sendb.num_bytes = argc - (optind + 2);
 		uint8_t byte_count;
 
 		for (byte_count = 0; byte_count < sendb.num_bytes; byte_count++) {
-			sendb.buff[byte_count] = (uint8_t)strtol(argv[byte_count + 3], NULL, 0);
+			sendb.buff[byte_count] = (uint8_t)strtol(argv[byte_count + optind + 2], NULL, 0);
 		}
 
 		/* send bytes */

From 76c2c9a46d3f67b1b799c584df272f32567aebaa Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <andrew@tridgell.net>
Date: Tue, 12 Apr 2016 18:59:45 +1000
Subject: [PATCH 0302/1230] drivers: added filter control ioctls

---
 src/drivers/drv_accel.h | 6 ++++++
 src/drivers/drv_gyro.h  | 6 ++++++
 2 files changed, 12 insertions(+)

diff --git a/src/drivers/drv_accel.h b/src/drivers/drv_accel.h
index 5d4ac5ea31..a985191a4e 100644
--- a/src/drivers/drv_accel.h
+++ b/src/drivers/drv_accel.h
@@ -103,4 +103,10 @@ struct accel_calibration_s {
 /** get the result of a sensor self-test */
 #define ACCELIOCSELFTEST	_ACCELIOC(9)
 
+/** set the hardware low-pass filter cut-off no lower than (arg) Hz */
+#define ACCELIOCSHWLOWPASS	_ACCELIOC(10)
+
+/** get the hardware low-pass filter cut-off in Hz*/
+#define ACCELIOCGHWLOWPASS	_ACCELIOC(11)
+
 #endif /* _DRV_ACCEL_H */
diff --git a/src/drivers/drv_gyro.h b/src/drivers/drv_gyro.h
index 124c6d2b34..431c2fac67 100644
--- a/src/drivers/drv_gyro.h
+++ b/src/drivers/drv_gyro.h
@@ -100,4 +100,10 @@ struct gyro_calibration_s {
 /** check the status of the sensor */
 #define GYROIOCSELFTEST		_GYROIOC(8)
 
+/** set the hardware low-pass filter cut-off no lower than (arg) Hz */
+#define GYROIOCSHWLOWPASS	_GYROIOC(9)
+
+/** get the hardware low-pass filter cut-off in Hz*/
+#define GYROIOCGHWLOWPASS	_GYROIOC(10)
+
 #endif /* _DRV_GYRO_H */

From 9c1a02d673f74677136ab67d09b42ea3ce2dfed1 Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <andrew@tridgell.net>
Date: Tue, 12 Apr 2016 19:00:33 +1000
Subject: [PATCH 0303/1230] gpio: added SET_OUTPUT ioctls

---
 src/drivers/drv_gpio.h | 6 ++++++
 1 file changed, 6 insertions(+)

diff --git a/src/drivers/drv_gpio.h b/src/drivers/drv_gpio.h
index 5d320fe7e6..856f604fec 100644
--- a/src/drivers/drv_gpio.h
+++ b/src/drivers/drv_gpio.h
@@ -194,4 +194,10 @@
 
 #define GPIO_PERIPHERAL_RAIL_RESET	GPIOC(14)
 
+/** configure the board GPIOs in (arg) as outputs, initially low */
+#define GPIO_SET_OUTPUT_LOW	GPIOC(15)
+
+/** configure the board GPIOs in (arg) as outputs, initially high */
+#define GPIO_SET_OUTPUT_HIGH	GPIOC(16)
+
 #endif /* _DRV_GPIO_H */

From f38b1bf160c93c4a306f34be307e425c2ccc8fc3 Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <andrew@tridgell.net>
Date: Mon, 17 Aug 2015 12:01:13 +1000
Subject: [PATCH 0304/1230] px4fmu: allow for GPIO_SET_OUTPUT with initial
 value

this is needed to prevent inadvertent camera trigger when setting up a
port
---
 src/drivers/px4fmu/fmu.cpp | 14 +++++++++++++-
 1 file changed, 13 insertions(+), 1 deletion(-)

diff --git a/src/drivers/px4fmu/fmu.cpp b/src/drivers/px4fmu/fmu.cpp
index d74765f6a1..bff4d5e9c4 100644
--- a/src/drivers/px4fmu/fmu.cpp
+++ b/src/drivers/px4fmu/fmu.cpp
@@ -2502,7 +2502,9 @@ PX4FMU::gpio_set_function(uint32_t gpios, int function)
 		gpios |= 3;
 
 		/* flip the buffer to output mode if required */
-		if (GPIO_SET_OUTPUT == function) {
+		if (GPIO_SET_OUTPUT == function ||
+		    GPIO_SET_OUTPUT_LOW == function ||
+		    GPIO_SET_OUTPUT_HIGH == function) {
 			stm32_gpiowrite(GPIO_GPIO_DIR, 1);
 		}
 	}
@@ -2521,6 +2523,14 @@ PX4FMU::gpio_set_function(uint32_t gpios, int function)
 				stm32_configgpio(_gpio_tab[i].output);
 				break;
 
+			case GPIO_SET_OUTPUT_LOW:
+				stm32_configgpio((_gpio_tab[i].output & ~(GPIO_OUTPUT_SET)) | GPIO_OUTPUT_CLEAR);
+				break;
+
+			case GPIO_SET_OUTPUT_HIGH:
+				stm32_configgpio((_gpio_tab[i].output & ~(GPIO_OUTPUT_CLEAR)) | GPIO_OUTPUT_SET);
+				break;
+
 			case GPIO_SET_ALT_1:
 				if (_gpio_tab[i].alt != 0) {
 					stm32_configgpio(_gpio_tab[i].alt);
@@ -2716,6 +2726,8 @@ PX4FMU::gpio_ioctl(struct file *filp, int cmd, unsigned long arg)
 		break;
 
 	case GPIO_SET_OUTPUT:
+	case GPIO_SET_OUTPUT_LOW:
+	case GPIO_SET_OUTPUT_HIGH:
 	case GPIO_SET_INPUT:
 	case GPIO_SET_ALT_1:
 		gpio_set_function(arg, cmd);

From 3f6f030fc464df146db9d98189ee1079116147bd Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <andrew@tridgell.net>
Date: Sun, 21 Jun 2015 18:02:31 +1000
Subject: [PATCH 0305/1230] px4iofirmware: allow override when RAW_PWM is
 active

if override is enabled then it should apply even if RAW_PWM is being
supplied by the FMU
---
 src/modules/px4iofirmware/mixer.cpp | 15 +++++++--------
 1 file changed, 7 insertions(+), 8 deletions(-)

diff --git a/src/modules/px4iofirmware/mixer.cpp b/src/modules/px4iofirmware/mixer.cpp
index 224b971e1b..0ff0eb4892 100644
--- a/src/modules/px4iofirmware/mixer.cpp
+++ b/src/modules/px4iofirmware/mixer.cpp
@@ -121,6 +121,11 @@ mixer_tick(void)
 	 * Decide which set of controls we're using.
 	 */
 
+	bool override_enabled = ((r_status_flags & PX4IO_P_STATUS_FLAGS_OVERRIDE) &&
+				 (r_status_flags & PX4IO_P_STATUS_FLAGS_RC_OK) &&
+				 (r_status_flags & PX4IO_P_STATUS_FLAGS_MIXER_OK) &&
+				 !(r_setup_arming & PX4IO_P_SETUP_ARMING_RC_HANDLING_DISABLED));
+
 	/* do not mix if RAW_PWM mode is on and FMU is good */
 	if ((r_status_flags & PX4IO_P_STATUS_FLAGS_RAW_PWM) &&
 	    (r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK)) {
@@ -138,10 +143,7 @@ mixer_tick(void)
 			source = MIX_FMU;
 		}
 
-		if ((r_status_flags & PX4IO_P_STATUS_FLAGS_OVERRIDE) &&
-		    (r_status_flags & PX4IO_P_STATUS_FLAGS_RC_OK) &&
-		    (r_status_flags & PX4IO_P_STATUS_FLAGS_MIXER_OK) &&
-		    !(r_setup_arming & PX4IO_P_SETUP_ARMING_RC_HANDLING_DISABLED) &&
+		if (override_enabled &&
 		    !(r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK) &&
 		    /* do not enter manual override if we asked for termination failsafe and FMU is lost */
 		    !(r_setup_arming & PX4IO_P_SETUP_ARMING_TERMINATION_FAILSAFE)) {
@@ -149,10 +151,7 @@ mixer_tick(void)
 			/* if allowed, mix from RC inputs directly */
 			source = MIX_OVERRIDE;
 
-		} else 	if ((r_status_flags & PX4IO_P_STATUS_FLAGS_OVERRIDE) &&
-			    (r_status_flags & PX4IO_P_STATUS_FLAGS_RC_OK) &&
-			    (r_status_flags & PX4IO_P_STATUS_FLAGS_MIXER_OK) &&
-			    !(r_setup_arming & PX4IO_P_SETUP_ARMING_RC_HANDLING_DISABLED) &&
+		} else 	if (override_enabled &&
 			    (r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK)) {
 
 			/* if allowed, mix from RC inputs directly up to available rc channels */

From 5cf8081a980dc95495ea91a2d0b0f57fa114b0c0 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Beat=20K=C3=BCng?= <beat-kueng@gmx.net>
Date: Tue, 17 May 2016 09:26:44 +0200
Subject: [PATCH 0306/1230] uorb template: add timestamp to the format string

We explicitly include the timestamp. This makes it possible to change it's
type later on.
This breaks the current ULog logging format.
---
 msg/templates/uorb/msg.cpp.template | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/msg/templates/uorb/msg.cpp.template b/msg/templates/uorb/msg.cpp.template
index 9244846c7f..abbf20f51a 100644
--- a/msg/templates/uorb/msg.cpp.template
+++ b/msg/templates/uorb/msg.cpp.template
@@ -61,7 +61,7 @@ topic_name = spec.short_name
 
 sorted_fields = sorted(spec.parsed_fields(), key=sizeof_field_type, reverse=True)
 struct_size, padding_end_size = add_padding_bytes(sorted_fields, search_path)
-topic_fields = ["%s %s" % (convert_type(field.type), field.name) for field in sorted_fields]
+topic_fields = ["uint64_t timestamp"]+["%s %s" % (convert_type(field.type), field.name) for field in sorted_fields]
 }@
 
 #include <px4_config.h>

From 1719d9a957d847d51aa68ac2116ee542d6261585 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Beat=20K=C3=BCng?= <beat-kueng@gmx.net>
Date: Tue, 17 May 2016 11:10:37 +0200
Subject: [PATCH 0307/1230] logger: add a file magic

---
 src/modules/logger/logger.cpp | 25 +++++++++++++++++++++++++
 src/modules/logger/logger.h   |  5 +++++
 2 files changed, 30 insertions(+)

diff --git a/src/modules/logger/logger.cpp b/src/modules/logger/logger.cpp
index 90c311a58f..9fe9ae799f 100644
--- a/src/modules/logger/logger.cpp
+++ b/src/modules/logger/logger.cpp
@@ -291,6 +291,13 @@ enum class MessageType : uint8_t {
 
 /* declare message data structs with byte alignment (no padding) */
 #pragma pack(push, 1)
+
+/** first bytes of the file */
+struct message_file_header_s {
+	uint8_t magic[8];
+	uint64_t timestamp;
+};
+
 struct message_format_s {
 	uint8_t msg_type = static_cast<uint8_t>(MessageType::FORMAT);
 	uint16_t msg_size;
@@ -849,6 +856,7 @@ void Logger::start_log()
 	mavlink_log_info(&_mavlink_log_pub, "[logger] file: %s", file_name);
 
 	_writer.start_log(file_name);
+	write_header();
 	write_version();
 	write_formats();
 	write_parameters();
@@ -926,6 +934,23 @@ void Logger::write_info(const char *name, const char *value)
 	_writer.unlock();
 }
 
+void Logger::write_header()
+{
+	message_file_header_s header;
+	header.magic[0] = 'U';
+	header.magic[1] = 'L';
+	header.magic[2] = 'o';
+	header.magic[3] = 'g';
+	header.magic[4] = 0x01;
+	header.magic[5] = 0x12;
+	header.magic[6] = 0x35;
+	header.magic[7] = 0x00; //file version 0
+	header.timestamp = hrt_absolute_time();
+	_writer.lock();
+	write_wait(&header, sizeof(header));
+	_writer.unlock();
+}
+
 /* write version info messages */
 void Logger::write_version()
 {
diff --git a/src/modules/logger/logger.h b/src/modules/logger/logger.h
index 36b4bae958..c2b81b097b 100644
--- a/src/modules/logger/logger.h
+++ b/src/modules/logger/logger.h
@@ -118,6 +118,11 @@ private:
 
 	void stop_log();
 
+	/**
+	 * write the file header with file magic and timestamp.
+	 */
+	void write_header();
+
 	void write_formats();
 
 	void write_version();

From 908f7eb47f47941d95edb8a5d6f9bdc0e713347c Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Beat=20K=C3=BCng?= <beat-kueng@gmx.net>
Date: Tue, 17 May 2016 11:10:59 +0200
Subject: [PATCH 0308/1230] logger: remove some comments, logging output

---
 src/modules/logger/logger.cpp | 4 +---
 1 file changed, 1 insertion(+), 3 deletions(-)

diff --git a/src/modules/logger/logger.cpp b/src/modules/logger/logger.cpp
index 9fe9ae799f..10d972f51f 100644
--- a/src/modules/logger/logger.cpp
+++ b/src/modules/logger/logger.cpp
@@ -432,13 +432,12 @@ bool Logger::copy_if_updated_multi(orb_id_t topic, int multi_instance, int *hand
 	// only try to subscribe to topic if this is the first time
 	// after that just check after a certain interval to avoid high cpu usage
 	if (*handle < 0 && (*time_last_checked == 0 || hrt_elapsed_time(time_last_checked) > TRY_SUBSCRIBE_INTERVAL)) {
-		//if (multi_instance == 1) warnx("checking instance 1 of topic %s", topic->o_name);
 		*time_last_checked = hrt_absolute_time();
 
 		if (OK == orb_exists(topic, multi_instance)) {
 			*handle = orb_subscribe_multi(topic, multi_instance);
 
-			//warnx("subscribed to instance %d of topic %s", multi_instance, topic->o_name);
+			//PX4_INFO("subscribed to instance %d of topic %s", multi_instance, topic->o_name);
 
 			/* copy first data */
 			if (*handle >= 0) {
@@ -564,7 +563,6 @@ void Logger::run()
 			/* Check if parameters have changed */
 			// this needs to change to a timestamped record to record a history of parameter changes
 			if (parameter_update_sub.check_updated()) {
-				warnx("parameter update");
 				parameter_update_sub.update();
 				write_changed_parameters();
 			}

From 0ddd7a408c3ea34a30661b2ac2feb356feb0d723 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Beat=20K=C3=BCng?= <beat-kueng@gmx.net>
Date: Tue, 17 May 2016 12:28:23 +0200
Subject: [PATCH 0309/1230] logger: write "sys_name" and "time_ref_utc" info to
 log file

---
 src/modules/logger/logger.cpp | 28 ++++++++++++++++++++++++++++
 src/modules/logger/logger.h   |  1 +
 2 files changed, 29 insertions(+)

diff --git a/src/modules/logger/logger.cpp b/src/modules/logger/logger.cpp
index 10d972f51f..6bd30e122f 100644
--- a/src/modules/logger/logger.cpp
+++ b/src/modules/logger/logger.cpp
@@ -931,6 +931,27 @@ void Logger::write_info(const char *name, const char *value)
 
 	_writer.unlock();
 }
+void Logger::write_info(const char *name, uint32_t value)
+{
+	_writer.lock();
+	uint8_t buffer[sizeof(message_info_header_s)];
+	message_info_header_s *msg = reinterpret_cast<message_info_header_s *>(buffer);
+	msg->msg_type = static_cast<uint8_t>(MessageType::INFO);
+
+	/* construct format key (type and name) */
+	msg->key_len = snprintf(msg->key, sizeof(msg->key), "uint32_t %s", name);
+	size_t msg_size = sizeof(*msg) - sizeof(msg->key) + msg->key_len;
+
+	/* copy string value directly to buffer */
+	memcpy(&buffer[msg_size], &value, sizeof(uint32_t));
+	msg_size += sizeof(uint32_t);
+
+	msg->msg_size = msg_size - 3;
+
+	write_wait(buffer, msg_size);
+
+	_writer.unlock();
+}
 
 void Logger::write_header()
 {
@@ -954,6 +975,13 @@ void Logger::write_version()
 {
 	write_info("ver_sw", PX4_GIT_VERSION_STR);
 	write_info("ver_hw", HW_ARCH);
+	write_info("sys_name", "PX4");
+	int32_t utc_offset = 0;
+
+	if (_log_utc_offset != PARAM_INVALID) {
+		param_get(_log_utc_offset, &utc_offset);
+		write_info("time_ref_utc", utc_offset * 60);
+	}
 }
 
 void Logger::write_parameters()
diff --git a/src/modules/logger/logger.h b/src/modules/logger/logger.h
index c2b81b097b..af03d4b8c6 100644
--- a/src/modules/logger/logger.h
+++ b/src/modules/logger/logger.h
@@ -128,6 +128,7 @@ private:
 	void write_version();
 
 	void write_info(const char *name, const char *value);
+	void write_info(const char *name, uint32_t value);
 
 	void write_parameters();
 

From 971e97745fb0fdb0f05747154ad2518addaad3e9 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Beat=20K=C3=BCng?= <beat-kueng@gmx.net>
Date: Tue, 17 May 2016 12:40:11 +0200
Subject: [PATCH 0310/1230] logger: reorder message header fields (for
 alignment)

---
 src/modules/logger/logger.cpp | 8 ++++----
 1 file changed, 4 insertions(+), 4 deletions(-)

diff --git a/src/modules/logger/logger.cpp b/src/modules/logger/logger.cpp
index 6bd30e122f..c4936b2093 100644
--- a/src/modules/logger/logger.cpp
+++ b/src/modules/logger/logger.cpp
@@ -299,8 +299,8 @@ struct message_file_header_s {
 };
 
 struct message_format_s {
+	uint16_t msg_size; //size of message - header size (=3)
 	uint8_t msg_type = static_cast<uint8_t>(MessageType::FORMAT);
-	uint16_t msg_size;
 
 	uint8_t msg_id;
 	uint16_t format_len;
@@ -308,24 +308,24 @@ struct message_format_s {
 };
 
 struct message_data_header_s {
+	uint16_t msg_size; //size of message - header size (=3)
 	uint8_t msg_type = static_cast<uint8_t>(MessageType::DATA);
-	uint16_t msg_size;
 
 	uint8_t msg_id;
 	uint8_t multi_id;
 };
 
 struct message_info_header_s {
+	uint16_t msg_size; //size of message - header size (=3)
 	uint8_t msg_type = static_cast<uint8_t>(MessageType::INFO);
-	uint16_t msg_size;
 
 	uint8_t key_len;
 	char key[255];
 };
 
 struct message_parameter_header_s {
-	uint8_t msg_type = static_cast<uint8_t>(MessageType::PARAMETER);
 	uint16_t msg_size;
+	uint8_t msg_type = static_cast<uint8_t>(MessageType::PARAMETER);
 
 	uint8_t key_len;
 	char key[255];

From df803fc4b987e851473fd8d2d494e4fb2ab02923 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Beat=20K=C3=BCng?= <beat-kueng@gmx.net>
Date: Tue, 17 May 2016 12:58:34 +0200
Subject: [PATCH 0311/1230] logger: remove redundant format_len from
 message_format_s

---
 src/modules/logger/logger.cpp | 5 ++---
 1 file changed, 2 insertions(+), 3 deletions(-)

diff --git a/src/modules/logger/logger.cpp b/src/modules/logger/logger.cpp
index c4936b2093..9ce04ae089 100644
--- a/src/modules/logger/logger.cpp
+++ b/src/modules/logger/logger.cpp
@@ -303,7 +303,6 @@ struct message_format_s {
 	uint8_t msg_type = static_cast<uint8_t>(MessageType::FORMAT);
 
 	uint8_t msg_id;
-	uint16_t format_len;
 	char format[2096];
 };
 
@@ -894,8 +893,8 @@ void Logger::write_formats()
 
 	for (LoggerSubscription &sub : _subscriptions) {
 		msg.msg_id = msg_id;
-		msg.format_len = snprintf(msg.format, sizeof(msg.format), "%s", sub.metadata->o_fields);
-		size_t msg_size = sizeof(msg) - sizeof(msg.format) + msg.format_len;
+		int format_len = snprintf(msg.format, sizeof(msg.format), "%s", sub.metadata->o_fields);
+		size_t msg_size = sizeof(msg) - sizeof(msg.format) + format_len;
 		msg.msg_size = msg_size - 3;
 
 		write_wait(&msg, msg_size);

From 2d037d1a1cd125f19d5bd645e6049cf3cae097ba Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Beat=20K=C3=BCng?= <beat-kueng@gmx.net>
Date: Tue, 17 May 2016 13:00:29 +0200
Subject: [PATCH 0312/1230] logger: add an MSG_HEADER_LEN to clarify meaning

---
 src/modules/logger/logger.cpp | 20 +++++++++++---------
 1 file changed, 11 insertions(+), 9 deletions(-)

diff --git a/src/modules/logger/logger.cpp b/src/modules/logger/logger.cpp
index 9ce04ae089..e333e1c642 100644
--- a/src/modules/logger/logger.cpp
+++ b/src/modules/logger/logger.cpp
@@ -292,6 +292,8 @@ enum class MessageType : uint8_t {
 /* declare message data structs with byte alignment (no padding) */
 #pragma pack(push, 1)
 
+#define MSG_HEADER_LEN 3 //accounts for msg_size and msg_type
+
 /** first bytes of the file */
 struct message_file_header_s {
 	uint8_t magic[8];
@@ -299,7 +301,7 @@ struct message_file_header_s {
 };
 
 struct message_format_s {
-	uint16_t msg_size; //size of message - header size (=3)
+	uint16_t msg_size; //size of message - MSG_HEADER_LEN
 	uint8_t msg_type = static_cast<uint8_t>(MessageType::FORMAT);
 
 	uint8_t msg_id;
@@ -307,7 +309,7 @@ struct message_format_s {
 };
 
 struct message_data_header_s {
-	uint16_t msg_size; //size of message - header size (=3)
+	uint16_t msg_size; //size of message - MSG_HEADER_LEN
 	uint8_t msg_type = static_cast<uint8_t>(MessageType::DATA);
 
 	uint8_t msg_id;
@@ -315,7 +317,7 @@ struct message_data_header_s {
 };
 
 struct message_info_header_s {
-	uint16_t msg_size; //size of message - header size (=3)
+	uint16_t msg_size; //size of message - MSG_HEADER_LEN
 	uint8_t msg_type = static_cast<uint8_t>(MessageType::INFO);
 
 	uint8_t key_len;
@@ -588,7 +590,7 @@ void Logger::run()
 
 						message_data_header_s *header = reinterpret_cast<message_data_header_s *>(buffer);
 						header->msg_type = static_cast<uint8_t>(MessageType::DATA);
-						header->msg_size = static_cast<uint16_t>(msg_size - 3);
+						header->msg_size = static_cast<uint16_t>(msg_size - MSG_HEADER_LEN);
 						header->msg_id = msg_id;
 						header->multi_id = 0x80 + instance;	// Non multi, active
 
@@ -895,7 +897,7 @@ void Logger::write_formats()
 		msg.msg_id = msg_id;
 		int format_len = snprintf(msg.format, sizeof(msg.format), "%s", sub.metadata->o_fields);
 		size_t msg_size = sizeof(msg) - sizeof(msg.format) + format_len;
-		msg.msg_size = msg_size - 3;
+		msg.msg_size = msg_size - MSG_HEADER_LEN;
 
 		write_wait(&msg, msg_size);
 
@@ -923,7 +925,7 @@ void Logger::write_info(const char *name, const char *value)
 		memcpy(&buffer[msg_size], value, vlen);
 		msg_size += vlen;
 
-		msg->msg_size = msg_size - 3;
+		msg->msg_size = msg_size - MSG_HEADER_LEN;
 
 		write_wait(buffer, msg_size);
 	}
@@ -945,7 +947,7 @@ void Logger::write_info(const char *name, uint32_t value)
 	memcpy(&buffer[msg_size], &value, sizeof(uint32_t));
 	msg_size += sizeof(uint32_t);
 
-	msg->msg_size = msg_size - 3;
+	msg->msg_size = msg_size - MSG_HEADER_LEN;
 
 	write_wait(buffer, msg_size);
 
@@ -1030,7 +1032,7 @@ void Logger::write_parameters()
 			param_get(param, &buffer[msg_size]);
 			msg_size += value_size;
 
-			msg->msg_size = msg_size - 3;
+			msg->msg_size = msg_size - MSG_HEADER_LEN;
 
 			write_wait(buffer, msg_size);
 		}
@@ -1090,7 +1092,7 @@ void Logger::write_changed_parameters()
 			msg_size += value_size;
 
 			/* msg_size is now 1 (msg_type) + 2 (msg_size) + 1 (key_len) + key_len + value_size */
-			msg->msg_size = msg_size - 3;
+			msg->msg_size = msg_size - MSG_HEADER_LEN;
 
 			write_wait(buffer, msg_size);
 		}

From c13247e14f51a51d1e67cbe14aec74ff6be4eb01 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Beat=20K=C3=BCng?= <beat-kueng@gmx.net>
Date: Tue, 17 May 2016 14:18:59 +0200
Subject: [PATCH 0313/1230] logger: log all known formats, add ADD_LOGGED_MSG
 message

this is needed for nested topics.
---
 src/modules/logger/logger.cpp | 110 ++++++++++++++++++++++++----------
 src/modules/logger/logger.h   |  29 ++++++++-
 2 files changed, 106 insertions(+), 33 deletions(-)

diff --git a/src/modules/logger/logger.cpp b/src/modules/logger/logger.cpp
index e333e1c642..034eece065 100644
--- a/src/modules/logger/logger.cpp
+++ b/src/modules/logger/logger.cpp
@@ -287,6 +287,10 @@ enum class MessageType : uint8_t {
 	DATA = 'D',
 	INFO = 'I',
 	PARAMETER = 'P',
+	ADD_LOGGED_MSG = 'A',
+	REMOVE_LOGGED_MSG = 'R',
+	SYNC = 'S',
+	DROPOUT = 'O',
 };
 
 /* declare message data structs with byte alignment (no padding) */
@@ -304,16 +308,29 @@ struct message_format_s {
 	uint16_t msg_size; //size of message - MSG_HEADER_LEN
 	uint8_t msg_type = static_cast<uint8_t>(MessageType::FORMAT);
 
-	uint8_t msg_id;
 	char format[2096];
 };
 
+struct message_add_logged_s {
+	uint16_t msg_size; //size of message - MSG_HEADER_LEN
+	uint8_t msg_type = static_cast<uint8_t>(MessageType::ADD_LOGGED_MSG);
+
+	uint8_t multi_id;
+	uint16_t msg_id;
+	char message_name[255];
+};
+
+struct message_remove_logged_s {
+	uint16_t msg_size; //size of message - MSG_HEADER_LEN
+	uint8_t msg_type = static_cast<uint8_t>(MessageType::REMOVE_LOGGED_MSG);
+
+	uint16_t msg_id;
+};
 struct message_data_header_s {
 	uint16_t msg_size; //size of message - MSG_HEADER_LEN
 	uint8_t msg_type = static_cast<uint8_t>(MessageType::DATA);
 
-	uint8_t msg_id;
-	uint8_t multi_id;
+	uint16_t msg_id;
 };
 
 struct message_info_header_s {
@@ -425,33 +442,36 @@ int Logger::add_topic(const char *name, unsigned interval = 0)
 	return fd;
 }
 
-bool Logger::copy_if_updated_multi(orb_id_t topic, int multi_instance, int *handle, void *buffer,
-				   uint64_t *time_last_checked)
+bool Logger::copy_if_updated_multi(LoggerSubscription &sub, int multi_instance, void *buffer)
 {
 	bool updated = false;
+	int &handle = sub.fd[multi_instance];
 
 	// only try to subscribe to topic if this is the first time
 	// after that just check after a certain interval to avoid high cpu usage
-	if (*handle < 0 && (*time_last_checked == 0 || hrt_elapsed_time(time_last_checked) > TRY_SUBSCRIBE_INTERVAL)) {
-		*time_last_checked = hrt_absolute_time();
+	if (handle < 0 && (sub.time_tried_subscribe == 0 ||
+			   hrt_elapsed_time(&sub.time_tried_subscribe) > TRY_SUBSCRIBE_INTERVAL)) {
+		sub.time_tried_subscribe = hrt_absolute_time();
 
-		if (OK == orb_exists(topic, multi_instance)) {
-			*handle = orb_subscribe_multi(topic, multi_instance);
+		if (OK == orb_exists(sub.metadata, multi_instance)) {
+			handle = orb_subscribe_multi(sub.metadata, multi_instance);
+
+			write_add_logged_msg(sub, multi_instance);
 
 			//PX4_INFO("subscribed to instance %d of topic %s", multi_instance, topic->o_name);
 
 			/* copy first data */
-			if (*handle >= 0) {
-				orb_copy(topic, *handle, buffer);
+			if (handle >= 0) {
+				orb_copy(sub.metadata, handle, buffer);
 				updated = true;
 			}
 		}
 
-	} else if (*handle >= 0) {
-		orb_check(*handle, &updated);
+	} else if (handle >= 0) {
+		orb_check(handle, &updated);
 
 		if (updated) {
-			orb_copy(topic, *handle, buffer);
+			orb_copy(sub.metadata, handle, buffer);
 		}
 	}
 
@@ -568,9 +588,6 @@ void Logger::run()
 				write_changed_parameters();
 			}
 
-			// Write data messages for normal subscriptions
-			int msg_id = 0;
-
 			/* wait for lock on log buffer */
 			_writer.lock();
 
@@ -585,14 +602,12 @@ void Logger::run()
 				 * and write a message to the log
 				 */
 				for (uint8_t instance = 0; instance < ORB_MULTI_MAX_INSTANCES; instance++) {
-					if (copy_if_updated_multi(sub.metadata, instance, &sub.fd[instance], buffer + sizeof(message_data_header_s),
-								  &sub.time_tried_subscribe)) {
+					if (copy_if_updated_multi(sub, instance, buffer + sizeof(message_data_header_s))) {
 
 						message_data_header_s *header = reinterpret_cast<message_data_header_s *>(buffer);
 						header->msg_type = static_cast<uint8_t>(MessageType::DATA);
 						header->msg_size = static_cast<uint16_t>(msg_size - MSG_HEADER_LEN);
-						header->msg_id = msg_id;
-						header->multi_id = 0x80 + instance;	// Non multi, active
+						header->msg_id = sub.msg_ids[instance];
 
 						//PX4_INFO("topic: %s, size = %zu, out_size = %zu", sub.metadata->o_name, sub.metadata->o_size, msg_size);
 
@@ -626,8 +641,6 @@ void Logger::run()
 						}
 					}
 				}
-
-				msg_id++;
 			}
 
 			if (!_dropout_start && _writer.get_buffer_fill_count() > _high_water) {
@@ -853,12 +866,14 @@ void Logger::start_log()
 
 	/* print logging path, important to find log file later */
 	mavlink_log_info(&_mavlink_log_pub, "[logger] file: %s", file_name);
+	_next_topic_id = 0;
 
 	_writer.start_log(file_name);
 	write_header();
 	write_version();
 	write_formats();
 	write_parameters();
+	write_all_add_logged_msg();
 	_writer.notify();
 	_enabled = true;
 	_start_time = hrt_absolute_time();
@@ -890,23 +905,58 @@ void Logger::write_formats()
 {
 	_writer.lock();
 	message_format_s msg;
+	const orb_metadata **topics = orb_get_topics();
 
-	int msg_id = 0;
-
-	for (LoggerSubscription &sub : _subscriptions) {
-		msg.msg_id = msg_id;
-		int format_len = snprintf(msg.format, sizeof(msg.format), "%s", sub.metadata->o_fields);
+	//write all known formats
+	for (size_t i = 0; i < orb_topics_count(); i++) {
+		int format_len = snprintf(msg.format, sizeof(msg.format), "%s", topics[i]->o_fields);
 		size_t msg_size = sizeof(msg) - sizeof(msg.format) + format_len;
 		msg.msg_size = msg_size - MSG_HEADER_LEN;
 
 		write_wait(&msg, msg_size);
-
-		msg_id++;
 	}
 
 	_writer.unlock();
 }
 
+void Logger::write_all_add_logged_msg()
+{
+	_writer.lock();
+
+	for (LoggerSubscription &sub : _subscriptions) {
+		for (int instance = 0; instance < ORB_MULTI_MAX_INSTANCES; ++instance) {
+			if (sub.fd[instance] >= 0) {
+				write_add_logged_msg(sub, instance);
+			}
+		}
+	}
+
+	_writer.unlock();
+}
+
+void Logger::write_add_logged_msg(LoggerSubscription &subscription, int instance)
+{
+	message_add_logged_s msg;
+
+	msg.multi_id = instance;
+	subscription.msg_ids[instance] = _next_topic_id;
+	msg.msg_id = _next_topic_id;
+
+	int message_name_len = 0;
+
+	//we get the name from o_fields, it ends with ':'
+	while (subscription.metadata->o_fields[++message_name_len] != ':');
+
+	memcpy(msg.message_name, subscription.metadata->o_fields, message_name_len);
+
+	size_t msg_size = sizeof(msg) - sizeof(msg.message_name) + message_name_len;
+	msg.msg_size = msg_size - MSG_HEADER_LEN;
+
+	write_wait(&msg, msg_size);
+
+	++_next_topic_id;
+}
+
 /* write info message */
 void Logger::write_info(const char *name, const char *value)
 {
diff --git a/src/modules/logger/logger.h b/src/modules/logger/logger.h
index af03d4b8c6..eb7e64255b 100644
--- a/src/modules/logger/logger.h
+++ b/src/modules/logger/logger.h
@@ -54,6 +54,7 @@ namespace logger
 
 struct LoggerSubscription {
 	int fd[ORB_MULTI_MAX_INSTANCES];
+	uint16_t msg_ids[ORB_MULTI_MAX_INSTANCES];
 	uint64_t time_tried_subscribe;	// captures the time at which we checked last time if this instance existed
 	const orb_metadata *metadata = nullptr;
 
@@ -79,10 +80,20 @@ public:
 
 	~Logger();
 
-	int add_topic(const orb_metadata *topic);
-
+	/**
+	 * Add a topic to be logged. This must be called before start_log()
+	 * (because it does not write an ADD_LOGGED_MSG message).
+	 * @param name topic name
+	 * @param interval limit rate if >0, otherwise log as fast as the topic is updated.
+	 * @return 0 on success
+	 */
 	int add_topic(const char *name, unsigned interval);
 
+	/**
+	 * add a logged topic (called by add_topic() above)
+	 */
+	int add_topic(const orb_metadata *topic);
+
 	static int start(char *const *argv);
 
 	static void usage(const char *reason);
@@ -94,6 +105,17 @@ private:
 
 	void run();
 
+	/**
+	 * Write an ADD_LOGGED_MSG to the log for a all current subscriptions and instances
+	 */
+	void write_all_add_logged_msg();
+
+	/**
+	 * Write an ADD_LOGGED_MSG to the log for a given subscription and instance.
+	 * _writer.lock() must be held when calling this.
+	 */
+	void write_add_logged_msg(LoggerSubscription &subscription, int instance);
+
 	/**
 	 * Create logging directory
 	 * @param tt if not null, use it for the directory name
@@ -134,7 +156,7 @@ private:
 
 	void write_changed_parameters();
 
-	bool copy_if_updated_multi(orb_id_t topic, int multi_instance, int *handle, void *buffer, uint64_t *time_last_checked);
+	bool copy_if_updated_multi(LoggerSubscription &sub, int multi_instance, void *buffer);
 
 	/**
 	 * Write data to the logger. Waits if buffer is full until all data is written.
@@ -180,6 +202,7 @@ private:
 	uint32_t					_log_interval;
 	param_t						_log_utc_offset;
 	orb_advert_t					_mavlink_log_pub = nullptr;
+	uint16_t					_next_topic_id; ///< id of next subscribed topic
 };
 
 Logger *logger_ptr = nullptr;

From 4f8d16cc4de352cb0e22718da934efa5b2c2f38a Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Beat=20K=C3=BCng?= <beat-kueng@gmx.net>
Date: Tue, 17 May 2016 16:54:20 +0200
Subject: [PATCH 0314/1230] logger: log dropout events

---
 src/modules/logger/log_writer.cpp |  24 +++++-
 src/modules/logger/log_writer.h   |   8 +-
 src/modules/logger/logger.cpp     |  71 +-----------------
 src/modules/logger/messages.h     | 118 ++++++++++++++++++++++++++++++
 4 files changed, 148 insertions(+), 73 deletions(-)
 create mode 100644 src/modules/logger/messages.h

diff --git a/src/modules/logger/log_writer.cpp b/src/modules/logger/log_writer.cpp
index 2d99cf6b69..1268ecb596 100644
--- a/src/modules/logger/log_writer.cpp
+++ b/src/modules/logger/log_writer.cpp
@@ -32,6 +32,7 @@
  ****************************************************************************/
 
 #include "log_writer.h"
+#include "messages.h"
 #include <fcntl.h>
 #include <string.h>
 
@@ -241,17 +242,35 @@ void LogWriter::run()
 	}
 }
 
-bool LogWriter::write(void *ptr, size_t size)
+bool LogWriter::write(void *ptr, size_t size, uint64_t dropout_start)
 {
 
 	// Bytes available to write
 	size_t available = _buffer_size - _count;
+	size_t dropout_size = 0;
 
-	if (size > available) {
+	if (dropout_start) {
+		dropout_size = sizeof(message_dropout_s);
+	}
+
+	if (size + dropout_size > available) {
 		// buffer overflow
 		return false;
 	}
 
+	if (dropout_start) {
+		//write dropout msg
+		message_dropout_s dropout_msg;
+		dropout_msg.duration = (uint16_t)(hrt_elapsed_time(&dropout_start) / 1000);
+		write_no_check(&dropout_msg, sizeof(dropout_msg));
+	}
+
+	write_no_check(ptr, size);
+	return true;
+}
+
+void LogWriter::write_no_check(void *ptr, size_t size)
+{
 	size_t n = _buffer_size - _head;	// bytes to end of the buffer
 
 	uint8_t *buffer_c = reinterpret_cast<uint8_t *>(ptr);
@@ -270,7 +289,6 @@ bool LogWriter::write(void *ptr, size_t size)
 	memcpy(&(_buffer[_head]), &(buffer_c[n]), p);
 	_head = (_head + p) % _buffer_size;
 	_count += size;
-	return true;
 }
 
 size_t LogWriter::get_read_ptr(void **ptr, bool *is_part)
diff --git a/src/modules/logger/log_writer.h b/src/modules/logger/log_writer.h
index 115f4e03ff..1d230f5602 100644
--- a/src/modules/logger/log_writer.h
+++ b/src/modules/logger/log_writer.h
@@ -67,9 +67,10 @@ public:
 
 	/**
 	 * Write data to be logged. The caller must call lock() before calling this.
+	 * @param dropout_start timestamp when lastest dropout occured. 0 if no dropout at the moment.
 	 * @return true on success, false if not enough space in the buffer left
 	 */
-	bool write(void *ptr, size_t size);
+	bool write(void *ptr, size_t size, uint64_t dropout_start = 0);
 
 	void lock()
 	{
@@ -113,6 +114,11 @@ private:
 		_count -= n;
 	}
 
+	/**
+	 * Write to the buffer but assuming there is enough space
+	 */
+	inline void write_no_check(void *ptr, size_t size);
+
 	/* 512 didn't seem to work properly, 4096 should match the FAT cluster size */
 	static constexpr size_t	_min_write_chunk = 4096;
 
diff --git a/src/modules/logger/logger.cpp b/src/modules/logger/logger.cpp
index 034eece065..0ac8134b0b 100644
--- a/src/modules/logger/logger.cpp
+++ b/src/modules/logger/logger.cpp
@@ -32,6 +32,7 @@
  ****************************************************************************/
 
 #include "logger.h"
+#include "messages.h"
 
 #include <sys/stat.h>
 #include <errno.h>
@@ -282,74 +283,6 @@ void Logger::run_trampoline(int argc, char *argv[])
 	logger_task = -1;
 }
 
-enum class MessageType : uint8_t {
-	FORMAT = 'F',
-	DATA = 'D',
-	INFO = 'I',
-	PARAMETER = 'P',
-	ADD_LOGGED_MSG = 'A',
-	REMOVE_LOGGED_MSG = 'R',
-	SYNC = 'S',
-	DROPOUT = 'O',
-};
-
-/* declare message data structs with byte alignment (no padding) */
-#pragma pack(push, 1)
-
-#define MSG_HEADER_LEN 3 //accounts for msg_size and msg_type
-
-/** first bytes of the file */
-struct message_file_header_s {
-	uint8_t magic[8];
-	uint64_t timestamp;
-};
-
-struct message_format_s {
-	uint16_t msg_size; //size of message - MSG_HEADER_LEN
-	uint8_t msg_type = static_cast<uint8_t>(MessageType::FORMAT);
-
-	char format[2096];
-};
-
-struct message_add_logged_s {
-	uint16_t msg_size; //size of message - MSG_HEADER_LEN
-	uint8_t msg_type = static_cast<uint8_t>(MessageType::ADD_LOGGED_MSG);
-
-	uint8_t multi_id;
-	uint16_t msg_id;
-	char message_name[255];
-};
-
-struct message_remove_logged_s {
-	uint16_t msg_size; //size of message - MSG_HEADER_LEN
-	uint8_t msg_type = static_cast<uint8_t>(MessageType::REMOVE_LOGGED_MSG);
-
-	uint16_t msg_id;
-};
-struct message_data_header_s {
-	uint16_t msg_size; //size of message - MSG_HEADER_LEN
-	uint8_t msg_type = static_cast<uint8_t>(MessageType::DATA);
-
-	uint16_t msg_id;
-};
-
-struct message_info_header_s {
-	uint16_t msg_size; //size of message - MSG_HEADER_LEN
-	uint8_t msg_type = static_cast<uint8_t>(MessageType::INFO);
-
-	uint8_t key_len;
-	char key[255];
-};
-
-struct message_parameter_header_s {
-	uint16_t msg_size;
-	uint8_t msg_type = static_cast<uint8_t>(MessageType::PARAMETER);
-
-	uint8_t key_len;
-	char key[255];
-};
-#pragma pack(pop)
-
 
 static constexpr size_t MAX_DATA_SIZE = 740;
 
@@ -611,7 +544,7 @@ void Logger::run()
 
 						//PX4_INFO("topic: %s, size = %zu, out_size = %zu", sub.metadata->o_name, sub.metadata->o_size, msg_size);
 
-						if (_writer.write(buffer, msg_size)) {
+						if (_writer.write(buffer, msg_size, _dropout_start)) {
 
 #ifdef DBGPRINT
 							total_bytes += msg_size;
diff --git a/src/modules/logger/messages.h b/src/modules/logger/messages.h
new file mode 100644
index 0000000000..58fdd56a94
--- /dev/null
+++ b/src/modules/logger/messages.h
@@ -0,0 +1,118 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2016 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+#pragma once
+
+enum class MessageType : uint8_t {
+	FORMAT = 'F',
+	DATA = 'D',
+	INFO = 'I',
+	PARAMETER = 'P',
+	ADD_LOGGED_MSG = 'A',
+	REMOVE_LOGGED_MSG = 'R',
+	SYNC = 'S',
+	DROPOUT = 'O',
+};
+
+
+/* declare message data structs with byte alignment (no padding) */
+#pragma pack(push, 1)
+
+#define MSG_HEADER_LEN 3 //accounts for msg_size and msg_type
+
+/** first bytes of the file */
+struct message_file_header_s {
+	uint8_t magic[8];
+	uint64_t timestamp;
+};
+
+struct message_format_s {
+	uint16_t msg_size; //size of message - MSG_HEADER_LEN
+	uint8_t msg_type = static_cast<uint8_t>(MessageType::FORMAT);
+
+	char format[2096];
+};
+
+struct message_add_logged_s {
+	uint16_t msg_size; //size of message - MSG_HEADER_LEN
+	uint8_t msg_type = static_cast<uint8_t>(MessageType::ADD_LOGGED_MSG);
+
+	uint8_t multi_id;
+	uint16_t msg_id;
+	char message_name[255];
+};
+
+struct message_remove_logged_s {
+	uint16_t msg_size; //size of message - MSG_HEADER_LEN
+	uint8_t msg_type = static_cast<uint8_t>(MessageType::REMOVE_LOGGED_MSG);
+
+	uint16_t msg_id;
+};
+
+struct message_sync_s {
+	uint16_t msg_size; //size of message - MSG_HEADER_LEN
+	uint8_t msg_type = static_cast<uint8_t>(MessageType::SYNC);
+
+	uint8_t sync_magic[8];
+};
+
+struct message_dropout_s {
+	uint16_t msg_size = sizeof(uint16_t); //size of message - MSG_HEADER_LEN
+	uint8_t msg_type = static_cast<uint8_t>(MessageType::DROPOUT);
+
+	uint16_t duration; //in ms
+};
+
+struct message_data_header_s {
+	uint16_t msg_size; //size of message - MSG_HEADER_LEN
+	uint8_t msg_type = static_cast<uint8_t>(MessageType::DATA);
+
+	uint16_t msg_id;
+};
+
+struct message_info_header_s {
+	uint16_t msg_size; //size of message - MSG_HEADER_LEN
+	uint8_t msg_type = static_cast<uint8_t>(MessageType::INFO);
+
+	uint8_t key_len;
+	char key[255];
+};
+
+struct message_parameter_header_s {
+	uint16_t msg_size;
+	uint8_t msg_type = static_cast<uint8_t>(MessageType::PARAMETER);
+
+	uint8_t key_len;
+	char key[255];
+};
+#pragma pack(pop)

From 623fe7ca2cbd499ee3a46674d2d5a88ce258dc73 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Beat=20K=C3=BCng?= <beat-kueng@gmx.net>
Date: Wed, 18 May 2016 09:46:10 +0200
Subject: [PATCH 0315/1230] logger + uorb msg template: rm msg name from
 o_fields to save space

Instead we use o_name to get the topic name. Now the topic names are not
upper case anymore in the log format. This makes it more consistent, eg.
if used as a nested topic
---
 msg/templates/uorb/msg.cpp.template |  3 ++-
 src/modules/logger/logger.cpp       | 11 ++++-------
 2 files changed, 6 insertions(+), 8 deletions(-)

diff --git a/msg/templates/uorb/msg.cpp.template b/msg/templates/uorb/msg.cpp.template
index abbf20f51a..67789a8152 100644
--- a/msg/templates/uorb/msg.cpp.template
+++ b/msg/templates/uorb/msg.cpp.template
@@ -70,7 +70,8 @@ topic_fields = ["uint64_t timestamp"]+["%s %s" % (convert_type(field.type), fiel
 
 
 @# join all msg files in one line e.g: "float[3] position;float[3] velocity;bool armed"
-const char *__orb_@(topic_name)_fields = "@( topic_name.upper() ):@( ";".join(topic_fields) );";
+@# This is used for the logger
+const char *__orb_@(topic_name)_fields = "@( ";".join(topic_fields) );";
 
 @[for multi_topic in topics]@
 ORB_DEFINE(@multi_topic, struct @uorb_struct, @(struct_size-padding_end_size),
diff --git a/src/modules/logger/logger.cpp b/src/modules/logger/logger.cpp
index 0ac8134b0b..a025576934 100644
--- a/src/modules/logger/logger.cpp
+++ b/src/modules/logger/logger.cpp
@@ -330,7 +330,7 @@ int Logger::add_topic(const orb_metadata *topic)
 
 	}
 
-	size_t fields_len = strlen(topic->o_fields);
+	size_t fields_len = strlen(topic->o_fields) + strlen(topic->o_name) + 1; //1 for ':'
 
 	if (fields_len > sizeof(message_format_s::format)) {
 		PX4_WARN("skip topic %s, format string is too large: %zu (max is %zu)", topic->o_name, fields_len,
@@ -842,7 +842,7 @@ void Logger::write_formats()
 
 	//write all known formats
 	for (size_t i = 0; i < orb_topics_count(); i++) {
-		int format_len = snprintf(msg.format, sizeof(msg.format), "%s", topics[i]->o_fields);
+		int format_len = snprintf(msg.format, sizeof(msg.format), "%s:%s", topics[i]->o_name, topics[i]->o_fields);
 		size_t msg_size = sizeof(msg) - sizeof(msg.format) + format_len;
 		msg.msg_size = msg_size - MSG_HEADER_LEN;
 
@@ -875,12 +875,9 @@ void Logger::write_add_logged_msg(LoggerSubscription &subscription, int instance
 	subscription.msg_ids[instance] = _next_topic_id;
 	msg.msg_id = _next_topic_id;
 
-	int message_name_len = 0;
+	int message_name_len = strlen(subscription.metadata->o_name);
 
-	//we get the name from o_fields, it ends with ':'
-	while (subscription.metadata->o_fields[++message_name_len] != ':');
-
-	memcpy(msg.message_name, subscription.metadata->o_fields, message_name_len);
+	memcpy(msg.message_name, subscription.metadata->o_name, message_name_len);
 
 	size_t msg_size = sizeof(msg) - sizeof(msg.message_name) + message_name_len;
 	msg.msg_size = msg_size - MSG_HEADER_LEN;

From bd96afa00b28ec2bbc8cb4427619339914921e1c Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Beat=20K=C3=BCng?= <beat-kueng@gmx.net>
Date: Wed, 18 May 2016 11:16:18 +0200
Subject: [PATCH 0316/1230] logger: use int32_t for utc_offset instead of
 uint32_t

---
 src/modules/logger/logger.cpp | 8 ++++----
 src/modules/logger/logger.h   | 2 +-
 2 files changed, 5 insertions(+), 5 deletions(-)

diff --git a/src/modules/logger/logger.cpp b/src/modules/logger/logger.cpp
index a025576934..e3dc4179d5 100644
--- a/src/modules/logger/logger.cpp
+++ b/src/modules/logger/logger.cpp
@@ -912,7 +912,7 @@ void Logger::write_info(const char *name, const char *value)
 
 	_writer.unlock();
 }
-void Logger::write_info(const char *name, uint32_t value)
+void Logger::write_info(const char *name, int32_t value)
 {
 	_writer.lock();
 	uint8_t buffer[sizeof(message_info_header_s)];
@@ -920,12 +920,12 @@ void Logger::write_info(const char *name, uint32_t value)
 	msg->msg_type = static_cast<uint8_t>(MessageType::INFO);
 
 	/* construct format key (type and name) */
-	msg->key_len = snprintf(msg->key, sizeof(msg->key), "uint32_t %s", name);
+	msg->key_len = snprintf(msg->key, sizeof(msg->key), "int32_t %s", name);
 	size_t msg_size = sizeof(*msg) - sizeof(msg->key) + msg->key_len;
 
 	/* copy string value directly to buffer */
-	memcpy(&buffer[msg_size], &value, sizeof(uint32_t));
-	msg_size += sizeof(uint32_t);
+	memcpy(&buffer[msg_size], &value, sizeof(int32_t));
+	msg_size += sizeof(int32_t);
 
 	msg->msg_size = msg_size - MSG_HEADER_LEN;
 
diff --git a/src/modules/logger/logger.h b/src/modules/logger/logger.h
index eb7e64255b..d061b9917f 100644
--- a/src/modules/logger/logger.h
+++ b/src/modules/logger/logger.h
@@ -150,7 +150,7 @@ private:
 	void write_version();
 
 	void write_info(const char *name, const char *value);
-	void write_info(const char *name, uint32_t value);
+	void write_info(const char *name, int32_t value);
 
 	void write_parameters();
 

From a9930c2173af7ba1bc5ca551d1d60179aed40afb Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Beat=20K=C3=BCng?= <beat-kueng@gmx.net>
Date: Fri, 20 May 2016 10:20:40 +0200
Subject: [PATCH 0317/1230] logger: avoid logging the padding if it's at the
 end of a message format

---
 src/modules/logger/logger.cpp | 3 +--
 1 file changed, 1 insertion(+), 2 deletions(-)

diff --git a/src/modules/logger/logger.cpp b/src/modules/logger/logger.cpp
index e3dc4179d5..6b6e909b82 100644
--- a/src/modules/logger/logger.cpp
+++ b/src/modules/logger/logger.cpp
@@ -527,8 +527,7 @@ void Logger::run()
 			for (LoggerSubscription &sub : _subscriptions) {
 				/* each message consists of a header followed by an orb data object
 				 */
-				size_t msg_size = sizeof(message_data_header_s) + sub.metadata->o_size;
-				//TODO: use sub.metadata->o_size_no_padding
+				size_t msg_size = sizeof(message_data_header_s) + sub.metadata->o_size_no_padding;
 				uint8_t buffer[msg_size];
 
 				/* if this topic has been updated, copy the new data into the message buffer

From cd7c955067fd33235e8b8afbb86ba3ddee81b37b Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Beat=20K=C3=BCng?= <beat-kueng@gmx.net>
Date: Fri, 20 May 2016 11:05:44 +0200
Subject: [PATCH 0318/1230] logger: -t param: fall back to px4_clock_gettime if
 there's no gps fix

---
 src/modules/logger/logger.cpp | 5 ++++-
 1 file changed, 4 insertions(+), 1 deletion(-)

diff --git a/src/modules/logger/logger.cpp b/src/modules/logger/logger.cpp
index 6b6e909b82..9a4eb1553a 100644
--- a/src/modules/logger/logger.cpp
+++ b/src/modules/logger/logger.cpp
@@ -761,7 +761,10 @@ bool Logger::get_log_time(struct tm *tt, bool boot_time)
 	time_t utc_time_sec = gps_pos.time_utc_usec / 1e6;
 
 	if (gps_pos.fix_type < 2 || utc_time_sec < GPS_EPOCH_SECS) {
-		return false;
+		//take clock time if there's no fix (yet)
+		struct timespec ts;
+		px4_clock_gettime(CLOCK_REALTIME, &ts);
+		utc_time_sec = ts.tv_sec + (ts.tv_nsec / 1e9);
 	}
 
 	/* strip the time elapsed since boot */

From 7f3a95508e03bfddf33ec240b06e09f1d21fa757 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Beat=20K=C3=BCng?= <beat-kueng@gmx.net>
Date: Fri, 20 May 2016 12:11:32 +0200
Subject: [PATCH 0319/1230] logger: move SDLOG_UTC_OFFSET param definition into
 logger module

It's still used in sdlog2, but once we'll depricate/remove sdlog2, we will
not remove this parameter as well.
---
 src/modules/logger/params.c | 50 +++++++++++++++++++++++++++++++++++++
 src/modules/sdlog2/params.c | 18 -------------
 2 files changed, 50 insertions(+), 18 deletions(-)
 create mode 100644 src/modules/logger/params.c

diff --git a/src/modules/logger/params.c b/src/modules/logger/params.c
new file mode 100644
index 0000000000..3ffc1eb6ca
--- /dev/null
+++ b/src/modules/logger/params.c
@@ -0,0 +1,50 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2012-2016 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * UTC offset (unit: min)
+ *
+ * the difference in hours and minutes from Coordinated
+ * Universal Time (UTC) for a your place and date.
+ *
+ * for example, In case of South Korea(UTC+09:00),
+ * UTC offset is 540 min (9*60)
+ *
+ * refer to https://en.wikipedia.org/wiki/List_of_UTC_time_offsets
+ *
+ * @unit min
+ * @min -1000
+ * @max  1000
+ * @group SD Logging
+ */
+PARAM_DEFINE_INT32(SDLOG_UTC_OFFSET, 0);
diff --git a/src/modules/sdlog2/params.c b/src/modules/sdlog2/params.c
index 72c513114a..0e9f49f950 100644
--- a/src/modules/sdlog2/params.c
+++ b/src/modules/sdlog2/params.c
@@ -77,24 +77,6 @@ PARAM_DEFINE_INT32(SDLOG_EXT, -1);
  */
 PARAM_DEFINE_INT32(SDLOG_GPSTIME, 1);
 
-/**
- * UTC offset (unit: min)
- *
- * the difference in hours and minutes from Coordinated 
- * Universal Time (UTC) for a your place and date.
- *
- * for example, In case of South Korea(UTC+09:00), 
- * UTC offset is 540 min (9*60)
- *
- * refer to https://en.wikipedia.org/wiki/List_of_UTC_time_offsets
- *
- * @unit min
- * @min -1000
- * @max  1000
- * @group SD Logging
- */
-PARAM_DEFINE_INT32(SDLOG_UTC_OFFSET, 0);
-
 /**
  * Give logging app higher thread priority to avoid data loss.
  * This is used for gathering replay logs for the ekf2 module.

From 1d3669714b20e15db2822880490e4901566fff7a Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Sun, 22 May 2016 14:27:53 +0200
Subject: [PATCH 0320/1230] EKF2 config: Do not build other estimators

---
 cmake/configs/nuttx_px4fmu-v2_ekf2.cmake | 4 ----
 1 file changed, 4 deletions(-)

diff --git a/cmake/configs/nuttx_px4fmu-v2_ekf2.cmake b/cmake/configs/nuttx_px4fmu-v2_ekf2.cmake
index 289eb4098a..1eeab2fa96 100644
--- a/cmake/configs/nuttx_px4fmu-v2_ekf2.cmake
+++ b/cmake/configs/nuttx_px4fmu-v2_ekf2.cmake
@@ -79,11 +79,7 @@ set(config_module_list
 	#
 	# Estimation modules (EKF/ SO3 / other filters)
 	#
-	# Too high RAM usage due to static allocations
-	# modules/attitude_estimator_ekf
-	modules/attitude_estimator_q
 	modules/ekf2
-	modules/position_estimator_inav
 
 	#
 	# Vehicle Control

From 655b605431a57b269f252d99fa7b52f52eaf71be Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Sun, 22 May 2016 14:54:57 +0200
Subject: [PATCH 0321/1230] Do not force safety disable for all FMUv4 units

---
 ROMFS/px4fmu_common/init.d/rcS | 8 --------
 1 file changed, 8 deletions(-)

diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS
index 997459183b..dbb71f1db4 100644
--- a/ROMFS/px4fmu_common/init.d/rcS
+++ b/ROMFS/px4fmu_common/init.d/rcS
@@ -541,14 +541,6 @@ then
 		fi
 	fi
 
-	# Transitional support: Disable safety on all Pixracer boards
-	if ver hwcmp PX4FMU_V4
-	then
-		if param set CBRK_IO_SAFETY 22027
-		then
-		fi
-	fi
-
 	#
 	# Starting stuff according to UAVCAN_ENABLE value
 	#

From c4da55e40ff2ab3c747f72003b79f59746c195cc Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Sun, 22 May 2016 15:03:22 +0200
Subject: [PATCH 0322/1230] MAVLink app: Reduce excessive allocation

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

diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp
index 349f670059..417975faab 100644
--- a/src/modules/mavlink/mavlink_main.cpp
+++ b/src/modules/mavlink/mavlink_main.cpp
@@ -2286,7 +2286,7 @@ Mavlink::start(int argc, char *argv[])
 	px4_task_spawn_cmd(buf,
 			   SCHED_DEFAULT,
 			   SCHED_PRIORITY_DEFAULT,
-			   3000,
+			   2800,
 			   (px4_main_t)&Mavlink::start_helper,
 			   (char *const *)argv);
 

From e6bfe4348c26b400125d3e7e2b8bb4609f54adcc Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Sun, 22 May 2016 15:17:44 +0200
Subject: [PATCH 0323/1230] Reduce logging buffers to free some RAM

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

diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS
index dbb71f1db4..8a4e0fb441 100644
--- a/ROMFS/px4fmu_common/init.d/rcS
+++ b/ROMFS/px4fmu_common/init.d/rcS
@@ -623,7 +623,7 @@ then
 		then
 			if param compare SYS_LOGGER 0
 			then
-				if sdlog2 start -r 500 -e -b 20 -t
+				if sdlog2 start -r 500 -e -b 18 -t
 				then
 				fi
 			else
@@ -634,11 +634,11 @@ then
 		else
 			if param compare SYS_LOGGER 0
 			then
-				if sdlog2 start -r 100 -a -b 12 -t
+				if sdlog2 start -r 100 -a -b 10 -t
 				then
 				fi
 			else
-				if logger start -b 24
+				if logger start -b 20
 				then
 				fi
 			fi

From 2c6a8c0ce619a7ba57fb55a474e7f6ac35b682ef Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Sun, 22 May 2016 15:21:56 +0200
Subject: [PATCH 0324/1230] MS5611: Drop readout rate to 25 Hz to alleviate
 self-heating.

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

diff --git a/src/drivers/ms5611/ms5611_nuttx.cpp b/src/drivers/ms5611/ms5611_nuttx.cpp
index 7de852eebc..07443ecd8b 100644
--- a/src/drivers/ms5611/ms5611_nuttx.cpp
+++ b/src/drivers/ms5611/ms5611_nuttx.cpp
@@ -98,7 +98,7 @@ static const int ERROR = -1;
  */
 
 /* internal conversion time: 9.17 ms, so should not be read at rates higher than 100 Hz */
-#define MS5611_CONVERSION_INTERVAL	10000	/* microseconds */
+#define MS5611_CONVERSION_INTERVAL	40000	/* microseconds */
 #define MS5611_MEASUREMENT_RATIO	3	/* pressure measurements per temperature measurement */
 #define MS5611_BARO_DEVICE_PATH_EXT	"/dev/ms5611_ext"
 #define MS5611_BARO_DEVICE_PATH_INT	"/dev/ms5611_int"

From 7c6d24d6b01b5cdeafb9fe225a9481a0cd5141b1 Mon Sep 17 00:00:00 2001
From: Mark Charlebois <charlebm@gmail.com>
Date: Fri, 20 May 2016 14:42:37 -0700
Subject: [PATCH 0325/1230] Updated DriverFramework with critical qurt fixes

Signed-off-by: Mark Charlebois <charlebm@gmail.com>
---
 src/lib/DriverFramework | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/src/lib/DriverFramework b/src/lib/DriverFramework
index 62449aef6d..1fd7742774 160000
--- a/src/lib/DriverFramework
+++ b/src/lib/DriverFramework
@@ -1 +1 @@
-Subproject commit 62449aef6decd2faa75bc2abd509333df5a675c4
+Subproject commit 1fd7742774bc257bd419eaea581887f0782ef334

From 8845070d89644e61de8810f4aad6d17dda5555bc Mon Sep 17 00:00:00 2001
From: Mark Charlebois <charlebm@gmail.com>
Date: Fri, 20 May 2016 15:11:33 -0700
Subject: [PATCH 0326/1230] Revered changes by Daniel Agar that broke posix
 eagle build

The following cflags must be set for the eagle builds that compile the
generated IDL stubs:

	-Wno-missing-prototypes -Wno-missing-declarations

Signed-off-by: Mark Charlebois <charlebm@gmail.com>
---
 src/firmware/posix/CMakeLists.txt | 6 +++---
 1 file changed, 3 insertions(+), 3 deletions(-)

diff --git a/src/firmware/posix/CMakeLists.txt b/src/firmware/posix/CMakeLists.txt
index 8d0232a017..7a28919ed8 100644
--- a/src/firmware/posix/CMakeLists.txt
+++ b/src/firmware/posix/CMakeLists.txt
@@ -11,7 +11,7 @@ if ("${BOARD}" STREQUAL "eagle" OR ("${BOARD}" STREQUAL "excelsior"))
 
 	FASTRPC_STUB_GEN(../qurt/px4muorb.idl)
 
-	set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS}")
+	set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -Wno-missing-prototypes -Wno-missing-declarations")
 	LINUX_APP(
 		APP_NAME mainapp
 		IDL_NAME px4muorb
@@ -36,14 +36,14 @@ if ("${BOARD}" STREQUAL "eagle" OR ("${BOARD}" STREQUAL "excelsior"))
 				 ${CMAKE_SOURCE_DIR}/posix-configs/eagle/flight/mainapp.config
 			 DEPENDS mainapp
 			 DEST /home/linaro)
-			 
+
 elseif ("${BOARD}" STREQUAL "rpi2")
 
 	add_executable(mainapp
 		${CMAKE_SOURCE_DIR}/src/platforms/posix/main.cpp
 		apps.h
 		)
-		
+
 	target_link_libraries(mainapp
 		-Wl,--start-group
 		${module_libraries}

From ea138cfd380c06179d2e89cdd8167637dbd96d54 Mon Sep 17 00:00:00 2001
From: Mark Charlebois <charlebm@gmail.com>
Date: Sat, 21 May 2016 13:02:16 -0700
Subject: [PATCH 0327/1230] Updated DriverFramework with NuttX fixes

See required DriverFramework change:

   https://github.com/PX4/DriverFramework/pull/83

Signed-off-by: Mark Charlebois <charlebm@gmail.com>
---
 src/lib/DriverFramework | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/src/lib/DriverFramework b/src/lib/DriverFramework
index 1fd7742774..8622af59d2 160000
--- a/src/lib/DriverFramework
+++ b/src/lib/DriverFramework
@@ -1 +1 @@
-Subproject commit 1fd7742774bc257bd419eaea581887f0782ef334
+Subproject commit 8622af59d2126a5d8fd25726a1a76d35561e1979

From 64c3b330eef72395dc1243f9a4de0f6041cf13f2 Mon Sep 17 00:00:00 2001
From: jwilson <jywilson99@hotmail.com>
Date: Thu, 5 May 2016 19:51:13 -0700
Subject: [PATCH 0328/1230] Partial fix for Snapdragon HITL mode

---
 Makefile                            |  1 +
 posix-configs/eagle/hil/px4.config  | 22 +++++++++++++---------
 src/modules/commander/commander.cpp |  2 +-
 3 files changed, 15 insertions(+), 10 deletions(-)

diff --git a/Makefile b/Makefile
index 85fb00742a..8b3295aed4 100644
--- a/Makefile
+++ b/Makefile
@@ -189,6 +189,7 @@ posix_eagle_default:
 	$(call cmake-build,$@)
 
 eagle_default: posix_eagle_default qurt_eagle_default
+eagle_legacy_default: posix_eagle_legacy_driver_default qurt_eagle_legacy_driver_default
 
 qurt_eagle_legacy_driver_default:
 	$(call cmake-build,$@)	
diff --git a/posix-configs/eagle/hil/px4.config b/posix-configs/eagle/hil/px4.config
index 8b626bd322..e446ad144e 100644
--- a/posix-configs/eagle/hil/px4.config
+++ b/posix-configs/eagle/hil/px4.config
@@ -1,16 +1,12 @@
 uorb start
+qshell start
 param set CAL_GYRO0_ID 2293760
 param set CAL_ACC0_ID 1310720
 param set CAL_ACC1_ID 1376256
 param set CAL_MAG0_ID 196608
-commander start -hil
-sensors start
-attitude_estimator_q start
-position_estimator_inav start
-mc_pos_control start
-mc_att_control start
-sleep 1
-pwm_out_sim mode_pwm
+param set SYS_AUTOSTART 4001
+param set SYS_AUTOCONFIG 1
+param set MAV_TYPE 2
 param set RC1_MAX 2015
 param set RC1_MIN 996
 param set RC1_TRIM 1502
@@ -42,7 +38,15 @@ param set ATT_W_ACC 0.0002
 param set ATT_W_MAG 0.002
 param set ATT_W_GYRO_BIAS 0.05
 sleep 1
-param set MAV_TYPE 2
+commander start -hil
+sensors start
+attitude_estimator_q start
+position_estimator_inav start
+mc_pos_control start
+mc_att_control start
+land_detector start multicopter
+sleep 1
+pwm_out_sim mode_pwm
 mixer load /dev/pwm_output0 /startup/quad_x.main.mix
 list_devices
 list_files
diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp
index d61805f9aa..e472298bba 100644
--- a/src/modules/commander/commander.cpp
+++ b/src/modules/commander/commander.cpp
@@ -1157,7 +1157,7 @@ int commander_thread_main(int argc, char *argv[])
 #endif
 
 	if (argc > 2) {
-		if (!strcmp(argv[3],"-hil")) {
+		if (!strcmp(argv[2],"-hil")) {
 			startup_in_hil = true;
 		} else {
 			PX4_ERR("Argument %s not supported.", argv[3]);

From 723b50118697b3bbd45b6aeed451dbc1b4ed8f96 Mon Sep 17 00:00:00 2001
From: bharathr <bharathr.mail@gmail.com>
Date: Thu, 19 May 2016 01:15:31 -0700
Subject: [PATCH 0329/1230] Restricted the previous commit to __PX4_QURT only

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

diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp
index e472298bba..8edc0500a3 100644
--- a/src/modules/commander/commander.cpp
+++ b/src/modules/commander/commander.cpp
@@ -1157,7 +1157,9 @@ int commander_thread_main(int argc, char *argv[])
 #endif
 
 	if (argc > 2) {
+#ifdef __PX4_QURT
 		if (!strcmp(argv[2],"-hil")) {
+#endif
 			startup_in_hil = true;
 		} else {
 			PX4_ERR("Argument %s not supported.", argv[3]);

From 250aab66ed796dd7a38cdb3f3b9fcd68c1d9d631 Mon Sep 17 00:00:00 2001
From: bharathr <bharathr.mail@gmail.com>
Date: Thu, 19 May 2016 13:14:35 -0700
Subject: [PATCH 0330/1230] Fixed the parsing of commander arguments for
 non-QURT builds

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

diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp
index 8edc0500a3..15299abff2 100644
--- a/src/modules/commander/commander.cpp
+++ b/src/modules/commander/commander.cpp
@@ -1159,10 +1159,16 @@ int commander_thread_main(int argc, char *argv[])
 	if (argc > 2) {
 #ifdef __PX4_QURT
 		if (!strcmp(argv[2],"-hil")) {
+#else
+		if (!strcmp(argv[3],"-hil")) {
 #endif
 			startup_in_hil = true;
 		} else {
+#ifdef __PX4_QURT		
+			PX4_ERR("Argument %s not supported.", argv[2]);
+#else
 			PX4_ERR("Argument %s not supported.", argv[3]);
+#endif			
 			PX4_ERR("COMMANDER NOT STARTED");
 			thread_should_exit = true;
 		}

From d9718d0d9869cb94c0f80383cfb334010c46d982 Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Wed, 25 May 2016 20:56:25 +0200
Subject: [PATCH 0331/1230] Fix commander argc handling

---
 src/modules/commander/commander.cpp | 14 +++-----------
 1 file changed, 3 insertions(+), 11 deletions(-)

diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp
index 15299abff2..4f6e2327f1 100644
--- a/src/modules/commander/commander.cpp
+++ b/src/modules/commander/commander.cpp
@@ -1154,22 +1154,14 @@ int commander_thread_main(int argc, char *argv[])
 #ifdef __PX4_NUTTX
 	/* NuttX indicates 3 arguments when only 2 are present */
 	argc -= 1;
+	argv += 1;
 #endif
 
 	if (argc > 2) {
-#ifdef __PX4_QURT
 		if (!strcmp(argv[2],"-hil")) {
-#else
-		if (!strcmp(argv[3],"-hil")) {
-#endif
 			startup_in_hil = true;
 		} else {
-#ifdef __PX4_QURT		
-			PX4_ERR("Argument %s not supported.", argv[2]);
-#else
-			PX4_ERR("Argument %s not supported.", argv[3]);
-#endif			
-			PX4_ERR("COMMANDER NOT STARTED");
+			PX4_ERR("Argument %s not supported, abort.", argv[2]);	
 			thread_should_exit = true;
 		}
 	}
@@ -1269,7 +1261,7 @@ int commander_thread_main(int argc, char *argv[])
 	status.nav_state = vehicle_status_s::NAVIGATION_STATE_MANUAL;
 	status.arming_state = vehicle_status_s::ARMING_STATE_INIT;
 
-	if(startup_in_hil) {
+	if (startup_in_hil) {
 		status.hil_state = vehicle_status_s::HIL_STATE_ON;
 	} else {
 		status.hil_state = vehicle_status_s::HIL_STATE_OFF;

From af06737e7e3e536c5043adfc943e4b4600d19e36 Mon Sep 17 00:00:00 2001
From: Julian Oes <julian@oes.ch>
Date: Wed, 25 May 2016 21:50:23 +0200
Subject: [PATCH 0332/1230] sdlog2: fix Segfault on Eagle (#4638)

Since orb_exist doesn't work on the Snapdragon Linux side, we need to do
an additional orb_check after the orb_subscribe_multi, otherwise we copy
garbage.

The segfault was triggered by a count/length information about ESC
packets which lead to access outside the struct in the garbage case.
---
 src/modules/sdlog2/sdlog2.c | 9 +++++++--
 1 file changed, 7 insertions(+), 2 deletions(-)

diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c
index 1c07400fd8..db783325a5 100644
--- a/src/modules/sdlog2/sdlog2.c
+++ b/src/modules/sdlog2/sdlog2.c
@@ -917,8 +917,13 @@ bool copy_if_updated_multi(orb_id_t topic, int multi_instance, int *handle, void
 			*handle = orb_subscribe_multi(topic, multi_instance);
 			/* copy first data */
 			if (*handle >= 0) {
-				orb_copy(topic, *handle, buffer);
-				updated = true;
+
+				/* but only if it has really been updated */
+				orb_check(*handle, &updated);
+
+				if (updated) {
+					orb_copy(topic, *handle, buffer);
+				}
 			}
 		}
 	} else {

From 0ad0602560c58d496d861ca687ca462cdeff7755 Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Wed, 25 May 2016 22:20:53 +0200
Subject: [PATCH 0333/1230] Added missing timestamp to topic listener

---
 Tools/generate_listener.py | 1 +
 1 file changed, 1 insertion(+)

diff --git a/Tools/generate_listener.py b/Tools/generate_listener.py
index 4e428d3409..d3b4f27d0e 100755
--- a/Tools/generate_listener.py
+++ b/Tools/generate_listener.py
@@ -173,6 +173,7 @@ for index,m in enumerate(messages[1:]):
 	print("\t\t\ti++;")
 	print("\t\t\tprintf(\"\\nTOPIC: %s #%%d\\n\", i);" % m)
 	print("\t\t\torb_copy(ID,sub,&container);")
+	print("\t\t\tprintf(\"timestamp: %\" PRIu64 \"\\n\", container.timestamp);")
 	for item in message_elements[index+1]:
 		if item[0] == "float":
 			print("\t\t\tprintf(\"%s: %%8.4f\\n\",(double)container.%s);" % (item[1], item[1]))

From acdcb14d7969e954aaf754e8b538f56686737f77 Mon Sep 17 00:00:00 2001
From: David Sidrane <david_s5@nscdg.com>
Date: Wed, 25 May 2016 10:24:01 -1000
Subject: [PATCH 0334/1230] In support of merging Spread the checks multiline
 lists (#4626)

* Insupport merging Spread the checks multiline lists

* Removed long line of checks
---
 Makefile | 29 ++++++++++++++++++++++++++++-
 1 file changed, 28 insertions(+), 1 deletion(-)

diff --git a/Makefile b/Makefile
index 8b3295aed4..d97a18a1fd 100644
--- a/Makefile
+++ b/Makefile
@@ -241,7 +241,34 @@ ifeq ($(VECTORCONTROL),1)
 	@(rm -rf vectorcontrol && git clone --quiet --depth 1 https://github.com/thiemar/vectorcontrol.git && cd vectorcontrol && BOARD=s2740vc_1_0 make --silent --no-print-directory && BOARD=px4esc_1_6 make --silent --no-print-directory && ../Tools/uavcan_copy.sh)
 endif
 
-check: check_px4fmu-v1_default check_px4fmu-v2_default check_px4fmu-v2_test check_px4fmu-v2_lpe check_px4fmu-v2_ekf2 check_px4fmu-v4_default_and_uavcan check_mindpx-v2_default check_px4-stm32f4discovery_default check_posix_sitl_default check_posix_sitl_test check_unittest check_format
+checks_defaults: \
+	check_px4fmu-v1_default \
+	check_px4fmu-v2_default \
+	check_mindpx-v2_default \
+	check_px4-stm32f4discovery_default \
+
+checks_bootloaders: \
+
+
+checks_tests: \
+	check_px4fmu-v2_test
+
+checks_alts: \
+	check_px4fmu-v2_lpe \
+	check_px4fmu-v2_ekf2 \
+
+checks_uavcan: \
+	check_px4fmu-v4_default_and_uavcan
+
+checks_sitls: \
+	check_posix_sitl_default \
+	check_posix_sitl_test \
+
+checks_last: \
+	check_unittest \
+	check_format \
+
+check: checks_defaults checks_tests checks_alts checks_uavcan checks_bootloaders checks_sitls checks_last
 
 check_format:
 	$(call colorecho,"Checking formatting with astyle")

From e8aae9c7ab274581fdc33408a79f5fc6e21d2448 Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Thu, 26 May 2016 14:05:50 +0200
Subject: [PATCH 0335/1230] Update DriverFramework reference to point to DF
 master

---
 src/lib/DriverFramework | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/src/lib/DriverFramework b/src/lib/DriverFramework
index 8622af59d2..e4feef571a 160000
--- a/src/lib/DriverFramework
+++ b/src/lib/DriverFramework
@@ -1 +1 @@
-Subproject commit 8622af59d2126a5d8fd25726a1a76d35561e1979
+Subproject commit e4feef571af397196f8b22ce53edd91bfac968f7

From a25bbdd12dd14e75042e714c92a8b79966cf4032 Mon Sep 17 00:00:00 2001
From: Julian Oes <julian@oes.ch>
Date: Thu, 26 May 2016 19:57:20 +0200
Subject: [PATCH 0336/1230] DriverFramework: update submodule (#4648)

This fixes a bug where the accelsim and gyrosim were conflicting with
work handles which lead to a state where no gyro data was published.
---
 src/lib/DriverFramework | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/src/lib/DriverFramework b/src/lib/DriverFramework
index e4feef571a..46e6573cd7 160000
--- a/src/lib/DriverFramework
+++ b/src/lib/DriverFramework
@@ -1 +1 @@
-Subproject commit e4feef571af397196f8b22ce53edd91bfac968f7
+Subproject commit 46e6573cd7eb455881e06a4bf38752156c247475

From c08eec0a23fb2c715ed029a1fe1f06564a8fcf7a Mon Sep 17 00:00:00 2001
From: Pavel Kirienko <pavel.kirienko@gmail.com>
Date: Thu, 26 May 2016 21:00:10 +0300
Subject: [PATCH 0337/1230] Fixed stack overflow in UAVCAN process (#4643)

* Increased uavcan stack size; the old value of 1800 was insufficient

* Removed a misleading warning message from uavcan servers initialization
---
 src/modules/uavcan/uavcan_main.hpp    | 2 +-
 src/modules/uavcan/uavcan_servers.cpp | 1 -
 2 files changed, 1 insertion(+), 2 deletions(-)

diff --git a/src/modules/uavcan/uavcan_main.hpp b/src/modules/uavcan/uavcan_main.hpp
index dd508b1abc..dbb74b39af 100644
--- a/src/modules/uavcan/uavcan_main.hpp
+++ b/src/modules/uavcan/uavcan_main.hpp
@@ -99,7 +99,7 @@ class UavcanNode : public device::CDev
 	 */
 
 	static constexpr unsigned RxQueueLenPerIface	= FramePerMSecond * PollTimeoutMs; // At
-	static constexpr unsigned StackSize		= 1800;
+	static constexpr unsigned StackSize		= 2400;
 
 public:
 	typedef uavcan_stm32::CanInitHelper<RxQueueLenPerIface> CanInitHelper;
diff --git a/src/modules/uavcan/uavcan_servers.cpp b/src/modules/uavcan/uavcan_servers.cpp
index 5ed3e83ee6..7e372a1d91 100644
--- a/src/modules/uavcan/uavcan_servers.cpp
+++ b/src/modules/uavcan/uavcan_servers.cpp
@@ -923,7 +923,6 @@ void UavcanServers::unpackFwFromROMFS(const char* sd_path, const char* romfs_pat
 
 	DIR* const romfs_dir = opendir(romfs_path);
 	if (!romfs_dir) {
-		warnx("base: couldn't open %s", romfs_path);
 		return;
 	}
 

From d4262bce2a192e8c938b3c1738f7980f5e428c25 Mon Sep 17 00:00:00 2001
From: Paul Riseborough <priseborough@users.noreply.github.com>
Date: Fri, 27 May 2016 16:36:37 +1000
Subject: [PATCH 0338/1230] EKF2 output predictor update (#4606)

* ekf2: Update tuning parameter documentation

* ecl: update submodule reference

Enables selection of a new output predictor method
---
 src/lib/ecl                    | 2 +-
 src/modules/ekf2/ekf2_params.c | 5 ++---
 2 files changed, 3 insertions(+), 4 deletions(-)

diff --git a/src/lib/ecl b/src/lib/ecl
index e3b9800cac..e2d9e19a5d 160000
--- a/src/lib/ecl
+++ b/src/lib/ecl
@@ -1 +1 @@
-Subproject commit e3b9800cac459e7c279395c18191a872f465f848
+Subproject commit e2d9e19a5dde27e8db0d8f838ddd614bfd4b0f1b
diff --git a/src/modules/ekf2/ekf2_params.c b/src/modules/ekf2/ekf2_params.c
index 4837d7df89..1c09ec2320 100644
--- a/src/modules/ekf2/ekf2_params.c
+++ b/src/modules/ekf2/ekf2_params.c
@@ -701,10 +701,9 @@ PARAM_DEFINE_FLOAT(EKF2_OF_POS_Y, 0.0f);
 PARAM_DEFINE_FLOAT(EKF2_OF_POS_Z, 0.0f);
 
 /**
- * Time constant of the velocity output prediction and smoothing filter
+ * Time constant of the velocity output prediction and smoothing filter. Controls how tightly the velocity output tracks the EKF states. Set to a negative number to disable the EKF velocity state tracking. This will cause the output velocity to track the output position time derivative.
  *
  * @group EKF2
- * @min 0.1
  * @max 1.0
  * @unit s
  * @decimal 2
@@ -712,7 +711,7 @@ PARAM_DEFINE_FLOAT(EKF2_OF_POS_Z, 0.0f);
 PARAM_DEFINE_FLOAT(EKF2_TAU_VEL, 0.5f);
 
 /**
- * Time constant of the position output prediction and smoothing filter
+ * Time constant of the position output prediction and smoothing filter. Controls how tightly the output track the EKF states.
  *
  * @group EKF2
  * @min 0.1

From 224fbbc26b046e2666f330e2731a1dfe8403e0c4 Mon Sep 17 00:00:00 2001
From: Julian Oes <julian@oes.ch>
Date: Fri, 27 May 2016 17:18:51 +0200
Subject: [PATCH 0339/1230] land_detector: fix uninitialized value (#4659)

---
 src/modules/land_detector/MulticopterLandDetector.cpp | 3 ++-
 1 file changed, 2 insertions(+), 1 deletion(-)

diff --git a/src/modules/land_detector/MulticopterLandDetector.cpp b/src/modules/land_detector/MulticopterLandDetector.cpp
index 50720e430d..a87c3a6658 100644
--- a/src/modules/land_detector/MulticopterLandDetector.cpp
+++ b/src/modules/land_detector/MulticopterLandDetector.cpp
@@ -67,7 +67,8 @@ MulticopterLandDetector::MulticopterLandDetector() : LandDetector(),
 	_ctrl_state{},
 	_ctrl_mode{},
 	_landTimer(0),
-	_freefallTimer(0)
+	_freefallTimer(0),
+	_min_trust_start(0)
 {
 	_paramHandle.maxRotation = param_find("LNDMC_ROT_MAX");
 	_paramHandle.maxVelocity = param_find("LNDMC_XY_VEL_MAX");

From 45bb1786b63d32f85bcc905419f64dcc5abeb556 Mon Sep 17 00:00:00 2001
From: Julian Oes <julian@oes.ch>
Date: Fri, 27 May 2016 18:52:38 +0200
Subject: [PATCH 0340/1230] Fix SITL CPU (#4657)

* accelsim: add debug output like in gyrosim

* DriverFramework: update submodule

This brings lower CPU usage because of scheduling in us instead of ms.
---
 src/lib/DriverFramework                          |  2 +-
 .../posix/drivers/accelsim/accelsim.cpp          | 16 ++++++++++++++--
 2 files changed, 15 insertions(+), 3 deletions(-)

diff --git a/src/lib/DriverFramework b/src/lib/DriverFramework
index 46e6573cd7..5e055c7d11 160000
--- a/src/lib/DriverFramework
+++ b/src/lib/DriverFramework
@@ -1 +1 @@
-Subproject commit 46e6573cd7eb455881e06a4bf38752156c247475
+Subproject commit 5e055c7d11ec828f59716478c3c5d32c8c1809f0
diff --git a/src/platforms/posix/drivers/accelsim/accelsim.cpp b/src/platforms/posix/drivers/accelsim/accelsim.cpp
index 664ea8ea8c..481eb57788 100644
--- a/src/platforms/posix/drivers/accelsim/accelsim.cpp
+++ b/src/platforms/posix/drivers/accelsim/accelsim.cpp
@@ -802,9 +802,21 @@ ACCELSIM::stop()
 void
 ACCELSIM::_measure()
 {
-	//PX4_INFO("ACCELSIM::_measure");
-	/* status register and data as read back from the device */
+#if 0
+	static int x = 0;
 
+	// Verify the samples are being taken at the expected rate
+	if (x == 99) {
+		x = 0;
+		PX4_INFO("ACCELSIM::measure %" PRIu64, hrt_absolute_time());
+
+	} else {
+		x++;
+	}
+
+#endif
+
+	/* status register and data as read back from the device */
 #pragma pack(push, 1)
 	struct {
 		uint8_t		cmd;

From d34edfd435d81576766a9867f3a966e4b3dc9d08 Mon Sep 17 00:00:00 2001
From: Andreas Antener <antener_a@gmx.ch>
Date: Wed, 25 May 2016 22:28:13 +0200
Subject: [PATCH 0341/1230] changed mavros connection string, updated gazebo
 sitl build process

---
 integrationtests/run_tests.bash        | 6 +-----
 integrationtests/setup_gazebo_ros.bash | 2 ++
 launch/mavros_posix_sitl.launch        | 3 ++-
 launch/posix_sitl.launch               | 2 +-
 4 files changed, 6 insertions(+), 7 deletions(-)

diff --git a/integrationtests/run_tests.bash b/integrationtests/run_tests.bash
index 6707af90aa..6894252a56 100755
--- a/integrationtests/run_tests.bash
+++ b/integrationtests/run_tests.bash
@@ -43,11 +43,7 @@ ln -s ${SRC_DIR} /root/Firmware
 echo "=====> compile ($SRC_DIR)"
 cd $SRC_DIR
 make ${BUILD}
-mkdir -p Tools/sitl_gazebo/Build
-cd Tools/sitl_gazebo/Build
-cmake -Wno-dev ..
-make -j4
-make sdf
+make --no-print-directory gazebo_build
 echo "<====="
 
 # don't exit on error anymore from here on (because single tests or exports might fail)
diff --git a/integrationtests/setup_gazebo_ros.bash b/integrationtests/setup_gazebo_ros.bash
index 7655fec9a7..f02943eb3d 100755
--- a/integrationtests/setup_gazebo_ros.bash
+++ b/integrationtests/setup_gazebo_ros.bash
@@ -18,3 +18,5 @@ export GAZEBO_MODEL_PATH=${GAZEBO_MODEL_PATH}:${SRC_DIR}/Tools/sitl_gazebo/model
 export GAZEBO_PLUGIN_PATH=${SRC_DIR}/Tools/sitl_gazebo/Build/:${GAZEBO_PLUGIN_PATH}
 export LD_LIBRARY_PATH=${LD_LIBRARY_PATH}:${SRC_DIR}/Tools/sitl_gazebo/Build/msgs/
 export ROS_PACKAGE_PATH=${ROS_PACKAGE_PATH}:${SRC_DIR}
+export GAZEBO_MODEL_DATABASE_URI=""
+export SITL_GAZEBO_PATH=$SRC_DIR/Tools/sitl_gazebo
diff --git a/launch/mavros_posix_sitl.launch b/launch/mavros_posix_sitl.launch
index 905e0a2ff1..32a21fcf3b 100644
--- a/launch/mavros_posix_sitl.launch
+++ b/launch/mavros_posix_sitl.launch
@@ -6,6 +6,7 @@
     <arg name="ns" default="/"/>
     <arg name="world" default="iris"/>
     <arg name="build" default="posix_sitl_default"/>
+    <arg name="fcu_url" default="udp://:14540@localhost:14557"/>
 
     <include file="$(find px4)/launch/posix_sitl.launch">
         <arg name="headless" value="$(arg headless)"/>
@@ -17,6 +18,6 @@
 
     <include file="$(find px4)/launch/mavros.launch">
         <arg name="ns" value="$(arg ns)"/>
-        <arg name="fcu_url" value="udp://:14567@localhost:14557"/>
+        <arg name="fcu_url" value="$(arg fcu_url)"/>
     </include>
 </launch>
diff --git a/launch/posix_sitl.launch b/launch/posix_sitl.launch
index 4e3b71f70c..9895ee9d43 100644
--- a/launch/posix_sitl.launch
+++ b/launch/posix_sitl.launch
@@ -7,7 +7,7 @@
     <arg name="world" default="iris"/>
     <arg name="build" default="posix_sitl_default"/>
 
-    <node pkg="px4" type="sitl_run.sh" name="simulator" args="posix-configs/SITL/init/rcS none gazebo iris $(find px4)/build_$(arg build)">
+    <node pkg="px4" type="sitl_run.sh" name="simulator" args="posix-configs/SITL/init/rcS none gazebo $(arg world) $(find px4)/build_$(arg build)">
         <env name="no_sim" value="1" />
     </node>
 

From a2d78eaa50d041b4a22d4007ac74137e18b4b3e9 Mon Sep 17 00:00:00 2001
From: Andreas Antener <antener_a@gmx.ch>
Date: Fri, 27 May 2016 18:39:57 +0200
Subject: [PATCH 0342/1230] use new build path for gazebo - sim connection

---
 integrationtests/setup_gazebo_ros.bash | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/integrationtests/setup_gazebo_ros.bash b/integrationtests/setup_gazebo_ros.bash
index f02943eb3d..6c22bbe192 100755
--- a/integrationtests/setup_gazebo_ros.bash
+++ b/integrationtests/setup_gazebo_ros.bash
@@ -15,7 +15,7 @@ SRC_DIR=$1
 
 # setup Gazebo env and update package path
 export GAZEBO_MODEL_PATH=${GAZEBO_MODEL_PATH}:${SRC_DIR}/Tools/sitl_gazebo/models
-export GAZEBO_PLUGIN_PATH=${SRC_DIR}/Tools/sitl_gazebo/Build/:${GAZEBO_PLUGIN_PATH}
+export GAZEBO_PLUGIN_PATH=${SRC_DIR}/build_gazebo:${GAZEBO_PLUGIN_PATH}
 export LD_LIBRARY_PATH=${LD_LIBRARY_PATH}:${SRC_DIR}/Tools/sitl_gazebo/Build/msgs/
 export ROS_PACKAGE_PATH=${ROS_PACKAGE_PATH}:${SRC_DIR}
 export GAZEBO_MODEL_DATABASE_URI=""

From 26feb018d904c6062d9d5e7e0522e55a25cce8c4 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Beat=20K=C3=BCng?= <beat-kueng@gmx.net>
Date: Thu, 26 May 2016 07:37:55 +0200
Subject: [PATCH 0343/1230] getprogname on posix: fix locking (mutex was not
 unlocked in some cases)

---
 src/platforms/posix/px4_layer/px4_posix_tasks.cpp | 11 +++++++----
 1 file changed, 7 insertions(+), 4 deletions(-)

diff --git a/src/platforms/posix/px4_layer/px4_posix_tasks.cpp b/src/platforms/posix/px4_layer/px4_posix_tasks.cpp
index 9b3730c7b9..ffcd74f5e1 100644
--- a/src/platforms/posix/px4_layer/px4_posix_tasks.cpp
+++ b/src/platforms/posix/px4_layer/px4_posix_tasks.cpp
@@ -391,16 +391,19 @@ unsigned long px4_getpid()
 const char *getprogname()
 {
 	pthread_t pid = pthread_self();
+	const char *prog_name = "UnknownApp";
+
+	pthread_mutex_lock(&task_mutex);
 
 	for (int i = 0; i < PX4_MAX_TASKS; i++) {
 		if (taskmap[i].isused && taskmap[i].pid == pid) {
-			pthread_mutex_lock(&task_mutex);
-			return taskmap[i].name.c_str();
-			pthread_mutex_unlock(&task_mutex);
+			prog_name = taskmap[i].name.c_str();
 		}
 	}
 
-	return "Unknown App";
+	pthread_mutex_unlock(&task_mutex);
+
+	return prog_name;
 }
 
 int px4_prctl(int option, const char *arg2, unsigned pid)

From f397d40f09c7a6ab54d0df1b4513ea668a836e4f Mon Sep 17 00:00:00 2001
From: Jimmy Johnson <catch22@fastmail.net>
Date: Wed, 25 May 2016 23:06:45 -0700
Subject: [PATCH 0344/1230] follow target updates

---
 src/modules/navigator/follow_target.cpp | 137 ++++++++++--------------
 src/modules/navigator/follow_target.h   |   1 +
 2 files changed, 58 insertions(+), 80 deletions(-)

diff --git a/src/modules/navigator/follow_target.cpp b/src/modules/navigator/follow_target.cpp
index 974b463e9b..0af04b7f88 100644
--- a/src/modules/navigator/follow_target.cpp
+++ b/src/modules/navigator/follow_target.cpp
@@ -53,6 +53,7 @@
 #include <uORB/topics/follow_target.h>
 #include <lib/geo/geo.h>
 #include <lib/mathlib/math/Limits.hpp>
+#include <lib/mathlib/math/Quaternion.hpp>
 
 #include "navigator.h"
 
@@ -74,8 +75,6 @@ FollowTarget::FollowTarget(Navigator *navigator, const char *name) :
 	_current_target_motion({}),
 	_previous_target_motion({}),
 	_avg_cos_ratio(0.0F),
-	_filtered_target_lat(0.0F),
-	_filtered_target_lon(0.0F),
 	_yaw_rate(0.0F),
 	_responsiveness(0.0F),
 	_yaw_auto_max(0.0F),
@@ -88,6 +87,7 @@ FollowTarget::FollowTarget(Navigator *navigator, const char *name) :
 	_target_distance.zero();
 	_target_position_offset.zero();
 	_target_position_delta.zero();
+	_rot_vector.zero();
 }
 
 FollowTarget::~FollowTarget()
@@ -126,7 +126,7 @@ void FollowTarget::on_active()
 {
 	struct map_projection_reference_s target_ref;
 	math::Vector<3> target_reported_velocity(0, 0, 0);
-	follow_target_s target_motion = {};
+	follow_target_s target_motion_with_offset = {};
 	uint64_t current_time = hrt_absolute_time();
 	bool _radius_entered = false;
 	bool _radius_exited = false;
@@ -136,6 +136,7 @@ void FollowTarget::on_active()
 	orb_check(_follow_target_sub, &updated);
 
 	if (updated) {
+		follow_target_s target_motion;
 
 		_target_updates++;
 
@@ -143,7 +144,15 @@ void FollowTarget::on_active()
 
 		_previous_target_motion = _current_target_motion;
 
-		orb_copy(ORB_ID(follow_target), _follow_target_sub, &_current_target_motion);
+		orb_copy(ORB_ID(follow_target), _follow_target_sub, &target_motion);
+
+		if(_current_target_motion.timestamp == 0) {
+			_current_target_motion = target_motion;
+		}
+
+		_current_target_motion.timestamp = target_motion.timestamp;
+		_current_target_motion.lat = (_current_target_motion.lat*(double)_responsiveness) + target_motion.lat*(double)(1 - _responsiveness);
+		_current_target_motion.lon = (_current_target_motion.lon*(double)_responsiveness) + target_motion.lon*(double)(1 - _responsiveness);
 
 		target_reported_velocity(0) = _current_target_motion.vx;
 		target_reported_velocity(1) = _current_target_motion.vy;
@@ -159,14 +168,7 @@ void FollowTarget::on_active()
 		// get distance to target
 
 		map_projection_init(&target_ref, _navigator->get_global_position()->lat, _navigator->get_global_position()->lon);
-		map_projection_project(&target_ref, _filtered_target_lat, _filtered_target_lon, &_target_distance(0), &_target_distance(1));
-
-		target_motion = _current_target_motion;
-
-		// use target offset
-
-		map_projection_init(&target_ref,  _filtered_target_lat, _filtered_target_lon);
-		map_projection_reproject(&target_ref, _target_position_offset(0), _target_position_offset(1), &target_motion.lat, &target_motion.lon);
+		map_projection_project(&target_ref, _current_target_motion.lat, _current_target_motion.lon, &_target_distance(0), &_target_distance(1));
 
 	}
 
@@ -190,39 +192,15 @@ void FollowTarget::on_active()
 
 			map_projection_project(&target_ref, _current_target_motion.lat, _current_target_motion.lon, &(_target_position_delta(0)), &(_target_position_delta(1)));
 
-			// filter out gps noise to figure out if we are actually moving
-
-			if (_target_position_delta.length() > 0.1F && prev_position_delta.length() > 0.1F) {
-
-				float cos_ratio = (_target_position_delta * prev_position_delta)/(_target_position_delta.length() * prev_position_delta.length());
-
-				_avg_cos_ratio = _responsiveness*_avg_cos_ratio + (1 - _responsiveness) * cos_ratio;
-
-				if(_avg_cos_ratio < 0) {
-					_avg_cos_ratio = 0.0F;
-				}
-
-				if (_avg_cos_ratio > 0.0F) {
-					_filtered_target_position_delta = _target_position_delta*_avg_cos_ratio + _filtered_target_position_delta*(1.0F - _avg_cos_ratio);
-				}
-
-				// if ratio is high enough, track target from a side
-
-				if(_avg_cos_ratio  > .70F) {
-					_target_position_offset = _rot_matrix * (_filtered_target_position_delta.normalized() * _follow_offset);
-				}
-
-			} else  {
-				_filtered_target_position_delta.zero();
-				_avg_cos_ratio = 0.0F;
-			}
-
 			// update the average velocity of the target based on the position
 
-			_est_target_vel = _filtered_target_position_delta / (dt_ms / 1000.0f);
+			_est_target_vel = _target_position_delta / (dt_ms / 1000.0f);
 
-			_filtered_target_lat = (_current_target_motion.lat*(double)_avg_cos_ratio) + _filtered_target_lat*(double)(1 - _avg_cos_ratio);
-			_filtered_target_lon = (_current_target_motion.lon*(double)_avg_cos_ratio) + _filtered_target_lon*(double)(1 - _avg_cos_ratio);
+			// if the target is moving add an offset and rotation
+
+			if(_est_target_vel.length() > .5F) {
+				_target_position_offset = _rot_matrix*_est_target_vel.normalized()*_follow_offset;
+			}
 
 			// are we within the target acceptance radius?
 			// give a buffer to exit/enter the radius to give the velocity controller
@@ -243,55 +221,60 @@ void FollowTarget::on_active()
 			_step_vel /= (dt_ms / 1000.0F * (float) INTERPOLATION_PNTS);
 			_step_time_in_ms = (dt_ms / (float) INTERPOLATION_PNTS);
 
-			// if we are less than 3 meters from the target don't worry about trying to yaw
-			// lock the yaw until we are a distance that makes sense
+			// if we are less than 1 meter from the target don't worry about trying to yaw
+			// lock the yaw until we are at a distance that makes sense
 
-			if((_target_distance).length() > 3.0F) {
+			if((_target_distance).length() > 1.0F) {
 
-				// yaw smoothing
+				// yaw rate smoothing
 
 				// this really needs to control the yaw rate directly in the attitude pid controller
-				// but seems to work ok for now since that cannot be controlled directly in auto mode
-				// right now
+				// but seems to work ok for now since the yaw rate cannot be controlled directly in auto mode
 
 				_yaw_angle = get_bearing_to_next_waypoint(_navigator->get_global_position()->lat,
-						_navigator->get_global_position()->lon,
-						_current_target_motion.lat,
-						_current_target_motion.lon);
+														  _navigator->get_global_position()->lon,
+														 _current_target_motion.lat,
+														 _current_target_motion.lon);
 
 				_yaw_rate = (_yaw_angle - _navigator->get_global_position()->yaw) / (dt_ms / 1000.0F);
 
 				_yaw_rate = _wrap_pi(_yaw_rate);
 
-				_yaw_rate = math::constrain(_yaw_rate, -1.0F*_yaw_auto_max, _yaw_auto_max);//*.80F;
+				_yaw_rate = math::constrain(_yaw_rate, -1.0F*_yaw_auto_max, _yaw_auto_max);
 
 			} else {
 				_yaw_angle = _yaw_rate = NAN;
 			}
 		}
 
-//		warnx(" _step_vel x %3.6f y %3.6f cur vel %3.6f %3.6f tar vel %3.6f %3.6f dist = %3.6f (%3.6f) mode = %d con ratio = %3.6f yaw rate = %3.6f",
-//				(double) _step_vel(0),
-//				(double) _step_vel(1),
-//				(double) _current_vel(0),
-//				(double) _current_vel(1),
-//				(double) _est_target_vel(0),
-//				(double) _est_target_vel(1),
-//				(double) (_target_distance).length(),
-//				(double) (_target_position_offset + _target_distance).length(),
-//				_follow_target_state,
-//				(double)_avg_cos_ratio, (double) _yaw_rate);
+		warnx(" _step_vel x %3.6f y %3.6f cur vel %3.6f %3.6f tar vel %3.6f %3.6f dist = %3.6f (%3.6f) mode = %d con ratio = %3.6f yaw rate = %3.6f",
+				(double) _step_vel(0),
+				(double) _step_vel(1),
+				(double) _current_vel(0),
+				(double) _current_vel(1),
+				(double) _est_target_vel(0),
+				(double) _est_target_vel(1),
+				(double) (_target_distance).length(),
+				(double) (_target_position_offset + _target_distance).length(),
+				_follow_target_state,
+				(double)_avg_cos_ratio, (double) _yaw_rate);
 	}
 
-	// prevent yaw rate smoothing from over shooting target
-	// uses modulus of two pi to get diff
-	// by converting float to int
+	if(target_position_valid()) {
 
-	int angle_diff = (int) ((fabsf(_yaw_angle) - fabsf(_navigator->get_global_position()->yaw)) * 1e4F);
-	float mod_diff = ((float)(angle_diff % ((int) (M_PI_F * 2.0F * 1e4F))))/1e4F;
+		// get the target position using the calculated offset
 
-	if (fabsf(mod_diff) < math::radians(5.0F)) {
-		_yaw_angle = _yaw_rate = NAN;
+		map_projection_init(&target_ref,  _current_target_motion.lat, _current_target_motion.lon);
+		map_projection_reproject(&target_ref, _target_position_offset(0), _target_position_offset(1), &target_motion_with_offset.lat, &target_motion_with_offset.lon);
+	}
+
+	// clamp yaw rate smoothing if we are with in
+	// 3 degrees of facing target
+
+	if (PX4_ISFINITE(_yaw_rate)) {
+		if (fabsf(fabsf(_yaw_angle) - fabsf(_navigator->get_global_position()->yaw)) < math::radians(3.0F)) {
+			_yaw_rate = NAN;
+		}
 	}
 
 	// update state machine
@@ -303,12 +286,11 @@ void FollowTarget::on_active()
 			if (_radius_entered == true) {
 				_follow_target_state = TRACK_VELOCITY;
 			} else if (target_velocity_valid()) {
-				set_follow_target_item(&_mission_item, _param_min_alt.get(), target_motion, _yaw_angle);
+				set_follow_target_item(&_mission_item, _param_min_alt.get(), target_motion_with_offset, _yaw_angle);
 				// keep the current velocity updated with the target velocity for when it's needed
 				_current_vel = _est_target_vel;
-				_filtered_target_lat = _current_target_motion.lat;
-				_filtered_target_lon = _current_target_motion.lon;
-				update_position_sp(true, true, NAN);
+
+				update_position_sp(true, true, _yaw_rate);
 			} else {
 				_follow_target_state = SET_WAIT_FOR_TARGET_POSITION;
 			}
@@ -327,7 +309,7 @@ void FollowTarget::on_active()
 					_last_update_time = current_time;
 				}
 
-				set_follow_target_item(&_mission_item, _param_min_alt.get(), target_motion, _yaw_angle);
+				set_follow_target_item(&_mission_item, _param_min_alt.get(), target_motion_with_offset, _yaw_angle);
 
 				update_position_sp(true, false, _yaw_rate);
 			} else {
@@ -357,11 +339,6 @@ void FollowTarget::on_active()
 	}
 	case WAIT_FOR_TARGET_POSITION: {
 
-		if(target_position_valid()) {
-			_filtered_target_lat = _current_target_motion.lat;
-			_filtered_target_lon = _current_target_motion.lon;
-		}
-
 		if (is_mission_item_reached() && target_velocity_valid()) {
 			_target_position_offset(0) = _follow_offset;
 			_follow_target_state = TRACK_POSITION;
diff --git a/src/modules/navigator/follow_target.h b/src/modules/navigator/follow_target.h
index b54e2d5f30..9f467f28c0 100644
--- a/src/modules/navigator/follow_target.h
+++ b/src/modules/navigator/follow_target.h
@@ -121,6 +121,7 @@ private:
 	math::Vector<3> _est_target_vel;
 	math::Vector<3> _target_distance;
 	math::Vector<3> _target_position_offset;
+	math::Vector<3> _rot_vector;
 	math::Vector<3> _target_position_delta;
 	math::Vector<3> _filtered_target_position_delta;
 

From 38acd15ec6d34dc5a39d1ae87efa45cd6e296d8f Mon Sep 17 00:00:00 2001
From: Jimmy Johnson <catch22@fastmail.net>
Date: Thu, 26 May 2016 15:35:03 -0700
Subject: [PATCH 0345/1230] more clean up

---
 src/modules/navigator/follow_target.cpp | 26 +++++++++++--------------
 src/modules/navigator/follow_target.h   |  4 ----
 2 files changed, 11 insertions(+), 19 deletions(-)

diff --git a/src/modules/navigator/follow_target.cpp b/src/modules/navigator/follow_target.cpp
index 0af04b7f88..0c80f9bc99 100644
--- a/src/modules/navigator/follow_target.cpp
+++ b/src/modules/navigator/follow_target.cpp
@@ -53,7 +53,6 @@
 #include <uORB/topics/follow_target.h>
 #include <lib/geo/geo.h>
 #include <lib/mathlib/math/Limits.hpp>
-#include <lib/mathlib/math/Quaternion.hpp>
 
 #include "navigator.h"
 
@@ -74,7 +73,6 @@ FollowTarget::FollowTarget(Navigator *navigator, const char *name) :
 	_last_update_time(0),
 	_current_target_motion({}),
 	_previous_target_motion({}),
-	_avg_cos_ratio(0.0F),
 	_yaw_rate(0.0F),
 	_responsiveness(0.0F),
 	_yaw_auto_max(0.0F),
@@ -87,7 +85,6 @@ FollowTarget::FollowTarget(Navigator *navigator, const char *name) :
 	_target_distance.zero();
 	_target_position_offset.zero();
 	_target_position_delta.zero();
-	_rot_vector.zero();
 }
 
 FollowTarget::~FollowTarget()
@@ -247,17 +244,17 @@ void FollowTarget::on_active()
 			}
 		}
 
-		warnx(" _step_vel x %3.6f y %3.6f cur vel %3.6f %3.6f tar vel %3.6f %3.6f dist = %3.6f (%3.6f) mode = %d con ratio = %3.6f yaw rate = %3.6f",
-				(double) _step_vel(0),
-				(double) _step_vel(1),
-				(double) _current_vel(0),
-				(double) _current_vel(1),
-				(double) _est_target_vel(0),
-				(double) _est_target_vel(1),
-				(double) (_target_distance).length(),
-				(double) (_target_position_offset + _target_distance).length(),
-				_follow_target_state,
-				(double)_avg_cos_ratio, (double) _yaw_rate);
+//		warnx(" _step_vel x %3.6f y %3.6f cur vel %3.6f %3.6f tar vel %3.6f %3.6f dist = %3.6f (%3.6f) mode = %d con ratio = %3.6f yaw rate = %3.6f",
+//				(double) _step_vel(0),
+//				(double) _step_vel(1),
+//				(double) _current_vel(0),
+//				(double) _current_vel(1),
+//				(double) _est_target_vel(0),
+//				(double) _est_target_vel(1),
+//				(double) (_target_distance).length(),
+//				(double) (_target_position_offset + _target_distance).length(),
+//				_follow_target_state,
+//				(double)_avg_cos_ratio, (double) _yaw_rate);
 	}
 
 	if(target_position_valid()) {
@@ -374,7 +371,6 @@ void FollowTarget::update_position_sp(bool use_velocity, bool use_position, floa
 void FollowTarget::reset_target_validity()
 {
 	_yaw_rate = NAN;
-	_avg_cos_ratio = 0.0F;
 	_previous_target_motion = {};
 	_current_target_motion = {};
 	_target_updates = 0;
diff --git a/src/modules/navigator/follow_target.h b/src/modules/navigator/follow_target.h
index 9f467f28c0..52fa108560 100644
--- a/src/modules/navigator/follow_target.h
+++ b/src/modules/navigator/follow_target.h
@@ -121,15 +121,11 @@ private:
 	math::Vector<3> _est_target_vel;
 	math::Vector<3> _target_distance;
 	math::Vector<3> _target_position_offset;
-	math::Vector<3> _rot_vector;
 	math::Vector<3> _target_position_delta;
 	math::Vector<3> _filtered_target_position_delta;
 
 	follow_target_s _current_target_motion;
 	follow_target_s _previous_target_motion;
-	float _avg_cos_ratio;
-	double _filtered_target_lat;
-	double _filtered_target_lon;
 	float _yaw_rate;
 	float _responsiveness;
 	float _yaw_auto_max;

From 24622131c9e764bf453d7413c409958aa03cb39b Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Fri, 27 May 2016 23:11:51 +0200
Subject: [PATCH 0346/1230] Sensors: Reinstate boot stack

---
 src/modules/sensors/CMakeLists.txt | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/src/modules/sensors/CMakeLists.txt b/src/modules/sensors/CMakeLists.txt
index b373b18173..e26df79246 100644
--- a/src/modules/sensors/CMakeLists.txt
+++ b/src/modules/sensors/CMakeLists.txt
@@ -35,7 +35,7 @@ px4_add_module(
 	MODULE modules__sensors
 	MAIN sensors
 	PRIORITY "SCHED_PRIORITY_MAX-5"
-	STACK_MAIN 1200
+	STACK_MAIN 1300
 	COMPILE_FLAGS
 		-O3
 	SRCS

From 0dc36d149ef3a1ece2b59b6abad6fbfcee47ed09 Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Fri, 27 May 2016 23:12:07 +0200
Subject: [PATCH 0347/1230] Sensors: Reinstate main stack

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

diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp
index a84552c93b..cbc3e062ac 100644
--- a/src/modules/sensors/sensors.cpp
+++ b/src/modules/sensors/sensors.cpp
@@ -2276,7 +2276,7 @@ Sensors::start()
 	_sensors_task = px4_task_spawn_cmd("sensors",
 					   SCHED_DEFAULT,
 					   SCHED_PRIORITY_MAX - 5,
-					   2200,
+					   2000,
 					   (px4_main_t)&Sensors::task_main_trampoline,
 					   nullptr);
 

From 7be5ae9b93c526605f87064b1377aa6f24ed36b2 Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Fri, 27 May 2016 23:12:51 +0200
Subject: [PATCH 0348/1230] Version: Report in common MAVLink format

---
 src/systemcmds/ver/ver.c | 21 +++++++++++++++------
 1 file changed, 15 insertions(+), 6 deletions(-)

diff --git a/src/systemcmds/ver/ver.c b/src/systemcmds/ver/ver.c
index d1046ad76d..14425afe61 100644
--- a/src/systemcmds/ver/ver.c
+++ b/src/systemcmds/ver/ver.c
@@ -77,20 +77,20 @@ uint32_t version_tag_to_number(const char *tag)
 {
 	uint32_t ver = 0;
 	unsigned len = strlen(tag);
-	unsigned mag = 1;
+	unsigned mag = 0;
 	bool dotparsed = false;
 
 	for (int i = len - 1; i >= 0; i--) {
 		if (tag[i] >= '0' && tag[i] <= '9') {
 			unsigned number = tag[i] - '0';
 
-			ver += number * mag;
-			mag *= 10;
+			ver += (number << mag);
+			mag += 8;
 
 		} else if (tag[i] == '.') {
 			continue;
 
-		} else if (ver > 100 && dotparsed) {
+		} else if (mag > 2 * 8 && dotparsed) {
 			/* this is a full version and we have enough digits */
 			return ver;
 
@@ -99,10 +99,13 @@ uint32_t version_tag_to_number(const char *tag)
 			 * are seeing non-numeric characters again
 			 */
 			ver = 0;
-			mag = 1;
+			mag = 0;
 		}
 	}
 
+	// XXX not reporting patch version yet
+	ver = (ver << 8);
+
 	return ver;
 }
 
@@ -154,7 +157,13 @@ int ver_main(int argc, char *argv[])
 
 			if (show_all || !strncmp(argv[1], sz_ver_git_str, sizeof(sz_ver_git_str))) {
 				printf("FW git-hash: %s\n", px4_git_version);
-				printf("FW version: %s (%u)\n", px4_git_tag, version_tag_to_number(px4_git_tag));
+				unsigned fwver = version_tag_to_number(px4_git_tag);
+				unsigned major = (fwver >> (8 * 3)) & 0xFF;
+				unsigned minor = (fwver >> (8 * 2)) & 0xFF;
+				unsigned patch = (fwver >> (8 * 1)) & 0xFF;
+				unsigned type = (fwver >> (8 * 0)) & 0xFF;
+				printf("FW version: %s (%u.%u.%u %s)\n", px4_git_tag, major, minor, patch,
+					(type == 0) ? "stable" : "beta");
 				/* middleware is currently the same thing as firmware, so not printing yet */
 				printf("OS version: %s (%u)\n", os_git_tag, version_tag_to_number(os_git_tag));
 				ret = 0;

From 6fa446b465bf06bca0eeeb543856c89293fa298b Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Sat, 28 May 2016 00:10:02 +0200
Subject: [PATCH 0349/1230] Format fix

---
 src/systemcmds/ver/ver.c | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/src/systemcmds/ver/ver.c b/src/systemcmds/ver/ver.c
index 14425afe61..df041f018f 100644
--- a/src/systemcmds/ver/ver.c
+++ b/src/systemcmds/ver/ver.c
@@ -163,7 +163,7 @@ int ver_main(int argc, char *argv[])
 				unsigned patch = (fwver >> (8 * 1)) & 0xFF;
 				unsigned type = (fwver >> (8 * 0)) & 0xFF;
 				printf("FW version: %s (%u.%u.%u %s)\n", px4_git_tag, major, minor, patch,
-					(type == 0) ? "stable" : "beta");
+				       (type == 0) ? "dev" : "stable");
 				/* middleware is currently the same thing as firmware, so not printing yet */
 				printf("OS version: %s (%u)\n", os_git_tag, version_tag_to_number(os_git_tag));
 				ret = 0;

From 7a0d43586f7e11b669fb976b172b4d6761ec2611 Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lorenz@px4.io>
Date: Sat, 28 May 2016 09:57:07 +0200
Subject: [PATCH 0350/1230] MAVLink app: Only start transmitting when boot is
 complete (#4666)

---
 src/modules/mavlink/mavlink_main.cpp | 1 -
 src/modules/mavlink/mavlink_main.h   | 2 +-
 2 files changed, 1 insertion(+), 2 deletions(-)

diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp
index 417975faab..42b7ba00e6 100644
--- a/src/modules/mavlink/mavlink_main.cpp
+++ b/src/modules/mavlink/mavlink_main.cpp
@@ -912,7 +912,6 @@ Mavlink::send_bytes(const uint8_t *buf, unsigned packet_len)
 	/* If the wait until transmit flag is on, only transmit after we've received messages.
 	   Otherwise, transmit all the time. */
 	if (!should_transmit()) {
-		warnx("not transmitting");
 		return;
 	}
 
diff --git a/src/modules/mavlink/mavlink_main.h b/src/modules/mavlink/mavlink_main.h
index 6026acf793..2687cc2059 100644
--- a/src/modules/mavlink/mavlink_main.h
+++ b/src/modules/mavlink/mavlink_main.h
@@ -327,7 +327,7 @@ public:
 	bool			get_has_received_messages() { return _received_messages; }
 	void			set_wait_to_transmit(bool wait) { _wait_to_transmit = wait; }
 	bool			get_wait_to_transmit() { return _wait_to_transmit; }
-	bool			should_transmit() { return (!_wait_to_transmit || (_wait_to_transmit && _received_messages)); }
+	bool			should_transmit() { return (_boot_complete && (!_wait_to_transmit || (_wait_to_transmit && _received_messages))); }
 
 	bool			message_buffer_write(const void *ptr, int size);
 

From 8b510270a959f11e37050346c46bf9f01bde85e6 Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Sat, 28 May 2016 10:34:15 +0200
Subject: [PATCH 0351/1230] CPU load: add missing header

---
 src/modules/systemlib/cpuload.h | 1 +
 1 file changed, 1 insertion(+)

diff --git a/src/modules/systemlib/cpuload.h b/src/modules/systemlib/cpuload.h
index e997bb3d9a..1c63964cee 100644
--- a/src/modules/systemlib/cpuload.h
+++ b/src/modules/systemlib/cpuload.h
@@ -38,6 +38,7 @@
 __BEGIN_DECLS
 
 #include <sched.h>
+#include <stdint.h>
 
 struct system_load_taskinfo_s {
 	uint64_t total_runtime;			///< Runtime since start (start_time - total_runtime)/(start_time - current_time) = load

From 959c9e3817685f4313fce4dbf23a0136dd5cb258 Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Sat, 28 May 2016 11:25:15 +0200
Subject: [PATCH 0352/1230] Update gazebo plugin to fix inertial sensor
 oscillation

---
 Tools/sitl_gazebo | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/Tools/sitl_gazebo b/Tools/sitl_gazebo
index 9353834b2f..f1a66579a2 160000
--- a/Tools/sitl_gazebo
+++ b/Tools/sitl_gazebo
@@ -1 +1 @@
-Subproject commit 9353834b2f89d08beaa055fc97321e1871bc2b3a
+Subproject commit f1a66579a2a25d162886f6dd7b1c02f26eba1109

From c3974446bdb2b5f1ab6783a8eee00eeeb3755480 Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Sat, 28 May 2016 11:31:39 +0200
Subject: [PATCH 0353/1230] Update Gazebo tuning gains

---
 posix-configs/SITL/init/rcS_ekf2_gazebo_iris  | 74 +++++++++++++++++++
 posix-configs/SITL/init/rcS_gazebo_iris       | 10 ++-
 .../SITL/init/rcS_gazebo_iris_opt_flow        | 10 ++-
 3 files changed, 90 insertions(+), 4 deletions(-)
 create mode 100644 posix-configs/SITL/init/rcS_ekf2_gazebo_iris

diff --git a/posix-configs/SITL/init/rcS_ekf2_gazebo_iris b/posix-configs/SITL/init/rcS_ekf2_gazebo_iris
new file mode 100644
index 0000000000..ab0e045afc
--- /dev/null
+++ b/posix-configs/SITL/init/rcS_ekf2_gazebo_iris
@@ -0,0 +1,74 @@
+uorb start
+param load
+param set MAV_TYPE 2
+param set MC_PITCHRATE_P 0.15
+param set MC_PITCH_P 7
+param set MC_ROLL_P 7
+param set MC_ROLLRATE_P 0.15
+param set MC_YAW_P 2.8
+param set MC_YAWRATE_P 0.35
+param set SYS_AUTOSTART 4010
+param set SYS_RESTART_TYPE 2
+dataman start
+param set CAL_GYRO0_ID 2293768
+param set CAL_ACC0_ID 1376264
+param set CAL_ACC1_ID 1310728
+param set CAL_MAG0_ID 196616
+param set CAL_GYRO0_XOFF 0.01
+param set CAL_ACC0_XOFF 0.01
+param set CAL_ACC0_YOFF -0.01
+param set CAL_ACC0_ZOFF 0.01
+param set CAL_ACC0_XSCALE 1.01
+param set CAL_ACC0_YSCALE 1.01
+param set CAL_ACC0_ZSCALE 1.01
+param set CAL_ACC1_XOFF 0.01
+param set CAL_MAG0_XOFF 0.01
+param set MPC_XY_P 0.4
+param set MPC_XY_VEL_P 0.2
+param set MPC_XY_VEL_D 0.005
+param set SENS_BOARD_ROT 0
+param set SENS_BOARD_X_OFF 0.000001
+param set COM_RC_IN_MODE 1
+param set NAV_DLL_ACT 2
+param set COM_DISARM_LAND 3
+param set NAV_ACC_RAD 2.0
+param set RTL_RETURN_ALT 30.0
+param set RTL_DESCEND_ALT 10.0
+param set RTL_LAND_DELAY 0
+param set MIS_TAKEOFF_ALT 2.5
+param set MPC_HOLD_MAX_Z 2.0
+param set MPC_HOLD_XY_DZ 0.1
+param set MPC_Z_VEL_MAX 2.0
+param set MPC_Z_VEL_P 0.4
+simulator start -s
+rgbledsim start
+tone_alarm start
+gyrosim start
+accelsim start
+barosim start
+adcsim start
+gpssim start
+pwm_out_sim mode_pwm
+sleep 1
+sensors start
+commander start
+land_detector start multicopter
+navigator start
+ekf2 start
+mc_pos_control start
+mc_att_control start
+mixer load /dev/pwm_output0 ../../../../ROMFS/px4fmu_common/mixers/quad_x.main.mix
+mavlink start -u 14556 -r 2000000 -t 127.0.0.1
+mavlink start -u 14557 -r 2000000 -m onboard -o 14540 -t 127.0.0.1
+mavlink stream -r 80 -s POSITION_TARGET_LOCAL_NED -u 14556
+mavlink stream -r 80 -s LOCAL_POSITION_NED -u 14556
+mavlink stream -r 80 -s GLOBAL_POSITION_INT -u 14556
+mavlink stream -r 80 -s ATTITUDE -u 14556
+mavlink stream -r 80 -s ATTITUDE_QUATERNION -u 14556
+mavlink stream -r 80 -s ATTITUDE_TARGET -u 14556
+mavlink stream -r 20 -s RC_CHANNELS -u 14556
+mavlink stream -r 250 -s HIGHRES_IMU -u 14556
+mavlink stream -r 10 -s OPTICAL_FLOW_RAD -u 14556
+mavlink stream -r 20 -s MANUAL_CONTROL -u 14556
+mavlink boot_complete
+sdlog2 start -r 100 -e -t -a
diff --git a/posix-configs/SITL/init/rcS_gazebo_iris b/posix-configs/SITL/init/rcS_gazebo_iris
index 650c7212f4..fdadd243bb 100644
--- a/posix-configs/SITL/init/rcS_gazebo_iris
+++ b/posix-configs/SITL/init/rcS_gazebo_iris
@@ -27,12 +27,18 @@ param set RTL_RETURN_ALT 30.0
 param set RTL_DESCEND_ALT 10.0
 param set RTL_LAND_DELAY 0
 param set MIS_TAKEOFF_ALT 2.5
-param set MC_ROLLRATE_P 0.25
-param set MC_PITCHRATE_P 0.25
+param set MC_ROLLRATE_P 0.5
+param set MC_PITCHRATE_P 0.5
 param set MC_PITCH_P 5
 param set MC_ROLL_P 5
+param set MC_ROLLRATE_I 0.1
+param set MC_PITCHRATE_I 0.1
+param set MPC_HOLD_MAX_Z 2.0
+param set MPC_HOLD_XY_DZ 0.1
 param set MPC_Z_VEL_P 0.8
 param set MPC_Z_VEL_I 0.15
+param set MPC_XY_VEL_P 0.15
+param set MPC_XY_VEL_I 0.2
 param set EKF2_GBIAS_INIT 0.01
 param set EKF2_ANGERR_INIT 0.01
 simulator start -s
diff --git a/posix-configs/SITL/init/rcS_gazebo_iris_opt_flow b/posix-configs/SITL/init/rcS_gazebo_iris_opt_flow
index 271a9bc9e4..3c6a577bad 100644
--- a/posix-configs/SITL/init/rcS_gazebo_iris_opt_flow
+++ b/posix-configs/SITL/init/rcS_gazebo_iris_opt_flow
@@ -27,12 +27,18 @@ param set RTL_RETURN_ALT 30.0
 param set RTL_DESCEND_ALT 10.0
 param set RTL_LAND_DELAY 0
 param set MIS_TAKEOFF_ALT 2.5
-param set MC_ROLLRATE_P 0.25
-param set MC_PITCHRATE_P 0.25
+param set MC_ROLLRATE_P 0.5
+param set MC_PITCHRATE_P 0.5
 param set MC_PITCH_P 5
 param set MC_ROLL_P 5
+param set MC_ROLLRATE_I 0.1
+param set MC_PITCHRATE_I 0.1
+param set MPC_HOLD_MAX_Z 2.0
+param set MPC_HOLD_XY_DZ 0.1
 param set MPC_Z_VEL_P 0.8
 param set MPC_Z_VEL_I 0.15
+param set MPC_XY_VEL_P 0.15
+param set MPC_XY_VEL_I 0.2
 simulator start -s
 rgbledsim start
 tone_alarm start

From ff5a481c34ce4d5af28cde627a5ca30a286c0bba Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Sat, 28 May 2016 11:34:08 +0200
Subject: [PATCH 0354/1230] Remove unneeded EKF2 configs

---
 Makefile                                      |  3 -
 cmake/configs/posix_sitl_ekf2.cmake           | 92 -------------------
 posix-configs/SITL/init/rc.fixed_wing         | 51 ----------
 posix-configs/SITL/init/rcS_ekf2_gazebo_iris  | 74 ---------------
 posix-configs/SITL/init/rcS_ekf2_jmavsim_iris | 65 -------------
 5 files changed, 285 deletions(-)
 delete mode 100644 cmake/configs/posix_sitl_ekf2.cmake
 delete mode 100644 posix-configs/SITL/init/rc.fixed_wing
 delete mode 100644 posix-configs/SITL/init/rcS_ekf2_gazebo_iris
 delete mode 100644 posix-configs/SITL/init/rcS_ekf2_jmavsim_iris

diff --git a/Makefile b/Makefile
index d97a18a1fd..382e8ded72 100644
--- a/Makefile
+++ b/Makefile
@@ -167,9 +167,6 @@ posix_sitl_default:
 posix_sitl_test:
 	$(call cmake-build,$@)
 
-posix_sitl_ekf2:
-	$(call cmake-build,$@)
-
 posix_sitl_replay:
 	$(call cmake-build,$@)
 
diff --git a/cmake/configs/posix_sitl_ekf2.cmake b/cmake/configs/posix_sitl_ekf2.cmake
deleted file mode 100644
index d92ed793a0..0000000000
--- a/cmake/configs/posix_sitl_ekf2.cmake
+++ /dev/null
@@ -1,92 +0,0 @@
-include(posix/px4_impl_posix)
-
-set(CMAKE_TOOLCHAIN_FILE ${CMAKE_SOURCE_DIR}/cmake/toolchains/Toolchain-native.cmake)
-
-set(config_module_list
-	drivers/device
-	drivers/boards/sitl
-	drivers/pwm_out_sim
-	platforms/common
-	platforms/posix/px4_layer
-	platforms/posix/work_queue
-	platforms/posix/drivers/adcsim
-	platforms/posix/drivers/gpssim
-	platforms/posix/drivers/tonealrmsim
-	platforms/posix/drivers/accelsim
-	platforms/posix/drivers/airspeedsim
-	platforms/posix/drivers/barosim
-	platforms/posix/drivers/gyrosim
-	platforms/posix/drivers/rgbledsim
-	platforms/posix/drivers/ledsim
-	systemcmds/param
-	systemcmds/mixer
-	systemcmds/ver
-	systemcmds/esc_calib
-	systemcmds/reboot
-	systemcmds/topic_listener
-	systemcmds/perf
-	modules/uORB
-	modules/param
-	modules/systemlib
-	modules/systemlib/mixer
-	modules/sensors
-	modules/simulator
-	modules/mavlink
-	modules/attitude_estimator_ekf
-	modules/attitude_estimator_q
-	modules/ekf_att_pos_estimator
-	modules/ekf2
-	modules/position_estimator_inav
-	modules/navigator
-	modules/vtol_att_control
-	modules/mc_pos_control
-	modules/mc_att_control
-	modules/mc_pos_control_multiplatform
-	modules/mc_att_control_multiplatform
-	modules/land_detector
-	modules/fw_att_control
-	modules/fw_pos_control_l1
-	modules/dataman
-	modules/sdlog2
-	modules/logger
-	modules/commander
-	modules/load_mon
-	lib/controllib
-	lib/mathlib
-	lib/mathlib/math/filter
-	lib/conversion
-	lib/ecl
-	lib/external_lgpl
-	lib/geo
-	lib/geo_lookup
-	lib/launchdetection
-	lib/terrain_estimation
-	lib/runway_takeoff
-	lib/tailsitter_recovery
-	lib/DriverFramework/framework
-	examples/px4_simple_app
-	)
-
-set(config_extra_builtin_cmds
-	serdis
-	sercon
-	)
-
-set(config_sitl_rcS
-	posix-configs/SITL/init/rcS_ekf2
-	CACHE FILEPATH "init script for sitl"
-	)
-
-set(config_sitl_viewer
-	jmavsim
-	CACHE STRING "viewer for sitl"
-	)
-set_property(CACHE config_sitl_viewer
-	PROPERTY STRINGS "jmavsim;none")
-
-set(config_sitl_debugger
-	disable
-	CACHE STRING "debugger for sitl"
-	)
-set_property(CACHE config_sitl_debugger
-	PROPERTY STRINGS "disable;gdb;lldb")
diff --git a/posix-configs/SITL/init/rc.fixed_wing b/posix-configs/SITL/init/rc.fixed_wing
deleted file mode 100644
index bc8a314e2f..0000000000
--- a/posix-configs/SITL/init/rc.fixed_wing
+++ /dev/null
@@ -1,51 +0,0 @@
-uorb start
-param load
-param set MAV_TYPE 1
-param set SYS_AUTOSTART 3033
-param set SYS_RESTART_TYPE 2
-dataman start
-param set CAL_GYRO0_ID 2293768
-param set CAL_ACC0_ID 1376264
-param set CAL_ACC1_ID 1310728
-param set CAL_MAG0_ID 196616
-param set CAL_GYRO0_XOFF 0.01
-param set CAL_ACC0_XOFF 0.01
-param set CAL_ACC0_YOFF -0.01
-param set CAL_ACC0_ZOFF 0.01
-param set CAL_ACC0_XSCALE 1.01
-param set CAL_ACC0_YSCALE 1.01
-param set CAL_ACC0_ZSCALE 1.01
-param set CAL_ACC1_XOFF 0.01
-param set CAL_MAG0_XOFF 0.01
-param set MPC_XY_P 0.4
-param set MPC_XY_VEL_P 0.2
-param set MPC_XY_VEL_D 0.005
-param set COM_RC_IN_MODE 1
-param set NAV_DLL_ACT 2
-simulator start -s
-rgbled start
-tone_alarm start
-gyrosim start
-accelsim start
-barosim start
-adcsim start
-gpssim start
-measairspeedsim start
-pwm_out_sim mode_pwm
-sleep 1
-sensors start
-commander start
-land_detector start fixedwing
-navigator start
-ekf_att_pos_estimator start
-fw_att_control start
-fw_pos_control_l1 start
-mixer load /dev/pwm_output0 ../../../../ROMFS/px4fmu_common/mixers/IO_pass.main.mix
-mavlink start -u 14556 -r 60000
-mavlink stream -r 50 -s POSITION_TARGET_LOCAL_NED -u 14556
-mavlink stream -r 50 -s LOCAL_POSITION_NED -u 14556
-mavlink stream -r 50 -s ATTITUDE -u 14556
-mavlink stream -r 80 -s ATTITUDE_QUATERNION -u 14556
-mavlink stream -r 50 -s ATTITUDE_TARGET -u 14556
-mavlink boot_complete
-sdlog2 start -r 100 -e -t -a
diff --git a/posix-configs/SITL/init/rcS_ekf2_gazebo_iris b/posix-configs/SITL/init/rcS_ekf2_gazebo_iris
deleted file mode 100644
index ab0e045afc..0000000000
--- a/posix-configs/SITL/init/rcS_ekf2_gazebo_iris
+++ /dev/null
@@ -1,74 +0,0 @@
-uorb start
-param load
-param set MAV_TYPE 2
-param set MC_PITCHRATE_P 0.15
-param set MC_PITCH_P 7
-param set MC_ROLL_P 7
-param set MC_ROLLRATE_P 0.15
-param set MC_YAW_P 2.8
-param set MC_YAWRATE_P 0.35
-param set SYS_AUTOSTART 4010
-param set SYS_RESTART_TYPE 2
-dataman start
-param set CAL_GYRO0_ID 2293768
-param set CAL_ACC0_ID 1376264
-param set CAL_ACC1_ID 1310728
-param set CAL_MAG0_ID 196616
-param set CAL_GYRO0_XOFF 0.01
-param set CAL_ACC0_XOFF 0.01
-param set CAL_ACC0_YOFF -0.01
-param set CAL_ACC0_ZOFF 0.01
-param set CAL_ACC0_XSCALE 1.01
-param set CAL_ACC0_YSCALE 1.01
-param set CAL_ACC0_ZSCALE 1.01
-param set CAL_ACC1_XOFF 0.01
-param set CAL_MAG0_XOFF 0.01
-param set MPC_XY_P 0.4
-param set MPC_XY_VEL_P 0.2
-param set MPC_XY_VEL_D 0.005
-param set SENS_BOARD_ROT 0
-param set SENS_BOARD_X_OFF 0.000001
-param set COM_RC_IN_MODE 1
-param set NAV_DLL_ACT 2
-param set COM_DISARM_LAND 3
-param set NAV_ACC_RAD 2.0
-param set RTL_RETURN_ALT 30.0
-param set RTL_DESCEND_ALT 10.0
-param set RTL_LAND_DELAY 0
-param set MIS_TAKEOFF_ALT 2.5
-param set MPC_HOLD_MAX_Z 2.0
-param set MPC_HOLD_XY_DZ 0.1
-param set MPC_Z_VEL_MAX 2.0
-param set MPC_Z_VEL_P 0.4
-simulator start -s
-rgbledsim start
-tone_alarm start
-gyrosim start
-accelsim start
-barosim start
-adcsim start
-gpssim start
-pwm_out_sim mode_pwm
-sleep 1
-sensors start
-commander start
-land_detector start multicopter
-navigator start
-ekf2 start
-mc_pos_control start
-mc_att_control start
-mixer load /dev/pwm_output0 ../../../../ROMFS/px4fmu_common/mixers/quad_x.main.mix
-mavlink start -u 14556 -r 2000000 -t 127.0.0.1
-mavlink start -u 14557 -r 2000000 -m onboard -o 14540 -t 127.0.0.1
-mavlink stream -r 80 -s POSITION_TARGET_LOCAL_NED -u 14556
-mavlink stream -r 80 -s LOCAL_POSITION_NED -u 14556
-mavlink stream -r 80 -s GLOBAL_POSITION_INT -u 14556
-mavlink stream -r 80 -s ATTITUDE -u 14556
-mavlink stream -r 80 -s ATTITUDE_QUATERNION -u 14556
-mavlink stream -r 80 -s ATTITUDE_TARGET -u 14556
-mavlink stream -r 20 -s RC_CHANNELS -u 14556
-mavlink stream -r 250 -s HIGHRES_IMU -u 14556
-mavlink stream -r 10 -s OPTICAL_FLOW_RAD -u 14556
-mavlink stream -r 20 -s MANUAL_CONTROL -u 14556
-mavlink boot_complete
-sdlog2 start -r 100 -e -t -a
diff --git a/posix-configs/SITL/init/rcS_ekf2_jmavsim_iris b/posix-configs/SITL/init/rcS_ekf2_jmavsim_iris
deleted file mode 100644
index 2504e9f4e3..0000000000
--- a/posix-configs/SITL/init/rcS_ekf2_jmavsim_iris
+++ /dev/null
@@ -1,65 +0,0 @@
-uorb start
-param load
-param set MAV_TYPE 2
-param set MC_PITCHRATE_P 0.15
-param set MC_PITCH_P 7
-param set MC_ROLL_P 7
-param set MC_ROLLRATE_P 0.15
-param set MC_YAW_P 2.8
-param set MC_YAWRATE_P 0.35
-param set SYS_AUTOSTART 4010
-param set SYS_RESTART_TYPE 2
-dataman start
-param set CAL_GYRO0_ID 2293768
-param set CAL_ACC0_ID 1376264
-param set CAL_ACC1_ID 1310728
-param set CAL_MAG0_ID 196616
-param set CAL_GYRO0_XOFF 0.01
-param set CAL_ACC0_XOFF 0.01
-param set CAL_ACC0_YOFF -0.01
-param set CAL_ACC0_ZOFF 0.01
-param set CAL_ACC0_XSCALE 1.01
-param set CAL_ACC0_YSCALE 1.01
-param set CAL_ACC0_ZSCALE 1.01
-param set CAL_ACC1_XOFF 0.01
-param set CAL_MAG0_XOFF 0.01
-param set MPC_XY_P 0.4
-param set MPC_XY_VEL_P 0.2
-param set MPC_XY_VEL_D 0.005
-param set SENS_BOARD_ROT 0
-param set SENS_BOARD_X_OFF 0.000001
-param set COM_RC_IN_MODE 1
-param set NAV_DLL_ACT 2
-param set NAV_ACC_RAD 2.0
-param set RTL_RETURN_ALT 30.0
-param set RTL_DESCEND_ALT 10.0
-simulator start -s
-rgbledsim start
-tone_alarm start
-gyrosim start
-accelsim start
-barosim start
-adcsim start
-gpssim start
-pwm_out_sim mode_pwm
-sleep 1
-sensors start
-commander start
-land_detector start multicopter
-navigator start
-mc_pos_control start
-mc_att_control start
-mixer load /dev/pwm_output0 ../../../../ROMFS/px4fmu_common/mixers/quad_x.main.mix
-mavlink start -u 14556 -r 2000000
-mavlink stream -r 80 -s POSITION_TARGET_LOCAL_NED -u 14556
-mavlink stream -r 80 -s LOCAL_POSITION_NED -u 14556
-mavlink stream -r 80 -s GLOBAL_POSITION_INT -u 14556
-mavlink stream -r 80 -s ATTITUDE -u 14556
-mavlink stream -r 80 -s ATTITUDE_QUATERNION -u 14556
-mavlink stream -r 80 -s ATTITUDE_TARGET -u 14556
-mavlink stream -r 20 -s RC_CHANNELS -u 14556
-mavlink stream -r 250 -s HIGHRES_IMU -u 14556
-mavlink stream -r 10 -s OPTICAL_FLOW_RAD -u 14556
-mavlink boot_complete
-sdlog2 start -r 100 -e -t -a
-ekf2 start

From 73d70fa7e8834c34c4ba1e342875b0b499a0dce2 Mon Sep 17 00:00:00 2001
From: Julian Oes <julian@oes.ch>
Date: Sat, 28 May 2016 11:35:28 +0200
Subject: [PATCH 0355/1230] adb_upload: try to sync after uploading (#4669)

---
 Tools/adb_upload.sh | 4 ++++
 1 file changed, 4 insertions(+)

diff --git a/Tools/adb_upload.sh b/Tools/adb_upload.sh
index 166aa29716..b4e4f649cb 100755
--- a/Tools/adb_upload.sh
+++ b/Tools/adb_upload.sh
@@ -23,3 +23,7 @@ do
 	adb push $arg $last
 	((i+=1))
 done
+
+# Make sure they are synced to the file system
+echo "Syncing FS..."
+adb shell sync

From 6dfb80ddd18905ed373c5ed36a2b01c4594c7f03 Mon Sep 17 00:00:00 2001
From: Julian Oes <julian@oes.ch>
Date: Sat, 28 May 2016 10:25:04 +0200
Subject: [PATCH 0356/1230] snapdragon_rc_pwm: fix comments

---
 .../snapdragon_rc_pwm/snapdragon_rc_pwm.cpp     | 17 +++++++++--------
 1 file changed, 9 insertions(+), 8 deletions(-)

diff --git a/src/drivers/snapdragon_rc_pwm/snapdragon_rc_pwm.cpp b/src/drivers/snapdragon_rc_pwm/snapdragon_rc_pwm.cpp
index ce86147904..ee82fa8f4a 100644
--- a/src/drivers/snapdragon_rc_pwm/snapdragon_rc_pwm.cpp
+++ b/src/drivers/snapdragon_rc_pwm/snapdragon_rc_pwm.cpp
@@ -35,9 +35,10 @@
 /**
  * @file snapdragon_rc_pwm.cpp
  * @author Roman Bapst <roman@px4.io>
+ * @author Julian Oes <julian@oes.ch>
  *
- * This driver sends rc commands to the Snapdragon via UART. On the same UART it receives pwm
- * motor commands from the Snapdragon.
+ * This driver sends RC commands to the Snapdragon via UART. On the same UART
+ * it receives pwm motor commands from the Snapdragon.
  */
 
 
@@ -66,9 +67,9 @@ namespace snapdragon_rc_pwm
 static const uint8_t mavlink_message_lengths[256] = MAVLINK_MESSAGE_LENGTHS;
 static const uint8_t mavlink_message_crcs[256] = MAVLINK_MESSAGE_CRCS;
 
-volatile bool _task_should_exit = false; // flag indicating if uart_esc task should exit
+volatile bool _task_should_exit = false; // flag indicating if snapdragon_rc_pwm task should exit
 static char _device[MAX_LEN_DEV_PATH];
-static bool _is_running = false;         // flag indicating if uart_esc app is running
+static bool _is_running = false;         // flag indicating if snapdragon_rc_pwm app is running
 static px4_task_t _task_handle = -1;     // handle to the task main thread
 static int _uart_fd = -1;
 int _pwm_fd = -1;
@@ -85,7 +86,7 @@ void usage();
 
 void start();
 
-/** uart_esc stop */
+/** snapdragon_rc_pwm stop */
 void stop();
 
 int initialise_uart();
@@ -103,7 +104,7 @@ void set_pwm_output(mavlink_actuator_control_target_t *actuator_controls);
 /** task main trampoline function */
 void task_main_trampoline(int argc, char *argv[]);
 
-/** uart_esc thread primary entry point */
+/** snapdragon_rc_pwm thread primary entry point */
 void task_main(int argc, char *argv[]);
 
 void task_main(int argc, char *argv[])
@@ -365,7 +366,7 @@ int deinitialize_uart()
 	return close(_uart_fd);
 }
 
-// uart_esc main entrance
+// snapdragon_rc_pwm main entrance
 void task_main_trampoline(int argc, char *argv[])
 {
 	task_main(argc, argv);
@@ -450,7 +451,7 @@ int snapdragon_rc_pwm_main(int argc, char *argv[])
 
 
 		if (snapdragon_rc_pwm::_is_running) {
-			PX4_WARN("uart_esc already running");
+			PX4_WARN("snapdragon_rc_pwm already running");
 			return 1;
 		}
 

From 9d489b9b0f26e3c082aeaf051d837761933ad480 Mon Sep 17 00:00:00 2001
From: Julian Oes <julian@oes.ch>
Date: Sat, 28 May 2016 10:54:13 +0200
Subject: [PATCH 0357/1230] Snapdragon: rename uart_esc to pwm_out_rc_in

The name uart_esc was initially taken by Qualcomm's UART ESC driver but
then got changed into the current mavlink ESC/RC helper. Since the
uart_esc is still around, we should prevent the names clashing.
---
 .../qurt_eagle_legacy_driver_default.cmake    |  2 +-
 cmake/configs/qurt_sdflight_default.cmake     |  2 +-
 posix-configs/eagle/flight/px4.config         |  2 +-
 .../CMakeLists.txt                            |  8 +--
 .../pwm_out_rc_in.cpp}                        | 51 ++++++++++---------
 5 files changed, 35 insertions(+), 30 deletions(-)
 rename src/drivers/{uart_esc => pwm_out_rc_in}/CMakeLists.txt (92%)
 rename src/drivers/{uart_esc/uart_esc.cpp => pwm_out_rc_in/pwm_out_rc_in.cpp} (91%)

diff --git a/cmake/configs/qurt_eagle_legacy_driver_default.cmake b/cmake/configs/qurt_eagle_legacy_driver_default.cmake
index 1ff1845c32..46dfbd2df5 100644
--- a/cmake/configs/qurt_eagle_legacy_driver_default.cmake
+++ b/cmake/configs/qurt_eagle_legacy_driver_default.cmake
@@ -64,7 +64,7 @@ set(config_module_list
 	# PX4 drivers
 	#
 	drivers/gps
-	drivers/uart_esc
+	drivers/pwm_out_rc_in
 	drivers/qshell/qurt
 
 	#
diff --git a/cmake/configs/qurt_sdflight_default.cmake b/cmake/configs/qurt_sdflight_default.cmake
index c646a6dfb6..03b6a7353c 100644
--- a/cmake/configs/qurt_sdflight_default.cmake
+++ b/cmake/configs/qurt_sdflight_default.cmake
@@ -61,7 +61,7 @@ set(config_module_list
 	# PX4 drivers
 	#
 	drivers/gps
-	drivers/uart_esc
+	drivers/pwm_out_rc_in
 	drivers/qshell/qurt
 
 	#
diff --git a/posix-configs/eagle/flight/px4.config b/posix-configs/eagle/flight/px4.config
index 8149cb4d4c..2c92e90ba1 100644
--- a/posix-configs/eagle/flight/px4.config
+++ b/posix-configs/eagle/flight/px4.config
@@ -15,7 +15,7 @@ ekf2 start
 land_detector start multicopter
 mc_pos_control start
 mc_att_control start
-uart_esc start -d /dev/tty-2
+pwm_out_rc_in start -d /dev/tty-2
 sleep 1
 list_devices
 list_files
diff --git a/src/drivers/uart_esc/CMakeLists.txt b/src/drivers/pwm_out_rc_in/CMakeLists.txt
similarity index 92%
rename from src/drivers/uart_esc/CMakeLists.txt
rename to src/drivers/pwm_out_rc_in/CMakeLists.txt
index cb060c82da..08764c7f05 100644
--- a/src/drivers/uart_esc/CMakeLists.txt
+++ b/src/drivers/pwm_out_rc_in/CMakeLists.txt
@@ -1,6 +1,6 @@
 ############################################################################
 #
-#   Copyright (c) 2015 PX4 Development Team. All rights reserved.
+#   Copyright (c) 2015-2016 PX4 Development Team. All rights reserved.
 #
 # Redistribution and use in source and binary forms, with or without
 # modification, are permitted provided that the following conditions
@@ -31,12 +31,12 @@
 #
 ############################################################################
 px4_add_module(
-	MODULE drivers__uart_esc
-	MAIN uart_esc
+	MODULE drivers__pwm_out_rc_in
+	MAIN pwm_out_rc_in
 	COMPILE_FLAGS
 		-Os
 	SRCS
-		uart_esc.cpp
+		pwm_out_rc_in.cpp
 	DEPENDS
 		platforms__common
 	)
diff --git a/src/drivers/uart_esc/uart_esc.cpp b/src/drivers/pwm_out_rc_in/pwm_out_rc_in.cpp
similarity index 91%
rename from src/drivers/uart_esc/uart_esc.cpp
rename to src/drivers/pwm_out_rc_in/pwm_out_rc_in.cpp
index 458a8463bb..4b2b68775a 100644
--- a/src/drivers/uart_esc/uart_esc.cpp
+++ b/src/drivers/pwm_out_rc_in/pwm_out_rc_in.cpp
@@ -56,15 +56,20 @@
 #include <v1.0/common/mavlink.h>
 
 
-namespace uart_esc
+/*
+ * This driver is supposed to run on Snapdragon. It sends actuator_controls (PWM)
+ * to a Pixhawk/Pixfalcon/Pixracer over UART (mavlink) and receives RC input.
+ */
+
+namespace pwm_out_rc_in
 {
 
 static const uint8_t mavlink_message_lengths[256] = MAVLINK_MESSAGE_LENGTHS;
 static const uint8_t mavlink_message_crcs[256] = MAVLINK_MESSAGE_CRCS;
 
-volatile bool _task_should_exit = false; // flag indicating if uart_esc task should exit
+volatile bool _task_should_exit = false; // flag indicating if pwm_out_rc_in task should exit
 static char _device[32] = {};
-static bool _is_running = false;         // flag indicating if uart_esc app is running
+static bool _is_running = false;         // flag indicating if pwm_out_rc_in app is running
 static px4_task_t _task_handle = -1;     // handle to the task main thread
 
 // subscriptions
@@ -473,7 +478,7 @@ void start()
 	_task_should_exit = false;
 
 	/* start the task */
-	_task_handle = px4_task_spawn_cmd("uart_esc_main",
+	_task_handle = px4_task_spawn_cmd("pwm_out_rc_in_main",
 					  SCHED_DEFAULT,
 					  SCHED_PRIORITY_MAX,
 					  1500,
@@ -501,17 +506,17 @@ void stop()
 
 void usage()
 {
-	PX4_INFO("usage: uart_esc start -d /dev/tty-3");
-	PX4_INFO("       uart_esc stop");
-	PX4_INFO("       uart_esc status");
+	PX4_INFO("usage: pwm_out_rc_in start -d /dev/tty-3");
+	PX4_INFO("       pwm_out_rc_in stop");
+	PX4_INFO("       pwm_out_rc_in status");
 }
 
-} // namespace uart_esc
+} // namespace pwm_out_rc_in
 
 /* driver 'main' command */
-extern "C" __EXPORT int uart_esc_main(int argc, char *argv[]);
+extern "C" __EXPORT int pwm_out_rc_in_main(int argc, char *argv[]);
 
-int uart_esc_main(int argc, char *argv[])
+int pwm_out_rc_in_main(int argc, char *argv[])
 {
 	const char *device = nullptr;
 	int ch;
@@ -528,49 +533,49 @@ int uart_esc_main(int argc, char *argv[])
 		switch (ch) {
 		case 'd':
 			device = myoptarg;
-			strncpy(uart_esc::_device, device, strlen(device));
+			strncpy(pwm_out_rc_in::_device, device, strlen(device));
 			break;
 		}
 	}
 
 	// gets the parameters for the esc's pwm
-	param_get(param_find("PWM_DISARMED"), &uart_esc::_pwm_disarmed);
-	param_get(param_find("PWM_MIN"), &uart_esc::_pwm_min);
-	param_get(param_find("PWM_MAX"), &uart_esc::_pwm_max);
+	param_get(param_find("PWM_DISARMED"), &pwm_out_rc_in::_pwm_disarmed);
+	param_get(param_find("PWM_MIN"), &pwm_out_rc_in::_pwm_min);
+	param_get(param_find("PWM_MAX"), &pwm_out_rc_in::_pwm_max);
 
 	/*
 	 * Start/load the driver.
 	 */
 	if (!strcmp(verb, "start")) {
-		if (uart_esc::_is_running) {
-			PX4_WARN("uart_esc already running");
+		if (pwm_out_rc_in::_is_running) {
+			PX4_WARN("pwm_out_rc_in already running");
 			return 1;
 		}
 
 		// Check on required arguments
 		if (device == nullptr || strlen(device) == 0) {
-			uart_esc::usage();
+			pwm_out_rc_in::usage();
 			return 1;
 		}
 
-		uart_esc::start();
+		pwm_out_rc_in::start();
 	}
 
 	else if (!strcmp(verb, "stop")) {
-		if (!uart_esc::_is_running) {
-			PX4_WARN("uart_esc is not running");
+		if (!pwm_out_rc_in::_is_running) {
+			PX4_WARN("pwm_out_rc_in is not running");
 			return 1;
 		}
 
-		uart_esc::stop();
+		pwm_out_rc_in::stop();
 	}
 
 	else if (!strcmp(verb, "status")) {
-		PX4_WARN("uart_esc is %s", uart_esc::_is_running ? "running" : "not running");
+		PX4_WARN("pwm_out_rc_in is %s", pwm_out_rc_in::_is_running ? "running" : "not running");
 		return 0;
 
 	} else {
-		uart_esc::usage();
+		pwm_out_rc_in::usage();
 		return 1;
 	}
 

From 2b370417e87b88e44f7a6a37f18fc511c83deb6d Mon Sep 17 00:00:00 2001
From: Paul Riseborough <p_riseborough@live.com.au>
Date: Thu, 26 May 2016 20:28:01 +1000
Subject: [PATCH 0358/1230] drivers: Correct IMU coning correction
 implementation

Previous did not match the matlab simulation in: https://github.com/priseborough/InertialNav/blob/master/models/imu_error_modelling.m
---
 src/drivers/device/integrator.cpp | 49 +++++++++++++++++++++----------
 src/drivers/device/integrator.h   | 10 ++++---
 2 files changed, 40 insertions(+), 19 deletions(-)

diff --git a/src/drivers/device/integrator.cpp b/src/drivers/device/integrator.cpp
index 072e3096a2..000b9513cc 100644
--- a/src/drivers/device/integrator.cpp
+++ b/src/drivers/device/integrator.cpp
@@ -46,9 +46,11 @@ Integrator::Integrator(uint64_t auto_reset_interval, bool coning_compensation) :
 	_auto_reset_interval(auto_reset_interval),
 	_last_integration_time(0),
 	_last_reset_time(0),
-	_integral(0.0f, 0.0f, 0.0f),
+	_alpha(0.0f, 0.0f, 0.0f),
+	_last_alpha(0.0f, 0.0f, 0.0f),
+	_beta(0.0f, 0.0f, 0.0f),
 	_last_val(0.0f, 0.0f, 0.0f),
-	_last_delta(0.0f, 0.0f, 0.0f),
+	_last_delta_alpha(0.0f, 0.0f, 0.0f),
 	_coning_comp_on(coning_compensation)
 {
 
@@ -80,28 +82,39 @@ Integrator::put(uint64_t timestamp, math::Vector<3> &val, math::Vector<3> &integ
 		dt = (double)(timestamp - _last_integration_time) / 1000000.0;
 	}
 
-	math::Vector<3> delta = (val + _last_val) * dt * 0.5f;
+	// Use trapezoidal integration to calculate the delta integral
+	math::Vector<3> delta_alpha = (val + _last_val) * dt * 0.5f;
+	_last_val = val;
 
-	// Apply coning compensation if required
+	// Calculate coning corrections if required
 	if (_coning_comp_on) {
 		// Coning compensation derived by Paul Riseborough and Jonathan Challinger,
 		// following:
 		// Tian et al (2010) Three-loop Integration of GPS and Strapdown INS with Coning and Sculling Compensation
-		// Available: http://www.sage.unsw.edu.au/snap/publications/tian_etal2010b.pdf
-
-		delta += ((_integral + _last_delta * (1.0f / 6.0f)) % delta) * 0.5f;
+		// Sourced: http://www.sage.unsw.edu.au/snap/publications/tian_etal2010b.pdf
+		// Simulated: https://github.com/priseborough/InertialNav/blob/master/models/imu_error_modelling.m
+		_beta += ((_last_alpha + _last_delta_alpha * (1.0f / 6.0f)) % delta_alpha) * 0.5f;
+		_last_delta_alpha = delta_alpha;
+		_last_alpha = _alpha;
 	}
 
-	_integral += delta;
+	// accumulate delta integrals
+	_alpha += delta_alpha;
 
 	_last_integration_time = timestamp;
-	_last_val = val;
-	_last_delta = delta;
 
 	// Only do auto reset if auto reset interval is not 0.
 	if (_auto_reset_interval > 0 && (timestamp - _last_reset_time) > _auto_reset_interval) {
 
-		integral = _integral;
+		// apply coning corrections if required
+		if (_coning_comp_on) {
+			integral = _alpha + _beta;
+
+		} else {
+			integral = _alpha;
+		}
+
+		// reset the integrals and coning corrections
 		_reset(integral_dt);
 
 		return true;
@@ -134,7 +147,7 @@ Integrator::put_with_interval(unsigned interval_us, math::Vector<3> &val, math::
 math::Vector<3>
 Integrator::get(bool reset, uint64_t &integral_dt)
 {
-	math::Vector<3> val = _integral;
+	math::Vector<3> val = _alpha;
 
 	if (reset) {
 		_reset(integral_dt);
@@ -160,9 +173,15 @@ Integrator::get_and_filtered(bool reset, uint64_t &integral_dt, math::Vector<3>
 void
 Integrator::_reset(uint64_t &integral_dt)
 {
-	_integral(0) = 0.0f;
-	_integral(1) = 0.0f;
-	_integral(2) = 0.0f;
+	_alpha(0) = 0.0f;
+	_alpha(1) = 0.0f;
+	_alpha(2) = 0.0f;
+	_last_alpha(0) = 0.0f;
+	_last_alpha(1) = 0.0f;
+	_last_alpha(2) = 0.0f;
+	_beta(0) = 0.0f;
+	_beta(1) = 0.0f;
+	_beta(2) = 0.0f;
 
 	integral_dt = (_last_integration_time - _last_reset_time);
 	_last_reset_time = _last_integration_time;
diff --git a/src/drivers/device/integrator.h b/src/drivers/device/integrator.h
index 48a232066f..2917fbcab6 100644
--- a/src/drivers/device/integrator.h
+++ b/src/drivers/device/integrator.h
@@ -102,10 +102,12 @@ private:
 							     and the integrator reset, 0 if no auto-reset */
 	uint64_t _last_integration_time;		/**< timestamp of the last integration step */
 	uint64_t _last_reset_time;			/**< last auto-announcement of integral value */
-	math::Vector<3> _integral;			/**< the integrated value */
-	math::Vector<3> _last_val;			/**< previously integrated last value */
-	math::Vector<3> _last_delta;			/**< last local delta */
-	bool _coning_comp_on;				/**< coning compensation */
+	math::Vector<3> _alpha;				/**< integrated value before coning corrections are applied */
+	math::Vector<3> _last_alpha;			/**< previous value of _alpha */
+	math::Vector<3> _beta;				/**< accumulated coning corrections */
+	math::Vector<3> _last_val;			/**< previous input */
+	math::Vector<3> _last_delta_alpha;		/**< integral from previous previous sampling interval */
+	bool _coning_comp_on;				/**< true to turn on coning corrections */
 
 	/* we don't want this class to be copied */
 	Integrator(const Integrator &);

From 120ff6ea15f8cd9d6c328aa2f323ecbdd980f7dc Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Sat, 28 May 2016 12:07:28 +0200
Subject: [PATCH 0359/1230] CPU load header cleanup

---
 src/modules/systemlib/cpuload.c | 7 ++-----
 1 file changed, 2 insertions(+), 5 deletions(-)

diff --git a/src/modules/systemlib/cpuload.c b/src/modules/systemlib/cpuload.c
index 77743bdd8c..7f4c967196 100644
--- a/src/modules/systemlib/cpuload.c
+++ b/src/modules/systemlib/cpuload.c
@@ -1,8 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved.
- *   Author: Lorenz Meier <lm@inf.ethz.ch>
- * 	     Petri Tanskanen <petri.tanskanen@inf.ethz.ch>
+ *   Copyright (c) 2012-2016 PX4 Development Team. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -38,11 +36,10 @@
  *
  * Measurement of CPU load of each individual task.
  *
- * @author Lorenz Meier <lm@inf.ethz.ch>
+ * @author Lorenz Meier <lorenz@px4.io>
  * @author Petri Tanskanen <petri.tanskanen@inf.ethz.ch>
  */
 #include <px4_config.h>
-//#include <nuttx/sched.h>
 
 #include <sys/types.h>
 #include <stdint.h>

From b412980f90e984509bfa8b5dfd14dffa7899f475 Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Sat, 28 May 2016 12:10:19 +0200
Subject: [PATCH 0360/1230] Enable top in SITL

---
 cmake/configs/posix_sitl_default.cmake | 1 +
 src/systemcmds/top/top.c               | 6 +++---
 2 files changed, 4 insertions(+), 3 deletions(-)

diff --git a/cmake/configs/posix_sitl_default.cmake b/cmake/configs/posix_sitl_default.cmake
index b5343ff73d..3adc2a425c 100644
--- a/cmake/configs/posix_sitl_default.cmake
+++ b/cmake/configs/posix_sitl_default.cmake
@@ -29,6 +29,7 @@ set(config_module_list
 	systemcmds/sd_bench
 	systemcmds/topic_listener
 	systemcmds/ver
+	systemcmds/top
 
 	modules/attitude_estimator_ekf
 	modules/attitude_estimator_q
diff --git a/src/systemcmds/top/top.c b/src/systemcmds/top/top.c
index fe9614bc69..68e2801bab 100644
--- a/src/systemcmds/top/top.c
+++ b/src/systemcmds/top/top.c
@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2012, 2013, 2015 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2012-2016 PX4 Development Team. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -96,7 +96,7 @@ top_main(int argc, char *argv[])
 				case 0x1b: // esc
 				case 'c':
 				case 'q':
-					return OK;
+					return 0;
 					/* not reached */
 				}
 			}
@@ -107,5 +107,5 @@ top_main(int argc, char *argv[])
 		curr_time = hrt_absolute_time();
 	}
 
-	return OK;
+	return 0;
 }

From 7a44ee7429161eb1cf5aeb2af0cf919efb55ccac Mon Sep 17 00:00:00 2001
From: Mark Charlebois <charlebm@gmail.com>
Date: Fri, 29 Apr 2016 16:09:47 -0700
Subject: [PATCH 0361/1230] Added support for external shared libraries

The FC_ADDON drivers are shared libraries that have PX4 wrappers.
The wrappers are built as modules which are static libraries and
cannot have shared library dependencies.

The shared libraries are required to resolve the symbols at runtime
and need to be linked with the libmainapp shared library.

Signed-off-by: Mark Charlebois <charlebm@gmail.com>
---
 CMakeLists.txt                   | 3 +++
 src/firmware/qurt/CMakeLists.txt | 2 ++
 2 files changed, 5 insertions(+)

diff --git a/CMakeLists.txt b/CMakeLists.txt
index f9e11ee448..5c2e1ae5f0 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -348,6 +348,9 @@ foreach(module ${config_module_list})
 	#message(STATUS "adding module: ${module}")
 endforeach()
 
+# Keep track of external shared libs required for modules
+set(module_shared_libraries "${module_shared_libraries}" CACHE INTERNAL "module_shared_libraries")
+
 add_subdirectory(src/firmware/${OS})
 
 #add_dependencies(df_driver_framework nuttx_export_${CONFIG}.stamp)
diff --git a/src/firmware/qurt/CMakeLists.txt b/src/firmware/qurt/CMakeLists.txt
index 29046e54bb..bacebe70ef 100644
--- a/src/firmware/qurt/CMakeLists.txt
+++ b/src/firmware/qurt/CMakeLists.txt
@@ -34,6 +34,7 @@ else()
 	message("module_libraries = ${module_libraries}")
 	message("target_libraries = ${target_libraries}")
 	message("df_driver_libs = ${df_driver_libs}")
+	message("module_external_libraries = ${module_external_libraries}")
 	# Generate the DSP lib and stubs but not the apps side executable
 	# The Apps side executable is generated via the posix_eagle_xxxx target
 	QURT_LIB(LIB_NAME mainapp
@@ -45,6 +46,7 @@ else()
 			${target_libraries}
 			${df_driver_libs}
 			m
+			${module_external_libraries}
 		)
 
 	px4_add_adb_push(OUT upload

From dea4c55897af6d18c9194fc30345184ae558a03e Mon Sep 17 00:00:00 2001
From: Mark Charlebois <charlebm@gmail.com>
Date: Tue, 3 May 2016 11:27:24 -0700
Subject: [PATCH 0362/1230] Fixed incorrect variable name

Signed-off-by: Mark Charlebois <charlebm@gmail.com>
---
 CMakeLists.txt | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/CMakeLists.txt b/CMakeLists.txt
index 5c2e1ae5f0..9bc92c91d1 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -349,7 +349,7 @@ foreach(module ${config_module_list})
 endforeach()
 
 # Keep track of external shared libs required for modules
-set(module_shared_libraries "${module_shared_libraries}" CACHE INTERNAL "module_shared_libraries")
+set(module_external_libraries "${module_external_libraries}" CACHE INTERNAL "module_external_libraries")
 
 add_subdirectory(src/firmware/${OS})
 

From 6f8f8279b7abd9f0ee419de982fc5fecab28b82b Mon Sep 17 00:00:00 2001
From: Mark Charlebois <charlebm@gmail.com>
Date: Thu, 5 May 2016 11:37:14 -0700
Subject: [PATCH 0363/1230] Added support for fc_addon drivers

Signed-off-by: Mark Charlebois <charlebm@gmail.com>
---
 .../qurt_eagle_legacy_driver_default.cmake    |   6 +
 cmake/configs/qurt_sdflight_default.cmake     | 101 ---
 .../qurt/fc_addon/mpu_spi/CMakeLists.txt      |  70 ++
 .../qurt/fc_addon/mpu_spi/mpu9x50_main.cpp    | 664 ++++++++++++++++++
 .../qurt/fc_addon/mpu_spi/mpu9x50_params.c    |  89 +++
 .../qurt/fc_addon/rc_receiver/CMakeLists.txt  |  71 ++
 .../fc_addon/rc_receiver/rc_receiver_main.cpp | 347 +++++++++
 .../fc_addon/rc_receiver/rc_receiver_params.c |  53 ++
 8 files changed, 1300 insertions(+), 101 deletions(-)
 delete mode 100644 cmake/configs/qurt_sdflight_default.cmake
 create mode 100644 src/platforms/qurt/fc_addon/mpu_spi/CMakeLists.txt
 create mode 100644 src/platforms/qurt/fc_addon/mpu_spi/mpu9x50_main.cpp
 create mode 100644 src/platforms/qurt/fc_addon/mpu_spi/mpu9x50_params.c
 create mode 100644 src/platforms/qurt/fc_addon/rc_receiver/CMakeLists.txt
 create mode 100644 src/platforms/qurt/fc_addon/rc_receiver/rc_receiver_main.cpp
 create mode 100644 src/platforms/qurt/fc_addon/rc_receiver/rc_receiver_params.c

diff --git a/cmake/configs/qurt_eagle_legacy_driver_default.cmake b/cmake/configs/qurt_eagle_legacy_driver_default.cmake
index 46dfbd2df5..35f48f63c4 100644
--- a/cmake/configs/qurt_eagle_legacy_driver_default.cmake
+++ b/cmake/configs/qurt_eagle_legacy_driver_default.cmake
@@ -67,6 +67,12 @@ set(config_module_list
 	drivers/pwm_out_rc_in
 	drivers/qshell/qurt
 
+	#
+	# FC_ADDON drivers
+	#
+	#platforms/qurt/fc_addon/rc_receiver
+	#platforms/qurt/fc_addon/mpu_spi
+
 	#
 	# Libraries
 	#
diff --git a/cmake/configs/qurt_sdflight_default.cmake b/cmake/configs/qurt_sdflight_default.cmake
deleted file mode 100644
index 03b6a7353c..0000000000
--- a/cmake/configs/qurt_sdflight_default.cmake
+++ /dev/null
@@ -1,101 +0,0 @@
-include(qurt/px4_impl_qurt)
-
-if ("$ENV{HEXAGON_SDK_ROOT}" STREQUAL "")
-	message(FATAL_ERROR "Enviroment variable HEXAGON_SDK_ROOT must be set")
-else()
-	set(HEXAGON_SDK_ROOT $ENV{HEXAGON_SDK_ROOT})
-endif()
-
-set(CONFIG_SHMEM "1")
-
-set(config_generate_parameters_scope ALL)
-
-set(CMAKE_TOOLCHAIN_FILE ${CMAKE_SOURCE_DIR}/cmake/cmake_hexagon/toolchain/Toolchain-qurt.cmake)
-
-set(CMAKE_MODULE_PATH ${CMAKE_MODULE_PATH} "${CMAKE_SOURCE_DIR}/cmake/cmake_hexagon")
-
-set(config_module_list
-	#
-	# Board support modules
-	#
-	drivers/device
-	modules/sensors
-	platforms/posix/drivers/df_mpu9250_wrapper
-	platforms/posix/drivers/df_bmp280_wrapper
-	platforms/posix/drivers/df_hmc5883_wrapper
-	platforms/posix/drivers/df_trone_wrapper
-	platforms/posix/drivers/df_isl29501_wrapper
-
-	#
-	# System commands
-	#
-	systemcmds/param
-
-	#
-	# Estimation modules (EKF/ SO3 / other filters)
-	#
-	#modules/attitude_estimator_ekf
-	modules/ekf_att_pos_estimator
-	modules/attitude_estimator_q
-	modules/position_estimator_inav
-	modules/ekf2
-
-	#
-	# Vehicle Control
-	#
-	modules/mc_att_control
-	modules/mc_pos_control
-
-	#
-	# Library modules
-	#
-	modules/param
-	modules/systemlib
-	modules/systemlib/mixer
-	modules/uORB
-	modules/commander
-	modules/land_detector
-	modules/load_mon
-
-	#
-	# PX4 drivers
-	#
-	drivers/gps
-	drivers/pwm_out_rc_in
-	drivers/qshell/qurt
-
-	#
-	# Libraries
-	#
-	lib/controllib
-	lib/mathlib
-	lib/mathlib/math/filter
-	lib/geo
-	lib/ecl
-	lib/geo_lookup
-	lib/conversion
-	lib/terrain_estimation
-	lib/runway_takeoff
-	lib/tailsitter_recovery
-	lib/DriverFramework/framework
-
-	#
-	# QuRT port
-	#
-	platforms/common
-	platforms/qurt/px4_layer
-	platforms/posix/work_queue
-
-	#
-	# sources for muorb over fastrpc
-	#
-	modules/muorb/adsp
-	)
-
-set(config_df_driver_list
-	mpu9250
-	bmp280
-	hmc5883
-	trone
-	isl29501
-	)
diff --git a/src/platforms/qurt/fc_addon/mpu_spi/CMakeLists.txt b/src/platforms/qurt/fc_addon/mpu_spi/CMakeLists.txt
new file mode 100644
index 0000000000..d227c1f1cb
--- /dev/null
+++ b/src/platforms/qurt/fc_addon/mpu_spi/CMakeLists.txt
@@ -0,0 +1,70 @@
+############################################################################
+#
+#   Copyright (C) 2015 Mark Charlebois. All rights reserved.
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions
+# are met:
+#
+# 1. Redistributions of source code must retain the above copyright
+#    notice, this list of conditions and the following disclaimer.
+# 2. Redistributions in binary form must reproduce the above copyright
+#    notice, this list of conditions and the following disclaimer in
+#    the documentation and/or other materials provided with the
+#    distribution.
+# 3. Neither the name PX4 nor the names of its contributors may be
+#    used to endorse or promote products derived from this software
+#    without specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.
+#
+############################################################################
+
+set(TOOLS_ERROR_MSG 
+		"The FC_ADDON must be installed and the environment variable FC_ADDON must be set"
+		"(e.g. export FC_ADDON=${HOME}/Qualcomm/addon)")
+
+if ("$ENV{FC_ADDON}" STREQUAL "")
+	message(FATAL_ERROR ${TOOLS_ERROR_MSG})
+else()
+	set(FC_ADDON $ENV{FC_ADDON})
+endif()
+
+add_library(libmpu9x50 SHARED IMPORTED GLOBAL)
+set_target_properties(libmpu9x50 PROPERTIES IMPORTED_LOCATION ${FC_ADDON}/flight_controller/hexagon/libs/libmpu9x50.so)
+
+include_directories(
+	${FC_ADDON}/flight_controller/hexagon/inc
+	)
+
+px4_add_module(
+	MODULE platforms__qurt__fc_addon__mpu_spi
+	MAIN mpu9x50
+	STACK_MAIN 1200
+	COMPILE_FLAGS
+		-Os -Weffc++
+	SRCS
+		mpu9x50_main.cpp
+		mpu9x50_params.c
+	DEPENDS
+		platforms__common
+	)
+
+set(module_external_libraries 
+	${module_external_libraries}
+	libmpu9x50
+	CACHE INTERNAL "module_external_libraries"
+	)
+
+# vim: set noet ft=cmake fenc=utf-8 ff=unix : 
diff --git a/src/platforms/qurt/fc_addon/mpu_spi/mpu9x50_main.cpp b/src/platforms/qurt/fc_addon/mpu_spi/mpu9x50_main.cpp
new file mode 100644
index 0000000000..8e705eb25d
--- /dev/null
+++ b/src/platforms/qurt/fc_addon/mpu_spi/mpu9x50_main.cpp
@@ -0,0 +1,664 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2015 Mark Charlebois. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+#include <stdint.h>
+#include <fcntl.h>
+#include <unistd.h>
+#include <signal.h>
+
+#include <px4_tasks.h>
+
+#include <px4_log.h>
+#include <px4_getopt.h>
+
+// TODO-JYW:
+// Include references to the driver framework.  This will produce a duplicate definition
+// of qurt_log.  Use a #define for the qurt_log function to avoid redefinition.  This code
+// change must still be made.  Document the latter change to be clear.
+// #include <df_stubs.h>
+// #include <SPIDevObj.hpp>
+
+#include <uORB/uORB.h>
+#include <uORB/topics/parameter_update.h>
+#include <drivers/drv_accel.h>
+#include <drivers/drv_gyro.h>
+#include <drivers/drv_mag.h>
+#include <drivers/drv_hrt.h>
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+#include <semaphore.h>
+#include <mpu9x50.h>
+#ifdef __cplusplus
+}
+#endif
+
+/** driver 'main' command */
+extern "C" { __EXPORT int mpu9x50_main(int argc, char *argv[]); }
+
+#define MAX_LEN_DEV_PATH 32
+
+// TODO - need to load this from parameter file
+#define SPI_INT_GPIO 65  // GPIO pin for Data Ready Interrupt
+namespace mpu9x50
+{
+
+/** SPI device path that mpu9x50 is connected to */
+static char _device[MAX_LEN_DEV_PATH];
+
+/** flag indicating if mpu9x50 app is running */
+static bool _is_running = false;
+
+/** flag indicating if measurement thread should exit */
+static bool _task_should_exit = false;
+
+/** handle to the task main thread */
+static px4_task_t _task_handle = -1;
+
+/** IMU measurement data */
+static struct mpu9x50_data _data;
+
+static orb_advert_t _gyro_pub = nullptr;	/**< gyro data publication */
+static int _gyro_orb_class_instance;        /**< instance handle for gyro devices */
+static orb_advert_t _accel_pub = nullptr;	/**< accelerameter data publication */
+static int _accel_orb_class_instance;        /**< instance handle for accel devices */
+static orb_advert_t _mag_pub = nullptr;		/**< compass data publication */
+static int _mag_orb_class_instance;        /**< instance handle for mag devices */
+static int _params_sub;										/**< parameter update subscription */
+static struct gyro_report _gyro;					/**< gyro report */
+static struct accel_report _accel;				/**< accel report */
+static struct mag_report _mag;						/**< mag report */
+static struct gyro_calibration_s _gyro_sc;				/**< gyro scale */
+static struct accel_calibration_s _accel_sc;			/**< accel scale */
+static struct mag_calibration_s _mag_sc;					/**< mag scale */
+static enum gyro_lpf_e _gyro_lpf = MPU9X50_GYRO_LPF_20HZ;	/**< gyro lpf enum value */
+static enum acc_lpf_e _accel_lpf = MPU9X50_ACC_LPF_20HZ;	/**< accel lpf enum value */
+static enum gyro_sample_rate_e _imu_sample_rate = MPU9x50_SAMPLE_RATE_500HZ;	/**< IMU sample rate enum */
+
+struct {
+	param_t accel_x_offset;
+	param_t accel_x_scale;
+	param_t accel_y_offset;
+	param_t accel_y_scale;
+	param_t accel_z_offset;
+	param_t accel_z_scale;
+	param_t gyro_x_offset;
+	param_t gyro_x_scale;
+	param_t gyro_y_offset;
+	param_t gyro_y_scale;
+	param_t gyro_z_offset;
+	param_t gyro_z_scale;
+	param_t mag_x_offset;
+	param_t mag_x_scale;
+	param_t mag_y_offset;
+	param_t mag_y_scale;
+	param_t mag_z_offset;
+	param_t mag_z_scale;
+	param_t gyro_lpf_enum;
+	param_t accel_lpf_enum;
+	param_t imu_sample_rate_enum;
+} _params_handles;  /**< parameter handles */
+
+bool exit_mreasurement = false;
+
+
+/** Print out the usage information */
+static void usage();
+
+/** mpu9x50 stop measurement */
+static void stop();
+
+/** task main trampoline function */
+static void	task_main_trampoline(int argc, char *argv[]);
+
+/** mpu9x50 measurement thread primary entry point */
+static void task_main(int argc, char *argv[]);
+
+/**
+ * create and advertise all publicatoins
+ * @return   true on success, false otherwise
+ */
+static bool create_pubs();
+
+/** update all sensor reports with the latest sensor reading */
+static void update_reports();
+
+/** publish all sensor reports */
+static void publish_reports();
+
+/** update all parameters */
+static void parameters_update();
+
+/** initialize all parameter handles and load the initial parameter values */
+static void parameters_init();
+
+/** poll parameter update */
+static void parameter_update_poll();
+
+static int64_t _isr_data_ready_timestamp = 0;
+
+/**
+ * MPU9x50 data ready interrupt service routine
+ * @param[out]	context	address of the context data provided by user whill
+ *							registering the interrupt servcie routine
+ */
+static void *data_ready_isr(void *context);
+
+void *data_ready_isr(void *context)
+{
+	if (exit_mreasurement) {
+		return NULL;
+	}
+
+	_isr_data_ready_timestamp = hrt_absolute_time();
+	// send signal to measurement thread
+	px4_task_kill(_task_handle, SIGRTMIN);
+
+	return NULL;
+}
+
+void parameter_update_poll()
+{
+	bool updated;
+
+	/* Check if parameters have changed */
+	orb_check(_params_sub, &updated);
+
+	if (updated) {
+		struct parameter_update_s param_update;
+		orb_copy(ORB_ID(parameter_update), _params_sub, &param_update);
+		parameters_update();
+	}
+}
+
+void parameters_update()
+{
+	PX4_DEBUG("mpu9x50_parameters_update");
+	float v;
+	int v_int;
+
+	// accel params
+	if (param_get(_params_handles.accel_x_offset, &v) == 0) {
+		_accel_sc.x_offset = v;
+		PX4_DEBUG("mpu9x50_parameters_update accel_x_offset %f", v);
+	}
+
+	if (param_get(_params_handles.accel_x_scale, &v) == 0) {
+		_accel_sc.x_scale  = v;
+		PX4_DEBUG("mpu9x50_parameters_update accel_x_scale %f", v);
+	}
+
+	if (param_get(_params_handles.accel_y_offset, &v) == 0) {
+		_accel_sc.y_offset  = v;
+		PX4_DEBUG("mpu9x50_parameters_update accel_y_offset %f", v);
+	}
+
+	if (param_get(_params_handles.accel_y_scale, &v) == 0) {
+		_accel_sc.y_scale  = v;
+		PX4_DEBUG("mpu9x50_parameters_update accel_y_scale %f", v);
+	}
+
+	if (param_get(_params_handles.accel_z_offset, &v) == 0) {
+		_accel_sc.z_offset  = v;
+		PX4_DEBUG("mpu9x50_parameters_update accel_z_offset %f", v);
+	}
+
+	if (param_get(_params_handles.accel_z_scale, &v) == 0) {
+		_accel_sc.z_scale  = v;
+		PX4_DEBUG("mpu9x50_parameters_update accel_z_scale %f", v);
+	}
+
+	// gyro params
+	if (param_get(_params_handles.gyro_x_offset, &v) == 0) {
+		_gyro_sc.x_offset = v;
+		PX4_DEBUG("mpu9x50_parameters_update gyro_x_offset %f", v);
+	}
+
+	if (param_get(_params_handles.gyro_x_scale, &v) == 0) {
+		_gyro_sc.x_scale  = v;
+		PX4_DEBUG("mpu9x50_parameters_update gyro_x_scale %f", v);
+	}
+
+	if (param_get(_params_handles.gyro_y_offset, &v) == 0) {
+		_gyro_sc.y_offset  = v;
+		PX4_DEBUG("mpu9x50_parameters_update gyro_y_offset %f", v);
+	}
+
+	if (param_get(_params_handles.gyro_y_scale, &v) == 0) {
+		_gyro_sc.y_scale  = v;
+		PX4_DEBUG("mpu9x50_parameters_update gyro_y_scale %f", v);
+	}
+
+	if (param_get(_params_handles.gyro_z_offset, &v) == 0) {
+		_gyro_sc.z_offset  = v;
+		PX4_DEBUG("mpu9x50_parameters_update gyro_z_offset %f", v);
+	}
+
+	if (param_get(_params_handles.gyro_z_scale, &v) == 0) {
+		_gyro_sc.z_scale  = v;
+		PX4_DEBUG("mpu9x50_parameters_update gyro_z_scale %f", v);
+	}
+
+	// mag params
+	if (param_get(_params_handles.mag_x_offset, &v) == 0) {
+		_mag_sc.x_offset = v;
+		PX4_DEBUG("mpu9x50_parameters_update mag_x_offset %f", v);
+	}
+
+	if (param_get(_params_handles.mag_x_scale, &v) == 0) {
+		_mag_sc.x_scale  = v;
+		PX4_DEBUG("mpu9x50_parameters_update mag_x_scale %f", v);
+	}
+
+	if (param_get(_params_handles.mag_y_offset, &v) == 0) {
+		_mag_sc.y_offset  = v;
+		PX4_DEBUG("mpu9x50_parameters_update mag_y_offset %f", v);
+	}
+
+	if (param_get(_params_handles.mag_y_scale, &v) == 0) {
+		_mag_sc.y_scale  = v;
+		PX4_DEBUG("mpu9x50_parameters_update mag_y_scale %f", v);
+	}
+
+	if (param_get(_params_handles.mag_z_offset, &v) == 0) {
+		_mag_sc.z_offset  = v;
+		PX4_DEBUG("mpu9x50_parameters_update mag_z_offset %f", v);
+	}
+
+	if (param_get(_params_handles.mag_z_scale, &v) == 0) {
+		_mag_sc.z_scale  = v;
+		PX4_DEBUG("mpu9x50_parameters_update mag_z_scale %f", v);
+	}
+
+	// LPF params
+	if (param_get(_params_handles.gyro_lpf_enum, &v_int) == 0) {
+		if (v_int >= NUM_MPU9X50_GYRO_LPF) {
+			PX4_WARN("invalid gyro_lpf_enum %d use default %d", v_int, _gyro_lpf);
+
+		} else {
+			_gyro_lpf = (enum gyro_lpf_e)v_int;
+			PX4_DEBUG("mpu9x50_parameters_update gyro_lpf_enum %d", _gyro_lpf);
+		}
+	}
+
+	if (param_get(_params_handles.accel_lpf_enum, &v_int) == 0) {
+		if (v_int >= NUM_MPU9X50_ACC_LPF) {
+			PX4_WARN("invalid accel_lpf_enum %d use default %d", v_int, _accel_lpf);
+
+		} else {
+			_accel_lpf = (enum acc_lpf_e)v_int;
+			PX4_DEBUG("mpu9x50_parameters_update accel_lpf_enum %d", _accel_lpf);
+		}
+	}
+
+	if (param_get(_params_handles.imu_sample_rate_enum, &v_int) == 0) {
+		if (v_int >= NUM_MPU9X50_SAMPLE_RATE) {
+			PX4_WARN("invalid imu_sample_rate %d use default %d", v_int, _imu_sample_rate);
+
+		} else {
+			_imu_sample_rate = (enum gyro_sample_rate_e)v_int;
+			PX4_DEBUG("mpu9x50_parameters_update imu_sample_rate %d", _imu_sample_rate);
+		}
+	}
+}
+
+void parameters_init()
+{
+	_params_handles.accel_x_offset		=	param_find("CAL_ACC0_XOFF");
+	_params_handles.accel_x_scale		=	param_find("CAL_ACC0_XSCALE");
+	_params_handles.accel_y_offset		=	param_find("CAL_ACC0_YOFF");
+	_params_handles.accel_y_scale		=	param_find("CAL_ACC0_YSCALE");
+	_params_handles.accel_z_offset		=	param_find("CAL_ACC0_ZOFF");
+	_params_handles.accel_z_scale		=	param_find("CAL_ACC0_ZSCALE");
+
+	_params_handles.gyro_x_offset		=	param_find("CAL_GYRO0_XOFF");
+	_params_handles.gyro_x_scale		=	param_find("CAL_GYRO0_XSCALE");
+	_params_handles.gyro_y_offset		=	param_find("CAL_GYRO0_YOFF");
+	_params_handles.gyro_y_scale		=	param_find("CAL_GYRO0_YSCALE");
+	_params_handles.gyro_z_offset		=	param_find("CAL_GYRO0_ZOFF");
+	_params_handles.gyro_z_scale		=	param_find("CAL_GYRO0_ZSCALE");
+
+	_params_handles.mag_x_offset		=	param_find("CAL_MAG0_XOFF");
+	_params_handles.mag_x_scale		=	param_find("CAL_MAG0_XSCALE");
+	_params_handles.mag_y_offset		=	param_find("CAL_MAG0_YOFF");
+	_params_handles.mag_y_scale		=	param_find("CAL_MAG0_YSCALE");
+	_params_handles.mag_z_offset		=	param_find("CAL_MAG0_ZOFF");
+	_params_handles.mag_z_scale		=	param_find("CAL_MAG0_ZSCALE");
+
+	_params_handles.gyro_lpf_enum		=	param_find("MPU_GYRO_LPF_ENUM");
+	_params_handles.accel_lpf_enum		=	param_find("MPU_ACC_LPF_ENUM");
+
+	_params_handles.imu_sample_rate_enum	=	param_find("MPU_SAMPLE_RATE_ENUM");
+
+	parameters_update();
+}
+
+bool create_pubs()
+{
+	// initialize the reports
+	memset(&_gyro, 0, sizeof(struct gyro_report));
+	memset(&_accel, 0, sizeof(struct accel_report));
+	memset(&_mag, 0, sizeof(struct mag_report));
+
+	_gyro_pub = orb_advertise_multi(ORB_ID(sensor_gyro), &_gyro,
+					  &_gyro_orb_class_instance, ORB_PRIO_HIGH - 1);
+	if (_gyro_pub == nullptr) {
+		PX4_ERR("sensor_gyro advert fail");
+		return false;
+	}
+
+	_accel_pub = orb_advertise_multi(ORB_ID(sensor_accel), &_accel,
+					  &_accel_orb_class_instance, ORB_PRIO_HIGH - 1);
+	if (_accel_pub == nullptr) {
+		PX4_ERR("sensor_accel advert fail");
+		return false;
+	}
+
+	_mag_pub = orb_advertise_multi(ORB_ID(sensor_mag), &_mag,
+					  &_mag_orb_class_instance, ORB_PRIO_HIGH - 1);
+	if (_mag_pub == nullptr) {
+		PX4_ERR("sensor_mag advert fail");
+		return false;
+	}
+
+	return true;
+}
+
+void update_reports()
+{
+	_gyro.timestamp = _data.timestamp;
+	_gyro.x = ((_data.gyro_raw[0] * _data.gyro_scaling) - _gyro_sc.x_offset) * _gyro_sc.x_scale;
+	_gyro.y = ((_data.gyro_raw[1] * _data.gyro_scaling) - _gyro_sc.y_offset) * _gyro_sc.y_scale;
+	_gyro.z = ((_data.gyro_raw[2] * _data.gyro_scaling) - _gyro_sc.z_offset) * _gyro_sc.z_scale;
+	_gyro.temperature = _data.temperature;
+	_gyro.range_rad_s = _data.gyro_range_rad_s;
+	_gyro.scaling = _data.gyro_scaling;
+	_gyro.x_raw = _data.gyro_raw[0];
+	_gyro.y_raw = _data.gyro_raw[1];
+	_gyro.z_raw = _data.gyro_raw[2];
+	_gyro.temperature_raw = _data.temperature_raw;
+
+	_accel.timestamp = _data.timestamp;
+	_accel.x = ((_data.accel_raw[0] * _data.accel_scaling) - _accel_sc.x_offset) * _accel_sc.x_scale;
+	_accel.y = ((_data.accel_raw[1] * _data.accel_scaling) - _accel_sc.y_offset) * _accel_sc.y_scale;
+	_accel.z = ((_data.accel_raw[2] * _data.accel_scaling) - _accel_sc.z_offset) * _accel_sc.z_scale;
+	_accel.temperature = _data.temperature;
+	_accel.range_m_s2 = _data.accel_range_m_s2;
+	_accel.scaling = _data.accel_scaling;
+	_accel.x_raw = _data.accel_raw[0];
+	_accel.y_raw = _data.accel_raw[1];
+	_accel.z_raw = _data.accel_raw[2];
+	_accel.temperature_raw = _data.temperature_raw;
+
+	if (_data.mag_data_ready) {
+		_mag.timestamp = _data.timestamp;
+		_mag.x = ((_data.mag_raw[0] * _data.mag_scaling) - _mag_sc.x_offset) * _mag_sc.x_scale;
+		_mag.y = ((_data.mag_raw[1] * _data.mag_scaling) - _mag_sc.y_offset) * _mag_sc.y_scale;
+		_mag.z = ((_data.mag_raw[2] * _data.mag_scaling) - _mag_sc.z_offset) * _mag_sc.z_scale;
+		_mag.range_ga = _data.mag_range_ga;
+		_mag.scaling = _data.mag_scaling;
+		_mag.temperature = _data.temperature;
+		_mag.x_raw = _data.mag_raw[0];
+		_mag.y_raw = _data.mag_raw[1];
+		_mag.z_raw = _data.mag_raw[2];
+	}
+}
+
+void publish_reports()
+{
+	if (orb_publish(ORB_ID(sensor_gyro), _gyro_pub, &_gyro) != OK) {
+		PX4_WARN("failed to publish gyro report");
+
+	} else {
+		//PX4_DEBUG("MPU_GYRO_RAW: %d %d %d", _gyro.x_raw, _gyro.y_raw, _gyro.z_raw)
+		//PX4_DEBUG("MPU_GYRO: %f %f %f", _gyro.x, _gyro.y, _gyro.z)
+	}
+
+	if (orb_publish(ORB_ID(sensor_accel), _accel_pub, &_accel) != OK) {
+		PX4_WARN("failed to publish accel report");
+
+	} else {
+		//PX4_DEBUG("MPU_ACCEL_RAW: %d %d %d", _accel.x_raw, _accel.y_raw, _accel.z_raw)
+		//PX4_DEBUG("MPU_ACCEL: %f %f %f", _accel.x, _accel.y, _accel.z)
+	}
+
+	if (_data.mag_data_ready) {
+		if (orb_publish(ORB_ID(sensor_mag), _mag_pub, &_mag) != OK) {
+			PX4_WARN("failed to publish mag report");
+
+		} else {
+			//PX4_DEBUG("MPU_MAG_RAW: %d %d %d", _mag.x_raw, _mag.y_raw, _mag.z_raw)
+			//PX4_DEBUG("MPU_MAG: %f %f %f", _mag.x, _mag.y, _mag.z)
+		}
+	}
+}
+
+void task_main(int argc, char *argv[])
+{
+	PX4_WARN("enter mpu9x50 task_main");
+
+	sigset_t set;
+	int      sig = 0;
+	int rv;
+	exit_mreasurement = false;
+
+	parameters_init();
+
+	// create the mpu9x50 default configuration structure
+	struct mpu9x50_config config = {
+		.gyro_lpf = _gyro_lpf,
+		.acc_lpf  = _accel_lpf,
+		.gyro_fsr = MPU9X50_GYRO_FSR_500DPS,
+		.acc_fsr  = MPU9X50_ACC_FSR_4G,
+		.gyro_sample_rate = _imu_sample_rate,
+		.compass_enabled = true,
+		.compass_sample_rate = MPU9x50_COMPASS_SAMPLE_RATE_100HZ,
+		.spi_dev_path = _device,
+	};
+
+	// TODO-JYW:
+	// manually register with the DriverFramework to allow this driver to
+	// be found by other modules
+//	DriverFramework::StubSensor<DriverFramework::SPIDevObj> stub_sensor("mpu9x50", "/dev/accel0", "/dev/accel");
+
+	if (mpu9x50_initialize(&config) != 0) {
+		PX4_WARN("error initializing mpu9x50. Quit!");
+		goto exit;
+	}
+
+	if (mpu9x50_register_interrupt(SPI_INT_GPIO, &mpu9x50::data_ready_isr, NULL)
+	    //if (mpu9x50_register_interrupt(SPI_INT_GPIO, &mpu9x50::data_ready_isr, NULL)
+
+	    != 0) {
+		PX4_WARN("error registering data ready interrupt. Quit!");
+		goto exit;
+	}
+
+	// create all uorb publications
+	if (!create_pubs()) {
+		goto exit;
+	}
+
+	// subscribe to parameter_update topic
+	_params_sub = orb_subscribe(ORB_ID(parameter_update));
+
+	// initialize signal
+	sigemptyset(&set);
+	sigaddset(&set, SIGRTMIN);
+
+	while (!_task_should_exit) {
+		// wait on signal
+		rv = sigwait(&set, &sig);
+
+		// check if we are waken up by the proper signal
+		if (rv != 0 || sig != SIGRTMIN) {
+			PX4_WARN("mpu9x50 sigwait failed rv %d sig %d", rv, sig);
+			continue;
+		}
+
+		// read new IMU data and store the results in data
+		if (mpu9x50_get_data(&_data) != 0) {
+			PX4_WARN("mpu9x50_get_data() failed");
+			continue;
+		}
+
+		// since the context switch takes long time, override the timestamp provided by mpu9x50_get_data()
+		// with the ts of isr.
+		// Note: This is ok for MPU sample rate of < 1000; Higer than 1000 Sample rates will be an issue
+		// as the data is not consistent.
+		_data.timestamp = _isr_data_ready_timestamp;
+
+		// poll parameter update
+		parameter_update_poll();
+
+		// data is ready
+		update_reports();
+
+		// publish all sensor reports
+		publish_reports();
+
+	}
+
+	exit_mreasurement = true;
+
+exit:
+	PX4_WARN("closing mpu9x50");
+	mpu9x50_close();
+}
+
+/** mpu9x50 main entrance */
+void task_main_trampoline(int argc, char *argv[])
+{
+	PX4_WARN("task_main_trampoline");
+	task_main(argc, argv);
+}
+
+void start()
+{
+	ASSERT(_task_handle == -1);
+
+	/* start the task */
+	_task_handle = px4_task_spawn_cmd("mpu9x50_main",
+					  SCHED_DEFAULT,
+					  SCHED_PRIORITY_MAX,
+					  1500,
+					  (px4_main_t)&task_main_trampoline,
+					  nullptr);
+
+	if (_task_handle < 0) {
+		warn("mpu9x50_main task start failed");
+		return;
+	}
+
+	_is_running = true;
+}
+
+void stop()
+{
+	// TODO - set thread exit signal to terminate the task main thread
+
+	_is_running = false;
+	_task_handle = -1;
+}
+
+void usage()
+{
+	PX4_WARN("missing command: try 'start', 'stop', 'status'");
+	PX4_WARN("options:");
+	PX4_WARN("    -D device");
+}
+
+}; // namespace mpu9x50
+
+
+int mpu9x50_main(int argc, char *argv[])
+{
+	const char *device = NULL;
+	int ch;
+	int myoptind = 1;
+	const char *myoptarg = NULL;
+
+	while ((ch = px4_getopt(argc, argv, "D:", &myoptind, &myoptarg)) != EOF) {
+		switch (ch) {
+		case 'D':
+			device = myoptarg;
+			break;
+
+		default:
+			mpu9x50::usage();
+			return 1;
+		}
+	}
+
+	// Check on required arguments
+	if (device == NULL || strlen(device) == 0) {
+		mpu9x50::usage();
+		return 1;
+	}
+
+	memset(mpu9x50::_device, 0, MAX_LEN_DEV_PATH);
+	strncpy(mpu9x50::_device, device, strlen(device));
+
+	const char *verb = argv[myoptind];
+
+	/*
+	 * Start/load the driver.
+	 */
+	if (!strcmp(verb, "start")) {
+		if (mpu9x50::_is_running) {
+			PX4_WARN("mpu9x50 already running");
+			return 1;
+		}
+
+		mpu9x50::start();
+
+	} else if (!strcmp(verb, "stop")) {
+		if (mpu9x50::_is_running) {
+			PX4_WARN("mpu9x50 is not running");
+			return 1;
+		}
+
+		mpu9x50::stop();
+
+	} else if (!strcmp(verb, "status")) {
+		PX4_WARN("mpu9x50 is %s", mpu9x50::_is_running ? "running" : "stopped");
+		return 0;
+
+	} else {
+		mpu9x50::usage();
+		return 1;
+	}
+
+	return 0;
+}
diff --git a/src/platforms/qurt/fc_addon/mpu_spi/mpu9x50_params.c b/src/platforms/qurt/fc_addon/mpu_spi/mpu9x50_params.c
new file mode 100644
index 0000000000..36cacdc566
--- /dev/null
+++ b/src/platforms/qurt/fc_addon/mpu_spi/mpu9x50_params.c
@@ -0,0 +1,89 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2015 Mark Charlebois. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file mpu9x50_params.c
+ *
+ * Parameters defined by the mpu9x50 driver
+ */
+
+#include <px4_config.h>
+#include <systemlib/param/param.h>
+
+/**
+ * IMU Low pass filter enum value for Gyro
+ *
+ * Acceptable values:
+ *
+ * MPU9X50_GYRO_LPF_250HZ = 0,
+ * MPU9X50_GYRO_LPF_184HZ = 1,
+ * MPU9X50_GYRO_LPF_92HZ = 2,
+ * MPU9X50_GYRO_LPF_41HZ = 3,
+ * MPU9X50_GYRO_LPF_20HZ = 4 (default),
+ * MPU9X50_GYRO_LPF_10HZ = 5,
+ * MPU9X50_GYRO_LPF_5HZ = 6,
+ * MPU9X50_GYRO_LPF_3600HZ_NOLPF = 7,
+ *
+ * @group MPU9x50 Configuration
+ */
+PARAM_DEFINE_INT32(MPU_GYRO_LPF_ENUM, 4);
+
+/**
+ * IMU Low pass filter enum value for Accelerometer
+ *
+ * Acceptable values:
+ *
+ *  MPU9X50_ACC_LPF_460HZ = 0,
+ *  MPU9X50_ACC_LPF_184HZ = 1,
+ *  MPU9X50_ACC_LPF_92HZ = 2,
+ *  MPU9X50_ACC_LPF_41HZ = 3,
+ *  MPU9X50_ACC_LPF_20HZ = 4 (default),
+ *  MPU9X50_ACC_LPF_10HZ = 5,
+ *  MPU9X50_ACC_LPF_5HZ = 6,
+ *  MPU9X50_ACC_LPF_460HZ_NOLPF = 7,
+ *
+ * @group MPU9x50 Configuration
+ */
+PARAM_DEFINE_INT32(MPU_ACC_LPF_ENUM, 4);
+
+/**
+ * IMU sample rate in Hz
+ * acceptable values:
+ * MPU9x50_SAMPLE_RATE_100HZ = 0,
+ * MPU9x50_SAMPLE_RATE_200HZ,
+ * MPU9x50_SAMPLE_RATE_500HZ, (default)
+ * MPU9x50_SAMPLE_RATE_1000HZ,
+ *
+ * @group MPU9x50 Configuration
+ */
+PARAM_DEFINE_INT32(MPU_SAMPLE_RATE_ENUM, 2);
diff --git a/src/platforms/qurt/fc_addon/rc_receiver/CMakeLists.txt b/src/platforms/qurt/fc_addon/rc_receiver/CMakeLists.txt
new file mode 100644
index 0000000000..3b7399f492
--- /dev/null
+++ b/src/platforms/qurt/fc_addon/rc_receiver/CMakeLists.txt
@@ -0,0 +1,71 @@
+############################################################################
+#
+#   Copyright (c) 2016 Mark Charlebois. All rights reserved.
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions
+# are met:
+#
+# 1. Redistributions of source code must retain the above copyright
+#    notice, this list of conditions and the following disclaimer.
+# 2. Redistributions in binary form must reproduce the above copyright
+#    notice, this list of conditions and the following disclaimer in
+#    the documentation and/or other materials provided with the
+#    distribution.
+# 3. Neither the name AtlFlight nor the names of its contributors may be
+#    used to endorse or promote products derived from this software
+#    without specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.
+#
+############################################################################
+
+set(TOOLS_ERROR_MSG 
+		"The FC_ADDON must be installed and the environment variable FC_ADDON must be set"
+		"(e.g. export FC_ADDON=${HOME}/Qualcomm/addon)")
+
+if ("$ENV{FC_ADDON}" STREQUAL "")
+	message(FATAL_ERROR ${TOOLS_ERROR_MSG})
+else()
+	set(FC_ADDON $ENV{FC_ADDON})
+endif()
+
+include_directories(
+	${FC_ADDON}/flight_controller/hexagon/inc
+	)
+
+add_library(librc_receiver SHARED IMPORTED GLOBAL)
+set_target_properties(librc_receiver PROPERTIES IMPORTED_LOCATION ${FC_ADDON}/flight_controller/hexagon/libs/librc_receiver.so)
+
+px4_add_module(
+        MODULE platforms__qurt__fc_addon__rc_receiver
+        MAIN rc_receiver
+	STACK_MAIN 1200
+        COMPILE_FLAGS
+                -Os -Weffc++
+        SRCS
+                rc_receiver_main.cpp
+		rc_receiver_params.c
+
+        DEPENDS
+                platforms__common
+        )
+
+set(module_external_libraries 
+	${module_external_libraries}
+	librc_receiver
+	CACHE INTERNAL "module_external_libraries"
+	)
+
+# vim: set noet ft=cmake fenc=utf-8 ff=unix : 
diff --git a/src/platforms/qurt/fc_addon/rc_receiver/rc_receiver_main.cpp b/src/platforms/qurt/fc_addon/rc_receiver/rc_receiver_main.cpp
new file mode 100644
index 0000000000..7ae0a4a8f6
--- /dev/null
+++ b/src/platforms/qurt/fc_addon/rc_receiver/rc_receiver_main.cpp
@@ -0,0 +1,347 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2015 Mark Charlebois. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+#include <px4_getopt.h>
+#include <px4_tasks.h>
+#include <px4_log.h>
+#include <string.h>
+#include <drivers/drv_hrt.h>
+#include <uORB/topics/input_rc.h>
+#include <uORB/topics/parameter_update.h>
+#include <drivers/drv_rc_input.h>
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+#include <semaphore.h>
+#include "rc_receiver_api.h"
+#ifdef __cplusplus
+}
+#endif
+
+/** driver 'main' command */
+extern "C" { __EXPORT int rc_receiver_main(int argc, char *argv[]); }
+
+#define MAX_LEN_DEV_PATH 32
+
+namespace rc_receiver
+{
+
+/** Threshold value to detect rc receiver signal lost in millisecond */
+#define SIGNAL_LOST_THRESHOLD_MS   500
+
+/** serial device path that rc receiver is connected to */
+static char _device[MAX_LEN_DEV_PATH];
+
+/** flag indicating if rc_receiver module is running */
+static bool _is_running = false;
+
+/** flag indicating if task thread should exit */
+static bool _task_should_exit = false;
+
+/** handle to the task main thread */
+static px4_task_t _task_handle = -1;
+
+/** RC receiver type */
+static enum RC_RECEIVER_TYPES _rc_receiver_type;
+
+/** RC input channel value array accomondating up to RC_INPUT_MAX_CHANNELS */
+static uint16_t rc_inputs[input_rc_s::RC_INPUT_MAX_CHANNELS];
+
+/** rc_input uorb topic publication handle */
+static orb_advert_t _input_rc_pub = nullptr;
+
+/** rc_input uorb topic data */
+static struct input_rc_s _rc_in;
+
+/**< parameter update subscription */
+static int _params_sub;
+
+struct {
+	param_t rc_receiver_type;
+} _params_handles;  /**< parameter handles */
+
+/** Print out the usage information */
+static void usage();
+
+/** mpu9x50 start measurement */
+static void start();
+
+/** mpu9x50 stop measurement */
+static void stop();
+
+/** task main trampoline function */
+static void	task_main_trampoline(int argc, char *argv[]);
+
+/** mpu9x50 measurement thread primary entry point */
+static void task_main(int argc, char *argv[]);
+
+/** update all parameters */
+static void parameters_update();
+
+/** poll parameter update */
+static void parameter_update_poll();
+
+void parameters_update()
+{
+	PX4_DEBUG("rc_receiver_parameters_update");
+	float v;
+
+	// accel params
+	if (param_get(_params_handles.rc_receiver_type, &v) == 0) {
+		_rc_receiver_type = (enum RC_RECEIVER_TYPES)v;
+		PX4_DEBUG("rc_receiver_parameters_update rc_receiver_type %f", v);
+	}
+}
+
+void parameters_init()
+{
+	_params_handles.rc_receiver_type	=	param_find("RC_RECEIVER_TYPE");
+
+	parameters_update();
+}
+
+void parameter_update_poll()
+{
+	bool updated;
+
+	/* Check if parameters have changed */
+	orb_check(_params_sub, &updated);
+
+	if (updated) {
+		struct parameter_update_s param_update;
+		orb_copy(ORB_ID(parameter_update), _params_sub, &param_update);
+		parameters_update();
+	}
+}
+
+void start()
+{
+	ASSERT(_task_handle == -1);
+
+	/* start the task */
+	_task_handle = px4_task_spawn_cmd("rc_receiver_main",
+					  SCHED_DEFAULT,
+					  SCHED_PRIORITY_MAX,
+					  1500,
+					  (px4_main_t)&task_main_trampoline,
+					  nullptr);
+
+	if (_task_handle < 0) {
+		warn("task start failed");
+		return;
+	}
+
+	_is_running = true;
+}
+
+void stop()
+{
+	// TODO - set thread exit signal to terminate the task main thread
+
+	_is_running = false;
+	_task_handle = -1;
+}
+
+void task_main_trampoline(int argc, char *argv[])
+{
+	PX4_WARN("task_main_trampoline");
+	task_main(argc, argv);
+}
+
+void task_main(int argc, char *argv[])
+{
+	PX4_WARN("enter rc_receiver task_main");
+	uint32_t fd;
+
+	// clear the rc_input report for topic advertise
+	memset(&_rc_in, 0, sizeof(struct input_rc_s));
+
+	_input_rc_pub = orb_advertise(ORB_ID(input_rc), &_rc_in);
+
+	if (_input_rc_pub == nullptr) {
+		PX4_WARN("failed to advertise input_rc uorb topic. Quit!");
+		return ;
+	}
+
+	// subscribe to parameter_update topic
+	_params_sub = orb_subscribe(ORB_ID(parameter_update));
+
+	// Open the RC receiver device on the specified serial port
+	fd = rc_receiver_open(_rc_receiver_type, _device);
+
+	if (fd <= 0) {
+		PX4_WARN("failed to open rc receiver type %d dev %s. Quit!",
+			 _rc_receiver_type, _device);
+		return ;
+	}
+
+	// Continuously receive RC packet from serial device, until task is signaled
+	// to exit
+	uint32_t num_channels;
+	uint64_t ts = hrt_absolute_time();
+	int ret;
+	int counter = 0;
+
+	_rc_in.timestamp_last_signal = ts;
+
+	while (!_task_should_exit) {
+		// poll parameter update
+		parameter_update_poll();
+
+		// read RC packet from serial device in blocking mode.
+		num_channels = input_rc_s::RC_INPUT_MAX_CHANNELS;
+
+		ret = rc_receiver_get_packet(fd, rc_inputs, &num_channels);
+		ts = hrt_absolute_time();
+
+		_rc_in.input_source = input_rc_s::RC_INPUT_SOURCE_QURT;
+
+		if (ret < 0) {
+			// enum RC_RECEIVER_ERRORS error_code = rc_receiver_get_last_error(fd);
+			// PX4_WARN("RC packet receiving timed out. error code %d", error_code);
+
+			uint64_t time_diff_us = ts - _rc_in.timestamp_last_signal;
+
+			if (time_diff_us > SIGNAL_LOST_THRESHOLD_MS * 1000) {
+				_rc_in.rc_lost = true;
+
+				if (++counter == 500) {
+					PX4_WARN("RC signal lost for %u ms", time_diff_us / 1000);
+					counter = 0;
+				}
+
+			} else {
+				continue;
+			}
+		}
+
+		// populate the input_rc_s structure
+		if (ret == 0) {
+			_rc_in.timestamp_publication = ts;
+			_rc_in.timestamp_last_signal = _rc_in.timestamp_publication;
+			_rc_in.channel_count = num_channels;
+
+			// TODO - need to add support for RSSI, failsafe mode
+			_rc_in.rssi = RC_INPUT_RSSI_MAX;
+			_rc_in.rc_failsafe = false;
+			_rc_in.rc_lost = false;
+			_rc_in.rc_lost_frame_count = 0;
+			_rc_in.rc_total_frame_count = 1;
+		}
+
+		for (uint32_t i = 0; i < num_channels; i++) {
+			// Scale the Spektrum DSM value to ppm encoding. This is for the
+			// consistency with PX4 which internally translates DSM to PPM.
+			// See modules/px4iofirmware/dsm.c for details
+			// NOTE: rc_receiver spektrum driver outputs the data in 10bit DSM
+			// format, so we need to double the channel value before the scaling
+			_rc_in.values[i] = ((((int)(rc_inputs[i]*2) - 1024) * 1000) / 1700) + 1500;
+		}
+
+		orb_publish(ORB_ID(input_rc), _input_rc_pub, &_rc_in);
+	}
+
+	rc_receiver_close(fd);
+}
+
+void usage()
+{
+	PX4_WARN("missing command: try 'start', 'stop', 'status'");
+	PX4_WARN("options:");
+	PX4_WARN("    -D device");
+}
+
+} // namespace rc_receiver
+
+int rc_receiver_main(int argc, char *argv[])
+{
+	const char *device = NULL;
+	int ch;
+	int myoptind = 1;
+	const char *myoptarg = NULL;
+
+	/* jump over start/off/etc and look at options first */
+	while ((ch = px4_getopt(argc, argv, "D:", &myoptind, &myoptarg)) != EOF) {
+		switch (ch) {
+		case 'D':
+			device = myoptarg;
+			break;
+
+		default:
+			rc_receiver::usage();
+			return 1;
+		}
+	}
+
+	// Check on required arguments
+	if (device == NULL || strlen(device) == 0) {
+		rc_receiver::usage();
+		return 1;
+	}
+
+	memset(rc_receiver::_device, 0, MAX_LEN_DEV_PATH);
+	strncpy(rc_receiver::_device, device, strlen(device));
+
+	const char *verb = argv[myoptind];
+
+	/*
+	 * Start/load the driver.
+	 */
+	if (!strcmp(verb, "start")) {
+		if (rc_receiver::_is_running) {
+			PX4_WARN("rc_receiver already running");
+			return 1;
+		}
+
+		rc_receiver::start();
+
+	} else if (!strcmp(verb, "stop")) {
+		if (rc_receiver::_is_running) {
+			PX4_WARN("rc_receiver is not running");
+			return 1;
+		}
+
+		rc_receiver::stop();
+
+	} else if (!strcmp(verb, "status")) {
+		PX4_WARN("rc_receiver is %s", rc_receiver::_is_running ? "running" : "stopped");
+		return 0;
+
+	} else {
+		rc_receiver::usage();
+		return 1;
+	}
+
+	return 0;
+}
diff --git a/src/platforms/qurt/fc_addon/rc_receiver/rc_receiver_params.c b/src/platforms/qurt/fc_addon/rc_receiver/rc_receiver_params.c
new file mode 100644
index 0000000000..d7a43550e0
--- /dev/null
+++ b/src/platforms/qurt/fc_addon/rc_receiver/rc_receiver_params.c
@@ -0,0 +1,53 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2015 Mark Charlebois. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file rc_receiver_params.c
+ *
+ * Parameters defined by the rc_receiver driver
+ */
+
+#include <px4_config.h>
+#include <systemlib/param/param.h>
+
+/**
+ * RC receiver type
+ *
+ * Acceptable values:
+ *
+ * - RC_RECEIVER_SPEKTRUM = 1,
+ * - RC_RECEIVER_LEMONRX = 2,
+ *
+ * @group RC Receiver Configuration
+ */
+PARAM_DEFINE_INT32(RC_RECEIVER_TYPE, 1);

From b8c22718afbedf56cd6b9cb1b499b4c4fc32913c Mon Sep 17 00:00:00 2001
From: Mark Charlebois <charlebm@gmail.com>
Date: Thu, 5 May 2016 11:47:28 -0700
Subject: [PATCH 0364/1230] Added build of rc_receiver to
 qurt_eagle_legacy_driver_default

Signed-off-by: Mark Charlebois <charlebm@gmail.com>
---
 cmake/configs/qurt_eagle_legacy_driver_default.cmake | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/cmake/configs/qurt_eagle_legacy_driver_default.cmake b/cmake/configs/qurt_eagle_legacy_driver_default.cmake
index 35f48f63c4..1c7c584c70 100644
--- a/cmake/configs/qurt_eagle_legacy_driver_default.cmake
+++ b/cmake/configs/qurt_eagle_legacy_driver_default.cmake
@@ -70,7 +70,7 @@ set(config_module_list
 	#
 	# FC_ADDON drivers
 	#
-	#platforms/qurt/fc_addon/rc_receiver
+	platforms/qurt/fc_addon/rc_receiver
 	#platforms/qurt/fc_addon/mpu_spi
 
 	#

From 0b3cd3d0088df108c06fb03ae7b7e2a8159bb861 Mon Sep 17 00:00:00 2001
From: Mark Charlebois <charlebm@gmail.com>
Date: Thu, 5 May 2016 11:51:41 -0700
Subject: [PATCH 0365/1230] Restored cmake/configs/qurt_sdflight_default.cmake

Signed-off-by: Mark Charlebois <charlebm@gmail.com>
---
 cmake/configs/qurt_sdflight_default.cmake | 100 ++++++++++++++++++++++
 1 file changed, 100 insertions(+)
 create mode 100644 cmake/configs/qurt_sdflight_default.cmake

diff --git a/cmake/configs/qurt_sdflight_default.cmake b/cmake/configs/qurt_sdflight_default.cmake
new file mode 100644
index 0000000000..3ea8ad37b4
--- /dev/null
+++ b/cmake/configs/qurt_sdflight_default.cmake
@@ -0,0 +1,100 @@
+include(qurt/px4_impl_qurt)
+
+if ("$ENV{HEXAGON_SDK_ROOT}" STREQUAL "")
+	message(FATAL_ERROR "Enviroment variable HEXAGON_SDK_ROOT must be set")
+else()
+	set(HEXAGON_SDK_ROOT $ENV{HEXAGON_SDK_ROOT})
+endif()
+
+set(CONFIG_SHMEM "1")
+
+set(config_generate_parameters_scope ALL)
+
+set(CMAKE_TOOLCHAIN_FILE ${CMAKE_SOURCE_DIR}/cmake/cmake_hexagon/toolchain/Toolchain-qurt.cmake)
+
+set(CMAKE_MODULE_PATH ${CMAKE_MODULE_PATH} "${CMAKE_SOURCE_DIR}/cmake/cmake_hexagon")
+
+set(config_module_list
+	#
+	# Board support modules
+	#
+	drivers/device
+	modules/sensors
+	platforms/posix/drivers/df_mpu9250_wrapper
+	platforms/posix/drivers/df_bmp280_wrapper
+	platforms/posix/drivers/df_hmc5883_wrapper
+	platforms/posix/drivers/df_trone_wrapper
+	platforms/posix/drivers/df_isl29501_wrapper
+
+	#
+	# System commands
+	#
+	systemcmds/param
+
+	#
+	# Estimation modules (EKF/ SO3 / other filters)
+	#
+	#modules/attitude_estimator_ekf
+	modules/ekf_att_pos_estimator
+	modules/attitude_estimator_q
+	modules/position_estimator_inav
+	modules/ekf2
+
+	#
+	# Vehicle Control
+	#
+	modules/mc_att_control
+	modules/mc_pos_control
+
+	#
+	# Library modules
+	#
+	modules/param
+	modules/systemlib
+	modules/systemlib/mixer
+	modules/uORB
+	modules/commander
+	modules/land_detector
+
+	#
+	# PX4 drivers
+	#
+	drivers/gps
+	drivers/uart_esc
+	drivers/qshell/qurt
+
+	#
+	# Libraries
+	#
+	lib/controllib
+	lib/mathlib
+	lib/mathlib/math/filter
+	lib/geo
+	lib/ecl
+	lib/geo_lookup
+	lib/conversion
+	lib/terrain_estimation
+	lib/runway_takeoff
+	lib/tailsitter_recovery
+	lib/DriverFramework/framework
+
+	#
+	# QuRT port
+	#
+	platforms/common
+	platforms/qurt/px4_layer
+	platforms/posix/work_queue
+
+	#
+	# sources for muorb over fastrpc
+	#
+	modules/muorb/adsp
+	)
+
+set(config_df_driver_list
+	mpu9250
+	bmp280
+	hmc5883
+	trone
+	isl29501
+	)

From af1e20a1d02f27184d9cb5e2cd5c3310f5009aa3 Mon Sep 17 00:00:00 2001
From: Mark Charlebois <charlebm@gmail.com>
Date: Thu, 5 May 2016 12:49:20 -0700
Subject: [PATCH 0366/1230] Added fc_addon uart_esc support

Signed-off-by: Mark Charlebois <charlebm@gmail.com>
---
 .../qurt_eagle_legacy_driver_default.cmake    |   1 +
 .../qurt/fc_addon/uart_esc/CMakeLists.txt     |  71 +++
 .../qurt/fc_addon/uart_esc/uart_esc_main.cpp  | 518 ++++++++++++++++++
 .../qurt/fc_addon/uart_esc/uart_esc_params.c  | 114 ++++
 4 files changed, 704 insertions(+)
 create mode 100644 src/platforms/qurt/fc_addon/uart_esc/CMakeLists.txt
 create mode 100644 src/platforms/qurt/fc_addon/uart_esc/uart_esc_main.cpp
 create mode 100644 src/platforms/qurt/fc_addon/uart_esc/uart_esc_params.c

diff --git a/cmake/configs/qurt_eagle_legacy_driver_default.cmake b/cmake/configs/qurt_eagle_legacy_driver_default.cmake
index 1c7c584c70..ad73ffb12d 100644
--- a/cmake/configs/qurt_eagle_legacy_driver_default.cmake
+++ b/cmake/configs/qurt_eagle_legacy_driver_default.cmake
@@ -71,6 +71,7 @@ set(config_module_list
 	# FC_ADDON drivers
 	#
 	platforms/qurt/fc_addon/rc_receiver
+	platforms/qurt/fc_addon/uart_esc
 	#platforms/qurt/fc_addon/mpu_spi
 
 	#
diff --git a/src/platforms/qurt/fc_addon/uart_esc/CMakeLists.txt b/src/platforms/qurt/fc_addon/uart_esc/CMakeLists.txt
new file mode 100644
index 0000000000..0bc92f856d
--- /dev/null
+++ b/src/platforms/qurt/fc_addon/uart_esc/CMakeLists.txt
@@ -0,0 +1,71 @@
+############################################################################
+#
+#   Copyright (C) 2015 Mark Charlebois. All rights reserved.
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions
+# are met:
+#
+# 1. Redistributions of source code must retain the above copyright
+#    notice, this list of conditions and the following disclaimer.
+# 2. Redistributions in binary form must reproduce the above copyright
+#    notice, this list of conditions and the following disclaimer in
+#    the documentation and/or other materials provided with the
+#    distribution.
+# 3. Neither the name PX4 nor the names of its contributors may be
+#    used to endorse or promote products derived from this software
+#    without specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.
+#
+############################################################################
+
+set(TOOLS_ERROR_MSG 
+		"The FC_ADDON must be installed and the environment variable FC_ADDON must be set"
+		"(e.g. export FC_ADDON=${HOME}/Qualcomm/addon)")
+
+if ("$ENV{FC_ADDON}" STREQUAL "")
+	message(FATAL_ERROR ${TOOLS_ERROR_MSG})
+else()
+	set(FC_ADDON $ENV{FC_ADDON})
+endif()
+
+include_directories(
+	${FC_ADDON}/flight_controller/hexagon/inc
+	)
+
+add_library(libuart_esc SHARED IMPORTED GLOBAL)
+set_target_properties(libuart_esc PROPERTIES IMPORTED_LOCATION ${FC_ADDON}/flight_controller/hexagon/libs/libuart_esc.so)
+
+px4_add_module(
+        MODULE platforms__qurt__fc_addon__uart_esc
+        MAIN uart_esc
+	STACK_MAIN 1200
+        COMPILE_FLAGS
+                -Os -Weffc++
+        SRCS
+                uart_esc_main.cpp
+		uart_esc_params.c
+
+        DEPENDS
+                platforms__common
+        )
+
+set(module_external_libraries 
+	${module_external_libraries}
+	libuart_esc 
+	CACHE INTERNAL "module_external_libraries"
+	)
+	
+# vim: set noet ft=cmake fenc=utf-8 ff=unix : 
diff --git a/src/platforms/qurt/fc_addon/uart_esc/uart_esc_main.cpp b/src/platforms/qurt/fc_addon/uart_esc/uart_esc_main.cpp
new file mode 100644
index 0000000000..33548c6659
--- /dev/null
+++ b/src/platforms/qurt/fc_addon/uart_esc/uart_esc_main.cpp
@@ -0,0 +1,518 @@
+/****************************************************************************
+*
+*   Copyright (c) 2015 Mark Charlebois. All rights reserved.
+*
+* Redistribution and use in source and binary forms, with or without
+* modification, are permitted provided that the following conditions
+* are met:
+*
+* 1. Redistributions of source code must retain the above copyright
+*    notice, this list of conditions and the following disclaimer.
+* 2. Redistributions in binary form must reproduce the above copyright
+*    notice, this list of conditions and the following disclaimer in
+*    the documentation and/or other materials provided with the
+*    distribution.
+* 3. Neither the name PX4 nor the names of its contributors may be
+*    used to endorse or promote products derived from this software
+*    without specific prior written permission.
+*
+* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+* POSSIBILITY OF SUCH DAMAGE.
+*
+****************************************************************************/
+
+#include <stdint.h>
+
+#include <px4_tasks.h>
+#include <px4_getopt.h>
+#include <px4_posix.h>
+#include <errno.h>
+
+#include <uORB/uORB.h>
+#include <uORB/topics/actuator_controls.h>
+#include <uORB/topics/actuator_armed.h>
+#include <uORB/topics/actuator_outputs.h>
+#include <uORB/topics/parameter_update.h>
+
+#include <drivers/drv_hrt.h>
+#include <drivers/drv_mixer.h>
+#include <systemlib/mixer/mixer.h>
+#include <systemlib/mixer/mixer_multirotor.generated.h>
+#include <systemlib/param/param.h>
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+#include <uart_esc.h>
+#ifdef __cplusplus
+}
+#endif
+
+/** driver 'main' command */
+extern "C" { __EXPORT int uart_esc_main(int argc, char *argv[]); }
+
+#define MAX_LEN_DEV_PATH 32
+
+namespace uart_esc
+{
+#define UART_ESC_MAX_MOTORS  4
+
+volatile bool _task_should_exit = false; // flag indicating if uart_esc task should exit
+static char _device[MAX_LEN_DEV_PATH];
+static bool _is_running = false;         // flag indicating if uart_esc app is running
+static px4_task_t _task_handle = -1;     // handle to the task main thread
+UartEsc *esc;                            // esc instance
+void uart_esc_rotate_motors(int16_t *motor_rpm, int num_rotors); // motor re-mapping
+
+// subscriptions
+int		_controls_sub;
+int		_armed_sub;
+int		_param_sub;
+
+// filenames
+// /dev/fs/ is mapped to /usr/share/data/adsp/
+static const char *MIXER_FILENAME = "/dev/fs/mixer_config.mix";
+
+
+// publications
+orb_advert_t        	_outputs_pub;
+
+// topic structures
+actuator_controls_s     _controls;
+actuator_armed_s        _armed;
+parameter_update_s      _param_update;
+actuator_outputs_s      _outputs;
+
+/** Print out the usage information */
+static void usage();
+
+/** uart_esc start */
+static void start(const char *device) __attribute__ ((unused));
+
+/** uart_esc stop */
+static void stop();
+
+/** task main trampoline function */
+static void	task_main_trampoline(int argc, char *argv[]);
+
+/** uart_esc thread primary entry point */
+static void task_main(int argc, char *argv[]);
+
+/** mixer initialization */
+static MultirotorMixer *mixer;
+static int initialize_mixer(const char *mixer_filename);
+static int mixer_control_callback(uintptr_t handle, uint8_t control_group, uint8_t control_index, float &input);
+
+static void parameters_init();
+static void parameters_update();
+
+struct {
+	int model;
+  int baudrate;
+  int px4_motor_mapping[UART_ESC_MAX_MOTORS];
+} _parameters;
+
+struct {
+	param_t model;
+	param_t baudrate;
+	param_t px4_motor_mapping[UART_ESC_MAX_MOTORS];
+} _parameter_handles;
+
+void parameters_init()
+{
+	_parameter_handles.model 		= param_find("UART_ESC_MODEL");
+	_parameter_handles.baudrate = param_find("UART_ESC_BAUDRATE");
+
+	/* PX4 motor mapping parameters */
+	for (unsigned int i = 0; i < UART_ESC_MAX_MOTORS; i++) {
+		char nbuf[20];
+
+		/* min values */
+		sprintf(nbuf, "UART_ESC_PX4MOTOR%d", i + 1);
+		_parameter_handles.px4_motor_mapping[i] = param_find(nbuf);
+	}
+
+	parameters_update();
+}
+
+void parameters_update()
+{
+	PX4_WARN("uart_esc_main parameters_update");
+	int v_int;
+
+	if (param_get(_parameter_handles.model, &v_int) == 0) {
+		_parameters.model = v_int;
+		PX4_WARN("UART_ESC_MODEL %d", _parameters.model);
+	}
+
+	if (param_get(_parameter_handles.baudrate, &v_int) == 0) {
+		_parameters.baudrate = v_int;
+		PX4_WARN("UART_ESC_BAUDRATE %d", _parameters.baudrate);
+	}
+
+	for (unsigned int i = 0; i < UART_ESC_MAX_MOTORS; i++) {
+		if (param_get(_parameter_handles.px4_motor_mapping[i], &v_int) == 0) {
+			_parameters.px4_motor_mapping[i] = v_int;
+			PX4_WARN("UART_ESC_PX4MOTOR%d %d", i+1, _parameters.px4_motor_mapping[i]);
+		}
+	}
+}
+
+int mixer_control_callback(uintptr_t handle,
+			   uint8_t control_group,
+			   uint8_t control_index,
+			   float &input)
+{
+	const actuator_controls_s *controls = (actuator_controls_s *)handle;
+
+	input = controls[control_group].control[control_index];
+
+	/* motor spinup phase - lock throttle to zero *
+	if (_pwm_limit.state == PWM_LIMIT_STATE_RAMP) {
+		if (control_group == actuator_controls_s::GROUP_INDEX_ATTITUDE &&
+			control_index == actuator_controls_s::INDEX_THROTTLE) {
+			* limit the throttle output to zero during motor spinup,
+			 * as the motors cannot follow any demand yet
+			 *
+			input = 0.0f;
+		}
+	}
+	*/
+	return 0;
+}
+
+
+int initialize_mixer(const char *mixer_filename)
+{
+   mixer = nullptr;
+
+   int mixer_initialized = -1;
+
+	char buf[2048];
+   unsigned int buflen = sizeof(buf);
+
+   PX4_INFO("Initializing mixer from config file in %s", mixer_filename);
+
+   int fd_load = open(mixer_filename, O_RDONLY);
+
+	if (fd_load != -1) {
+		int nRead = read(fd_load, buf, buflen);
+		close(fd_load);
+
+		if (nRead > 0) {
+		   mixer = MultirotorMixer::from_text(mixer_control_callback, (uintptr_t)&_controls, buf, buflen);
+			if (mixer != nullptr) {
+				PX4_INFO("Successfully initialized mixer from config file");
+				mixer_initialized = 0;
+			} else {
+			   PX4_WARN("Unable to parse from mixer config file");
+			}
+		} else {
+		   PX4_WARN("Unable to read from mixer config file");
+		}
+	} else {
+	   PX4_WARN("Unable to open mixer config file");
+	}
+
+	// mixer file loading failed, fall back to default mixer configuration for
+	// QUAD_X airframe
+	if (mixer_initialized < 0) {
+		float roll_scale = 1;
+		float pitch_scale = 1;
+		float yaw_scale = 1;
+		float deadband = 0;
+
+		mixer = new MultirotorMixer(mixer_control_callback, (uintptr_t)&_controls,
+		                            MultirotorGeometry::QUAD_X,
+		                            roll_scale, pitch_scale, yaw_scale, deadband);
+
+		if (mixer == nullptr) {
+			PX4_ERR("mixer initialization failed");
+			mixer_initialized = -1;
+			return mixer_initialized;
+		}
+
+		PX4_WARN("mixer config file not found, successfully initialized default quad x mixer");
+		mixer_initialized = 0;
+	}
+
+	return mixer_initialized;
+}
+
+/**
+* Rotate the motor rpm values based on the motor mappings configuration stored
+* in motor_mapping
+*/
+void uart_esc_rotate_motors(int16_t *motor_rpm, int num_rotors)
+{
+	ASSERT(num_rotors <= UART_ESC_MAX_MOTORS);
+	int i;
+	int16_t motor_rpm_copy[UART_ESC_MAX_MOTORS];
+
+	for (i = 0; i < num_rotors; i++) {
+		motor_rpm_copy[i] = motor_rpm[i];
+	}
+
+	for (i = 0; i < num_rotors; i++) {
+		motor_rpm[_parameters.px4_motor_mapping[i] - 1] = motor_rpm_copy[i];
+	}
+}
+
+void task_main(int argc, char *argv[])
+{
+	PX4_INFO("enter uart_esc task_main");
+
+	_outputs_pub = nullptr;
+
+	parameters_init();
+
+	esc = UartEsc::get_instance();
+
+	if (esc == NULL) {
+		PX4_ERR("failed to new UartEsc instance");
+
+	} else if (esc->initialize((enum esc_model_t)_parameters.model,
+	           _device, _parameters.baudrate) < 0) {
+		PX4_ERR("failed to initialize UartEsc");
+
+	} else {
+		// Subscribe for orb topics
+		_controls_sub = orb_subscribe(ORB_ID(actuator_controls_0)); // single group for now
+		_armed_sub    = orb_subscribe(ORB_ID(actuator_armed));
+		_param_sub    = orb_subscribe(ORB_ID(parameter_update));
+
+		// initialize publication structures
+		memset(&_outputs, 0, sizeof(_outputs));
+
+		// set up poll topic and limit poll interval
+		px4_pollfd_struct_t fds[1];
+		fds[0].fd     = _controls_sub;
+		fds[0].events = POLLIN;
+		//orb_set_interval(_controls_sub, 10);  // max actuator update period, ms
+
+		// set up mixer
+		if (initialize_mixer(MIXER_FILENAME) < 0) {
+			PX4_ERR("Mixer initialization failed.");
+			_task_should_exit = true;
+		}
+
+		// Main loop
+		while (!_task_should_exit) {
+			int pret = px4_poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 100);
+
+			/* timed out - periodic check for _task_should_exit */
+			if (pret == 0) {
+				continue;
+			}
+
+			/* this is undesirable but not much we can do - might want to flag unhappy status */
+			if (pret < 0) {
+				PX4_WARN("poll error %d, %d", pret, errno);
+				/* sleep a bit before next try */
+				usleep(100000);
+				continue;
+			}
+
+			// Handle new actuator controls data
+			if (fds[0].revents & POLLIN) {
+
+				// Grab new controls data
+				orb_copy(ORB_ID(actuator_controls_0), _controls_sub, &_controls);
+				// Mix to the outputs
+				_outputs.timestamp = hrt_absolute_time();
+				int16_t motor_rpms[UART_ESC_MAX_MOTORS];
+
+				if (_armed.armed) {
+					_outputs.noutputs = mixer->mix(&_outputs.output[0],
+					                               actuator_controls_0_s::NUM_ACTUATOR_CONTROLS,
+					                               NULL);
+
+					// Make sure we support only up to UART_ESC_MAX_MOTORS motors
+					if (_outputs.noutputs > UART_ESC_MAX_MOTORS) {
+						PX4_ERR("Unsupported motors %d, up to %d motors supported",
+						        _outputs.noutputs, UART_ESC_MAX_MOTORS);
+						continue;
+					}
+
+					// Send outputs to the ESCs
+					for (unsigned outIdx = 0; outIdx < _outputs.noutputs; outIdx++) {
+						// map -1.0 - 1.0 outputs to RPMs
+						motor_rpms[outIdx] = (int16_t)(((_outputs.output[outIdx] + 1.0) / 2.0) *
+								     (esc->max_rpm() - esc->min_rpm()) + esc->min_rpm());
+					}
+
+					uart_esc_rotate_motors(motor_rpms, _outputs.noutputs);
+
+				} else {
+					_outputs.noutputs = UART_ESC_MAX_MOTORS;
+					for (unsigned outIdx = 0; outIdx < _outputs.noutputs; outIdx++) {
+						motor_rpms[outIdx]      = 0;
+						_outputs.output[outIdx] = -1.0;
+					}
+				}
+
+				esc->send_rpms(&motor_rpms[0], _outputs.noutputs);
+
+				// TODO-JYW: TESTING-TESTING
+				// MAINTAIN FOR REFERENCE, COMMENT OUT FOR RELEASE BUILDS
+//				static int count=0;
+//				count++;
+//				if (!(count % 100)) {
+//					PX4_DEBUG("                                                                  ");
+//					PX4_DEBUG("Time       t: %13lu, Armed: %d                                  ",(unsigned long)_outputs.timestamp,_armed.armed);
+//					PX4_DEBUG("Act Controls: 0: %+8.4f, 1: %+8.4f,   2: %+8.4f, 3: %+8.4f  ",_controls.control[0],_controls.control[1],_controls.control[2],_controls.control[3]);
+//					PX4_DEBUG("Act Outputs : 0: %+8.4f, 1: %+8.4f,   2: %+8.4f, 3: %+8.4f  ",_outputs.output[0],_outputs.output[1],_outputs.output[2],_outputs.output[3]);
+//				}
+				// TODO-JYW: TESTING-TESTING
+
+
+				/* Publish mixed control outputs */
+				if (_outputs_pub != nullptr) {
+					orb_publish(ORB_ID(actuator_outputs), _outputs_pub, &_outputs);
+
+				} else {
+					_outputs_pub = orb_advertise(ORB_ID(actuator_outputs), &_outputs);
+				}
+			}
+
+			// Check for updates in other subscripions
+			bool updated = false;
+			orb_check(_armed_sub, &updated);
+
+			if (updated) {
+				orb_copy(ORB_ID(actuator_armed), _armed_sub, &_armed);
+				PX4_DEBUG("arming status updated, _armed.armed: %d", _armed.armed);
+			}
+
+			orb_check(_param_sub, &updated);
+
+			if (updated) {
+				// Even though we are only interested in the update status of the parameters, copy
+				// the subscription to clear the update status.
+				orb_copy(ORB_ID(parameter_update), _param_sub, &_param_update);
+				parameters_update();
+			}
+		}
+	}
+
+	PX4_WARN("closing uart_esc");
+
+	delete esc;
+}
+
+/** uart_esc main entrance */
+void task_main_trampoline(int argc, char *argv[])
+{
+	PX4_WARN("task_main_trampoline");
+	task_main(argc, argv);
+}
+
+void start()
+{
+	ASSERT(_task_handle == -1);
+
+	/* start the task */
+	_task_handle = px4_task_spawn_cmd("uart_esc_main",
+					  SCHED_DEFAULT,
+					  SCHED_PRIORITY_MAX,
+					  1500,
+					  (px4_main_t)&task_main_trampoline,
+					  nullptr);
+
+	if (_task_handle < 0) {
+		warn("task start failed");
+		return;
+	}
+
+	_is_running = true;
+}
+
+void stop()
+{
+	// TODO - set thread exit signal to terminate the task main thread
+
+	_is_running = false;
+	_task_handle = -1;
+}
+
+void usage()
+{
+	PX4_WARN("missing command: try 'start', 'stop', 'status'");
+	PX4_WARN("options:");
+	PX4_WARN("    -D device");
+}
+
+}; // namespace uart_esc
+
+int uart_esc_main(int argc, char *argv[])
+{
+	const char *device = NULL;
+	int ch;
+	int myoptind = 1;
+	const char *myoptarg = NULL;
+
+	while ((ch = px4_getopt(argc, argv, "D:", &myoptind, &myoptarg)) != EOF) {
+		switch (ch) {
+		case 'D':
+			device = myoptarg;
+			break;
+
+		default:
+			uart_esc::usage();
+			return 1;
+		}
+	}
+
+	// Check on required arguments
+	if (device == NULL || strlen(device) == 0) {
+		uart_esc::usage();
+		return 1;
+	}
+
+	memset(uart_esc::_device, 0, MAX_LEN_DEV_PATH);
+	strncpy(uart_esc::_device, device, strlen(device));
+
+	const char *verb = argv[myoptind];
+
+	/*
+	 * Start/load the driver.
+	 */
+	if (!strcmp(verb, "start")) {
+		if (uart_esc::_is_running) {
+			PX4_WARN("uart_esc already running");
+			return 1;
+		}
+
+		uart_esc::start();
+	}
+
+	else if (!strcmp(verb, "stop")) {
+		if (uart_esc::_is_running) {
+			PX4_WARN("uart_esc is not running");
+			return 1;
+		}
+
+		uart_esc::stop();
+	}
+
+	else if (!strcmp(verb, "status")) {
+		PX4_WARN("uart_esc is %s", uart_esc::_is_running ? "running" : "stopped");
+		return 0;
+
+	} else {
+		uart_esc::usage();
+		return 1;
+	}
+
+	return 0;
+}
diff --git a/src/platforms/qurt/fc_addon/uart_esc/uart_esc_params.c b/src/platforms/qurt/fc_addon/uart_esc/uart_esc_params.c
new file mode 100644
index 0000000000..1d4ee10374
--- /dev/null
+++ b/src/platforms/qurt/fc_addon/uart_esc/uart_esc_params.c
@@ -0,0 +1,114 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2015 Ramakrishna Kintada. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file uart_esc_params.c
+ *
+ * Parameters defined for the uart esc driver
+ */
+#include <px4_config.h>
+#include <systemlib/param/param.h>
+
+/**
+ * ESC model
+ *
+ * See esc_model_t enum definition in uart_esc_dev.h for all supported ESC
+ * model enum values.
+ * ESC_200QX = 0
+ * ESC_350QX = 1
+ * ESC_210QC = 2
+ *
+ * Default is 210QC
+ */
+PARAM_DEFINE_INT32(UART_ESC_MODEL, 2);
+
+/**
+ * ESC UART baud rate
+ *
+ * Default rate is 250Kbps, whic is used in off-the-shelf QRP ESC products.
+ */
+PARAM_DEFINE_INT32(UART_ESC_BAUDRATE, 250000);
+
+/**
+ * The PX4 default motor mappings are
+ *         1     4
+ * [front]
+ *         3     2
+ *
+ * The following paramters define the motor mappings in reference to the
+ * PX4 motor mapping convention.
+ */
+/**
+ * Default PX4 motor mappings
+ *         1     4
+ * [front]
+ *         3     2
+ */
+// PARAM_DEFINE_INT32(UART_ESC_PX4MOTOR1, 1);
+// PARAM_DEFINE_INT32(UART_ESC_PX4MOTOR2, 2);
+// PARAM_DEFINE_INT32(UART_ESC_PX4MOTOR3, 3);
+// PARAM_DEFINE_INT32(UART_ESC_PX4MOTOR4, 4);
+
+/**
+ * Motor mappings for 350QX
+ *         4     3
+ * [front]
+ *         1     2
+ */
+// PARAM_DEFINE_INT32(UART_ESC_PX4MOTOR1, 4);
+// PARAM_DEFINE_INT32(UART_ESC_PX4MOTOR2, 2);
+// PARAM_DEFINE_INT32(UART_ESC_PX4MOTOR3, 1);
+// PARAM_DEFINE_INT32(UART_ESC_PX4MOTOR4, 3);
+
+/**
+ * Motor mappings for 200QX
+ *         2     3
+ * [front]
+ *         1     4
+ */
+// PARAM_DEFINE_INT32(UART_ESC_PX4MOTOR1, 2);
+// PARAM_DEFINE_INT32(UART_ESC_PX4MOTOR2, 4);
+// PARAM_DEFINE_INT32(UART_ESC_PX4MOTOR3, 1);
+// PARAM_DEFINE_INT32(UART_ESC_PX4MOTOR4, 3);
+
+/**
+ * Motor mappings for 210QC
+ *         4     3
+ * [front]
+ *         1     2
+ */
+PARAM_DEFINE_INT32(UART_ESC_PX4MOTOR1, 4);
+PARAM_DEFINE_INT32(UART_ESC_PX4MOTOR2, 2);
+PARAM_DEFINE_INT32(UART_ESC_PX4MOTOR3, 1);
+PARAM_DEFINE_INT32(UART_ESC_PX4MOTOR4, 3);
+

From 86e470b1c28cff810c890348295290eb188cd55d Mon Sep 17 00:00:00 2001
From: Mark Charlebois <charlebm@gmail.com>
Date: Thu, 5 May 2016 12:55:09 -0700
Subject: [PATCH 0367/1230] Fixed syle issues

Signed-off-by: Mark Charlebois <charlebm@gmail.com>
---
 .../qurt/fc_addon/mpu_spi/mpu9x50_main.cpp    |  9 ++--
 .../fc_addon/rc_receiver/rc_receiver_main.cpp |  2 +-
 .../qurt/fc_addon/uart_esc/uart_esc_main.cpp  | 45 ++++++++++---------
 3 files changed, 32 insertions(+), 24 deletions(-)

diff --git a/src/platforms/qurt/fc_addon/mpu_spi/mpu9x50_main.cpp b/src/platforms/qurt/fc_addon/mpu_spi/mpu9x50_main.cpp
index 8e705eb25d..d0221b7d83 100644
--- a/src/platforms/qurt/fc_addon/mpu_spi/mpu9x50_main.cpp
+++ b/src/platforms/qurt/fc_addon/mpu_spi/mpu9x50_main.cpp
@@ -372,21 +372,24 @@ bool create_pubs()
 	memset(&_mag, 0, sizeof(struct mag_report));
 
 	_gyro_pub = orb_advertise_multi(ORB_ID(sensor_gyro), &_gyro,
-					  &_gyro_orb_class_instance, ORB_PRIO_HIGH - 1);
+					&_gyro_orb_class_instance, ORB_PRIO_HIGH - 1);
+
 	if (_gyro_pub == nullptr) {
 		PX4_ERR("sensor_gyro advert fail");
 		return false;
 	}
 
 	_accel_pub = orb_advertise_multi(ORB_ID(sensor_accel), &_accel,
-					  &_accel_orb_class_instance, ORB_PRIO_HIGH - 1);
+					 &_accel_orb_class_instance, ORB_PRIO_HIGH - 1);
+
 	if (_accel_pub == nullptr) {
 		PX4_ERR("sensor_accel advert fail");
 		return false;
 	}
 
 	_mag_pub = orb_advertise_multi(ORB_ID(sensor_mag), &_mag,
-					  &_mag_orb_class_instance, ORB_PRIO_HIGH - 1);
+				       &_mag_orb_class_instance, ORB_PRIO_HIGH - 1);
+
 	if (_mag_pub == nullptr) {
 		PX4_ERR("sensor_mag advert fail");
 		return false;
diff --git a/src/platforms/qurt/fc_addon/rc_receiver/rc_receiver_main.cpp b/src/platforms/qurt/fc_addon/rc_receiver/rc_receiver_main.cpp
index 7ae0a4a8f6..8f3e194ec9 100644
--- a/src/platforms/qurt/fc_addon/rc_receiver/rc_receiver_main.cpp
+++ b/src/platforms/qurt/fc_addon/rc_receiver/rc_receiver_main.cpp
@@ -266,7 +266,7 @@ void task_main(int argc, char *argv[])
 			// See modules/px4iofirmware/dsm.c for details
 			// NOTE: rc_receiver spektrum driver outputs the data in 10bit DSM
 			// format, so we need to double the channel value before the scaling
-			_rc_in.values[i] = ((((int)(rc_inputs[i]*2) - 1024) * 1000) / 1700) + 1500;
+			_rc_in.values[i] = ((((int)(rc_inputs[i] * 2) - 1024) * 1000) / 1700) + 1500;
 		}
 
 		orb_publish(ORB_ID(input_rc), _input_rc_pub, &_rc_in);
diff --git a/src/platforms/qurt/fc_addon/uart_esc/uart_esc_main.cpp b/src/platforms/qurt/fc_addon/uart_esc/uart_esc_main.cpp
index 33548c6659..606e8535b8 100644
--- a/src/platforms/qurt/fc_addon/uart_esc/uart_esc_main.cpp
+++ b/src/platforms/qurt/fc_addon/uart_esc/uart_esc_main.cpp
@@ -97,7 +97,7 @@ actuator_outputs_s      _outputs;
 static void usage();
 
 /** uart_esc start */
-static void start(const char *device) __attribute__ ((unused));
+static void start(const char *device) __attribute__((unused));
 
 /** uart_esc stop */
 static void stop();
@@ -118,8 +118,8 @@ static void parameters_update();
 
 struct {
 	int model;
-  int baudrate;
-  int px4_motor_mapping[UART_ESC_MAX_MOTORS];
+	int baudrate;
+	int px4_motor_mapping[UART_ESC_MAX_MOTORS];
 } _parameters;
 
 struct {
@@ -163,7 +163,7 @@ void parameters_update()
 	for (unsigned int i = 0; i < UART_ESC_MAX_MOTORS; i++) {
 		if (param_get(_parameter_handles.px4_motor_mapping[i], &v_int) == 0) {
 			_parameters.px4_motor_mapping[i] = v_int;
-			PX4_WARN("UART_ESC_PX4MOTOR%d %d", i+1, _parameters.px4_motor_mapping[i]);
+			PX4_WARN("UART_ESC_PX4MOTOR%d %d", i + 1, _parameters.px4_motor_mapping[i]);
 		}
 	}
 }
@@ -194,34 +194,38 @@ int mixer_control_callback(uintptr_t handle,
 
 int initialize_mixer(const char *mixer_filename)
 {
-   mixer = nullptr;
+	mixer = nullptr;
 
-   int mixer_initialized = -1;
+	int mixer_initialized = -1;
 
 	char buf[2048];
-   unsigned int buflen = sizeof(buf);
+	unsigned int buflen = sizeof(buf);
 
-   PX4_INFO("Initializing mixer from config file in %s", mixer_filename);
+	PX4_INFO("Initializing mixer from config file in %s", mixer_filename);
 
-   int fd_load = open(mixer_filename, O_RDONLY);
+	int fd_load = open(mixer_filename, O_RDONLY);
 
 	if (fd_load != -1) {
 		int nRead = read(fd_load, buf, buflen);
 		close(fd_load);
 
 		if (nRead > 0) {
-		   mixer = MultirotorMixer::from_text(mixer_control_callback, (uintptr_t)&_controls, buf, buflen);
+			mixer = MultirotorMixer::from_text(mixer_control_callback, (uintptr_t)&_controls, buf, buflen);
+
 			if (mixer != nullptr) {
 				PX4_INFO("Successfully initialized mixer from config file");
 				mixer_initialized = 0;
+
 			} else {
-			   PX4_WARN("Unable to parse from mixer config file");
+				PX4_WARN("Unable to parse from mixer config file");
 			}
+
 		} else {
-		   PX4_WARN("Unable to read from mixer config file");
+			PX4_WARN("Unable to read from mixer config file");
 		}
+
 	} else {
-	   PX4_WARN("Unable to open mixer config file");
+		PX4_WARN("Unable to open mixer config file");
 	}
 
 	// mixer file loading failed, fall back to default mixer configuration for
@@ -233,8 +237,8 @@ int initialize_mixer(const char *mixer_filename)
 		float deadband = 0;
 
 		mixer = new MultirotorMixer(mixer_control_callback, (uintptr_t)&_controls,
-		                            MultirotorGeometry::QUAD_X,
-		                            roll_scale, pitch_scale, yaw_scale, deadband);
+					    MultirotorGeometry::QUAD_X,
+					    roll_scale, pitch_scale, yaw_scale, deadband);
 
 		if (mixer == nullptr) {
 			PX4_ERR("mixer initialization failed");
@@ -282,7 +286,7 @@ void task_main(int argc, char *argv[])
 		PX4_ERR("failed to new UartEsc instance");
 
 	} else if (esc->initialize((enum esc_model_t)_parameters.model,
-	           _device, _parameters.baudrate) < 0) {
+				   _device, _parameters.baudrate) < 0) {
 		PX4_ERR("failed to initialize UartEsc");
 
 	} else {
@@ -334,13 +338,13 @@ void task_main(int argc, char *argv[])
 
 				if (_armed.armed) {
 					_outputs.noutputs = mixer->mix(&_outputs.output[0],
-					                               actuator_controls_0_s::NUM_ACTUATOR_CONTROLS,
-					                               NULL);
+								       actuator_controls_0_s::NUM_ACTUATOR_CONTROLS,
+								       NULL);
 
 					// Make sure we support only up to UART_ESC_MAX_MOTORS motors
 					if (_outputs.noutputs > UART_ESC_MAX_MOTORS) {
 						PX4_ERR("Unsupported motors %d, up to %d motors supported",
-						        _outputs.noutputs, UART_ESC_MAX_MOTORS);
+							_outputs.noutputs, UART_ESC_MAX_MOTORS);
 						continue;
 					}
 
@@ -348,13 +352,14 @@ void task_main(int argc, char *argv[])
 					for (unsigned outIdx = 0; outIdx < _outputs.noutputs; outIdx++) {
 						// map -1.0 - 1.0 outputs to RPMs
 						motor_rpms[outIdx] = (int16_t)(((_outputs.output[outIdx] + 1.0) / 2.0) *
-								     (esc->max_rpm() - esc->min_rpm()) + esc->min_rpm());
+									       (esc->max_rpm() - esc->min_rpm()) + esc->min_rpm());
 					}
 
 					uart_esc_rotate_motors(motor_rpms, _outputs.noutputs);
 
 				} else {
 					_outputs.noutputs = UART_ESC_MAX_MOTORS;
+
 					for (unsigned outIdx = 0; outIdx < _outputs.noutputs; outIdx++) {
 						motor_rpms[outIdx]      = 0;
 						_outputs.output[outIdx] = -1.0;

From c19ac3425f86cfe58952bb9aed47ce5069003a28 Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Sat, 28 May 2016 14:24:19 +0200
Subject: [PATCH 0368/1230] MPU9x driver: Fix param names

---
 .../qurt/fc_addon/mpu_spi/mpu9x50_main.cpp    |  6 +-
 .../qurt/fc_addon/mpu_spi/mpu9x50_params.c    | 57 +++++++++----------
 2 files changed, 30 insertions(+), 33 deletions(-)

diff --git a/src/platforms/qurt/fc_addon/mpu_spi/mpu9x50_main.cpp b/src/platforms/qurt/fc_addon/mpu_spi/mpu9x50_main.cpp
index d0221b7d83..246e91d548 100644
--- a/src/platforms/qurt/fc_addon/mpu_spi/mpu9x50_main.cpp
+++ b/src/platforms/qurt/fc_addon/mpu_spi/mpu9x50_main.cpp
@@ -356,10 +356,10 @@ void parameters_init()
 	_params_handles.mag_z_offset		=	param_find("CAL_MAG0_ZOFF");
 	_params_handles.mag_z_scale		=	param_find("CAL_MAG0_ZSCALE");
 
-	_params_handles.gyro_lpf_enum		=	param_find("MPU_GYRO_LPF_ENUM");
-	_params_handles.accel_lpf_enum		=	param_find("MPU_ACC_LPF_ENUM");
+	_params_handles.gyro_lpf_enum		=	param_find("MPU_GYRO_LPF_ENM");
+	_params_handles.accel_lpf_enum		=	param_find("MPU_ACC_LPF_ENM");
 
-	_params_handles.imu_sample_rate_enum	=	param_find("MPU_SAMPLE_RATE_ENUM");
+	_params_handles.imu_sample_rate_enum	=	param_find("MPU_SAMPLE_R_ENM");
 
 	parameters_update();
 }
diff --git a/src/platforms/qurt/fc_addon/mpu_spi/mpu9x50_params.c b/src/platforms/qurt/fc_addon/mpu_spi/mpu9x50_params.c
index 36cacdc566..4352c10185 100644
--- a/src/platforms/qurt/fc_addon/mpu_spi/mpu9x50_params.c
+++ b/src/platforms/qurt/fc_addon/mpu_spi/mpu9x50_params.c
@@ -41,49 +41,46 @@
 #include <systemlib/param/param.h>
 
 /**
- * IMU Low pass filter enum value for Gyro
+ * Low pass filter frequency for Gyro
  *
- * Acceptable values:
- *
- * MPU9X50_GYRO_LPF_250HZ = 0,
- * MPU9X50_GYRO_LPF_184HZ = 1,
- * MPU9X50_GYRO_LPF_92HZ = 2,
- * MPU9X50_GYRO_LPF_41HZ = 3,
- * MPU9X50_GYRO_LPF_20HZ = 4 (default),
- * MPU9X50_GYRO_LPF_10HZ = 5,
- * MPU9X50_GYRO_LPF_5HZ = 6,
- * MPU9X50_GYRO_LPF_3600HZ_NOLPF = 7,
+ * @value 0 MPU9X50_GYRO_LPF_250HZ
+ * @value 1 MPU9X50_GYRO_LPF_184HZ
+ * @value 2 MPU9X50_GYRO_LPF_92HZ
+ * @value 3 MPU9X50_GYRO_LPF_41HZ
+ * @value 4 MPU9X50_GYRO_LPF_20HZ
+ * @value 5 MPU9X50_GYRO_LPF_10HZ
+ * @value 6 MPU9X50_GYRO_LPF_5HZ
+ * @value 7 MPU9X50_GYRO_LPF_3600HZ_NOLPF
  *
  * @group MPU9x50 Configuration
  */
-PARAM_DEFINE_INT32(MPU_GYRO_LPF_ENUM, 4);
+PARAM_DEFINE_INT32(MPU_GYRO_LPF_ENM, 4);
 
 /**
- * IMU Low pass filter enum value for Accelerometer
+ * Low pass filter frequency for Accelerometer
  *
- * Acceptable values:
  *
- *  MPU9X50_ACC_LPF_460HZ = 0,
- *  MPU9X50_ACC_LPF_184HZ = 1,
- *  MPU9X50_ACC_LPF_92HZ = 2,
- *  MPU9X50_ACC_LPF_41HZ = 3,
- *  MPU9X50_ACC_LPF_20HZ = 4 (default),
- *  MPU9X50_ACC_LPF_10HZ = 5,
- *  MPU9X50_ACC_LPF_5HZ = 6,
- *  MPU9X50_ACC_LPF_460HZ_NOLPF = 7,
+ * @value 0 MPU9X50_ACC_LPF_460HZ
+ * @value 1 MPU9X50_ACC_LPF_184HZ
+ * @value 2 MPU9X50_ACC_LPF_92HZ
+ * @value 3 MPU9X50_ACC_LPF_41HZ
+ * @value 4 MPU9X50_ACC_LPF_20HZ
+ * @value 5 MPU9X50_ACC_LPF_10HZ
+ * @value 6 MPU9X50_ACC_LPF_5HZ
+ * @value 7 MPU9X50_ACC_LPF_460HZ_NOLPF
  *
  * @group MPU9x50 Configuration
  */
-PARAM_DEFINE_INT32(MPU_ACC_LPF_ENUM, 4);
+PARAM_DEFINE_INT32(MPU_ACC_LPF_ENM, 4);
 
 /**
- * IMU sample rate in Hz
- * acceptable values:
- * MPU9x50_SAMPLE_RATE_100HZ = 0,
- * MPU9x50_SAMPLE_RATE_200HZ,
- * MPU9x50_SAMPLE_RATE_500HZ, (default)
- * MPU9x50_SAMPLE_RATE_1000HZ,
+ * Sample rate in Hz
+ *
+ * @value 0 MPU9x50_SAMPLE_RATE_100HZ
+ * @value 1 MPU9x50_SAMPLE_RATE_200HZ
+ * @value 2 MPU9x50_SAMPLE_RATE_500HZ
+ * @value 3 MPU9x50_SAMPLE_RATE_1000HZ
  *
  * @group MPU9x50 Configuration
  */
-PARAM_DEFINE_INT32(MPU_SAMPLE_RATE_ENUM, 2);
+PARAM_DEFINE_INT32(MPU_SAMPLE_R_ENM, 2);

From aa355d86d08a2198273f09ef21aff442ef68aa9c Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Sat, 28 May 2016 14:24:38 +0200
Subject: [PATCH 0369/1230] 200qx: Fix sample rate

---
 posix-configs/eagle/200qx/px4-calib.config           | 6 +++---
 posix-configs/eagle/200qx/px4-flight-v1-board.config | 6 +++---
 posix-configs/eagle/200qx/px4-flight-v2-board.config | 6 +++---
 3 files changed, 9 insertions(+), 9 deletions(-)

diff --git a/posix-configs/eagle/200qx/px4-calib.config b/posix-configs/eagle/200qx/px4-calib.config
index 5cda1c9323..cabe21c4a0 100644
--- a/posix-configs/eagle/200qx/px4-calib.config
+++ b/posix-configs/eagle/200qx/px4-calib.config
@@ -59,9 +59,9 @@ param set MC_ROLLRATE_D 0.0001
 param set ATT_W_MAG 0.00
 param set PE_MAG_NOISE 1.0f
 param set SENS_BOARD_ROT 0
-param set MPU_GYRO_LPF_ENUM 4
-param set MPU_ACC_LPF_ENUM 4
-param set MPU_SAMPLE_RATE_ENUM 2
+param set MPU_GYRO_LPF_ENM 4
+param set MPU_ACC_LPF_ENM 4
+param set MPU_SAMPLE_R_ENM 2
 sleep 1
 mpu9x50 start -D /dev/spi-1
 uart_esc start -D /dev/tty-2
diff --git a/posix-configs/eagle/200qx/px4-flight-v1-board.config b/posix-configs/eagle/200qx/px4-flight-v1-board.config
index 61120ad5e0..681eed39de 100644
--- a/posix-configs/eagle/200qx/px4-flight-v1-board.config
+++ b/posix-configs/eagle/200qx/px4-flight-v1-board.config
@@ -51,9 +51,9 @@ param set RC6_REV 1
 param set ATT_W_MAG 0.00
 param set PE_MAG_NOISE 1.0f
 param set SENS_BOARD_ROT 6
-param set MPU_GYRO_LPF_ENUM 4
-param set MPU_ACC_LPF_ENUM 4
-param set MPU_SAMPLE_RATE_ENUM 2
+param set MPU_GYRO_LPF_ENM 4
+param set MPU_ACC_LPF_ENM 4
+param set MPU_SAMPLE_R_ENM 2
 param set UART_ESC_MODEL 0
 param set UART_ESC_BAUDRATE 250000
 param set UART_ESC_PX4MOTOR1 2
diff --git a/posix-configs/eagle/200qx/px4-flight-v2-board.config b/posix-configs/eagle/200qx/px4-flight-v2-board.config
index 11072428e5..61aabe58cc 100644
--- a/posix-configs/eagle/200qx/px4-flight-v2-board.config
+++ b/posix-configs/eagle/200qx/px4-flight-v2-board.config
@@ -51,9 +51,9 @@ param set RC6_REV 1
 param set ATT_W_MAG 0.00
 param set PE_MAG_NOISE 1.0f
 param set SENS_BOARD_ROT 0
-param set MPU_GYRO_LPF_ENUM 4
-param set MPU_ACC_LPF_ENUM 4
-param set MPU_SAMPLE_RATE_ENUM 2
+param set MPU_GYRO_LPF_ENM 4
+param set MPU_ACC_LPF_ENM 4
+param set MPU_SAMPLE_R_ENM 2
 param set UART_ESC_MODEL 0
 param set UART_ESC_BAUDRATE 250000
 param set UART_ESC_PX4MOTOR1 2

From afa3429bdbcf09c4104461bc789fc4a851d6e453 Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Sat, 28 May 2016 14:24:55 +0200
Subject: [PATCH 0370/1230] 210qc: Fix sample rate

---
 posix-configs/eagle/210qc/px4-calib.config  | 6 +++---
 posix-configs/eagle/210qc/px4-flight.config | 6 +++---
 2 files changed, 6 insertions(+), 6 deletions(-)

diff --git a/posix-configs/eagle/210qc/px4-calib.config b/posix-configs/eagle/210qc/px4-calib.config
index 5cda1c9323..cabe21c4a0 100644
--- a/posix-configs/eagle/210qc/px4-calib.config
+++ b/posix-configs/eagle/210qc/px4-calib.config
@@ -59,9 +59,9 @@ param set MC_ROLLRATE_D 0.0001
 param set ATT_W_MAG 0.00
 param set PE_MAG_NOISE 1.0f
 param set SENS_BOARD_ROT 0
-param set MPU_GYRO_LPF_ENUM 4
-param set MPU_ACC_LPF_ENUM 4
-param set MPU_SAMPLE_RATE_ENUM 2
+param set MPU_GYRO_LPF_ENM 4
+param set MPU_ACC_LPF_ENM 4
+param set MPU_SAMPLE_R_ENM 2
 sleep 1
 mpu9x50 start -D /dev/spi-1
 uart_esc start -D /dev/tty-2
diff --git a/posix-configs/eagle/210qc/px4-flight.config b/posix-configs/eagle/210qc/px4-flight.config
index d4da43dce7..94964f9a28 100644
--- a/posix-configs/eagle/210qc/px4-flight.config
+++ b/posix-configs/eagle/210qc/px4-flight.config
@@ -49,9 +49,9 @@ param set RC6_REV 1
 param set ATT_W_MAG 0.00
 param set PE_MAG_NOISE 1.0f
 param set SENS_BOARD_ROT 0
-param set MPU_GYRO_LPF_ENUM 4
-param set MPU_ACC_LPF_ENUM 4
-param set MPU_SAMPLE_RATE_ENUM 2
+param set MPU_GYRO_LPF_ENM 4
+param set MPU_ACC_LPF_ENM 4
+param set MPU_SAMPLE_R_ENM 2
 param set GYRO0_XOFF -0.0028
 param set GYRO0_YOFF -0.0047
 param set GYRO0_ZOFF -0.0034

From 55c7ffbe62de08065394b8c3899e9814738a77bb Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Sat, 28 May 2016 14:36:51 +0200
Subject: [PATCH 0371/1230] Fix more Qualcomm param length issues

---
 .../eagle/200qx/px4-flight-v1-board.config    | 10 +--
 .../eagle/200qx/px4-flight-v2-board.config    | 10 +--
 posix-configs/eagle/210qc/px4-flight.config   | 10 +--
 .../qurt/fc_addon/uart_esc/uart_esc_main.cpp  |  8 +--
 .../qurt/fc_addon/uart_esc/uart_esc_params.c  | 67 +++++++++++++------
 5 files changed, 65 insertions(+), 40 deletions(-)

diff --git a/posix-configs/eagle/200qx/px4-flight-v1-board.config b/posix-configs/eagle/200qx/px4-flight-v1-board.config
index 681eed39de..f07aa5d026 100644
--- a/posix-configs/eagle/200qx/px4-flight-v1-board.config
+++ b/posix-configs/eagle/200qx/px4-flight-v1-board.config
@@ -55,11 +55,11 @@ param set MPU_GYRO_LPF_ENM 4
 param set MPU_ACC_LPF_ENM 4
 param set MPU_SAMPLE_R_ENM 2
 param set UART_ESC_MODEL 0
-param set UART_ESC_BAUDRATE 250000
-param set UART_ESC_PX4MOTOR1 2
-param set UART_ESC_PX4MOTOR2 4
-param set UART_ESC_PX4MOTOR3 1
-param set UART_ESC_PX4MOTOR4 3
+param set UART_ESC_BAUD 250000
+param set UART_ESC_MOTOR1 2
+param set UART_ESC_MOTOR2 4
+param set UART_ESC_MOTOR3 1
+param set UART_ESC_MOTOR4 3
 sleep 1
 commander start
 rc_receiver start -D /dev/tty-1
diff --git a/posix-configs/eagle/200qx/px4-flight-v2-board.config b/posix-configs/eagle/200qx/px4-flight-v2-board.config
index 61aabe58cc..90fb8c2a0c 100644
--- a/posix-configs/eagle/200qx/px4-flight-v2-board.config
+++ b/posix-configs/eagle/200qx/px4-flight-v2-board.config
@@ -55,11 +55,11 @@ param set MPU_GYRO_LPF_ENM 4
 param set MPU_ACC_LPF_ENM 4
 param set MPU_SAMPLE_R_ENM 2
 param set UART_ESC_MODEL 0
-param set UART_ESC_BAUDRATE 250000
-param set UART_ESC_PX4MOTOR1 2
-param set UART_ESC_PX4MOTOR2 4
-param set UART_ESC_PX4MOTOR3 1
-param set UART_ESC_PX4MOTOR4 3
+param set UART_ESC_BAUD 250000
+param set UART_ESC_MOTOR1 2
+param set UART_ESC_MOTOR2 4
+param set UART_ESC_MOTOR3 1
+param set UART_ESC_MOTOR4 3
 sleep 1
 commander start
 rc_receiver start -D /dev/tty-1
diff --git a/posix-configs/eagle/210qc/px4-flight.config b/posix-configs/eagle/210qc/px4-flight.config
index 94964f9a28..0a83b68fae 100644
--- a/posix-configs/eagle/210qc/px4-flight.config
+++ b/posix-configs/eagle/210qc/px4-flight.config
@@ -72,11 +72,11 @@ param set ACC0_YSCALE 0.9974
 param set ACC0_ZSCALE 0.9951
 param set RC_RECEIVER_TYPE 1
 param set UART_ESC_MODEL 2
-param set UART_ESC_BAUDRATE 250000
-param set UART_ESC_PX4MOTOR1 4
-param set UART_ESC_PX4MOTOR2 2
-param set UART_ESC_PX4MOTOR3 1
-param set UART_ESC_PX4MOTOR4 3
+param set UART_ESC_BAUD 250000
+param set UART_ESC_MOTOR1 4
+param set UART_ESC_MOTOR2 2
+param set UART_ESC_MOTOR3 1
+param set UART_ESC_MOTOR4 3
 sleep 1
 commander start
 rc_receiver start -D /dev/tty-1
diff --git a/src/platforms/qurt/fc_addon/uart_esc/uart_esc_main.cpp b/src/platforms/qurt/fc_addon/uart_esc/uart_esc_main.cpp
index 606e8535b8..ebeefcd893 100644
--- a/src/platforms/qurt/fc_addon/uart_esc/uart_esc_main.cpp
+++ b/src/platforms/qurt/fc_addon/uart_esc/uart_esc_main.cpp
@@ -131,14 +131,14 @@ struct {
 void parameters_init()
 {
 	_parameter_handles.model 		= param_find("UART_ESC_MODEL");
-	_parameter_handles.baudrate = param_find("UART_ESC_BAUDRATE");
+	_parameter_handles.baudrate = param_find("UART_ESC_BAUD");
 
 	/* PX4 motor mapping parameters */
 	for (unsigned int i = 0; i < UART_ESC_MAX_MOTORS; i++) {
 		char nbuf[20];
 
 		/* min values */
-		sprintf(nbuf, "UART_ESC_PX4MOTOR%d", i + 1);
+		sprintf(nbuf, "UART_ESC_MOTOR%d", i + 1);
 		_parameter_handles.px4_motor_mapping[i] = param_find(nbuf);
 	}
 
@@ -157,13 +157,13 @@ void parameters_update()
 
 	if (param_get(_parameter_handles.baudrate, &v_int) == 0) {
 		_parameters.baudrate = v_int;
-		PX4_WARN("UART_ESC_BAUDRATE %d", _parameters.baudrate);
+		PX4_WARN("UART_ESC_BAUD %d", _parameters.baudrate);
 	}
 
 	for (unsigned int i = 0; i < UART_ESC_MAX_MOTORS; i++) {
 		if (param_get(_parameter_handles.px4_motor_mapping[i], &v_int) == 0) {
 			_parameters.px4_motor_mapping[i] = v_int;
-			PX4_WARN("UART_ESC_PX4MOTOR%d %d", i + 1, _parameters.px4_motor_mapping[i]);
+			PX4_WARN("UART_ESC_MOTOR%d %d", i + 1, _parameters.px4_motor_mapping[i]);
 		}
 	}
 }
diff --git a/src/platforms/qurt/fc_addon/uart_esc/uart_esc_params.c b/src/platforms/qurt/fc_addon/uart_esc/uart_esc_params.c
index 1d4ee10374..ca1ca273b7 100644
--- a/src/platforms/qurt/fc_addon/uart_esc/uart_esc_params.c
+++ b/src/platforms/qurt/fc_addon/uart_esc/uart_esc_params.c
@@ -44,11 +44,12 @@
  *
  * See esc_model_t enum definition in uart_esc_dev.h for all supported ESC
  * model enum values.
- * ESC_200QX = 0
- * ESC_350QX = 1
- * ESC_210QC = 2
  *
- * Default is 210QC
+ * @value 0 ESC_200QX
+ * @value 1 ESC_350QX
+ * @value 2 ESC_210QC
+ *
+ * @group Snapdragon UART ESC
  */
 PARAM_DEFINE_INT32(UART_ESC_MODEL, 2);
 
@@ -56,8 +57,9 @@ PARAM_DEFINE_INT32(UART_ESC_MODEL, 2);
  * ESC UART baud rate
  *
  * Default rate is 250Kbps, whic is used in off-the-shelf QRP ESC products.
+ * @group Snapdragon UART ESC
  */
-PARAM_DEFINE_INT32(UART_ESC_BAUDRATE, 250000);
+PARAM_DEFINE_INT32(UART_ESC_BAUD, 250000);
 
 /**
  * The PX4 default motor mappings are
@@ -74,10 +76,10 @@ PARAM_DEFINE_INT32(UART_ESC_BAUDRATE, 250000);
  * [front]
  *         3     2
  */
-// PARAM_DEFINE_INT32(UART_ESC_PX4MOTOR1, 1);
-// PARAM_DEFINE_INT32(UART_ESC_PX4MOTOR2, 2);
-// PARAM_DEFINE_INT32(UART_ESC_PX4MOTOR3, 3);
-// PARAM_DEFINE_INT32(UART_ESC_PX4MOTOR4, 4);
+// PARAM_DEFINE_INT32(UART_ESC_MOTOR1, 1);
+// PARAM_DEFINE_INT32(UART_ESC_MOTOR2, 2);
+// PARAM_DEFINE_INT32(UART_ESC_MOTOR3, 3);
+// PARAM_DEFINE_INT32(UART_ESC_MOTOR4, 4);
 
 /**
  * Motor mappings for 350QX
@@ -85,10 +87,10 @@ PARAM_DEFINE_INT32(UART_ESC_BAUDRATE, 250000);
  * [front]
  *         1     2
  */
-// PARAM_DEFINE_INT32(UART_ESC_PX4MOTOR1, 4);
-// PARAM_DEFINE_INT32(UART_ESC_PX4MOTOR2, 2);
-// PARAM_DEFINE_INT32(UART_ESC_PX4MOTOR3, 1);
-// PARAM_DEFINE_INT32(UART_ESC_PX4MOTOR4, 3);
+// PARAM_DEFINE_INT32(UART_ESC_MOTOR1, 4);
+// PARAM_DEFINE_INT32(UART_ESC_MOTOR2, 2);
+// PARAM_DEFINE_INT32(UART_ESC_MOTOR3, 1);
+// PARAM_DEFINE_INT32(UART_ESC_MOTOR4, 3);
 
 /**
  * Motor mappings for 200QX
@@ -96,10 +98,10 @@ PARAM_DEFINE_INT32(UART_ESC_BAUDRATE, 250000);
  * [front]
  *         1     4
  */
-// PARAM_DEFINE_INT32(UART_ESC_PX4MOTOR1, 2);
-// PARAM_DEFINE_INT32(UART_ESC_PX4MOTOR2, 4);
-// PARAM_DEFINE_INT32(UART_ESC_PX4MOTOR3, 1);
-// PARAM_DEFINE_INT32(UART_ESC_PX4MOTOR4, 3);
+// PARAM_DEFINE_INT32(UART_ESC_MOTOR1, 2);
+// PARAM_DEFINE_INT32(UART_ESC_MOTOR2, 4);
+// PARAM_DEFINE_INT32(UART_ESC_MOTOR3, 1);
+// PARAM_DEFINE_INT32(UART_ESC_MOTOR4, 3);
 
 /**
  * Motor mappings for 210QC
@@ -107,8 +109,31 @@ PARAM_DEFINE_INT32(UART_ESC_BAUDRATE, 250000);
  * [front]
  *         1     2
  */
-PARAM_DEFINE_INT32(UART_ESC_PX4MOTOR1, 4);
-PARAM_DEFINE_INT32(UART_ESC_PX4MOTOR2, 2);
-PARAM_DEFINE_INT32(UART_ESC_PX4MOTOR3, 1);
-PARAM_DEFINE_INT32(UART_ESC_PX4MOTOR4, 3);
 
+/**
+ * Motor 1 Mapping
+ *
+ * @group Snapdragon UART ESC
+ */
+PARAM_DEFINE_INT32(UART_ESC_MOTOR1, 4);
+
+/**
+ * Motor 2 Mapping
+ *
+ * @group Snapdragon UART ESC
+ */
+PARAM_DEFINE_INT32(UART_ESC_MOTOR2, 2);
+
+/**
+ * Motor 3 Mapping
+ *
+ * @group Snapdragon UART ESC
+ */
+PARAM_DEFINE_INT32(UART_ESC_MOTOR3, 1);
+
+/**
+ * Motor 4 Mapping
+ *
+ * @group Snapdragon UART ESC
+ */
+PARAM_DEFINE_INT32(UART_ESC_MOTOR4, 3);

From 8f37e02c59ea24d5e46c7ab44ae61e399c2da4da Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Sat, 28 May 2016 12:23:29 +0200
Subject: [PATCH 0372/1230] Darwin: Print the relative CPU load produced by
 each thread

---
 src/modules/systemlib/print_load_posix.c | 101 ++++++++++++++++++++++-
 1 file changed, 100 insertions(+), 1 deletion(-)

diff --git a/src/modules/systemlib/print_load_posix.c b/src/modules/systemlib/print_load_posix.c
index 5e12dc3323..b89f3412cf 100644
--- a/src/modules/systemlib/print_load_posix.c
+++ b/src/modules/systemlib/print_load_posix.c
@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2015 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2015-2016 PX4 Development Team. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -39,13 +39,21 @@
  * @author Lorenz Meier <lorenz@px4.io>
  */
 
+#include <px4_posix.h>
+
+#include <unistd.h>
 #include <string.h>
 #include <stdio.h>
+#include <stdbool.h>
 
 #include <systemlib/cpuload.h>
 #include <systemlib/printload.h>
 #include <drivers/drv_hrt.h>
 
+#ifdef __PX4_DARWIN
+#include <mach/mach.h>
+#endif
+
 extern struct system_load_s system_load;
 
 #define CL "\033[K" // clear line
@@ -70,5 +78,96 @@ void init_print_load_s(uint64_t t, struct print_load_s *s)
 
 void print_load(uint64_t t, int fd, struct print_load_s *print_state)
 {
+	char *clear_line = "";
+
+	/* print system information */
+	if (fd == 1) {
+		dprintf(fd, "\033[H"); /* move cursor home and clear screen */
+		clear_line = CL;
+	}
+
+#ifdef __PX4_DARWIN
+	pid_t pid = getpid();   //-- this is the process id you need info for
+	task_t port;
+	task_for_pid(mach_task_self(), pid, &port);
+
+	task_info_data_t tinfo;
+	mach_msg_type_number_t task_info_count;
+
+	task_info_count = TASK_INFO_MAX;
+	kern_return_t kr = task_info(port, TASK_BASIC_INFO, (task_info_t)tinfo, &task_info_count);
+
+	if (kr != KERN_SUCCESS) {
+		return;
+	}
+
+	task_basic_info_t basic_info;
+	thread_array_t thread_list;
+	mach_msg_type_number_t thread_count;
+
+	thread_info_data_t th_info_data;
+	mach_msg_type_number_t thread_info_count;
+
+	thread_basic_info_t basic_info_th;
+	uint32_t stat_thread = 0;
+
+	basic_info = (task_basic_info_t)tinfo;
+
+	// get all threads of the PX4 main task
+	kr = task_threads(port, &thread_list, &thread_count);
+
+	if (kr != KERN_SUCCESS) {
+		PX4_WARN("ERROR getting thread list");
+		return;
+	}
+
+	if (thread_count > 0) {
+		stat_thread += thread_count;
+	}
+
+	long tot_sec = 0;
+	long tot_usec = 0;
+	long tot_cpu = 0;
+
+	dprintf(fd, "%sThreads: %d total\n",
+		clear_line,
+		thread_count);
+
+	for (int j = 0; j < thread_count; j++) {
+		thread_info_count = THREAD_INFO_MAX;
+		kr = thread_info(thread_list[j], THREAD_BASIC_INFO,
+				 (thread_info_t)th_info_data, &thread_info_count);
+
+		if (kr != KERN_SUCCESS) {
+			PX4_WARN("ERROR getting thread info");
+			continue;
+		}
+
+		basic_info_th = (thread_basic_info_t)th_info_data;
+
+
+		if (!(basic_info_th->flags & TH_FLAGS_IDLE)) {
+			tot_sec = tot_sec + basic_info_th->user_time.seconds + basic_info_th->system_time.seconds;
+			tot_usec = tot_usec + basic_info_th->system_time.microseconds + basic_info_th->system_time.microseconds;
+			tot_cpu = tot_cpu + basic_info_th->cpu_usage;
+		}
+
+		// char tname[128];
+
+		// int ret = pthread_getname_np(pthread_t *thread,
+		//                      const char *name, size_t len);
+
+		dprintf(fd, "thread %d\t\t %d\n", j, basic_info_th->cpu_usage);
+	}
+
+	kr = vm_deallocate(mach_task_self(), (vm_offset_t)thread_list,
+			   thread_count * sizeof(thread_t));
+
+	if (kr != KERN_SUCCESS) {
+		PX4_WARN("ERROR cleaning up thread info");
+		return;
+	}
+
+#endif
 }
 

From 04b491c365e048e5a434bdcc2f686c425883f851 Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Sat, 28 May 2016 13:40:14 +0200
Subject: [PATCH 0373/1230] Exit top on read failure

---
 src/systemcmds/top/top.c | 6 +++++-
 1 file changed, 5 insertions(+), 1 deletion(-)

diff --git a/src/systemcmds/top/top.c b/src/systemcmds/top/top.c
index 68e2801bab..6de022e9a0 100644
--- a/src/systemcmds/top/top.c
+++ b/src/systemcmds/top/top.c
@@ -89,7 +89,11 @@ top_main(int argc, char *argv[])
 
 			if (ret > 0) {
 
-				read(0, &c, 1);
+				ret = read(0, &c, 1);
+
+				if (ret) {
+					return 1;
+				}
 
 				switch (c) {
 				case 0x03: // ctrl-c

From a80223bc81e3e5bef40f4b4f8b497be5301073a1 Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Sat, 28 May 2016 13:57:32 +0200
Subject: [PATCH 0374/1230] systemlib: Added cases for TOP for Linux and QuRT

---
 src/modules/systemlib/print_load_posix.c | 35 ++++++++++++++----------
 1 file changed, 21 insertions(+), 14 deletions(-)

diff --git a/src/modules/systemlib/print_load_posix.c b/src/modules/systemlib/print_load_posix.c
index b89f3412cf..0ac8a615e1 100644
--- a/src/modules/systemlib/print_load_posix.c
+++ b/src/modules/systemlib/print_load_posix.c
@@ -86,16 +86,24 @@ void print_load(uint64_t t, int fd, struct print_load_s *print_state)
 		clear_line = CL;
 	}
 
-#ifdef __PX4_DARWIN
+#if defined (__PX4_LINUX)
+	dprintf(fd, "%sTOP NOT IMPLEMENTED ON LINUX\n",
+		clear_line);
+
+#elif defined (__PX4_QURT)
+	dprintf(fd, "%sTOP NOT IMPLEMENTED ON QURT\n",
+		clear_line);
+
+#elif defined (__PX4_DARWIN)
 	pid_t pid = getpid();   //-- this is the process id you need info for
-	task_t port;
-	task_for_pid(mach_task_self(), pid, &port);
+	task_t task_handle;
+	task_for_pid(mach_task_self(), pid, &task_handle);
 
 	task_info_data_t tinfo;
-	mach_msg_type_number_t task_info_count;
+	mach_msg_type_number_t th_info_cnt;
 
-	task_info_count = TASK_INFO_MAX;
-	kern_return_t kr = task_info(port, TASK_BASIC_INFO, (task_info_t)tinfo, &task_info_count);
+	th_info_cnt = TASK_INFO_MAX;
+	kern_return_t kr = task_info(task_handle, TASK_BASIC_INFO, (task_info_t)tinfo, &th_info_cnt);
 
 	if (kr != KERN_SUCCESS) {
 		return;
@@ -103,7 +111,7 @@ void print_load(uint64_t t, int fd, struct print_load_s *print_state)
 
 	task_basic_info_t basic_info;
 	thread_array_t thread_list;
-	mach_msg_type_number_t thread_count;
+	mach_msg_type_number_t th_cnt;
 
 	thread_info_data_t th_info_data;
 	mach_msg_type_number_t thread_info_count;
@@ -114,15 +122,15 @@ void print_load(uint64_t t, int fd, struct print_load_s *print_state)
 	basic_info = (task_basic_info_t)tinfo;
 
 	// get all threads of the PX4 main task
-	kr = task_threads(port, &thread_list, &thread_count);
+	kr = task_threads(task_handle, &thread_list, &th_cnt);
 
 	if (kr != KERN_SUCCESS) {
 		PX4_WARN("ERROR getting thread list");
 		return;
 	}
 
-	if (thread_count > 0) {
-		stat_thread += thread_count;
+	if (th_cnt > 0) {
+		stat_thread += th_cnt;
 	}
 
 	long tot_sec = 0;
@@ -131,9 +139,9 @@ void print_load(uint64_t t, int fd, struct print_load_s *print_state)
 
 	dprintf(fd, "%sThreads: %d total\n",
 		clear_line,
-		thread_count);
+		th_cnt);
 
-	for (int j = 0; j < thread_count; j++) {
+	for (int j = 0; j < th_cnt; j++) {
 		thread_info_count = THREAD_INFO_MAX;
 		kr = thread_info(thread_list[j], THREAD_BASIC_INFO,
 				 (thread_info_t)th_info_data, &thread_info_count);
@@ -145,7 +153,6 @@ void print_load(uint64_t t, int fd, struct print_load_s *print_state)
 
 		basic_info_th = (thread_basic_info_t)th_info_data;
 
-
 		if (!(basic_info_th->flags & TH_FLAGS_IDLE)) {
 			tot_sec = tot_sec + basic_info_th->user_time.seconds + basic_info_th->system_time.seconds;
 			tot_usec = tot_usec + basic_info_th->system_time.microseconds + basic_info_th->system_time.microseconds;
@@ -161,7 +168,7 @@ void print_load(uint64_t t, int fd, struct print_load_s *print_state)
 	}
 
 	kr = vm_deallocate(mach_task_self(), (vm_offset_t)thread_list,
-			   thread_count * sizeof(thread_t));
+			   th_cnt * sizeof(thread_t));
 
 	if (kr != KERN_SUCCESS) {
 		PX4_WARN("ERROR cleaning up thread info");

From 0d26bccbbc93b07beaddb169ad7fc87ab2651c13 Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Sat, 28 May 2016 14:50:53 +0200
Subject: [PATCH 0375/1230] Controllib: Params are not actual C files

---
 src/modules/controllib_test/CMakeLists.txt | 1 -
 1 file changed, 1 deletion(-)

diff --git a/src/modules/controllib_test/CMakeLists.txt b/src/modules/controllib_test/CMakeLists.txt
index 5d3a221fce..7cb86e5f8d 100644
--- a/src/modules/controllib_test/CMakeLists.txt
+++ b/src/modules/controllib_test/CMakeLists.txt
@@ -37,7 +37,6 @@ px4_add_module(
 		-Os
 	SRCS
 		controllib_test_main.cpp
-		test_params.c
 	DEPENDS
 		platforms__common
 	)

From 8bccd69e6c6ed0ad3404fcfa3f8f7b3d00992bb3 Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Sat, 28 May 2016 14:51:05 +0200
Subject: [PATCH 0376/1230] LPE: Params are not actually C files

---
 src/modules/local_position_estimator/CMakeLists.txt | 1 -
 1 file changed, 1 deletion(-)

diff --git a/src/modules/local_position_estimator/CMakeLists.txt b/src/modules/local_position_estimator/CMakeLists.txt
index 56ed858d57..7b118a18d0 100644
--- a/src/modules/local_position_estimator/CMakeLists.txt
+++ b/src/modules/local_position_estimator/CMakeLists.txt
@@ -46,7 +46,6 @@ px4_add_module(
 		sensors/baro.cpp
 		sensors/vision.cpp
 		sensors/mocap.cpp
-		params.c
 	DEPENDS
 		platforms__common
 	)

From e5431543d8e4bd9493d5a0608b5c4748c0e89e39 Mon Sep 17 00:00:00 2001
From: Paul Riseborough <p_riseborough@live.com.au>
Date: Thu, 26 May 2016 23:09:53 +1000
Subject: [PATCH 0377/1230] msg: add external vision data for ekf2 replay

---
 msg/ekf2_replay.msg | 6 ++++++
 1 file changed, 6 insertions(+)

diff --git a/msg/ekf2_replay.msg b/msg/ekf2_replay.msg
index e3127ef0e9..e19daae59b 100644
--- a/msg/ekf2_replay.msg
+++ b/msg/ekf2_replay.msg
@@ -7,6 +7,7 @@ uint64 baro_timestamp			# timestamp of barometer measurement in us
 uint64 rng_timestamp			# timestamp of range finder measurement in us
 uint64 flow_timestamp			# timestamp of optical flow measurement in us
 uint64 asp_timestamp			# timestamp of airspeed measurement in us
+uint64 ev_timestamp			# timestamp of external vision measurement in us
 
 float32[3] gyro_integral_rad		# integrated gyro vector in rad
 float32[3] accelerometer_integral_m_s  	# integrated accelerometer vector in m/s
@@ -46,3 +47,8 @@ float32 true_airspeed_unfiltered_m_s	# true airspeed in meters per second, -1 if
 float32 air_temperature_celsius		# air temperature in degrees celsius, -1000 if unknown
 float32 confidence			# confidence value from 0 to 1 for this sensor
 
+# external vision measurements
+float32[3] pos_ev			# position in earth (NED) frame (metres)
+float32[4] quat_ev			# quaternion rotation from earth (NED) to body (XYZ) frame
+float32 pos_err				# position error 1-std for each axis (metres)
+float32 ang_err				# angular error 1-std for each axis (rad)

From fc4d5ddb67e564517b49fbd180023e3723ee88e0 Mon Sep 17 00:00:00 2001
From: Paul Riseborough <p_riseborough@live.com.au>
Date: Thu, 26 May 2016 23:11:04 +1000
Subject: [PATCH 0378/1230] msg: add error estimates to vision pose and
 position data

---
 msg/vision_position_estimate.msg | 3 +++
 1 file changed, 3 insertions(+)

diff --git a/msg/vision_position_estimate.msg b/msg/vision_position_estimate.msg
index 53e2b9c0f0..62c6ff6b52 100644
--- a/msg/vision_position_estimate.msg
+++ b/msg/vision_position_estimate.msg
@@ -12,3 +12,6 @@ float32 vy			# Y velocity in meters per second in NED earth-fixed frame
 float32 vz			# Z velocity in meters per second in NED earth-fixed frame
 
 float32[4] q			# Estimated attitude as quaternion
+
+float32 pos_err			# position error 1-std for each axis (metres)
+float32 ang_err			# angular error 1-std for each axis (rad)

From 2e127a4737c5f365e93c6e16777741894f0a41d3 Mon Sep 17 00:00:00 2001
From: Paul Riseborough <p_riseborough@live.com.au>
Date: Thu, 26 May 2016 23:14:54 +1000
Subject: [PATCH 0379/1230] sdlog2: Add external vision data to ekf2 replay

Put struct definitions in enum order
Fix duplicate enum value
Add ekf2 replay message for external vision data
---
 src/modules/sdlog2/sdlog2.c          | 18 +++++-
 src/modules/sdlog2/sdlog2_messages.h | 84 +++++++++++++++++-----------
 2 files changed, 68 insertions(+), 34 deletions(-)

diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c
index db783325a5..aef45b0377 100644
--- a/src/modules/sdlog2/sdlog2.c
+++ b/src/modules/sdlog2/sdlog2.c
@@ -1251,8 +1251,9 @@ int sdlog2_thread_main(int argc, char *argv[])
 			struct log_EST6_s log_INO3;
 			struct log_RPL3_s log_RPL3;
 			struct log_RPL4_s log_RPL4;
-			struct log_RPL6_s log_RPL6;
+			struct log_RPL5_s log_RPL5;
 			struct log_LAND_s log_LAND;
+			struct log_RPL6_s log_RPL6;
 			struct log_LOAD_s log_LOAD;
 		} body;
 	} log_msg = {
@@ -1605,6 +1606,21 @@ int sdlog2_thread_main(int argc, char *argv[])
 					log_msg.body.log_RPL6.confidence = buf.replay.confidence;
 					LOGBUFFER_WRITE_AND_COUNT(RPL6);
 				}
+
+				if (buf.replay.ev_timestamp > 0) {
+					log_msg.msg_type = LOG_RPL5_MSG;
+					log_msg.body.log_RPL5.time_ev_usec = buf.replay.ev_timestamp;
+					log_msg.body.log_RPL5.x = buf.replay.pos_ev[0];
+					log_msg.body.log_RPL5.y = buf.replay.pos_ev[1];
+					log_msg.body.log_RPL5.z = buf.replay.pos_ev[2];
+					log_msg.body.log_RPL5.q0 = buf.replay.quat_ev[0];
+					log_msg.body.log_RPL5.q1 = buf.replay.quat_ev[1];
+					log_msg.body.log_RPL5.q2 = buf.replay.quat_ev[2];
+					log_msg.body.log_RPL5.q3 = buf.replay.quat_ev[3];
+					log_msg.body.log_RPL5.pos_err = buf.replay.pos_err;
+					log_msg.body.log_RPL5.ang_err = buf.replay.ang_err;
+					LOGBUFFER_WRITE_AND_COUNT(RPL5);
+				}
 			}
 		}
 
diff --git a/src/modules/sdlog2/sdlog2_messages.h b/src/modules/sdlog2/sdlog2_messages.h
index 71913d9041..48d22b1c94 100644
--- a/src/modules/sdlog2/sdlog2_messages.h
+++ b/src/modules/sdlog2/sdlog2_messages.h
@@ -420,24 +420,6 @@ struct log_EST3_s {
     float cov[16];
 };
 
-/* --- EST4 - ESTIMATOR INNOVATIONS --- */
-#define LOG_EST4_MSG 48
-struct log_EST4_s {
-    float s[12];
-};
-
-/* --- EST5 - ESTIMATOR INNOVATIONS --- */
-#define LOG_EST5_MSG 49
-struct log_EST5_s {
-    float s[10];
-};
-
-/* --- EST6 - ESTIMATOR INNOVATIONS --- */
-#define LOG_EST6_MSG 53
-struct log_EST6_s {
-    float s[6];
-};
-
 /* --- TEL0..3 - TELEMETRY STATUS --- */
 #define LOG_TEL0_MSG 36
 #define LOG_TEL1_MSG 37
@@ -518,6 +500,18 @@ struct log_CTS_s {
 	float yaw_rate;
 };
 
+/* --- EST4 - ESTIMATOR INNOVATIONS --- */
+#define LOG_EST4_MSG 48
+struct log_EST4_s {
+    float s[12];
+};
+
+/* --- EST5 - ESTIMATOR INNOVATIONS --- */
+#define LOG_EST5_MSG 49
+struct log_EST5_s {
+    float s[10];
+};
+
 #define LOG_OUT1_MSG 50
 
 /* --- EKF2 REPLAY Part 1 --- */
@@ -558,6 +552,13 @@ struct log_RPL2_s {
 	float vel_d_m_s;
 	bool vel_ned_valid;
 };
+
+/* --- EST6 - ESTIMATOR INNOVATIONS --- */
+#define LOG_EST6_MSG 53
+struct log_EST6_s {
+    float s[6];
+};
+
 /* --- EKF2 REPLAY Part 3 --- */
 #define LOG_RPL3_MSG 54
 struct log_RPL3_s {
@@ -570,6 +571,13 @@ struct log_RPL3_s {
 	uint8_t flow_quality;
 };
 
+/* --- CAMERA TRIGGER --- */
+#define LOG_CAMT_MSG 55
+struct log_CAMT_s {
+	uint64_t timestamp;
+	uint32_t seq;
+};
+
 /* --- EKF2 REPLAY Part 4 --- */
 #define LOG_RPL4_MSG 56
 struct log_RPL4_s {
@@ -577,7 +585,16 @@ struct log_RPL4_s {
 	float range_to_ground;
 };
 
-/* --- EKF2 REPLAY Part 4 --- */
+/* --- LAND DETECTOR --- */
+#define LOG_LAND_MSG 57
+struct log_LAND_s {
+	uint8_t landed;
+};
+
+/* 58 used for DGPS message
+ shares struct with GPS MSG 8*/
+
+/* --- EKF2 REPLAY Part 6 --- */
 #define LOG_RPL6_MSG 59
 struct log_RPL6_s {
 	uint64_t time_airs_usec;
@@ -588,23 +605,23 @@ struct log_RPL6_s {
 	float confidence;
 };
 
-
-
-/* --- CAMERA TRIGGER --- */
-#define LOG_CAMT_MSG 55
-struct log_CAMT_s {
-	uint64_t timestamp;
-	uint32_t seq;
-};
-
-/* --- LAND DETECTOR --- */
-#define LOG_LAND_MSG 57
-struct log_LAND_s {
-	uint8_t landed;
+/* --- EKF2 REPLAY Part 5 --- */
+#define LOG_RPL5_MSG 60
+struct log_RPL5_s {
+	uint64_t time_ev_usec;
+	float x;
+	float y;
+	float z;
+	float q0;
+	float q1;
+	float q2;
+	float q3;
+	float pos_err;
+	float ang_err;
 };
 
 /* --- SYSTEM LOAD --- */
-#define LOG_LOAD_MSG 58
+#define LOG_LOAD_MSG 61
 struct log_LOAD_s {
 	float cpu_load;
 };
@@ -694,6 +711,7 @@ static const struct log_format_s log_formats[] = {
 	LOG_FORMAT(RPL2, "QQLLiMMfffffffM", "Tpos,Tvel,lat,lon,alt,fix,nsats,eph,epv,sacc,v,vN,vE,vD,v_val"),
 	LOG_FORMAT(RPL3, "QffffIB", "Tflow,fx,fy,gx,gy,delT,qual"),
 	LOG_FORMAT(RPL4, "Qf", "Trng,rng"),
+	LOG_FORMAT(RPL5, "Qfffffffff", "Tev,x,y,z,q0,q1,q2,q3,posErr,angErr"),
 	LOG_FORMAT(RPL6, "Qfffff", "Tasp,inAsp,trAsp,ufAsp,tpAsp,confAsp"),
 	LOG_FORMAT(LAND, "B", "Landed"),
 	LOG_FORMAT(LOAD, "f", "CPU"),

From 57c1138d284c3a7668bc357868a0b319608ec45b Mon Sep 17 00:00:00 2001
From: Paul Riseborough <p_riseborough@live.com.au>
Date: Thu, 26 May 2016 23:25:21 +1000
Subject: [PATCH 0380/1230] ekf2: add parameters for control of external vision
 fusion

---
 src/modules/ekf2/ekf2_main.cpp | 16 +++++++
 src/modules/ekf2/ekf2_params.c | 77 ++++++++++++++++++++++++++++++++--
 2 files changed, 90 insertions(+), 3 deletions(-)

diff --git a/src/modules/ekf2/ekf2_main.cpp b/src/modules/ekf2/ekf2_main.cpp
index de40308a0c..fd73caccea 100644
--- a/src/modules/ekf2/ekf2_main.cpp
+++ b/src/modules/ekf2/ekf2_main.cpp
@@ -168,6 +168,7 @@ private:
 	control::BlockParamFloat _flow_delay_ms;
 	control::BlockParamFloat _rng_delay_ms;
 	control::BlockParamFloat _airspeed_delay_ms;
+	control::BlockParamFloat _ev_delay_ms;
 
 	control::BlockParamFloat _gyro_noise;
 	control::BlockParamFloat _accel_noise;
@@ -220,6 +221,11 @@ private:
 	control::BlockParamFloat _range_innov_gate;	// range finder fusion innovation consistency gate size (STD)
 	control::BlockParamFloat _rng_gnd_clearance;	// minimum valid value for range when on ground (m)
 
+	// vision estimate fusion
+	control::BlockParamFloat _ev_noise;		// observation noise for range finder measurements (m)
+	control::BlockParamFloat _ev_innov_gate;	// range finder fusion innovation consistency gate size (STD)
+	control::BlockParamFloat _ev_gnd_clearance;	// minimum valid value for range when on ground (m)
+
 	// optical flow fusion
 	control::BlockParamFloat
 	_flow_noise;		// best quality observation noise for optical flow LOS rate measurements (rad/sec)
@@ -242,6 +248,9 @@ private:
 	control::BlockParamFloat _flow_pos_x;	// X position of optical flow sensor focal point in body frame (m)
 	control::BlockParamFloat _flow_pos_y;	// Y position of optical flow sensor focal point in body frame (m)
 	control::BlockParamFloat _flow_pos_z;	// Z position of optical flow sensor focal point in body frame (m)
+	control::BlockParamFloat _ev_pos_x;	// X position of VI sensor focal point in body frame (m)
+	control::BlockParamFloat _ev_pos_y;	// Y position of VI sensor focal point in body frame (m)
+	control::BlockParamFloat _ev_pos_z;	// Z position of VI sensor focal point in body frame (m)
 
 	// output predictor filter time constants
 	control::BlockParamFloat _tau_vel;	// time constant used by the output velocity complementary filter (s)
@@ -279,6 +288,7 @@ Ekf2::Ekf2():
 	_flow_delay_ms(this, "EKF2_OF_DELAY", false, &_params->flow_delay_ms),
 	_rng_delay_ms(this, "EKF2_RNG_DELAY", false, &_params->range_delay_ms),
 	_airspeed_delay_ms(this, "EKF2_ASP_DELAY", false, &_params->airspeed_delay_ms),
+	_ev_delay_ms(this, "EKF2_EV_DELAY", false, &_params->ev_delay_ms),
 	_gyro_noise(this, "EKF2_GYR_NOISE", false, &_params->gyro_noise),
 	_accel_noise(this, "EKF2_ACC_NOISE", false, &_params->accel_noise),
 	_gyro_bias_p_noise(this, "EKF2_GYR_B_NOISE", false, &_params->gyro_bias_p_noise),
@@ -318,6 +328,9 @@ Ekf2::Ekf2():
 	_range_noise(this, "EKF2_RNG_NOISE", false, &_params->range_noise),
 	_range_innov_gate(this, "EKF2_RNG_GATE", false, &_params->range_innov_gate),
 	_rng_gnd_clearance(this, "EKF2_MIN_RNG", false, &_params->rng_gnd_clearance),
+	_ev_noise(this, "EKF2_EV_NOISE", false, &_params->ev_noise),
+	_ev_innov_gate(this, "EKF2_EV_GATE", false, &_params->ev_innov_gate),
+	_ev_gnd_clearance(this, "EKF2_MIN_EV", false, &_params->ev_gnd_clearance),
 	_flow_noise(this, "EKF2_OF_N_MIN", false, &_params->flow_noise),
 	_flow_noise_qual_min(this, "EKF2_OF_N_MAX", false, &_params->flow_noise_qual_min),
 	_flow_qual_min(this, "EKF2_OF_QMIN", false, &_params->flow_qual_min),
@@ -335,6 +348,9 @@ Ekf2::Ekf2():
 	_flow_pos_x(this, "EKF2_OF_POS_X", false, &_params->flow_pos_body(0)),
 	_flow_pos_y(this, "EKF2_OF_POS_Y", false, &_params->flow_pos_body(1)),
 	_flow_pos_z(this, "EKF2_OF_POS_Z", false, &_params->flow_pos_body(2)),
+	_ev_pos_x(this, "EKF2_EV_POS_X", false, &_params->ev_pos_body(0)),
+	_ev_pos_y(this, "EKF2_EV_POS_Y", false, &_params->ev_pos_body(1)),
+	_ev_pos_z(this, "EKF2_EV_POS_Z", false, &_params->ev_pos_body(2)),
 	_tau_vel(this, "EKF2_TAU_VEL", false, &_params->vel_Tau),
 	_tau_pos(this, "EKF2_TAU_POS", false, &_params->pos_Tau),
 	_gyr_bias_init(this, "EKF2_GBIAS_INIT", false, &_params->switch_on_gyro_bias),
diff --git a/src/modules/ekf2/ekf2_params.c b/src/modules/ekf2/ekf2_params.c
index 1c09ec2320..33e23c6413 100644
--- a/src/modules/ekf2/ekf2_params.c
+++ b/src/modules/ekf2/ekf2_params.c
@@ -108,6 +108,17 @@ PARAM_DEFINE_FLOAT(EKF2_RNG_DELAY, 5);
  */
 PARAM_DEFINE_FLOAT(EKF2_ASP_DELAY, 200);
 
+/**
+ * Vision Position Estimator delay relative to IMU measurements
+ *
+ * @group EKF2
+ * @min 0
+ * @max 300
+ * @unit ms
+ * @decimal 1
+ */
+PARAM_DEFINE_FLOAT(EKF2_EV_DELAY, 175);
+
 /**
  * Integer bitmask controlling GPS checks.
  * 
@@ -471,10 +482,12 @@ PARAM_DEFINE_INT32(EKF2_REC_RPL, 0);
  * 0 : Set to true to use GPS data if available
  * 1 : Set to true to use optical flow data if available
  * 2 : Set to true to inhibit IMU bias estimation
- *
+ * 3 : Set to true to enable vision position fusion
+ * 4 : Set to true to enable vision yaw fusion
+ * *
  * @group EKF2
  * @min 0
- * @max 15
+ * @max 28
  */
 PARAM_DEFINE_INT32(EKF2_AID_MASK, 1);
 
@@ -520,6 +533,36 @@ PARAM_DEFINE_FLOAT(EKF2_RNG_GATE, 5.0f);
  */
 PARAM_DEFINE_FLOAT(EKF2_MIN_RNG, 0.1f);
 
+
+/**
+ * Measurement noise for vision estimate fusion
+ *
+ * @group EKF2
+ * @min 0.01
+ * @unit m
+ * @decimal 2
+ */
+PARAM_DEFINE_FLOAT(EKF2_EV_NOISE, 0.05f);
+
+/**
+ * Gate size for vision estimate fusion
+ *
+ * @group EKF2
+ * @min 1.0
+ * @unit SD
+ * @decimal 1
+ */
+PARAM_DEFINE_FLOAT(EKF2_EV_GATE, 5.0f);
+
+/**
+ * Minimum valid range for the vision estimate
+ *
+ * @group EKF2
+ * @min 0.01
+ * @unit m
+ * @decimal 2
+ */
+PARAM_DEFINE_FLOAT(EKF2_MIN_EV, 0.01f);
 /**
  * Measurement noise for the optical flow sensor when it's reported quality metric is at the maximum
  *
@@ -701,7 +744,35 @@ PARAM_DEFINE_FLOAT(EKF2_OF_POS_Y, 0.0f);
 PARAM_DEFINE_FLOAT(EKF2_OF_POS_Z, 0.0f);
 
 /**
- * Time constant of the velocity output prediction and smoothing filter. Controls how tightly the velocity output tracks the EKF states. Set to a negative number to disable the EKF velocity state tracking. This will cause the output velocity to track the output position time derivative.
+* X position of VI sensor focal point in body frame
+ *
+ * @group EKF2
+ * @unit m
+ * @decimal 3
+ */
+PARAM_DEFINE_FLOAT(EKF2_EV_POS_X, 0.0f);
+
+/**
+ * Y position of VI sensor focal point in body frame
+ *
+ * @group EKF2
+ * @unit m
+ * @decimal 3
+ */
+PARAM_DEFINE_FLOAT(EKF2_EV_POS_Y, 0.0f);
+
+/**
+ * Z position of VI sensor focal point in body frame
+ *
+ * @group EKF2
+ * @unit m
+ * @decimal 3
+ */
+PARAM_DEFINE_FLOAT(EKF2_EV_POS_Z, 0.0f);
+
+/**
+
+ * Time constant of the velocity output prediction and smoothing filter
  *
  * @group EKF2
  * @max 1.0

From 37b4955f07ed3dbdda949b92a2c9518847041848 Mon Sep 17 00:00:00 2001
From: Paul Riseborough <p_riseborough@live.com.au>
Date: Thu, 26 May 2016 23:29:19 +1000
Subject: [PATCH 0381/1230] ekf2: Add use and logging of external vision data

---
 src/modules/ekf2/ekf2_main.cpp | 44 +++++++++++++++++++++++++++++++++-
 1 file changed, 43 insertions(+), 1 deletion(-)

diff --git a/src/modules/ekf2/ekf2_main.cpp b/src/modules/ekf2/ekf2_main.cpp
index fd73caccea..1670e0d4e2 100644
--- a/src/modules/ekf2/ekf2_main.cpp
+++ b/src/modules/ekf2/ekf2_main.cpp
@@ -69,6 +69,7 @@
 #include <uORB/topics/airspeed.h>
 #include <uORB/topics/vehicle_attitude.h>
 #include <uORB/topics/vehicle_local_position.h>
+#include <uORB/topics/vision_position_estimate.h>
 #include <uORB/topics/control_state.h>
 #include <uORB/topics/vehicle_global_position.h>
 #include <uORB/topics/wind_estimate.h>
@@ -126,7 +127,7 @@ public:
 
 private:
 	static constexpr float _dt_max = 0.02;
-	bool		_task_should_exit = false;
+	bool	_task_should_exit = false;
 	int		_control_task = -1;			// task handle for task
 	bool 	_replay_mode;	// should we use replay data from a log
 	int 	_publish_replay_mode;	// defines if we should publish replay messages
@@ -137,6 +138,7 @@ private:
 	int		_params_sub = -1;
 	int 	_optical_flow_sub = -1;
 	int 	_range_finder_sub = -1;
+	int 	_ev_pos_sub = -1;
 	int		_actuator_armed_sub = -1;
 	int		_vehicle_land_detected_sub = -1;
 
@@ -380,6 +382,7 @@ void Ekf2::task_main()
 	_params_sub = orb_subscribe(ORB_ID(parameter_update));
 	_optical_flow_sub = orb_subscribe(ORB_ID(optical_flow));
 	_range_finder_sub = orb_subscribe(ORB_ID(distance_sensor));
+	_ev_pos_sub = orb_subscribe(ORB_ID(vision_position_estimate));
 	_vehicle_land_detected_sub = orb_subscribe(ORB_ID(vehicle_land_detected));
 
 	px4_pollfd_struct_t fds[2] = {};
@@ -400,6 +403,11 @@ void Ekf2::task_main()
 	optical_flow_s optical_flow = {};
 	distance_sensor_s range_finder = {};
 	vehicle_land_detected_s vehicle_land_detected = {};
+	vision_position_estimate_s ev = {};
+
+	// assumed errors for external vision data missing from current interface
+	const float pos_ev_err = 0.05; // position error 1-std (metres)
+	const float ang_ev_err = 0.05; // angular error 1-std (rad)
 
 	while (!_task_should_exit) {
 		int ret = px4_poll(fds, sizeof(fds) / sizeof(fds[0]), 1000);
@@ -433,6 +441,7 @@ void Ekf2::task_main()
 		bool optical_flow_updated = false;
 		bool range_finder_updated = false;
 		bool vehicle_land_detected_updated = false;
+		bool vision_position_updated = false;
 
 		orb_copy(ORB_ID(sensor_combined), _sensors_sub, &sensors);
 		// update all other topics if they have new data
@@ -460,6 +469,12 @@ void Ekf2::task_main()
 			orb_copy(ORB_ID(distance_sensor), _range_finder_sub, &range_finder);
 		}
 
+		orb_check(_ev_pos_sub, &vision_position_updated);
+
+		if (vision_position_updated) {
+			orb_copy(ORB_ID(vision_position_estimate), _ev_pos_sub, &ev);
+		}
+
 		// in replay mode we are getting the actual timestamp from the sensor topic
 		hrt_abstime now = 0;
 
@@ -531,6 +546,18 @@ void Ekf2::task_main()
 			_ekf.setRangeData(range_finder.timestamp, &range_finder.current_distance);
 		}
 
+		if (vision_position_updated) {
+			ext_vision_message ev_data;
+			ev_data.posNED(0) = ev.x;
+			ev_data.posNED(1) = ev.y;
+			ev_data.posNED(2) = ev.z;
+			Quaternion q(ev.q);
+			ev_data.quat = q;
+			ev_data.posErr = pos_ev_err; // TODO replace with errors published by the EV system when available
+			ev_data.angErr = ang_ev_err; // TODO replace with errors published by the EV system when available
+			_ekf.setExtVisionData(ev.timestamp_computer, &ev_data);
+		}
+
 		orb_check(_vehicle_land_detected_sub, &vehicle_land_detected_updated);
 
 		if (vehicle_land_detected_updated) {
@@ -881,6 +908,21 @@ void Ekf2::task_main()
 				replay.asp_timestamp = 0;
 			}
 
+			if (vision_position_updated) {
+				replay.ev_timestamp = ev.timestamp_computer;
+				replay.pos_ev[0] = ev.x;
+				replay.pos_ev[1] = ev.y;
+				replay.pos_ev[2] = ev.z;
+				replay.quat_ev[0] = ev.q[0];
+				replay.quat_ev[1] = ev.q[1];
+				replay.quat_ev[2] = ev.q[2];
+				replay.quat_ev[3] = ev.q[3];
+				replay.pos_err = pos_ev_err; // TODO replace with errors published by the EV system when available
+				replay.ang_err = ang_ev_err; // TODO replace with errors published by the EV system when available
+
+			} else {
+				replay.ev_timestamp = 0;
+			}
 
 			if (_replay_pub == nullptr) {
 				_replay_pub = orb_advertise(ORB_ID(ekf2_replay), &replay);

From 26d81418fa5f442a07865007110b2ec398019583 Mon Sep 17 00:00:00 2001
From: Paul Riseborough <p_riseborough@live.com.au>
Date: Thu, 26 May 2016 23:30:14 +1000
Subject: [PATCH 0382/1230] ekf2: Add external vision to replay

---
 src/modules/ekf2_replay/ekf2_replay_main.cpp | 38 +++++++++++++++++---
 1 file changed, 33 insertions(+), 5 deletions(-)

diff --git a/src/modules/ekf2_replay/ekf2_replay_main.cpp b/src/modules/ekf2_replay/ekf2_replay_main.cpp
index bb212935d6..c6ff14e86a 100644
--- a/src/modules/ekf2_replay/ekf2_replay_main.cpp
+++ b/src/modules/ekf2_replay/ekf2_replay_main.cpp
@@ -70,6 +70,7 @@
 #include <uORB/topics/optical_flow.h>
 #include <uORB/topics/distance_sensor.h>
 #include <uORB/topics/airspeed.h>
+#include <uORB/topics/vision_position_estimate.h>
 
 #include <sdlog2/sdlog2_messages.h>
 
@@ -135,6 +136,7 @@ private:
 	orb_advert_t _flow_pub;
 	orb_advert_t _range_pub;
 	orb_advert_t _airspeed_pub;
+	orb_advert_t _ev_pub;
 
 	int _att_sub;
 	int _estimator_status_sub;
@@ -151,6 +153,7 @@ private:
 	struct optical_flow_s _flow;
 	struct distance_sensor_s _range;
 	struct airspeed_s _airspeed;
+	struct vision_position_estimate_s _ev;
 
 	unsigned _message_counter; // counter which will increase with every message read from the log
 	unsigned _part1_counter_ref;		// this is the value of _message_counter when the part1 of the replay message is read (imu data)
@@ -158,6 +161,7 @@ private:
 	bool _read_part3;
 	bool _read_part4;
 	bool _read_part6;
+	bool _read_part5;
 
 	int _write_fd = -1;
 	px4_pollfd_struct_t _fds[1];
@@ -206,6 +210,7 @@ Ekf2Replay::Ekf2Replay(char *logfile) :
 	_flow_pub(nullptr),
 	_range_pub(nullptr),
 	_airspeed_pub(nullptr),
+	_ev_pub(nullptr),
 	_att_sub(-1),
 	_estimator_status_sub(-1),
 	_innov_sub(-1),
@@ -223,6 +228,7 @@ Ekf2Replay::Ekf2Replay(char *logfile) :
 	_read_part3(false),
 	_read_part4(false),
 	_read_part6(false),
+	_read_part5(false),
 	_write_fd(-1)
 {
 	// build the path to the log
@@ -270,6 +276,15 @@ void Ekf2Replay::publishEstimatorInput()
 
 	_read_part4 = false;
 
+	if (_ev_pub == nullptr && _read_part5) {
+		_ev_pub = orb_advertise(ORB_ID(vision_position_estimate), &_ev);
+
+	} else if (_ev_pub != nullptr && _read_part5) {
+		orb_publish(ORB_ID(vision_position_estimate), _ev_pub, &_ev);
+	}
+
+	_read_part5 = false;
+
 	if (_sensors_pub == nullptr) {
 		_sensors_pub = orb_advertise(ORB_ID(sensor_combined), &_sensors);
 
@@ -347,6 +362,7 @@ void Ekf2Replay::setEstimatorInput(uint8_t *data, uint8_t type)
 	struct log_RPL3_s replay_part3 = {};
 	struct log_RPL4_s replay_part4 = {};
 	struct log_RPL6_s replay_part6 = {};
+	struct log_RPL5_s replay_part5 = {};
 	struct log_LAND_s vehicle_landed = {};
 
 	if (type == LOG_RPL1_MSG) {
@@ -409,9 +425,7 @@ void Ekf2Replay::setEstimatorInput(uint8_t *data, uint8_t type)
 		_range.current_distance = replay_part4.range_to_ground;
 		_read_part4 = true;
 
-	} 
-
-	else if (type == LOG_RPL6_MSG){
+	} else if (type == LOG_RPL6_MSG){
 		uint8_t *dest_ptr = (uint8_t *)&replay_part6.time_airs_usec;
 		parseMessage(data, dest_ptr, type);
 		_airspeed.timestamp = replay_part6.time_airs_usec;
@@ -422,9 +436,23 @@ void Ekf2Replay::setEstimatorInput(uint8_t *data, uint8_t type)
 		_airspeed.confidence = replay_part6.confidence;
 		_read_part6 = true;
 
-	}
+	} else if (type == LOG_RPL5_MSG) {
+		uint8_t *dest_ptr = (uint8_t *)&replay_part5.time_ev_usec;
+		parseMessage(data, dest_ptr, type);
+		_ev.timestamp = replay_part5.time_ev_usec;
+		_ev.timestamp_computer = replay_part5.time_ev_usec; // fake this timestamp
+		_ev.x = replay_part5.x;
+		_ev.y = replay_part5.y;
+		_ev.z = replay_part5.z;
+		_ev.q[0] = replay_part5.q0;
+		_ev.q[1] = replay_part5.q1;
+		_ev.q[2] = replay_part5.q2;
+		_ev.q[3] = replay_part5.q3;
+		_ev.pos_err = replay_part5.pos_err;
+		_ev.ang_err = replay_part5.pos_err;
+		_read_part5 = true;
 
-	else if (type == LOG_LAND_MSG) {
+	} else if (type == LOG_LAND_MSG) {
 		uint8_t *dest_ptr = (uint8_t *)&vehicle_landed.landed;
 		parseMessage(data, dest_ptr, type);
 		_land_detected.landed =  vehicle_landed.landed;

From ac50510c78ada1067cd47d8d7a939048ab2f8730 Mon Sep 17 00:00:00 2001
From: Paul Riseborough <p_riseborough@live.com.au>
Date: Fri, 27 May 2016 11:39:45 +1000
Subject: [PATCH 0383/1230] ekf2: Use parameter defined values for EV noise if
 vision system estimates not available

---
 src/modules/ekf2/ekf2_main.cpp | 57 ++++++++++++++++++++--------------
 src/modules/ekf2/ekf2_params.c | 14 +++++++--
 2 files changed, 46 insertions(+), 25 deletions(-)

diff --git a/src/modules/ekf2/ekf2_main.cpp b/src/modules/ekf2/ekf2_main.cpp
index 1670e0d4e2..26c59cf93d 100644
--- a/src/modules/ekf2/ekf2_main.cpp
+++ b/src/modules/ekf2/ekf2_main.cpp
@@ -128,21 +128,23 @@ public:
 private:
 	static constexpr float _dt_max = 0.02;
 	bool	_task_should_exit = false;
-	int		_control_task = -1;			// task handle for task
-	bool 	_replay_mode;	// should we use replay data from a log
-	int 	_publish_replay_mode;	// defines if we should publish replay messages
+	int	_control_task = -1;		// task handle for task
+	bool 	_replay_mode;			// should we use replay data from a log
+	int 	_publish_replay_mode;		// defines if we should publish replay messages
+	float	_default_ev_pos_noise = 0.05f;	// external vision position noise used when an invalid value is supplied
+	float	_default_ev_ang_noise = 0.05f;	// external vision angle noise used when an invalid value is supplied
 
-	int		_sensors_sub = -1;
-	int		_gps_sub = -1;
-	int		_airspeed_sub = -1;
-	int		_params_sub = -1;
+	int	_sensors_sub = -1;
+	int	_gps_sub = -1;
+	int	_airspeed_sub = -1;
+	int	_params_sub = -1;
 	int 	_optical_flow_sub = -1;
 	int 	_range_finder_sub = -1;
 	int 	_ev_pos_sub = -1;
-	int		_actuator_armed_sub = -1;
-	int		_vehicle_land_detected_sub = -1;
+	int	_actuator_armed_sub = -1;
+	int	_vehicle_land_detected_sub = -1;
 
-	bool            _prev_landed = true; // landed status from the previous frame
+	bool	_prev_landed = true;	// landed status from the previous frame
 
 	float _acc_hor_filt = 0.0f; 	// low-pass filtered horizontal acceleration
 
@@ -224,9 +226,9 @@ private:
 	control::BlockParamFloat _rng_gnd_clearance;	// minimum valid value for range when on ground (m)
 
 	// vision estimate fusion
-	control::BlockParamFloat _ev_noise;		// observation noise for range finder measurements (m)
-	control::BlockParamFloat _ev_innov_gate;	// range finder fusion innovation consistency gate size (STD)
-	control::BlockParamFloat _ev_gnd_clearance;	// minimum valid value for range when on ground (m)
+	control::BlockParamFloat _ev_pos_noise;		// default position observation noise for exernal vision measurements (m)
+	control::BlockParamFloat _ev_ang_noise;		// default angular observation noise for exernal vision measurements (rad)
+	control::BlockParamFloat _ev_innov_gate;	// external vision position innovation consistency gate size (STD)
 
 	// optical flow fusion
 	control::BlockParamFloat
@@ -330,9 +332,9 @@ Ekf2::Ekf2():
 	_range_noise(this, "EKF2_RNG_NOISE", false, &_params->range_noise),
 	_range_innov_gate(this, "EKF2_RNG_GATE", false, &_params->range_innov_gate),
 	_rng_gnd_clearance(this, "EKF2_MIN_RNG", false, &_params->rng_gnd_clearance),
-	_ev_noise(this, "EKF2_EV_NOISE", false, &_params->ev_noise),
+	_ev_pos_noise(this, "EKF2_EVP_NOISE", false, &_default_ev_pos_noise),
+	_ev_ang_noise(this, "EKF2_EVA_NOISE", false, &_default_ev_ang_noise),
 	_ev_innov_gate(this, "EKF2_EV_GATE", false, &_params->ev_innov_gate),
-	_ev_gnd_clearance(this, "EKF2_MIN_EV", false, &_params->ev_gnd_clearance),
 	_flow_noise(this, "EKF2_OF_N_MIN", false, &_params->flow_noise),
 	_flow_noise_qual_min(this, "EKF2_OF_N_MAX", false, &_params->flow_noise_qual_min),
 	_flow_qual_min(this, "EKF2_OF_QMIN", false, &_params->flow_qual_min),
@@ -405,10 +407,6 @@ void Ekf2::task_main()
 	vehicle_land_detected_s vehicle_land_detected = {};
 	vision_position_estimate_s ev = {};
 
-	// assumed errors for external vision data missing from current interface
-	const float pos_ev_err = 0.05; // position error 1-std (metres)
-	const float ang_ev_err = 0.05; // angular error 1-std (rad)
-
 	while (!_task_should_exit) {
 		int ret = px4_poll(fds, sizeof(fds) / sizeof(fds[0]), 1000);
 
@@ -546,6 +544,8 @@ void Ekf2::task_main()
 			_ekf.setRangeData(range_finder.timestamp, &range_finder.current_distance);
 		}
 
+		// get external vision data
+		// if error estimates are unavailable, use parameter defined defaults
 		if (vision_position_updated) {
 			ext_vision_message ev_data;
 			ev_data.posNED(0) = ev.x;
@@ -553,8 +553,19 @@ void Ekf2::task_main()
 			ev_data.posNED(2) = ev.z;
 			Quaternion q(ev.q);
 			ev_data.quat = q;
-			ev_data.posErr = pos_ev_err; // TODO replace with errors published by the EV system when available
-			ev_data.angErr = ang_ev_err; // TODO replace with errors published by the EV system when available
+			// position measurement error
+			if (ev.pos_err >= 0.001f) {
+				ev_data.posErr = ev.pos_err;
+			} else {
+				ev_data.posErr = _default_ev_pos_noise;
+			}
+			// angle measurement error
+			if (ev.ang_err >= 0.001f) {
+				ev_data.angErr = ev.ang_err;
+			} else {
+				ev_data.angErr = _default_ev_ang_noise;
+			}
+			// use timestamp from external computer - requires clocks to be synchronised so may not be a good idea
 			_ekf.setExtVisionData(ev.timestamp_computer, &ev_data);
 		}
 
@@ -917,8 +928,8 @@ void Ekf2::task_main()
 				replay.quat_ev[1] = ev.q[1];
 				replay.quat_ev[2] = ev.q[2];
 				replay.quat_ev[3] = ev.q[3];
-				replay.pos_err = pos_ev_err; // TODO replace with errors published by the EV system when available
-				replay.ang_err = ang_ev_err; // TODO replace with errors published by the EV system when available
+				replay.pos_err = ev.pos_err;
+				replay.ang_err = ev.ang_err;
 
 			} else {
 				replay.ev_timestamp = 0;
diff --git a/src/modules/ekf2/ekf2_params.c b/src/modules/ekf2/ekf2_params.c
index 33e23c6413..df97467614 100644
--- a/src/modules/ekf2/ekf2_params.c
+++ b/src/modules/ekf2/ekf2_params.c
@@ -535,14 +535,24 @@ PARAM_DEFINE_FLOAT(EKF2_MIN_RNG, 0.1f);
 
 
 /**
- * Measurement noise for vision estimate fusion
+ * Measurement noise for vision position observations used when the vision system does not supply error estimates
  *
  * @group EKF2
  * @min 0.01
  * @unit m
  * @decimal 2
  */
-PARAM_DEFINE_FLOAT(EKF2_EV_NOISE, 0.05f);
+PARAM_DEFINE_FLOAT(EKF2_EVP_NOISE, 0.05f);
+
+/**
+ * Measurement noise for vision angle observations used when the vision system does not supply error estimates
+ *
+ * @group EKF2
+ * @min 0.01
+ * @unit rad
+ * @decimal 2
+ */
+PARAM_DEFINE_FLOAT(EKF2_EVA_NOISE, 0.05f);
 
 /**
  * Gate size for vision estimate fusion

From 2ce8056b8f2d9d3c2489e9f11f3a1edc9b14fb0e Mon Sep 17 00:00:00 2001
From: Paul Riseborough <p_riseborough@live.com.au>
Date: Fri, 27 May 2016 11:45:04 +1000
Subject: [PATCH 0384/1230] mavlink: publish values for vision system errors

Sets zero values as a placeholder until mavlink can be updated.
---
 src/modules/mavlink/mavlink_receiver.cpp | 4 ++++
 1 file changed, 4 insertions(+)

diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp
index 5ee0312fdc..557c5caefd 100644
--- a/src/modules/mavlink/mavlink_receiver.cpp
+++ b/src/modules/mavlink/mavlink_receiver.cpp
@@ -967,6 +967,10 @@ MavlinkReceiver::handle_message_vision_position_estimate(mavlink_message_t *msg)
 	vision_position.q[2] = q(2);
 	vision_position.q[3] = q(3);
 
+	// TODO fix this
+	vision_position.pos_err = 0.0f;
+	vision_position.ang_err = 0.0f;
+
 	if (_vision_position_pub == nullptr) {
 		_vision_position_pub = orb_advertise(ORB_ID(vision_position_estimate), &vision_position);
 

From 9e0ab5d266c05f0ca5c674cac13ae4a549ebc86a Mon Sep 17 00:00:00 2001
From: Paul Riseborough <p_riseborough@live.com.au>
Date: Fri, 27 May 2016 13:19:23 +1000
Subject: [PATCH 0385/1230] ekf2: update height source parameter documentation

---
 src/modules/ekf2/ekf2_params.c | 4 +++-
 1 file changed, 3 insertions(+), 1 deletion(-)

diff --git a/src/modules/ekf2/ekf2_params.c b/src/modules/ekf2/ekf2_params.c
index df97467614..f316495926 100644
--- a/src/modules/ekf2/ekf2_params.c
+++ b/src/modules/ekf2/ekf2_params.c
@@ -498,8 +498,10 @@ PARAM_DEFINE_INT32(EKF2_AID_MASK, 1);
  *
  * @group EKF2
  * @value 0 Barometric pressure
- * @value 1 Reserved (GPS)
+ * @value 1 GPS
  * @value 2 Range sensor
+ * @value 3 Vision
+ *
  */
 PARAM_DEFINE_INT32(EKF2_HGT_MODE, 0);
 

From c2e2645c828a545b5c5d16a620d5191462907e5a Mon Sep 17 00:00:00 2001
From: Paul Riseborough <p_riseborough@live.com.au>
Date: Thu, 26 May 2016 23:32:11 +1000
Subject: [PATCH 0386/1230] ecl: update submodule reference

Improvements and bug fixes for external vision processing and filter initialisation
---
 src/lib/ecl | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/src/lib/ecl b/src/lib/ecl
index e2d9e19a5d..654093cae1 160000
--- a/src/lib/ecl
+++ b/src/lib/ecl
@@ -1 +1 @@
-Subproject commit e2d9e19a5dde27e8db0d8f838ddd614bfd4b0f1b
+Subproject commit 654093cae1af0f76fa1ebf42a706d69043476f95

From 7398164fccd727886144195d8a450cc5e34b07a6 Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Sat, 28 May 2016 14:56:17 +0200
Subject: [PATCH 0387/1230] Updated PX4 use / API of low level GPIO and other
 hardware-centric system facilities

---
 src/drivers/airspeed/airspeed.cpp             |   6 +-
 src/drivers/bma180/bma180.cpp                 |   6 +-
 src/drivers/bmi160/bmi160.cpp                 |  12 +-
 src/drivers/bmp280/bmp280.cpp                 |   6 +-
 src/drivers/boards/aerocore/aerocore_init.c   |  12 +-
 src/drivers/boards/aerocore/aerocore_led.c    |  24 +-
 src/drivers/boards/aerocore/aerocore_spi.c    |  54 +-
 src/drivers/boards/aerocore/board_config.h    |  18 +
 src/drivers/boards/mindpx-v2/board_config.h   |  34 +-
 src/drivers/boards/mindpx-v2/mindpx2_init.c   |  46 +-
 src/drivers/boards/mindpx-v2/mindpx2_led.c    |  14 +-
 src/drivers/boards/mindpx-v2/mindpx_can.c     |   2 +-
 src/drivers/boards/mindpx-v2/mindpx_spi.c     | 203 +++++---
 src/drivers/boards/mindpx-v2/mindpx_usb.c     |   8 +-
 .../px4-stm32f4discovery/board_config.h       |   2 +-
 .../px4-stm32f4discovery/px4discovery_init.c  |   2 +-
 .../px4-stm32f4discovery/px4discovery_led.c   |  14 +-
 .../px4-stm32f4discovery/px4discovery_usb.c   |   8 +-
 src/drivers/boards/px4fmu-v1/board_config.h   |  23 +-
 src/drivers/boards/px4fmu-v1/px4fmu_init.c    |  14 +-
 src/drivers/boards/px4fmu-v1/px4fmu_led.c     |  24 +-
 src/drivers/boards/px4fmu-v1/px4fmu_spi.c     |  36 +-
 src/drivers/boards/px4fmu-v1/px4fmu_usb.c     |   6 +-
 src/drivers/boards/px4fmu-v2/board_config.h   |  29 ++
 src/drivers/boards/px4fmu-v2/px4fmu2_init.c   |  71 ++-
 src/drivers/boards/px4fmu-v2/px4fmu2_led.c    |  12 +-
 src/drivers/boards/px4fmu-v2/px4fmu_spi.c     | 221 +++++---
 src/drivers/boards/px4fmu-v2/px4fmu_usb.c     |   6 +-
 src/drivers/boards/px4fmu-v4/board_config.h   |  39 +-
 src/drivers/boards/px4fmu-v4/px4fmu_init.c    |  74 ++-
 src/drivers/boards/px4fmu-v4/px4fmu_led.c     |   6 +-
 src/drivers/boards/px4fmu-v4/px4fmu_spi.c     | 143 +++--
 src/drivers/boards/px4fmu-v4/px4fmu_usb.c     |   6 +-
 src/drivers/boards/px4io-v1/board_config.h    |   8 +-
 src/drivers/boards/px4io-v1/px4io_init.c      |  32 +-
 src/drivers/boards/px4io-v2/board_config.h    |   8 +-
 src/drivers/boards/px4io-v2/px4io_init.c      |  62 +--
 src/drivers/camera_trigger/camera_trigger.cpp |   6 +-
 src/drivers/device/cdev.cpp                   |   4 +-
 src/drivers/device/i2c_nuttx.cpp              |   8 +-
 src/drivers/device/spi.cpp                    |   6 +-
 src/drivers/gps/gps.cpp                       |   6 +-
 src/drivers/hc_sr04/hc_sr04.cpp               |  16 +-
 src/drivers/hmc5883/hmc5883.cpp               |   6 +-
 src/drivers/l3gd20/l3gd20.cpp                 |   6 +-
 src/drivers/lis3mdl/lis3mdl.cpp               |   6 +-
 src/drivers/ll40ls/LidarLiteI2C.cpp           |   6 +-
 src/drivers/lsm303d/lsm303d.cpp               |  12 +-
 src/drivers/mb12xx/mb12xx.cpp                 |   6 +-
 src/drivers/mpu6000/mpu6000.cpp               |  16 +-
 src/drivers/mpu6500/mpu6500.cpp               |  16 +-
 src/drivers/mpu9250/mag.cpp                   |   6 +-
 src/drivers/mpu9250/mpu9250.cpp               |  12 +-
 src/drivers/ms5611/ms5611_nuttx.cpp           |   6 +-
 src/drivers/ms5611/ms5611_spi.cpp             |   5 -
 src/drivers/pwm_input/pwm_input.cpp           |  18 +-
 src/drivers/px4flow/px4flow.cpp               |   6 +-
 src/drivers/px4fmu/fmu.cpp                    | 491 +++---------------
 src/drivers/px4io/px4io_serial.cpp            |   8 +-
 src/drivers/rgbled_pwm/drv_led_pwm.cpp        |   2 +-
 src/drivers/rgbled_pwm/rgbled_pwm.cpp         |   4 -
 src/drivers/sf0x/sf0x.cpp                     |   6 +-
 src/drivers/sf10a/sf10a.cpp                   |   6 +-
 src/drivers/srf02/srf02.cpp                   |   6 +-
 src/drivers/srf02_i2c/srf02_i2c.cpp           |   6 +-
 src/drivers/stm32/adc/adc.cpp                 |  42 +-
 src/drivers/stm32/drv_hrt.c                   |  22 +-
 src/drivers/stm32/drv_input_capture.c         |  22 +-
 src/drivers/stm32/drv_io_timer.c              |  16 +-
 src/drivers/stm32/tone_alarm/tone_alarm.cpp   |  10 +-
 src/drivers/test_ppm/test_ppm.cpp             |   6 +-
 src/drivers/trone/trone.cpp                   |   6 +-
 .../commander/state_machine_helper.cpp        |   5 +-
 src/modules/px4iofirmware/controls.c          |   4 +-
 src/modules/px4iofirmware/i2c.c               |   8 +-
 src/modules/px4iofirmware/px4io.h             |  28 +-
 src/modules/px4iofirmware/serial.c            |   4 +-
 src/modules/systemlib/param/param.c           |   6 +-
 src/modules/systemlib/px4_macros.h            | 103 ++++
 src/modules/uORB/uORBDevices_nuttx.cpp        |  16 +-
 src/modules/uavcan/sensors/baro.cpp           |   6 +-
 src/modules/uavcan/uavcan_main.cpp            |   8 +-
 .../posix/drivers/airspeedsim/airspeedsim.cpp |   6 +-
 src/platforms/px4_config.h                    |   1 +
 src/platforms/px4_micro_hal.h                 |  53 ++
 src/systemcmds/i2c/i2c.c                      |   2 +-
 src/systemcmds/mtd/mtd.c                      |   4 +-
 src/systemcmds/tests/test_time.c              |   8 +-
 src/systemcmds/usb_connected/usb_connected.c  |   2 +-
 89 files changed, 1294 insertions(+), 1087 deletions(-)
 create mode 100644 src/modules/systemlib/px4_macros.h
 create mode 100644 src/platforms/px4_micro_hal.h

diff --git a/src/drivers/airspeed/airspeed.cpp b/src/drivers/airspeed/airspeed.cpp
index 0f4ffd1671..2a324e6d4a 100644
--- a/src/drivers/airspeed/airspeed.cpp
+++ b/src/drivers/airspeed/airspeed.cpp
@@ -255,14 +255,14 @@ Airspeed::ioctl(struct file *filp, int cmd, unsigned long arg)
 				return -EINVAL;
 			}
 
-			irqstate_t flags = irqsave();
+			irqstate_t flags = px4_enter_critical_section();
 
 			if (!_reports->resize(arg)) {
-				irqrestore(flags);
+				px4_leave_critical_section(flags);
 				return -ENOMEM;
 			}
 
-			irqrestore(flags);
+			px4_leave_critical_section(flags);
 
 			return OK;
 		}
diff --git a/src/drivers/bma180/bma180.cpp b/src/drivers/bma180/bma180.cpp
index ee895d8adf..f6a3c7a00f 100644
--- a/src/drivers/bma180/bma180.cpp
+++ b/src/drivers/bma180/bma180.cpp
@@ -465,14 +465,14 @@ BMA180::ioctl(struct file *filp, int cmd, unsigned long arg)
 				return -EINVAL;
 			}
 
-			irqstate_t flags = irqsave();
+			irqstate_t flags = px4_enter_critical_section();
 
 			if (!_reports->resize(arg)) {
-				irqrestore(flags);
+				px4_leave_critical_section(flags);
 				return -ENOMEM;
 			}
 
-			irqrestore(flags);
+			px4_leave_critical_section(flags);
 
 			return OK;
 		}
diff --git a/src/drivers/bmi160/bmi160.cpp b/src/drivers/bmi160/bmi160.cpp
index 260800755e..bf477e4e39 100644
--- a/src/drivers/bmi160/bmi160.cpp
+++ b/src/drivers/bmi160/bmi160.cpp
@@ -741,14 +741,14 @@ BMI160::ioctl(struct file *filp, int cmd, unsigned long arg)
 				return -EINVAL;
 			}
 
-			irqstate_t flags = irqsave();
+			irqstate_t flags = px4_enter_critical_section();
 
 			if (!_accel_reports->resize(arg)) {
-				irqrestore(flags);
+				px4_leave_critical_section(flags);
 				return -ENOMEM;
 			}
 
-			irqrestore(flags);
+			px4_leave_critical_section(flags);
 
 			return OK;
 		}
@@ -837,14 +837,14 @@ BMI160::gyro_ioctl(struct file *filp, int cmd, unsigned long arg)
 				return -EINVAL;
 			}
 
-			irqstate_t flags = irqsave();
+			irqstate_t flags = px4_enter_critical_section();
 
 			if (!_gyro_reports->resize(arg)) {
-				irqrestore(flags);
+				px4_leave_critical_section(flags);
 				return -ENOMEM;
 			}
 
-			irqrestore(flags);
+			px4_leave_critical_section(flags);
 
 			return OK;
 		}
diff --git a/src/drivers/bmp280/bmp280.cpp b/src/drivers/bmp280/bmp280.cpp
index 6c7af54d5a..65fd7aca3c 100644
--- a/src/drivers/bmp280/bmp280.cpp
+++ b/src/drivers/bmp280/bmp280.cpp
@@ -394,14 +394,14 @@ BMP280::ioctl(struct file *filp, int cmd, unsigned long arg)
 				return -EINVAL;
 			}
 
-			irqstate_t flags = irqsave();
+			irqstate_t flags = px4_enter_critical_section();
 
 			if (!_reports->resize(arg)) {
-				irqrestore(flags);
+				px4_leave_critical_section(flags);
 				return -ENOMEM;
 			}
 
-			irqrestore(flags);
+			px4_leave_critical_section(flags);
 			return OK;
 		}
 
diff --git a/src/drivers/boards/aerocore/aerocore_init.c b/src/drivers/boards/aerocore/aerocore_init.c
index 0597d56bcc..0a58f4a845 100644
--- a/src/drivers/boards/aerocore/aerocore_init.c
+++ b/src/drivers/boards/aerocore/aerocore_init.c
@@ -225,10 +225,10 @@ __EXPORT int nsh_archinitialize(void)
 {
 
 	/* configure ADC pins */
-	stm32_configgpio(GPIO_ADC1_IN10);	/* used by VBUS valid */
-	stm32_configgpio(GPIO_ADC1_IN11);	/* J1 breakout */
-	stm32_configgpio(GPIO_ADC1_IN12);	/* J1 breakout */
-	stm32_configgpio(GPIO_ADC1_IN13);	/* J1 breakout */
+	px4_arch_configgpio(GPIO_ADC1_IN10);	/* used by VBUS valid */
+	px4_arch_configgpio(GPIO_ADC1_IN11);	/* J1 breakout */
+	px4_arch_configgpio(GPIO_ADC1_IN12);	/* J1 breakout */
+	px4_arch_configgpio(GPIO_ADC1_IN13);	/* J1 breakout */
 
 	/* configure the high-resolution time/callout interface */
 	hrt_init();
@@ -263,7 +263,7 @@ __EXPORT int nsh_archinitialize(void)
 	led_off(LED_AMBER);
 
 	/* Configure Sensors on SPI bus #3 */
-	spi3 = up_spiinitialize(3);
+	spi3 = px4_spibus_initialize(3);
 
 	if (!spi3) {
 		message("[boot] FAILED to initialize SPI port 3\n");
@@ -282,7 +282,7 @@ __EXPORT int nsh_archinitialize(void)
 	message("[boot] Initialized SPI port 3 (SENSORS)\n");
 
 	/* Configure FRAM on SPI bus #4 */
-	spi4 = up_spiinitialize(4);
+	spi4 = px4_spibus_initialize(4);
 
 	if (!spi4) {
 		message("[boot] FAILED to initialize SPI port 4\n");
diff --git a/src/drivers/boards/aerocore/aerocore_led.c b/src/drivers/boards/aerocore/aerocore_led.c
index 89c6367419..91956e420b 100644
--- a/src/drivers/boards/aerocore/aerocore_led.c
+++ b/src/drivers/boards/aerocore/aerocore_led.c
@@ -63,19 +63,19 @@ __END_DECLS
 
 __EXPORT void led_init()
 {
-	stm32_configgpio(GPIO_LED0);
-	stm32_configgpio(GPIO_LED1);
+	px4_arch_configgpio(GPIO_LED0);
+	px4_arch_configgpio(GPIO_LED1);
 }
 
 __EXPORT void led_on(int led)
 {
 	switch (led) {
 	case 0:
-		stm32_gpiowrite(GPIO_LED0, true);
+		px4_arch_gpiowrite(GPIO_LED0, true);
 		break;
 
 	case 1:
-		stm32_gpiowrite(GPIO_LED1, true);
+		px4_arch_gpiowrite(GPIO_LED1, true);
 		break;
 
 	default:
@@ -87,11 +87,11 @@ __EXPORT void led_off(int led)
 {
 	switch (led) {
 	case 0:
-		stm32_gpiowrite(GPIO_LED0, false);
+		px4_arch_gpiowrite(GPIO_LED0, false);
 		break;
 
 	case 1:
-		stm32_gpiowrite(GPIO_LED1, false);
+		px4_arch_gpiowrite(GPIO_LED1, false);
 		break;
 
 	default:
@@ -103,21 +103,21 @@ __EXPORT void led_toggle(int led)
 {
 	switch (led) {
 	case 0:
-		if (stm32_gpioread(GPIO_LED0)) {
-			stm32_gpiowrite(GPIO_LED0, false);
+		if (px4_arch_gpioread(GPIO_LED0)) {
+			px4_arch_gpiowrite(GPIO_LED0, false);
 
 		} else {
-			stm32_gpiowrite(GPIO_LED0, true);
+			px4_arch_gpiowrite(GPIO_LED0, true);
 		}
 
 		break;
 
 	case 1:
-		if (stm32_gpioread(GPIO_LED1)) {
-			stm32_gpiowrite(GPIO_LED1, false);
+		if (px4_arch_gpioread(GPIO_LED1)) {
+			px4_arch_gpiowrite(GPIO_LED1, false);
 
 		} else {
-			stm32_gpiowrite(GPIO_LED1, true);
+			px4_arch_gpiowrite(GPIO_LED1, true);
 		}
 
 		break;
diff --git a/src/drivers/boards/aerocore/aerocore_spi.c b/src/drivers/boards/aerocore/aerocore_spi.c
index 28103c843e..c6015689c0 100644
--- a/src/drivers/boards/aerocore/aerocore_spi.c
+++ b/src/drivers/boards/aerocore/aerocore_spi.c
@@ -70,36 +70,36 @@
 __EXPORT void stm32_spiinitialize(void)
 {
 #ifdef CONFIG_STM32_SPI1
-	stm32_configgpio(GPIO_SPI1_NSS);
-	stm32_gpiowrite(GPIO_SPI1_NSS, 1);
+	px4_arch_configgpio(GPIO_SPI1_NSS);
+	px4_arch_gpiowrite(GPIO_SPI1_NSS, 1);
 #endif
 
 #ifdef CONFIG_STM32_SPI2
-	stm32_configgpio(GPIO_SPI2_NSS);
-	stm32_gpiowrite(GPIO_SPI2_NSS, 1);
+	px4_arch_configgpio(GPIO_SPI2_NSS);
+	px4_arch_gpiowrite(GPIO_SPI2_NSS, 1);
 #endif
 
 #ifdef CONFIG_STM32_SPI3
-	stm32_configgpio(GPIO_SPI_CS_GYRO);
-	stm32_configgpio(GPIO_SPI_CS_ACCEL_MAG);
-	stm32_configgpio(GPIO_SPI_CS_BARO);
+	px4_arch_configgpio(GPIO_SPI_CS_GYRO);
+	px4_arch_configgpio(GPIO_SPI_CS_ACCEL_MAG);
+	px4_arch_configgpio(GPIO_SPI_CS_BARO);
 
 	/* De-activate all peripherals,
 	 * required for some peripheral
 	 * state machines
 	 */
-	stm32_gpiowrite(GPIO_SPI_CS_GYRO, 1);
-	stm32_gpiowrite(GPIO_SPI_CS_ACCEL_MAG, 1);
-	stm32_gpiowrite(GPIO_SPI_CS_BARO, 1);
+	px4_arch_gpiowrite(GPIO_SPI_CS_GYRO, 1);
+	px4_arch_gpiowrite(GPIO_SPI_CS_ACCEL_MAG, 1);
+	px4_arch_gpiowrite(GPIO_SPI_CS_BARO, 1);
 
-	stm32_configgpio(GPIO_EXTI_GYRO_DRDY);
-	stm32_configgpio(GPIO_EXTI_MAG_DRDY);
-	stm32_configgpio(GPIO_EXTI_ACCEL_DRDY);
+	px4_arch_configgpio(GPIO_EXTI_GYRO_DRDY);
+	px4_arch_configgpio(GPIO_EXTI_MAG_DRDY);
+	px4_arch_configgpio(GPIO_EXTI_ACCEL_DRDY);
 #endif
 
 #ifdef CONFIG_STM32_SPI4
-	stm32_configgpio(GPIO_SPI4_NSS);
-	stm32_gpiowrite(GPIO_SPI4_NSS, 1);
+	px4_arch_configgpio(GPIO_SPI4_NSS);
+	px4_arch_gpiowrite(GPIO_SPI4_NSS, 1);
 #endif
 }
 
@@ -107,7 +107,7 @@ __EXPORT void stm32_spiinitialize(void)
 __EXPORT void stm32_spi1select(FAR struct spi_dev_s *dev, enum spi_dev_e devid, bool selected)
 {
 	/* there is only one device broken-out so select it */
-	stm32_gpiowrite(GPIO_SPI1_NSS, !selected);
+	px4_arch_gpiowrite(GPIO_SPI1_NSS, !selected);
 }
 
 __EXPORT uint8_t stm32_spi1status(FAR struct spi_dev_s *dev, enum spi_dev_e devid)
@@ -120,7 +120,7 @@ __EXPORT uint8_t stm32_spi1status(FAR struct spi_dev_s *dev, enum spi_dev_e devi
 __EXPORT void stm32_spi2select(FAR struct spi_dev_s *dev, enum spi_dev_e devid, bool selected)
 {
 	/* there is only one device broken-out so select it */
-	stm32_gpiowrite(GPIO_SPI2_NSS, !selected);
+	px4_arch_gpiowrite(GPIO_SPI2_NSS, !selected);
 }
 
 __EXPORT uint8_t stm32_spi2status(FAR struct spi_dev_s *dev, enum spi_dev_e devid)
@@ -137,23 +137,23 @@ __EXPORT void stm32_spi3select(FAR struct spi_dev_s *dev, enum spi_dev_e devid,
 	switch (devid) {
 	case PX4_SPIDEV_GYRO:
 		/* Making sure the other peripherals are not selected */
-		stm32_gpiowrite(GPIO_SPI_CS_GYRO, !selected);
-		stm32_gpiowrite(GPIO_SPI_CS_ACCEL_MAG, 1);
-		stm32_gpiowrite(GPIO_SPI_CS_BARO, 1);
+		px4_arch_gpiowrite(GPIO_SPI_CS_GYRO, !selected);
+		px4_arch_gpiowrite(GPIO_SPI_CS_ACCEL_MAG, 1);
+		px4_arch_gpiowrite(GPIO_SPI_CS_BARO, 1);
 		break;
 
 	case PX4_SPIDEV_ACCEL_MAG:
 		/* Making sure the other peripherals are not selected */
-		stm32_gpiowrite(GPIO_SPI_CS_GYRO, 1);
-		stm32_gpiowrite(GPIO_SPI_CS_ACCEL_MAG, !selected);
-		stm32_gpiowrite(GPIO_SPI_CS_BARO, 1);
+		px4_arch_gpiowrite(GPIO_SPI_CS_GYRO, 1);
+		px4_arch_gpiowrite(GPIO_SPI_CS_ACCEL_MAG, !selected);
+		px4_arch_gpiowrite(GPIO_SPI_CS_BARO, 1);
 		break;
 
 	case PX4_SPIDEV_BARO:
 		/* Making sure the other peripherals are not selected */
-		stm32_gpiowrite(GPIO_SPI_CS_GYRO, 1);
-		stm32_gpiowrite(GPIO_SPI_CS_ACCEL_MAG, 1);
-		stm32_gpiowrite(GPIO_SPI_CS_BARO, !selected);
+		px4_arch_gpiowrite(GPIO_SPI_CS_GYRO, 1);
+		px4_arch_gpiowrite(GPIO_SPI_CS_ACCEL_MAG, 1);
+		px4_arch_gpiowrite(GPIO_SPI_CS_BARO, !selected);
 		break;
 
 	default:
@@ -172,7 +172,7 @@ __EXPORT uint8_t stm32_spi3status(FAR struct spi_dev_s *dev, enum spi_dev_e devi
 __EXPORT void stm32_spi4select(FAR struct spi_dev_s *dev, enum spi_dev_e devid, bool selected)
 {
 	/* there can only be one device on this bus, so always select it */
-	stm32_gpiowrite(GPIO_SPI4_NSS, !selected);
+	px4_arch_gpiowrite(GPIO_SPI4_NSS, !selected);
 }
 
 __EXPORT uint8_t stm32_spi4status(FAR struct spi_dev_s *dev, enum spi_dev_e devid)
diff --git a/src/drivers/boards/aerocore/board_config.h b/src/drivers/boards/aerocore/board_config.h
index 3046839fb5..922ddb6ee4 100644
--- a/src/drivers/boards/aerocore/board_config.h
+++ b/src/drivers/boards/aerocore/board_config.h
@@ -174,6 +174,21 @@ __BEGIN_DECLS
 
 #define BOARD_NAME "AEROCORE"
 
+#define BOARD_HAS_PWM	8
+/* AeroCore breaks out User GPIOs on J11 */
+#define BOARD_FMU_GPIO_TAB  { \
+		{GPIO_GPIO0_INPUT,       GPIO_GPIO0_OUTPUT,       0}, \
+		{GPIO_GPIO1_INPUT,       GPIO_GPIO1_OUTPUT,       0}, \
+		{GPIO_GPIO3_INPUT,       GPIO_GPIO3_OUTPUT,       0}, \
+		{GPIO_GPIO4_INPUT,       GPIO_GPIO4_OUTPUT,       0}, \
+		{GPIO_GPIO5_INPUT,       GPIO_GPIO5_OUTPUT,       0}, \
+		{GPIO_GPIO6_INPUT,       GPIO_GPIO6_OUTPUT,       0}, \
+		{GPIO_GPIO7_INPUT,       GPIO_GPIO7_OUTPUT,       0}, \
+		{GPIO_GPIO8_INPUT,       GPIO_GPIO8_OUTPUT,       0}, \
+		{GPIO_GPIO9_INPUT,       GPIO_GPIO9_OUTPUT,       0}, \
+		{GPIO_GPIO10_INPUT,      GPIO_GPIO10_OUTPUT,      0}, \
+		{GPIO_GPIO11_INPUT,      GPIO_GPIO11_OUTPUT,      0}, }
+
 /****************************************************************************************************
  * Public Types
  ****************************************************************************************************/
@@ -197,6 +212,9 @@ __BEGIN_DECLS
  ****************************************************************************************************/
 
 extern void stm32_spiinitialize(void);
+#define board_spi_reset(ms)
+
+#define board_peripheral_reset(ms)
 
 /****************************************************************************
  * Name: nsh_archinitialize
diff --git a/src/drivers/boards/mindpx-v2/board_config.h b/src/drivers/boards/mindpx-v2/board_config.h
index 069b449d60..0c478057f3 100644
--- a/src/drivers/boards/mindpx-v2/board_config.h
+++ b/src/drivers/boards/mindpx-v2/board_config.h
@@ -43,7 +43,7 @@
  * Included Files
  ****************************************************************************************************/
 
-#include <nuttx/config.h>
+#include <px4_config.h>
 #include <nuttx/compiler.h>
 #include <stdint.h>
 
@@ -292,6 +292,34 @@ __BEGIN_DECLS
 
 #define BOARD_NAME "MINDPX_V2"
 
+/* By Providing BOARD_ADC_USB_CONNECTED this board support the ADC
+ * system_power interface, and therefore provides the true logic
+ * GPIO BOARD_ADC_xxxx macros.
+ */
+#define BOARD_ADC_USB_CONNECTED (px4_arch_gpioread(GPIO_OTGFS_VBUS))
+#define BOARD_ADC_BRICK_VALID   (1)
+#define BOARD_ADC_SERVO_VALID   (1)
+#define BOARD_ADC_PERIPH_5V_OC  (0)
+#define BOARD_ADC_HIPOWER_5V_OC (0)
+
+#define BOARD_HAS_PWM	DIRECT_PWM_OUTPUT_CHANNELS
+
+#define BOARD_FMU_GPIO_TAB  { \
+		{GPIO_GPIO0_INPUT,       GPIO_GPIO0_OUTPUT,       0}, \
+		{GPIO_GPIO1_INPUT,       GPIO_GPIO1_OUTPUT,       0}, \
+		{GPIO_GPIO2_INPUT,       GPIO_GPIO2_OUTPUT,       0}, \
+		{GPIO_GPIO3_INPUT,       GPIO_GPIO3_OUTPUT,       0}, \
+		{GPIO_GPIO4_INPUT,       GPIO_GPIO4_OUTPUT,       0}, \
+		{GPIO_GPIO5_INPUT,       GPIO_GPIO5_OUTPUT,       0}, \
+		{GPIO_GPIO6_INPUT,       GPIO_GPIO6_OUTPUT,       0}, \
+		{GPIO_GPIO7_INPUT,       GPIO_GPIO7_OUTPUT,       0}, \
+		{GPIO_GPIO8_INPUT,       GPIO_GPIO8_OUTPUT,       0}, \
+		{GPIO_GPIO9_INPUT,       GPIO_GPIO9_OUTPUT,       0}, \
+		{GPIO_GPIO10_INPUT,      GPIO_GPIO10_OUTPUT,      0}, \
+		{GPIO_GPIO11_INPUT,      GPIO_GPIO11_OUTPUT,      0}, \
+		{GPIO_GPIO12_INPUT,      GPIO_GPIO12_OUTPUT,      0}, }
+
+
 /****************************************************************************************************
  * Public Types
  ****************************************************************************************************/
@@ -315,9 +343,13 @@ __BEGIN_DECLS
  ****************************************************************************************************/
 
 extern void stm32_spiinitialize(void);
+void board_spi_reset(int ms);
 
 extern void stm32_usbinitialize(void);
 
+
+#define board_peripheral_reset(ms)
+
 /****************************************************************************
  * Name: nsh_archinitialize
  *
diff --git a/src/drivers/boards/mindpx-v2/mindpx2_init.c b/src/drivers/boards/mindpx-v2/mindpx2_init.c
index 94064c7669..94ea735f8d 100644
--- a/src/drivers/boards/mindpx-v2/mindpx2_init.c
+++ b/src/drivers/boards/mindpx-v2/mindpx2_init.c
@@ -45,7 +45,7 @@
  * Included Files
  ****************************************************************************/
 
-#include <nuttx/config.h>
+#include <px4_config.h>
 
 #include <stdbool.h>
 #include <stdio.h>
@@ -230,23 +230,23 @@ __EXPORT int nsh_archinitialize(void)
 {
 
 	/* configure ADC pins */
-	stm32_configgpio(GPIO_ADC1_IN3);	/* BATT_VOLTAGE_SENS */
-	stm32_configgpio(GPIO_ADC1_IN2);	/* BATT_CURRENT_SENS */
-	stm32_configgpio(GPIO_ADC1_IN4);	/* VDD_5V_SENS */
-	stm32_configgpio(GPIO_ADC1_IN10);	/* used by VBUS valid */
-	// stm32_configgpio(GPIO_ADC1_IN11);	/* unused */
-	// stm32_configgpio(GPIO_ADC1_IN12);	/* used by MPU6000 CS */
-	stm32_configgpio(GPIO_ADC1_IN13);	/* FMU_AUX_ADC_1 */
-	stm32_configgpio(GPIO_ADC1_IN14);	/* FMU_AUX_ADC_2 */
-	stm32_configgpio(GPIO_ADC1_IN15);	/* PRESSURE_SENS */
+	px4_arch_configgpio(GPIO_ADC1_IN3);	/* BATT_VOLTAGE_SENS */
+	px4_arch_configgpio(GPIO_ADC1_IN2);	/* BATT_CURRENT_SENS */
+	px4_arch_configgpio(GPIO_ADC1_IN4);	/* VDD_5V_SENS */
+	px4_arch_configgpio(GPIO_ADC1_IN10);	/* used by VBUS valid */
+	// px4_arch_configgpio(GPIO_ADC1_IN11);	/* unused */
+	// px4_arch_configgpio(GPIO_ADC1_IN12);	/* used by MPU6000 CS */
+	px4_arch_configgpio(GPIO_ADC1_IN13);	/* FMU_AUX_ADC_1 */
+	px4_arch_configgpio(GPIO_ADC1_IN14);	/* FMU_AUX_ADC_2 */
+	px4_arch_configgpio(GPIO_ADC1_IN15);	/* PRESSURE_SENS */
 
 	/* configure power supply control/sense pins */
-//	stm32_configgpio(GPIO_VDD_5V_PERIPH_EN);
-//	stm32_configgpio(GPIO_VDD_3V3_SENSORS_EN);
-//	stm32_configgpio(GPIO_VDD_BRICK_VALID);
-//	stm32_configgpio(GPIO_VDD_SERVO_VALID);
-//	stm32_configgpio(GPIO_VDD_5V_HIPOWER_OC);
-//	stm32_configgpio(GPIO_VDD_5V_PERIPH_OC);
+//	px4_arch_configgpio(GPIO_VDD_5V_PERIPH_EN);
+//	px4_arch_configgpio(GPIO_VDD_3V3_SENSORS_EN);
+//	px4_arch_configgpio(GPIO_VDD_BRICK_VALID);
+//	px4_arch_configgpio(GPIO_VDD_SERVO_VALID);
+//	px4_arch_configgpio(GPIO_VDD_5V_HIPOWER_OC);
+//	px4_arch_configgpio(GPIO_VDD_5V_PERIPH_OC);
 
 	/* configure the high-resolution time/callout interface */
 	hrt_init();
@@ -282,7 +282,7 @@ __EXPORT int nsh_archinitialize(void)
 
 	/* Configure SPI-based devices */
 	message("[boot] Initialized SPI port 4 (SENSORS)\n");
-	spi4 = up_spiinitialize(4);
+	spi4 = px4_spibus_initialize(4);
 
 	if (!spi4) {
 		message("[boot] FAILED to initialize SPI port 4\n");
@@ -303,7 +303,7 @@ __EXPORT int nsh_archinitialize(void)
 	/* Get the SPI port for the FRAM */
 	message("[boot] Initialized SPI port 1 (RAMTRON FRAM)\n");
 
-	spi1 = up_spiinitialize(1);
+	spi1 = px4_spibus_initialize(1);
 
 	if (!spi1) {
 		message("[boot] FAILED to initialize SPI port 1\n");
@@ -324,7 +324,7 @@ __EXPORT int nsh_archinitialize(void)
 
 	message("[boot] Initialized SPI port 2 (nRF24 and ext)\n");
 
-	spi2 = up_spiinitialize(2);
+	spi2 = px4_spibus_initialize(2);
 
 	/* Default SPI2 to 10MHz and de-assert the known chip selects. */
 	SPI_SETFREQUENCY(spi2, 10000000);
@@ -360,12 +360,12 @@ __EXPORT int nsh_archinitialize(void)
 #endif
 
 
-	stm32_configgpio(GPIO_I2C2_SCL);
-	stm32_configgpio(GPIO_I2C2_SDA);
+	px4_arch_configgpio(GPIO_I2C2_SCL);
+	px4_arch_configgpio(GPIO_I2C2_SDA);
 	message("[boot] Initialized ext I2C Port\n");
 
-	stm32_configgpio(GPIO_I2C1_SCL);
-	stm32_configgpio(GPIO_I2C1_SDA);
+	px4_arch_configgpio(GPIO_I2C1_SCL);
+	px4_arch_configgpio(GPIO_I2C1_SDA);
 	message("[boot] Initialized onboard I2C Port\n");
 
 
diff --git a/src/drivers/boards/mindpx-v2/mindpx2_led.c b/src/drivers/boards/mindpx-v2/mindpx2_led.c
index 5c3985d7d3..b4656c1a33 100644
--- a/src/drivers/boards/mindpx-v2/mindpx2_led.c
+++ b/src/drivers/boards/mindpx-v2/mindpx2_led.c
@@ -37,7 +37,7 @@
  * PX4FMU LED backend.
  */
 
-#include <nuttx/config.h>
+#include <px4_config.h>
 
 #include <stdbool.h>
 
@@ -64,14 +64,14 @@ __EXPORT void led_init()
 {
 	/* Configure LED1 GPIO for output */
 
-	stm32_configgpio(GPIO_LED1);
+	px4_arch_configgpio(GPIO_LED1);
 }
 
 __EXPORT void led_on(int led)
 {
 	if (led == 1) {
 		/* Pull down to switch on */
-		stm32_gpiowrite(GPIO_LED1, false);
+		px4_arch_gpiowrite(GPIO_LED1, false);
 	}
 }
 
@@ -79,18 +79,18 @@ __EXPORT void led_off(int led)
 {
 	if (led == 1) {
 		/* Pull up to switch off */
-		stm32_gpiowrite(GPIO_LED1, true);
+		px4_arch_gpiowrite(GPIO_LED1, true);
 	}
 }
 
 __EXPORT void led_toggle(int led)
 {
 	if (led == 1) {
-		if (stm32_gpioread(GPIO_LED1)) {
-			stm32_gpiowrite(GPIO_LED1, false);
+		if (px4_arch_gpioread(GPIO_LED1)) {
+			px4_arch_gpiowrite(GPIO_LED1, false);
 
 		} else {
-			stm32_gpiowrite(GPIO_LED1, true);
+			px4_arch_gpiowrite(GPIO_LED1, true);
 		}
 	}
 }
diff --git a/src/drivers/boards/mindpx-v2/mindpx_can.c b/src/drivers/boards/mindpx-v2/mindpx_can.c
index 58f8023597..84c7a28bf2 100644
--- a/src/drivers/boards/mindpx-v2/mindpx_can.c
+++ b/src/drivers/boards/mindpx-v2/mindpx_can.c
@@ -41,7 +41,7 @@
  * Included Files
  ************************************************************************************/
 
-#include <nuttx/config.h>
+#include <px4_config.h>
 
 #include <errno.h>
 #include <debug.h>
diff --git a/src/drivers/boards/mindpx-v2/mindpx_spi.c b/src/drivers/boards/mindpx-v2/mindpx_spi.c
index 012455b604..b25797d095 100644
--- a/src/drivers/boards/mindpx-v2/mindpx_spi.c
+++ b/src/drivers/boards/mindpx-v2/mindpx_spi.c
@@ -41,7 +41,7 @@
  * Included Files
  ************************************************************************************/
 
-#include <nuttx/config.h>
+#include <px4_config.h>
 
 #include <stdint.h>
 #include <stdbool.h>
@@ -54,6 +54,7 @@
 #include <chip.h>
 #include <stm32.h>
 #include "board_config.h"
+#include <systemlib/err.h>
 
 /************************************************************************************
  * Public Functions
@@ -70,42 +71,42 @@
 __EXPORT void weak_function stm32_spiinitialize(void)
 {
 #ifdef CONFIG_STM32_SPI4
-	stm32_configgpio(GPIO_SPI_CS_GYRO);
-	stm32_configgpio(GPIO_SPI_CS_ACCEL_MAG);
-	stm32_configgpio(GPIO_SPI_CS_BARO);
-//	stm32_configgpio(GPIO_SPI_CS_FRAM);
-	stm32_configgpio(GPIO_SPI_CS_MPU);
+	px4_arch_configgpio(GPIO_SPI_CS_GYRO);
+	px4_arch_configgpio(GPIO_SPI_CS_ACCEL_MAG);
+	px4_arch_configgpio(GPIO_SPI_CS_BARO);
+//	px4_arch_configgpio(GPIO_SPI_CS_FRAM);
+	px4_arch_configgpio(GPIO_SPI_CS_MPU);
 
 	/* De-activate all peripherals,
 	 * required for some peripheral
 	 * state machines
 	 */
-	stm32_gpiowrite(GPIO_SPI_CS_GYRO, 1);
-	stm32_gpiowrite(GPIO_SPI_CS_ACCEL_MAG, 1);
-	stm32_gpiowrite(GPIO_SPI_CS_BARO, 1);
-//	stm32_gpiowrite(GPIO_SPI_CS_FRAM,1);
-	stm32_gpiowrite(GPIO_SPI_CS_MPU, 1);
+	px4_arch_gpiowrite(GPIO_SPI_CS_GYRO, 1);
+	px4_arch_gpiowrite(GPIO_SPI_CS_ACCEL_MAG, 1);
+	px4_arch_gpiowrite(GPIO_SPI_CS_BARO, 1);
+//	px4_arch_gpiowrite(GPIO_SPI_CS_FRAM,1);
+	px4_arch_gpiowrite(GPIO_SPI_CS_MPU, 1);
 
-	stm32_configgpio(GPIO_EXTI_GYRO_DRDY);
-	stm32_configgpio(GPIO_EXTI_MAG_DRDY);
-	stm32_configgpio(GPIO_EXTI_ACCEL_DRDY);
-	stm32_configgpio(GPIO_EXTI_MPU_DRDY);
+	px4_arch_configgpio(GPIO_EXTI_GYRO_DRDY);
+	px4_arch_configgpio(GPIO_EXTI_MAG_DRDY);
+	px4_arch_configgpio(GPIO_EXTI_ACCEL_DRDY);
+	px4_arch_configgpio(GPIO_EXTI_MPU_DRDY);
 #endif
 
 #ifdef CONFIG_STM32_SPI1
-	stm32_configgpio(GPIO_SPI_CS_FRAM);
-	stm32_gpiowrite(GPIO_SPI_CS_FRAM, 1);
+	px4_arch_configgpio(GPIO_SPI_CS_FRAM);
+	px4_arch_gpiowrite(GPIO_SPI_CS_FRAM, 1);
 #endif
 
 #ifdef CONFIG_STM32_SPI2
-	stm32_configgpio(GPIO_SPI_CS_EXT0);
-	stm32_configgpio(GPIO_SPI_CS_EXT1);
-	stm32_configgpio(GPIO_SPI_CS_EXT2);
-	stm32_configgpio(GPIO_SPI_CS_EXT3);
-	stm32_gpiowrite(GPIO_SPI_CS_EXT0, 1);
-	stm32_gpiowrite(GPIO_SPI_CS_EXT1, 1);
-	stm32_gpiowrite(GPIO_SPI_CS_EXT2, 1);
-	stm32_gpiowrite(GPIO_SPI_CS_EXT3, 1);
+	px4_arch_configgpio(GPIO_SPI_CS_EXT0);
+	px4_arch_configgpio(GPIO_SPI_CS_EXT1);
+	px4_arch_configgpio(GPIO_SPI_CS_EXT2);
+	px4_arch_configgpio(GPIO_SPI_CS_EXT3);
+	px4_arch_gpiowrite(GPIO_SPI_CS_EXT0, 1);
+	px4_arch_gpiowrite(GPIO_SPI_CS_EXT1, 1);
+	px4_arch_gpiowrite(GPIO_SPI_CS_EXT2, 1);
+	px4_arch_gpiowrite(GPIO_SPI_CS_EXT3, 1);
 #endif
 }
 
@@ -116,40 +117,40 @@ __EXPORT void stm32_spi4select(FAR struct spi_dev_s *dev, enum spi_dev_e devid,
 	switch (devid) {
 	case PX4_SPIDEV_GYRO:
 		/* Making sure the other peripherals are not selected */
-		stm32_gpiowrite(GPIO_SPI_CS_GYRO, !selected);
-		stm32_gpiowrite(GPIO_SPI_CS_ACCEL_MAG, 1);
-		stm32_gpiowrite(GPIO_SPI_CS_BARO, 1);
-		stm32_gpiowrite(GPIO_SPI_CS_MPU, 1);
+		px4_arch_gpiowrite(GPIO_SPI_CS_GYRO, !selected);
+		px4_arch_gpiowrite(GPIO_SPI_CS_ACCEL_MAG, 1);
+		px4_arch_gpiowrite(GPIO_SPI_CS_BARO, 1);
+		px4_arch_gpiowrite(GPIO_SPI_CS_MPU, 1);
 		break;
 
 	case PX4_SPIDEV_ACCEL_MAG:
 		/* Making sure the other peripherals are not selected */
-		stm32_gpiowrite(GPIO_SPI_CS_GYRO, 1);
-		stm32_gpiowrite(GPIO_SPI_CS_ACCEL_MAG, !selected);
-		stm32_gpiowrite(GPIO_SPI_CS_BARO, 1);
-		stm32_gpiowrite(GPIO_SPI_CS_MPU, 1);
+		px4_arch_gpiowrite(GPIO_SPI_CS_GYRO, 1);
+		px4_arch_gpiowrite(GPIO_SPI_CS_ACCEL_MAG, !selected);
+		px4_arch_gpiowrite(GPIO_SPI_CS_BARO, 1);
+		px4_arch_gpiowrite(GPIO_SPI_CS_MPU, 1);
 		break;
 
 	case PX4_SPIDEV_BARO:
 		/* Making sure the other peripherals are not selected */
-		stm32_gpiowrite(GPIO_SPI_CS_GYRO, 1);
-		stm32_gpiowrite(GPIO_SPI_CS_ACCEL_MAG, 1);
-		stm32_gpiowrite(GPIO_SPI_CS_BARO, !selected);
-//		stm32_gpiowrite(GPIO_SPI_CS_FRAM,1);
-		stm32_gpiowrite(GPIO_SPI_CS_MPU, 1);
+		px4_arch_gpiowrite(GPIO_SPI_CS_GYRO, 1);
+		px4_arch_gpiowrite(GPIO_SPI_CS_ACCEL_MAG, 1);
+		px4_arch_gpiowrite(GPIO_SPI_CS_BARO, !selected);
+//		px4_arch_gpiowrite(GPIO_SPI_CS_FRAM,1);
+		px4_arch_gpiowrite(GPIO_SPI_CS_MPU, 1);
 		break;
 
 //	case PX4_SPIDEV_FLASH:
-//		stm32_gpiowrite(GPIO_SPI_CS_BARO,1);
-//		stm32_gpiowrite(GPIO_SPI_CS_FRAM,!selected);
+//		px4_arch_gpiowrite(GPIO_SPI_CS_BARO,1);
+//		px4_arch_gpiowrite(GPIO_SPI_CS_FRAM,!selected);
 //		break;
 
 	case PX4_SPIDEV_MPU:
 		/* Making sure the other peripherals are not selected */
-		stm32_gpiowrite(GPIO_SPI_CS_GYRO, 1);
-		stm32_gpiowrite(GPIO_SPI_CS_ACCEL_MAG, 1);
-		stm32_gpiowrite(GPIO_SPI_CS_BARO, 1);
-		stm32_gpiowrite(GPIO_SPI_CS_MPU, !selected);
+		px4_arch_gpiowrite(GPIO_SPI_CS_GYRO, 1);
+		px4_arch_gpiowrite(GPIO_SPI_CS_ACCEL_MAG, 1);
+		px4_arch_gpiowrite(GPIO_SPI_CS_BARO, 1);
+		px4_arch_gpiowrite(GPIO_SPI_CS_MPU, !selected);
 		break;
 
 	default:
@@ -167,7 +168,7 @@ __EXPORT uint8_t stm32_spi4status(FAR struct spi_dev_s *dev, enum spi_dev_e devi
 __EXPORT void stm32_spi1select(FAR struct spi_dev_s *dev, enum spi_dev_e devid, bool selected)
 {
 	/* there can only be one device on this bus, so always select it */
-	stm32_gpiowrite(GPIO_SPI_CS_FRAM, !selected);
+	px4_arch_gpiowrite(GPIO_SPI_CS_FRAM, !selected);
 }
 
 __EXPORT uint8_t stm32_spi1status(FAR struct spi_dev_s *dev, enum spi_dev_e devid)
@@ -184,34 +185,34 @@ __EXPORT void stm32_spi2select(FAR struct spi_dev_s *dev, enum spi_dev_e devid,
 	switch (devid) {
 	case PX4_SPIDEV_EXT0:
 		/* Making sure the other peripherals are not selected */
-		stm32_gpiowrite(GPIO_SPI_CS_EXT0, !selected);
-		stm32_gpiowrite(GPIO_SPI_CS_EXT1, 1);
-		stm32_gpiowrite(GPIO_SPI_CS_EXT2, 1);
-		stm32_gpiowrite(GPIO_SPI_CS_EXT3, 1);
+		px4_arch_gpiowrite(GPIO_SPI_CS_EXT0, !selected);
+		px4_arch_gpiowrite(GPIO_SPI_CS_EXT1, 1);
+		px4_arch_gpiowrite(GPIO_SPI_CS_EXT2, 1);
+		px4_arch_gpiowrite(GPIO_SPI_CS_EXT3, 1);
 		break;
 
 	case PX4_SPIDEV_EXT1:
 		/* Making sure the other peripherals are not selected */
-		stm32_gpiowrite(GPIO_SPI_CS_EXT0, 1);
-		stm32_gpiowrite(GPIO_SPI_CS_EXT1, !selected);
-		stm32_gpiowrite(GPIO_SPI_CS_EXT2, 1);
-		stm32_gpiowrite(GPIO_SPI_CS_EXT3, 1);
+		px4_arch_gpiowrite(GPIO_SPI_CS_EXT0, 1);
+		px4_arch_gpiowrite(GPIO_SPI_CS_EXT1, !selected);
+		px4_arch_gpiowrite(GPIO_SPI_CS_EXT2, 1);
+		px4_arch_gpiowrite(GPIO_SPI_CS_EXT3, 1);
 		break;
 
 	case PX4_SPIDEV_EXT2:
 		/* Making sure the other peripherals are not selected */
-		stm32_gpiowrite(GPIO_SPI_CS_EXT0, 1);
-		stm32_gpiowrite(GPIO_SPI_CS_EXT1, 1);
-		stm32_gpiowrite(GPIO_SPI_CS_EXT2, !selected);
-		stm32_gpiowrite(GPIO_SPI_CS_EXT3, 1);
+		px4_arch_gpiowrite(GPIO_SPI_CS_EXT0, 1);
+		px4_arch_gpiowrite(GPIO_SPI_CS_EXT1, 1);
+		px4_arch_gpiowrite(GPIO_SPI_CS_EXT2, !selected);
+		px4_arch_gpiowrite(GPIO_SPI_CS_EXT3, 1);
 		break;
 
 	case PX4_SPIDEV_EXT3:
 		/* Making sure the other peripherals are not selected */
-		stm32_gpiowrite(GPIO_SPI_CS_EXT0, 1);
-		stm32_gpiowrite(GPIO_SPI_CS_EXT1, 1);
-		stm32_gpiowrite(GPIO_SPI_CS_EXT2, 1);
-		stm32_gpiowrite(GPIO_SPI_CS_EXT3, !selected);
+		px4_arch_gpiowrite(GPIO_SPI_CS_EXT0, 1);
+		px4_arch_gpiowrite(GPIO_SPI_CS_EXT1, 1);
+		px4_arch_gpiowrite(GPIO_SPI_CS_EXT2, 1);
+		px4_arch_gpiowrite(GPIO_SPI_CS_EXT3, !selected);
 		break;
 
 	default:
@@ -224,3 +225,83 @@ __EXPORT uint8_t stm32_spi2status(FAR struct spi_dev_s *dev, enum spi_dev_e devi
 {
 	return SPI_STATUS_PRESENT;
 }
+
+__EXPORT void board_spi_reset(int ms)
+{
+	/* disable SPI bus */
+	px4_arch_configgpio(GPIO_SPI_CS_GYRO_OFF);
+	px4_arch_configgpio(GPIO_SPI_CS_ACCEL_MAG_OFF);
+	px4_arch_configgpio(GPIO_SPI_CS_BARO_OFF);
+	//      px4_arch_configgpio(GPIO_SPI_CS_FRAM_OFF);
+	px4_arch_configgpio(GPIO_SPI_CS_MPU_OFF);
+
+	px4_arch_gpiowrite(GPIO_SPI_CS_GYRO_OFF, 0);
+	px4_arch_gpiowrite(GPIO_SPI_CS_ACCEL_MAG_OFF, 0);
+	px4_arch_gpiowrite(GPIO_SPI_CS_BARO_OFF, 0);
+	//       px4_arch_gpiowrite(GPIO_SPI_CS_FRAM_OFF,0);
+	px4_arch_gpiowrite(GPIO_SPI_CS_MPU_OFF, 0);
+
+	px4_arch_configgpio(GPIO_SPI4_SCK_OFF);
+	px4_arch_configgpio(GPIO_SPI4_MISO_OFF);
+	px4_arch_configgpio(GPIO_SPI4_MOSI_OFF);
+
+	px4_arch_gpiowrite(GPIO_SPI4_SCK_OFF, 0);
+	px4_arch_gpiowrite(GPIO_SPI4_MISO_OFF, 0);
+	px4_arch_gpiowrite(GPIO_SPI4_MOSI_OFF, 0);
+
+	px4_arch_configgpio(GPIO_GYRO_DRDY_OFF);
+	px4_arch_configgpio(GPIO_MAG_DRDY_OFF);
+	px4_arch_configgpio(GPIO_ACCEL_DRDY_OFF);
+	px4_arch_configgpio(GPIO_EXTI_MPU_DRDY_OFF);
+
+	px4_arch_gpiowrite(GPIO_GYRO_DRDY_OFF, 0);
+	px4_arch_gpiowrite(GPIO_MAG_DRDY_OFF, 0);
+	px4_arch_gpiowrite(GPIO_ACCEL_DRDY_OFF, 0);
+	px4_arch_gpiowrite(GPIO_EXTI_MPU_DRDY_OFF, 0);
+
+	//        /* set the sensor rail off */
+	//        px4_arch_configgpio(GPIO_VDD_3V3_SENSORS_EN);
+	//        px4_arch_gpiowrite(GPIO_VDD_3V3_SENSORS_EN, 0);
+	//
+	/* wait for the sensor rail to reach GND */
+	usleep(ms * 1000);
+	warnx("reset done, %d ms", ms);
+	//
+	//        /* re-enable power */
+	//
+	//        /* switch the sensor rail back on */
+	//        px4_arch_gpiowrite(GPIO_VDD_3V3_SENSORS_EN, 1);
+	//
+	/* wait a bit before starting SPI, different times didn't influence results */
+	usleep(100);
+
+	/* reconfigure the SPI pins */
+#ifdef CONFIG_STM32_SPI4
+	px4_arch_configgpio(GPIO_SPI_CS_GYRO);
+	px4_arch_configgpio(GPIO_SPI_CS_ACCEL_MAG);
+	px4_arch_configgpio(GPIO_SPI_CS_BARO);
+	//        px4_arch_configgpio(GPIO_SPI_CS_FRAM);
+	px4_arch_configgpio(GPIO_SPI_CS_MPU);
+
+	/* De-activate all peripherals,
+	* required for some peripheral
+	* state machines
+	*/
+	px4_arch_gpiowrite(GPIO_SPI_CS_GYRO, 1);
+	px4_arch_gpiowrite(GPIO_SPI_CS_ACCEL_MAG, 1);
+	px4_arch_gpiowrite(GPIO_SPI_CS_BARO, 1);
+	px4_arch_gpiowrite(GPIO_SPI_CS_FRAM, 1);
+	px4_arch_gpiowrite(GPIO_SPI_CS_MPU, 1);
+
+	px4_arch_configgpio(GPIO_SPI4_SCK);
+	px4_arch_configgpio(GPIO_SPI4_MISO);
+	px4_arch_configgpio(GPIO_SPI4_MOSI);
+
+	// // XXX bring up the EXTI pins again
+	// px4_arch_configgpio(GPIO_GYRO_DRDY);
+	// px4_arch_configgpio(GPIO_MAG_DRDY);
+	// px4_arch_configgpio(GPIO_ACCEL_DRDY);
+
+#endif
+
+}
diff --git a/src/drivers/boards/mindpx-v2/mindpx_usb.c b/src/drivers/boards/mindpx-v2/mindpx_usb.c
index 1cbc5649c4..70d4371d79 100644
--- a/src/drivers/boards/mindpx-v2/mindpx_usb.c
+++ b/src/drivers/boards/mindpx-v2/mindpx_usb.c
@@ -41,7 +41,7 @@
  * Included Files
  ************************************************************************************/
 
-#include <nuttx/config.h>
+#include <px4_config.h>
 
 #include <sys/types.h>
 #include <stdint.h>
@@ -82,10 +82,10 @@ __EXPORT void stm32_usbinitialize(void)
 	/* Configure the OTG FS VBUS sensing GPIO, Power On, and Overcurrent GPIOs */
 
 #ifdef CONFIG_STM32_OTGFS
-	stm32_configgpio(GPIO_OTGFS_VBUS);
+	px4_arch_configgpio(GPIO_OTGFS_VBUS);
 	/* XXX We only support device mode
-	stm32_configgpio(GPIO_OTGFS_PWRON);
-	stm32_configgpio(GPIO_OTGFS_OVER);
+	px4_arch_configgpio(GPIO_OTGFS_PWRON);
+	px4_arch_configgpio(GPIO_OTGFS_OVER);
 	*/
 #endif
 }
diff --git a/src/drivers/boards/px4-stm32f4discovery/board_config.h b/src/drivers/boards/px4-stm32f4discovery/board_config.h
index 5673ecf6b0..1d6aba13a1 100644
--- a/src/drivers/boards/px4-stm32f4discovery/board_config.h
+++ b/src/drivers/boards/px4-stm32f4discovery/board_config.h
@@ -43,7 +43,7 @@
  * Included Files
  ****************************************************************************************************/
 
-#include <nuttx/config.h>
+#include <px4_config.h>
 #include <nuttx/compiler.h>
 #include <stdint.h>
 
diff --git a/src/drivers/boards/px4-stm32f4discovery/px4discovery_init.c b/src/drivers/boards/px4-stm32f4discovery/px4discovery_init.c
index 1c74be9058..adbfcc55b1 100644
--- a/src/drivers/boards/px4-stm32f4discovery/px4discovery_init.c
+++ b/src/drivers/boards/px4-stm32f4discovery/px4discovery_init.c
@@ -45,7 +45,7 @@
  * Included Files
  ****************************************************************************/
 
-#include <nuttx/config.h>
+#include <px4_config.h>
 
 #include <stdbool.h>
 #include <stdio.h>
diff --git a/src/drivers/boards/px4-stm32f4discovery/px4discovery_led.c b/src/drivers/boards/px4-stm32f4discovery/px4discovery_led.c
index 6a6c6eaa0d..58ba3372c9 100644
--- a/src/drivers/boards/px4-stm32f4discovery/px4discovery_led.c
+++ b/src/drivers/boards/px4-stm32f4discovery/px4discovery_led.c
@@ -37,7 +37,7 @@
  * PX4-stm32f4discovery LED backend.
  */
 
-#include <nuttx/config.h>
+#include <px4_config.h>
 
 #include <stdbool.h>
 
@@ -64,14 +64,14 @@ __EXPORT void led_init()
 {
 	/* Configure LED1 GPIO for output */
 
-	stm32_configgpio(GPIO_LED1);
+	px4_arch_configgpio(GPIO_LED1);
 }
 
 __EXPORT void led_on(int led)
 {
 	if (led == 1) {
 		/* Pull down to switch on */
-		stm32_gpiowrite(GPIO_LED1, false);
+		px4_arch_gpiowrite(GPIO_LED1, false);
 	}
 }
 
@@ -79,18 +79,18 @@ __EXPORT void led_off(int led)
 {
 	if (led == 1) {
 		/* Pull up to switch off */
-		stm32_gpiowrite(GPIO_LED1, true);
+		px4_arch_gpiowrite(GPIO_LED1, true);
 	}
 }
 
 __EXPORT void led_toggle(int led)
 {
 	if (led == 1) {
-		if (stm32_gpioread(GPIO_LED1)) {
-			stm32_gpiowrite(GPIO_LED1, false);
+		if (px4_arch_gpioread(GPIO_LED1)) {
+			px4_arch_gpiowrite(GPIO_LED1, false);
 
 		} else {
-			stm32_gpiowrite(GPIO_LED1, true);
+			px4_arch_gpiowrite(GPIO_LED1, true);
 		}
 	}
 }
diff --git a/src/drivers/boards/px4-stm32f4discovery/px4discovery_usb.c b/src/drivers/boards/px4-stm32f4discovery/px4discovery_usb.c
index 5cd01909b4..379c033456 100644
--- a/src/drivers/boards/px4-stm32f4discovery/px4discovery_usb.c
+++ b/src/drivers/boards/px4-stm32f4discovery/px4discovery_usb.c
@@ -41,7 +41,7 @@
  * Included Files
  ************************************************************************************/
 
-#include <nuttx/config.h>
+#include <px4_config.h>
 
 #include <sys/types.h>
 #include <stdint.h>
@@ -82,10 +82,10 @@ __EXPORT void stm32_usbinitialize(void)
 	/* Configure the OTG FS VBUS sensing GPIO, Power On, and Overcurrent GPIOs */
 
 #ifdef CONFIG_STM32_OTGFS
-	stm32_configgpio(GPIO_OTGFS_VBUS);
+	px4_arch_configgpio(GPIO_OTGFS_VBUS);
 	/* XXX We only support device mode
-	stm32_configgpio(GPIO_OTGFS_PWRON);
-	stm32_configgpio(GPIO_OTGFS_OVER);
+	px4_arch_configgpio(GPIO_OTGFS_PWRON);
+	px4_arch_configgpio(GPIO_OTGFS_OVER);
 	*/
 #endif
 }
diff --git a/src/drivers/boards/px4fmu-v1/board_config.h b/src/drivers/boards/px4fmu-v1/board_config.h
index 5ebdb57506..549e0ec892 100644
--- a/src/drivers/boards/px4fmu-v1/board_config.h
+++ b/src/drivers/boards/px4fmu-v1/board_config.h
@@ -155,7 +155,7 @@ __BEGIN_DECLS
 #define GPIO_GPIO6_OUTPUT	(GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN13)
 #define GPIO_GPIO7_OUTPUT	(GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN12)
 #define GPIO_GPIO_DIR		(GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTC|GPIO_PIN13)
-
+#define BOARD_GPIO_SHARED_BUFFERED_BITS 3
 /*
  * Tone alarm output
  */
@@ -206,6 +206,24 @@ __BEGIN_DECLS
 
 #define	BOARD_NAME "PX4FMU_V1"
 
+#define BOARD_HAS_PWM	DIRECT_PWM_OUTPUT_CHANNELS
+
+#define BOARD_FMU_GPIO_TAB  { \
+		{GPIO_GPIO0_INPUT, GPIO_GPIO0_OUTPUT, 0}, \
+		{GPIO_GPIO1_INPUT, GPIO_GPIO1_OUTPUT, 0}, \
+		{GPIO_GPIO2_INPUT, GPIO_GPIO2_OUTPUT, GPIO_USART2_CTS_1}, \
+		{GPIO_GPIO3_INPUT, GPIO_GPIO3_OUTPUT, GPIO_USART2_RTS_1}, \
+		{GPIO_GPIO4_INPUT, GPIO_GPIO4_OUTPUT, GPIO_USART2_TX_1}, \
+		{GPIO_GPIO5_INPUT, GPIO_GPIO5_OUTPUT, GPIO_USART2_RX_1}, \
+		{GPIO_GPIO6_INPUT, GPIO_GPIO6_OUTPUT, GPIO_CAN2_TX_2}, \
+		{GPIO_GPIO7_INPUT, GPIO_GPIO7_OUTPUT, GPIO_CAN2_RX_2}, }
+
+/* BOARD_HAS_MULTI_PURPOSE_GPIO defined because the board
+ * has alternate uses for GPIO as noted in that the third
+ * column above has entries.
+ */
+#define BOARD_HAS_MULTI_PURPOSE_GPIO 1
+
 /****************************************************************************************************
  * Public Types
  ****************************************************************************************************/
@@ -229,9 +247,12 @@ __BEGIN_DECLS
  ****************************************************************************************************/
 
 extern void stm32_spiinitialize(void);
+#define board_spi_reset(ms)
 
 extern void stm32_usbinitialize(void);
 
+#define board_peripheral_reset(ms)
+
 /****************************************************************************
  * Name: nsh_archinitialize
  *
diff --git a/src/drivers/boards/px4fmu-v1/px4fmu_init.c b/src/drivers/boards/px4fmu-v1/px4fmu_init.c
index 4e969692bc..d807f6726f 100644
--- a/src/drivers/boards/px4fmu-v1/px4fmu_init.c
+++ b/src/drivers/boards/px4fmu-v1/px4fmu_init.c
@@ -150,8 +150,8 @@ __EXPORT int nsh_archinitialize(void)
 	int result;
 
 	/* configure always-on ADC pins */
-	stm32_configgpio(GPIO_ADC1_IN10);
-	stm32_configgpio(GPIO_ADC1_IN11);
+	px4_arch_configgpio(GPIO_ADC1_IN10);
+	px4_arch_configgpio(GPIO_ADC1_IN11);
 	/* IN12 and IN13 further below */
 
 	/* configure the high-resolution time/callout interface */
@@ -187,7 +187,7 @@ __EXPORT int nsh_archinitialize(void)
 
 	/* Configure SPI-based devices */
 
-	spi1 = up_spiinitialize(1);
+	spi1 = px4_spibus_initialize(1);
 
 	if (!spi1) {
 		message("[boot] FAILED to initialize SPI port 1\r\n");
@@ -210,7 +210,7 @@ __EXPORT int nsh_archinitialize(void)
 	 */
 
 #ifdef CONFIG_STM32_SPI2
-	spi2 = up_spiinitialize(2);
+	spi2 = px4_spibus_initialize(2);
 	/* Default SPI2 to 1MHz and de-assert the known chip selects. */
 	SPI_SETFREQUENCY(spi2, 10000000);
 	SPI_SETBITS(spi2, 8);
@@ -223,13 +223,13 @@ __EXPORT int nsh_archinitialize(void)
 	spi2 = NULL;
 	message("[boot] Enabling IN12/13 instead of SPI2\n");
 	/* no SPI2, use pins for ADC */
-	stm32_configgpio(GPIO_ADC1_IN12);
-	stm32_configgpio(GPIO_ADC1_IN13);	// jumperable to MPU6000 DRDY on some boards
+	px4_arch_configgpio(GPIO_ADC1_IN12);
+	px4_arch_configgpio(GPIO_ADC1_IN13);	// jumperable to MPU6000 DRDY on some boards
 #endif
 
 	/* Get the SPI port for the microSD slot */
 
-	spi3 = up_spiinitialize(3);
+	spi3 = px4_spibus_initialize(3);
 
 	if (!spi3) {
 		message("[boot] FAILED to initialize SPI port 3\n");
diff --git a/src/drivers/boards/px4fmu-v1/px4fmu_led.c b/src/drivers/boards/px4fmu-v1/px4fmu_led.c
index 8a92cc6e3f..5678056d51 100644
--- a/src/drivers/boards/px4fmu-v1/px4fmu_led.c
+++ b/src/drivers/boards/px4fmu-v1/px4fmu_led.c
@@ -64,20 +64,20 @@ __EXPORT void led_init(void)
 {
 	/* Configure LED1-2 GPIOs for output */
 
-	stm32_configgpio(GPIO_LED1);
-	stm32_configgpio(GPIO_LED2);
+	px4_arch_configgpio(GPIO_LED1);
+	px4_arch_configgpio(GPIO_LED2);
 }
 
 __EXPORT void led_on(int led)
 {
 	if (led == 0) {
 		/* Pull down to switch on */
-		stm32_gpiowrite(GPIO_LED1, false);
+		px4_arch_gpiowrite(GPIO_LED1, false);
 	}
 
 	if (led == 1) {
 		/* Pull down to switch on */
-		stm32_gpiowrite(GPIO_LED2, false);
+		px4_arch_gpiowrite(GPIO_LED2, false);
 	}
 }
 
@@ -85,32 +85,32 @@ __EXPORT void led_off(int led)
 {
 	if (led == 0) {
 		/* Pull up to switch off */
-		stm32_gpiowrite(GPIO_LED1, true);
+		px4_arch_gpiowrite(GPIO_LED1, true);
 	}
 
 	if (led == 1) {
 		/* Pull up to switch off */
-		stm32_gpiowrite(GPIO_LED2, true);
+		px4_arch_gpiowrite(GPIO_LED2, true);
 	}
 }
 
 __EXPORT void led_toggle(int led)
 {
 	if (led == 0) {
-		if (stm32_gpioread(GPIO_LED1)) {
-			stm32_gpiowrite(GPIO_LED1, false);
+		if (px4_arch_gpioread(GPIO_LED1)) {
+			px4_arch_gpiowrite(GPIO_LED1, false);
 
 		} else {
-			stm32_gpiowrite(GPIO_LED1, true);
+			px4_arch_gpiowrite(GPIO_LED1, true);
 		}
 	}
 
 	if (led == 1) {
-		if (stm32_gpioread(GPIO_LED2)) {
-			stm32_gpiowrite(GPIO_LED2, false);
+		if (px4_arch_gpioread(GPIO_LED2)) {
+			px4_arch_gpiowrite(GPIO_LED2, false);
 
 		} else {
-			stm32_gpiowrite(GPIO_LED2, true);
+			px4_arch_gpiowrite(GPIO_LED2, true);
 		}
 	}
 }
diff --git a/src/drivers/boards/px4fmu-v1/px4fmu_spi.c b/src/drivers/boards/px4fmu-v1/px4fmu_spi.c
index e877a6dff4..3a0348fd1f 100644
--- a/src/drivers/boards/px4fmu-v1/px4fmu_spi.c
+++ b/src/drivers/boards/px4fmu-v1/px4fmu_spi.c
@@ -69,19 +69,19 @@
 
 __EXPORT void stm32_spiinitialize(void)
 {
-	stm32_configgpio(GPIO_SPI_CS_GYRO);
-	stm32_configgpio(GPIO_SPI_CS_ACCEL);
-	stm32_configgpio(GPIO_SPI_CS_MPU);
-	stm32_configgpio(GPIO_SPI_CS_SDCARD);
+	px4_arch_configgpio(GPIO_SPI_CS_GYRO);
+	px4_arch_configgpio(GPIO_SPI_CS_ACCEL);
+	px4_arch_configgpio(GPIO_SPI_CS_MPU);
+	px4_arch_configgpio(GPIO_SPI_CS_SDCARD);
 
 	/* De-activate all peripherals,
 	 * required for some peripheral
 	 * state machines
 	 */
-	stm32_gpiowrite(GPIO_SPI_CS_GYRO, 1);
-	stm32_gpiowrite(GPIO_SPI_CS_ACCEL, 1);
-	stm32_gpiowrite(GPIO_SPI_CS_MPU, 1);
-	stm32_gpiowrite(GPIO_SPI_CS_SDCARD, 1);
+	px4_arch_gpiowrite(GPIO_SPI_CS_GYRO, 1);
+	px4_arch_gpiowrite(GPIO_SPI_CS_ACCEL, 1);
+	px4_arch_gpiowrite(GPIO_SPI_CS_MPU, 1);
+	px4_arch_gpiowrite(GPIO_SPI_CS_SDCARD, 1);
 }
 
 __EXPORT void stm32_spi1select(FAR struct spi_dev_s *dev, enum spi_dev_e devid, bool selected)
@@ -91,23 +91,23 @@ __EXPORT void stm32_spi1select(FAR struct spi_dev_s *dev, enum spi_dev_e devid,
 	switch (devid) {
 	case PX4_SPIDEV_GYRO:
 		/* Making sure the other peripherals are not selected */
-		stm32_gpiowrite(GPIO_SPI_CS_GYRO, !selected);
-		stm32_gpiowrite(GPIO_SPI_CS_MPU, 1);
-		stm32_gpiowrite(GPIO_SPI_CS_ACCEL, 1);
+		px4_arch_gpiowrite(GPIO_SPI_CS_GYRO, !selected);
+		px4_arch_gpiowrite(GPIO_SPI_CS_MPU, 1);
+		px4_arch_gpiowrite(GPIO_SPI_CS_ACCEL, 1);
 		break;
 
 	case PX4_SPIDEV_ACCEL:
 		/* Making sure the other peripherals are not selected */
-		stm32_gpiowrite(GPIO_SPI_CS_ACCEL, !selected);
-		stm32_gpiowrite(GPIO_SPI_CS_MPU, 1);
-		stm32_gpiowrite(GPIO_SPI_CS_GYRO, 1);
+		px4_arch_gpiowrite(GPIO_SPI_CS_ACCEL, !selected);
+		px4_arch_gpiowrite(GPIO_SPI_CS_MPU, 1);
+		px4_arch_gpiowrite(GPIO_SPI_CS_GYRO, 1);
 		break;
 
 	case PX4_SPIDEV_MPU:
 		/* Making sure the other peripherals are not selected */
-		stm32_gpiowrite(GPIO_SPI_CS_ACCEL, 1);
-		stm32_gpiowrite(GPIO_SPI_CS_GYRO, 1);
-		stm32_gpiowrite(GPIO_SPI_CS_MPU, !selected);
+		px4_arch_gpiowrite(GPIO_SPI_CS_ACCEL, 1);
+		px4_arch_gpiowrite(GPIO_SPI_CS_GYRO, 1);
+		px4_arch_gpiowrite(GPIO_SPI_CS_MPU, !selected);
 		break;
 
 	default:
@@ -143,7 +143,7 @@ __EXPORT uint8_t stm32_spi2status(FAR struct spi_dev_s *dev, enum spi_dev_e devi
 __EXPORT void stm32_spi3select(FAR struct spi_dev_s *dev, enum spi_dev_e devid, bool selected)
 {
 	/* there can only be one device on this bus, so always select it */
-	stm32_gpiowrite(GPIO_SPI_CS_SDCARD, !selected);
+	px4_arch_gpiowrite(GPIO_SPI_CS_SDCARD, !selected);
 }
 
 __EXPORT uint8_t stm32_spi3status(FAR struct spi_dev_s *dev, enum spi_dev_e devid)
diff --git a/src/drivers/boards/px4fmu-v1/px4fmu_usb.c b/src/drivers/boards/px4fmu-v1/px4fmu_usb.c
index 073b3d34f9..3b73d43ed3 100644
--- a/src/drivers/boards/px4fmu-v1/px4fmu_usb.c
+++ b/src/drivers/boards/px4fmu-v1/px4fmu_usb.c
@@ -82,10 +82,10 @@ __EXPORT void stm32_usbinitialize(void)
 	/* Configure the OTG FS VBUS sensing GPIO, Power On, and Overcurrent GPIOs */
 
 #ifdef CONFIG_STM32_OTGFS
-	stm32_configgpio(GPIO_OTGFS_VBUS);
+	px4_arch_configgpio(GPIO_OTGFS_VBUS);
 	/* XXX We only support device mode
-	stm32_configgpio(GPIO_OTGFS_PWRON);
-	stm32_configgpio(GPIO_OTGFS_OVER);
+	px4_arch_configgpio(GPIO_OTGFS_PWRON);
+	px4_arch_configgpio(GPIO_OTGFS_OVER);
 	*/
 #endif
 }
diff --git a/src/drivers/boards/px4fmu-v2/board_config.h b/src/drivers/boards/px4fmu-v2/board_config.h
index daad230434..4ff10f0c99 100644
--- a/src/drivers/boards/px4fmu-v2/board_config.h
+++ b/src/drivers/boards/px4fmu-v2/board_config.h
@@ -244,6 +244,32 @@ __BEGIN_DECLS
 
 #define BOARD_NAME "PX4FMU_V2"
 
+/* By Providing BOARD_ADC_USB_CONNECTED this board support the ADC
+ * system_power interface, and therefore provides the true logic
+ * GPIO BOARD_ADC_xxxx macros.
+ */
+#define BOARD_ADC_USB_CONNECTED (px4_arch_gpioread(GPIO_OTGFS_VBUS))
+#define BOARD_ADC_BRICK_VALID   (!px4_arch_gpioread(GPIO_VDD_BRICK_VALID))
+#define BOARD_ADC_SERVO_VALID   (!px4_arch_gpioread(GPIO_VDD_SERVO_VALID))
+#define BOARD_ADC_PERIPH_5V_OC  (!px4_arch_gpioread(GPIO_VDD_5V_PERIPH_OC))
+#define BOARD_ADC_HIPOWER_5V_OC (!px4_arch_gpioread(GPIO_VDD_5V_HIPOWER_OC))
+
+#define BOARD_HAS_PWM	DIRECT_PWM_OUTPUT_CHANNELS
+
+#define BOARD_FMU_GPIO_TAB { \
+		{GPIO_GPIO0_INPUT,       GPIO_GPIO0_OUTPUT,       0}, \
+		{GPIO_GPIO1_INPUT,       GPIO_GPIO1_OUTPUT,       0}, \
+		{GPIO_GPIO2_INPUT,       GPIO_GPIO2_OUTPUT,       0}, \
+		{GPIO_GPIO3_INPUT,       GPIO_GPIO3_OUTPUT,       0}, \
+		{GPIO_GPIO4_INPUT,       GPIO_GPIO4_OUTPUT,       0}, \
+		{GPIO_GPIO5_INPUT,       GPIO_GPIO5_OUTPUT,       0}, \
+		{0,                      GPIO_VDD_5V_PERIPH_EN,   0}, \
+		{0,                      GPIO_VDD_3V3_SENSORS_EN, 0}, \
+		{GPIO_VDD_BRICK_VALID,   0,                       0}, \
+		{GPIO_VDD_SERVO_VALID,   0,                       0}, \
+		{GPIO_VDD_5V_HIPOWER_OC, 0,                       0}, \
+		{GPIO_VDD_5V_PERIPH_OC,  0,                       0}, }
+
 /****************************************************************************************************
  * Public Types
  ****************************************************************************************************/
@@ -267,9 +293,12 @@ __BEGIN_DECLS
  ****************************************************************************************************/
 
 extern void stm32_spiinitialize(void);
+extern void board_spi_reset(int ms);
 
 extern void stm32_usbinitialize(void);
 
+extern void board_peripheral_reset(int ms);
+
 /****************************************************************************
  * Name: nsh_archinitialize
  *
diff --git a/src/drivers/boards/px4fmu-v2/px4fmu2_init.c b/src/drivers/boards/px4fmu-v2/px4fmu2_init.c
index 8055a06e18..c95c5ffd44 100644
--- a/src/drivers/boards/px4fmu-v2/px4fmu2_init.c
+++ b/src/drivers/boards/px4fmu-v2/px4fmu2_init.c
@@ -71,6 +71,7 @@
 
 #include <systemlib/cpuload.h>
 #include <systemlib/perf_counter.h>
+#include <systemlib/err.h>
 
 /****************************************************************************
  * Pre-Processor Definitions
@@ -177,6 +178,28 @@ fat_dma_free(FAR void *memory, size_t size)
 
 #endif
 
+/************************************************************************************
+ * Name: board_peripheral_reset
+ *
+ * Description:
+ *
+ ************************************************************************************/
+__EXPORT void board_peripheral_reset(int ms)
+{
+	/* set the peripheral rails off */
+	px4_arch_configgpio(GPIO_VDD_5V_PERIPH_EN);
+	px4_arch_gpiowrite(GPIO_VDD_5V_PERIPH_EN, 1);
+
+	/* wait for the peripheral rail to reach GND */
+	usleep(ms * 1000);
+	warnx("reset done, %d ms", ms);
+
+	/* re-enable power */
+
+	/* switch the peripheral rail back on */
+	px4_arch_gpiowrite(GPIO_VDD_5V_PERIPH_EN, 0);
+}
+
 /************************************************************************************
  * Name: stm32_boardinitialize
  *
@@ -216,31 +239,31 @@ __EXPORT int nsh_archinitialize(void)
 {
 
 	/* configure ADC pins */
-	stm32_configgpio(GPIO_ADC1_IN2);	/* BATT_VOLTAGE_SENS */
-	stm32_configgpio(GPIO_ADC1_IN3);	/* BATT_CURRENT_SENS */
-	stm32_configgpio(GPIO_ADC1_IN4);	/* VDD_5V_SENS */
-	// stm32_configgpio(GPIO_ADC1_IN10);	/* used by VBUS valid */
-	// stm32_configgpio(GPIO_ADC1_IN11);	/* unused */
-	// stm32_configgpio(GPIO_ADC1_IN12);	/* used by MPU6000 CS */
-	stm32_configgpio(GPIO_ADC1_IN13);	/* FMU_AUX_ADC_1 */
-	stm32_configgpio(GPIO_ADC1_IN14);	/* FMU_AUX_ADC_2 */
-	stm32_configgpio(GPIO_ADC1_IN15);	/* PRESSURE_SENS */
+	px4_arch_configgpio(GPIO_ADC1_IN2);	/* BATT_VOLTAGE_SENS */
+	px4_arch_configgpio(GPIO_ADC1_IN3);	/* BATT_CURRENT_SENS */
+	px4_arch_configgpio(GPIO_ADC1_IN4);	/* VDD_5V_SENS */
+	// px4_arch_configgpio(GPIO_ADC1_IN10);	/* used by VBUS valid */
+	// px4_arch_configgpio(GPIO_ADC1_IN11);	/* unused */
+	// px4_arch_configgpio(GPIO_ADC1_IN12);	/* used by MPU6000 CS */
+	px4_arch_configgpio(GPIO_ADC1_IN13);	/* FMU_AUX_ADC_1 */
+	px4_arch_configgpio(GPIO_ADC1_IN14);	/* FMU_AUX_ADC_2 */
+	px4_arch_configgpio(GPIO_ADC1_IN15);	/* PRESSURE_SENS */
 
 	/* configure power supply control/sense pins */
-	stm32_configgpio(GPIO_VDD_5V_PERIPH_EN);
-	stm32_configgpio(GPIO_VDD_3V3_SENSORS_EN);
-	stm32_configgpio(GPIO_VDD_BRICK_VALID);
-	stm32_configgpio(GPIO_VDD_SERVO_VALID);
-	stm32_configgpio(GPIO_VDD_5V_HIPOWER_OC);
-	stm32_configgpio(GPIO_VDD_5V_PERIPH_OC);
+	px4_arch_configgpio(GPIO_VDD_5V_PERIPH_EN);
+	px4_arch_configgpio(GPIO_VDD_3V3_SENSORS_EN);
+	px4_arch_configgpio(GPIO_VDD_BRICK_VALID);
+	px4_arch_configgpio(GPIO_VDD_SERVO_VALID);
+	px4_arch_configgpio(GPIO_VDD_5V_HIPOWER_OC);
+	px4_arch_configgpio(GPIO_VDD_5V_PERIPH_OC);
 
 	/* configure the GPIO pins to outputs and keep them low */
-	stm32_configgpio(GPIO_GPIO0_OUTPUT);
-	stm32_configgpio(GPIO_GPIO1_OUTPUT);
-	stm32_configgpio(GPIO_GPIO2_OUTPUT);
-	stm32_configgpio(GPIO_GPIO3_OUTPUT);
-	stm32_configgpio(GPIO_GPIO4_OUTPUT);
-	stm32_configgpio(GPIO_GPIO5_OUTPUT);
+	px4_arch_configgpio(GPIO_GPIO0_OUTPUT);
+	px4_arch_configgpio(GPIO_GPIO1_OUTPUT);
+	px4_arch_configgpio(GPIO_GPIO2_OUTPUT);
+	px4_arch_configgpio(GPIO_GPIO3_OUTPUT);
+	px4_arch_configgpio(GPIO_GPIO4_OUTPUT);
+	px4_arch_configgpio(GPIO_GPIO5_OUTPUT);
 
 	/* configure the high-resolution time/callout interface */
 	hrt_init();
@@ -276,7 +299,7 @@ __EXPORT int nsh_archinitialize(void)
 
 	/* Configure SPI-based devices */
 
-	spi1 = up_spiinitialize(1);
+	spi1 = px4_spibus_initialize(1);
 
 	if (!spi1) {
 		message("[boot] FAILED to initialize SPI port 1\n");
@@ -296,7 +319,7 @@ __EXPORT int nsh_archinitialize(void)
 
 	/* Get the SPI port for the FRAM */
 
-	spi2 = up_spiinitialize(2);
+	spi2 = px4_spibus_initialize(2);
 
 	if (!spi2) {
 		message("[boot] FAILED to initialize SPI port 2\n");
@@ -313,7 +336,7 @@ __EXPORT int nsh_archinitialize(void)
 	SPI_SETMODE(spi2, SPIDEV_MODE3);
 	SPI_SELECT(spi2, SPIDEV_FLASH, false);
 
-	spi4 = up_spiinitialize(4);
+	spi4 = px4_spibus_initialize(4);
 
 	/* Default SPI4 to 1MHz and de-assert the known chip selects. */
 	SPI_SETFREQUENCY(spi4, 10000000);
diff --git a/src/drivers/boards/px4fmu-v2/px4fmu2_led.c b/src/drivers/boards/px4fmu-v2/px4fmu2_led.c
index 09edb2ba66..6eb8aa58f3 100644
--- a/src/drivers/boards/px4fmu-v2/px4fmu2_led.c
+++ b/src/drivers/boards/px4fmu-v2/px4fmu2_led.c
@@ -64,14 +64,14 @@ __EXPORT void led_init()
 {
 	/* Configure LED1 GPIO for output */
 
-	stm32_configgpio(GPIO_LED1);
+	px4_arch_configgpio(GPIO_LED1);
 }
 
 __EXPORT void led_on(int led)
 {
 	if (led == 1) {
 		/* Pull down to switch on */
-		stm32_gpiowrite(GPIO_LED1, false);
+		px4_arch_gpiowrite(GPIO_LED1, false);
 	}
 }
 
@@ -79,18 +79,18 @@ __EXPORT void led_off(int led)
 {
 	if (led == 1) {
 		/* Pull up to switch off */
-		stm32_gpiowrite(GPIO_LED1, true);
+		px4_arch_gpiowrite(GPIO_LED1, true);
 	}
 }
 
 __EXPORT void led_toggle(int led)
 {
 	if (led == 1) {
-		if (stm32_gpioread(GPIO_LED1)) {
-			stm32_gpiowrite(GPIO_LED1, false);
+		if (px4_arch_gpioread(GPIO_LED1)) {
+			px4_arch_gpiowrite(GPIO_LED1, false);
 
 		} else {
-			stm32_gpiowrite(GPIO_LED1, true);
+			px4_arch_gpiowrite(GPIO_LED1, true);
 		}
 	}
 }
diff --git a/src/drivers/boards/px4fmu-v2/px4fmu_spi.c b/src/drivers/boards/px4fmu-v2/px4fmu_spi.c
index 26808a6825..6fd5280015 100644
--- a/src/drivers/boards/px4fmu-v2/px4fmu_spi.c
+++ b/src/drivers/boards/px4fmu-v2/px4fmu_spi.c
@@ -46,7 +46,7 @@
 #include <stdint.h>
 #include <stdbool.h>
 #include <debug.h>
-
+#include <unistd.h>
 #include <nuttx/spi.h>
 #include <arch/board/board.h>
 
@@ -54,6 +54,7 @@
 #include <chip.h>
 #include <stm32.h>
 #include "board_config.h"
+#include <systemlib/err.h>
 
 /************************************************************************************
  * Public Functions
@@ -70,42 +71,42 @@
 __EXPORT void stm32_spiinitialize(void)
 {
 #ifdef CONFIG_STM32_SPI1
-	stm32_configgpio(GPIO_SPI_CS_GYRO);
-	stm32_configgpio(GPIO_SPI_CS_ACCEL_MAG);
-	stm32_configgpio(GPIO_SPI_CS_BARO);
-	stm32_configgpio(GPIO_SPI_CS_HMC);
-	stm32_configgpio(GPIO_SPI_CS_MPU);
+	px4_arch_configgpio(GPIO_SPI_CS_GYRO);
+	px4_arch_configgpio(GPIO_SPI_CS_ACCEL_MAG);
+	px4_arch_configgpio(GPIO_SPI_CS_BARO);
+	px4_arch_configgpio(GPIO_SPI_CS_HMC);
+	px4_arch_configgpio(GPIO_SPI_CS_MPU);
 
 	/* De-activate all peripherals,
 	 * required for some peripheral
 	 * state machines
 	 */
-	stm32_gpiowrite(GPIO_SPI_CS_GYRO, 1);
-	stm32_gpiowrite(GPIO_SPI_CS_ACCEL_MAG, 1);
-	stm32_gpiowrite(GPIO_SPI_CS_BARO, 1);
-	stm32_gpiowrite(GPIO_SPI_CS_HMC, 1);
-	stm32_gpiowrite(GPIO_SPI_CS_MPU, 1);
+	px4_arch_gpiowrite(GPIO_SPI_CS_GYRO, 1);
+	px4_arch_gpiowrite(GPIO_SPI_CS_ACCEL_MAG, 1);
+	px4_arch_gpiowrite(GPIO_SPI_CS_BARO, 1);
+	px4_arch_gpiowrite(GPIO_SPI_CS_HMC, 1);
+	px4_arch_gpiowrite(GPIO_SPI_CS_MPU, 1);
 
-	stm32_configgpio(GPIO_EXTI_GYRO_DRDY);
-	stm32_configgpio(GPIO_EXTI_MAG_DRDY);
-	stm32_configgpio(GPIO_EXTI_ACCEL_DRDY);
-	stm32_configgpio(GPIO_EXTI_MPU_DRDY);
+	px4_arch_configgpio(GPIO_EXTI_GYRO_DRDY);
+	px4_arch_configgpio(GPIO_EXTI_MAG_DRDY);
+	px4_arch_configgpio(GPIO_EXTI_ACCEL_DRDY);
+	px4_arch_configgpio(GPIO_EXTI_MPU_DRDY);
 #endif
 
 #ifdef CONFIG_STM32_SPI2
-	stm32_configgpio(GPIO_SPI_CS_FRAM);
-	stm32_gpiowrite(GPIO_SPI_CS_FRAM, 1);
+	px4_arch_configgpio(GPIO_SPI_CS_FRAM);
+	px4_arch_gpiowrite(GPIO_SPI_CS_FRAM, 1);
 #endif
 
 #ifdef CONFIG_STM32_SPI4
-	stm32_configgpio(GPIO_SPI_CS_EXT0);
-	stm32_configgpio(GPIO_SPI_CS_EXT1);
-	stm32_configgpio(GPIO_SPI_CS_EXT2);
-	stm32_configgpio(GPIO_SPI_CS_EXT3);
-	stm32_gpiowrite(GPIO_SPI_CS_EXT0, 1);
-	stm32_gpiowrite(GPIO_SPI_CS_EXT1, 1);
-	stm32_gpiowrite(GPIO_SPI_CS_EXT2, 1);
-	stm32_gpiowrite(GPIO_SPI_CS_EXT3, 1);
+	px4_arch_configgpio(GPIO_SPI_CS_EXT0);
+	px4_arch_configgpio(GPIO_SPI_CS_EXT1);
+	px4_arch_configgpio(GPIO_SPI_CS_EXT2);
+	px4_arch_configgpio(GPIO_SPI_CS_EXT3);
+	px4_arch_gpiowrite(GPIO_SPI_CS_EXT0, 1);
+	px4_arch_gpiowrite(GPIO_SPI_CS_EXT1, 1);
+	px4_arch_gpiowrite(GPIO_SPI_CS_EXT2, 1);
+	px4_arch_gpiowrite(GPIO_SPI_CS_EXT3, 1);
 #endif
 }
 
@@ -116,56 +117,56 @@ __EXPORT void stm32_spi1select(FAR struct spi_dev_s *dev, enum spi_dev_e devid,
 	switch (devid) {
 	case PX4_SPIDEV_GYRO:
 		/* Making sure the other peripherals are not selected */
-		stm32_gpiowrite(GPIO_SPI_CS_GYRO, !selected);
-		stm32_gpiowrite(GPIO_SPI_CS_ACCEL_MAG, 1);
-		stm32_gpiowrite(GPIO_SPI_CS_BARO, 1);
-		stm32_gpiowrite(GPIO_SPI_CS_HMC, 1);
-		stm32_gpiowrite(GPIO_SPI_CS_MPU, 1);
+		px4_arch_gpiowrite(GPIO_SPI_CS_GYRO, !selected);
+		px4_arch_gpiowrite(GPIO_SPI_CS_ACCEL_MAG, 1);
+		px4_arch_gpiowrite(GPIO_SPI_CS_BARO, 1);
+		px4_arch_gpiowrite(GPIO_SPI_CS_HMC, 1);
+		px4_arch_gpiowrite(GPIO_SPI_CS_MPU, 1);
 		break;
 
 	case PX4_SPIDEV_ACCEL_MAG:
 		/* Making sure the other peripherals are not selected */
-		stm32_gpiowrite(GPIO_SPI_CS_GYRO, 1);
-		stm32_gpiowrite(GPIO_SPI_CS_ACCEL_MAG, !selected);
-		stm32_gpiowrite(GPIO_SPI_CS_BARO, 1);
-		stm32_gpiowrite(GPIO_SPI_CS_HMC, 1);
-		stm32_gpiowrite(GPIO_SPI_CS_MPU, 1);
+		px4_arch_gpiowrite(GPIO_SPI_CS_GYRO, 1);
+		px4_arch_gpiowrite(GPIO_SPI_CS_ACCEL_MAG, !selected);
+		px4_arch_gpiowrite(GPIO_SPI_CS_BARO, 1);
+		px4_arch_gpiowrite(GPIO_SPI_CS_HMC, 1);
+		px4_arch_gpiowrite(GPIO_SPI_CS_MPU, 1);
 		break;
 
 	case PX4_SPIDEV_BARO:
 		/* Making sure the other peripherals are not selected */
-		stm32_gpiowrite(GPIO_SPI_CS_GYRO, 1);
-		stm32_gpiowrite(GPIO_SPI_CS_ACCEL_MAG, 1);
-		stm32_gpiowrite(GPIO_SPI_CS_BARO, !selected);
-		stm32_gpiowrite(GPIO_SPI_CS_HMC, 1);
-		stm32_gpiowrite(GPIO_SPI_CS_MPU, 1);
+		px4_arch_gpiowrite(GPIO_SPI_CS_GYRO, 1);
+		px4_arch_gpiowrite(GPIO_SPI_CS_ACCEL_MAG, 1);
+		px4_arch_gpiowrite(GPIO_SPI_CS_BARO, !selected);
+		px4_arch_gpiowrite(GPIO_SPI_CS_HMC, 1);
+		px4_arch_gpiowrite(GPIO_SPI_CS_MPU, 1);
 		break;
 
 	case PX4_SPIDEV_HMC:
 		/* Making sure the other peripherals are not selected */
-		stm32_gpiowrite(GPIO_SPI_CS_GYRO, 1);
-		stm32_gpiowrite(GPIO_SPI_CS_ACCEL_MAG, 1);
-		stm32_gpiowrite(GPIO_SPI_CS_BARO, 1);
-		stm32_gpiowrite(GPIO_SPI_CS_HMC, !selected);
-		stm32_gpiowrite(GPIO_SPI_CS_MPU, 1);
+		px4_arch_gpiowrite(GPIO_SPI_CS_GYRO, 1);
+		px4_arch_gpiowrite(GPIO_SPI_CS_ACCEL_MAG, 1);
+		px4_arch_gpiowrite(GPIO_SPI_CS_BARO, 1);
+		px4_arch_gpiowrite(GPIO_SPI_CS_HMC, !selected);
+		px4_arch_gpiowrite(GPIO_SPI_CS_MPU, 1);
 		break;
 
 	case PX4_SPIDEV_LIS:
 		/* Making sure the other peripherals are not selected */
-		stm32_gpiowrite(GPIO_SPI_CS_GYRO, 1);
-		stm32_gpiowrite(GPIO_SPI_CS_ACCEL_MAG, 1);
-		stm32_gpiowrite(GPIO_SPI_CS_BARO, 1);
-		stm32_gpiowrite(GPIO_SPI_CS_LIS, !selected);
-		stm32_gpiowrite(GPIO_SPI_CS_MPU, 1);
+		px4_arch_gpiowrite(GPIO_SPI_CS_GYRO, 1);
+		px4_arch_gpiowrite(GPIO_SPI_CS_ACCEL_MAG, 1);
+		px4_arch_gpiowrite(GPIO_SPI_CS_BARO, 1);
+		px4_arch_gpiowrite(GPIO_SPI_CS_LIS, !selected);
+		px4_arch_gpiowrite(GPIO_SPI_CS_MPU, 1);
 		break;
 
 	case PX4_SPIDEV_MPU:
 		/* Making sure the other peripherals are not selected */
-		stm32_gpiowrite(GPIO_SPI_CS_GYRO, 1);
-		stm32_gpiowrite(GPIO_SPI_CS_ACCEL_MAG, 1);
-		stm32_gpiowrite(GPIO_SPI_CS_BARO, 1);
-		stm32_gpiowrite(GPIO_SPI_CS_HMC, 1);
-		stm32_gpiowrite(GPIO_SPI_CS_MPU, !selected);
+		px4_arch_gpiowrite(GPIO_SPI_CS_GYRO, 1);
+		px4_arch_gpiowrite(GPIO_SPI_CS_ACCEL_MAG, 1);
+		px4_arch_gpiowrite(GPIO_SPI_CS_BARO, 1);
+		px4_arch_gpiowrite(GPIO_SPI_CS_HMC, 1);
+		px4_arch_gpiowrite(GPIO_SPI_CS_MPU, !selected);
 		break;
 
 	default:
@@ -183,7 +184,7 @@ __EXPORT uint8_t stm32_spi1status(FAR struct spi_dev_s *dev, enum spi_dev_e devi
 __EXPORT void stm32_spi2select(FAR struct spi_dev_s *dev, enum spi_dev_e devid, bool selected)
 {
 	/* there can only be one device on this bus, so always select it */
-	stm32_gpiowrite(GPIO_SPI_CS_FRAM, !selected);
+	px4_arch_gpiowrite(GPIO_SPI_CS_FRAM, !selected);
 }
 
 __EXPORT uint8_t stm32_spi2status(FAR struct spi_dev_s *dev, enum spi_dev_e devid)
@@ -200,34 +201,34 @@ __EXPORT void stm32_spi4select(FAR struct spi_dev_s *dev, enum spi_dev_e devid,
 	switch (devid) {
 	case PX4_SPIDEV_EXT0:
 		/* Making sure the other peripherals are not selected */
-		stm32_gpiowrite(GPIO_SPI_CS_EXT0, !selected);
-		stm32_gpiowrite(GPIO_SPI_CS_EXT1, 1);
-		stm32_gpiowrite(GPIO_SPI_CS_EXT2, 1);
-		stm32_gpiowrite(GPIO_SPI_CS_EXT3, 1);
+		px4_arch_gpiowrite(GPIO_SPI_CS_EXT0, !selected);
+		px4_arch_gpiowrite(GPIO_SPI_CS_EXT1, 1);
+		px4_arch_gpiowrite(GPIO_SPI_CS_EXT2, 1);
+		px4_arch_gpiowrite(GPIO_SPI_CS_EXT3, 1);
 		break;
 
 	case PX4_SPIDEV_EXT1:
 		/* Making sure the other peripherals are not selected */
-		stm32_gpiowrite(GPIO_SPI_CS_EXT0, 1);
-		stm32_gpiowrite(GPIO_SPI_CS_EXT1, !selected);
-		stm32_gpiowrite(GPIO_SPI_CS_EXT2, 1);
-		stm32_gpiowrite(GPIO_SPI_CS_EXT3, 1);
+		px4_arch_gpiowrite(GPIO_SPI_CS_EXT0, 1);
+		px4_arch_gpiowrite(GPIO_SPI_CS_EXT1, !selected);
+		px4_arch_gpiowrite(GPIO_SPI_CS_EXT2, 1);
+		px4_arch_gpiowrite(GPIO_SPI_CS_EXT3, 1);
 		break;
 
 	case PX4_SPIDEV_EXT2:
 		/* Making sure the other peripherals are not selected */
-		stm32_gpiowrite(GPIO_SPI_CS_EXT0, 1);
-		stm32_gpiowrite(GPIO_SPI_CS_EXT1, 1);
-		stm32_gpiowrite(GPIO_SPI_CS_EXT2, !selected);
-		stm32_gpiowrite(GPIO_SPI_CS_EXT3, 1);
+		px4_arch_gpiowrite(GPIO_SPI_CS_EXT0, 1);
+		px4_arch_gpiowrite(GPIO_SPI_CS_EXT1, 1);
+		px4_arch_gpiowrite(GPIO_SPI_CS_EXT2, !selected);
+		px4_arch_gpiowrite(GPIO_SPI_CS_EXT3, 1);
 		break;
 
 	case PX4_SPIDEV_EXT3:
 		/* Making sure the other peripherals are not selected */
-		stm32_gpiowrite(GPIO_SPI_CS_EXT0, 1);
-		stm32_gpiowrite(GPIO_SPI_CS_EXT1, 1);
-		stm32_gpiowrite(GPIO_SPI_CS_EXT2, 1);
-		stm32_gpiowrite(GPIO_SPI_CS_EXT3, !selected);
+		px4_arch_gpiowrite(GPIO_SPI_CS_EXT0, 1);
+		px4_arch_gpiowrite(GPIO_SPI_CS_EXT1, 1);
+		px4_arch_gpiowrite(GPIO_SPI_CS_EXT2, 1);
+		px4_arch_gpiowrite(GPIO_SPI_CS_EXT3, !selected);
 		break;
 
 	default:
@@ -240,3 +241,79 @@ __EXPORT uint8_t stm32_spi4status(FAR struct spi_dev_s *dev, enum spi_dev_e devi
 {
 	return SPI_STATUS_PRESENT;
 }
+
+__EXPORT void board_spi_reset(int ms)
+{
+	/* disable SPI bus */
+	px4_arch_configgpio(GPIO_SPI_CS_GYRO_OFF);
+	px4_arch_configgpio(GPIO_SPI_CS_ACCEL_MAG_OFF);
+	px4_arch_configgpio(GPIO_SPI_CS_BARO_OFF);
+	px4_arch_configgpio(GPIO_SPI_CS_MPU_OFF);
+
+	px4_arch_gpiowrite(GPIO_SPI_CS_GYRO_OFF, 0);
+	px4_arch_gpiowrite(GPIO_SPI_CS_ACCEL_MAG_OFF, 0);
+	px4_arch_gpiowrite(GPIO_SPI_CS_BARO_OFF, 0);
+	px4_arch_gpiowrite(GPIO_SPI_CS_MPU_OFF, 0);
+
+	px4_arch_configgpio(GPIO_SPI1_SCK_OFF);
+	px4_arch_configgpio(GPIO_SPI1_MISO_OFF);
+	px4_arch_configgpio(GPIO_SPI1_MOSI_OFF);
+
+	px4_arch_gpiowrite(GPIO_SPI1_SCK_OFF, 0);
+	px4_arch_gpiowrite(GPIO_SPI1_MISO_OFF, 0);
+	px4_arch_gpiowrite(GPIO_SPI1_MOSI_OFF, 0);
+
+	px4_arch_configgpio(GPIO_GYRO_DRDY_OFF);
+	px4_arch_configgpio(GPIO_MAG_DRDY_OFF);
+	px4_arch_configgpio(GPIO_ACCEL_DRDY_OFF);
+	px4_arch_configgpio(GPIO_EXTI_MPU_DRDY_OFF);
+
+	px4_arch_gpiowrite(GPIO_GYRO_DRDY_OFF, 0);
+	px4_arch_gpiowrite(GPIO_MAG_DRDY_OFF, 0);
+	px4_arch_gpiowrite(GPIO_ACCEL_DRDY_OFF, 0);
+	px4_arch_gpiowrite(GPIO_EXTI_MPU_DRDY_OFF, 0);
+
+	/* set the sensor rail off */
+	px4_arch_configgpio(GPIO_VDD_3V3_SENSORS_EN);
+	px4_arch_gpiowrite(GPIO_VDD_3V3_SENSORS_EN, 0);
+
+	/* wait for the sensor rail to reach GND */
+	usleep(ms * 1000);
+	warnx("reset done, %d ms", ms);
+
+	/* re-enable power */
+
+	/* switch the sensor rail back on */
+	px4_arch_gpiowrite(GPIO_VDD_3V3_SENSORS_EN, 1);
+
+	/* wait a bit before starting SPI, different times didn't influence results */
+	usleep(100);
+
+	/* reconfigure the SPI pins */
+#ifdef CONFIG_STM32_SPI1
+	px4_arch_configgpio(GPIO_SPI_CS_GYRO);
+	px4_arch_configgpio(GPIO_SPI_CS_ACCEL_MAG);
+	px4_arch_configgpio(GPIO_SPI_CS_BARO);
+	px4_arch_configgpio(GPIO_SPI_CS_MPU);
+
+	/* De-activate all peripherals,
+	 * required for some peripheral
+	 * state machines
+	 */
+	px4_arch_gpiowrite(GPIO_SPI_CS_GYRO, 1);
+	px4_arch_gpiowrite(GPIO_SPI_CS_ACCEL_MAG, 1);
+	px4_arch_gpiowrite(GPIO_SPI_CS_BARO, 1);
+	px4_arch_gpiowrite(GPIO_SPI_CS_MPU, 1);
+
+	px4_arch_configgpio(GPIO_SPI1_SCK);
+	px4_arch_configgpio(GPIO_SPI1_MISO);
+	px4_arch_configgpio(GPIO_SPI1_MOSI);
+
+	// // XXX bring up the EXTI pins again
+	// px4_arch_configgpio(GPIO_GYRO_DRDY);
+	// px4_arch_configgpio(GPIO_MAG_DRDY);
+	// px4_arch_configgpio(GPIO_ACCEL_DRDY);
+	// px4_arch_configgpio(GPIO_EXTI_MPU_DRDY);
+
+#endif
+}
diff --git a/src/drivers/boards/px4fmu-v2/px4fmu_usb.c b/src/drivers/boards/px4fmu-v2/px4fmu_usb.c
index d42c35301f..47359ca688 100644
--- a/src/drivers/boards/px4fmu-v2/px4fmu_usb.c
+++ b/src/drivers/boards/px4fmu-v2/px4fmu_usb.c
@@ -82,10 +82,10 @@ __EXPORT void stm32_usbinitialize(void)
 	/* Configure the OTG FS VBUS sensing GPIO, Power On, and Overcurrent GPIOs */
 
 #ifdef CONFIG_STM32_OTGFS
-	stm32_configgpio(GPIO_OTGFS_VBUS);
+	px4_arch_configgpio(GPIO_OTGFS_VBUS);
 	/* XXX We only support device mode
-	stm32_configgpio(GPIO_OTGFS_PWRON);
-	stm32_configgpio(GPIO_OTGFS_OVER);
+	px4_arch_configgpio(GPIO_OTGFS_PWRON);
+	px4_arch_configgpio(GPIO_OTGFS_OVER);
 	*/
 #endif
 }
diff --git a/src/drivers/boards/px4fmu-v4/board_config.h b/src/drivers/boards/px4fmu-v4/board_config.h
index e1871185a1..dfc691b5fa 100644
--- a/src/drivers/boards/px4fmu-v4/board_config.h
+++ b/src/drivers/boards/px4fmu-v4/board_config.h
@@ -251,10 +251,10 @@ __BEGIN_DECLS
 #define GPIO_PERIPH_3V3_EN		(GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTC|GPIO_PIN5)
 /* for R07, this signal is active low */
 //#define GPIO_SBUS_INV			(GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTC|GPIO_PIN13)
-//#define INVERT_RC_INPUT(_s)		stm32_gpiowrite(GPIO_SBUS_INV, 1-_s);
+//#define INVERT_RC_INPUT(_s)		px4_arch_gpiowrite(GPIO_SBUS_INV, 1-_s);
 /* for R12, this signal is active high */
 #define GPIO_SBUS_INV			(GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTC|GPIO_PIN13)
-#define INVERT_RC_INPUT(_s)		stm32_gpiowrite(GPIO_SBUS_INV, _s);
+#define INVERT_RC_INPUT(_s)		px4_arch_gpiowrite(GPIO_SBUS_INV, _s);
 
 #define GPIO_8266_GPIO0			(GPIO_INPUT|GPIO_PULLUP|GPIO_PORTE|GPIO_PIN2)
 #define GPIO_SPEKTRUM_PWR_EN		(GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTE|GPIO_PIN4)
@@ -263,18 +263,41 @@ __BEGIN_DECLS
 
 /* Power switch controls ******************************************************/
 
-#define POWER_SPEKTRUM(_s)			stm32_gpiowrite(GPIO_SPEKTRUM_PWR_EN, (1-_s))
+#define POWER_SPEKTRUM(_s)			px4_arch_gpiowrite(GPIO_SPEKTRUM_PWR_EN, (1-_s))
 //#define GPIO_USART1_RX_SPEKTRUM		(GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTC|GPIO_PIN7)
-#define SPEKTRUM_RX_AS_UART()		stm32_configgpio(GPIO_USART1_RX)
+#define SPEKTRUM_RX_AS_UART()		px4_arch_configgpio(GPIO_USART1_RX)
 
 // FMUv4 has a separate GPIO for serial RC output
 #define GPIO_RC_OUT			(GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTC|GPIO_PIN6)
-#define SPEKTRUM_RX_AS_GPIO()		stm32_configgpio(GPIO_RC_OUT)
-#define SPEKTRUM_RX_HIGH(_s)		stm32_gpiowrite(GPIO_RC_OUT, (_s))
+#define SPEKTRUM_RX_AS_GPIO()		px4_arch_configgpio(GPIO_RC_OUT)
+#define SPEKTRUM_RX_HIGH(_s)		px4_arch_gpiowrite(GPIO_RC_OUT, (_s))
 
 
 #define	BOARD_NAME "PX4FMU_V4"
 
+/* By Providing BOARD_ADC_USB_CONNECTED this board support the ADC
+ * system_power interface, and herefore provides the true logic
+ * GPIO BOARD_ADC_xxxx macros.
+ */
+#define BOARD_ADC_USB_CONNECTED (px4_arch_gpioread(GPIO_OTGFS_VBUS))
+#define BOARD_ADC_BRICK_VALID   (px4_arch_gpioread(GPIO_VDD_BRICK_VALID))
+#define BOARD_ADC_SERVO_VALID   (1)
+#define BOARD_ADC_PERIPH_5V_OC  (0)
+#define BOARD_ADC_HIPOWER_5V_OC (0)
+
+#define BOARD_HAS_PWM	DIRECT_PWM_OUTPUT_CHANNELS
+
+#define BOARD_FMU_GPIO_TAB { \
+		{GPIO_GPIO0_INPUT,       GPIO_GPIO0_OUTPUT,       0}, \
+		{GPIO_GPIO1_INPUT,       GPIO_GPIO1_OUTPUT,       0}, \
+		{GPIO_GPIO2_INPUT,       GPIO_GPIO2_OUTPUT,       0}, \
+		{GPIO_GPIO3_INPUT,       GPIO_GPIO3_OUTPUT,       0}, \
+		{GPIO_GPIO4_INPUT,       GPIO_GPIO4_OUTPUT,       0}, \
+		{GPIO_GPIO5_INPUT,       GPIO_GPIO5_OUTPUT,       0}, \
+		{0,                      GPIO_VDD_3V3_SENSORS_EN, 0}, \
+		{GPIO_VDD_BRICK_VALID,   0,                       0}, }
+
+
 /****************************************************************************************************
  * Public Types
  ****************************************************************************************************/
@@ -298,9 +321,13 @@ __BEGIN_DECLS
  ****************************************************************************************************/
 
 extern void stm32_spiinitialize(void);
+void board_spi_reset(int ms);
 
 extern void stm32_usbinitialize(void);
 
+extern void board_peripheral_reset(int ms);
+
+
 /****************************************************************************
  * Name: nsh_archinitialize
  *
diff --git a/src/drivers/boards/px4fmu-v4/px4fmu_init.c b/src/drivers/boards/px4fmu-v4/px4fmu_init.c
index 3b72690ba8..f3af5e3f6d 100644
--- a/src/drivers/boards/px4fmu-v4/px4fmu_init.c
+++ b/src/drivers/boards/px4fmu-v4/px4fmu_init.c
@@ -71,6 +71,7 @@
 
 #include <systemlib/cpuload.h>
 #include <systemlib/perf_counter.h>
+#include <systemlib/err.h>
 
 /****************************************************************************
  * Pre-Processor Definitions
@@ -177,6 +178,35 @@ fat_dma_free(FAR void *memory, size_t size)
 
 #endif
 
+/************************************************************************************
+ * Name: board_peripheral_reset
+ *
+ * Description:
+ *
+ ************************************************************************************/
+__EXPORT void board_peripheral_reset(int ms)
+{
+	/* set the peripheral rails off */
+	px4_arch_configgpio(GPIO_PERIPH_3V3_EN);
+
+	px4_arch_gpiowrite(GPIO_PERIPH_3V3_EN, 0);
+
+	bool last = px4_arch_gpioread(GPIO_SPEKTRUM_PWR_EN);
+	/* Keep Spektum on to discharge rail*/
+	px4_arch_gpiowrite(GPIO_SPEKTRUM_PWR_EN, 1);
+
+	/* wait for the peripheral rail to reach GND */
+	usleep(ms * 1000);
+	warnx("reset done, %d ms", ms);
+
+	/* re-enable power */
+
+	/* switch the peripheral rail back on */
+	px4_arch_gpiowrite(GPIO_SPEKTRUM_PWR_EN, last);
+	px4_arch_gpiowrite(GPIO_PERIPH_3V3_EN, 1);
+
+}
+
 /************************************************************************************
  * Name: stm32_boardinitialize
  *
@@ -215,34 +245,34 @@ __EXPORT int nsh_archinitialize(void)
 {
 
 	/* configure ADC pins */
-	stm32_configgpio(GPIO_ADC1_IN2);	/* BATT_VOLTAGE_SENS */
-	stm32_configgpio(GPIO_ADC1_IN3);	/* BATT_CURRENT_SENS */
-	stm32_configgpio(GPIO_ADC1_IN4);	/* VDD_5V_SENS */
-	stm32_configgpio(GPIO_ADC1_IN11);	/* RSSI analog in */
+	px4_arch_configgpio(GPIO_ADC1_IN2);	/* BATT_VOLTAGE_SENS */
+	px4_arch_configgpio(GPIO_ADC1_IN3);	/* BATT_CURRENT_SENS */
+	px4_arch_configgpio(GPIO_ADC1_IN4);	/* VDD_5V_SENS */
+	px4_arch_configgpio(GPIO_ADC1_IN11);	/* RSSI analog in */
 
 	/* configure power supply control/sense pins */
-	stm32_configgpio(GPIO_PERIPH_3V3_EN);
-	stm32_configgpio(GPIO_VDD_BRICK_VALID);
+	px4_arch_configgpio(GPIO_PERIPH_3V3_EN);
+	px4_arch_configgpio(GPIO_VDD_BRICK_VALID);
 
-	stm32_configgpio(GPIO_SBUS_INV);
-	stm32_configgpio(GPIO_8266_GPIO0);
-	stm32_configgpio(GPIO_SPEKTRUM_PWR_EN);
-	stm32_configgpio(GPIO_8266_PD);
-	stm32_configgpio(GPIO_8266_RST);
-	stm32_configgpio(GPIO_BTN_SAFETY);
+	px4_arch_configgpio(GPIO_SBUS_INV);
+	px4_arch_configgpio(GPIO_8266_GPIO0);
+	px4_arch_configgpio(GPIO_SPEKTRUM_PWR_EN);
+	px4_arch_configgpio(GPIO_8266_PD);
+	px4_arch_configgpio(GPIO_8266_RST);
+	px4_arch_configgpio(GPIO_BTN_SAFETY);
 
 #ifdef GPIO_RC_OUT
-	stm32_configgpio(GPIO_RC_OUT);      /* Serial RC output pin */
-	stm32_gpiowrite(GPIO_RC_OUT, 1);    /* set it high to pull RC input up */
+	px4_arch_configgpio(GPIO_RC_OUT);      /* Serial RC output pin */
+	px4_arch_gpiowrite(GPIO_RC_OUT, 1);    /* set it high to pull RC input up */
 #endif
 
 	/* configure the GPIO pins to outputs and keep them low */
-	stm32_configgpio(GPIO_GPIO0_OUTPUT);
-	stm32_configgpio(GPIO_GPIO1_OUTPUT);
-	stm32_configgpio(GPIO_GPIO2_OUTPUT);
-	stm32_configgpio(GPIO_GPIO3_OUTPUT);
-	stm32_configgpio(GPIO_GPIO4_OUTPUT);
-	stm32_configgpio(GPIO_GPIO5_OUTPUT);
+	px4_arch_configgpio(GPIO_GPIO0_OUTPUT);
+	px4_arch_configgpio(GPIO_GPIO1_OUTPUT);
+	px4_arch_configgpio(GPIO_GPIO2_OUTPUT);
+	px4_arch_configgpio(GPIO_GPIO3_OUTPUT);
+	px4_arch_configgpio(GPIO_GPIO4_OUTPUT);
+	px4_arch_configgpio(GPIO_GPIO5_OUTPUT);
 
 	/* configure the high-resolution time/callout interface */
 	hrt_init();
@@ -280,7 +310,7 @@ __EXPORT int nsh_archinitialize(void)
 
 	/* Configure SPI-based devices */
 
-	spi1 = up_spiinitialize(1);
+	spi1 = px4_spibus_initialize(1);
 
 	if (!spi1) {
 		message("[boot] FAILED to initialize SPI port 1\n");
@@ -299,7 +329,7 @@ __EXPORT int nsh_archinitialize(void)
 
 	/* Get the SPI port for the FRAM */
 
-	spi2 = up_spiinitialize(2);
+	spi2 = px4_spibus_initialize(2);
 
 	if (!spi2) {
 		message("[boot] FAILED to initialize SPI port 2\n");
diff --git a/src/drivers/boards/px4fmu-v4/px4fmu_led.c b/src/drivers/boards/px4fmu-v4/px4fmu_led.c
index ddb34e8a56..4ea7a6227b 100644
--- a/src/drivers/boards/px4fmu-v4/px4fmu_led.c
+++ b/src/drivers/boards/px4fmu-v4/px4fmu_led.c
@@ -73,20 +73,20 @@ __EXPORT void led_init(void)
 {
 	/* Configure LED GPIOs for output */
 	for (size_t l = 0; l < (sizeof(g_ledmap) / sizeof(g_ledmap[0])); l++) {
-		stm32_configgpio(g_ledmap[l]);
+		px4_arch_configgpio(g_ledmap[l]);
 	}
 }
 
 static void phy_set_led(int led, bool state)
 {
 	/* Pull Down to switch on */
-	stm32_gpiowrite(g_ledmap[led], !state);
+	px4_arch_gpiowrite(g_ledmap[led], !state);
 }
 
 static bool phy_get_led(int led)
 {
 
-	return !stm32_gpioread(g_ledmap[led]);
+	return !px4_arch_gpioread(g_ledmap[led]);
 }
 
 __EXPORT void led_on(int led)
diff --git a/src/drivers/boards/px4fmu-v4/px4fmu_spi.c b/src/drivers/boards/px4fmu-v4/px4fmu_spi.c
index 30560b717f..b1776bd953 100644
--- a/src/drivers/boards/px4fmu-v4/px4fmu_spi.c
+++ b/src/drivers/boards/px4fmu-v4/px4fmu_spi.c
@@ -46,6 +46,7 @@
 #include <stdint.h>
 #include <stdbool.h>
 #include <debug.h>
+#include <unistd.h>
 
 #include <nuttx/spi.h>
 #include <arch/board/board.h>
@@ -54,6 +55,7 @@
 #include <chip.h>
 #include <stm32.h>
 #include "board_config.h"
+#include <systemlib/err.h>
 
 /************************************************************************************
  * Public Functions
@@ -70,28 +72,28 @@
 __EXPORT void stm32_spiinitialize(void)
 {
 #ifdef CONFIG_STM32_SPI1
-	stm32_configgpio(GPIO_SPI_CS_MPU9250);
-	stm32_configgpio(GPIO_SPI_CS_HMC5983);
-	stm32_configgpio(GPIO_SPI_CS_MS5611);
-	stm32_configgpio(GPIO_SPI_CS_ICM_20608_G);
+	px4_arch_configgpio(GPIO_SPI_CS_MPU9250);
+	px4_arch_configgpio(GPIO_SPI_CS_HMC5983);
+	px4_arch_configgpio(GPIO_SPI_CS_MS5611);
+	px4_arch_configgpio(GPIO_SPI_CS_ICM_20608_G);
 
 	/* De-activate all peripherals,
 	 * required for some peripheral
 	 * state machines
 	 */
-	stm32_gpiowrite(GPIO_SPI_CS_MPU9250, 1);
-	stm32_gpiowrite(GPIO_SPI_CS_HMC5983, 1);
-	stm32_gpiowrite(GPIO_SPI_CS_MS5611, 1);
-	stm32_gpiowrite(GPIO_SPI_CS_ICM_20608_G, 1);
+	px4_arch_gpiowrite(GPIO_SPI_CS_MPU9250, 1);
+	px4_arch_gpiowrite(GPIO_SPI_CS_HMC5983, 1);
+	px4_arch_gpiowrite(GPIO_SPI_CS_MS5611, 1);
+	px4_arch_gpiowrite(GPIO_SPI_CS_ICM_20608_G, 1);
 
-	stm32_configgpio(GPIO_DRDY_MPU9250);
-	stm32_configgpio(GPIO_DRDY_HMC5983);
-	stm32_configgpio(GPIO_DRDY_ICM_20608_G);
+	px4_arch_configgpio(GPIO_DRDY_MPU9250);
+	px4_arch_configgpio(GPIO_DRDY_HMC5983);
+	px4_arch_configgpio(GPIO_DRDY_ICM_20608_G);
 #endif
 
 #ifdef CONFIG_STM32_SPI2
-	stm32_configgpio(GPIO_SPI_CS_FRAM);
-	stm32_gpiowrite(GPIO_SPI_CS_FRAM, 1);
+	px4_arch_configgpio(GPIO_SPI_CS_FRAM);
+	px4_arch_gpiowrite(GPIO_SPI_CS_FRAM, 1);
 #endif
 
 }
@@ -103,10 +105,10 @@ __EXPORT void stm32_spi1select(FAR struct spi_dev_s *dev, enum spi_dev_e devid,
 	switch (devid) {
 	case PX4_SPIDEV_ICM:
 		/* Making sure the other peripherals are not selected */
-		stm32_gpiowrite(GPIO_SPI_CS_MPU9250, 1);
-		stm32_gpiowrite(GPIO_SPI_CS_HMC5983, 1);
-		stm32_gpiowrite(GPIO_SPI_CS_MS5611, 1);
-		stm32_gpiowrite(GPIO_SPI_CS_ICM_20608_G, !selected);
+		px4_arch_gpiowrite(GPIO_SPI_CS_MPU9250, 1);
+		px4_arch_gpiowrite(GPIO_SPI_CS_HMC5983, 1);
+		px4_arch_gpiowrite(GPIO_SPI_CS_MS5611, 1);
+		px4_arch_gpiowrite(GPIO_SPI_CS_ICM_20608_G, !selected);
 		break;
 
 	case PX4_SPIDEV_ACCEL_MAG:
@@ -115,26 +117,26 @@ __EXPORT void stm32_spi1select(FAR struct spi_dev_s *dev, enum spi_dev_e devid,
 
 	case PX4_SPIDEV_BARO:
 		/* Making sure the other peripherals are not selected */
-		stm32_gpiowrite(GPIO_SPI_CS_MPU9250, 1);
-		stm32_gpiowrite(GPIO_SPI_CS_HMC5983, 1);
-		stm32_gpiowrite(GPIO_SPI_CS_MS5611, !selected);
-		stm32_gpiowrite(GPIO_SPI_CS_ICM_20608_G, 1);
+		px4_arch_gpiowrite(GPIO_SPI_CS_MPU9250, 1);
+		px4_arch_gpiowrite(GPIO_SPI_CS_HMC5983, 1);
+		px4_arch_gpiowrite(GPIO_SPI_CS_MS5611, !selected);
+		px4_arch_gpiowrite(GPIO_SPI_CS_ICM_20608_G, 1);
 		break;
 
 	case PX4_SPIDEV_HMC:
 		/* Making sure the other peripherals are not selected */
-		stm32_gpiowrite(GPIO_SPI_CS_MPU9250, 1);
-		stm32_gpiowrite(GPIO_SPI_CS_HMC5983, !selected);
-		stm32_gpiowrite(GPIO_SPI_CS_MS5611, 1);
-		stm32_gpiowrite(GPIO_SPI_CS_ICM_20608_G, 1);
+		px4_arch_gpiowrite(GPIO_SPI_CS_MPU9250, 1);
+		px4_arch_gpiowrite(GPIO_SPI_CS_HMC5983, !selected);
+		px4_arch_gpiowrite(GPIO_SPI_CS_MS5611, 1);
+		px4_arch_gpiowrite(GPIO_SPI_CS_ICM_20608_G, 1);
 		break;
 
 	case PX4_SPIDEV_MPU:
 		/* Making sure the other peripherals are not selected */
-		stm32_gpiowrite(GPIO_SPI_CS_MPU9250, !selected);
-		stm32_gpiowrite(GPIO_SPI_CS_HMC5983, 1);
-		stm32_gpiowrite(GPIO_SPI_CS_MS5611, 1);
-		stm32_gpiowrite(GPIO_SPI_CS_ICM_20608_G, 1);
+		px4_arch_gpiowrite(GPIO_SPI_CS_MPU9250, !selected);
+		px4_arch_gpiowrite(GPIO_SPI_CS_HMC5983, 1);
+		px4_arch_gpiowrite(GPIO_SPI_CS_MS5611, 1);
+		px4_arch_gpiowrite(GPIO_SPI_CS_ICM_20608_G, 1);
 		break;
 
 	default:
@@ -156,14 +158,14 @@ __EXPORT void stm32_spi2select(FAR struct spi_dev_s *dev, enum spi_dev_e devid,
 	switch (devid) {
 	case SPIDEV_FLASH:
 		/* Making sure the other peripherals are not selected */
-		stm32_gpiowrite(GPIO_SPI_CS_MS5611, 1);
-		stm32_gpiowrite(GPIO_SPI_CS_FRAM, !selected);
+		px4_arch_gpiowrite(GPIO_SPI_CS_MS5611, 1);
+		px4_arch_gpiowrite(GPIO_SPI_CS_FRAM, !selected);
 		break;
 
 	case PX4_SPIDEV_BARO:
 		/* Making sure the other peripherals are not selected */
-		stm32_gpiowrite(GPIO_SPI_CS_FRAM, 1);
-		stm32_gpiowrite(GPIO_SPI_CS_MS5611, !selected);
+		px4_arch_gpiowrite(GPIO_SPI_CS_FRAM, 1);
+		px4_arch_gpiowrite(GPIO_SPI_CS_MS5611, !selected);
 		break;
 
 	default:
@@ -177,3 +179,78 @@ __EXPORT uint8_t stm32_spi2status(FAR struct spi_dev_s *dev, enum spi_dev_e devi
 	return SPI_STATUS_PRESENT;
 }
 #endif
+
+__EXPORT void board_spi_reset(int ms)
+{
+	/* disable SPI bus */
+	px4_arch_configgpio(GPIO_SPI_CS_OFF_MPU9250);
+	px4_arch_configgpio(GPIO_SPI_CS_OFF_HMC5983);
+	px4_arch_configgpio(GPIO_SPI_CS_OFF_MS5611);
+	px4_arch_configgpio(GPIO_SPI_CS_OFF_ICM_20608_G);
+
+	px4_arch_gpiowrite(GPIO_SPI_CS_OFF_MPU9250, 0);
+	px4_arch_gpiowrite(GPIO_SPI_CS_OFF_HMC5983, 0);
+	px4_arch_gpiowrite(GPIO_SPI_CS_OFF_MS5611, 0);
+	px4_arch_gpiowrite(GPIO_SPI_CS_OFF_ICM_20608_G, 0);
+
+	px4_arch_configgpio(GPIO_SPI1_SCK_OFF);
+	px4_arch_configgpio(GPIO_SPI1_MISO_OFF);
+	px4_arch_configgpio(GPIO_SPI1_MOSI_OFF);
+
+	px4_arch_gpiowrite(GPIO_SPI1_SCK_OFF, 0);
+	px4_arch_gpiowrite(GPIO_SPI1_MISO_OFF, 0);
+	px4_arch_gpiowrite(GPIO_SPI1_MOSI_OFF, 0);
+
+	px4_arch_configgpio(GPIO_DRDY_OFF_MPU9250);
+	px4_arch_configgpio(GPIO_DRDY_OFF_HMC5983);
+	px4_arch_configgpio(GPIO_DRDY_OFF_ICM_20608_G);
+
+	px4_arch_gpiowrite(GPIO_DRDY_OFF_MPU9250, 0);
+	px4_arch_gpiowrite(GPIO_DRDY_OFF_HMC5983, 0);
+	px4_arch_gpiowrite(GPIO_DRDY_OFF_ICM_20608_G, 0);
+
+	/* set the sensor rail off */
+	px4_arch_configgpio(GPIO_VDD_3V3_SENSORS_EN);
+	px4_arch_gpiowrite(GPIO_VDD_3V3_SENSORS_EN, 0);
+
+	/* wait for the sensor rail to reach GND */
+	usleep(ms * 1000);
+	warnx("reset done, %d ms", ms);
+
+	/* re-enable power */
+
+	/* switch the sensor rail back on */
+	px4_arch_gpiowrite(GPIO_VDD_3V3_SENSORS_EN, 1);
+
+	/* wait a bit before starting SPI, different times didn't influence results */
+	usleep(100);
+
+	/* reconfigure the SPI pins */
+#ifdef CONFIG_STM32_SPI1
+	px4_arch_configgpio(GPIO_SPI_CS_MPU9250);
+	px4_arch_configgpio(GPIO_SPI_CS_HMC5983);
+	px4_arch_configgpio(GPIO_SPI_CS_MS5611);
+	px4_arch_configgpio(GPIO_SPI_CS_ICM_20608_G);
+
+	/* De-activate all peripherals,
+	 * required for some peripheral
+	 * state machines
+	 */
+	px4_arch_gpiowrite(GPIO_SPI_CS_MPU9250, 1);
+	px4_arch_gpiowrite(GPIO_SPI_CS_HMC5983, 1);
+	px4_arch_gpiowrite(GPIO_SPI_CS_MS5611, 1);
+	px4_arch_gpiowrite(GPIO_SPI_CS_ICM_20608_G, 1);
+
+	px4_arch_configgpio(GPIO_SPI1_SCK);
+	px4_arch_configgpio(GPIO_SPI1_MISO);
+	px4_arch_configgpio(GPIO_SPI1_MOSI);
+
+	// // XXX bring up the EXTI pins again
+	// px4_arch_configgpio(GPIO_GYRO_DRDY);
+	// px4_arch_configgpio(GPIO_MAG_DRDY);
+	// px4_arch_configgpio(GPIO_ACCEL_DRDY);
+	// px4_arch_configgpio(GPIO_EXTI_MPU_DRDY);
+
+#endif
+
+}
diff --git a/src/drivers/boards/px4fmu-v4/px4fmu_usb.c b/src/drivers/boards/px4fmu-v4/px4fmu_usb.c
index d42c35301f..47359ca688 100644
--- a/src/drivers/boards/px4fmu-v4/px4fmu_usb.c
+++ b/src/drivers/boards/px4fmu-v4/px4fmu_usb.c
@@ -82,10 +82,10 @@ __EXPORT void stm32_usbinitialize(void)
 	/* Configure the OTG FS VBUS sensing GPIO, Power On, and Overcurrent GPIOs */
 
 #ifdef CONFIG_STM32_OTGFS
-	stm32_configgpio(GPIO_OTGFS_VBUS);
+	px4_arch_configgpio(GPIO_OTGFS_VBUS);
 	/* XXX We only support device mode
-	stm32_configgpio(GPIO_OTGFS_PWRON);
-	stm32_configgpio(GPIO_OTGFS_OVER);
+	px4_arch_configgpio(GPIO_OTGFS_PWRON);
+	px4_arch_configgpio(GPIO_OTGFS_OVER);
 	*/
 #endif
 }
diff --git a/src/drivers/boards/px4io-v1/board_config.h b/src/drivers/boards/px4io-v1/board_config.h
index 6475d717d8..862112ccac 100644
--- a/src/drivers/boards/px4io-v1/board_config.h
+++ b/src/drivers/boards/px4io-v1/board_config.h
@@ -84,11 +84,11 @@
 #define GPIO_RELAY2_EN (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTA|GPIO_PIN11)
 
 #define GPIO_SPEKTRUM_PWR_EN GPIO_RELAY1_EN
-#define POWER_SPEKTRUM(_s)		stm32_gpiowrite(GPIO_RELAY1_EN, (_s))
+#define POWER_SPEKTRUM(_s)		px4_arch_gpiowrite(GPIO_RELAY1_EN, (_s))
 
-#define SPEKTRUM_RX_HIGH(_s)	stm32_gpiowrite(GPIO_USART1_RX_SPEKTRUM, (_s))
-#define SPEKTRUM_RX_AS_UART()		stm32_configgpio(GPIO_USART1_RX)
-#define SPEKTRUM_RX_AS_GPIO()		stm32_configgpio(GPIO_USART1_RX_SPEKTRUM)
+#define SPEKTRUM_RX_HIGH(_s)	px4_arch_gpiowrite(GPIO_USART1_RX_SPEKTRUM, (_s))
+#define SPEKTRUM_RX_AS_UART()		px4_arch_configgpio(GPIO_USART1_RX)
+#define SPEKTRUM_RX_AS_GPIO()		px4_arch_configgpio(GPIO_USART1_RX_SPEKTRUM)
 
 /* Analog inputs ********************************************************************/
 
diff --git a/src/drivers/boards/px4io-v1/px4io_init.c b/src/drivers/boards/px4io-v1/px4io_init.c
index 019f011fa7..1de9875df5 100644
--- a/src/drivers/boards/px4io-v1/px4io_init.c
+++ b/src/drivers/boards/px4io-v1/px4io_init.c
@@ -81,26 +81,26 @@
 __EXPORT void stm32_boardinitialize(void)
 {
 	/* configure GPIOs */
-	stm32_configgpio(GPIO_ACC1_PWR_EN);
-	stm32_configgpio(GPIO_ACC2_PWR_EN);
-	stm32_configgpio(GPIO_SERVO_PWR_EN);
-	stm32_configgpio(GPIO_RELAY1_EN);
-	stm32_configgpio(GPIO_RELAY2_EN);
+	px4_arch_configgpio(GPIO_ACC1_PWR_EN);
+	px4_arch_configgpio(GPIO_ACC2_PWR_EN);
+	px4_arch_configgpio(GPIO_SERVO_PWR_EN);
+	px4_arch_configgpio(GPIO_RELAY1_EN);
+	px4_arch_configgpio(GPIO_RELAY2_EN);
 
 	/* turn off - all leds are active low */
-	stm32_gpiowrite(GPIO_LED1, true);
-	stm32_gpiowrite(GPIO_LED2, true);
-	stm32_gpiowrite(GPIO_LED3, true);
+	px4_arch_gpiowrite(GPIO_LED1, true);
+	px4_arch_gpiowrite(GPIO_LED2, true);
+	px4_arch_gpiowrite(GPIO_LED3, true);
 
 	/* LED config */
-	stm32_configgpio(GPIO_LED1);
-	stm32_configgpio(GPIO_LED2);
-	stm32_configgpio(GPIO_LED3);
+	px4_arch_configgpio(GPIO_LED1);
+	px4_arch_configgpio(GPIO_LED2);
+	px4_arch_configgpio(GPIO_LED3);
 
-	stm32_configgpio(GPIO_ACC_OC_DETECT);
-	stm32_configgpio(GPIO_SERVO_OC_DETECT);
-	stm32_configgpio(GPIO_BTN_SAFETY);
+	px4_arch_configgpio(GPIO_ACC_OC_DETECT);
+	px4_arch_configgpio(GPIO_SERVO_OC_DETECT);
+	px4_arch_configgpio(GPIO_BTN_SAFETY);
 
-	stm32_configgpio(GPIO_ADC_VBATT);
-	stm32_configgpio(GPIO_ADC_IN5);
+	px4_arch_configgpio(GPIO_ADC_VBATT);
+	px4_arch_configgpio(GPIO_ADC_IN5);
 }
diff --git a/src/drivers/boards/px4io-v2/board_config.h b/src/drivers/boards/px4io-v2/board_config.h
index c54cf7b6f6..5269c825d2 100644
--- a/src/drivers/boards/px4io-v2/board_config.h
+++ b/src/drivers/boards/px4io-v2/board_config.h
@@ -88,11 +88,11 @@
 /* Power switch controls ******************************************************/
 
 #define GPIO_SPEKTRUM_PWR_EN (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_SET|GPIO_PORTC|GPIO_PIN13)
-#define POWER_SPEKTRUM(_s)		stm32_gpiowrite(GPIO_SPEKTRUM_PWR_EN, (_s))
+#define POWER_SPEKTRUM(_s)		px4_arch_gpiowrite(GPIO_SPEKTRUM_PWR_EN, (_s))
 
-#define SPEKTRUM_RX_HIGH(_s)	stm32_gpiowrite(GPIO_USART1_RX_SPEKTRUM, (_s))
-#define SPEKTRUM_RX_AS_UART()		stm32_configgpio(GPIO_USART1_RX)
-#define SPEKTRUM_RX_AS_GPIO()		stm32_configgpio(GPIO_USART1_RX_SPEKTRUM)
+#define SPEKTRUM_RX_HIGH(_s)	px4_arch_gpiowrite(GPIO_USART1_RX_SPEKTRUM, (_s))
+#define SPEKTRUM_RX_AS_UART()		px4_arch_configgpio(GPIO_USART1_RX)
+#define SPEKTRUM_RX_AS_GPIO()		px4_arch_configgpio(GPIO_USART1_RX_SPEKTRUM)
 
 #define GPIO_SERVO_FAULT_DETECT (GPIO_INPUT|GPIO_CNF_INPULLUP|GPIO_MODE_INPUT|GPIO_PORTA|GPIO_PIN15)
 
diff --git a/src/drivers/boards/px4io-v2/px4io_init.c b/src/drivers/boards/px4io-v2/px4io_init.c
index 0856e0b7f1..103105beb4 100644
--- a/src/drivers/boards/px4io-v2/px4io_init.c
+++ b/src/drivers/boards/px4io-v2/px4io_init.c
@@ -105,55 +105,55 @@ __EXPORT void stm32_boardinitialize(void)
 	/* configure GPIOs */
 
 	/* LEDS - default to off */
-	stm32_configgpio(GPIO_LED1);
-	stm32_configgpio(GPIO_LED2);
-	stm32_configgpio(GPIO_LED3);
-	stm32_configgpio(GPIO_LED4);
+	px4_arch_configgpio(GPIO_LED1);
+	px4_arch_configgpio(GPIO_LED2);
+	px4_arch_configgpio(GPIO_LED3);
+	px4_arch_configgpio(GPIO_LED4);
 
-	stm32_configgpio(GPIO_BTN_SAFETY);
+	px4_arch_configgpio(GPIO_BTN_SAFETY);
 
 	/* spektrum power enable is active high - enable it by default */
-	stm32_configgpio(GPIO_SPEKTRUM_PWR_EN);
+	px4_arch_configgpio(GPIO_SPEKTRUM_PWR_EN);
 
-	stm32_configgpio(GPIO_SERVO_FAULT_DETECT);
+	px4_arch_configgpio(GPIO_SERVO_FAULT_DETECT);
 
 	/* RSSI inputs */
-	stm32_configgpio(GPIO_TIM_RSSI); /* xxx alternate function */
-	stm32_configgpio(GPIO_ADC_RSSI);
+	px4_arch_configgpio(GPIO_TIM_RSSI); /* xxx alternate function */
+	px4_arch_configgpio(GPIO_ADC_RSSI);
 
 	/* servo rail voltage */
-	stm32_configgpio(GPIO_ADC_VSERVO);
+	px4_arch_configgpio(GPIO_ADC_VSERVO);
 
-	stm32_configgpio(GPIO_SBUS_INPUT); /* xxx alternate function */
-	stm32_configgpio(GPIO_SBUS_OUTPUT);
+	px4_arch_configgpio(GPIO_SBUS_INPUT); /* xxx alternate function */
+	px4_arch_configgpio(GPIO_SBUS_OUTPUT);
 
 	/* sbus output enable is active low - disable it by default */
-	stm32_gpiowrite(GPIO_SBUS_OENABLE, true);
-	stm32_configgpio(GPIO_SBUS_OENABLE);
+	px4_arch_gpiowrite(GPIO_SBUS_OENABLE, true);
+	px4_arch_configgpio(GPIO_SBUS_OENABLE);
 
-	stm32_configgpio(GPIO_PPM); /* xxx alternate function */
+	px4_arch_configgpio(GPIO_PPM); /* xxx alternate function */
 
-	stm32_gpiowrite(GPIO_PWM1, true);
-	stm32_configgpio(GPIO_PWM1);
+	px4_arch_gpiowrite(GPIO_PWM1, true);
+	px4_arch_configgpio(GPIO_PWM1);
 
-	stm32_gpiowrite(GPIO_PWM2, true);
-	stm32_configgpio(GPIO_PWM2);
+	px4_arch_gpiowrite(GPIO_PWM2, true);
+	px4_arch_configgpio(GPIO_PWM2);
 
-	stm32_gpiowrite(GPIO_PWM3, true);
-	stm32_configgpio(GPIO_PWM3);
+	px4_arch_gpiowrite(GPIO_PWM3, true);
+	px4_arch_configgpio(GPIO_PWM3);
 
-	stm32_gpiowrite(GPIO_PWM4, true);
-	stm32_configgpio(GPIO_PWM4);
+	px4_arch_gpiowrite(GPIO_PWM4, true);
+	px4_arch_configgpio(GPIO_PWM4);
 
-	stm32_gpiowrite(GPIO_PWM5, true);
-	stm32_configgpio(GPIO_PWM5);
+	px4_arch_gpiowrite(GPIO_PWM5, true);
+	px4_arch_configgpio(GPIO_PWM5);
 
-	stm32_gpiowrite(GPIO_PWM6, true);
-	stm32_configgpio(GPIO_PWM6);
+	px4_arch_gpiowrite(GPIO_PWM6, true);
+	px4_arch_configgpio(GPIO_PWM6);
 
-	stm32_gpiowrite(GPIO_PWM7, true);
-	stm32_configgpio(GPIO_PWM7);
+	px4_arch_gpiowrite(GPIO_PWM7, true);
+	px4_arch_configgpio(GPIO_PWM7);
 
-	stm32_gpiowrite(GPIO_PWM8, true);
-	stm32_configgpio(GPIO_PWM8);
+	px4_arch_gpiowrite(GPIO_PWM8, true);
+	px4_arch_configgpio(GPIO_PWM8);
 }
diff --git a/src/drivers/camera_trigger/camera_trigger.cpp b/src/drivers/camera_trigger/camera_trigger.cpp
index 5c144b45c8..fff4acd3da 100644
--- a/src/drivers/camera_trigger/camera_trigger.cpp
+++ b/src/drivers/camera_trigger/camera_trigger.cpp
@@ -283,8 +283,8 @@ CameraTrigger::start()
 {
 
 	for (unsigned i = 0; i < sizeof(_pins) / sizeof(_pins[0]); i++) {
-		stm32_configgpio(_gpios[_pins[i]]);
-		stm32_gpiowrite(_gpios[_pins[i]], !_polarity);
+		px4_arch_configgpio(_gpios[_pins[i]]);
+		px4_arch_gpiowrite(_gpios[_pins[i]], !_polarity);
 	}
 
 	// enable immediate if configured that way
@@ -448,7 +448,7 @@ CameraTrigger::trigger(CameraTrigger *trig, bool trigger)
 	for (unsigned i = 0; i < sizeof(trig->_pins) / sizeof(trig->_pins[0]); i++) {
 		if (trig->_pins[i] >= 0) {
 			// ACTIVE_LOW == 1
-			stm32_gpiowrite(trig->_gpios[trig->_pins[i]], trigger);
+			px4_arch_gpiowrite(trig->_gpios[trig->_pins[i]], trigger);
 		}
 	}
 }
diff --git a/src/drivers/device/cdev.cpp b/src/drivers/device/cdev.cpp
index c43ab1353e..3b9d56e253 100644
--- a/src/drivers/device/cdev.cpp
+++ b/src/drivers/device/cdev.cpp
@@ -339,14 +339,14 @@ void
 CDev::poll_notify(pollevent_t events)
 {
 	/* lock against poll() as well as other wakeups */
-	irqstate_t state = irqsave();
+	irqstate_t state = px4_enter_critical_section();
 
 	for (unsigned i = 0; i < _max_pollwaiters; i++)
 		if (nullptr != _pollset[i]) {
 			poll_notify_one(_pollset[i], events);
 		}
 
-	irqrestore(state);
+	px4_leave_critical_section(state);
 }
 
 void
diff --git a/src/drivers/device/i2c_nuttx.cpp b/src/drivers/device/i2c_nuttx.cpp
index 5efd2eac23..d7bbee0280 100644
--- a/src/drivers/device/i2c_nuttx.cpp
+++ b/src/drivers/device/i2c_nuttx.cpp
@@ -75,7 +75,7 @@ I2C::I2C(const char *name,
 I2C::~I2C()
 {
 	if (_dev) {
-		up_i2cuninitialize(_dev);
+		px4_i2cbus_uninitialize(_dev);
 		_dev = nullptr;
 	}
 }
@@ -105,7 +105,7 @@ I2C::init()
 	unsigned bus_index;
 
 	// attach to the i2c bus
-	_dev = up_i2cinitialize(_bus);
+	_dev = px4_i2cbus_initialize(_bus);
 
 	if (_dev == nullptr) {
 		DEVICE_DEBUG("failed to init I2C");
@@ -120,7 +120,7 @@ I2C::init()
 	// abort if the max frequency we allow (the frequency we ask)
 	// is smaller than the bus frequency
 	if (_bus_clocks[bus_index] > _frequency) {
-		(void)up_i2cuninitialize(_dev);
+		(void)px4_i2cbus_uninitialize(_dev);
 		_dev = nullptr;
 		DEVICE_LOG("FAIL: too slow for bus #%u: %u KHz, device max: %u KHz)",
 			   _bus, _bus_clocks[bus_index] / 1000, _frequency / 1000);
@@ -168,7 +168,7 @@ I2C::init()
 out:
 
 	if ((ret != OK) && (_dev != nullptr)) {
-		up_i2cuninitialize(_dev);
+		px4_i2cbus_uninitialize(_dev);
 		_dev = nullptr;
 	}
 
diff --git a/src/drivers/device/spi.cpp b/src/drivers/device/spi.cpp
index ce81878102..a25fca10f8 100644
--- a/src/drivers/device/spi.cpp
+++ b/src/drivers/device/spi.cpp
@@ -95,7 +95,7 @@ SPI::init()
 
 	/* attach to the spi bus */
 	if (_dev == nullptr) {
-		_dev = up_spiinitialize(_bus);
+		_dev = px4_spibus_initialize(_bus);
 	}
 
 	if (_dev == nullptr) {
@@ -152,9 +152,9 @@ SPI::transfer(uint8_t *send, uint8_t *recv, unsigned len)
 	switch (mode) {
 	default:
 	case LOCK_PREEMPTION: {
-			irqstate_t state = irqsave();
+			irqstate_t state = px4_enter_critical_section();
 			result = _transfer(send, recv, len);
-			irqrestore(state);
+			px4_leave_critical_section(state);
 		}
 		break;
 
diff --git a/src/drivers/gps/gps.cpp b/src/drivers/gps/gps.cpp
index b981e80ec3..926611c7b6 100644
--- a/src/drivers/gps/gps.cpp
+++ b/src/drivers/gps/gps.cpp
@@ -910,10 +910,10 @@ GPS::cmd_reset()
 {
 #ifdef GPIO_GPS_NRESET
 	PX4_WARN("Toggling GPS reset pin");
-	stm32_configgpio(GPIO_GPS_NRESET);
-	stm32_gpiowrite(GPIO_GPS_NRESET, 0);
+	px4_arch_configgpio(GPIO_GPS_NRESET);
+	px4_arch_gpiowrite(GPIO_GPS_NRESET, 0);
 	usleep(100);
-	stm32_gpiowrite(GPIO_GPS_NRESET, 1);
+	px4_arch_gpiowrite(GPIO_GPS_NRESET, 1);
 	PX4_WARN("Toggled GPS reset pin");
 #endif
 }
diff --git a/src/drivers/hc_sr04/hc_sr04.cpp b/src/drivers/hc_sr04/hc_sr04.cpp
index efe239e53f..e5571c5fa2 100644
--- a/src/drivers/hc_sr04/hc_sr04.cpp
+++ b/src/drivers/hc_sr04/hc_sr04.cpp
@@ -297,9 +297,9 @@ HC_SR04::init()
 
 	/* init echo port : */
 	for (unsigned i = 0; i <= _sonars; i++) {
-		stm32_configgpio(_gpio_tab[i].trig_port);
-		stm32_gpiowrite(_gpio_tab[i].trig_port, false);
-		stm32_configgpio(_gpio_tab[i].echo_port);
+		px4_arch_configgpio(_gpio_tab[i].trig_port);
+		px4_arch_gpiowrite(_gpio_tab[i].trig_port, false);
+		px4_arch_configgpio(_gpio_tab[i].echo_port);
 		_latest_sonar_measurements.push_back(0);
 	}
 
@@ -441,14 +441,14 @@ HC_SR04::ioctl(struct file *filp, int cmd, unsigned long arg)
 				return -EINVAL;
 			}
 
-			irqstate_t flags = irqsave();
+			irqstate_t flags = px4_enter_critical_section();
 
 			if (!_reports->resize(arg)) {
-				irqrestore(flags);
+				px4_leave_critical_section(flags);
 				return -ENOMEM;
 			}
 
-			irqrestore(flags);
+			px4_leave_critical_section(flags);
 
 			return OK;
 		}
@@ -546,9 +546,9 @@ HC_SR04::measure()
 	/*
 	 * Send a plus begin a measurement.
 	 */
-	stm32_gpiowrite(_gpio_tab[_cycle_counter].trig_port, true);
+	px4_arch_gpiowrite(_gpio_tab[_cycle_counter].trig_port, true);
 	usleep(10);  // 10us
-	stm32_gpiowrite(_gpio_tab[_cycle_counter].trig_port, false);
+	px4_arch_gpiowrite(_gpio_tab[_cycle_counter].trig_port, false);
 
 	stm32_gpiosetevent(_gpio_tab[_cycle_counter].echo_port, true, true, false, sonar_isr);
 	_status = 0;
diff --git a/src/drivers/hmc5883/hmc5883.cpp b/src/drivers/hmc5883/hmc5883.cpp
index 73d81020f7..c16cd69dfd 100644
--- a/src/drivers/hmc5883/hmc5883.cpp
+++ b/src/drivers/hmc5883/hmc5883.cpp
@@ -698,14 +698,14 @@ HMC5883::ioctl(struct file *filp, int cmd, unsigned long arg)
 				return -EINVAL;
 			}
 
-			irqstate_t flags = irqsave();
+			irqstate_t flags = px4_enter_critical_section();
 
 			if (!_reports->resize(arg)) {
-				irqrestore(flags);
+				px4_leave_critical_section(flags);
 				return -ENOMEM;
 			}
 
-			irqrestore(flags);
+			px4_leave_critical_section(flags);
 
 			return OK;
 		}
diff --git a/src/drivers/l3gd20/l3gd20.cpp b/src/drivers/l3gd20/l3gd20.cpp
index 61345628b1..fccbbfbb21 100644
--- a/src/drivers/l3gd20/l3gd20.cpp
+++ b/src/drivers/l3gd20/l3gd20.cpp
@@ -666,14 +666,14 @@ L3GD20::ioctl(struct file *filp, int cmd, unsigned long arg)
 				return -EINVAL;
 			}
 
-			irqstate_t flags = irqsave();
+			irqstate_t flags = px4_enter_critical_section();
 
 			if (!_reports->resize(arg)) {
-				irqrestore(flags);
+				px4_leave_critical_section(flags);
 				return -ENOMEM;
 			}
 
-			irqrestore(flags);
+			px4_leave_critical_section(flags);
 
 			return OK;
 		}
diff --git a/src/drivers/lis3mdl/lis3mdl.cpp b/src/drivers/lis3mdl/lis3mdl.cpp
index 5a994df648..a3dfe3d869 100644
--- a/src/drivers/lis3mdl/lis3mdl.cpp
+++ b/src/drivers/lis3mdl/lis3mdl.cpp
@@ -706,14 +706,14 @@ LIS3MDL::ioctl(struct file *filp, int cmd, unsigned long arg)
 				return -EINVAL;
 			}
 
-			irqstate_t flags = irqsave();
+			irqstate_t flags = px4_enter_critical_section();
 
 			if (!_reports->resize(arg)) {
-				irqrestore(flags);
+				px4_leave_critical_section(flags);
 				return -ENOMEM;
 			}
 
-			irqrestore(flags);
+			px4_leave_critical_section(flags);
 
 			return OK;
 		}
diff --git a/src/drivers/ll40ls/LidarLiteI2C.cpp b/src/drivers/ll40ls/LidarLiteI2C.cpp
index f85b672c5d..64bdf9a11c 100644
--- a/src/drivers/ll40ls/LidarLiteI2C.cpp
+++ b/src/drivers/ll40ls/LidarLiteI2C.cpp
@@ -216,14 +216,14 @@ int LidarLiteI2C::ioctl(struct file *filp, int cmd, unsigned long arg)
 				return -EINVAL;
 			}
 
-			irqstate_t flags = irqsave();
+			irqstate_t flags = px4_enter_critical_section();
 
 			if (!_reports->resize(arg)) {
-				irqrestore(flags);
+				px4_leave_critical_section(flags);
 				return -ENOMEM;
 			}
 
-			irqrestore(flags);
+			px4_leave_critical_section(flags);
 
 			return OK;
 		}
diff --git a/src/drivers/lsm303d/lsm303d.cpp b/src/drivers/lsm303d/lsm303d.cpp
index 71796cfe8c..e2f02e397c 100644
--- a/src/drivers/lsm303d/lsm303d.cpp
+++ b/src/drivers/lsm303d/lsm303d.cpp
@@ -928,14 +928,14 @@ LSM303D::ioctl(struct file *filp, int cmd, unsigned long arg)
 				return -EINVAL;
 			}
 
-			irqstate_t flags = irqsave();
+			irqstate_t flags = px4_enter_critical_section();
 
 			if (!_accel_reports->resize(arg)) {
-				irqrestore(flags);
+				px4_leave_critical_section(flags);
 				return -ENOMEM;
 			}
 
-			irqrestore(flags);
+			px4_leave_critical_section(flags);
 
 			return OK;
 		}
@@ -1063,14 +1063,14 @@ LSM303D::mag_ioctl(struct file *filp, int cmd, unsigned long arg)
 				return -EINVAL;
 			}
 
-			irqstate_t flags = irqsave();
+			irqstate_t flags = px4_enter_critical_section();
 
 			if (!_mag_reports->resize(arg)) {
-				irqrestore(flags);
+				px4_leave_critical_section(flags);
 				return -ENOMEM;
 			}
 
-			irqrestore(flags);
+			px4_leave_critical_section(flags);
 
 			return OK;
 		}
diff --git a/src/drivers/mb12xx/mb12xx.cpp b/src/drivers/mb12xx/mb12xx.cpp
index 56886a99eb..6c6222c1f9 100644
--- a/src/drivers/mb12xx/mb12xx.cpp
+++ b/src/drivers/mb12xx/mb12xx.cpp
@@ -431,14 +431,14 @@ MB12XX::ioctl(struct file *filp, int cmd, unsigned long arg)
 				return -EINVAL;
 			}
 
-			irqstate_t flags = irqsave();
+			irqstate_t flags = px4_enter_critical_section();
 
 			if (!_reports->resize(arg)) {
-				irqrestore(flags);
+				px4_leave_critical_section(flags);
 				return -ENOMEM;
 			}
 
-			irqrestore(flags);
+			px4_leave_critical_section(flags);
 
 			return OK;
 		}
diff --git a/src/drivers/mpu6000/mpu6000.cpp b/src/drivers/mpu6000/mpu6000.cpp
index 4bf6384b3f..7a479dc30f 100644
--- a/src/drivers/mpu6000/mpu6000.cpp
+++ b/src/drivers/mpu6000/mpu6000.cpp
@@ -737,7 +737,7 @@ int MPU6000::reset()
 
 	while (--tries != 0) {
 		irqstate_t state;
-		state = irqsave();
+		state = px4_enter_critical_section();
 
 		write_reg(MPUREG_PWR_MGMT_1, BIT_H_RESET);
 		up_udelay(10000);
@@ -750,7 +750,7 @@ int MPU6000::reset()
 
 		// Disable I2C bus (recommended on datasheet)
 		write_checked_reg(MPUREG_USER_CTRL, BIT_I2C_IF_DIS);
-		irqrestore(state);
+		px4_leave_critical_section(state);
 
 		if (read_reg(MPUREG_PWR_MGMT_1) == MPU_CLK_SEL_PLLGYROZ) {
 			break;
@@ -1364,14 +1364,14 @@ MPU6000::ioctl(struct file *filp, int cmd, unsigned long arg)
 				return -EINVAL;
 			}
 
-			irqstate_t flags = irqsave();
+			irqstate_t flags = px4_enter_critical_section();
 
 			if (!_accel_reports->resize(arg)) {
-				irqrestore(flags);
+				px4_leave_critical_section(flags);
 				return -ENOMEM;
 			}
 
-			irqrestore(flags);
+			px4_leave_critical_section(flags);
 
 			return OK;
 		}
@@ -1449,14 +1449,14 @@ MPU6000::gyro_ioctl(struct file *filp, int cmd, unsigned long arg)
 				return -EINVAL;
 			}
 
-			irqstate_t flags = irqsave();
+			irqstate_t flags = px4_enter_critical_section();
 
 			if (!_gyro_reports->resize(arg)) {
-				irqrestore(flags);
+				px4_leave_critical_section(flags);
 				return -ENOMEM;
 			}
 
-			irqrestore(flags);
+			px4_leave_critical_section(flags);
 
 			return OK;
 		}
diff --git a/src/drivers/mpu6500/mpu6500.cpp b/src/drivers/mpu6500/mpu6500.cpp
index c3457c5b8b..96256c2909 100644
--- a/src/drivers/mpu6500/mpu6500.cpp
+++ b/src/drivers/mpu6500/mpu6500.cpp
@@ -703,7 +703,7 @@ int MPU6500::reset()
 
 	while (--tries != 0) {
 		irqstate_t state;
-		state = irqsave();
+		state = px4_enter_critical_section();
 
 		write_reg(MPUREG_PWR_MGMT_1, BIT_H_RESET);
 		up_udelay(10000);
@@ -716,7 +716,7 @@ int MPU6500::reset()
 
 		// Disable I2C bus (recommended on datasheet)
 		write_checked_reg(MPUREG_USER_CTRL, BIT_I2C_IF_DIS);
-		irqrestore(state);
+		px4_leave_critical_section(state);
 
 		if (read_reg(MPUREG_PWR_MGMT_1) == MPU_CLK_SEL_PLLGYROZ) {
 			break;
@@ -1292,14 +1292,14 @@ MPU6500::ioctl(struct file *filp, int cmd, unsigned long arg)
 				return -EINVAL;
 			}
 
-			irqstate_t flags = irqsave();
+			irqstate_t flags = px4_enter_critical_section();
 
 			if (!_accel_reports->resize(arg)) {
-				irqrestore(flags);
+				px4_leave_critical_section(flags);
 				return -ENOMEM;
 			}
 
-			irqrestore(flags);
+			px4_leave_critical_section(flags);
 
 			return OK;
 		}
@@ -1377,14 +1377,14 @@ MPU6500::gyro_ioctl(struct file *filp, int cmd, unsigned long arg)
 				return -EINVAL;
 			}
 
-			irqstate_t flags = irqsave();
+			irqstate_t flags = px4_enter_critical_section();
 
 			if (!_gyro_reports->resize(arg)) {
-				irqrestore(flags);
+				px4_leave_critical_section(flags);
 				return -ENOMEM;
 			}
 
-			irqrestore(flags);
+			px4_leave_critical_section(flags);
 
 			return OK;
 		}
diff --git a/src/drivers/mpu9250/mag.cpp b/src/drivers/mpu9250/mag.cpp
index 6180cb13c0..2609965042 100644
--- a/src/drivers/mpu9250/mag.cpp
+++ b/src/drivers/mpu9250/mag.cpp
@@ -412,14 +412,14 @@ MPU9250_mag::ioctl(struct file *filp, int cmd, unsigned long arg)
 				return -EINVAL;
 			}
 
-			irqstate_t flags = irqsave();
+			irqstate_t flags = px4_enter_critical_section();
 
 			if (!_mag_reports->resize(arg)) {
-				irqrestore(flags);
+				px4_leave_critical_section(flags);
 				return -ENOMEM;
 			}
 
-			irqrestore(flags);
+			px4_leave_critical_section(flags);
 
 			return OK;
 		}
diff --git a/src/drivers/mpu9250/mpu9250.cpp b/src/drivers/mpu9250/mpu9250.cpp
index 761c8f1008..e9bc3e2462 100644
--- a/src/drivers/mpu9250/mpu9250.cpp
+++ b/src/drivers/mpu9250/mpu9250.cpp
@@ -876,14 +876,14 @@ MPU9250::ioctl(struct file *filp, int cmd, unsigned long arg)
 				return -EINVAL;
 			}
 
-			irqstate_t flags = irqsave();
+			irqstate_t flags = px4_enter_critical_section();
 
 			if (!_accel_reports->resize(arg)) {
-				irqrestore(flags);
+				px4_leave_critical_section(flags);
 				return -ENOMEM;
 			}
 
-			irqrestore(flags);
+			px4_leave_critical_section(flags);
 
 			return OK;
 		}
@@ -972,14 +972,14 @@ MPU9250::gyro_ioctl(struct file *filp, int cmd, unsigned long arg)
 				return -EINVAL;
 			}
 
-			irqstate_t flags = irqsave();
+			irqstate_t flags = px4_enter_critical_section();
 
 			if (!_gyro_reports->resize(arg)) {
-				irqrestore(flags);
+				px4_leave_critical_section(flags);
 				return -ENOMEM;
 			}
 
-			irqrestore(flags);
+			px4_leave_critical_section(flags);
 
 			return OK;
 		}
diff --git a/src/drivers/ms5611/ms5611_nuttx.cpp b/src/drivers/ms5611/ms5611_nuttx.cpp
index 07443ecd8b..5b50b1ff91 100644
--- a/src/drivers/ms5611/ms5611_nuttx.cpp
+++ b/src/drivers/ms5611/ms5611_nuttx.cpp
@@ -486,14 +486,14 @@ MS5611::ioctl(struct file *filp, int cmd, unsigned long arg)
 				return -EINVAL;
 			}
 
-			irqstate_t flags = irqsave();
+			irqstate_t flags = px4_enter_critical_section();
 
 			if (!_reports->resize(arg)) {
-				irqrestore(flags);
+				px4_leave_critical_section(flags);
 				return -ENOMEM;
 			}
 
-			irqrestore(flags);
+			px4_leave_critical_section(flags);
 			return OK;
 		}
 
diff --git a/src/drivers/ms5611/ms5611_spi.cpp b/src/drivers/ms5611/ms5611_spi.cpp
index 5fe0fc501d..c10b71be3a 100644
--- a/src/drivers/ms5611/ms5611_spi.cpp
+++ b/src/drivers/ms5611/ms5611_spi.cpp
@@ -152,11 +152,6 @@ MS5611_SPI::init()
 		goto out;
 	}
 
-	/* sharing a bus with NuttX drivers */
-#if defined (CONFIG_ARCH_BOARD_PX4FMU_V4)
-	//set_lockmode(SPI::LOCK_THREADS);
-#endif
-
 	/* send reset command */
 	ret = _reset();
 
diff --git a/src/drivers/pwm_input/pwm_input.cpp b/src/drivers/pwm_input/pwm_input.cpp
index 0ec2af74f7..af2ed43a36 100644
--- a/src/drivers/pwm_input/pwm_input.cpp
+++ b/src/drivers/pwm_input/pwm_input.cpp
@@ -321,14 +321,14 @@ void PWMIN::_timer_init(void)
 {
 	/* run with interrupts disabled in case the timer is already
 	 * setup. We don't want it firing while we are doing the setup */
-	irqstate_t flags = irqsave();
+	irqstate_t flags = px4_enter_critical_section();
 
 	/* configure input pin */
-	stm32_configgpio(GPIO_PWM_IN);
+	px4_arch_configgpio(GPIO_PWM_IN);
 
 	// XXX refactor this out of this driver
 	/* configure reset pin */
-	stm32_configgpio(GPIO_VDD_RANGEFINDER_EN);
+	px4_arch_configgpio(GPIO_VDD_RANGEFINDER_EN);
 
 	/* claim our interrupt vector */
 	irq_attach(PWMIN_TIMER_VECTOR, pwmin_tim_isr);
@@ -373,7 +373,7 @@ void PWMIN::_timer_init(void)
 	/* enable interrupts */
 	up_enable_irq(PWMIN_TIMER_VECTOR);
 
-	irqrestore(flags);
+	px4_leave_critical_section(flags);
 
 	_timer_started = true;
 }
@@ -392,14 +392,14 @@ PWMIN::_freeze_test()
 void
 PWMIN::_turn_on()
 {
-	stm32_gpiowrite(GPIO_VDD_RANGEFINDER_EN, 1);
+	px4_arch_gpiowrite(GPIO_VDD_RANGEFINDER_EN, 1);
 }
 
 // XXX refactor this out of this driver
 void
 PWMIN::_turn_off()
 {
-	stm32_gpiowrite(GPIO_VDD_RANGEFINDER_EN, 0);
+	px4_arch_gpiowrite(GPIO_VDD_RANGEFINDER_EN, 0);
 }
 
 // XXX refactor this out of this driver
@@ -444,14 +444,14 @@ PWMIN::ioctl(struct file *filp, int cmd, unsigned long arg)
 				return -EINVAL;
 			}
 
-			irqstate_t flags = irqsave();
+			irqstate_t flags = px4_enter_critical_section();
 
 			if (!_reports->resize(arg)) {
-				irqrestore(flags);
+				px4_leave_critical_section(flags);
 				return -ENOMEM;
 			}
 
-			irqrestore(flags);
+			px4_leave_critical_section(flags);
 
 			return OK;
 		}
diff --git a/src/drivers/px4flow/px4flow.cpp b/src/drivers/px4flow/px4flow.cpp
index 095d70e38b..03015e315b 100644
--- a/src/drivers/px4flow/px4flow.cpp
+++ b/src/drivers/px4flow/px4flow.cpp
@@ -364,14 +364,14 @@ PX4FLOW::ioctl(struct file *filp, int cmd, unsigned long arg)
 				return -EINVAL;
 			}
 
-			irqstate_t flags = irqsave();
+			irqstate_t flags = px4_enter_critical_section();
 
 			if (!_reports->resize(arg)) {
-				irqrestore(flags);
+				px4_leave_critical_section(flags);
 				return -ENOMEM;
 			}
 
-			irqrestore(flags);
+			px4_leave_critical_section(flags);
 
 			return OK;
 		}
diff --git a/src/drivers/px4fmu/fmu.cpp b/src/drivers/px4fmu/fmu.cpp
index bff4d5e9c4..48fc9950d1 100644
--- a/src/drivers/px4fmu/fmu.cpp
+++ b/src/drivers/px4fmu/fmu.cpp
@@ -63,6 +63,7 @@
 
 #include <board_config.h>
 
+#include <systemlib/px4_macros.h>
 #include <systemlib/systemlib.h>
 #include <systemlib/err.h>
 #include <systemlib/mixer/mixer.h>
@@ -94,7 +95,7 @@
 
 #define SCHEDULE_INTERVAL	2000	/**< The schedule interval in usec (500 Hz) */
 #define NAN_VALUE	(0.0f/0.0f)		/**< NaN value for throttle lock mode */
-#define BUTTON_SAFETY	stm32_gpioread(GPIO_BTN_SAFETY)
+#define BUTTON_SAFETY	px4_arch_gpioread(GPIO_BTN_SAFETY)
 #define CYCLE_COUNT 10			/* safety switch must be held for 1 second to activate */
 
 /*
@@ -106,6 +107,9 @@
 #define LED_PATTERN_FMU_ARMED 			0x5500		/**< long off, then quad blink 		*/
 #define LED_PATTERN_IO_FMU_ARMED 		0xffff		/**< constantly on			*/
 
+#if !defined(BOARD_HAS_PWM)
+#  error "board_config.h needs to define BOARD_HAS_PWM"
+#endif
 class PX4FMU : public device::CDev
 {
 public:
@@ -271,76 +275,9 @@ private:
 	void safety_check_button(void);
 };
 
-const PX4FMU::GPIOConfig PX4FMU::_gpio_tab[] = {
-#if defined(CONFIG_ARCH_BOARD_PX4FMU_V1)
-	{GPIO_GPIO0_INPUT, GPIO_GPIO0_OUTPUT, 0},
-	{GPIO_GPIO1_INPUT, GPIO_GPIO1_OUTPUT, 0},
-	{GPIO_GPIO2_INPUT, GPIO_GPIO2_OUTPUT, GPIO_USART2_CTS_1},
-	{GPIO_GPIO3_INPUT, GPIO_GPIO3_OUTPUT, GPIO_USART2_RTS_1},
-	{GPIO_GPIO4_INPUT, GPIO_GPIO4_OUTPUT, GPIO_USART2_TX_1},
-	{GPIO_GPIO5_INPUT, GPIO_GPIO5_OUTPUT, GPIO_USART2_RX_1},
-	{GPIO_GPIO6_INPUT, GPIO_GPIO6_OUTPUT, GPIO_CAN2_TX_2},
-	{GPIO_GPIO7_INPUT, GPIO_GPIO7_OUTPUT, GPIO_CAN2_RX_2},
-#endif
-#if defined(CONFIG_ARCH_BOARD_PX4FMU_V2)
-	{GPIO_GPIO0_INPUT,       GPIO_GPIO0_OUTPUT,       0},
-	{GPIO_GPIO1_INPUT,       GPIO_GPIO1_OUTPUT,       0},
-	{GPIO_GPIO2_INPUT,       GPIO_GPIO2_OUTPUT,       0},
-	{GPIO_GPIO3_INPUT,       GPIO_GPIO3_OUTPUT,       0},
-	{GPIO_GPIO4_INPUT,       GPIO_GPIO4_OUTPUT,       0},
-	{GPIO_GPIO5_INPUT,       GPIO_GPIO5_OUTPUT,       0},
+const PX4FMU::GPIOConfig PX4FMU::_gpio_tab[] =	BOARD_FMU_GPIO_TAB;
 
-	{0,                      GPIO_VDD_5V_PERIPH_EN,   0},
-	{0,                      GPIO_VDD_3V3_SENSORS_EN, 0},
-	{GPIO_VDD_BRICK_VALID,   0,                       0},
-	{GPIO_VDD_SERVO_VALID,   0,                       0},
-	{GPIO_VDD_5V_HIPOWER_OC, 0,                       0},
-	{GPIO_VDD_5V_PERIPH_OC,  0,                       0},
-#endif
-#if defined(CONFIG_ARCH_BOARD_PX4FMU_V4)
-	{GPIO_GPIO0_INPUT,       GPIO_GPIO0_OUTPUT,       0},
-	{GPIO_GPIO1_INPUT,       GPIO_GPIO1_OUTPUT,       0},
-	{GPIO_GPIO2_INPUT,       GPIO_GPIO2_OUTPUT,       0},
-	{GPIO_GPIO3_INPUT,       GPIO_GPIO3_OUTPUT,       0},
-	{GPIO_GPIO4_INPUT,       GPIO_GPIO4_OUTPUT,       0},
-	{GPIO_GPIO5_INPUT,       GPIO_GPIO5_OUTPUT,       0},
-
-	{0,                      GPIO_VDD_3V3_SENSORS_EN, 0},
-	{GPIO_VDD_BRICK_VALID,   0,                       0},
-#endif
-#if defined(CONFIG_ARCH_BOARD_AEROCORE)
-	/* AeroCore breaks out User GPIOs on J11 */
-	{GPIO_GPIO0_INPUT,       GPIO_GPIO0_OUTPUT,       0},
-	{GPIO_GPIO1_INPUT,       GPIO_GPIO1_OUTPUT,       0},
-	{GPIO_GPIO3_INPUT,       GPIO_GPIO3_OUTPUT,       0},
-	{GPIO_GPIO4_INPUT,       GPIO_GPIO4_OUTPUT,       0},
-	{GPIO_GPIO5_INPUT,       GPIO_GPIO5_OUTPUT,       0},
-	{GPIO_GPIO6_INPUT,       GPIO_GPIO6_OUTPUT,       0},
-	{GPIO_GPIO7_INPUT,       GPIO_GPIO7_OUTPUT,       0},
-	{GPIO_GPIO8_INPUT,       GPIO_GPIO8_OUTPUT,       0},
-	{GPIO_GPIO9_INPUT,       GPIO_GPIO9_OUTPUT,       0},
-	{GPIO_GPIO10_INPUT,      GPIO_GPIO10_OUTPUT,      0},
-	{GPIO_GPIO11_INPUT,      GPIO_GPIO11_OUTPUT,      0},
-#endif
-#if  defined(CONFIG_ARCH_BOARD_MINDPX_V2)
-	{GPIO_GPIO0_INPUT,       GPIO_GPIO0_OUTPUT,       0},
-	{GPIO_GPIO1_INPUT,       GPIO_GPIO1_OUTPUT,       0},
-	{GPIO_GPIO2_INPUT,       GPIO_GPIO2_OUTPUT,       0},
-	{GPIO_GPIO3_INPUT,       GPIO_GPIO3_OUTPUT,       0},
-	{GPIO_GPIO4_INPUT,       GPIO_GPIO4_OUTPUT,       0},
-	{GPIO_GPIO5_INPUT,       GPIO_GPIO5_OUTPUT,       0},
-	{GPIO_GPIO6_INPUT,       GPIO_GPIO6_OUTPUT,       0},
-	{GPIO_GPIO7_INPUT,       GPIO_GPIO7_OUTPUT,       0},
-	{GPIO_GPIO8_INPUT,       GPIO_GPIO8_OUTPUT,       0},
-	{GPIO_GPIO9_INPUT,       GPIO_GPIO9_OUTPUT,       0},
-	{GPIO_GPIO10_INPUT,      GPIO_GPIO10_OUTPUT,       0},
-	{GPIO_GPIO11_INPUT,      GPIO_GPIO11_OUTPUT,       0},
-	{GPIO_GPIO12_INPUT,      GPIO_GPIO12_OUTPUT,       0},
-	{0,       0,       0},
-#endif
-};
-
-const unsigned		PX4FMU::_ngpio = sizeof(PX4FMU::_gpio_tab) / sizeof(PX4FMU::_gpio_tab[0]);
+const unsigned		PX4FMU::_ngpio = arraySize(PX4FMU::_gpio_tab);
 pwm_limit_t		PX4FMU::_pwm_limit;
 actuator_armed_s	PX4FMU::_armed = {};
 
@@ -408,7 +345,7 @@ PX4FMU::PX4FMU() :
 
 #ifdef GPIO_SBUS_INV
 	// this board has a GPIO to control SBUS inversion
-	stm32_configgpio(GPIO_SBUS_INV);
+	px4_arch_configgpio(GPIO_SBUS_INV);
 #endif
 
 	// If there is no safety button, disable it on boot.
@@ -540,7 +477,7 @@ PX4FMU:: safety_check_button(void)
 	}
 
 	/* Turn the LED on if we have a 1 at the current bit position */
-	stm32_gpiowrite(GPIO_LED_SAFETY, !(pattern & (1 << blink_counter++)));
+	px4_arch_gpiowrite(GPIO_LED_SAFETY, !(pattern & (1 << blink_counter++)));
 
 	if (blink_counter > 15) {
 		blink_counter = 0;
@@ -608,7 +545,9 @@ PX4FMU::set_mode(Mode mode)
 
 		break;
 
-	case MODE_6PWM: // v2 PWMs as 6 PWM outs
+#if defined(BOARD_HAS_PWM) && BOARD_HAS_PWM >= 6
+
+	case MODE_6PWM:
 		DEVICE_DEBUG("MODE_6PWM");
 
 		/* default output rates */
@@ -619,8 +558,9 @@ PX4FMU::set_mode(Mode mode)
 		_pwm_initialized = false;
 
 		break;
+#endif
 
-#ifdef CONFIG_ARCH_BOARD_AEROCORE
+#if defined(BOARD_HAS_PWM) && BOARD_HAS_PWM >= 8
 
 	case MODE_8PWM: // AeroCore PWMs as 8 PWM outs
 		DEVICE_DEBUG("MODE_8PWM");
@@ -901,7 +841,7 @@ void PX4FMU::rc_io_invert(bool invert)
 
 	if (!invert) {
 		// set FMU_RC_OUTPUT high to pull RC_INPUT up
-		stm32_gpiowrite(GPIO_RC_OUT, 1);
+		px4_arch_gpiowrite(GPIO_RC_OUT, 1);
 	}
 }
 #endif
@@ -951,7 +891,7 @@ PX4FMU::cycle()
 		// assume SBUS input
 		sbus_config(_rcs_fd, false);
 		// disable CPPM input by mapping it away from the timer capture input
-		stm32_unconfiggpio(GPIO_PPM_IN);
+		px4_arch_unconfiggpio(GPIO_PPM_IN);
 #endif
 
 		_initialized = true;
@@ -1122,7 +1062,7 @@ PX4FMU::cycle()
 
 		if (_safety_disabled) {
 			/* safety switch disabled, turn LED on solid */
-			stm32_gpiowrite(GPIO_LED_SAFETY, 0);
+			px4_arch_gpiowrite(GPIO_LED_SAFETY, 0);
 			_safety_off = true;
 
 		} else {
@@ -1391,7 +1331,7 @@ PX4FMU::cycle()
 		if (_rc_scan_begin == 0) {
 			_rc_scan_begin = _cycle_timestamp;
 			// Configure timer input pin for CPPM
-			stm32_configgpio(GPIO_PPM_IN);
+			px4_arch_configgpio(GPIO_PPM_IN);
 			rc_io_invert(false);
 
 		} else if (_rc_scan_locked
@@ -1412,7 +1352,7 @@ PX4FMU::cycle()
 
 		} else {
 			// disable CPPM input by mapping it away from the timer capture input
-			stm32_unconfiggpio(GPIO_PPM_IN);
+			px4_arch_unconfiggpio(GPIO_PPM_IN);
 			// Scan the next protocol
 			set_rc_scan_state(RC_SCAN_SBUS);
 		}
@@ -1548,8 +1488,10 @@ PX4FMU::ioctl(file *filp, int cmd, unsigned long arg)
 	case MODE_4PWM:
 	case MODE_2PWM2CAP:
 	case MODE_3PWM1CAP:
+#if defined(BOARD_HAS_PWM) && BOARD_HAS_PWM >= 6
 	case MODE_6PWM:
-#ifdef CONFIG_ARCH_BOARD_AEROCORE
+#endif
+#if defined(BOARD_HAS_PWM) && BOARD_HAS_PWM >= 8
 	case MODE_8PWM:
 #endif
 		ret = pwm_ioctl(filp, cmd, arg);
@@ -1792,7 +1734,7 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg)
 			break;
 		}
 
-#ifdef CONFIG_ARCH_BOARD_AEROCORE
+#if defined(BOARD_HAS_PWM) && BOARD_HAS_PWM >= 8
 
 	case PWM_SERVO_SET(7):
 	case PWM_SERVO_SET(6):
@@ -1803,6 +1745,8 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg)
 
 #endif
 
+#if defined(BOARD_HAS_PWM) && BOARD_HAS_PWM >= 6
+
 	case PWM_SERVO_SET(5):
 	case PWM_SERVO_SET(4):
 		if (_mode < MODE_6PWM) {
@@ -1810,6 +1754,8 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg)
 			break;
 		}
 
+#endif
+
 	/* FALLTHROUGH */
 	case PWM_SERVO_SET(3):
 		if (_mode < MODE_4PWM) {
@@ -1836,7 +1782,7 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg)
 
 		break;
 
-#ifdef CONFIG_ARCH_BOARD_AEROCORE
+#if defined(BOARD_HAS_PWM) && BOARD_HAS_PWM >= 8
 
 	case PWM_SERVO_GET(7):
 	case PWM_SERVO_GET(6):
@@ -1847,6 +1793,8 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg)
 
 #endif
 
+#if defined(BOARD_HAS_PWM) && BOARD_HAS_PWM >= 6
+
 	case PWM_SERVO_GET(5):
 	case PWM_SERVO_GET(4):
 		if (_mode < MODE_6PWM) {
@@ -1854,6 +1802,8 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg)
 			break;
 		}
 
+#endif
+
 	/* FALLTHROUGH */
 	case PWM_SERVO_GET(3):
 		if (_mode < MODE_4PWM) {
@@ -1878,9 +1828,11 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg)
 	case PWM_SERVO_GET_RATEGROUP(1):
 	case PWM_SERVO_GET_RATEGROUP(2):
 	case PWM_SERVO_GET_RATEGROUP(3):
+#if defined(BOARD_HAS_PWM) && BOARD_HAS_PWM >= 6
 	case PWM_SERVO_GET_RATEGROUP(4):
 	case PWM_SERVO_GET_RATEGROUP(5):
-#ifdef CONFIG_ARCH_BOARD_AEROCORE
+#endif
+#if defined(BOARD_HAS_PWM) && BOARD_HAS_PWM >= 8
 	case PWM_SERVO_GET_RATEGROUP(6):
 	case PWM_SERVO_GET_RATEGROUP(7):
 #endif
@@ -1890,16 +1842,20 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg)
 	case PWM_SERVO_GET_COUNT:
 	case MIXERIOCGETOUTPUTCOUNT:
 		switch (_mode) {
-#ifdef CONFIG_ARCH_BOARD_AEROCORE
+
+#if defined(BOARD_HAS_PWM) && BOARD_HAS_PWM >= 8
 
 		case MODE_8PWM:
 			*(unsigned *)arg = 8;
 			break;
 #endif
 
+#if defined(BOARD_HAS_PWM) && BOARD_HAS_PWM >= 6
+
 		case MODE_6PWM:
 			*(unsigned *)arg = 6;
 			break;
+#endif
 
 		case MODE_4PWM:
 			*(unsigned *)arg = 4;
@@ -1947,14 +1903,14 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg)
 				set_mode(MODE_4PWM);
 				break;
 
-#if defined(CONFIG_ARCH_BOARD_PX4FMU_V2) ||  defined(CONFIG_ARCH_BOARD_PX4FMU_V4) \
-	|| defined(CONFIG_ARCH_BOARD_MINDPX_V2)
+#if defined(BOARD_HAS_PWM) && BOARD_HAS_PWM >=6
 
 			case 6:
 				set_mode(MODE_6PWM);
 				break;
 #endif
-#if defined(CONFIG_ARCH_BOARD_AEROCORE)
+
+#if defined(BOARD_HAS_PWM) && BOARD_HAS_PWM >=8
 
 			case 8:
 				set_mode(MODE_8PWM);
@@ -2138,22 +2094,11 @@ PX4FMU::write(file *filp, const char *buffer, size_t len)
 	unsigned count = len / 2;
 	uint16_t values[8];
 
-#ifdef CONFIG_ARCH_BOARD_AEROCORE
-
-	if (count > 8) {
-		// we have at most 8 outputs
-		count = 8;
+	if (count > BOARD_HAS_PWM) {
+		// we have at most BOARD_HAS_PWM outputs
+		count = BOARD_HAS_PWM;
 	}
 
-#else
-
-	if (count > 6) {
-		// we have at most 6 outputs
-		count = 6;
-	}
-
-#endif
-
 	// allow for misaligned values
 	memcpy(values, buffer, count * 2);
 
@@ -2169,301 +2114,21 @@ PX4FMU::write(file *filp, const char *buffer, size_t len)
 void
 PX4FMU::sensor_reset(int ms)
 {
-#if defined(CONFIG_ARCH_BOARD_PX4FMU_V2)
-
 	if (ms < 1) {
 		ms = 1;
 	}
 
-	/* disable SPI bus */
-	stm32_configgpio(GPIO_SPI_CS_GYRO_OFF);
-	stm32_configgpio(GPIO_SPI_CS_ACCEL_MAG_OFF);
-	stm32_configgpio(GPIO_SPI_CS_BARO_OFF);
-	stm32_configgpio(GPIO_SPI_CS_MPU_OFF);
-
-	stm32_gpiowrite(GPIO_SPI_CS_GYRO_OFF, 0);
-	stm32_gpiowrite(GPIO_SPI_CS_ACCEL_MAG_OFF, 0);
-	stm32_gpiowrite(GPIO_SPI_CS_BARO_OFF, 0);
-	stm32_gpiowrite(GPIO_SPI_CS_MPU_OFF, 0);
-
-	stm32_configgpio(GPIO_SPI1_SCK_OFF);
-	stm32_configgpio(GPIO_SPI1_MISO_OFF);
-	stm32_configgpio(GPIO_SPI1_MOSI_OFF);
-
-	stm32_gpiowrite(GPIO_SPI1_SCK_OFF, 0);
-	stm32_gpiowrite(GPIO_SPI1_MISO_OFF, 0);
-	stm32_gpiowrite(GPIO_SPI1_MOSI_OFF, 0);
-
-	stm32_configgpio(GPIO_GYRO_DRDY_OFF);
-	stm32_configgpio(GPIO_MAG_DRDY_OFF);
-	stm32_configgpio(GPIO_ACCEL_DRDY_OFF);
-	stm32_configgpio(GPIO_EXTI_MPU_DRDY_OFF);
-
-	stm32_gpiowrite(GPIO_GYRO_DRDY_OFF, 0);
-	stm32_gpiowrite(GPIO_MAG_DRDY_OFF, 0);
-	stm32_gpiowrite(GPIO_ACCEL_DRDY_OFF, 0);
-	stm32_gpiowrite(GPIO_EXTI_MPU_DRDY_OFF, 0);
-
-	/* set the sensor rail off */
-	stm32_configgpio(GPIO_VDD_3V3_SENSORS_EN);
-	stm32_gpiowrite(GPIO_VDD_3V3_SENSORS_EN, 0);
-
-	/* wait for the sensor rail to reach GND */
-	usleep(ms * 1000);
-	warnx("reset done, %d ms", ms);
-
-	/* re-enable power */
-
-	/* switch the sensor rail back on */
-	stm32_gpiowrite(GPIO_VDD_3V3_SENSORS_EN, 1);
-
-	/* wait a bit before starting SPI, different times didn't influence results */
-	usleep(100);
-
-	/* reconfigure the SPI pins */
-#ifdef CONFIG_STM32_SPI1
-	stm32_configgpio(GPIO_SPI_CS_GYRO);
-	stm32_configgpio(GPIO_SPI_CS_ACCEL_MAG);
-	stm32_configgpio(GPIO_SPI_CS_BARO);
-	stm32_configgpio(GPIO_SPI_CS_MPU);
-
-	/* De-activate all peripherals,
-	 * required for some peripheral
-	 * state machines
-	 */
-	stm32_gpiowrite(GPIO_SPI_CS_GYRO, 1);
-	stm32_gpiowrite(GPIO_SPI_CS_ACCEL_MAG, 1);
-	stm32_gpiowrite(GPIO_SPI_CS_BARO, 1);
-	stm32_gpiowrite(GPIO_SPI_CS_MPU, 1);
-
-	stm32_configgpio(GPIO_SPI1_SCK);
-	stm32_configgpio(GPIO_SPI1_MISO);
-	stm32_configgpio(GPIO_SPI1_MOSI);
-
-	// // XXX bring up the EXTI pins again
-	// stm32_configgpio(GPIO_GYRO_DRDY);
-	// stm32_configgpio(GPIO_MAG_DRDY);
-	// stm32_configgpio(GPIO_ACCEL_DRDY);
-	// stm32_configgpio(GPIO_EXTI_MPU_DRDY);
-
-#endif
-#endif
-#if defined(CONFIG_ARCH_BOARD_PX4FMU_V4)
-
-	if (ms < 1) {
-		ms = 1;
-	}
-
-	/* disable SPI bus */
-	stm32_configgpio(GPIO_SPI_CS_OFF_MPU9250);
-	stm32_configgpio(GPIO_SPI_CS_OFF_HMC5983);
-	stm32_configgpio(GPIO_SPI_CS_OFF_MS5611);
-	stm32_configgpio(GPIO_SPI_CS_OFF_ICM_20608_G);
-
-	stm32_gpiowrite(GPIO_SPI_CS_OFF_MPU9250, 0);
-	stm32_gpiowrite(GPIO_SPI_CS_OFF_HMC5983, 0);
-	stm32_gpiowrite(GPIO_SPI_CS_OFF_MS5611, 0);
-	stm32_gpiowrite(GPIO_SPI_CS_OFF_ICM_20608_G, 0);
-
-	stm32_configgpio(GPIO_SPI1_SCK_OFF);
-	stm32_configgpio(GPIO_SPI1_MISO_OFF);
-	stm32_configgpio(GPIO_SPI1_MOSI_OFF);
-
-	stm32_gpiowrite(GPIO_SPI1_SCK_OFF, 0);
-	stm32_gpiowrite(GPIO_SPI1_MISO_OFF, 0);
-	stm32_gpiowrite(GPIO_SPI1_MOSI_OFF, 0);
-
-	stm32_configgpio(GPIO_DRDY_OFF_MPU9250);
-	stm32_configgpio(GPIO_DRDY_OFF_HMC5983);
-	stm32_configgpio(GPIO_DRDY_OFF_ICM_20608_G);
-
-	stm32_gpiowrite(GPIO_DRDY_OFF_MPU9250, 0);
-	stm32_gpiowrite(GPIO_DRDY_OFF_HMC5983, 0);
-	stm32_gpiowrite(GPIO_DRDY_OFF_ICM_20608_G, 0);
-
-	/* set the sensor rail off */
-	stm32_configgpio(GPIO_VDD_3V3_SENSORS_EN);
-	stm32_gpiowrite(GPIO_VDD_3V3_SENSORS_EN, 0);
-
-	/* wait for the sensor rail to reach GND */
-	usleep(ms * 1000);
-	warnx("reset done, %d ms", ms);
-
-	/* re-enable power */
-
-	/* switch the sensor rail back on */
-	stm32_gpiowrite(GPIO_VDD_3V3_SENSORS_EN, 1);
-
-	/* wait a bit before starting SPI, different times didn't influence results */
-	usleep(100);
-
-	/* reconfigure the SPI pins */
-#ifdef CONFIG_STM32_SPI1
-	stm32_configgpio(GPIO_SPI_CS_MPU9250);
-	stm32_configgpio(GPIO_SPI_CS_HMC5983);
-	stm32_configgpio(GPIO_SPI_CS_MS5611);
-	stm32_configgpio(GPIO_SPI_CS_ICM_20608_G);
-
-	/* De-activate all peripherals,
-	 * required for some peripheral
-	 * state machines
-	 */
-	stm32_gpiowrite(GPIO_SPI_CS_MPU9250, 1);
-	stm32_gpiowrite(GPIO_SPI_CS_HMC5983, 1);
-	stm32_gpiowrite(GPIO_SPI_CS_MS5611, 1);
-	stm32_gpiowrite(GPIO_SPI_CS_ICM_20608_G, 1);
-
-	stm32_configgpio(GPIO_SPI1_SCK);
-	stm32_configgpio(GPIO_SPI1_MISO);
-	stm32_configgpio(GPIO_SPI1_MOSI);
-
-	// // XXX bring up the EXTI pins again
-	// stm32_configgpio(GPIO_GYRO_DRDY);
-	// stm32_configgpio(GPIO_MAG_DRDY);
-	// stm32_configgpio(GPIO_ACCEL_DRDY);
-	// stm32_configgpio(GPIO_EXTI_MPU_DRDY);
-
-#endif
-#endif
-
-#if  defined(CONFIG_ARCH_BOARD_MINDPX_V2)
-
-	if (ms < 1) {
-		ms = 1;
-	}
-
-	/* disable SPI bus */
-	stm32_configgpio(GPIO_SPI_CS_GYRO_OFF);
-	stm32_configgpio(GPIO_SPI_CS_ACCEL_MAG_OFF);
-	stm32_configgpio(GPIO_SPI_CS_BARO_OFF);
-	//      stm32_configgpio(GPIO_SPI_CS_FRAM_OFF);
-	stm32_configgpio(GPIO_SPI_CS_MPU_OFF);
-
-	stm32_gpiowrite(GPIO_SPI_CS_GYRO_OFF, 0);
-	stm32_gpiowrite(GPIO_SPI_CS_ACCEL_MAG_OFF, 0);
-	stm32_gpiowrite(GPIO_SPI_CS_BARO_OFF, 0);
-	//       stm32_gpiowrite(GPIO_SPI_CS_FRAM_OFF,0);
-	stm32_gpiowrite(GPIO_SPI_CS_MPU_OFF, 0);
-
-	stm32_configgpio(GPIO_SPI4_SCK_OFF);
-	stm32_configgpio(GPIO_SPI4_MISO_OFF);
-	stm32_configgpio(GPIO_SPI4_MOSI_OFF);
-
-	stm32_gpiowrite(GPIO_SPI4_SCK_OFF, 0);
-	stm32_gpiowrite(GPIO_SPI4_MISO_OFF, 0);
-	stm32_gpiowrite(GPIO_SPI4_MOSI_OFF, 0);
-
-	stm32_configgpio(GPIO_GYRO_DRDY_OFF);
-	stm32_configgpio(GPIO_MAG_DRDY_OFF);
-	stm32_configgpio(GPIO_ACCEL_DRDY_OFF);
-	stm32_configgpio(GPIO_EXTI_MPU_DRDY_OFF);
-
-	stm32_gpiowrite(GPIO_GYRO_DRDY_OFF, 0);
-	stm32_gpiowrite(GPIO_MAG_DRDY_OFF, 0);
-	stm32_gpiowrite(GPIO_ACCEL_DRDY_OFF, 0);
-	stm32_gpiowrite(GPIO_EXTI_MPU_DRDY_OFF, 0);
-
-	//        /* set the sensor rail off */
-	//        stm32_configgpio(GPIO_VDD_3V3_SENSORS_EN);
-	//        stm32_gpiowrite(GPIO_VDD_3V3_SENSORS_EN, 0);
-	//
-	/* wait for the sensor rail to reach GND */
-	usleep(ms * 1000);
-	warnx("reset done, %d ms", ms);
-	//
-	//        /* re-enable power */
-	//
-	//        /* switch the sensor rail back on */
-	//        stm32_gpiowrite(GPIO_VDD_3V3_SENSORS_EN, 1);
-	//
-	/* wait a bit before starting SPI, different times didn't influence results */
-	usleep(100);
-
-	/* reconfigure the SPI pins */
-#ifdef CONFIG_STM32_SPI4
-	stm32_configgpio(GPIO_SPI_CS_GYRO);
-	stm32_configgpio(GPIO_SPI_CS_ACCEL_MAG);
-	stm32_configgpio(GPIO_SPI_CS_BARO);
-	//        stm32_configgpio(GPIO_SPI_CS_FRAM);
-	stm32_configgpio(GPIO_SPI_CS_MPU);
-
-	/* De-activate all peripherals,
-	* required for some peripheral
-	* state machines
-	*/
-	stm32_gpiowrite(GPIO_SPI_CS_GYRO, 1);
-	stm32_gpiowrite(GPIO_SPI_CS_ACCEL_MAG, 1);
-	stm32_gpiowrite(GPIO_SPI_CS_BARO, 1);
-	stm32_gpiowrite(GPIO_SPI_CS_FRAM, 1);
-	stm32_gpiowrite(GPIO_SPI_CS_MPU, 1);
-
-	stm32_configgpio(GPIO_SPI4_SCK);
-	stm32_configgpio(GPIO_SPI4_MISO);
-	stm32_configgpio(GPIO_SPI4_MOSI);
-
-	// // XXX bring up the EXTI pins again
-	// stm32_configgpio(GPIO_GYRO_DRDY);
-	// stm32_configgpio(GPIO_MAG_DRDY);
-	// stm32_configgpio(GPIO_ACCEL_DRDY);
-
-#endif
-#endif
+	board_spi_reset(ms);
 }
 
 void
 PX4FMU::peripheral_reset(int ms)
 {
-#if defined(CONFIG_ARCH_BOARD_PX4FMU_V2)
-
 	if (ms < 1) {
 		ms = 10;
 	}
 
-	/* set the peripheral rails off */
-	stm32_configgpio(GPIO_VDD_5V_PERIPH_EN);
-	stm32_gpiowrite(GPIO_VDD_5V_PERIPH_EN, 1);
-
-	/* wait for the peripheral rail to reach GND */
-	usleep(ms * 1000);
-	warnx("reset done, %d ms", ms);
-
-	/* re-enable power */
-
-	/* switch the peripheral rail back on */
-	stm32_gpiowrite(GPIO_VDD_5V_PERIPH_EN, 0);
-#endif
-#if defined(CONFIG_ARCH_BOARD_PX4FMU_V4)
-
-	if (ms < 1) {
-		ms = 10;
-	}
-
-	/* set the peripheral rails off */
-	stm32_configgpio(GPIO_PERIPH_3V3_EN);
-
-	stm32_gpiowrite(GPIO_PERIPH_3V3_EN, 0);
-
-	bool last = stm32_gpioread(GPIO_SPEKTRUM_PWR_EN);
-	/* Keep Spektum on to discharge rail*/
-	stm32_gpiowrite(GPIO_SPEKTRUM_PWR_EN, 1);
-
-	/* wait for the peripheral rail to reach GND */
-	usleep(ms * 1000);
-	warnx("reset done, %d ms", ms);
-
-	/* re-enable power */
-
-	/* switch the peripheral rail back on */
-	stm32_gpiowrite(GPIO_SPEKTRUM_PWR_EN, last);
-	stm32_gpiowrite(GPIO_PERIPH_3V3_EN, 1);
-#endif
-#if defined(CONFIG_ARCH_BOARD_MINDPX_V2)
-
-	if (ms < 1) {
-		ms = 10;
-	}
-
-#endif
+	board_peripheral_reset(ms);
 }
 
 void
@@ -2475,37 +2140,37 @@ PX4FMU::gpio_reset(void)
 	 */
 	for (unsigned i = 0; i < _ngpio; i++) {
 		if (_gpio_tab[i].input != 0) {
-			stm32_configgpio(_gpio_tab[i].input);
+			px4_arch_configgpio(_gpio_tab[i].input);
 
 		} else if (_gpio_tab[i].output != 0) {
-			stm32_configgpio(_gpio_tab[i].output);
+			px4_arch_configgpio(_gpio_tab[i].output);
 		}
 	}
 
-#if defined(CONFIG_ARCH_BOARD_PX4FMU_V1)
+#if defined(GPIO_GPIO_DIR)
 	/* if we have a GPIO direction control, set it to zero (input) */
-	stm32_gpiowrite(GPIO_GPIO_DIR, 0);
-	stm32_configgpio(GPIO_GPIO_DIR);
+	px4_arch_gpiowrite(GPIO_GPIO_DIR, 0);
+	px4_arch_configgpio(GPIO_GPIO_DIR);
 #endif
 }
 
 void
 PX4FMU::gpio_set_function(uint32_t gpios, int function)
 {
-#if defined(CONFIG_ARCH_BOARD_PX4FMU_V1)
+#if defined(BOARD_GPIO_SHARED_BUFFERED_BITS) && defined(GPIO_GPIO_DIR)
 
 	/*
 	 * GPIOs 0 and 1 must have the same direction as they are buffered
 	 * by a shared 2-port driver.  Any attempt to set either sets both.
 	 */
-	if (gpios & 3) {
-		gpios |= 3;
+	if (gpios & BOARD_GPIO_SHARED_BUFFERED_BITS) {
+		gpios |= BOARD_GPIO_SHARED_BUFFERED_BITS;
 
 		/* flip the buffer to output mode if required */
 		if (GPIO_SET_OUTPUT == function ||
 		    GPIO_SET_OUTPUT_LOW == function ||
 		    GPIO_SET_OUTPUT_HIGH == function) {
-			stm32_gpiowrite(GPIO_GPIO_DIR, 1);
+			px4_arch_gpiowrite(GPIO_GPIO_DIR, 1);
 		}
 	}
 
@@ -2516,24 +2181,24 @@ PX4FMU::gpio_set_function(uint32_t gpios, int function)
 		if (gpios & (1 << i)) {
 			switch (function) {
 			case GPIO_SET_INPUT:
-				stm32_configgpio(_gpio_tab[i].input);
+				px4_arch_configgpio(_gpio_tab[i].input);
 				break;
 
 			case GPIO_SET_OUTPUT:
-				stm32_configgpio(_gpio_tab[i].output);
+				px4_arch_configgpio(_gpio_tab[i].output);
 				break;
 
 			case GPIO_SET_OUTPUT_LOW:
-				stm32_configgpio((_gpio_tab[i].output & ~(GPIO_OUTPUT_SET)) | GPIO_OUTPUT_CLEAR);
+				px4_arch_configgpio((_gpio_tab[i].output & ~(GPIO_OUTPUT_SET)) | GPIO_OUTPUT_CLEAR);
 				break;
 
 			case GPIO_SET_OUTPUT_HIGH:
-				stm32_configgpio((_gpio_tab[i].output & ~(GPIO_OUTPUT_CLEAR)) | GPIO_OUTPUT_SET);
+				px4_arch_configgpio((_gpio_tab[i].output & ~(GPIO_OUTPUT_CLEAR)) | GPIO_OUTPUT_SET);
 				break;
 
 			case GPIO_SET_ALT_1:
 				if (_gpio_tab[i].alt != 0) {
-					stm32_configgpio(_gpio_tab[i].alt);
+					px4_arch_configgpio(_gpio_tab[i].alt);
 				}
 
 				break;
@@ -2541,11 +2206,11 @@ PX4FMU::gpio_set_function(uint32_t gpios, int function)
 		}
 	}
 
-#if defined(CONFIG_ARCH_BOARD_PX4FMU_V1)
+#if defined(BOARD_GPIO_SHARED_BUFFERED_BITS) && defined(GPIO_GPIO_DIR)
 
 	/* flip buffer to input mode if required */
-	if ((GPIO_SET_INPUT == function) && (gpios & 3)) {
-		stm32_gpiowrite(GPIO_GPIO_DIR, 0);
+	if ((GPIO_SET_INPUT == function) && (gpios & BOARD_GPIO_SHARED_BUFFERED_BITS)) {
+		px4_arch_gpiowrite(GPIO_GPIO_DIR, 0);
 	}
 
 #endif
@@ -2558,7 +2223,7 @@ PX4FMU::gpio_write(uint32_t gpios, int function)
 
 	for (unsigned i = 0; i < _ngpio; i++)
 		if (gpios & (1 << i)) {
-			stm32_gpiowrite(_gpio_tab[i].output, value);
+			px4_arch_gpiowrite(_gpio_tab[i].output, value);
 		}
 }
 
@@ -2568,7 +2233,7 @@ PX4FMU::gpio_read(void)
 	uint32_t bits = 0;
 
 	for (unsigned i = 0; i < _ngpio; i++)
-		if (stm32_gpioread(_gpio_tab[i].input)) {
+		if (px4_arch_gpioread(_gpio_tab[i].input)) {
 			bits |= (1 << i);
 		}
 
@@ -2814,21 +2479,19 @@ fmu_new_mode(PortMode new_mode)
 		break;
 
 	case PORT_FULL_PWM:
-#if defined(CONFIG_ARCH_BOARD_PX4FMU_V1)
+#if defined(BOARD_HAS_PWM) && BOARD_HAS_PWM == 4
 		/* select 4-pin PWM mode */
 		servo_mode = PX4FMU::MODE_4PWM;
 #endif
-#if defined(CONFIG_ARCH_BOARD_PX4FMU_V2) ||  defined(CONFIG_ARCH_BOARD_PX4FMU_V4) \
-	||  defined(CONFIG_ARCH_BOARD_MINDPX_V2)
+#if defined(BOARD_HAS_PWM) && BOARD_HAS_PWM == 6
 		servo_mode = PX4FMU::MODE_6PWM;
 #endif
-#if defined(CONFIG_ARCH_BOARD_AEROCORE)
+#if defined(BOARD_HAS_PWM) && BOARD_HAS_PWM == 8
 		servo_mode = PX4FMU::MODE_8PWM;
 #endif
 		break;
 
-#if defined(CONFIG_ARCH_BOARD_PX4FMU_V2) ||  defined(CONFIG_ARCH_BOARD_PX4FMU_V4) \
-	|| defined(CONFIG_ARCH_BOARD_MINDPX_V2)
+#if defined(BOARD_HAS_PWM) && BOARD_HAS_PWM >= 6
 
 	case PORT_PWM4:
 		/* select 4-pin PWM mode */
@@ -2859,7 +2522,7 @@ fmu_new_mode(PortMode new_mode)
 #endif
 
 		/* mixed modes supported on v1 board only */
-#if defined(CONFIG_ARCH_BOARD_PX4FMU_V1)
+#if defined(BOARD_HAS_MULTI_PURPOSE_GPIO)
 
 	case PORT_FULL_SERIAL:
 		/* set all multi-GPIOs to serial mode */
@@ -3261,8 +2924,7 @@ fmu_main(int argc, char *argv[])
 	} else if (!strcmp(verb, "mode_pwm")) {
 		new_mode = PORT_FULL_PWM;
 
-#if defined(CONFIG_ARCH_BOARD_PX4FMU_V2) ||  defined(CONFIG_ARCH_BOARD_PX4FMU_V4) \
-	||  defined(CONFIG_ARCH_BOARD_MINDPX_V2)
+#if defined(BOARD_HAS_PWM) && BOARD_HAS_PWM >= 6
 
 	} else if (!strcmp(verb, "mode_pwm4")) {
 		new_mode = PORT_PWM4;
@@ -3279,7 +2941,7 @@ fmu_main(int argc, char *argv[])
 	} else if (!strcmp(verb, "mode_pwm2cap2")) {
 		new_mode = PORT_PWM2CAP2;
 #endif
-#if defined(CONFIG_ARCH_BOARD_PX4FMU_V1)
+#if defined(BOARD_HAS_MULTI_PURPOSE_GPIO)
 
 	} else if (!strcmp(verb, "mode_serial")) {
 		new_mode = PORT_FULL_SERIAL;
@@ -3350,11 +3012,10 @@ fmu_main(int argc, char *argv[])
 	}
 
 	fprintf(stderr, "FMU: unrecognised command %s, try:\n", verb);
-#if defined(CONFIG_ARCH_BOARD_PX4FMU_V1)
+#if defined(BOARD_HAS_MULTI_PURPOSE_GPIO)
 	fprintf(stderr,
 		"  mode_gpio, mode_serial, mode_pwm, mode_gpio_serial, mode_pwm_serial, mode_pwm_gpio, test, fake, sensor_reset, id\n");
-#elif defined(CONFIG_ARCH_BOARD_PX4FMU_V2) || defined(CONFIG_ARCH_BOARD_PX4FMU_V4) || defined(CONFIG_ARCH_BOARD_AEROCORE) \
-	|| defined(CONFIG_ARCH_BOARD_MINDPX_V2)
+#elif defined(BOARD_HAS_PWM) && BOARD_HAS_PWM >= 6
 	fprintf(stderr, "  mode_gpio, mode_pwm, mode_pwm4, test, sensor_reset [milliseconds], i2c <bus> <hz>\n");
 #endif
 	exit(1);
diff --git a/src/drivers/px4io/px4io_serial.cpp b/src/drivers/px4io/px4io_serial.cpp
index 258fa0b5db..f5a08d8545 100644
--- a/src/drivers/px4io/px4io_serial.cpp
+++ b/src/drivers/px4io/px4io_serial.cpp
@@ -228,8 +228,8 @@ PX4IO_serial::~PX4IO_serial()
 	irq_detach(PX4IO_SERIAL_VECTOR);
 
 	/* restore the GPIOs */
-	stm32_unconfiggpio(PX4IO_SERIAL_TX_GPIO);
-	stm32_unconfiggpio(PX4IO_SERIAL_RX_GPIO);
+	px4_arch_unconfiggpio(PX4IO_SERIAL_TX_GPIO);
+	px4_arch_unconfiggpio(PX4IO_SERIAL_RX_GPIO);
 
 	/* and kill our semaphores */
 	px4_sem_destroy(&_completion_semaphore);
@@ -264,8 +264,8 @@ PX4IO_serial::init()
 	}
 
 	/* configure pins for serial use */
-	stm32_configgpio(PX4IO_SERIAL_TX_GPIO);
-	stm32_configgpio(PX4IO_SERIAL_RX_GPIO);
+	px4_arch_configgpio(PX4IO_SERIAL_TX_GPIO);
+	px4_arch_configgpio(PX4IO_SERIAL_RX_GPIO);
 
 	/* reset & configure the UART */
 	rCR1 = 0;
diff --git a/src/drivers/rgbled_pwm/drv_led_pwm.cpp b/src/drivers/rgbled_pwm/drv_led_pwm.cpp
index e495e975ca..17590004ec 100644
--- a/src/drivers/rgbled_pwm/drv_led_pwm.cpp
+++ b/src/drivers/rgbled_pwm/drv_led_pwm.cpp
@@ -216,7 +216,7 @@ led_pwm_channel_init(unsigned channel)
 	unsigned timer = led_pwm_channels[channel].timer_index;
 
 	/* configure the GPIO first */
-	stm32_configgpio(led_pwm_channels[channel].gpio_out);
+	px4_arch_configgpio(led_pwm_channels[channel].gpio_out);
 
 	/* configure the channel */
 	switch (led_pwm_channels[channel].timer_channel) {
diff --git a/src/drivers/rgbled_pwm/rgbled_pwm.cpp b/src/drivers/rgbled_pwm/rgbled_pwm.cpp
index cf2a91b91c..1857d9447c 100644
--- a/src/drivers/rgbled_pwm/rgbled_pwm.cpp
+++ b/src/drivers/rgbled_pwm/rgbled_pwm.cpp
@@ -170,11 +170,9 @@ RGBLED_PWM::init()
 {
 	/* switch off LED on start */
 	CDev::init();
-#if defined(CONFIG_ARCH_BOARD_MINDPX_V2)
 	led_pwm_servo_init();
 	send_led_enable(false);
 	send_led_rgb();
-#endif
 	return OK;
 }
 void
@@ -536,7 +534,6 @@ RGBLED_PWM::send_led_enable(bool enable)
 int
 RGBLED_PWM::send_led_rgb()
 {
-#if defined(CONFIG_ARCH_BOARD_MINDPX_V2)
 
 	if (_enable) {
 		led_pwm_servo_set(0, _r);
@@ -549,7 +546,6 @@ RGBLED_PWM::send_led_rgb()
 		led_pwm_servo_set(2, 0);
 	}
 
-#endif
 	return (OK);
 }
 
diff --git a/src/drivers/sf0x/sf0x.cpp b/src/drivers/sf0x/sf0x.cpp
index 0106261dd6..13492ca2f7 100644
--- a/src/drivers/sf0x/sf0x.cpp
+++ b/src/drivers/sf0x/sf0x.cpp
@@ -423,14 +423,14 @@ SF0X::ioctl(struct file *filp, int cmd, unsigned long arg)
 				return -EINVAL;
 			}
 
-			irqstate_t flags = irqsave();
+			irqstate_t flags = px4_enter_critical_section();
 
 			if (!_reports->resize(arg)) {
-				irqrestore(flags);
+				px4_leave_critical_section(flags);
 				return -ENOMEM;
 			}
 
-			irqrestore(flags);
+			px4_leave_critical_section(flags);
 
 			return OK;
 		}
diff --git a/src/drivers/sf10a/sf10a.cpp b/src/drivers/sf10a/sf10a.cpp
index 967aae5176..32919a4033 100644
--- a/src/drivers/sf10a/sf10a.cpp
+++ b/src/drivers/sf10a/sf10a.cpp
@@ -386,14 +386,14 @@ SF10A::ioctl(struct file *filp, int cmd, unsigned long arg)
 				return -EINVAL;
 			}
 
-			irqstate_t flags = irqsave();
+			irqstate_t flags = px4_enter_critical_section();
 
 			if (!_reports->resize(arg)) {
-				irqrestore(flags);
+				px4_leave_critical_section(flags);
 				return -ENOMEM;
 			}
 
-			irqrestore(flags);
+			px4_leave_critical_section(flags);
 
 			return OK;
 		}
diff --git a/src/drivers/srf02/srf02.cpp b/src/drivers/srf02/srf02.cpp
index b5600e2a73..7988196049 100644
--- a/src/drivers/srf02/srf02.cpp
+++ b/src/drivers/srf02/srf02.cpp
@@ -430,14 +430,14 @@ SRF02::ioctl(struct file *filp, int cmd, unsigned long arg)
 				return -EINVAL;
 			}
 
-			irqstate_t flags = irqsave();
+			irqstate_t flags = px4_enter_critical_section();
 
 			if (!_reports->resize(arg)) {
-				irqrestore(flags);
+				px4_leave_critical_section(flags);
 				return -ENOMEM;
 			}
 
-			irqrestore(flags);
+			px4_leave_critical_section(flags);
 
 			return OK;
 		}
diff --git a/src/drivers/srf02_i2c/srf02_i2c.cpp b/src/drivers/srf02_i2c/srf02_i2c.cpp
index 3ea72f7e15..de8ed55cff 100644
--- a/src/drivers/srf02_i2c/srf02_i2c.cpp
+++ b/src/drivers/srf02_i2c/srf02_i2c.cpp
@@ -432,14 +432,14 @@ SRF02_I2C::ioctl(struct file *filp, int cmd, unsigned long arg)
 				return -EINVAL;
 			}
 
-			irqstate_t flags = irqsave();
+			irqstate_t flags = px4_enter_critical_section();
 
 			if (!_reports->resize(arg)) {
-				irqrestore(flags);
+				px4_leave_critical_section(flags);
 				return -ENOMEM;
 			}
 
-			irqrestore(flags);
+			px4_leave_critical_section(flags);
 
 			return OK;
 		}
diff --git a/src/drivers/stm32/adc/adc.cpp b/src/drivers/stm32/adc/adc.cpp
index bb2c992730..18d1d2c218 100644
--- a/src/drivers/stm32/adc/adc.cpp
+++ b/src/drivers/stm32/adc/adc.cpp
@@ -273,9 +273,9 @@ ADC::read(file *filp, char *buffer, size_t len)
 	}
 
 	/* block interrupts while copying samples to avoid racing with an update */
-	irqstate_t flags = irqsave();
+	irqstate_t flags = px4_enter_critical_section();
 	memcpy(buffer, _samples, len);
-	irqrestore(flags);
+	px4_leave_critical_section(flags);
 
 	return len;
 }
@@ -343,9 +343,7 @@ ADC::update_adc_report(hrt_abstime now)
 void
 ADC::update_system_power(hrt_abstime now)
 {
-#if defined (CONFIG_ARCH_BOARD_PX4FMU_V2) || \
-    defined (CONFIG_ARCH_BOARD_MINDPX_V2) || \
-    defined (CONFIG_ARCH_BOARD_PX4FMU_V4)
+#if defined (BOARD_ADC_USB_CONNECTED)
 	system_power_s system_power = {};
 	system_power.timestamp = now;
 
@@ -358,35 +356,19 @@ ADC::update_system_power(hrt_abstime now)
 		}
 	}
 
+	/* Note once the board_config.h provides BOARD_ADC_USB_CONNECTED,
+	 * It must provide the true logic GPIO BOARD_ADC_xxxx macros.
+	 */
 	// these are not ADC related, but it is convenient to
 	// publish these to the same topic
-	system_power.usb_connected = stm32_gpioread(GPIO_OTGFS_VBUS);
+	system_power.usb_connected = BOARD_ADC_USB_CONNECTED;
 
-#if defined (CONFIG_ARCH_BOARD_MINDPX_V2)
-	// note that the valid pins are active low
-	system_power.brick_valid   = 1;
-	system_power.servo_valid   = 1;
+	system_power.brick_valid   = BOARD_ADC_BRICK_VALID;
+	system_power.servo_valid   = BOARD_ADC_SERVO_VALID;
 
 	// OC pins are active low
-	system_power.periph_5V_OC  = 1;
-	system_power.hipower_5V_OC = 1;
-#elif defined (CONFIG_ARCH_BOARD_PX4FMU_V4)
-	// note that the valid pins are active high
-	system_power.brick_valid   = stm32_gpioread(GPIO_VDD_BRICK_VALID);
-	system_power.servo_valid   = 1;
-
-	// OC pins are not supported
-	system_power.periph_5V_OC  = 0;
-	system_power.hipower_5V_OC = 0;
-#else
-	// note that the valid pins are active low
-	system_power.brick_valid   = !stm32_gpioread(GPIO_VDD_BRICK_VALID);
-	system_power.servo_valid   = !stm32_gpioread(GPIO_VDD_SERVO_VALID);
-
-	// OC pins are active low
-	system_power.periph_5V_OC  = !stm32_gpioread(GPIO_VDD_5V_PERIPH_OC);
-	system_power.hipower_5V_OC = !stm32_gpioread(GPIO_VDD_5V_HIPOWER_OC);
-#endif
+	system_power.periph_5V_OC  = BOARD_ADC_PERIPH_5V_OC;
+	system_power.hipower_5V_OC = BOARD_ADC_HIPOWER_5V_OC;
 
 	/* lazily publish */
 	if (_to_system_power != nullptr) {
@@ -396,7 +378,7 @@ ADC::update_system_power(hrt_abstime now)
 		_to_system_power = orb_advertise(ORB_ID(system_power), &system_power);
 	}
 
-#endif // CONFIG_ARCH_BOARD_PX4FMU_V2
+#endif // BOARD_ADC_USB_CONNECTED
 }
 
 uint16_t
diff --git a/src/drivers/stm32/drv_hrt.c b/src/drivers/stm32/drv_hrt.c
index 3bea3fda05..f3ec622a3d 100644
--- a/src/drivers/stm32/drv_hrt.c
+++ b/src/drivers/stm32/drv_hrt.c
@@ -667,7 +667,7 @@ hrt_absolute_time(void)
 	static volatile uint32_t last_count;
 
 	/* prevent re-entry */
-	flags = irqsave();
+	flags = px4_enter_critical_section();
 
 	/* get the current counter value */
 	count = rCNT;
@@ -689,7 +689,7 @@ hrt_absolute_time(void)
 	/* compute the current time */
 	abstime = HRT_COUNTER_SCALE(base_time + count);
 
-	irqrestore(flags);
+	px4_leave_critical_section(flags);
 
 	return abstime;
 }
@@ -725,11 +725,11 @@ abstime_to_ts(struct timespec *ts, hrt_abstime abstime)
 hrt_abstime
 hrt_elapsed_time(const volatile hrt_abstime *then)
 {
-	irqstate_t flags = irqsave();
+	irqstate_t flags = px4_enter_critical_section();
 
 	hrt_abstime delta = hrt_absolute_time() - *then;
 
-	irqrestore(flags);
+	px4_leave_critical_section(flags);
 
 	return delta;
 }
@@ -740,11 +740,11 @@ hrt_elapsed_time(const volatile hrt_abstime *then)
 hrt_abstime
 hrt_store_absolute_time(volatile hrt_abstime *now)
 {
-	irqstate_t flags = irqsave();
+	irqstate_t flags = px4_enter_critical_section();
 
 	hrt_abstime ts = hrt_absolute_time();
 
-	irqrestore(flags);
+	px4_leave_critical_section(flags);
 
 	return ts;
 }
@@ -760,7 +760,7 @@ hrt_init(void)
 
 #ifdef HRT_PPM_CHANNEL
 	/* configure the PPM input pin */
-	stm32_configgpio(GPIO_PPM_IN);
+	px4_arch_configgpio(GPIO_PPM_IN);
 #endif
 }
 
@@ -802,7 +802,7 @@ hrt_call_every(struct hrt_call *entry, hrt_abstime delay, hrt_abstime interval,
 static void
 hrt_call_internal(struct hrt_call *entry, hrt_abstime deadline, hrt_abstime interval, hrt_callout callout, void *arg)
 {
-	irqstate_t flags = irqsave();
+	irqstate_t flags = px4_enter_critical_section();
 
 	/* if the entry is currently queued, remove it */
 	/* note that we are using a potentially uninitialised
@@ -823,7 +823,7 @@ hrt_call_internal(struct hrt_call *entry, hrt_abstime deadline, hrt_abstime inte
 
 	hrt_call_enter(entry);
 
-	irqrestore(flags);
+	px4_leave_critical_section(flags);
 }
 
 /**
@@ -843,7 +843,7 @@ hrt_called(struct hrt_call *entry)
 void
 hrt_cancel(struct hrt_call *entry)
 {
-	irqstate_t flags = irqsave();
+	irqstate_t flags = px4_enter_critical_section();
 
 	sq_rem(&entry->link, &callout_queue);
 	entry->deadline = 0;
@@ -853,7 +853,7 @@ hrt_cancel(struct hrt_call *entry)
 	 */
 	entry->period = 0;
 
-	irqrestore(flags);
+	px4_leave_critical_section(flags);
 }
 
 static void
diff --git a/src/drivers/stm32/drv_input_capture.c b/src/drivers/stm32/drv_input_capture.c
index 330404582b..850397c3df 100644
--- a/src/drivers/stm32/drv_input_capture.c
+++ b/src/drivers/stm32/drv_input_capture.c
@@ -107,7 +107,7 @@ static void input_capture_chan_handler(void *context, const io_timers_t *timer,
 				       hrt_abstime isrs_time , uint16_t isrs_rcnt)
 {
 	uint16_t capture = _REG32(timer, chan->ccr_offset);
-	channel_stats[chan_index].last_edge = stm32_gpioread(chan->gpio_in);
+	channel_stats[chan_index].last_edge = px4_arch_gpioread(chan->gpio_in);
 
 	if ((isrs_rcnt - capture) > channel_stats[chan_index].latnecy) {
 		channel_stats[chan_index].latnecy = (isrs_rcnt - capture);
@@ -133,10 +133,10 @@ static void input_capture_chan_handler(void *context, const io_timers_t *timer,
 
 static void input_capture_bind(unsigned channel, capture_callback_t callback, void *context)
 {
-	irqstate_t flags = irqsave();
+	irqstate_t flags = px4_enter_critical_section();
 	channel_handlers[channel].callback = callback;
 	channel_handlers[channel].context = context;
-	irqrestore(flags);
+	px4_leave_critical_section(flags);
 }
 
 static void input_capture_unbind(unsigned channel)
@@ -258,7 +258,7 @@ int up_input_capture_set_filter(unsigned channel,  capture_filter_t filter)
 			uint32_t timer = timer_io_channels[channel].timer_index;
 			uint16_t rvalue;
 
-			irqstate_t flags = irqsave();
+			irqstate_t flags = px4_enter_critical_section();
 
 			switch (timer_io_channels[channel].timer_channel) {
 
@@ -290,7 +290,7 @@ int up_input_capture_set_filter(unsigned channel,  capture_filter_t filter)
 				rv = -EIO;
 			}
 
-			irqrestore(flags);
+			px4_leave_critical_section(flags);
 		}
 	}
 
@@ -401,7 +401,7 @@ int up_input_capture_set_trigger(unsigned channel,  input_capture_edge edge)
 			uint16_t rvalue;
 			rv = OK;
 
-			irqstate_t flags = irqsave();
+			irqstate_t flags = px4_enter_critical_section();
 
 			switch (timer_io_channels[channel].timer_channel) {
 
@@ -437,7 +437,7 @@ int up_input_capture_set_trigger(unsigned channel,  input_capture_edge edge)
 				rv = -EIO;
 			}
 
-			irqrestore(flags);
+			px4_leave_critical_section(flags);
 		}
 	}
 
@@ -456,10 +456,10 @@ int up_input_capture_get_callback(unsigned channel, capture_callback_t *callback
 
 		if (io_timer_get_channel_mode(channel) == IOTimerChanMode_Capture) {
 
-			irqstate_t flags = irqsave();
+			irqstate_t flags = px4_enter_critical_section();
 			*callback = channel_handlers[channel].callback;
 			*context = channel_handlers[channel].context;
-			irqrestore(flags);
+			px4_leave_critical_section(flags);
 			rv = OK;
 		}
 	}
@@ -492,14 +492,14 @@ int up_input_capture_get_stats(unsigned channel, input_capture_stats_t *stats, b
 	int rv = io_timer_validate_channel_index(channel);
 
 	if (rv == 0) {
-		irqstate_t flags = irqsave();
+		irqstate_t flags = px4_enter_critical_section();
 		*stats =  channel_stats[channel];
 
 		if (clear) {
 			memset(&channel_stats[channel], 0, sizeof(*stats));
 		}
 
-		irqrestore(flags);
+		px4_leave_critical_section(flags);
 	}
 
 	return rv;
diff --git a/src/drivers/stm32/drv_io_timer.c b/src/drivers/stm32/drv_io_timer.c
index 9fb9393cbb..385d250ef0 100644
--- a/src/drivers/stm32/drv_io_timer.c
+++ b/src/drivers/stm32/drv_io_timer.c
@@ -397,7 +397,7 @@ static int io_timer_init_timer(unsigned timer)
 
 	if (rv == 0) {
 
-		irqstate_t flags = irqsave();
+		irqstate_t flags = px4_enter_critical_section();
 
 		set_timer_initalized(timer);
 
@@ -448,7 +448,7 @@ static int io_timer_init_timer(unsigned timer)
 
 		up_enable_irq(io_timers[timer].vectorno);
 
-		irqrestore(flags);
+		px4_leave_critical_section(flags);
 	}
 
 	return rv;
@@ -520,11 +520,11 @@ int io_timer_channel_init(unsigned channel, io_timer_channel_mode_t mode,
 
 		io_timer_init_timer(channels_timer(channel));
 
-		irqstate_t flags = irqsave();
+		irqstate_t flags = px4_enter_critical_section();
 
 		/* Set up IO */
 		if (gpio) {
-			stm32_configgpio(gpio);
+			px4_arch_configgpio(gpio);
 		}
 
 
@@ -570,7 +570,7 @@ int io_timer_channel_init(unsigned channel, io_timer_channel_mode_t mode,
 		channel_handlers[channel].callback = channel_handler;
 		channel_handlers[channel].context = context;
 		rDIER(timer) |= dier_setbits << shifts;
-		irqrestore(flags);
+		px4_leave_critical_section(flags);
 	}
 
 	return rv;
@@ -643,7 +643,7 @@ int io_timer_set_enable(bool state, io_timer_channel_mode_t mode, io_timer_chann
 		}
 	}
 
-	irqstate_t flags = irqsave();
+	irqstate_t flags = px4_enter_critical_section();
 
 	for (unsigned actions = 0; actions < arraySize(action_cache) && action_cache[actions].base != 0 ; actions++) {
 		uint32_t rvalue = _REG32(action_cache[actions].base, STM32_GTIM_CCER_OFFSET);
@@ -667,7 +667,7 @@ int io_timer_set_enable(bool state, io_timer_channel_mode_t mode, io_timer_chann
 
 			for (unsigned chan = 0; chan < arraySize(action_cache[actions].gpio); chan++) {
 				if (action_cache[actions].gpio[chan]) {
-					stm32_configgpio(action_cache[actions].gpio[chan]);
+					px4_arch_configgpio(action_cache[actions].gpio[chan]);
 					action_cache[actions].gpio[chan] = 0;
 				}
 			}
@@ -681,7 +681,7 @@ int io_timer_set_enable(bool state, io_timer_channel_mode_t mode, io_timer_chann
 		}
 	}
 
-	irqrestore(flags);
+	px4_leave_critical_section(flags);
 
 	return 0;
 }
diff --git a/src/drivers/stm32/tone_alarm/tone_alarm.cpp b/src/drivers/stm32/tone_alarm/tone_alarm.cpp
index 1a54e11809..34f7b3b296 100644
--- a/src/drivers/stm32/tone_alarm/tone_alarm.cpp
+++ b/src/drivers/stm32/tone_alarm/tone_alarm.cpp
@@ -439,7 +439,7 @@ ToneAlarm::init()
 	}
 
 	/* configure the GPIO to the idle state */
-	stm32_configgpio(GPIO_TONE_ALARM_IDLE);
+	px4_arch_configgpio(GPIO_TONE_ALARM_IDLE);
 
 	/* clock/power on our timer */
 	modifyreg32(TONE_ALARM_CLOCK_POWER_REG, 0, TONE_ALARM_CLOCK_ENABLE);
@@ -571,7 +571,7 @@ ToneAlarm::start_note(unsigned note)
 	rCCER |= TONE_CCER;	// enable the output
 
 	// configure the GPIO to enable timer output
-	stm32_configgpio(GPIO_TONE_ALARM);
+	px4_arch_configgpio(GPIO_TONE_ALARM);
 }
 
 void
@@ -583,7 +583,7 @@ ToneAlarm::stop_note()
 	/*
 	 * Make sure the GPIO is not driving the speaker.
 	 */
-	stm32_configgpio(GPIO_TONE_ALARM_IDLE);
+	px4_arch_configgpio(GPIO_TONE_ALARM_IDLE);
 }
 
 void
@@ -876,7 +876,7 @@ ToneAlarm::ioctl(file *filp, int cmd, unsigned long arg)
 
 	DEVICE_DEBUG("ioctl %i %u", cmd, arg);
 
-//	irqstate_t flags = irqsave();
+//	irqstate_t flags = px4_enter_critical_section();
 
 	/* decide whether to increase the alarm level to cmd or leave it alone */
 	switch (cmd) {
@@ -911,7 +911,7 @@ ToneAlarm::ioctl(file *filp, int cmd, unsigned long arg)
 		break;
 	}
 
-//	irqrestore(flags);
+//	px4_leave_critical_section(flags);
 
 	/* give it to the superclass if we didn't like it */
 	if (result == -ENOTTY) {
diff --git a/src/drivers/test_ppm/test_ppm.cpp b/src/drivers/test_ppm/test_ppm.cpp
index 5dce26cfe5..db36b7a16b 100644
--- a/src/drivers/test_ppm/test_ppm.cpp
+++ b/src/drivers/test_ppm/test_ppm.cpp
@@ -118,7 +118,7 @@ TEST_PPM::~TEST_PPM()
 int
 TEST_PPM::init()
 {
-	stm32_configgpio(TEST_PPM_PIN);
+	px4_arch_configgpio(TEST_PPM_PIN);
 	start();
 	return OK;
 }
@@ -148,11 +148,11 @@ void
 TEST_PPM::do_out(void)
 {
 	if ((_call_times % 2) == 0) {
-		stm32_gpiowrite(TEST_PPM_PIN, false);
+		px4_arch_gpiowrite(TEST_PPM_PIN, false);
 		hrt_call_after(&_call, _values[_call_times / 2] - _plus_width, (hrt_callout)&TEST_PPM::loops, this);
 
 	} else {
-		stm32_gpiowrite(TEST_PPM_PIN, true);
+		px4_arch_gpiowrite(TEST_PPM_PIN, true);
 		hrt_call_after(&_call, _plus_width, (hrt_callout)&TEST_PPM::loops, this);
 	}
 
diff --git a/src/drivers/trone/trone.cpp b/src/drivers/trone/trone.cpp
index aea4bc9449..ff6362b2ff 100644
--- a/src/drivers/trone/trone.cpp
+++ b/src/drivers/trone/trone.cpp
@@ -442,14 +442,14 @@ TRONE::ioctl(struct file *filp, int cmd, unsigned long arg)
 				return -EINVAL;
 			}
 
-			irqstate_t flags = irqsave();
+			irqstate_t flags = px4_enter_critical_section();
 
 			if (!_reports->resize(arg)) {
-				irqrestore(flags);
+				px4_leave_critical_section(flags);
 				return -ENOMEM;
 			}
 
-			irqrestore(flags);
+			px4_leave_critical_section(flags);
 
 			return OK;
 		}
diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp
index b68358685e..72b8551834 100644
--- a/src/modules/commander/state_machine_helper.cpp
+++ b/src/modules/commander/state_machine_helper.cpp
@@ -39,6 +39,7 @@
  * @author Julian Oes		<julian@oes.ch>
  * @author Sander Smeets	<sander@droneslab.com>
  */
+#include <px4_config.h>
 
 #include <stdio.h>
 #include <unistd.h>
@@ -167,7 +168,7 @@ transition_result_t arming_state_transition(struct vehicle_status_s *status,
 		 * Perform an atomic state update
 		 */
 		#ifdef __PX4_NUTTX
-		irqstate_t flags = irqsave();
+		irqstate_t flags = px4_enter_critical_section();
 		#endif
 
 		/* enforce lockdown in HIL */
@@ -306,7 +307,7 @@ transition_result_t arming_state_transition(struct vehicle_status_s *status,
 
 		/* end of atomic state update */
 		#ifdef __PX4_NUTTX
-		irqrestore(flags);
+		px4_leave_critical_section(flags);
 		#endif
 	}
 
diff --git a/src/modules/px4iofirmware/controls.c b/src/modules/px4iofirmware/controls.c
index b857aa41fa..99570d1056 100644
--- a/src/modules/px4iofirmware/controls.c
+++ b/src/modules/px4iofirmware/controls.c
@@ -542,7 +542,7 @@ ppm_input(uint16_t *values, uint16_t *num_values, uint16_t *frame_len)
 	}
 
 	/* avoid racing with PPM updates */
-	irqstate_t state = irqsave();
+	irqstate_t state = px4_enter_critical_section();
 
 	/*
 	 * If we have received a new PPM frame within the last 200ms, accept it
@@ -571,7 +571,7 @@ ppm_input(uint16_t *values, uint16_t *num_values, uint16_t *frame_len)
 		result = (*num_values > 0);
 	}
 
-	irqrestore(state);
+	px4_leave_critical_section(state);
 
 	return result;
 }
diff --git a/src/modules/px4iofirmware/i2c.c b/src/modules/px4iofirmware/i2c.c
index 74692a2675..75aee69cac 100644
--- a/src/modules/px4iofirmware/i2c.c
+++ b/src/modules/px4iofirmware/i2c.c
@@ -112,8 +112,8 @@ interface_init(void)
 	modifyreg32(STM32_RCC_APB1RSTR, RCC_APB1RSTR_I2C1RST, 0);
 
 	/* configure the i2c GPIOs */
-	stm32_configgpio(GPIO_I2C1_SCL);
-	stm32_configgpio(GPIO_I2C1_SDA);
+	px4_arch_configgpio(GPIO_I2C1_SCL);
+	px4_arch_configgpio(GPIO_I2C1_SDA);
 
 	/* soft-reset the block */
 	rCR1 |= I2C_CR1_SWRST;
@@ -301,12 +301,12 @@ i2c_rx_complete(void)
 			}
 
 			/* disable interrupts while reconfiguring DMA for the selected registers */
-			irqstate_t flags = irqsave();
+			irqstate_t flags = px4_enter_critical_section();
 
 			stm32_dmastop(tx_dma);
 			i2c_tx_setup();
 
-			irqrestore(flags);
+			px4_leave_critical_section(flags);
 		}
 	}
 
diff --git a/src/modules/px4iofirmware/px4io.h b/src/modules/px4iofirmware/px4io.h
index 0d394778a9..ed38b7769e 100644
--- a/src/modules/px4iofirmware/px4io.h
+++ b/src/modules/px4iofirmware/px4io.h
@@ -151,22 +151,22 @@ extern pwm_limit_t pwm_limit;
 /*
  * GPIO handling.
  */
-#define LED_BLUE(_s)			stm32_gpiowrite(GPIO_LED1, !(_s))
-#define LED_AMBER(_s)			stm32_gpiowrite(GPIO_LED2, !(_s))
-#define LED_SAFETY(_s)			stm32_gpiowrite(GPIO_LED3, !(_s))
-#define LED_RING(_s)			stm32_gpiowrite(GPIO_LED4, (_s))
+#define LED_BLUE(_s)			px4_arch_gpiowrite(GPIO_LED1, !(_s))
+#define LED_AMBER(_s)			px4_arch_gpiowrite(GPIO_LED2, !(_s))
+#define LED_SAFETY(_s)			px4_arch_gpiowrite(GPIO_LED3, !(_s))
+#define LED_RING(_s)			px4_arch_gpiowrite(GPIO_LED4, (_s))
 
 #ifdef CONFIG_ARCH_BOARD_PX4IO_V1
 
 # define PX4IO_RELAY_CHANNELS		4
-# define POWER_SERVO(_s)		stm32_gpiowrite(GPIO_SERVO_PWR_EN, (_s))
-# define POWER_ACC1(_s)			stm32_gpiowrite(GPIO_ACC1_PWR_EN, (_s))
-# define POWER_ACC2(_s)			stm32_gpiowrite(GPIO_ACC2_PWR_EN, (_s))
-# define POWER_RELAY1(_s)		stm32_gpiowrite(GPIO_RELAY1_EN, (_s))
-# define POWER_RELAY2(_s)		stm32_gpiowrite(GPIO_RELAY2_EN, (_s))
+# define POWER_SERVO(_s)		px4_arch_gpiowrite(GPIO_SERVO_PWR_EN, (_s))
+# define POWER_ACC1(_s)			px4_arch_gpiowrite(GPIO_ACC1_PWR_EN, (_s))
+# define POWER_ACC2(_s)			px4_arch_gpiowrite(GPIO_ACC2_PWR_EN, (_s))
+# define POWER_RELAY1(_s)		px4_arch_gpiowrite(GPIO_RELAY1_EN, (_s))
+# define POWER_RELAY2(_s)		px4_arch_gpiowrite(GPIO_RELAY2_EN, (_s))
 
-# define OVERCURRENT_ACC		(!stm32_gpioread(GPIO_ACC_OC_DETECT))
-# define OVERCURRENT_SERVO		(!stm32_gpioread(GPIO_SERVO_OC_DETECT))
+# define OVERCURRENT_ACC		(!px4_arch_gpioread(GPIO_ACC_OC_DETECT))
+# define OVERCURRENT_SERVO		(!px4_arch_gpioread(GPIO_SERVO_OC_DETECT))
 
 # define PX4IO_ADC_CHANNEL_COUNT	2
 # define ADC_VBATT			4
@@ -177,9 +177,9 @@ extern pwm_limit_t pwm_limit;
 #ifdef CONFIG_ARCH_BOARD_PX4IO_V2
 
 # define PX4IO_RELAY_CHANNELS		0
-# define ENABLE_SBUS_OUT(_s)		stm32_gpiowrite(GPIO_SBUS_OENABLE, !(_s))
+# define ENABLE_SBUS_OUT(_s)		px4_arch_gpiowrite(GPIO_SBUS_OENABLE, !(_s))
 
-# define VDD_SERVO_FAULT		(!stm32_gpioread(GPIO_SERVO_FAULT_DETECT))
+# define VDD_SERVO_FAULT		(!px4_arch_gpioread(GPIO_SERVO_FAULT_DETECT))
 
 # define PX4IO_ADC_CHANNEL_COUNT	2
 # define ADC_VSERVO			4
@@ -187,7 +187,7 @@ extern pwm_limit_t pwm_limit;
 
 #endif
 
-#define BUTTON_SAFETY		stm32_gpioread(GPIO_BTN_SAFETY)
+#define BUTTON_SAFETY		px4_arch_gpioread(GPIO_BTN_SAFETY)
 
 #define CONTROL_PAGE_INDEX(_group, _channel) (_group * PX4IO_CONTROL_CHANNELS + _channel)
 
diff --git a/src/modules/px4iofirmware/serial.c b/src/modules/px4iofirmware/serial.c
index 300ec0b784..8a00ef1242 100644
--- a/src/modules/px4iofirmware/serial.c
+++ b/src/modules/px4iofirmware/serial.c
@@ -104,8 +104,8 @@ interface_init(void)
 	rx_dma = stm32_dmachannel(PX4FMU_SERIAL_RX_DMA);
 
 	/* configure pins for serial use */
-	stm32_configgpio(PX4FMU_SERIAL_TX_GPIO);
-	stm32_configgpio(PX4FMU_SERIAL_RX_GPIO);
+	px4_arch_configgpio(PX4FMU_SERIAL_TX_GPIO);
+	px4_arch_configgpio(PX4FMU_SERIAL_RX_GPIO);
 
 	/* reset and configure the UART */
 	rCR1 = 0;
diff --git a/src/modules/systemlib/param/param.c b/src/modules/systemlib/param/param.c
index 02e384e0e5..ffd2d6dc0f 100644
--- a/src/modules/systemlib/param/param.c
+++ b/src/modules/systemlib/param/param.c
@@ -807,17 +807,17 @@ param_bus_lock(bool lock)
 
 	// XXX this would be the preferred locking method
 	// if (dev == nullptr) {
-	// 	dev = up_spiinitialize(PX4_SPI_BUS_BARO);
+	// 	dev = px4_spibus_initialize(PX4_SPI_BUS_BARO);
 	// }
 
 	// SPI_LOCK(dev, lock);
 
 	// we lock like this for Pixracer for now
 	if (lock) {
-		irq_state = irqsave();
+		irq_state = px4_enter_critical_section();
 
 	} else {
-		irqrestore(irq_state);
+		px4_leave_critical_section(irq_state);
 	}
 
 #endif
diff --git a/src/modules/systemlib/px4_macros.h b/src/modules/systemlib/px4_macros.h
new file mode 100644
index 0000000000..fc8079fea6
--- /dev/null
+++ b/src/modules/systemlib/px4_macros.h
@@ -0,0 +1,103 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2014 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file px4_macros.h
+ *
+ * A set of useful macros for enhanced runtime and compile time
+ * error detection and warning suppression.
+ *
+ * Define NO_BLOAT to reduce bloat from file name inclusion.
+ *
+ * The arraySize() will compute the size of an array regardless
+ * it's type
+ *
+ * INVALID_CASE(c) should be used is case statements to ferret out
+ * unintended behavior
+ *
+ * UNUSED(var) will suppress compile time warnings of unused
+ * variables
+ *
+ * CCASSERT(predicate) Will generate a compile time error it the
+ * predicate is false
+ */
+#include <assert.h>
+
+#ifndef _PX4_MACROS_H
+#define _PX4_MACROS_H
+
+
+#if !defined(arraySize)
+#define arraySize(a) (sizeof((a))/sizeof((a[0])))
+#endif
+
+#if !defined(NO_BLOAT)
+#if defined(__BASE_FILE__)
+#define _FILE_NAME_ __BASE_FILE__
+#else
+#define _FILE_NAME_ __FILE__
+#endif
+#else
+#define _FILE_NAME_ ""
+#endif
+
+#if !defined(INVALID_CASE)
+#define INVALID_CASE(c) printf("Invalid Case %d, %s:%d",(c),__BASE_FILE__,__LINE__) /* todo use PANIC */
+#endif
+
+#if !defined(UNUSED)
+#define UNUSED(var) (void)(var)
+#endif
+
+#if !defined(CAT)
+#if !defined(_CAT)
+#define _CAT(a, b) a ## b
+#endif
+#define CAT(a, b) _CAT(a, b)
+#endif
+
+#if !defined(CCASSERT)
+#if defined(static_assert)
+#		define CCASSERT(predicate) static_assert(predicate)
+#	else
+#		define CCASSERT(predicate) _x_CCASSERT_LINE(predicate, __LINE__)
+#		if !defined(_x_CCASSERT_LINE)
+#			define _x_CCASSERT_LINE(predicate, line) typedef char CAT(constraint_violated_on_line_,line)[2*((predicate)!=0)-1] __attribute__ ((unused)) ;
+#		endif
+#	endif
+#endif
+
+#define DO_PRAGMA(x) _Pragma (#x)
+#define TODO(x) DO_PRAGMA(message ("TODO - " #x))
+
+#endif /* _PX4_MACROS_H */
diff --git a/src/modules/uORB/uORBDevices_nuttx.cpp b/src/modules/uORB/uORBDevices_nuttx.cpp
index 19b814e699..7eb6e52fa4 100644
--- a/src/modules/uORB/uORBDevices_nuttx.cpp
+++ b/src/modules/uORB/uORBDevices_nuttx.cpp
@@ -183,7 +183,7 @@ uORB::DeviceNode::read(struct file *filp, char *buffer, size_t buflen)
 	/*
 	 * Perform an atomic copy & state update
 	 */
-	irqstate_t flags = irqsave();
+	irqstate_t flags = px4_enter_critical_section();
 
 	/* if the caller doesn't want the data, don't give it to them */
 	if (nullptr != buffer) {
@@ -202,7 +202,7 @@ uORB::DeviceNode::read(struct file *filp, char *buffer, size_t buflen)
 	 */
 	sd->update_reported = false;
 
-	irqrestore(flags);
+	px4_leave_critical_section(flags);
 
 	return _meta->o_size;
 }
@@ -244,7 +244,7 @@ uORB::DeviceNode::write(struct file *filp, const char *buffer, size_t buflen)
 	}
 
 	/* Perform an atomic copy. */
-	irqstate_t flags = irqsave();
+	irqstate_t flags = px4_enter_critical_section();
 	memcpy(_data, buffer, _meta->o_size);
 
 	/* update the timestamp and generation count */
@@ -253,7 +253,7 @@ uORB::DeviceNode::write(struct file *filp, const char *buffer, size_t buflen)
 
 	_published = true;
 
-	irqrestore(flags);
+	px4_leave_critical_section(flags);
 
 	/* notify any poll waiters */
 	poll_notify(POLLIN);
@@ -268,9 +268,9 @@ uORB::DeviceNode::ioctl(struct file *filp, int cmd, unsigned long arg)
 
 	switch (cmd) {
 	case ORBIOCLASTUPDATE: {
-			irqstate_t state = irqsave();
+			irqstate_t state = px4_enter_critical_section();
 			*(hrt_abstime *)arg = _last_update;
-			irqrestore(state);
+			px4_leave_critical_section(state);
 			return OK;
 		}
 
@@ -400,7 +400,7 @@ uORB::DeviceNode::appears_updated(SubscriberData *sd)
 	bool ret = false;
 
 	/* avoid racing between interrupt and non-interrupt context calls */
-	irqstate_t state = irqsave();
+	irqstate_t state = px4_enter_critical_section();
 
 	/* check if this topic has been published yet, if not bail out */
 	if (_data == nullptr) {
@@ -466,7 +466,7 @@ uORB::DeviceNode::appears_updated(SubscriberData *sd)
 	}
 
 out:
-	irqrestore(state);
+	px4_leave_critical_section(state);
 
 	/* consider it updated */
 	return ret;
diff --git a/src/modules/uavcan/sensors/baro.cpp b/src/modules/uavcan/sensors/baro.cpp
index 36f28a423c..022bb23f23 100644
--- a/src/modules/uavcan/sensors/baro.cpp
+++ b/src/modules/uavcan/sensors/baro.cpp
@@ -123,14 +123,14 @@ int UavcanBarometerBridge::ioctl(struct file *filp, int cmd, unsigned long arg)
 				return -EINVAL;
 			}
 
-			irqstate_t flags = irqsave();
+			irqstate_t flags = px4_enter_critical_section();
 
 			if (!_reports.resize(arg)) {
-				irqrestore(flags);
+				px4_leave_critical_section(flags);
 				return -ENOMEM;
 			}
 
-			irqrestore(flags);
+			px4_leave_critical_section(flags);
 
 			return OK;
 		}
diff --git a/src/modules/uavcan/uavcan_main.cpp b/src/modules/uavcan/uavcan_main.cpp
index a7b92ee7d8..7a40f825a2 100644
--- a/src/modules/uavcan/uavcan_main.cpp
+++ b/src/modules/uavcan/uavcan_main.cpp
@@ -537,12 +537,12 @@ int UavcanNode::start(uavcan::NodeID node_id, uint32_t bitrate)
 	 * fail during initialization.
 	 */
 #if defined(GPIO_CAN1_RX)
-	stm32_configgpio(GPIO_CAN1_RX);
-	stm32_configgpio(GPIO_CAN1_TX);
+	px4_arch_configgpio(GPIO_CAN1_RX);
+	px4_arch_configgpio(GPIO_CAN1_TX);
 #endif
 #if defined(GPIO_CAN2_RX)
-	stm32_configgpio(GPIO_CAN2_RX | GPIO_PULLUP);
-	stm32_configgpio(GPIO_CAN2_TX);
+	px4_arch_configgpio(GPIO_CAN2_RX | GPIO_PULLUP);
+	px4_arch_configgpio(GPIO_CAN2_TX);
 #endif
 #if !defined(GPIO_CAN1_RX) &&  !defined(GPIO_CAN2_RX)
 # error  "Need to define GPIO_CAN1_RX and/or GPIO_CAN2_RX"
diff --git a/src/platforms/posix/drivers/airspeedsim/airspeedsim.cpp b/src/platforms/posix/drivers/airspeedsim/airspeedsim.cpp
index 25bc4672fb..e3283f2fe7 100644
--- a/src/platforms/posix/drivers/airspeedsim/airspeedsim.cpp
+++ b/src/platforms/posix/drivers/airspeedsim/airspeedsim.cpp
@@ -251,13 +251,13 @@ AirspeedSim::ioctl(device::file_t *filp, int cmd, unsigned long arg)
 				return -EINVAL;
 			}
 
-			//irqstate_t flags = irqsave();
+			//irqstate_t flags = px4_enter_critical_section();
 			if (!_reports->resize(arg)) {
-				//irqrestore(flags);
+				//px4_leave_critical_section(flags);
 				return -ENOMEM;
 			}
 
-			//irqrestore(flags);
+			//px4_leave_critical_section(flags);
 
 			return OK;
 		}
diff --git a/src/platforms/px4_config.h b/src/platforms/px4_config.h
index e44fd4afce..22879330fd 100644
--- a/src/platforms/px4_config.h
+++ b/src/platforms/px4_config.h
@@ -43,6 +43,7 @@
 
 #include <nuttx/config.h>
 #include <board_config.h>
+#include "px4_micro_hal.h"
 
 #elif defined (__PX4_POSIX)
 
diff --git a/src/platforms/px4_micro_hal.h b/src/platforms/px4_micro_hal.h
new file mode 100644
index 0000000000..25017b3f0e
--- /dev/null
+++ b/src/platforms/px4_micro_hal.h
@@ -0,0 +1,53 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2016 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+#pragma once
+/*
+ * This file is a shim to bridge to nuttx_v3
+ */
+#ifdef __PX4_NUTTX
+
+#  define px4_enter_critical_section()       irqsave()
+#  define px4_leave_critical_section(flags)  irqrestore(flags)
+
+#  define px4_spibus_initialize(port_1based)       up_spiinitialize(port_1based)
+
+#  define px4_i2cbus_initialize(bus_num_1based)    up_i2cinitialize(bus_num_1based)
+#  define px4_i2cbus_uninitialize(pdev)            up_i2cuninitialize(pdev)
+
+#  if defined(CONFIG_STM32_VALUELINE) || defined(CONFIG_STM32_STM32F40XX)
+#    define px4_arch_configgpio(pinset)      stm32_configgpio(pinset)
+#    define px4_arch_unconfiggpio(pinset)     stm32_unconfiggpio(pinset)
+#    define px4_arch_gpioread(pinset)         stm32_gpioread(pinset)
+#    define px4_arch_gpiowrite(pinset, value) stm32_gpiowrite(pinset, value)
+#  endif
+#endif
diff --git a/src/systemcmds/i2c/i2c.c b/src/systemcmds/i2c/i2c.c
index 1fa60a7e47..c686591bf6 100644
--- a/src/systemcmds/i2c/i2c.c
+++ b/src/systemcmds/i2c/i2c.c
@@ -73,7 +73,7 @@ static struct i2c_dev_s *i2c;
 int i2c_main(int argc, char *argv[])
 {
 	/* find the right I2C */
-	i2c = up_i2cinitialize(PX4_I2C_BUS_ONBOARD);
+	i2c = px4_i2cbus_initialize(PX4_I2C_BUS_ONBOARD);
 
 	if (i2c == NULL) {
 		errx(1, "failed to locate I2C bus");
diff --git a/src/systemcmds/mtd/mtd.c b/src/systemcmds/mtd/mtd.c
index b38504c623..30658de117 100644
--- a/src/systemcmds/mtd/mtd.c
+++ b/src/systemcmds/mtd/mtd.c
@@ -177,7 +177,7 @@ static void
 ramtron_attach(void)
 {
 	/* initialize the right spi */
-	struct spi_dev_s *spi = up_spiinitialize(PX4_SPI_BUS_RAMTRON);
+	struct spi_dev_s *spi = px4_spibus_initialize(PX4_SPI_BUS_RAMTRON);
 
 	/* this resets the spi bus, set correct bus speed again */
 	SPI_SETFREQUENCY(spi, 10 * 1000 * 1000);
@@ -225,7 +225,7 @@ static void
 at24xxx_attach(void)
 {
 	/* find the right I2C */
-	struct i2c_dev_s *i2c = up_i2cinitialize(PX4_I2C_BUS_ONBOARD);
+	struct i2c_dev_s *i2c = px4_i2cbus_initialize(PX4_I2C_BUS_ONBOARD);
 	/* this resets the I2C bus, set correct bus speed again */
 	I2C_SETFREQUENCY(i2c, 400000);
 
diff --git a/src/systemcmds/tests/test_time.c b/src/systemcmds/tests/test_time.c
index c67f11f467..bc41fe78bc 100644
--- a/src/systemcmds/tests/test_time.c
+++ b/src/systemcmds/tests/test_time.c
@@ -121,12 +121,12 @@ int test_time(int argc, char *argv[])
 	delta = 0;
 
 	for (unsigned i = 0; i < 100; i++) {
-		uint32_t flags = irqsave();
+		uint32_t flags = px4_enter_critical_section();
 
 		h = hrt_absolute_time();
 		c = cycletime();
 
-		irqrestore(flags);
+		px4_leave_critical_section(flags);
 
 		delta += h - c;
 	}
@@ -138,12 +138,12 @@ int test_time(int argc, char *argv[])
 
 		usleep(rand());
 
-		uint32_t flags = irqsave();
+		uint32_t flags = px4_enter_critical_section();
 
 		c = cycletime();
 		h = hrt_absolute_time();
 
-		irqrestore(flags);
+		px4_leave_critical_section(flags);
 
 		delta = abs(h - c);
 		deltadelta = abs(delta - lowdelta);
diff --git a/src/systemcmds/usb_connected/usb_connected.c b/src/systemcmds/usb_connected/usb_connected.c
index d938863b62..d071fc9661 100644
--- a/src/systemcmds/usb_connected/usb_connected.c
+++ b/src/systemcmds/usb_connected/usb_connected.c
@@ -52,5 +52,5 @@ __EXPORT int usb_connected_main(int argc, char *argv[]);
 int
 usb_connected_main(int argc, char *argv[])
 {
-	return stm32_gpioread(GPIO_OTGFS_VBUS) ? 0 : 1;
+	return px4_arch_gpioread(GPIO_OTGFS_VBUS) ? 0 : 1;
 }

From ef343dc45278792eaff68f1dfa4905694d767ab5 Mon Sep 17 00:00:00 2001
From: Pavel Kirienko <pavel.kirienko@gmail.com>
Date: Sun, 29 May 2016 14:54:45 +0300
Subject: [PATCH 0388/1230] STM32 CAN driver moved from .data to heap;
 partially resolves #4677 (#4681)

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

diff --git a/src/modules/uavcan/uavcan_main.cpp b/src/modules/uavcan/uavcan_main.cpp
index 7a40f825a2..d7c3455157 100644
--- a/src/modules/uavcan/uavcan_main.cpp
+++ b/src/modules/uavcan/uavcan_main.cpp
@@ -550,25 +550,33 @@ int UavcanNode::start(uavcan::NodeID node_id, uint32_t bitrate)
 
 	/*
 	 * CAN driver init
+	 * Note that we instantiate and initialize CanInitHelper only once, because the STM32's bxCAN driver
+	 * shipped with libuavcan does not support deinitialization.
 	 */
-	static CanInitHelper can;
-	static bool can_initialized = false;
+	static CanInitHelper* can = nullptr;
 
-	if (!can_initialized) {
-		const int can_init_res = can.init(bitrate);
+	if (can == nullptr) {
+		warnx("CAN driver init...");
+
+		can = new CanInitHelper();
+
+		if (can == nullptr) {                    // We don't have exceptions so bad_alloc cannot be thrown
+			warnx("Out of memory");
+			return -1;
+		}
+
+		const int can_init_res = can->init(bitrate);
 
 		if (can_init_res < 0) {
 			warnx("CAN driver init failed %i", can_init_res);
 			return can_init_res;
 		}
-
-		can_initialized = true;
 	}
 
 	/*
 	 * Node init
 	 */
-	_instance = new UavcanNode(can.driver, uavcan_stm32::SystemClock::instance());
+	_instance = new UavcanNode(can->driver, uavcan_stm32::SystemClock::instance());
 
 	if (_instance == nullptr) {
 		warnx("Out of memory");

From b4bed73282bcb17de07a2c9e215a36d16922e8b3 Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Sun, 29 May 2016 15:19:06 +0200
Subject: [PATCH 0389/1230] Update SITL Gazebo

---
 Tools/sitl_gazebo | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/Tools/sitl_gazebo b/Tools/sitl_gazebo
index f1a66579a2..7096721ff4 160000
--- a/Tools/sitl_gazebo
+++ b/Tools/sitl_gazebo
@@ -1 +1 @@
-Subproject commit f1a66579a2a25d162886f6dd7b1c02f26eba1109
+Subproject commit 7096721ff4c0314ebeee4cb1f24a84e88f2e8c9f

From fe69be05caf485e33c96bbbfb3c61c8500beff0d Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Sun, 29 May 2016 15:54:04 +0200
Subject: [PATCH 0390/1230] INAV: Move to -Os

---
 src/modules/position_estimator_inav/CMakeLists.txt | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/src/modules/position_estimator_inav/CMakeLists.txt b/src/modules/position_estimator_inav/CMakeLists.txt
index 518acd712a..10fcf57043 100644
--- a/src/modules/position_estimator_inav/CMakeLists.txt
+++ b/src/modules/position_estimator_inav/CMakeLists.txt
@@ -36,7 +36,7 @@ px4_add_module(
 	MAIN position_estimator_inav
 	STACK_MAIN 1200
 	STACK_MAX 4000
-	COMPILE_FLAGS
+	COMPILE_FLAGS -Os
 	SRCS
 		position_estimator_inav_main.cpp
 		position_estimator_inav_params.cpp

From 1b2043b9298bceae16188e807c51de39de5ca3cc Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Sun, 29 May 2016 14:15:48 +0200
Subject: [PATCH 0391/1230] MAVLink app: Allocate buffers only as they are
 needed

---
 src/modules/mavlink/CMakeLists.txt   |  1 -
 src/modules/mavlink/mavlink_main.cpp | 22 ++++++++++++++++------
 src/modules/mavlink/mavlink_main.h   | 13 +++++--------
 3 files changed, 21 insertions(+), 15 deletions(-)

diff --git a/src/modules/mavlink/CMakeLists.txt b/src/modules/mavlink/CMakeLists.txt
index 8487dad685..bc573e500e 100644
--- a/src/modules/mavlink/CMakeLists.txt
+++ b/src/modules/mavlink/CMakeLists.txt
@@ -36,7 +36,6 @@ px4_add_module(
 	STACK_MAIN 1200
 	STACK_MAX 1500
 	COMPILE_FLAGS
-		-DMAVLINK_COMM_NUM_BUFFERS=4
 		-Os
 	SRCS
 		mavlink.c
diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp
index 42b7ba00e6..4ad617cb57 100644
--- a/src/modules/mavlink/mavlink_main.cpp
+++ b/src/modules/mavlink/mavlink_main.cpp
@@ -136,7 +136,12 @@ void mavlink_end_uart_send(mavlink_channel_t chan, int length)
  */
 mavlink_status_t *mavlink_get_channel_status(uint8_t channel)
 {
-	return Mavlink::get_status_for_instance(channel);
+	Mavlink* m = Mavlink::get_instance((unsigned)channel);
+	if (m != nullptr) {
+		return m->get_status();
+	} else {
+		return nullptr;
+	}
 }
 
 /*
@@ -144,15 +149,18 @@ mavlink_status_t *mavlink_get_channel_status(uint8_t channel)
  */
 mavlink_message_t *mavlink_get_channel_buffer(uint8_t channel)
 {
-	return Mavlink::get_buffer_for_instance(channel);
+	Mavlink* m = Mavlink::get_instance((unsigned)channel);
+	if (m != nullptr) {
+		return m->get_buffer();
+	} else {
+		return nullptr;
+	}
 }
 
 static void usage(void);
 
 bool Mavlink::_boot_complete = false;
 bool Mavlink::_config_link_on = false;
-mavlink_message_t Mavlink::_mavlink_buffer[MAVLINK_COMM_NUM_BUFFERS] = {};
-mavlink_status_t Mavlink::_mavlink_status[MAVLINK_COMM_NUM_BUFFERS] = {};
 
 Mavlink::Mavlink() :
 	_device_name("/dev/ttyS1"),
@@ -161,6 +169,8 @@ Mavlink::Mavlink() :
 	_instance_id(0),
 	_mavlink_log_pub(nullptr),
 	_task_running(false),
+	_mavlink_buffer{},
+	_mavlink_status{},
 	_hil_enabled(false),
 	_generate_rc(false),
 	_use_hil_gps(false),
@@ -2268,9 +2278,9 @@ Mavlink::start(int argc, char *argv[])
 	// before returning to the shell
 	int ic = Mavlink::instance_count();
 
-	if (ic == MAVLINK_COMM_NUM_BUFFERS) {
+	if (ic == Mavlink::MAVLINK_MAX_INSTANCES) {
 		warnx("Maximum MAVLink instance count of %d reached.",
-			(int)MAVLINK_COMM_NUM_BUFFERS);
+			(int)Mavlink::MAVLINK_MAX_INSTANCES);
 		return 1;
 	}
 
diff --git a/src/modules/mavlink/mavlink_main.h b/src/modules/mavlink/mavlink_main.h
index 2687cc2059..c9d6f414d0 100644
--- a/src/modules/mavlink/mavlink_main.h
+++ b/src/modules/mavlink/mavlink_main.h
@@ -114,13 +114,9 @@ public:
 
 	static Mavlink 		*get_instance_for_network_port(unsigned long port);
 
-	static mavlink_message_t *get_buffer_for_instance(unsigned instance) { return &_mavlink_buffer[instance]; }
+	mavlink_message_t 	*get_buffer() { return &_mavlink_buffer; }
 
-	mavlink_message_t 	*get_buffer() { return Mavlink::get_buffer_for_instance(_instance_id); }
-
-	static mavlink_status_t *get_status_for_instance(unsigned instance) { return &_mavlink_status[instance]; }
-
-	mavlink_status_t 	*get_status() { return Mavlink::get_status_for_instance(_instance_id); }
+	mavlink_status_t 	*get_status() { return &_mavlink_status; }
 
 	/**
 	 * Set the MAVLink version
@@ -404,8 +400,9 @@ private:
 	orb_advert_t		_mavlink_log_pub;
 	bool			_task_running;
 	static bool		_boot_complete;
-	static mavlink_message_t _mavlink_buffer[MAVLINK_COMM_NUM_BUFFERS];
-	static mavlink_status_t _mavlink_status[MAVLINK_COMM_NUM_BUFFERS];
+	static const unsigned MAVLINK_MAX_INSTANCES = 4;
+	mavlink_message_t _mavlink_buffer;
+	mavlink_status_t _mavlink_status;
 
 	/* states */
 	bool			_hil_enabled;		/**< Hardware In the Loop mode */

From 001a2c01f060874f8788131b934a35d406c04cde Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Sun, 29 May 2016 14:16:07 +0200
Subject: [PATCH 0392/1230] PWM out RC in app: Only reserve one MAVLink buffer

---
 src/drivers/pwm_out_rc_in/CMakeLists.txt | 1 +
 1 file changed, 1 insertion(+)

diff --git a/src/drivers/pwm_out_rc_in/CMakeLists.txt b/src/drivers/pwm_out_rc_in/CMakeLists.txt
index 08764c7f05..24faeff27f 100644
--- a/src/drivers/pwm_out_rc_in/CMakeLists.txt
+++ b/src/drivers/pwm_out_rc_in/CMakeLists.txt
@@ -35,6 +35,7 @@ px4_add_module(
 	MAIN pwm_out_rc_in
 	COMPILE_FLAGS
 		-Os
+		-DMAVLINK_COMM_NUM_BUFFERS=1
 	SRCS
 		pwm_out_rc_in.cpp
 	DEPENDS

From 49f960728938c3666e7492b5ce3191e3eb17ab3b Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Sun, 29 May 2016 14:16:27 +0200
Subject: [PATCH 0393/1230] Snapdragon RC pwm: Only reserve one MAVLink buffer

---
 src/drivers/snapdragon_rc_pwm/CMakeLists.txt        | 1 +
 src/drivers/snapdragon_rc_pwm/snapdragon_rc_pwm.cpp | 3 ++-
 2 files changed, 3 insertions(+), 1 deletion(-)

diff --git a/src/drivers/snapdragon_rc_pwm/CMakeLists.txt b/src/drivers/snapdragon_rc_pwm/CMakeLists.txt
index 6fae0f3157..254e85d931 100644
--- a/src/drivers/snapdragon_rc_pwm/CMakeLists.txt
+++ b/src/drivers/snapdragon_rc_pwm/CMakeLists.txt
@@ -35,6 +35,7 @@ px4_add_module(
 	MAIN snapdragon_rc_pwm
 	COMPILE_FLAGS
 		-Os
+		-DMAVLINK_COMM_NUM_BUFFERS=1
 	SRCS
 		snapdragon_rc_pwm.cpp
 	DEPENDS
diff --git a/src/drivers/snapdragon_rc_pwm/snapdragon_rc_pwm.cpp b/src/drivers/snapdragon_rc_pwm/snapdragon_rc_pwm.cpp
index ee82fa8f4a..bda765783e 100644
--- a/src/drivers/snapdragon_rc_pwm/snapdragon_rc_pwm.cpp
+++ b/src/drivers/snapdragon_rc_pwm/snapdragon_rc_pwm.cpp
@@ -52,6 +52,7 @@
 #include <termios.h>
 #include <uORB/topics/input_rc.h>
 #include <uORB/topics/actuator_controls.h>
+
 #include <v1.0/mavlink_types.h>
 #include <v1.0/common/mavlink.h>
 
@@ -155,7 +156,7 @@ void task_main(int argc, char *argv[])
 				mavlink_message_t msg;
 
 				for (int i = 0; i < len; ++i) {
-					if (mavlink_parse_char(MAVLINK_COMM_1, serial_buf[i], &msg, &serial_status)) {
+					if (mavlink_parse_char(MAVLINK_COMM_0, serial_buf[i], &msg, &serial_status)) {
 						// have a message, handle it
 						handle_message(&msg);
 					}

From 5198fc39e82c060687978d042203b33da2ad8995 Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Sun, 29 May 2016 14:19:08 +0200
Subject: [PATCH 0394/1230] PWM out: Fix usage of MAVLink buffer

---
 src/drivers/pwm_out_rc_in/pwm_out_rc_in.cpp | 5 +++--
 1 file changed, 3 insertions(+), 2 deletions(-)

diff --git a/src/drivers/pwm_out_rc_in/pwm_out_rc_in.cpp b/src/drivers/pwm_out_rc_in/pwm_out_rc_in.cpp
index 4b2b68775a..ffecef0915 100644
--- a/src/drivers/pwm_out_rc_in/pwm_out_rc_in.cpp
+++ b/src/drivers/pwm_out_rc_in/pwm_out_rc_in.cpp
@@ -303,8 +303,9 @@ void serial_callback(void *context, char *buffer, size_t num_bytes)
 		mavlink_message_t msg;
 
 		for (int i = 0; i < num_bytes; ++i) {
-			// TODO FIXME: we don't know if MAVLINK_COMM_1 is already taken.
-			if (mavlink_parse_char(MAVLINK_COMM_1, buffer[i], &msg, &serial_status)) {
+			// The MAVLink app doesn't use the internal buffer functions
+			// and hence the first port can be used here.
+			if (mavlink_parse_char(MAVLINK_COMM_0, buffer[i], &msg, &serial_status)) {
 				// have a message, handle it
 				if (msg.msgid == MAVLINK_MSG_ID_RC_CHANNELS) {
 					// we should publish but would be great if this works

From aebe4db52abb2a7d336553584eee1e7c7a47709f Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Sun, 29 May 2016 15:47:10 +0200
Subject: [PATCH 0395/1230] Q estimator: Optimize for size

---
 src/modules/attitude_estimator_q/CMakeLists.txt | 3 ++-
 1 file changed, 2 insertions(+), 1 deletion(-)

diff --git a/src/modules/attitude_estimator_q/CMakeLists.txt b/src/modules/attitude_estimator_q/CMakeLists.txt
index 83f17d3e81..115d0820ba 100644
--- a/src/modules/attitude_estimator_q/CMakeLists.txt
+++ b/src/modules/attitude_estimator_q/CMakeLists.txt
@@ -34,7 +34,8 @@ set(MODULE_CFLAGS)
 px4_add_module(
 	MODULE modules__attitude_estimator_q
 	MAIN attitude_estimator_q
-	COMPILE_FLAGS ${MODULE_CFLAGS}
+	COMPILE_FLAGS
+		-Os
 	STACK_MAIN 1200
 	STACK_MAX 1600
 	SRCS

From e134a683e03056ea3488be8cba62f61ee02f28cf Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Sun, 29 May 2016 16:24:36 +0200
Subject: [PATCH 0396/1230] Loggers: Be more efficient

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

diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS
index 8a4e0fb441..7b01c11c78 100644
--- a/ROMFS/px4fmu_common/init.d/rcS
+++ b/ROMFS/px4fmu_common/init.d/rcS
@@ -634,11 +634,11 @@ then
 		else
 			if param compare SYS_LOGGER 0
 			then
-				if sdlog2 start -r 100 -a -b 10 -t
+				if sdlog2 start -r 100 -a -b 9 -t
 				then
 				fi
 			else
-				if logger start -b 20
+				if logger start -b 9
 				then
 				fi
 			fi

From 4da0ddb8cb432547da38392cd0171be4d86230cb Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Sun, 29 May 2016 16:26:23 +0200
Subject: [PATCH 0397/1230] EKF1: Safe ROM space

---
 src/modules/ekf_att_pos_estimator/CMakeLists.txt | 1 +
 1 file changed, 1 insertion(+)

diff --git a/src/modules/ekf_att_pos_estimator/CMakeLists.txt b/src/modules/ekf_att_pos_estimator/CMakeLists.txt
index 1a24c4805d..4c0f4ae1a4 100644
--- a/src/modules/ekf_att_pos_estimator/CMakeLists.txt
+++ b/src/modules/ekf_att_pos_estimator/CMakeLists.txt
@@ -34,6 +34,7 @@
 px4_add_module(
 	MODULE modules__ekf_att_pos_estimator
 	MAIN ekf_att_pos_estimator
+	COMPILE_FLAGS -Os
 	STACK_MAIN 3000
 	STACK_MAX 3400
 	SRCS

From c838469db948acd8c0b087dcf6ae4c79f4f10e02 Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Sun, 29 May 2016 16:26:37 +0200
Subject: [PATCH 0398/1230] Load man: Be more efficient

---
 src/modules/load_mon/CMakeLists.txt | 1 +
 1 file changed, 1 insertion(+)

diff --git a/src/modules/load_mon/CMakeLists.txt b/src/modules/load_mon/CMakeLists.txt
index e76f6a46b7..bd435caf57 100644
--- a/src/modules/load_mon/CMakeLists.txt
+++ b/src/modules/load_mon/CMakeLists.txt
@@ -34,6 +34,7 @@ px4_add_module(
 	MODULE modules__load_mon
 	MAIN load_mon
 	STACK_MAIN 1200
+	COMPILE_FLAGS -Os
 	SRCS
 		load_mon.cpp
 	DEPENDS

From 4d4f8d25c2efff82998cc9abf0820aaef28763b3 Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Sun, 29 May 2016 16:26:47 +0200
Subject: [PATCH 0399/1230] Logger: Be more efficient

---
 src/modules/logger/CMakeLists.txt | 4 +---
 1 file changed, 1 insertion(+), 3 deletions(-)

diff --git a/src/modules/logger/CMakeLists.txt b/src/modules/logger/CMakeLists.txt
index cb88ca6eaf..c918621779 100644
--- a/src/modules/logger/CMakeLists.txt
+++ b/src/modules/logger/CMakeLists.txt
@@ -36,9 +36,7 @@ px4_add_module(
 	MAIN logger
 	PRIORITY "SCHED_PRIORITY_MAX-30"
 	STACK_MAIN 2200
-	COMPILE_FLAGS
-		${MODULE_CFLAGS}
-		-Os
+	COMPILE_FLAGS -Os
 	SRCS
 		logger.cpp
 		log_writer.cpp

From 8032787faa0bbeeadfd9ce2acaed36414225f399 Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Sun, 29 May 2016 16:27:01 +0200
Subject: [PATCH 0400/1230] MC att control: Be more efficient

---
 src/modules/mc_att_control/CMakeLists.txt | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/src/modules/mc_att_control/CMakeLists.txt b/src/modules/mc_att_control/CMakeLists.txt
index 20aec10ac2..6071940370 100644
--- a/src/modules/mc_att_control/CMakeLists.txt
+++ b/src/modules/mc_att_control/CMakeLists.txt
@@ -35,7 +35,7 @@ px4_add_module(
 	MAIN mc_att_control
 	STACK_MAIN 1200
 	STACK_MAX 3500
-	COMPILE_FLAGS
+	COMPILE_FLAGS -Os
 	SRCS
 		mc_att_control_main.cpp
 	DEPENDS

From 1e9fae830156e66122018a909437b576ac404def Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Sun, 29 May 2016 16:27:14 +0200
Subject: [PATCH 0401/1230] MC pos control: Be more efficient

---
 src/modules/mc_pos_control/CMakeLists.txt | 1 +
 1 file changed, 1 insertion(+)

diff --git a/src/modules/mc_pos_control/CMakeLists.txt b/src/modules/mc_pos_control/CMakeLists.txt
index 5d58414732..0de1f49235 100644
--- a/src/modules/mc_pos_control/CMakeLists.txt
+++ b/src/modules/mc_pos_control/CMakeLists.txt
@@ -33,6 +33,7 @@
 px4_add_module(
 	MODULE modules__mc_pos_control
 	MAIN mc_pos_control
+	COMPILE_FLAGS -Os
 	STACK_MAIN 1200
 	SRCS
 		mc_pos_control_main.cpp

From ba9e9397aa685db8f6f270e4829d7f53f57abd40 Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Sun, 29 May 2016 16:27:27 +0200
Subject: [PATCH 0402/1230] Navigator: Be more efficient

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

diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp
index 812fe49821..e74e75905e 100644
--- a/src/modules/navigator/navigator_main.cpp
+++ b/src/modules/navigator/navigator_main.cpp
@@ -640,7 +640,7 @@ Navigator::start()
 	_navigator_task = px4_task_spawn_cmd("navigator",
 					 SCHED_DEFAULT,
 					 SCHED_PRIORITY_DEFAULT + 5,
-					 1500,
+					 1300,
 					 (px4_main_t)&Navigator::task_main_trampoline,
 					 nullptr);
 

From 0354ada5d377c0f535a394cfda636d9079631206 Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Sun, 29 May 2016 16:27:39 +0200
Subject: [PATCH 0403/1230] Sensors: be more efficient

---
 src/modules/sensors/CMakeLists.txt | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/src/modules/sensors/CMakeLists.txt b/src/modules/sensors/CMakeLists.txt
index e26df79246..a23f3c5341 100644
--- a/src/modules/sensors/CMakeLists.txt
+++ b/src/modules/sensors/CMakeLists.txt
@@ -37,7 +37,7 @@ px4_add_module(
 	PRIORITY "SCHED_PRIORITY_MAX-5"
 	STACK_MAIN 1300
 	COMPILE_FLAGS
-		-O3
+		-Os
 	SRCS
 		sensors.cpp
 		sensors_init.cpp

From 9dd050b3930f2bc0ecafa6f820affb31ef5de2f0 Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Sun, 29 May 2016 16:27:50 +0200
Subject: [PATCH 0404/1230] VTOL: Be more efficient

---
 src/modules/vtol_att_control/CMakeLists.txt            | 3 ++-
 src/modules/vtol_att_control/vtol_att_control_main.cpp | 2 +-
 2 files changed, 3 insertions(+), 2 deletions(-)

diff --git a/src/modules/vtol_att_control/CMakeLists.txt b/src/modules/vtol_att_control/CMakeLists.txt
index 7e424e55b6..24b2b2c6f0 100644
--- a/src/modules/vtol_att_control/CMakeLists.txt
+++ b/src/modules/vtol_att_control/CMakeLists.txt
@@ -33,7 +33,8 @@
 px4_add_module(
 	MODULE modules__vtol_att_control
 	MAIN vtol_att_control
-	COMPILE_FLAGS
+	STACK_MAIN 1300
+	COMPILE_FLAGS -Os
 	SRCS
 		vtol_att_control_main.cpp
 		tiltrotor.cpp
diff --git a/src/modules/vtol_att_control/vtol_att_control_main.cpp b/src/modules/vtol_att_control/vtol_att_control_main.cpp
index 890a09a840..3e86813861 100644
--- a/src/modules/vtol_att_control/vtol_att_control_main.cpp
+++ b/src/modules/vtol_att_control/vtol_att_control_main.cpp
@@ -830,7 +830,7 @@ VtolAttitudeControl::start()
 	_control_task = px4_task_spawn_cmd("vtol_att_control",
 					   SCHED_DEFAULT,
 					   SCHED_PRIORITY_MAX - 10,
-					   2048,
+					   1100,
 					   (px4_main_t)&VtolAttitudeControl::task_main_trampoline,
 					   nullptr);
 

From aa961c8d284831cb9480bdd88040b87860b1c126 Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Sun, 29 May 2016 16:28:15 +0200
Subject: [PATCH 0405/1230] FMUv2: Allow LPE config

---
 cmake/configs/nuttx_px4fmu-v2_default.cmake | 5 ++---
 1 file changed, 2 insertions(+), 3 deletions(-)

diff --git a/cmake/configs/nuttx_px4fmu-v2_default.cmake b/cmake/configs/nuttx_px4fmu-v2_default.cmake
index cf17ffce66..8f5f7f5e7e 100644
--- a/cmake/configs/nuttx_px4fmu-v2_default.cmake
+++ b/cmake/configs/nuttx_px4fmu-v2_default.cmake
@@ -76,7 +76,7 @@ set(config_module_list
 	modules/load_mon
 	modules/navigator
 	modules/mavlink
-	modules/gpio_led
+	#modules/gpio_led
 	modules/uavcan
 	modules/land_detector
 
@@ -86,12 +86,11 @@ set(config_module_list
 	modules/attitude_estimator_q
 	modules/ekf_att_pos_estimator
 	modules/position_estimator_inav
-	#modules/local_position_estimator
+	modules/local_position_estimator
 
 	#
 	# Vehicle Control
 	#
-	# modules/segway # XXX Needs GCC 4.7 fix
 	modules/fw_pos_control_l1
 	modules/fw_att_control
 	modules/mc_att_control

From 4a0d7808a55bac06e12be9194aec0b1ecef657fd Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Sun, 29 May 2016 16:34:23 +0200
Subject: [PATCH 0406/1230] Remove LPE config

---
 Makefile                                |   4 -
 cmake/configs/nuttx_px4fmu-v2_lpe.cmake | 196 ------------------------
 2 files changed, 200 deletions(-)
 delete mode 100644 cmake/configs/nuttx_px4fmu-v2_lpe.cmake

diff --git a/Makefile b/Makefile
index 382e8ded72..d9b3f6b801 100644
--- a/Makefile
+++ b/Makefile
@@ -155,9 +155,6 @@ px4-stm32f4discovery_default:
 px4fmu-v2_ekf2:
 	$(call cmake-build,nuttx_px4fmu-v2_ekf2)
 
-px4fmu-v2_lpe:
-	$(call cmake-build,nuttx_px4fmu-v2_lpe)
-
 mindpx-v2_default:
 	$(call cmake-build,nuttx_mindpx-v2_default)
 
@@ -251,7 +248,6 @@ checks_tests: \
 	check_px4fmu-v2_test
 
 checks_alts: \
-	check_px4fmu-v2_lpe \
 	check_px4fmu-v2_ekf2 \
 
 checks_uavcan: \
diff --git a/cmake/configs/nuttx_px4fmu-v2_lpe.cmake b/cmake/configs/nuttx_px4fmu-v2_lpe.cmake
deleted file mode 100644
index dce99d090c..0000000000
--- a/cmake/configs/nuttx_px4fmu-v2_lpe.cmake
+++ /dev/null
@@ -1,196 +0,0 @@
-include(nuttx/px4_impl_nuttx)
-
-set(CMAKE_TOOLCHAIN_FILE ${CMAKE_SOURCE_DIR}/cmake/toolchains/Toolchain-arm-none-eabi.cmake)
-
-set(config_uavcan_num_ifaces 2)
-
-set(config_module_list
-	#
-	# Board support modules
-	#
-	drivers/device
-	drivers/stm32
-	drivers/stm32/adc
-	drivers/stm32/tone_alarm
-	drivers/led
-	drivers/px4fmu
-	drivers/px4io
-	drivers/boards/px4fmu-v2
-	drivers/rgbled
-	drivers/mpu6000
-	drivers/mpu9250
-	drivers/lsm303d
-	drivers/l3gd20
-	drivers/hmc5883
-	drivers/ms5611
-	#drivers/mb12xx
-	drivers/srf02
-	drivers/sf0x
-	drivers/ll40ls
-	drivers/trone
-	drivers/gps
-	drivers/pwm_out_sim
-	#drivers/hott
-	#drivers/hott/hott_telemetry
-	#drivers/hott/hott_sensors
-	drivers/blinkm
-	drivers/airspeed
-	drivers/ets_airspeed
-	drivers/meas_airspeed
-	drivers/frsky_telemetry
-	modules/sensors
-	#drivers/mkblctrl
-	drivers/px4flow
-	#drivers/oreoled
-	drivers/gimbal
-	drivers/pwm_input
-	drivers/camera_trigger
-	drivers/bst
-	drivers/snapdragon_rc_pwm
-	drivers/lis3mdl
-	drivers/bmi160
-
-	#
-	# System commands
-	#
-	systemcmds/bl_update
-	systemcmds/mixer
-	systemcmds/param
-	systemcmds/perf
-	systemcmds/pwm
-	systemcmds/esc_calib
-	systemcmds/reboot
-	#systemcmds/topic_listener
-	systemcmds/top
-	systemcmds/config
-	systemcmds/nshterm
-	systemcmds/mtd
-	systemcmds/dumpfile
-	systemcmds/ver
-
-	#
-	# General system control
-	#
-	modules/commander
-	modules/load_mon
-	modules/navigator
-	modules/mavlink
-	modules/gpio_led
-	modules/uavcan
-	modules/land_detector
-
-	#
-	# Estimation modules (EKF/ SO3 / other filters)
-	#
-	modules/attitude_estimator_q
-	#modules/ekf_att_pos_estimator
-	#modules/position_estimator_inav
-	modules/local_position_estimator
-
-	#
-	# Vehicle Control
-	#
-	# modules/segway # XXX Needs GCC 4.7 fix
-	modules/fw_pos_control_l1
-	modules/fw_att_control
-	modules/mc_att_control
-	modules/mc_pos_control
-	modules/vtol_att_control
-
-	#
-	# Logging
-	#
-	modules/logger
-	modules/sdlog2
-
-	#
-	# Library modules
-	#
-	modules/param
-	modules/systemlib
-	modules/systemlib/mixer
-	modules/uORB
-	modules/dataman
-
-	#
-	# Libraries
-	#
-	lib/controllib
-	lib/mathlib
-	lib/mathlib/math/filter
-	lib/ecl
-	lib/external_lgpl
-	lib/geo
-	lib/geo_lookup
-	lib/conversion
-	lib/launchdetection
-	lib/terrain_estimation
-	lib/runway_takeoff
-	lib/tailsitter_recovery
-	lib/DriverFramework/framework
-	platforms/nuttx
-
-	# had to add for cmake, not sure why wasn't in original config
-	platforms/common 
-	platforms/nuttx/px4_layer
-
-	#
-	# OBC challenge
-	#
-	#modules/bottle_drop
-
-	#
-	# Rover apps
-	#
-	#examples/rover_steering_control
-
-	#
-	# Demo apps
-	#
-	#examples/math_demo
-	# Tutorial code from
-	# https://px4.io/dev/px4_simple_app
-	#examples/px4_simple_app
-
-	# Tutorial code from
-	# https://px4.io/dev/daemon
-	#examples/px4_daemon_app
-
-	# Tutorial code from
-	# https://px4.io/dev/debug_values
-	#examples/px4_mavlink_debug
-
-	# Tutorial code from
-	# https://px4.io/dev/example_fixedwing_control
-	#examples/fixedwing_control
-
-	# Hardware test
-	#examples/hwtest
-)
-
-set(config_extra_builtin_cmds
-	serdis
-	sercon
-	)
-
-set(config_io_board
-	px4io-v2
-	)
-
-set(config_extra_libs
-	uavcan
-	uavcan_stm32_driver
-	)
-
-set(config_io_extra_libs
-	)
-
-add_custom_target(sercon)
-set_target_properties(sercon PROPERTIES
-	PRIORITY "SCHED_PRIORITY_DEFAULT"
-	MAIN "sercon" STACK_MAIN "2048")
-
-add_custom_target(serdis)
-set_target_properties(serdis PROPERTIES
-	PRIORITY "SCHED_PRIORITY_DEFAULT"
-	MAIN "serdis" STACK_MAIN "2048")

From 977eb2de17820e8160c1f024e1922a62ce26e802 Mon Sep 17 00:00:00 2001
From: Kabir Mohammed <mhkabir98@gmail.com>
Date: Mon, 30 May 2016 13:12:15 +0530
Subject: [PATCH 0407/1230] Fix missing dprintf on QURT

---
 src/modules/systemlib/print_load_posix.c | 5 +++++
 1 file changed, 5 insertions(+)

diff --git a/src/modules/systemlib/print_load_posix.c b/src/modules/systemlib/print_load_posix.c
index 0ac8a615e1..f47c466103 100644
--- a/src/modules/systemlib/print_load_posix.c
+++ b/src/modules/systemlib/print_load_posix.c
@@ -54,6 +54,11 @@
 #include <mach/mach.h>
 #endif
 
+#ifdef __PX4_QURT
+// dprintf is not available on QURT. Use the usual output to mini-dm.
+#define dprintf(_fd, _text, ...) ((_fd) == 1 ? PX4_INFO((_text), ##__VA_ARGS__) : (void)(_fd))
+#endif
+
 extern struct system_load_s system_load;
 
 #define CL "\033[K" // clear line

From 95a705776829940468626e9774498555458bb4c0 Mon Sep 17 00:00:00 2001
From: Julian Oes <julian@oes.ch>
Date: Mon, 30 May 2016 15:26:02 +0100
Subject: [PATCH 0408/1230] cmake: fix merge mistake

---
 cmake/configs/qurt_sdflight_default.cmake | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/cmake/configs/qurt_sdflight_default.cmake b/cmake/configs/qurt_sdflight_default.cmake
index 3ea8ad37b4..195219f7af 100644
--- a/cmake/configs/qurt_sdflight_default.cmake
+++ b/cmake/configs/qurt_sdflight_default.cmake
@@ -60,7 +60,7 @@ set(config_module_list
 	# PX4 drivers
 	#
 	drivers/gps
-	drivers/uart_esc
+	drivers/pwm_out_rc_in
 	drivers/qshell/qurt
 
 	#

From 8c9b27254559f843423f2d9dbe697761f444672d Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Beat=20K=C3=BCng?= <beat-kueng@gmx.net>
Date: Mon, 30 May 2016 21:53:37 +0200
Subject: [PATCH 0409/1230] mavlink udp: avoid spamming the console when
 disconnecting the Network or bcast addr not found (#4611)

---
 src/modules/commander/commander.cpp     |  2 +-
 src/modules/mavlink/mavlink_main.cpp    | 13 ++++++++++---
 src/modules/mavlink/mavlink_main.h      |  2 ++
 src/modules/mavlink/mavlink_mission.cpp |  2 +-
 4 files changed, 14 insertions(+), 5 deletions(-)

diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp
index 4f6e2327f1..c8cc22ffab 100644
--- a/src/modules/commander/commander.cpp
+++ b/src/modules/commander/commander.cpp
@@ -3613,7 +3613,7 @@ void *commander_low_prio_loop(void *arg)
 					if (ret != OK) {
 						mavlink_and_console_log_critical(&mavlink_log_pub, "settings auto save error");
 					} else {
-						warnx("settings saved.");
+						PX4_INFO("commander: settings saved.");
 					}
 
 					need_param_autosave = false;
diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp
index 4ad617cb57..c76d39074e 100644
--- a/src/modules/mavlink/mavlink_main.cpp
+++ b/src/modules/mavlink/mavlink_main.cpp
@@ -223,6 +223,8 @@ Mavlink::Mavlink() :
 	_bcast_addr{},
 	_src_addr_initialized(false),
 	_broadcast_address_found(false),
+	_broadcast_address_not_found_warned(false),
+	_sendto_result(1),
 	_network_buf{},
 	_network_buf_len(0),
 #endif
@@ -899,9 +901,10 @@ Mavlink::send_packet()
 
 				int bret = sendto(_socket_fd, _network_buf, _network_buf_len, 0, (struct sockaddr *)&_bcast_addr, sizeof(_bcast_addr));
 
-				if (bret <= 0) {
-					PX4_WARN("sending broadcast failed, errno: %d: %s", errno, strerror(errno));
+				if (bret <= 0 && _sendto_result > 0) {
+					PX4_ERR("sending broadcast failed, errno: %d: %s", errno, strerror(errno));
 				}
+				_sendto_result = bret;
 			}
 		}
 
@@ -1150,9 +1153,13 @@ Mavlink::find_broadcast_address()
 		if (setsockopt(_socket_fd, SOL_SOCKET, SO_BROADCAST, &broadcast_opt, sizeof(broadcast_opt)) < 0) {
 			PX4_WARN("setting broadcast permission failed");
 		}
+		_broadcast_address_not_found_warned = false;
 
 	} else {
-		PX4_WARN("no broadcasting address found");
+		if (!_broadcast_address_not_found_warned) {
+			PX4_WARN("no broadcasting address found");
+			_broadcast_address_not_found_warned = true;
+		}
 	}
 
 	delete[] ifconf.ifc_req;
diff --git a/src/modules/mavlink/mavlink_main.h b/src/modules/mavlink/mavlink_main.h
index c9d6f414d0..d47289a32a 100644
--- a/src/modules/mavlink/mavlink_main.h
+++ b/src/modules/mavlink/mavlink_main.h
@@ -478,6 +478,8 @@ private:
 	struct sockaddr_in _bcast_addr;
 	bool _src_addr_initialized;
 	bool _broadcast_address_found;
+	bool _broadcast_address_not_found_warned;
+	int _sendto_result;
 	uint8_t _network_buf[MAVLINK_MAX_PACKET_LEN];
 	unsigned _network_buf_len;
 #endif
diff --git a/src/modules/mavlink/mavlink_mission.cpp b/src/modules/mavlink/mavlink_mission.cpp
index 05c62e02e3..976f4e94a1 100644
--- a/src/modules/mavlink/mavlink_mission.cpp
+++ b/src/modules/mavlink/mavlink_mission.cpp
@@ -131,7 +131,7 @@ MavlinkMissionManager::init_offboard_mission()
 			_dataman_id = 0;
 			_count = 0;
 			_current_seq = 0;
-			warnx("offboard mission init: ERROR");
+			warnx("offboard mission init failed");
 		}
 	}
 	_my_dataman_id = _dataman_id;

From 292b35d06de3c3b1fa06ab006818d1be3fcf225a Mon Sep 17 00:00:00 2001
From: Roman Bapst <bapstr@ethz.ch>
Date: Mon, 30 May 2016 23:31:06 +0200
Subject: [PATCH 0410/1230] updated matrix lib: (#4631)

- better documentation in header files
---
 src/lib/matrix | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/src/lib/matrix b/src/lib/matrix
index 07fba8322a..3c87ae78ff 160000
--- a/src/lib/matrix
+++ b/src/lib/matrix
@@ -1 +1 @@
-Subproject commit 07fba8322a16cb2dac47e6a8ac7d21ec346313ff
+Subproject commit 3c87ae78ff4d80d1643b72afd2783478e87b8e5b

From d9422e02960e84fc4e34faa35c9f10e32818972b Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Michael=20Sch=C3=A4uble?= <schaeuble.michael@googlemail.com>
Date: Tue, 31 May 2016 13:04:22 +0200
Subject: [PATCH 0411/1230] Add Parrot Bebop as build target (#4698)

* Add parrot bebop build structure

* Add upload functionality to bebop build

* Add modules and commands to bebop build
---
 Makefile                                |  3 +
 Tools/adb_upload_to_bebop.sh            | 32 +++++++++
 cmake/common/px4_base.cmake             | 18 +++++
 cmake/configs/posix_bebop_default.cmake | 96 +++++++++++++++++++++++++
 cmake/posix/px4_impl_posix.cmake        | 13 +++-
 posix-configs/bebop/mainapp.config      | 15 ++++
 src/firmware/posix/CMakeLists.txt       | 35 +++++++++
 src/lib/version/version.h               |  2 +
 8 files changed, 212 insertions(+), 2 deletions(-)
 create mode 100755 Tools/adb_upload_to_bebop.sh
 create mode 100644 cmake/configs/posix_bebop_default.cmake
 create mode 100644 posix-configs/bebop/mainapp.config

diff --git a/Makefile b/Makefile
index d9b3f6b801..27eb42e9a8 100644
--- a/Makefile
+++ b/Makefile
@@ -205,6 +205,9 @@ posix_rpi2_default:
 posix_rpi2_release:
 	$(call cmake-build,$@)
 
+posix_bebop_default:
+	$(call cmake-build,$@)
+
 posix: posix_sitl_default
 
 broadcast: posix_sitl_broadcast
diff --git a/Tools/adb_upload_to_bebop.sh b/Tools/adb_upload_to_bebop.sh
new file mode 100755
index 0000000000..64e87b6712
--- /dev/null
+++ b/Tools/adb_upload_to_bebop.sh
@@ -0,0 +1,32 @@
+#!/bin/bash
+
+ip=192.168.42.1
+port=9050
+
+echo "Connecting to bebop: $ip:$port"
+
+# adb returns also 0 as exit status if the connection fails
+adb_return=$(adb connect $ip:$port)
+adb_status=$(echo $adb_return | cut -f 1 -d " ")
+
+if [[ $adb_status == "unable" ]]; then
+  
+  echo ""
+  echo "Connection with Parrot Bebop could not be established:"
+  echo "  Make sure you are connected with the Bebop's WiFi and"
+  echo "  enable access to the board by pressing the power button 4 times."
+  echo ""
+  exit 50
+
+fi
+
+echo "Connection successfully established"
+
+sleep 1
+
+adb shell mount -o remount,rw /
+
+../Tools/adb_upload.sh $@
+
+echo "Disconnecting from bebop"
+adb disconnect
diff --git a/cmake/common/px4_base.cmake b/cmake/common/px4_base.cmake
index 210dcbcdf8..1b721b1401 100644
--- a/cmake/common/px4_base.cmake
+++ b/cmake/common/px4_base.cmake
@@ -528,6 +528,24 @@ function(px4_add_adb_push)
 		)
 endfunction()
 
+function(px4_add_adb_push_to_bebop)
+	px4_parse_function_args(
+		NAME px4_add_upload_to_bebop
+		ONE_VALUE OS BOARD OUT DEST
+		MULTI_VALUE FILES DEPENDS
+		REQUIRED OS BOARD OUT FILES DEPENDS DEST
+		ARGN ${ARGN})
+
+	add_custom_target(${OUT}
+		COMMAND ${CMAKE_SOURCE_DIR}/Tools/adb_upload_to_bebop.sh ${FILES} ${DEST}
+		DEPENDS ${DEPENDS}
+		WORKING_DIRECTORY ${CMAKE_BINARY_DIR}
+		COMMENT "uploading ${BUNDLE}"
+		VERBATIM
+		USES_TERMINAL
+		)
+endfunction()
+
 function(px4_add_scp_push)
 	px4_parse_function_args(
 		NAME px4_add_upload
diff --git a/cmake/configs/posix_bebop_default.cmake b/cmake/configs/posix_bebop_default.cmake
new file mode 100644
index 0000000000..fdb1d3eb8c
--- /dev/null
+++ b/cmake/configs/posix_bebop_default.cmake
@@ -0,0 +1,96 @@
+include(posix/px4_impl_posix)
+
+set(CMAKE_TOOLCHAIN_FILE ${CMAKE_SOURCE_DIR}/cmake/toolchains/Toolchain-arm-linux-gnueabihf-raspbian.cmake)
+
+add_definitions(
+  -D__PX4_POSIX_BEBOP
+	)
+
+set(CMAKE_PROGRAM_PATH
+	"${RPI_TOOLCHAIN_DIR}/gcc-linaro-arm-linux-gnueabihf-raspbian/bin"
+	${CMAKE_PROGRAM_PATH}
+	)
+
+set(config_module_list
+
+  examples/px4_simple_app
+
+	#
+	# Board support modules
+	#
+	drivers/device
+	modules/sensors
+
+	#
+	# System commands
+	#
+	systemcmds/param
+	systemcmds/mixer
+	systemcmds/ver
+	systemcmds/esc_calib
+	systemcmds/topic_listener
+	systemcmds/perf
+
+	#
+	# Estimation modules (EKF/ SO3 / other filters)
+	#
+	#modules/attitude_estimator_ekf
+	modules/ekf_att_pos_estimator
+	modules/attitude_estimator_q
+	modules/position_estimator_inav
+	modules/local_position_estimator
+	modules/ekf2
+
+	#
+	# Vehicle Control
+	#
+	modules/mc_att_control
+	modules/mc_pos_control
+	modules/fw_att_control
+	modules/fw_pos_control_l1	
+	modules/vtol_att_control
+
+	#
+	# Library modules
+	#
+	modules/sdlog2
+	modules/logger
+	modules/commander
+	modules/load_mon
+	modules/param
+	modules/systemlib
+	modules/systemlib/mixer
+	modules/uORB
+	modules/dataman
+	modules/land_detector
+	modules/navigator
+	modules/mavlink
+
+	#
+	# PX4 drivers
+	#
+
+	#
+	# Libraries
+	#
+	lib/controllib
+	lib/mathlib
+	lib/mathlib/math/filter
+	lib/geo
+	lib/ecl
+	lib/geo_lookup
+	lib/launchdetection
+	lib/external_lgpl
+	lib/conversion
+	lib/terrain_estimation
+	lib/runway_takeoff
+	lib/tailsitter_recovery
+	lib/DriverFramework/framework
+
+	#
+	# POSIX
+	#
+	platforms/common
+	platforms/posix/px4_layer
+	platforms/posix/work_queue
+)
diff --git a/cmake/posix/px4_impl_posix.cmake b/cmake/posix/px4_impl_posix.cmake
index 51afab20de..d661bfa5eb 100644
--- a/cmake/posix/px4_impl_posix.cmake
+++ b/cmake/posix/px4_impl_posix.cmake
@@ -166,6 +166,15 @@ function(px4_os_add_flags)
                 mavlink/include/mavlink
                 )
 
+# Use the pthread instead of lpthread if the firmware is build for the parrot
+# bebop. This resolves some linker errors in DriverFramework, when building a 
+# static target.
+if ("${BOARD}" STREQUAL "bebop")
+  set(PX4_PTHREAD_BUILD "-pthread")
+else()
+  set(PX4_PTHREAD_BUILD "-lpthread")
+endif()
+
 if(UNIX AND APPLE)
         set(added_definitions
 		-D__PX4_POSIX
@@ -177,7 +186,7 @@ if(UNIX AND APPLE)
                 )
 
         set(added_exe_linker_flags
-		-lpthread
+          ${PX4_PTHREAD_BUILD}
 		)
 
 else()
@@ -192,7 +201,7 @@ else()
                 )
 
         set(added_exe_linker_flags
-		-lpthread -lrt
+		      ${PX4_PTHREAD_BUILD} -lrt
 		)
 
 endif()
diff --git a/posix-configs/bebop/mainapp.config b/posix-configs/bebop/mainapp.config
new file mode 100644
index 0000000000..bd0cbbf4b7
--- /dev/null
+++ b/posix-configs/bebop/mainapp.config
@@ -0,0 +1,15 @@
+uorb start
+param set SYS_AUTOSTART 4001
+sleep 1
+param set MAV_TYPE 2
+sensors start
+commander start
+ekf2 start
+land_detector start multicopter
+mc_pos_control start
+mc_att_control start
+mavlink start -u 14556 -r 1000000
+sleep 1
+mavlink stream -u 14556 -s HIGHRES_IMU -r 50
+mavlink stream -u 14556 -s ATTITUDE -r 50
+mavlink boot_complete
diff --git a/src/firmware/posix/CMakeLists.txt b/src/firmware/posix/CMakeLists.txt
index 7a28919ed8..0da29d151f 100644
--- a/src/firmware/posix/CMakeLists.txt
+++ b/src/firmware/posix/CMakeLists.txt
@@ -61,7 +61,42 @@ elseif ("${BOARD}" STREQUAL "rpi2")
 			 DEPENDS mainapp
 			 DEST /home/pi)
 
+elseif ("${BOARD}" STREQUAL "bebop")
+
+	set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -static")
+	set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -static")
+
+	add_executable(mainapp
+		${CMAKE_SOURCE_DIR}/src/platforms/posix/main.cpp
+		apps.h
+		)
+
+	if (NOT APPLE)
+		target_link_libraries(mainapp
+			-Wl,--start-group
+			${module_libraries}
+			${df_driver_libs}
+			pthread m rt
+			-Wl,--end-group
+			)
+	else()
+		target_link_libraries(mainapp
+			${module_libraries}
+			${df_driver_libs}
+			pthread m
+			)
+	endif()
+
+	px4_add_adb_push_to_bebop(OUT upload
+			 OS ${OS}
+			 BOARD ${BOARD}
+			 FILES ${CMAKE_CURRENT_BINARY_DIR}/mainapp
+						 ${CMAKE_SOURCE_DIR}/posix-configs/bebop/mainapp.config
+			 DEPENDS mainapp
+			 DEST /usr/bin)
+			 
 else()
+
 	add_executable(mainapp
 		${CMAKE_SOURCE_DIR}/src/platforms/posix/main.cpp
 		apps.h
diff --git a/src/lib/version/version.h b/src/lib/version/version.h
index 6767f1b453..4a529f2751 100644
--- a/src/lib/version/version.h
+++ b/src/lib/version/version.h
@@ -61,6 +61,8 @@ __END_DECLS
 #  define HW_ARCH "LINUXTEST"
 #elif defined(CONFIG_ARCH_BOARD_RPI2)
 #  define	HW_ARCH "LINUXTEST"
+#elif defined(CONFIG_ARCH_BOARD_BEBOP)
+#  define	HW_ARCH "LINUXTEST"
 #else
 #define HW_ARCH (board_name())
 #endif

From c6250657ebfdd9a819cd4d10ec9fefd756987cdc Mon Sep 17 00:00:00 2001
From: jwilson <jwilson@qti.qualcomm.com>
Date: Thu, 26 May 2016 21:21:37 -0700
Subject: [PATCH 0412/1230] Added mag support to the DriverFramework mpu9250
 driver.  Shortened parameter names for legacy drivers.  Added temporary
 ifdef's in the calibration code for Snapdragon Flight builds.

Signed-off-by: jwilson <jwilson@qti.qualcomm.com>
---
 .../posix_eagle_legacy_driver_default.cmake   |   6 +-
 .../qurt_eagle_legacy_driver_default.cmake    |   8 +-
 posix-configs/eagle/200qx/px4-calib.config    |   6 +-
 .../eagle/200qx/px4-flight-v1-board.config    |  16 +-
 .../eagle/200qx/px4-flight-v2-board.config    |  16 +-
 .../eagle/210qc/mainapp-calib.config          |   8 -
 .../eagle/210qc/mainapp-flight.config         |   7 -
 posix-configs/eagle/210qc/px4-calib.config    |  72 -------
 posix-configs/eagle/210qc/px4-flight.config   |  97 ----------
 .../commander/accelerometer_calibration.cpp   |   8 +-
 src/modules/commander/gyro_calibration.cpp    |   8 +-
 src/modules/sensors/sensors.cpp               |   3 -
 .../df_mpu9250_wrapper/df_mpu9250_wrapper.cpp | 177 +++++++++++++++++-
 13 files changed, 202 insertions(+), 230 deletions(-)
 delete mode 100644 posix-configs/eagle/210qc/mainapp-calib.config
 delete mode 100644 posix-configs/eagle/210qc/mainapp-flight.config
 delete mode 100644 posix-configs/eagle/210qc/px4-calib.config
 delete mode 100644 posix-configs/eagle/210qc/px4-flight.config

diff --git a/cmake/configs/posix_eagle_legacy_driver_default.cmake b/cmake/configs/posix_eagle_legacy_driver_default.cmake
index ed6cd83c16..c3cbd6e475 100644
--- a/cmake/configs/posix_eagle_legacy_driver_default.cmake
+++ b/cmake/configs/posix_eagle_legacy_driver_default.cmake
@@ -1,6 +1,6 @@
 include(posix/px4_impl_posix)
 
-set(CMAKE_TOOLCHAIN_FILE ${CMAKE_SOURCE_DIR}/cmake/toolchains/Toolchain-arm-linux-gnueabihf.cmake)
+set(CMAKE_TOOLCHAIN_FILE ${CMAKE_SOURCE_DIR}/cmake/cmake_hexagon/toolchain/Toolchain-arm-linux-gnueabihf.cmake)
 
 set(CMAKE_MODULE_PATH ${CMAKE_MODULE_PATH} "${CMAKE_SOURCE_DIR}/cmake/cmake_hexagon")
 
@@ -12,7 +12,7 @@ set(CONFIG_SHMEM "1")
 # or if it is for the Snapdragon.
 add_definitions(
 	-D__PX4_POSIX_EAGLE
-   -D__USING_SNAPDRAGON_LEGACY_DRIVER
+    -D__USING_SNAPDRAGON_LEGACY_DRIVER
 	)
 
 set(config_module_list
@@ -48,6 +48,8 @@ set(config_module_list
 	modules/logger
 	modules/simulator
 	modules/commander
+	modules/navigator
+	modules/load_mon
 
 	lib/controllib
 	lib/mathlib
diff --git a/cmake/configs/qurt_eagle_legacy_driver_default.cmake b/cmake/configs/qurt_eagle_legacy_driver_default.cmake
index ad73ffb12d..2ccd5123d5 100644
--- a/cmake/configs/qurt_eagle_legacy_driver_default.cmake
+++ b/cmake/configs/qurt_eagle_legacy_driver_default.cmake
@@ -16,6 +16,8 @@ set(CMAKE_MODULE_PATH ${CMAKE_MODULE_PATH} "${CMAKE_SOURCE_DIR}/cmake/cmake_hexa
 
 add_definitions(
    -D__USING_SNAPDRAGON_LEGACY_DRIVER
+   -D__PX4_QURT
+   -D__PX4_QURT_EAGLE
    )
 
 set(config_module_list
@@ -26,8 +28,6 @@ set(config_module_list
 	modules/sensors
 	platforms/posix/drivers/df_mpu9250_wrapper
 	platforms/posix/drivers/df_bmp280_wrapper
-	platforms/posix/drivers/df_hmc5883_wrapper
-	platforms/posix/drivers/df_trone_wrapper
 
 	#
 	# System commands
@@ -37,7 +37,6 @@ set(config_module_list
 	#
 	# Estimation modules (EKF/ SO3 / other filters)
 	#
-	#modules/attitude_estimator_ekf
 	modules/ekf_att_pos_estimator
 	modules/attitude_estimator_q
 	modules/position_estimator_inav
@@ -72,7 +71,6 @@ set(config_module_list
 	#
 	platforms/qurt/fc_addon/rc_receiver
 	platforms/qurt/fc_addon/uart_esc
-	#platforms/qurt/fc_addon/mpu_spi
 
 	#
 	# Libraries
@@ -105,6 +103,4 @@ set(config_module_list
 set(config_df_driver_list
 	mpu9250
 	bmp280
-	hmc5883
-	trone
 	)
diff --git a/posix-configs/eagle/200qx/px4-calib.config b/posix-configs/eagle/200qx/px4-calib.config
index cabe21c4a0..74685189de 100644
--- a/posix-configs/eagle/200qx/px4-calib.config
+++ b/posix-configs/eagle/200qx/px4-calib.config
@@ -59,9 +59,9 @@ param set MC_ROLLRATE_D 0.0001
 param set ATT_W_MAG 0.00
 param set PE_MAG_NOISE 1.0f
 param set SENS_BOARD_ROT 0
-param set MPU_GYRO_LPF_ENM 4
-param set MPU_ACC_LPF_ENM 4
-param set MPU_SAMPLE_R_ENM 2
+param set MPU_GYR_LPF_ENUM 4
+param set MPU_ACC_LPF_ENUM 4
+param set MPU_SMPRATE_ENUM 2
 sleep 1
 mpu9x50 start -D /dev/spi-1
 uart_esc start -D /dev/tty-2
diff --git a/posix-configs/eagle/200qx/px4-flight-v1-board.config b/posix-configs/eagle/200qx/px4-flight-v1-board.config
index f07aa5d026..fa8ead0e46 100644
--- a/posix-configs/eagle/200qx/px4-flight-v1-board.config
+++ b/posix-configs/eagle/200qx/px4-flight-v1-board.config
@@ -51,15 +51,15 @@ param set RC6_REV 1
 param set ATT_W_MAG 0.00
 param set PE_MAG_NOISE 1.0f
 param set SENS_BOARD_ROT 6
-param set MPU_GYRO_LPF_ENM 4
-param set MPU_ACC_LPF_ENM 4
-param set MPU_SAMPLE_R_ENM 2
+param set MPU_GYR_LPF_ENUM 4
+param set MPU_ACC_LPF_ENUM 4
+param set MPU_SMPRATE_ENUM 2
 param set UART_ESC_MODEL 0
-param set UART_ESC_BAUD 250000
-param set UART_ESC_MOTOR1 2
-param set UART_ESC_MOTOR2 4
-param set UART_ESC_MOTOR3 1
-param set UART_ESC_MOTOR4 3
+param set UART_ESC_BAUDRAT 250000
+param set UART_ESC_PX4MOT1 2
+param set UART_ESC_PX4MOT2 4
+param set UART_ESC_PX4MOT3 1
+param set UART_ESC_PX4MOT4 3
 sleep 1
 commander start
 rc_receiver start -D /dev/tty-1
diff --git a/posix-configs/eagle/200qx/px4-flight-v2-board.config b/posix-configs/eagle/200qx/px4-flight-v2-board.config
index 90fb8c2a0c..c201cee7fc 100644
--- a/posix-configs/eagle/200qx/px4-flight-v2-board.config
+++ b/posix-configs/eagle/200qx/px4-flight-v2-board.config
@@ -51,15 +51,15 @@ param set RC6_REV 1
 param set ATT_W_MAG 0.00
 param set PE_MAG_NOISE 1.0f
 param set SENS_BOARD_ROT 0
-param set MPU_GYRO_LPF_ENM 4
-param set MPU_ACC_LPF_ENM 4
-param set MPU_SAMPLE_R_ENM 2
+param set MPU_GYR_LPF_ENUM 4
+param set MPU_ACC_LPF_ENUM 4
+param set MPU_SMPRATE_ENUM 2
 param set UART_ESC_MODEL 0
-param set UART_ESC_BAUD 250000
-param set UART_ESC_MOTOR1 2
-param set UART_ESC_MOTOR2 4
-param set UART_ESC_MOTOR3 1
-param set UART_ESC_MOTOR4 3
+param set UART_ESC_BAUDRAT 250000
+param set UART_ESC_PX4MOT1 2
+param set UART_ESC_PX4MOT2 4
+param set UART_ESC_PX4MOT3 1
+param set UART_ESC_PX4MOT4 3
 sleep 1
 commander start
 rc_receiver start -D /dev/tty-1
diff --git a/posix-configs/eagle/210qc/mainapp-calib.config b/posix-configs/eagle/210qc/mainapp-calib.config
deleted file mode 100644
index 7130d85ded..0000000000
--- a/posix-configs/eagle/210qc/mainapp-calib.config
+++ /dev/null
@@ -1,8 +0,0 @@
-uorb start
-muorb start
-mavlink start -u 14556
-sleep 1
-mavlink stream -u 14556 -s HIGHRES_IMU -r 50
-mavlink stream -u 14556 -s ATTITUDE -r 50
-mavlink boot_complete
-commander start
diff --git a/posix-configs/eagle/210qc/mainapp-flight.config b/posix-configs/eagle/210qc/mainapp-flight.config
deleted file mode 100644
index b511bb1121..0000000000
--- a/posix-configs/eagle/210qc/mainapp-flight.config
+++ /dev/null
@@ -1,7 +0,0 @@
-uorb start
-muorb start
-mavlink start -u 14556
-sleep 1
-mavlink stream -u 14556 -s HIGHRES_IMU -r 50
-mavlink stream -u 14556 -s ATTITUDE -r 50
-mavlink boot_complete
diff --git a/posix-configs/eagle/210qc/px4-calib.config b/posix-configs/eagle/210qc/px4-calib.config
deleted file mode 100644
index cabe21c4a0..0000000000
--- a/posix-configs/eagle/210qc/px4-calib.config
+++ /dev/null
@@ -1,72 +0,0 @@
-uorb start
-param set CAL_GYRO0_ID 2293760
-param set CAL_ACC0_ID 1310720
-param set CAL_ACC1_ID 1376256
-param set CAL_MAG0_ID 196608
-param set RC_RECEIVER_TYPE 1
-rc_receiver start -D /dev/tty-1
-attitude_estimator_q start
-position_estimator_inav start
-mc_pos_control start
-mc_att_control start
-sleep 1
-param set SYS_AUTOSTART 4001
-param set SYS_AUTOCONFIG 1
-param set MAV_TYPE 2​
-param set RC_MAP_THROTTLE 1
-param set RC_MAP_ROLL 2
-param set RC_MAP_PITCH 3
-param set RC_MAP_YAW 4
-param set RC_MAP_MODE_SW 5
-param set RC_MAP_POSCTL_SW 6
-param set RC1_MAX 1900
-param set RC1_MIN 1099
-param set RC1_TRIM 1099
-param set RC1_REV 1
-param set RC2_MAX 1900
-param set RC2_MIN 1099
-param set RC2_TRIM 1500
-param set RC2_REV -1
-param set RC3_MAX 1900
-param set RC3_MIN 1099
-param set RC3_TRIM 1500
-param set RC3_REV 1
-param set RC4_MAX 1900
-param set RC4_MIN 1099
-param set RC4_TRIM 1500
-param set RC4_REV -1
-param set RC5_MAX 1900
-param set RC5_MIN 1099
-param set RC5_TRIM 1500
-param set RC5_REV 1
-param set RC6_MAX 1900
-param set RC6_MIN 1099
-param set RC6_TRIM 1099
-param set RC6_REV 1
-sensors start
-param set MC_YAW_P 7.0
-param set MC_YAWRATE_P 0.08
-param set MC_YAWRATE_I 0.0
-param set MC_YAWRATE_D 0
-param set MC_PITCH_P 7.0
-param set MC_PITCHRATE_P 0.08
-param set MC_PITCHRATE_I 0.0
-param set MC_PITCHRATE_D 0.0001
-param set MC_ROLL_P 7.0
-param set MC_ROLLRATE_P 0.08
-param set MC_ROLLRATE_I 0.0
-param set MC_ROLLRATE_D 0.0001
-param set ATT_W_MAG 0.00
-param set PE_MAG_NOISE 1.0f
-param set SENS_BOARD_ROT 0
-param set MPU_GYRO_LPF_ENM 4
-param set MPU_ACC_LPF_ENM 4
-param set MPU_SAMPLE_R_ENM 2
-sleep 1
-mpu9x50 start -D /dev/spi-1
-uart_esc start -D /dev/tty-2
-csr_gps start -D /dev/tty-3
-list_devices
-list_files
-list_tasks
-list_topics
diff --git a/posix-configs/eagle/210qc/px4-flight.config b/posix-configs/eagle/210qc/px4-flight.config
deleted file mode 100644
index 0a83b68fae..0000000000
--- a/posix-configs/eagle/210qc/px4-flight.config
+++ /dev/null
@@ -1,97 +0,0 @@
-uorb start
-param set CAL_GYRO0_ID 2293760
-param set CAL_ACC0_ID 1310720
-param set CAL_ACC1_ID 1376256
-param set CAL_MAG0_ID 196608
-param set MAV_TYPE 2
-param set MC_YAW_P 12
-param set MC_YAWRATE_P 0.08
-param set MC_YAWRATE_I 0.0
-param set MC_YAWRATE_D 0
-param set MC_PITCH_P 7.0
-param set MC_PITCHRATE_P 0.08
-param set MC_PITCHRATE_I 0.0
-param set MC_PITCHRATE_D 0.001
-param set MC_ROLL_P 7.0
-param set MC_ROLLRATE_P 0.08
-param set MC_ROLLRATE_I 0.0
-param set MC_ROLLRATE_D 0.001
-param set RC_MAP_THROTTLE 1
-param set RC_MAP_ROLL 2
-param set RC_MAP_PITCH 3
-param set RC_MAP_YAW 4
-param set RC_MAP_MODE_SW 5
-param set RC_MAP_POSCTL_SW 6
-param set RC1_MAX 1900
-param set RC1_MIN 1099
-param set RC1_TRIM 1099
-param set RC1_REV 1
-param set RC2_MAX 1900
-param set RC2_MIN 1099
-param set RC2_TRIM 1500
-param set RC2_REV -1
-param set RC3_MAX 1900
-param set RC3_MIN 1099
-param set RC3_TRIM 1500
-param set RC3_REV 1
-param set RC4_MAX 1900
-param set RC4_MIN 1099
-param set RC4_TRIM 1500
-param set RC4_REV -1
-param set RC5_MAX 1900
-param set RC5_MIN 1099
-param set RC5_TRIM 1500
-param set RC5_REV 1
-param set RC6_MAX 1900
-param set RC6_MIN 1099
-param set RC6_TRIM 1099
-param set RC6_REV 1
-param set ATT_W_MAG 0.00
-param set PE_MAG_NOISE 1.0f
-param set SENS_BOARD_ROT 0
-param set MPU_GYRO_LPF_ENM 4
-param set MPU_ACC_LPF_ENM 4
-param set MPU_SAMPLE_R_ENM 2
-param set GYRO0_XOFF -0.0028
-param set GYRO0_YOFF -0.0047
-param set GYRO0_ZOFF -0.0034
-param set GYRO0_XSCALE 1.0000
-param set GYRO0_YSCALE 1.0000
-param set GYRO0_ZSCALE 1.0000
-param set MAG0_XOFF 0.0000
-param set MAG0_YOFF 0.0000
-param set MAG0_ZOFF 0.0000
-param set MAG0_XSCALE 1.0000
-param set MAG0_YSCALE 1.0000
-param set MAG0_ZSCALE 1.0000
-param set ACC0_XOFF -0.0941
-param set ACC0_YOFF 0.1168
-param set ACC0_ZOFF -0.0398
-param set ACC0_XSCALE 1.0004
-param set ACC0_YSCALE 0.9974
-param set ACC0_ZSCALE 0.9951
-param set RC_RECEIVER_TYPE 1
-param set UART_ESC_MODEL 2
-param set UART_ESC_BAUD 250000
-param set UART_ESC_MOTOR1 4
-param set UART_ESC_MOTOR2 2
-param set UART_ESC_MOTOR3 1
-param set UART_ESC_MOTOR4 3
-sleep 1
-commander start
-rc_receiver start -D /dev/tty-1
-sleep 1
-mpu9x50 start -D /dev/spi-1
-df_bmp280_wrapper start
-sensors start
-attitude_estimator_q start
-position_estimator_inav start
-mc_pos_control start
-mc_att_control start
-land_detector start multicopter
-sleep 1
-uart_esc start -D /dev/tty-2
-list_devices
-list_files
-list_tasks
-list_topics
diff --git a/src/modules/commander/accelerometer_calibration.cpp b/src/modules/commander/accelerometer_calibration.cpp
index 4ce21a6dbd..6384d415aa 100644
--- a/src/modules/commander/accelerometer_calibration.cpp
+++ b/src/modules/commander/accelerometer_calibration.cpp
@@ -175,7 +175,7 @@ typedef struct  {
 
 int do_accel_calibration(orb_advert_t *mavlink_log_pub)
 {
-#ifndef __PX4_QURT
+#if !defined(__PX4_QURT) && !defined(__PX4_POSIX_EAGLE)
 	int fd;
 #endif
 
@@ -195,7 +195,7 @@ int do_accel_calibration(orb_advert_t *mavlink_log_pub)
 
 	/* reset all sensors */
 	for (unsigned s = 0; s < max_accel_sens; s++) {
-#ifndef __PX4_QURT
+#if !defined(__PX4_QURT) && !defined(__PX4_POSIX_EAGLE)
 		sprintf(str, "%s%u", ACCEL_BASE_DEVICE_PATH, s);
 		/* reset all offsets to zero and all scales to one */
 		fd = px4_open(str, 0);
@@ -329,7 +329,7 @@ int do_accel_calibration(orb_advert_t *mavlink_log_pub)
 			return ERROR;
 		}
 
-#ifndef __PX4_QURT
+#if !defined(__PX4_QURT) && !defined(__PX4_POSIX_EAGLE)
 		sprintf(str, "%s%u", ACCEL_BASE_DEVICE_PATH, i);
 		fd = px4_open(str, 0);
 
@@ -419,7 +419,7 @@ calibrate_return do_accel_calibration_measurements(orb_advert_t *mavlink_log_pub
 			break;
 		}
 
-#ifdef __PX4_QURT
+#if defined(__PX4_QURT) || defined(__PX4_POSIX_EAGLE)
 		// For QURT respectively the driver framework, we need to get the device ID by copying one report.
 		struct accel_report	accel_report;
 		orb_copy(ORB_ID(sensor_accel), worker_data.subs[i], &accel_report);
diff --git a/src/modules/commander/gyro_calibration.cpp b/src/modules/commander/gyro_calibration.cpp
index 2f93432bc9..a9b005c0f7 100644
--- a/src/modules/commander/gyro_calibration.cpp
+++ b/src/modules/commander/gyro_calibration.cpp
@@ -186,7 +186,7 @@ int do_gyro_calibration(orb_advert_t *mavlink_log_pub)
 
 		// Reset all offsets to 0 and scales to 1
 		(void)memcpy(&worker_data.gyro_scale[s], &gyro_scale_zero, sizeof(gyro_scale_zero));
-#ifndef __PX4_QURT
+#if !defined(__PX4_QURT) && !defined(__PX4_POSIX_EAGLE)
 		sprintf(str, "%s%u", GYRO_BASE_DEVICE_PATH, s);
 		int fd = px4_open(str, 0);
 		if (fd >= 0) {
@@ -239,9 +239,9 @@ int do_gyro_calibration(orb_advert_t *mavlink_log_pub)
 	for (unsigned s = 0; s < gyro_count; s++) {
 
 		worker_data.gyro_sensor_sub[s] = orb_subscribe_multi(ORB_ID(sensor_gyro), s);
-#ifdef __PX4_QURT
+#if defined(__PX4_QURT) || defined(__PX4_POSIX_EAGLE)
 		// For QURT respectively the driver framework, we need to get the device ID by copying one report.
-		struct gyro_report	gyro_report;
+		struct gyro_report gyro_report;
 		orb_copy(ORB_ID(sensor_gyro), worker_data.gyro_sensor_sub[s], &gyro_report);
 		worker_data.device_id[s] = gyro_report.device_id;
 #endif
@@ -337,7 +337,7 @@ int do_gyro_calibration(orb_advert_t *mavlink_log_pub)
 				(void)sprintf(str, "CAL_GYRO%u_ID", s);
 				failed |= (OK != param_set_no_notification(param_find(str), &(worker_data.device_id[s])));
 
-#ifndef __PX4_QURT
+#if !defined(__PX4_QURT) && !defined(__PX4_POSIX_EAGLE)
 				/* apply new scaling and offsets */
 				(void)sprintf(str, "%s%u", GYRO_BASE_DEVICE_PATH, s);
 				int fd = px4_open(str, 0);
diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp
index cbc3e062ac..116911bb3a 100644
--- a/src/modules/sensors/sensors.cpp
+++ b/src/modules/sensors/sensors.cpp
@@ -46,9 +46,6 @@
  * @author Anton Babushkin <anton@px4.io>
  */
 
-// TODO-JYW: TESTING-TESTING
-#define DEBUG_BUILD 1
-
 #include <board_config.h>
 
 #include <px4_config.h>
diff --git a/src/platforms/posix/drivers/df_mpu9250_wrapper/df_mpu9250_wrapper.cpp b/src/platforms/posix/drivers/df_mpu9250_wrapper/df_mpu9250_wrapper.cpp
index 0f4fe127c0..e31ba33d02 100644
--- a/src/platforms/posix/drivers/df_mpu9250_wrapper/df_mpu9250_wrapper.cpp
+++ b/src/platforms/posix/drivers/df_mpu9250_wrapper/df_mpu9250_wrapper.cpp
@@ -58,6 +58,7 @@
 #include <drivers/drv_hrt.h>
 #include <drivers/drv_accel.h>
 #include <drivers/drv_gyro.h>
+#include <drivers/drv_mag.h>
 #include <drivers/device/integrator.h>
 
 #include <uORB/topics/parameter_update.h>
@@ -77,7 +78,7 @@ using namespace DriverFramework;
 class DfMpu9250Wrapper : public MPU9250
 {
 public:
-	DfMpu9250Wrapper(/*enum Rotation rotation*/);
+	DfMpu9250Wrapper(bool mag_enabled);
 	~DfMpu9250Wrapper();
 
 
@@ -105,11 +106,13 @@ private:
 
 	void _update_accel_calibration();
 	void _update_gyro_calibration();
+	void _update_mag_calibration();
 
 	//enum Rotation		_rotation;
 
 	orb_advert_t		    _accel_topic;
 	orb_advert_t		    _gyro_topic;
+	orb_advert_t		    _mag_topic;
 
 	orb_advert_t		    _mavlink_log_pub;
 
@@ -133,11 +136,22 @@ private:
 		float z_scale;
 	} _gyro_calibration;
 
+	struct mag_calibration_s {
+		float x_offset;
+		float x_scale;
+		float y_offset;
+		float y_scale;
+		float z_offset;
+		float z_scale;
+	} _mag_calibration;
+
 	int			    _accel_orb_class_instance;
 	int			    _gyro_orb_class_instance;
+	int			    _mag_orb_class_instance;
 
 	Integrator		    _accel_int;
 	Integrator		    _gyro_int;
+	Integrator		    _mag_int;
 
 	unsigned		    _publish_count;
 
@@ -147,24 +161,31 @@ private:
 	perf_counter_t		    _fifo_corruption_counter;
 	perf_counter_t		    _gyro_range_hit_counter;
 	perf_counter_t		    _accel_range_hit_counter;
+	perf_counter_t		    _mag_range_hit_counter;
 	perf_counter_t		    _publish_perf;
 
 	hrt_abstime		    _last_accel_range_hit_time;
 	uint64_t		    _last_accel_range_hit_count;
+
+	bool _mag_enabled;
 };
 
-DfMpu9250Wrapper::DfMpu9250Wrapper(/*enum Rotation rotation*/) :
-	MPU9250(IMU_DEVICE_PATH),
+DfMpu9250Wrapper::DfMpu9250Wrapper(bool mag_enabled) :
+	MPU9250(IMU_DEVICE_PATH, mag_enabled),
 	_accel_topic(nullptr),
 	_gyro_topic(nullptr),
+	_mag_topic(nullptr),
 	_mavlink_log_pub(nullptr),
 	_param_update_sub(-1),
 	_accel_calibration{},
 	_gyro_calibration{},
+	_mag_calibration{},
 	_accel_orb_class_instance(-1),
 	_gyro_orb_class_instance(-1),
+	_mag_orb_class_instance(-1),
 	_accel_int(MPU9250_NEVER_AUTOPUBLISH_US, false),
 	_gyro_int(MPU9250_NEVER_AUTOPUBLISH_US, true),
+	_mag_int(MPU9250_NEVER_AUTOPUBLISH_US, true),
 	/*_rotation(rotation)*/
 	_publish_count(0),
 	_read_counter(perf_alloc(PC_COUNT, "mpu9250_reads")),
@@ -173,9 +194,11 @@ DfMpu9250Wrapper::DfMpu9250Wrapper(/*enum Rotation rotation*/) :
 	_fifo_corruption_counter(perf_alloc(PC_COUNT, "mpu9250_fifo_corruptions")),
 	_gyro_range_hit_counter(perf_alloc(PC_COUNT, "mpu9250_gyro_range_hits")),
 	_accel_range_hit_counter(perf_alloc(PC_COUNT, "mpu9250_accel_range_hits")),
+	_mag_range_hit_counter(perf_alloc(PC_COUNT, "mpu9250_mag_range_hits")),
 	_publish_perf(perf_alloc(PC_ELAPSED, "mpu9250_publish")),
 	_last_accel_range_hit_time(0),
-	_last_accel_range_hit_count(0)
+	_last_accel_range_hit_count(0),
+	_mag_enabled(mag_enabled)
 {
 	// Set sane default calibration values
 	_accel_calibration.x_scale = 1.0f;
@@ -191,6 +214,15 @@ DfMpu9250Wrapper::DfMpu9250Wrapper(/*enum Rotation rotation*/) :
 	_gyro_calibration.x_offset = 0.0f;
 	_gyro_calibration.y_offset = 0.0f;
 	_gyro_calibration.z_offset = 0.0f;
+
+	if (_mag_enabled == true) {
+		_mag_calibration.x_scale = 1.0f;
+		_mag_calibration.y_scale = 1.0f;
+		_mag_calibration.z_scale = 1.0f;
+		_mag_calibration.x_offset = 0.0f;
+		_mag_calibration.y_offset = 0.0f;
+		_mag_calibration.z_offset = 0.0f;
+	}
 }
 
 DfMpu9250Wrapper::~DfMpu9250Wrapper()
@@ -201,6 +233,9 @@ DfMpu9250Wrapper::~DfMpu9250Wrapper()
 	perf_free(_fifo_corruption_counter);
 	perf_free(_gyro_range_hit_counter);
 	perf_free(_accel_range_hit_counter);
+	if (_mag_enabled == true) {
+		perf_free(_mag_range_hit_counter);
+	}
 	perf_free(_publish_perf);
 }
 
@@ -226,6 +261,18 @@ int DfMpu9250Wrapper::start()
 		return -1;
 	}
 
+	if (_mag_enabled == true) {
+		// TODO: don't publish garbage here
+		mag_report mag_report = {};
+		_mag_topic = orb_advertise_multi(ORB_ID(sensor_mag), &mag_report,
+						  &_mag_orb_class_instance, ORB_PRIO_DEFAULT);
+
+		if (_mag_topic == nullptr) {
+			PX4_ERR("sensor_mag advert fail");
+			return -1;
+		}
+	}
+
 	/* Subscribe to param update topic. */
 	if (_param_update_sub < 0) {
 		_param_update_sub = orb_subscribe(ORB_ID(parameter_update));
@@ -251,6 +298,7 @@ int DfMpu9250Wrapper::start()
 	/* Force getting the calibration values. */
 	_update_accel_calibration();
 	_update_gyro_calibration();
+	_update_mag_calibration();
 
 	return 0;
 }
@@ -276,6 +324,9 @@ void DfMpu9250Wrapper::info()
 	perf_print_counter(_fifo_corruption_counter);
 	perf_print_counter(_gyro_range_hit_counter);
 	perf_print_counter(_accel_range_hit_counter);
+	if (_mag_enabled == true) {
+		perf_print_counter(_mag_range_hit_counter);
+	}
 	perf_print_counter(_publish_perf);
 }
 
@@ -430,6 +481,83 @@ void DfMpu9250Wrapper::_update_accel_calibration()
 	_accel_calibration.z_offset = 0.0f;
 }
 
+void DfMpu9250Wrapper::_update_mag_calibration()
+{
+	if (_mag_enabled == false) {
+		return;
+	}
+
+	// TODO: replace magic number
+	for (unsigned i = 0; i < 3; ++i) {
+
+		// TODO: remove printfs and add error counter
+
+		char str[30];
+		(void)sprintf(str, "CAL_MAG%u_ID", i);
+		int32_t device_id;
+		int res = param_get(param_find(str), &device_id);
+
+		if (res != OK) {
+			PX4_ERR("Could not access param %s", str);
+			continue;
+		}
+
+		if ((uint32_t)device_id != m_id.dev_id) {
+			continue;
+		}
+
+		(void)sprintf(str, "CAL_MAG%u_XSCALE", i);
+		res = param_get(param_find(str), &_mag_calibration.x_scale);
+
+		if (res != OK) {
+			PX4_ERR("Could not access param %s", str);
+		}
+
+		(void)sprintf(str, "CAL_MAG%u_YSCALE", i);
+		res = param_get(param_find(str), &_mag_calibration.y_scale);
+
+		if (res != OK) {
+			PX4_ERR("Could not access param %s", str);
+		}
+
+		(void)sprintf(str, "CAL_MAG%u_ZSCALE", i);
+		res = param_get(param_find(str), &_mag_calibration.z_scale);
+
+		if (res != OK) {
+			PX4_ERR("Could not access param %s", str);
+		}
+
+		(void)sprintf(str, "CAL_MAG%u_XOFF", i);
+		res = param_get(param_find(str), &_mag_calibration.x_offset);
+
+		if (res != OK) {
+			PX4_ERR("Could not access param %s", str);
+		}
+
+		(void)sprintf(str, "CAL_MAG%u_YOFF", i);
+		res = param_get(param_find(str), &_mag_calibration.y_offset);
+
+		if (res != OK) {
+			PX4_ERR("Could not access param %s", str);
+		}
+
+		(void)sprintf(str, "CAL_MAG%u_ZOFF", i);
+		res = param_get(param_find(str), &_mag_calibration.z_offset);
+
+		if (res != OK) {
+			PX4_ERR("Could not access param %s", str);
+		}
+	}
+
+	// Set sane default calibration values
+	_mag_calibration.x_scale = 1.0f;
+	_mag_calibration.y_scale = 1.0f;
+	_mag_calibration.z_scale = 1.0f;
+	_mag_calibration.x_offset = 0.0f;
+	_mag_calibration.y_offset = 0.0f;
+	_mag_calibration.z_offset = 0.0f;
+}
+
 int DfMpu9250Wrapper::_publish(struct imu_sensor_data &data)
 {
 	/* Check if calibration values are still up-to-date. */
@@ -442,6 +570,7 @@ int DfMpu9250Wrapper::_publish(struct imu_sensor_data &data)
 
 		_update_accel_calibration();
 		_update_gyro_calibration();
+		_update_mag_calibration();
 	}
 
 	math::Vector<3> vec_integrated_unused;
@@ -491,13 +620,20 @@ int DfMpu9250Wrapper::_publish(struct imu_sensor_data &data)
 	perf_set_count(_fifo_corruption_counter, data.fifo_overflow_counter);
 	perf_set_count(_gyro_range_hit_counter, data.gyro_range_hit_counter);
 	perf_set_count(_accel_range_hit_counter, data.accel_range_hit_counter);
+	if (_mag_enabled == true) {
+		perf_set_count(_mag_range_hit_counter, data.mag_range_hit_counter);
+	}
 
 	perf_begin(_publish_perf);
 
 	accel_report accel_report = {};
 	gyro_report gyro_report = {};
+	mag_report mag_report = {};
 
 	accel_report.timestamp = gyro_report.timestamp = hrt_absolute_time();
+	if (_mag_enabled == true) {
+		mag_report.timestamp = accel_report.timestamp;
+	}
 
 	// TODO: get these right
 	gyro_report.scaling = -1.0f;
@@ -508,6 +644,12 @@ int DfMpu9250Wrapper::_publish(struct imu_sensor_data &data)
 	accel_report.range_m_s2 = -1.0f;
 	accel_report.device_id = m_id.dev_id;
 
+	if (_mag_enabled == true) {
+		mag_report.scaling = -1.0f;
+		mag_report.range_ga = -1.0f;
+		mag_report.device_id = m_id.dev_id;
+	}
+
 	// TODO: remove these (or get the values)
 	gyro_report.x_raw = NAN;
 	gyro_report.y_raw = NAN;
@@ -517,6 +659,12 @@ int DfMpu9250Wrapper::_publish(struct imu_sensor_data &data)
 	accel_report.y_raw = NAN;
 	accel_report.z_raw = NAN;
 
+	if (_mag_enabled == true) {
+		mag_report.x_raw = NAN;
+		mag_report.y_raw = NAN;
+		mag_report.z_raw = NAN;
+	}
+
 	math::Vector<3> gyro_val_filt;
 	math::Vector<3> accel_val_filt;
 
@@ -533,6 +681,12 @@ int DfMpu9250Wrapper::_publish(struct imu_sensor_data &data)
 	accel_report.y = accel_val_filt(1);
 	accel_report.z = accel_val_filt(2);
 
+	if (_mag_enabled == true) {
+		mag_report.x = (data.mag_ga_x - _mag_calibration.x_offset) * _mag_calibration.x_scale;
+		mag_report.y = (data.mag_ga_y - _mag_calibration.y_offset) * _mag_calibration.y_scale;
+		mag_report.z = (data.mag_ga_z - _mag_calibration.z_offset) * _mag_calibration.z_scale;
+	}
+
 	gyro_report.x_integral = gyro_val_integ(0);
 	gyro_report.y_integral = gyro_val_integ(1);
 	gyro_report.z_integral = gyro_val_integ(2);
@@ -552,6 +706,10 @@ int DfMpu9250Wrapper::_publish(struct imu_sensor_data &data)
 			orb_publish(ORB_ID(sensor_accel), _accel_topic, &accel_report);
 		}
 
+		if ((_mag_topic != nullptr) && (_mag_enabled == true)) {
+			orb_publish(ORB_ID(sensor_mag), _mag_topic, &mag_report);
+		}
+
 		/* Notify anyone waiting for data. */
 		DevMgr::updateNotify(*this);
 
@@ -588,9 +746,9 @@ int stop();
 int info();
 void usage();
 
-int start(/*enum Rotation rotation*/)
+int start(bool mag_enabled)
 {
-	g_dev = new DfMpu9250Wrapper(/*rotation*/);
+	g_dev = new DfMpu9250Wrapper(mag_enabled);
 
 	if (g_dev == nullptr) {
 		PX4_ERR("failed instantiating DfMpu9250Wrapper object");
@@ -695,9 +853,12 @@ df_mpu9250_wrapper_main(int argc, char *argv[])
 
 	const char *verb = argv[myoptind];
 
+	if (!strcmp(verb, "start_with_mag")) {
+		ret = df_mpu9250_wrapper::start(true);
+	}
 
-	if (!strcmp(verb, "start")) {
-		ret = df_mpu9250_wrapper::start(/*rotation*/);
+	else if (!strcmp(verb, "start")) {
+		ret = df_mpu9250_wrapper::start(false);
 	}
 
 	else if (!strcmp(verb, "stop")) {

From 67ea3d6ec624fa957c4cbc8d710dd9194674557a Mon Sep 17 00:00:00 2001
From: jwilson <jwilson@qti.qualcomm.com>
Date: Thu, 26 May 2016 21:47:19 -0700
Subject: [PATCH 0413/1230] Fixing function which retrievs mag calibration
 values.

Signed-off-by: jwilson <jwilson@qti.qualcomm.com>
---
 .../posix/drivers/df_mpu9250_wrapper/df_mpu9250_wrapper.cpp    | 3 +++
 1 file changed, 3 insertions(+)

diff --git a/src/platforms/posix/drivers/df_mpu9250_wrapper/df_mpu9250_wrapper.cpp b/src/platforms/posix/drivers/df_mpu9250_wrapper/df_mpu9250_wrapper.cpp
index e31ba33d02..85ebcea5a5 100644
--- a/src/platforms/posix/drivers/df_mpu9250_wrapper/df_mpu9250_wrapper.cpp
+++ b/src/platforms/posix/drivers/df_mpu9250_wrapper/df_mpu9250_wrapper.cpp
@@ -547,6 +547,9 @@ void DfMpu9250Wrapper::_update_mag_calibration()
 		if (res != OK) {
 			PX4_ERR("Could not access param %s", str);
 		}
+
+		// We got calibration values, let's exit.
+		return;
 	}
 
 	// Set sane default calibration values

From 76095ebba5ef92cb9ea5cc853b3997ddc6bdaac2 Mon Sep 17 00:00:00 2001
From: jwilson <jwilson@qti.qualcomm.com>
Date: Fri, 27 May 2016 15:28:29 -0700
Subject: [PATCH 0414/1230] Responding to PR feedback.  All items resolved.

Signed-off-by: jwilson <jwilson@qti.qualcomm.com>
---
 src/lib/DriverFramework                       |  2 +-
 .../df_mpu9250_wrapper/df_mpu9250_wrapper.cpp | 20 +++++++++----------
 2 files changed, 10 insertions(+), 12 deletions(-)

diff --git a/src/lib/DriverFramework b/src/lib/DriverFramework
index 5e055c7d11..1d57b39a12 160000
--- a/src/lib/DriverFramework
+++ b/src/lib/DriverFramework
@@ -1 +1 @@
-Subproject commit 5e055c7d11ec828f59716478c3c5d32c8c1809f0
+Subproject commit 1d57b39a127612d907bdb596512e58ac3b8d4e76
diff --git a/src/platforms/posix/drivers/df_mpu9250_wrapper/df_mpu9250_wrapper.cpp b/src/platforms/posix/drivers/df_mpu9250_wrapper/df_mpu9250_wrapper.cpp
index 85ebcea5a5..1daf21f416 100644
--- a/src/platforms/posix/drivers/df_mpu9250_wrapper/df_mpu9250_wrapper.cpp
+++ b/src/platforms/posix/drivers/df_mpu9250_wrapper/df_mpu9250_wrapper.cpp
@@ -151,7 +151,6 @@ private:
 
 	Integrator		    _accel_int;
 	Integrator		    _gyro_int;
-	Integrator		    _mag_int;
 
 	unsigned		    _publish_count;
 
@@ -161,7 +160,7 @@ private:
 	perf_counter_t		    _fifo_corruption_counter;
 	perf_counter_t		    _gyro_range_hit_counter;
 	perf_counter_t		    _accel_range_hit_counter;
-	perf_counter_t		    _mag_range_hit_counter;
+	perf_counter_t		    _mag_fifo_overflow_counter;
 	perf_counter_t		    _publish_perf;
 
 	hrt_abstime		    _last_accel_range_hit_time;
@@ -185,7 +184,6 @@ DfMpu9250Wrapper::DfMpu9250Wrapper(bool mag_enabled) :
 	_mag_orb_class_instance(-1),
 	_accel_int(MPU9250_NEVER_AUTOPUBLISH_US, false),
 	_gyro_int(MPU9250_NEVER_AUTOPUBLISH_US, true),
-	_mag_int(MPU9250_NEVER_AUTOPUBLISH_US, true),
 	/*_rotation(rotation)*/
 	_publish_count(0),
 	_read_counter(perf_alloc(PC_COUNT, "mpu9250_reads")),
@@ -194,7 +192,7 @@ DfMpu9250Wrapper::DfMpu9250Wrapper(bool mag_enabled) :
 	_fifo_corruption_counter(perf_alloc(PC_COUNT, "mpu9250_fifo_corruptions")),
 	_gyro_range_hit_counter(perf_alloc(PC_COUNT, "mpu9250_gyro_range_hits")),
 	_accel_range_hit_counter(perf_alloc(PC_COUNT, "mpu9250_accel_range_hits")),
-	_mag_range_hit_counter(perf_alloc(PC_COUNT, "mpu9250_mag_range_hits")),
+	_mag_fifo_overflow_counter(perf_alloc(PC_COUNT, "mpu9250_mag_fifo_overflows")),
 	_publish_perf(perf_alloc(PC_ELAPSED, "mpu9250_publish")),
 	_last_accel_range_hit_time(0),
 	_last_accel_range_hit_count(0),
@@ -234,7 +232,7 @@ DfMpu9250Wrapper::~DfMpu9250Wrapper()
 	perf_free(_gyro_range_hit_counter);
 	perf_free(_accel_range_hit_counter);
 	if (_mag_enabled == true) {
-		perf_free(_mag_range_hit_counter);
+		perf_free(_mag_fifo_overflow_counter);
 	}
 	perf_free(_publish_perf);
 }
@@ -325,7 +323,7 @@ void DfMpu9250Wrapper::info()
 	perf_print_counter(_gyro_range_hit_counter);
 	perf_print_counter(_accel_range_hit_counter);
 	if (_mag_enabled == true) {
-		perf_print_counter(_mag_range_hit_counter);
+		perf_print_counter(_mag_fifo_overflow_counter);
 	}
 	perf_print_counter(_publish_perf);
 }
@@ -624,7 +622,7 @@ int DfMpu9250Wrapper::_publish(struct imu_sensor_data &data)
 	perf_set_count(_gyro_range_hit_counter, data.gyro_range_hit_counter);
 	perf_set_count(_accel_range_hit_counter, data.accel_range_hit_counter);
 	if (_mag_enabled == true) {
-		perf_set_count(_mag_range_hit_counter, data.mag_range_hit_counter);
+		perf_set_count(_mag_fifo_overflow_counter, data.mag_fifo_overflow_counter);
 	}
 
 	perf_begin(_publish_perf);
@@ -819,7 +817,7 @@ info()
 void
 usage()
 {
-	PX4_WARN("Usage: df_mpu9250_wrapper 'start', 'info', 'stop'");
+	PX4_WARN("Usage: df_mpu9250_wrapper 'start', 'start_without_mag', 'info', 'stop'");
 	PX4_WARN("options:");
 	//PX4_WARN("    -R rotation");
 }
@@ -856,12 +854,12 @@ df_mpu9250_wrapper_main(int argc, char *argv[])
 
 	const char *verb = argv[myoptind];
 
-	if (!strcmp(verb, "start_with_mag")) {
-		ret = df_mpu9250_wrapper::start(true);
+	if (!strcmp(verb, "start_without_mag")) {
+		ret = df_mpu9250_wrapper::start(false);
 	}
 
 	else if (!strcmp(verb, "start")) {
-		ret = df_mpu9250_wrapper::start(false);
+		ret = df_mpu9250_wrapper::start(true);
 	}
 
 	else if (!strcmp(verb, "stop")) {

From a7b31e9fcac33e5b423e6baa5ef8bba6c6f82df4 Mon Sep 17 00:00:00 2001
From: jwilson <jwilson@qti.qualcomm.com>
Date: Fri, 27 May 2016 19:29:15 -0700
Subject: [PATCH 0415/1230] Pointing internal build to ATLFlight until PX4 pull
 request is accepted.

Signed-off-by: jwilson <jwilson@qti.qualcomm.com>
---
 .gitmodules | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/.gitmodules b/.gitmodules
index fe09b6399c..201381e287 100644
--- a/.gitmodules
+++ b/.gitmodules
@@ -28,7 +28,7 @@
 	url = https://github.com/PX4/Matrix.git
 [submodule "src/lib/DriverFramework"]
 	path = src/lib/DriverFramework
-	url = https://github.com/PX4/DriverFramework.git
+	url = https://github.com/ATLFlight/DriverFramework.git
 [submodule "src/lib/ecl"]
 	path = src/lib/ecl
 	url = https://github.com/PX4/ecl.git

From c6e7307ee188b4a18827afb46f0f1fa5abea1850 Mon Sep 17 00:00:00 2001
From: jywilson <jywilson99@hotmail.com>
Date: Fri, 27 May 2016 20:03:44 -0700
Subject: [PATCH 0416/1230] Update .gitmodules

---
 .gitmodules | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/.gitmodules b/.gitmodules
index 201381e287..fe09b6399c 100644
--- a/.gitmodules
+++ b/.gitmodules
@@ -28,7 +28,7 @@
 	url = https://github.com/PX4/Matrix.git
 [submodule "src/lib/DriverFramework"]
 	path = src/lib/DriverFramework
-	url = https://github.com/ATLFlight/DriverFramework.git
+	url = https://github.com/PX4/DriverFramework.git
 [submodule "src/lib/ecl"]
 	path = src/lib/ecl
 	url = https://github.com/PX4/ecl.git

From d030444733992b0c43d5261cb3e2afb678a54f49 Mon Sep 17 00:00:00 2001
From: Julian Oes <julian@oes.ch>
Date: Tue, 31 May 2016 15:17:00 +0100
Subject: [PATCH 0417/1230] posix-configs: start HMC5883 before MPU9250 in DF

We need to start the external mag before we start the internal mag on
the MPU9250 because the ekf2 does not support voting for the sensor with
the highest priority (or failures) yet, so it will just subscribe to
whatever is at 0.
---
 posix-configs/eagle/flight/px4.config | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/posix-configs/eagle/flight/px4.config b/posix-configs/eagle/flight/px4.config
index 2c92e90ba1..1d1084d36f 100644
--- a/posix-configs/eagle/flight/px4.config
+++ b/posix-configs/eagle/flight/px4.config
@@ -4,9 +4,9 @@ param set SYS_AUTOSTART 4001
 gps start -d /dev/tty-4
 sleep 1
 param set MAV_TYPE 2
+df_hmc5883_wrapper start
 df_mpu9250_wrapper start
 df_bmp280_wrapper start
-df_hmc5883_wrapper start
 df_trone_wrapper start
 #df_isl29501_wrapper start
 sensors start

From c170657dffe203e9a9ef33f93cd35bac96800f98 Mon Sep 17 00:00:00 2001
From: Julian Oes <julian@oes.ch>
Date: Tue, 31 May 2016 15:18:11 +0100
Subject: [PATCH 0418/1230] DriverFramework: update submodule

This brings MPU9250 mag support with the correct scaling in place.
---
 src/lib/DriverFramework | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/src/lib/DriverFramework b/src/lib/DriverFramework
index 1d57b39a12..791c584969 160000
--- a/src/lib/DriverFramework
+++ b/src/lib/DriverFramework
@@ -1 +1 @@
-Subproject commit 1d57b39a127612d907bdb596512e58ac3b8d4e76
+Subproject commit 791c584969b2f84b2921c8c8c373962c677cefdb

From b0b7832048df3fc7662297cbc751e5502f1717ff Mon Sep 17 00:00:00 2001
From: Julian Oes <julian@oes.ch>
Date: Tue, 31 May 2016 15:20:35 +0100
Subject: [PATCH 0419/1230] DF driver wrappers: advertise with actual data

---
 .../df_hmc5883_wrapper/df_hmc5883_wrapper.cpp | 15 ++++---------
 .../df_mpu9250_wrapper/df_mpu9250_wrapper.cpp | 21 ++++++++-----------
 2 files changed, 13 insertions(+), 23 deletions(-)

diff --git a/src/platforms/posix/drivers/df_hmc5883_wrapper/df_hmc5883_wrapper.cpp b/src/platforms/posix/drivers/df_hmc5883_wrapper/df_hmc5883_wrapper.cpp
index e354176f8c..5c0a3a7f03 100644
--- a/src/platforms/posix/drivers/df_hmc5883_wrapper/df_hmc5883_wrapper.cpp
+++ b/src/platforms/posix/drivers/df_hmc5883_wrapper/df_hmc5883_wrapper.cpp
@@ -143,16 +143,6 @@ DfHmc9250Wrapper::~DfHmc9250Wrapper()
 
 int DfHmc9250Wrapper::start()
 {
-	// TODO: don't publish garbage here
-	mag_report mag_report = {};
-	_mag_topic = orb_advertise_multi(ORB_ID(sensor_mag), &mag_report,
-					 &_mag_orb_class_instance, ORB_PRIO_DEFAULT);
-
-	if (_mag_topic == nullptr) {
-		PX4_ERR("sensor_mag advert fail");
-		return -1;
-	}
-
 	/* Subscribe to param update topic. */
 	if (_param_update_sub < 0) {
 		_param_update_sub = orb_subscribe(ORB_ID(parameter_update));
@@ -301,7 +291,10 @@ int DfHmc9250Wrapper::_publish(struct mag_sensor_data &data)
 	// TODO: when is this ever blocked?
 	if (!(m_pub_blocked)) {
 
-		if (_mag_topic != nullptr) {
+		if (_mag_topic == nullptr) {
+			_mag_topic = orb_advertise_multi(ORB_ID(sensor_mag), &mag_report,
+							 &_mag_orb_class_instance, ORB_PRIO_HIGH);
+		} else {
 			orb_publish(ORB_ID(sensor_mag), _mag_topic, &mag_report);
 		}
 
diff --git a/src/platforms/posix/drivers/df_mpu9250_wrapper/df_mpu9250_wrapper.cpp b/src/platforms/posix/drivers/df_mpu9250_wrapper/df_mpu9250_wrapper.cpp
index 1daf21f416..c09b84e555 100644
--- a/src/platforms/posix/drivers/df_mpu9250_wrapper/df_mpu9250_wrapper.cpp
+++ b/src/platforms/posix/drivers/df_mpu9250_wrapper/df_mpu9250_wrapper.cpp
@@ -259,16 +259,7 @@ int DfMpu9250Wrapper::start()
 		return -1;
 	}
 
-	if (_mag_enabled == true) {
-		// TODO: don't publish garbage here
-		mag_report mag_report = {};
-		_mag_topic = orb_advertise_multi(ORB_ID(sensor_mag), &mag_report,
-						  &_mag_orb_class_instance, ORB_PRIO_DEFAULT);
-
-		if (_mag_topic == nullptr) {
-			PX4_ERR("sensor_mag advert fail");
-			return -1;
-		}
+	if (_mag_enabled) {
 	}
 
 	/* Subscribe to param update topic. */
@@ -707,8 +698,14 @@ int DfMpu9250Wrapper::_publish(struct imu_sensor_data &data)
 			orb_publish(ORB_ID(sensor_accel), _accel_topic, &accel_report);
 		}
 
-		if ((_mag_topic != nullptr) && (_mag_enabled == true)) {
-			orb_publish(ORB_ID(sensor_mag), _mag_topic, &mag_report);
+		if (_mag_enabled) {
+
+			if (_mag_topic == nullptr) {
+				_mag_topic = orb_advertise_multi(ORB_ID(sensor_mag), &mag_report,
+								 &_mag_orb_class_instance, ORB_PRIO_LOW);
+			} else {
+				orb_publish(ORB_ID(sensor_mag), _mag_topic, &mag_report);
+			}
 		}
 
 		/* Notify anyone waiting for data. */

From 4e54bed051d7127e4b577ae5ab2719adb19de296 Mon Sep 17 00:00:00 2001
From: Julian Oes <julian@oes.ch>
Date: Tue, 31 May 2016 15:21:25 +0100
Subject: [PATCH 0420/1230] df_mpu9250_wrapper: bool comparison without ==

I prefer this way because I think it's more readable.
---
 .../df_mpu9250_wrapper/df_mpu9250_wrapper.cpp  | 18 +++++++++---------
 1 file changed, 9 insertions(+), 9 deletions(-)

diff --git a/src/platforms/posix/drivers/df_mpu9250_wrapper/df_mpu9250_wrapper.cpp b/src/platforms/posix/drivers/df_mpu9250_wrapper/df_mpu9250_wrapper.cpp
index c09b84e555..b9eec13be0 100644
--- a/src/platforms/posix/drivers/df_mpu9250_wrapper/df_mpu9250_wrapper.cpp
+++ b/src/platforms/posix/drivers/df_mpu9250_wrapper/df_mpu9250_wrapper.cpp
@@ -213,7 +213,7 @@ DfMpu9250Wrapper::DfMpu9250Wrapper(bool mag_enabled) :
 	_gyro_calibration.y_offset = 0.0f;
 	_gyro_calibration.z_offset = 0.0f;
 
-	if (_mag_enabled == true) {
+	if (_mag_enabled) {
 		_mag_calibration.x_scale = 1.0f;
 		_mag_calibration.y_scale = 1.0f;
 		_mag_calibration.z_scale = 1.0f;
@@ -231,7 +231,7 @@ DfMpu9250Wrapper::~DfMpu9250Wrapper()
 	perf_free(_fifo_corruption_counter);
 	perf_free(_gyro_range_hit_counter);
 	perf_free(_accel_range_hit_counter);
-	if (_mag_enabled == true) {
+	if (_mag_enabled) {
 		perf_free(_mag_fifo_overflow_counter);
 	}
 	perf_free(_publish_perf);
@@ -313,7 +313,7 @@ void DfMpu9250Wrapper::info()
 	perf_print_counter(_fifo_corruption_counter);
 	perf_print_counter(_gyro_range_hit_counter);
 	perf_print_counter(_accel_range_hit_counter);
-	if (_mag_enabled == true) {
+	if (_mag_enabled) {
 		perf_print_counter(_mag_fifo_overflow_counter);
 	}
 	perf_print_counter(_publish_perf);
@@ -472,7 +472,7 @@ void DfMpu9250Wrapper::_update_accel_calibration()
 
 void DfMpu9250Wrapper::_update_mag_calibration()
 {
-	if (_mag_enabled == false) {
+	if (!_mag_enabled) {
 		return;
 	}
 
@@ -612,7 +612,7 @@ int DfMpu9250Wrapper::_publish(struct imu_sensor_data &data)
 	perf_set_count(_fifo_corruption_counter, data.fifo_overflow_counter);
 	perf_set_count(_gyro_range_hit_counter, data.gyro_range_hit_counter);
 	perf_set_count(_accel_range_hit_counter, data.accel_range_hit_counter);
-	if (_mag_enabled == true) {
+	if (_mag_enabled) {
 		perf_set_count(_mag_fifo_overflow_counter, data.mag_fifo_overflow_counter);
 	}
 
@@ -623,7 +623,7 @@ int DfMpu9250Wrapper::_publish(struct imu_sensor_data &data)
 	mag_report mag_report = {};
 
 	accel_report.timestamp = gyro_report.timestamp = hrt_absolute_time();
-	if (_mag_enabled == true) {
+	if (_mag_enabled) {
 		mag_report.timestamp = accel_report.timestamp;
 	}
 
@@ -636,7 +636,7 @@ int DfMpu9250Wrapper::_publish(struct imu_sensor_data &data)
 	accel_report.range_m_s2 = -1.0f;
 	accel_report.device_id = m_id.dev_id;
 
-	if (_mag_enabled == true) {
+	if (_mag_enabled) {
 		mag_report.scaling = -1.0f;
 		mag_report.range_ga = -1.0f;
 		mag_report.device_id = m_id.dev_id;
@@ -651,7 +651,7 @@ int DfMpu9250Wrapper::_publish(struct imu_sensor_data &data)
 	accel_report.y_raw = NAN;
 	accel_report.z_raw = NAN;
 
-	if (_mag_enabled == true) {
+	if (_mag_enabled) {
 		mag_report.x_raw = NAN;
 		mag_report.y_raw = NAN;
 		mag_report.z_raw = NAN;
@@ -673,7 +673,7 @@ int DfMpu9250Wrapper::_publish(struct imu_sensor_data &data)
 	accel_report.y = accel_val_filt(1);
 	accel_report.z = accel_val_filt(2);
 
-	if (_mag_enabled == true) {
+	if (_mag_enabled) {
 		mag_report.x = (data.mag_ga_x - _mag_calibration.x_offset) * _mag_calibration.x_scale;
 		mag_report.y = (data.mag_ga_y - _mag_calibration.y_offset) * _mag_calibration.y_scale;
 		mag_report.z = (data.mag_ga_z - _mag_calibration.z_offset) * _mag_calibration.z_scale;

From 99cf6758d220e498ba6fab33210cad2fa4a8d6b7 Mon Sep 17 00:00:00 2001
From: Julian Oes <julian@oes.ch>
Date: Tue, 31 May 2016 15:23:39 +0100
Subject: [PATCH 0421/1230] DF driver wrappers: whitespace fixes

---
 .../drivers/df_hmc5883_wrapper/df_hmc5883_wrapper.cpp      | 1 +
 .../drivers/df_mpu9250_wrapper/df_mpu9250_wrapper.cpp      | 7 +++++++
 2 files changed, 8 insertions(+)

diff --git a/src/platforms/posix/drivers/df_hmc5883_wrapper/df_hmc5883_wrapper.cpp b/src/platforms/posix/drivers/df_hmc5883_wrapper/df_hmc5883_wrapper.cpp
index 5c0a3a7f03..22e410ea8d 100644
--- a/src/platforms/posix/drivers/df_hmc5883_wrapper/df_hmc5883_wrapper.cpp
+++ b/src/platforms/posix/drivers/df_hmc5883_wrapper/df_hmc5883_wrapper.cpp
@@ -294,6 +294,7 @@ int DfHmc9250Wrapper::_publish(struct mag_sensor_data &data)
 		if (_mag_topic == nullptr) {
 			_mag_topic = orb_advertise_multi(ORB_ID(sensor_mag), &mag_report,
 							 &_mag_orb_class_instance, ORB_PRIO_HIGH);
+
 		} else {
 			orb_publish(ORB_ID(sensor_mag), _mag_topic, &mag_report);
 		}
diff --git a/src/platforms/posix/drivers/df_mpu9250_wrapper/df_mpu9250_wrapper.cpp b/src/platforms/posix/drivers/df_mpu9250_wrapper/df_mpu9250_wrapper.cpp
index b9eec13be0..fe166d5f22 100644
--- a/src/platforms/posix/drivers/df_mpu9250_wrapper/df_mpu9250_wrapper.cpp
+++ b/src/platforms/posix/drivers/df_mpu9250_wrapper/df_mpu9250_wrapper.cpp
@@ -231,9 +231,11 @@ DfMpu9250Wrapper::~DfMpu9250Wrapper()
 	perf_free(_fifo_corruption_counter);
 	perf_free(_gyro_range_hit_counter);
 	perf_free(_accel_range_hit_counter);
+
 	if (_mag_enabled) {
 		perf_free(_mag_fifo_overflow_counter);
 	}
+
 	perf_free(_publish_perf);
 }
 
@@ -313,9 +315,11 @@ void DfMpu9250Wrapper::info()
 	perf_print_counter(_fifo_corruption_counter);
 	perf_print_counter(_gyro_range_hit_counter);
 	perf_print_counter(_accel_range_hit_counter);
+
 	if (_mag_enabled) {
 		perf_print_counter(_mag_fifo_overflow_counter);
 	}
+
 	perf_print_counter(_publish_perf);
 }
 
@@ -612,6 +616,7 @@ int DfMpu9250Wrapper::_publish(struct imu_sensor_data &data)
 	perf_set_count(_fifo_corruption_counter, data.fifo_overflow_counter);
 	perf_set_count(_gyro_range_hit_counter, data.gyro_range_hit_counter);
 	perf_set_count(_accel_range_hit_counter, data.accel_range_hit_counter);
+
 	if (_mag_enabled) {
 		perf_set_count(_mag_fifo_overflow_counter, data.mag_fifo_overflow_counter);
 	}
@@ -623,6 +628,7 @@ int DfMpu9250Wrapper::_publish(struct imu_sensor_data &data)
 	mag_report mag_report = {};
 
 	accel_report.timestamp = gyro_report.timestamp = hrt_absolute_time();
+
 	if (_mag_enabled) {
 		mag_report.timestamp = accel_report.timestamp;
 	}
@@ -703,6 +709,7 @@ int DfMpu9250Wrapper::_publish(struct imu_sensor_data &data)
 			if (_mag_topic == nullptr) {
 				_mag_topic = orb_advertise_multi(ORB_ID(sensor_mag), &mag_report,
 								 &_mag_orb_class_instance, ORB_PRIO_LOW);
+
 			} else {
 				orb_publish(ORB_ID(sensor_mag), _mag_topic, &mag_report);
 			}

From 28080f04b011ff0b3195e4fc1d5334cdf53ef6fa Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Wed, 1 Jun 2016 08:12:35 +0200
Subject: [PATCH 0422/1230] Update Gazebo models

---
 Tools/sitl_gazebo | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/Tools/sitl_gazebo b/Tools/sitl_gazebo
index 7096721ff4..96f1169cf9 160000
--- a/Tools/sitl_gazebo
+++ b/Tools/sitl_gazebo
@@ -1 +1 @@
-Subproject commit 7096721ff4c0314ebeee4cb1f24a84e88f2e8c9f
+Subproject commit 96f1169cf95e92e07c1edc2a15709be835e6d274

From b54ec99d2281c176f7f783568319a11fc94d8457 Mon Sep 17 00:00:00 2001
From: David Sidrane <david_s5@nscdg.com>
Date: Tue, 31 May 2016 10:52:35 -1000
Subject: [PATCH 0423/1230] Move the DMA allocation to a common code and
 provide an instrumentation interface

---
 src/drivers/boards/aerocore/board_config.h    |   2 +
 src/drivers/boards/common/board_common.h      | 101 ++++++++++++
 src/drivers/boards/common/board_dma_alloc.c   | 156 ++++++++++++++++++
 src/drivers/boards/mindpx-v2/CMakeLists.txt   |   1 +
 src/drivers/boards/mindpx-v2/board_config.h   |   5 +
 src/drivers/boards/mindpx-v2/mindpx2_init.c   |  69 +-------
 .../px4-stm32f4discovery/board_config.h       |   2 +
 src/drivers/boards/px4fmu-v1/board_config.h   |   2 +
 src/drivers/boards/px4fmu-v2/CMakeLists.txt   |   1 +
 src/drivers/boards/px4fmu-v2/board_config.h   |   5 +
 src/drivers/boards/px4fmu-v2/px4fmu2_init.c   |  70 +-------
 src/drivers/boards/px4fmu-v4/CMakeLists.txt   |   1 +
 src/drivers/boards/px4fmu-v4/board_config.h   |   5 +
 src/drivers/boards/px4fmu-v4/px4fmu_init.c    |  69 +-------
 14 files changed, 293 insertions(+), 196 deletions(-)
 create mode 100644 src/drivers/boards/common/board_common.h
 create mode 100644 src/drivers/boards/common/board_dma_alloc.c

diff --git a/src/drivers/boards/aerocore/board_config.h b/src/drivers/boards/aerocore/board_config.h
index 922ddb6ee4..9eb97d559a 100644
--- a/src/drivers/boards/aerocore/board_config.h
+++ b/src/drivers/boards/aerocore/board_config.h
@@ -235,6 +235,8 @@ extern void stm32_spiinitialize(void);
 int nsh_archinitialize(void);
 #endif
 
+#include "../common/board_common.h"
+
 #endif /* __ASSEMBLY__ */
 
 __END_DECLS
diff --git a/src/drivers/boards/common/board_common.h b/src/drivers/boards/common/board_common.h
new file mode 100644
index 0000000000..2b802639a5
--- /dev/null
+++ b/src/drivers/boards/common/board_common.h
@@ -0,0 +1,101 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2016 PX4 Development Team. All rights reserved.
+ *                 Author: David Sidrane <david_s5@nscdg.com>
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file board_common.h
+ *
+ * Provide the the common board interfaces
+ */
+
+#pragma once
+
+/************************************************************************************
+ * Included Files
+ ************************************************************************************/
+#include <errno.h>
+/************************************************************************************
+ * Definitions
+ ************************************************************************************/
+
+/************************************************************************************
+ * Private Functions
+ ************************************************************************************/
+
+/************************************************************************************
+ * Public Functions
+ ************************************************************************************/
+
+/************************************************************************************
+ * Name: board_name
+ *
+ * Description:
+ *   All boards must provide this API to return the board name.
+ *
+ ************************************************************************************/
+
+__EXPORT const char *board_name(void);
+
+/************************************************************************************
+ * Name: board_dma_alloc_init
+ *
+ * Description:
+ *   All boards may optionally provide this API to instantiate a pool of
+ *   memory for uses with FAST FS DMA operations.
+ *
+ *   Provision is controlled by declaring BOARD_DMA_ALLOC_POOL_SIZE in board_config.h
+ *
+ *
+ ************************************************************************************/
+#if defined(BOARD_DMA_ALLOC_POOL_SIZE)
+__EXPORT int board_dma_alloc_init(void);
+#else
+#define board_dma_alloc_init() (-EPERM)
+#endif
+
+/************************************************************************************
+ * Name: board_get_dma_usage
+ *
+ * Description:
+ *   All boards may optionally provide this API to supply instrumentation for a pool of
+ *   memory used for DMA operations.
+ *
+ *   Provision is controlled by declaring BOARD_DMA_ALLOC_POOL_SIZE in board_config.h
+ *
+ *
+ ************************************************************************************/
+#if defined(BOARD_DMA_ALLOC_POOL_SIZE)
+__EXPORT int board_get_dma_usage(uint16_t *dma_total, uint16_t *dma_used, uint16_t *dma_peek_used);
+#else
+#define board_get_dma_usage(dma_total,dma_used, dma_peek_used) (-ENOMEM)
+#endif
diff --git a/src/drivers/boards/common/board_dma_alloc.c b/src/drivers/boards/common/board_dma_alloc.c
new file mode 100644
index 0000000000..2c7126d1a0
--- /dev/null
+++ b/src/drivers/boards/common/board_dma_alloc.c
@@ -0,0 +1,156 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2016 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file board_dma_alloc.c
+ *
+ * Provide the board dma allocator interface.
+ */
+
+/************************************************************************************
+ * Included Files
+ ************************************************************************************/
+
+#include <px4_config.h>
+#include "board_config.h"
+
+#include <stdint.h>
+#include <errno.h>
+#include <nuttx/gran.h>
+
+#include <systemlib/perf_counter.h>
+
+/************************************************************************************
+ * Definitions
+ ************************************************************************************/
+
+/************************************************************************************
+ * Private Functions
+ ************************************************************************************/
+
+/************************************************************************************
+ * Public Functions
+ ************************************************************************************/
+
+/************************************************************************************
+ * Name: board_dma_alloc_init
+ *
+ * Description:
+ *   All boards may optionally provide this API to instantiate a pool of
+ *   memory for uses with FAST FS DMA operations.
+ *
+ ************************************************************************************/
+#if defined(BOARD_DMA_ALLOC_POOL_SIZE)
+
+#  if !defined(CONFIG_GRAN) || !defined(CONFIG_FAT_DMAMEMORY)
+#    error microSD DMA support requires CONFIG_GRAN
+#  endif
+
+static GRAN_HANDLE dma_allocator;
+
+/*
+ * The DMA heap size constrains the total number of things that can be
+ * ready to do DMA at a time.
+ *
+ * For example, FAT DMA depends on one sector-sized buffer per filesystem plus
+ * one sector-sized buffer per file.
+ *
+ * We use a fundamental alignment / granule size of 64B; this is sufficient
+ * to guarantee alignment for the largest STM32 DMA burst (16 beats x 32bits).
+ */
+static uint8_t g_dma_heap[BOARD_DMA_ALLOC_POOL_SIZE] __attribute__((aligned(64)));
+static perf_counter_t g_dma_perf;
+static uint16_t dma_heap_inuse;
+static uint16_t dma_heap_peek_use;
+/****************************************************************************
+ * Public Functions
+ ****************************************************************************/
+__EXPORT int board_dma_alloc_init(void)
+{
+	dma_allocator = gran_initialize(g_dma_heap,
+					sizeof(g_dma_heap),
+					7,  /* 128B granule - must be > alignment (XXX bug?) */
+					6); /* 64B alignment */
+
+	if (dma_allocator == NULL) {
+		return -ENOMEM;
+
+	} else {
+		dma_heap_inuse = 0;
+		dma_heap_peek_use = 0;
+		g_dma_perf = perf_alloc(PC_COUNT, "dma_alloc");
+	}
+
+	return OK;
+}
+
+__EXPORT int board_get_dma_usage(uint16_t *dma_total, uint16_t *dma_used, uint16_t *dma_peek_used)
+{
+	*dma_total = sizeof(g_dma_heap);
+	*dma_used = dma_heap_inuse;
+	*dma_peek_used = dma_heap_peek_use;
+
+	return OK;
+}
+/*
+ * DMA-aware allocator stubs for the FAT filesystem.
+ */
+
+__EXPORT void *fat_dma_alloc(size_t size);
+__EXPORT void fat_dma_free(FAR void *memory, size_t size);
+
+void *
+fat_dma_alloc(size_t size)
+{
+	void *rv = NULL;
+	perf_count(g_dma_perf);
+	rv = gran_alloc(dma_allocator, size);
+
+	if (rv != NULL) {
+		dma_heap_inuse += size;
+
+		if (dma_heap_inuse > dma_heap_peek_use) {
+			dma_heap_peek_use = dma_heap_inuse;
+		}
+	}
+
+	return rv;
+}
+
+void
+fat_dma_free(FAR void *memory, size_t size)
+{
+	gran_free(dma_allocator, memory, size);
+	dma_heap_inuse -= size;
+}
+#endif /* defined(BOARD_DMA_ALLOC_POOL_SIZE) */
diff --git a/src/drivers/boards/mindpx-v2/CMakeLists.txt b/src/drivers/boards/mindpx-v2/CMakeLists.txt
index e64b05469e..06598cda39 100644
--- a/src/drivers/boards/mindpx-v2/CMakeLists.txt
+++ b/src/drivers/boards/mindpx-v2/CMakeLists.txt
@@ -35,6 +35,7 @@ px4_add_module(
 	COMPILE_FLAGS
 		-Os
 	SRCS
+		../common/board_dma_alloc.c
 		../common/board_name.c
 		mindpx_can.c
 		mindpx2_init.c
diff --git a/src/drivers/boards/mindpx-v2/board_config.h b/src/drivers/boards/mindpx-v2/board_config.h
index 0c478057f3..5bd7e39c77 100644
--- a/src/drivers/boards/mindpx-v2/board_config.h
+++ b/src/drivers/boards/mindpx-v2/board_config.h
@@ -319,6 +319,9 @@ __BEGIN_DECLS
 		{GPIO_GPIO11_INPUT,      GPIO_GPIO11_OUTPUT,      0}, \
 		{GPIO_GPIO12_INPUT,      GPIO_GPIO12_OUTPUT,      0}, }
 
+/* This board provides a DMA pool and APIs */
+
+#define BOARD_DMA_ALLOC_POOL_SIZE 8192
 
 /****************************************************************************************************
  * Public Types
@@ -369,6 +372,8 @@ extern void stm32_usbinitialize(void);
 int nsh_archinitialize(void);
 #endif
 
+#include "../common/board_common.h"
+
 #endif /* __ASSEMBLY__ */
 
 __END_DECLS
diff --git a/src/drivers/boards/mindpx-v2/mindpx2_init.c b/src/drivers/boards/mindpx-v2/mindpx2_init.c
index 94ea735f8d..5639a3a0f8 100644
--- a/src/drivers/boards/mindpx-v2/mindpx2_init.c
+++ b/src/drivers/boards/mindpx-v2/mindpx2_init.c
@@ -110,73 +110,9 @@ __END_DECLS
 /****************************************************************************
  * Protected Functions
  ****************************************************************************/
-
-#if defined(CONFIG_FAT_DMAMEMORY)
-# if !defined(CONFIG_GRAN) || !defined(CONFIG_FAT_DMAMEMORY)
-#  error microSD DMA support requires CONFIG_GRAN
-# endif
-
-static GRAN_HANDLE dma_allocator;
-
-/*
- * The DMA heap size constrains the total number of things that can be
- * ready to do DMA at a time.
- *
- * For example, FAT DMA depends on one sector-sized buffer per filesystem plus
- * one sector-sized buffer per file.
- *
- * We use a fundamental alignment / granule size of 64B; this is sufficient
- * to guarantee alignment for the largest STM32 DMA burst (16 beats x 32bits).
- */
-static uint8_t g_dma_heap[8192] __attribute__((aligned(64)));
-static perf_counter_t g_dma_perf;
-
-static void
-dma_alloc_init(void)
-{
-	dma_allocator = gran_initialize(g_dma_heap,
-					sizeof(g_dma_heap),
-					7,  /* 128B granule - must be > alignment (XXX bug?) */
-					6); /* 64B alignment */
-
-	if (dma_allocator == NULL) {
-		message("DMA alloc FAILED");
-
-	} else {
-		g_dma_perf = perf_alloc(PC_COUNT, "dma_alloc");
-	}
-}
-
 /****************************************************************************
  * Public Functions
  ****************************************************************************/
-
-/*
- * DMA-aware allocator stubs for the FAT filesystem.
- */
-
-__EXPORT void *fat_dma_alloc(size_t size);
-__EXPORT void fat_dma_free(FAR void *memory, size_t size);
-
-void *
-fat_dma_alloc(size_t size)
-{
-	perf_count(g_dma_perf);
-	return gran_alloc(dma_allocator, size);
-}
-
-void
-fat_dma_free(FAR void *memory, size_t size)
-{
-	gran_free(dma_allocator, memory, size);
-}
-
-#else
-
-# define dma_alloc_init()
-
-#endif
-
 /************************************************************************************
  * Name: stm32_boardinitialize
  *
@@ -252,7 +188,10 @@ __EXPORT int nsh_archinitialize(void)
 	hrt_init();
 
 	/* configure the DMA allocator */
-	dma_alloc_init();
+
+	if (board_dma_alloc_init() < 0) {
+		message("DMA alloc FAILED");
+	}
 
 	/* configure CPU load estimation */
 #ifdef CONFIG_SCHED_INSTRUMENTATION
diff --git a/src/drivers/boards/px4-stm32f4discovery/board_config.h b/src/drivers/boards/px4-stm32f4discovery/board_config.h
index 1d6aba13a1..2b476bf644 100644
--- a/src/drivers/boards/px4-stm32f4discovery/board_config.h
+++ b/src/drivers/boards/px4-stm32f4discovery/board_config.h
@@ -134,6 +134,8 @@ extern void stm32_usbinitialize(void);
 int nsh_archinitialize(void);
 #endif
 
+#include "../common/board_common.h"
+
 #endif /* __ASSEMBLY__ */
 
 __END_DECLS
diff --git a/src/drivers/boards/px4fmu-v1/board_config.h b/src/drivers/boards/px4fmu-v1/board_config.h
index 549e0ec892..ec01c1bd5f 100644
--- a/src/drivers/boards/px4fmu-v1/board_config.h
+++ b/src/drivers/boards/px4fmu-v1/board_config.h
@@ -272,6 +272,8 @@ extern void stm32_usbinitialize(void);
 int nsh_archinitialize(void);
 #endif
 
+#include "../common/board_common.h"
+
 #endif /* __ASSEMBLY__ */
 
 __END_DECLS
diff --git a/src/drivers/boards/px4fmu-v2/CMakeLists.txt b/src/drivers/boards/px4fmu-v2/CMakeLists.txt
index fe43efae75..e53cc92ac0 100644
--- a/src/drivers/boards/px4fmu-v2/CMakeLists.txt
+++ b/src/drivers/boards/px4fmu-v2/CMakeLists.txt
@@ -36,6 +36,7 @@ px4_add_module(
 		-Os
 	SRCS
 		../common/board_name.c
+		../common/board_dma_alloc.c
 		px4fmu_can.c
 		px4fmu2_init.c
 		px4fmu_timer_config.c
diff --git a/src/drivers/boards/px4fmu-v2/board_config.h b/src/drivers/boards/px4fmu-v2/board_config.h
index 4ff10f0c99..e0b916c5cc 100644
--- a/src/drivers/boards/px4fmu-v2/board_config.h
+++ b/src/drivers/boards/px4fmu-v2/board_config.h
@@ -270,6 +270,9 @@ __BEGIN_DECLS
 		{GPIO_VDD_5V_HIPOWER_OC, 0,                       0}, \
 		{GPIO_VDD_5V_PERIPH_OC,  0,                       0}, }
 
+/* This board provides a DMA pool and APIs */
+
+#define BOARD_DMA_ALLOC_POOL_SIZE 8192
 /****************************************************************************************************
  * Public Types
  ****************************************************************************************************/
@@ -318,6 +321,8 @@ extern void board_peripheral_reset(int ms);
 int nsh_archinitialize(void);
 #endif
 
+#include "../common/board_common.h"
+
 #endif /* __ASSEMBLY__ */
 
 __END_DECLS
diff --git a/src/drivers/boards/px4fmu-v2/px4fmu2_init.c b/src/drivers/boards/px4fmu-v2/px4fmu2_init.c
index c95c5ffd44..494341f46f 100644
--- a/src/drivers/boards/px4fmu-v2/px4fmu2_init.c
+++ b/src/drivers/boards/px4fmu-v2/px4fmu2_init.c
@@ -58,7 +58,6 @@
 #include <nuttx/sdio.h>
 #include <nuttx/mmcsd.h>
 #include <nuttx/analog/adc.h>
-#include <nuttx/gran.h>
 
 #include <stm32.h>
 #include "board_config.h"
@@ -111,73 +110,9 @@ __END_DECLS
 /****************************************************************************
  * Protected Functions
  ****************************************************************************/
-
-#if defined(CONFIG_FAT_DMAMEMORY)
-# if !defined(CONFIG_GRAN) || !defined(CONFIG_FAT_DMAMEMORY)
-#  error microSD DMA support requires CONFIG_GRAN
-# endif
-
-static GRAN_HANDLE dma_allocator;
-
-/*
- * The DMA heap size constrains the total number of things that can be
- * ready to do DMA at a time.
- *
- * For example, FAT DMA depends on one sector-sized buffer per filesystem plus
- * one sector-sized buffer per file.
- *
- * We use a fundamental alignment / granule size of 64B; this is sufficient
- * to guarantee alignment for the largest STM32 DMA burst (16 beats x 32bits).
- */
-static uint8_t g_dma_heap[8192] __attribute__((aligned(64)));
-static perf_counter_t g_dma_perf;
-
-static void
-dma_alloc_init(void)
-{
-	dma_allocator = gran_initialize(g_dma_heap,
-					sizeof(g_dma_heap),
-					7,  /* 128B granule - must be > alignment (XXX bug?) */
-					6); /* 64B alignment */
-
-	if (dma_allocator == NULL) {
-		message("DMA alloc FAILED");
-
-	} else {
-		g_dma_perf = perf_alloc(PC_COUNT, "dma_alloc");
-	}
-}
-
 /****************************************************************************
  * Public Functions
  ****************************************************************************/
-
-/*
- * DMA-aware allocator stubs for the FAT filesystem.
- */
-
-__EXPORT void *fat_dma_alloc(size_t size);
-__EXPORT void fat_dma_free(FAR void *memory, size_t size);
-
-void *
-fat_dma_alloc(size_t size)
-{
-	perf_count(g_dma_perf);
-	return gran_alloc(dma_allocator, size);
-}
-
-void
-fat_dma_free(FAR void *memory, size_t size)
-{
-	gran_free(dma_allocator, memory, size);
-}
-
-#else
-
-# define dma_alloc_init()
-
-#endif
-
 /************************************************************************************
  * Name: board_peripheral_reset
  *
@@ -269,7 +204,10 @@ __EXPORT int nsh_archinitialize(void)
 	hrt_init();
 
 	/* configure the DMA allocator */
-	dma_alloc_init();
+
+	if (board_dma_alloc_init() < 0) {
+		message("DMA alloc FAILED");
+	}
 
 	/* configure CPU load estimation */
 #ifdef CONFIG_SCHED_INSTRUMENTATION
diff --git a/src/drivers/boards/px4fmu-v4/CMakeLists.txt b/src/drivers/boards/px4fmu-v4/CMakeLists.txt
index 2afe449392..8370a7c857 100644
--- a/src/drivers/boards/px4fmu-v4/CMakeLists.txt
+++ b/src/drivers/boards/px4fmu-v4/CMakeLists.txt
@@ -36,6 +36,7 @@ px4_add_module(
 		-Os
 	SRCS
 		../common/board_name.c
+		../common/board_dma_alloc.c
 		px4fmu_can.c
 		px4fmu_init.c
 		px4fmu_timer_config.c
diff --git a/src/drivers/boards/px4fmu-v4/board_config.h b/src/drivers/boards/px4fmu-v4/board_config.h
index dfc691b5fa..bf18bd9099 100644
--- a/src/drivers/boards/px4fmu-v4/board_config.h
+++ b/src/drivers/boards/px4fmu-v4/board_config.h
@@ -297,6 +297,9 @@ __BEGIN_DECLS
 		{0,                      GPIO_VDD_3V3_SENSORS_EN, 0}, \
 		{GPIO_VDD_BRICK_VALID,   0,                       0}, }
 
+/* This board provides a DMA pool and APIs */
+
+#define BOARD_DMA_ALLOC_POOL_SIZE 8192
 
 /****************************************************************************************************
  * Public Types
@@ -347,6 +350,8 @@ extern void board_peripheral_reset(int ms);
 int nsh_archinitialize(void);
 #endif
 
+#include "../common/board_common.h"
+
 #endif /* __ASSEMBLY__ */
 
 __END_DECLS
diff --git a/src/drivers/boards/px4fmu-v4/px4fmu_init.c b/src/drivers/boards/px4fmu-v4/px4fmu_init.c
index f3af5e3f6d..2e6adaf000 100644
--- a/src/drivers/boards/px4fmu-v4/px4fmu_init.c
+++ b/src/drivers/boards/px4fmu-v4/px4fmu_init.c
@@ -111,73 +111,9 @@ __END_DECLS
 /****************************************************************************
  * Protected Functions
  ****************************************************************************/
-
-#if defined(CONFIG_FAT_DMAMEMORY)
-# if !defined(CONFIG_GRAN) || !defined(CONFIG_FAT_DMAMEMORY)
-#  error microSD DMA support requires CONFIG_GRAN
-# endif
-
-static GRAN_HANDLE dma_allocator;
-
-/*
- * The DMA heap size constrains the total number of things that can be
- * ready to do DMA at a time.
- *
- * For example, FAT DMA depends on one sector-sized buffer per filesystem plus
- * one sector-sized buffer per file.
- *
- * We use a fundamental alignment / granule size of 64B; this is sufficient
- * to guarantee alignment for the largest STM32 DMA burst (16 beats x 32bits).
- */
-static uint8_t g_dma_heap[8192] __attribute__((aligned(64)));
-static perf_counter_t g_dma_perf;
-
-static void
-dma_alloc_init(void)
-{
-	dma_allocator = gran_initialize(g_dma_heap,
-					sizeof(g_dma_heap),
-					7,  /* 128B granule - must be > alignment (XXX bug?) */
-					6); /* 64B alignment */
-
-	if (dma_allocator == NULL) {
-		message("DMA alloc FAILED");
-
-	} else {
-		g_dma_perf = perf_alloc(PC_COUNT, "dma_alloc");
-	}
-}
-
 /****************************************************************************
  * Public Functions
  ****************************************************************************/
-
-/*
- * DMA-aware allocator stubs for the FAT filesystem.
- */
-
-__EXPORT void *fat_dma_alloc(size_t size);
-__EXPORT void fat_dma_free(FAR void *memory, size_t size);
-
-void *
-fat_dma_alloc(size_t size)
-{
-	perf_count(g_dma_perf);
-	return gran_alloc(dma_allocator, size);
-}
-
-void
-fat_dma_free(FAR void *memory, size_t size)
-{
-	gran_free(dma_allocator, memory, size);
-}
-
-#else
-
-# define dma_alloc_init()
-
-#endif
-
 /************************************************************************************
  * Name: board_peripheral_reset
  *
@@ -278,7 +214,10 @@ __EXPORT int nsh_archinitialize(void)
 	hrt_init();
 
 	/* configure the DMA allocator */
-	dma_alloc_init();
+
+	if (board_dma_alloc_init() < 0) {
+		message("DMA alloc FAILED");
+	}
 
 	/* configure CPU load estimation */
 #ifdef CONFIG_SCHED_INSTRUMENTATION

From 08f0cc1b24342aceb63bdce3ce68d6ab3e3a2675 Mon Sep 17 00:00:00 2001
From: David Sidrane <david_s5@nscdg.com>
Date: Tue, 31 May 2016 10:53:24 -1000
Subject: [PATCH 0424/1230] Print the DMA usage in top via instrumentation
 interface

---
 src/modules/systemlib/printload.c | 15 ++++++++++++++-
 1 file changed, 14 insertions(+), 1 deletion(-)

diff --git a/src/modules/systemlib/printload.c b/src/modules/systemlib/printload.c
index a179ddc51b..6955b4c6ef 100644
--- a/src/modules/systemlib/printload.c
+++ b/src/modules/systemlib/printload.c
@@ -215,12 +215,25 @@ void print_load(uint64_t t, int fd, struct print_load_s *print_state)
 					(double)(task_load * 100.f),
 					(double)(sched_load * 100.f),
 					(double)(idle * 100.f));
+#if defined(BOARD_DMA_ALLOC_POOL_SIZE)
+				uint16_t dma_total;
+				uint16_t dma_used;
+				uint16_t dma_peek_used;
+
+				if (board_get_dma_usage(&dma_total, &dma_used, &dma_peek_used) >= 0) {
+					dprintf(fd, "%sDMA Memory: %d total, %d used %d peek\n",
+						clear_line,
+						dma_total,
+						dma_used,
+						dma_peek_used);
+				}
+
+#endif
 				dprintf(fd, "%sUptime: %.3fs total, %.3fs idle\n%s\n",
 					clear_line,
 					(double)curr_time_us / 1000000.d,
 					(double)idle_time_us / 1000000.d,
 					clear_line);
-
 				/* header for task list */
 				dprintf(fd, "%s%4s %*-s %8s %6s %11s %10s %-6s\n",
 					clear_line,

From 9833ef13fb3836f632651b06af808ad4c5a9bd99 Mon Sep 17 00:00:00 2001
From: David Sidrane <david_s5@nscdg.com>
Date: Tue, 31 May 2016 16:47:46 -1000
Subject: [PATCH 0425/1230] Update adc.cpp

---
 src/drivers/stm32/adc/adc.cpp | 2 ++
 1 file changed, 2 insertions(+)

diff --git a/src/drivers/stm32/adc/adc.cpp b/src/drivers/stm32/adc/adc.cpp
index 18d1d2c218..1a8753d0ad 100644
--- a/src/drivers/stm32/adc/adc.cpp
+++ b/src/drivers/stm32/adc/adc.cpp
@@ -349,12 +349,14 @@ ADC::update_system_power(hrt_abstime now)
 
 	system_power.voltage5V_v = 0;
 
+#if defined(ADC_5V_RAIL_SENSE)
 	for (unsigned i = 0; i < _channel_count; i++) {
 		if (_samples[i].am_channel == ADC_5V_RAIL_SENSE) {
 			// it is 2:1 scaled
 			system_power.voltage5V_v = _samples[i].am_data * (6.6f / 4096);
 		}
 	}
+#endif
 
 	/* Note once the board_config.h provides BOARD_ADC_USB_CONNECTED,
 	 * It must provide the true logic GPIO BOARD_ADC_xxxx macros.

From 69d0a78c543c7caeff70fd46ecbb0b142da54c5f Mon Sep 17 00:00:00 2001
From: David Sidrane <david_s5@nscdg.com>
Date: Tue, 31 May 2016 17:14:51 -1000
Subject: [PATCH 0426/1230] Update adc.cpp

---
 src/drivers/stm32/adc/adc.cpp | 2 ++
 1 file changed, 2 insertions(+)

diff --git a/src/drivers/stm32/adc/adc.cpp b/src/drivers/stm32/adc/adc.cpp
index 1a8753d0ad..43d2dad252 100644
--- a/src/drivers/stm32/adc/adc.cpp
+++ b/src/drivers/stm32/adc/adc.cpp
@@ -350,12 +350,14 @@ ADC::update_system_power(hrt_abstime now)
 	system_power.voltage5V_v = 0;
 
 #if defined(ADC_5V_RAIL_SENSE)
+
 	for (unsigned i = 0; i < _channel_count; i++) {
 		if (_samples[i].am_channel == ADC_5V_RAIL_SENSE) {
 			// it is 2:1 scaled
 			system_power.voltage5V_v = _samples[i].am_data * (6.6f / 4096);
 		}
 	}
+
 #endif
 
 	/* Note once the board_config.h provides BOARD_ADC_USB_CONNECTED,

From a7ae7907f7c9561f44bcb3f4cf90e7203da79d6a Mon Sep 17 00:00:00 2001
From: Kabir Mohammed <mhkabir98@gmail.com>
Date: Mon, 30 May 2016 12:43:34 +0530
Subject: [PATCH 0427/1230] add rotation support to mpu9250 wrapper

---
 .../df_mpu9250_wrapper/df_mpu9250_wrapper.cpp | 39 +++++++++++--------
 1 file changed, 23 insertions(+), 16 deletions(-)

diff --git a/src/platforms/posix/drivers/df_mpu9250_wrapper/df_mpu9250_wrapper.cpp b/src/platforms/posix/drivers/df_mpu9250_wrapper/df_mpu9250_wrapper.cpp
index fe166d5f22..251ce72f2a 100644
--- a/src/platforms/posix/drivers/df_mpu9250_wrapper/df_mpu9250_wrapper.cpp
+++ b/src/platforms/posix/drivers/df_mpu9250_wrapper/df_mpu9250_wrapper.cpp
@@ -61,6 +61,8 @@
 #include <drivers/drv_mag.h>
 #include <drivers/device/integrator.h>
 
+#include <lib/conversion/rotation.h>
+
 #include <uORB/topics/parameter_update.h>
 
 #include <mpu9250/MPU9250.hpp>
@@ -78,7 +80,7 @@ using namespace DriverFramework;
 class DfMpu9250Wrapper : public MPU9250
 {
 public:
-	DfMpu9250Wrapper(bool mag_enabled);
+	DfMpu9250Wrapper(bool mag_enabled, enum Rotation rotation);
 	~DfMpu9250Wrapper();
 
 
@@ -108,8 +110,6 @@ private:
 	void _update_gyro_calibration();
 	void _update_mag_calibration();
 
-	//enum Rotation		_rotation;
-
 	orb_advert_t		    _accel_topic;
 	orb_advert_t		    _gyro_topic;
 	orb_advert_t		    _mag_topic;
@@ -136,6 +136,8 @@ private:
 		float z_scale;
 	} _gyro_calibration;
 
+	math::Matrix<3, 3>	    _rotation_matrix;
+
 	struct mag_calibration_s {
 		float x_offset;
 		float x_scale;
@@ -169,7 +171,7 @@ private:
 	bool _mag_enabled;
 };
 
-DfMpu9250Wrapper::DfMpu9250Wrapper(bool mag_enabled) :
+DfMpu9250Wrapper::DfMpu9250Wrapper(bool mag_enabled, enum Rotation rotation) :
 	MPU9250(IMU_DEVICE_PATH, mag_enabled),
 	_accel_topic(nullptr),
 	_gyro_topic(nullptr),
@@ -184,7 +186,6 @@ DfMpu9250Wrapper::DfMpu9250Wrapper(bool mag_enabled) :
 	_mag_orb_class_instance(-1),
 	_accel_int(MPU9250_NEVER_AUTOPUBLISH_US, false),
 	_gyro_int(MPU9250_NEVER_AUTOPUBLISH_US, true),
-	/*_rotation(rotation)*/
 	_publish_count(0),
 	_read_counter(perf_alloc(PC_COUNT, "mpu9250_reads")),
 	_error_counter(perf_alloc(PC_COUNT, "mpu9250_errors")),
@@ -221,6 +222,9 @@ DfMpu9250Wrapper::DfMpu9250Wrapper(bool mag_enabled) :
 		_mag_calibration.y_offset = 0.0f;
 		_mag_calibration.z_offset = 0.0f;
 	}
+
+	// Get sensor rotation matrix
+	get_rot_matrix(rotation, &_rotation_matrix);
 }
 
 DfMpu9250Wrapper::~DfMpu9250Wrapper()
@@ -576,6 +580,8 @@ int DfMpu9250Wrapper::_publish(struct imu_sensor_data &data)
 				  (data.accel_m_s2_y - _accel_calibration.y_offset) * _accel_calibration.y_scale,
 				  (data.accel_m_s2_z - _accel_calibration.z_offset) * _accel_calibration.z_scale);
 
+	// apply sensor rotation on the accel measurement
+	accel_val = _rotation_matrix * accel_val;
 
 	_accel_int.put_with_interval(data.fifo_sample_interval_us,
 				     accel_val,
@@ -586,7 +592,8 @@ int DfMpu9250Wrapper::_publish(struct imu_sensor_data &data)
 				 (data.gyro_rad_s_y - _gyro_calibration.y_offset) * _gyro_calibration.y_scale,
 				 (data.gyro_rad_s_z - _gyro_calibration.z_offset) * _gyro_calibration.z_scale);
 
-	math::Vector<3> gyro_val_integrated_unused;
+	// apply sensor rotation on the gyro measurement
+	gyro_val = _rotation_matrix * gyro_val;
 
 	_gyro_int.put_with_interval(data.fifo_sample_interval_us,
 				    gyro_val,
@@ -746,14 +753,14 @@ namespace df_mpu9250_wrapper
 
 DfMpu9250Wrapper *g_dev = nullptr;
 
-int start(/* enum Rotation rotation */);
+int start(enum Rotation rotation);
 int stop();
 int info();
 void usage();
 
-int start(bool mag_enabled)
+int start(bool mag_enabled, enum Rotation rotation)
 {
-	g_dev = new DfMpu9250Wrapper(mag_enabled);
+	g_dev = new DfMpu9250Wrapper(mag_enabled, rotation);
 
 	if (g_dev == nullptr) {
 		PX4_ERR("failed instantiating DfMpu9250Wrapper object");
@@ -823,7 +830,7 @@ usage()
 {
 	PX4_WARN("Usage: df_mpu9250_wrapper 'start', 'start_without_mag', 'info', 'stop'");
 	PX4_WARN("options:");
-	//PX4_WARN("    -R rotation");
+	PX4_WARN("    -R rotation");
 }
 
 } // namespace df_mpu9250_wrapper
@@ -833,7 +840,7 @@ int
 df_mpu9250_wrapper_main(int argc, char *argv[])
 {
 	int ch;
-	// enum Rotation rotation = ROTATION_NONE;
+	enum Rotation rotation = ROTATION_NONE;
 	int ret = 0;
 	int myoptind = 1;
 	const char *myoptarg = NULL;
@@ -841,9 +848,9 @@ df_mpu9250_wrapper_main(int argc, char *argv[])
 	/* jump over start/off/etc and look at options first */
 	while ((ch = px4_getopt(argc, argv, "R:", &myoptind, &myoptarg)) != EOF) {
 		switch (ch) {
-		//case 'R':
-		//	rotation = (enum Rotation)atoi(myoptarg);
-		//	break;
+		case 'R':
+			rotation = (enum Rotation)atoi(myoptarg);
+			break;
 
 		default:
 			df_mpu9250_wrapper::usage();
@@ -859,11 +866,11 @@ df_mpu9250_wrapper_main(int argc, char *argv[])
 	const char *verb = argv[myoptind];
 
 	if (!strcmp(verb, "start_without_mag")) {
-		ret = df_mpu9250_wrapper::start(false);
+		ret = df_mpu9250_wrapper::start(false, rotation);
 	}
 
 	else if (!strcmp(verb, "start")) {
-		ret = df_mpu9250_wrapper::start(true);
+		ret = df_mpu9250_wrapper::start(true, rotation);
 	}
 
 	else if (!strcmp(verb, "stop")) {

From 969940e98a60da5193bc44f1eebefbd71b359b68 Mon Sep 17 00:00:00 2001
From: Julian Oes <julian@oes.ch>
Date: Tue, 31 May 2016 17:31:24 +0100
Subject: [PATCH 0428/1230] df_mpu9250_wrapper: add rotation to internal mag

---
 .../df_mpu9250_wrapper/df_mpu9250_wrapper.cpp       | 13 ++++++++++---
 1 file changed, 10 insertions(+), 3 deletions(-)

diff --git a/src/platforms/posix/drivers/df_mpu9250_wrapper/df_mpu9250_wrapper.cpp b/src/platforms/posix/drivers/df_mpu9250_wrapper/df_mpu9250_wrapper.cpp
index 251ce72f2a..fa68568f3b 100644
--- a/src/platforms/posix/drivers/df_mpu9250_wrapper/df_mpu9250_wrapper.cpp
+++ b/src/platforms/posix/drivers/df_mpu9250_wrapper/df_mpu9250_wrapper.cpp
@@ -687,9 +687,16 @@ int DfMpu9250Wrapper::_publish(struct imu_sensor_data &data)
 	accel_report.z = accel_val_filt(2);
 
 	if (_mag_enabled) {
-		mag_report.x = (data.mag_ga_x - _mag_calibration.x_offset) * _mag_calibration.x_scale;
-		mag_report.y = (data.mag_ga_y - _mag_calibration.y_offset) * _mag_calibration.y_scale;
-		mag_report.z = (data.mag_ga_z - _mag_calibration.z_offset) * _mag_calibration.z_scale;
+
+		math::Vector<3> mag_val((data.mag_ga_x - _mag_calibration.x_offset) * _mag_calibration.x_scale,
+					(data.mag_ga_y - _mag_calibration.y_offset) * _mag_calibration.y_scale,
+					(data.mag_ga_z - _mag_calibration.z_offset) * _mag_calibration.z_scale);
+
+		mag_val = _rotation_matrix * mag_val;
+
+		mag_report.x = mag_val(0);
+		mag_report.y = mag_val(1);
+		mag_report.z = mag_val(2);
 	}
 
 	gyro_report.x_integral = gyro_val_integ(0);

From d5a7aaa09df7dea7920eba55a51841773c4b0f5f Mon Sep 17 00:00:00 2001
From: Julian Oes <julian@oes.ch>
Date: Tue, 31 May 2016 18:04:42 +0100
Subject: [PATCH 0429/1230] df_hmc5883_wrapper: rotation on driver level

---
 .../df_hmc5883_wrapper/df_hmc5883_wrapper.cpp | 54 +++++++++++--------
 1 file changed, 33 insertions(+), 21 deletions(-)

diff --git a/src/platforms/posix/drivers/df_hmc5883_wrapper/df_hmc5883_wrapper.cpp b/src/platforms/posix/drivers/df_hmc5883_wrapper/df_hmc5883_wrapper.cpp
index 22e410ea8d..3fc6a33850 100644
--- a/src/platforms/posix/drivers/df_hmc5883_wrapper/df_hmc5883_wrapper.cpp
+++ b/src/platforms/posix/drivers/df_hmc5883_wrapper/df_hmc5883_wrapper.cpp
@@ -59,8 +59,8 @@
 #include <uORB/topics/parameter_update.h>
 
 #include <board_config.h>
-//#include <mathlib/math/filter/LowPassFilter2p.hpp>
-//#include <lib/conversion/rotation.h>
+
+#include <lib/conversion/rotation.h>
 
 #include <hmc5883/HMC5883.hpp>
 #include <DevMgr.hpp>
@@ -74,7 +74,7 @@ using namespace DriverFramework;
 class DfHmc9250Wrapper : public HMC5883
 {
 public:
-	DfHmc9250Wrapper(/*enum Rotation rotation*/);
+	DfHmc9250Wrapper(enum Rotation rotation);
 	~DfHmc9250Wrapper();
 
 
@@ -97,8 +97,6 @@ private:
 
 	void _update_mag_calibration();
 
-	//enum Rotation		_rotation;
-
 	orb_advert_t		_mag_topic;
 
 	int			_param_update_sub;
@@ -112,20 +110,21 @@ private:
 		float z_scale;
 	} _mag_calibration;
 
+	math::Matrix<3, 3>      _rotation_matrix;
+
 	int			_mag_orb_class_instance;
 
 	perf_counter_t		_mag_sample_perf;
 
 };
 
-DfHmc9250Wrapper::DfHmc9250Wrapper(/*enum Rotation rotation*/) :
+DfHmc9250Wrapper::DfHmc9250Wrapper(enum Rotation rotation) :
 	HMC5883(MAG_DEVICE_PATH),
 	_mag_topic(nullptr),
 	_param_update_sub(-1),
 	_mag_calibration{},
 	_mag_orb_class_instance(-1),
 	_mag_sample_perf(perf_alloc(PC_ELAPSED, "df_mag_read"))
-	/*_rotation(rotation)*/
 {
 	// Set sane default calibration values
 	_mag_calibration.x_scale = 1.0f;
@@ -134,6 +133,9 @@ DfHmc9250Wrapper::DfHmc9250Wrapper(/*enum Rotation rotation*/) :
 	_mag_calibration.x_offset = 0.0f;
 	_mag_calibration.y_offset = 0.0f;
 	_mag_calibration.z_offset = 0.0f;
+
+	// Get sensor rotation matrix
+	get_rot_matrix(rotation, &_rotation_matrix);
 }
 
 DfHmc9250Wrapper::~DfHmc9250Wrapper()
@@ -278,9 +280,19 @@ int DfHmc9250Wrapper::_publish(struct mag_sensor_data &data)
 	mag_report.x_raw = NAN;
 	mag_report.y_raw = NAN;
 	mag_report.z_raw = NAN;
-	mag_report.x = (data.field_x_ga - _mag_calibration.x_offset) * _mag_calibration.x_scale;
-	mag_report.y = (data.field_y_ga - _mag_calibration.y_offset) * _mag_calibration.y_scale;
-	mag_report.z = (data.field_z_ga - _mag_calibration.z_offset) * _mag_calibration.z_scale;
+
+
+	math::Vector<3> mag_val((data.mag_ga_x - _mag_calibration.x_offset) * _mag_calibration.x_scale,
+				(data.mag_ga_y - _mag_calibration.y_offset) * _mag_calibration.y_scale,
+				(data.mag_ga_z - _mag_calibration.z_offset) * _mag_calibration.z_scale);
+
+	// apply sensor rotation on the accel measurement
+	mag_val = _rotation_matrix * mag_val;
+
+	mag_report.x = mag_val(0);
+	mag_report.y = mag_val(1);
+	mag_report.z = mag_val(2);
+
 
 	// TODO: get these right
 	//mag_report.scaling = -1.0f;
@@ -315,14 +327,14 @@ namespace df_hmc5883_wrapper
 
 DfHmc9250Wrapper *g_dev = nullptr;
 
-int start(/* enum Rotation rotation */);
+int start(enum Rotation rotation);
 int stop();
 int info();
 void usage();
 
-int start(/*enum Rotation rotation*/)
+int start(enum Rotation rotation)
 {
-	g_dev = new DfHmc9250Wrapper(/*rotation*/);
+	g_dev = new DfHmc9250Wrapper(rotation);
 
 	if (g_dev == nullptr) {
 		PX4_ERR("failed instantiating DfHmc9250Wrapper object");
@@ -389,9 +401,9 @@ info()
 void
 usage()
 {
-	PX4_WARN("Usage: df_hmc5883_wrapper 'start', 'info', 'stop'");
-	PX4_WARN("options:");
-	//PX4_WARN("    -R rotation");
+	PX4_INFO("Usage: df_hmc5883_wrapper 'start', 'info', 'stop'");
+	PX4_INFO("options:");
+	PX4_INFO("    -R rotation");
 }
 
 } // namespace df_hmc5883_wrapper
@@ -401,7 +413,7 @@ int
 df_hmc5883_wrapper_main(int argc, char *argv[])
 {
 	int ch;
-	// enum Rotation rotation = ROTATION_NONE;
+	enum Rotation rotation = ROTATION_NONE;
 	int ret = 0;
 	int myoptind = 1;
 	const char *myoptarg = NULL;
@@ -409,9 +421,9 @@ df_hmc5883_wrapper_main(int argc, char *argv[])
 	/* jump over start/off/etc and look at options first */
 	while ((ch = px4_getopt(argc, argv, "R:", &myoptind, &myoptarg)) != EOF) {
 		switch (ch) {
-		//case 'R':
-		//	rotation = (enum Rotation)atoi(myoptarg);
-		//	break;
+		case 'R':
+			rotation = (enum Rotation)atoi(myoptarg);
+			break;
 
 		default:
 			df_hmc5883_wrapper::usage();
@@ -428,7 +440,7 @@ df_hmc5883_wrapper_main(int argc, char *argv[])
 
 
 	if (!strcmp(verb, "start")) {
-		ret = df_hmc5883_wrapper::start(/*rotation*/);
+		ret = df_hmc5883_wrapper::start(rotation);
 	}
 
 	else if (!strcmp(verb, "stop")) {

From 3e176fa91f949e6a511dfe2eebdd7e0138efe4f4 Mon Sep 17 00:00:00 2001
From: Julian Oes <julian@oes.ch>
Date: Tue, 31 May 2016 18:05:18 +0100
Subject: [PATCH 0430/1230] df_mpu9250_wrapper: usage is info

---
 .../posix/drivers/df_mpu9250_wrapper/df_mpu9250_wrapper.cpp | 6 +++---
 1 file changed, 3 insertions(+), 3 deletions(-)

diff --git a/src/platforms/posix/drivers/df_mpu9250_wrapper/df_mpu9250_wrapper.cpp b/src/platforms/posix/drivers/df_mpu9250_wrapper/df_mpu9250_wrapper.cpp
index fa68568f3b..54b16d91b2 100644
--- a/src/platforms/posix/drivers/df_mpu9250_wrapper/df_mpu9250_wrapper.cpp
+++ b/src/platforms/posix/drivers/df_mpu9250_wrapper/df_mpu9250_wrapper.cpp
@@ -835,9 +835,9 @@ info()
 void
 usage()
 {
-	PX4_WARN("Usage: df_mpu9250_wrapper 'start', 'start_without_mag', 'info', 'stop'");
-	PX4_WARN("options:");
-	PX4_WARN("    -R rotation");
+	PX4_INFO("Usage: df_mpu9250_wrapper 'start', 'start_without_mag', 'info', 'stop'");
+	PX4_INFO("options:");
+	PX4_INFO("    -R rotation");
 }
 
 } // namespace df_mpu9250_wrapper

From 144b0a179a31f6efafed64cd185b9e0dbd2f9cae Mon Sep 17 00:00:00 2001
From: Julian Oes <julian@oes.ch>
Date: Tue, 31 May 2016 18:08:39 +0100
Subject: [PATCH 0431/1230] df_hmc5883_wrapper: fix wrong variable name

---
 .../posix/drivers/df_hmc5883_wrapper/df_hmc5883_wrapper.cpp | 6 +++---
 1 file changed, 3 insertions(+), 3 deletions(-)

diff --git a/src/platforms/posix/drivers/df_hmc5883_wrapper/df_hmc5883_wrapper.cpp b/src/platforms/posix/drivers/df_hmc5883_wrapper/df_hmc5883_wrapper.cpp
index 3fc6a33850..978977048a 100644
--- a/src/platforms/posix/drivers/df_hmc5883_wrapper/df_hmc5883_wrapper.cpp
+++ b/src/platforms/posix/drivers/df_hmc5883_wrapper/df_hmc5883_wrapper.cpp
@@ -282,9 +282,9 @@ int DfHmc9250Wrapper::_publish(struct mag_sensor_data &data)
 	mag_report.z_raw = NAN;
 
 
-	math::Vector<3> mag_val((data.mag_ga_x - _mag_calibration.x_offset) * _mag_calibration.x_scale,
-				(data.mag_ga_y - _mag_calibration.y_offset) * _mag_calibration.y_scale,
-				(data.mag_ga_z - _mag_calibration.z_offset) * _mag_calibration.z_scale);
+	math::Vector<3> mag_val((data.field_x_ga - _mag_calibration.x_offset) * _mag_calibration.x_scale,
+				(data.field_y_ga - _mag_calibration.y_offset) * _mag_calibration.y_scale,
+				(data.field_z_ga - _mag_calibration.z_offset) * _mag_calibration.z_scale);
 
 	// apply sensor rotation on the accel measurement
 	mag_val = _rotation_matrix * mag_val;

From 8f4ca16bae22b93ed669baacd80a022f33027071 Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Wed, 1 Jun 2016 17:12:02 +0200
Subject: [PATCH 0432/1230] FMUv1: Fix board config

---
 src/drivers/boards/px4fmu-v1/board_config.h | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/src/drivers/boards/px4fmu-v1/board_config.h b/src/drivers/boards/px4fmu-v1/board_config.h
index ec01c1bd5f..5f6edb6d70 100644
--- a/src/drivers/boards/px4fmu-v1/board_config.h
+++ b/src/drivers/boards/px4fmu-v1/board_config.h
@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2012-2014 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2012-2016 PX4 Development Team. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions

From eb7970bee2accad51f40669b78a610ce26bc806a Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Wed, 1 Jun 2016 17:12:19 +0200
Subject: [PATCH 0433/1230] MindPX: Save 3KB of RAM

---
 src/drivers/boards/mindpx-v2/board_config.h | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/src/drivers/boards/mindpx-v2/board_config.h b/src/drivers/boards/mindpx-v2/board_config.h
index 5bd7e39c77..207bf577f2 100644
--- a/src/drivers/boards/mindpx-v2/board_config.h
+++ b/src/drivers/boards/mindpx-v2/board_config.h
@@ -321,7 +321,7 @@ __BEGIN_DECLS
 
 /* This board provides a DMA pool and APIs */
 
-#define BOARD_DMA_ALLOC_POOL_SIZE 8192
+#define BOARD_DMA_ALLOC_POOL_SIZE 5120
 
 /****************************************************************************************************
  * Public Types

From ae5527fac71fa8a5ebaa89274fd2b8d7fe2549ae Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Wed, 1 Jun 2016 17:12:34 +0200
Subject: [PATCH 0434/1230] FMUv2: Save 3KB of RAM

---
 src/drivers/boards/px4fmu-v2/board_config.h | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/src/drivers/boards/px4fmu-v2/board_config.h b/src/drivers/boards/px4fmu-v2/board_config.h
index e0b916c5cc..c2c408b7af 100644
--- a/src/drivers/boards/px4fmu-v2/board_config.h
+++ b/src/drivers/boards/px4fmu-v2/board_config.h
@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2013, 2014 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2013-2016 PX4 Development Team. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -272,7 +272,7 @@ __BEGIN_DECLS
 
 /* This board provides a DMA pool and APIs */
 
-#define BOARD_DMA_ALLOC_POOL_SIZE 8192
+#define BOARD_DMA_ALLOC_POOL_SIZE 5120
 /****************************************************************************************************
  * Public Types
  ****************************************************************************************************/

From f79adacc9b908528ca8119ded9746215cc0c29d6 Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Wed, 1 Jun 2016 17:12:52 +0200
Subject: [PATCH 0435/1230] FMUv4: Save 3KB of RAM

---
 src/drivers/boards/px4fmu-v4/board_config.h | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/src/drivers/boards/px4fmu-v4/board_config.h b/src/drivers/boards/px4fmu-v4/board_config.h
index bf18bd9099..9c6584c4e5 100644
--- a/src/drivers/boards/px4fmu-v4/board_config.h
+++ b/src/drivers/boards/px4fmu-v4/board_config.h
@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2013, 2014 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2013-2016 PX4 Development Team. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -299,7 +299,7 @@ __BEGIN_DECLS
 
 /* This board provides a DMA pool and APIs */
 
-#define BOARD_DMA_ALLOC_POOL_SIZE 8192
+#define BOARD_DMA_ALLOC_POOL_SIZE 5120
 
 /****************************************************************************************************
  * Public Types

From 64109daff888996feadc2b922e40cde7f6793f34 Mon Sep 17 00:00:00 2001
From: Julian Oes <julian@oes.ch>
Date: Wed, 1 Jun 2016 16:15:27 +0100
Subject: [PATCH 0436/1230] land_detector: fix timestamp type (#4710)

The overflow of the uint32_t lead to the land_detector start getting
aborted.
---
 src/modules/land_detector/land_detector_main.cpp | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/src/modules/land_detector/land_detector_main.cpp b/src/modules/land_detector/land_detector_main.cpp
index 5572c1b2f1..bc9b9f8d76 100644
--- a/src/modules/land_detector/land_detector_main.cpp
+++ b/src/modules/land_detector/land_detector_main.cpp
@@ -140,7 +140,7 @@ static int land_detector_start(const char *mode)
 	}
 
 	/* avoid memory fragmentation by not exiting start handler until the task has fully started */
-	const uint32_t timeout = hrt_absolute_time() + 5000000; //5 second timeout
+	const uint64_t timeout = hrt_absolute_time() + 5000000; //5 second timeout
 
 	/* avoid printing dots just yet and do one sleep before the first check */
 	usleep(10000);

From 201b5e36fa191810bb46584451d9167fe23ef632 Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Wed, 1 Jun 2016 17:54:19 +0200
Subject: [PATCH 0437/1230] Update SITL Gazebo

---
 Tools/sitl_gazebo | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/Tools/sitl_gazebo b/Tools/sitl_gazebo
index 96f1169cf9..9247cdf57b 160000
--- a/Tools/sitl_gazebo
+++ b/Tools/sitl_gazebo
@@ -1 +1 @@
-Subproject commit 96f1169cf95e92e07c1edc2a15709be835e6d274
+Subproject commit 9247cdf57b4108b1be3cb4babe8f89e6268a2464

From a7946aa7716fe2e68a467b1510f590538700ce52 Mon Sep 17 00:00:00 2001
From: David Sidrane <david_s5@nscdg.com>
Date: Wed, 1 Jun 2016 06:35:42 -1000
Subject: [PATCH 0438/1230] Fixed Spelling (#4713)

---
 src/drivers/boards/common/board_common.h    |  4 ++--
 src/drivers/boards/common/board_dma_alloc.c | 12 ++++++------
 src/modules/systemlib/printload.c           |  8 ++++----
 3 files changed, 12 insertions(+), 12 deletions(-)

diff --git a/src/drivers/boards/common/board_common.h b/src/drivers/boards/common/board_common.h
index 2b802639a5..d98e218924 100644
--- a/src/drivers/boards/common/board_common.h
+++ b/src/drivers/boards/common/board_common.h
@@ -95,7 +95,7 @@ __EXPORT int board_dma_alloc_init(void);
  *
  ************************************************************************************/
 #if defined(BOARD_DMA_ALLOC_POOL_SIZE)
-__EXPORT int board_get_dma_usage(uint16_t *dma_total, uint16_t *dma_used, uint16_t *dma_peek_used);
+__EXPORT int board_get_dma_usage(uint16_t *dma_total, uint16_t *dma_used, uint16_t *dma_peak_used);
 #else
-#define board_get_dma_usage(dma_total,dma_used, dma_peek_used) (-ENOMEM)
+#define board_get_dma_usage(dma_total,dma_used, dma_peak_used) (-ENOMEM)
 #endif
diff --git a/src/drivers/boards/common/board_dma_alloc.c b/src/drivers/boards/common/board_dma_alloc.c
index 2c7126d1a0..9ba4f23483 100644
--- a/src/drivers/boards/common/board_dma_alloc.c
+++ b/src/drivers/boards/common/board_dma_alloc.c
@@ -91,7 +91,7 @@ static GRAN_HANDLE dma_allocator;
 static uint8_t g_dma_heap[BOARD_DMA_ALLOC_POOL_SIZE] __attribute__((aligned(64)));
 static perf_counter_t g_dma_perf;
 static uint16_t dma_heap_inuse;
-static uint16_t dma_heap_peek_use;
+static uint16_t dma_heap_peak_use;
 /****************************************************************************
  * Public Functions
  ****************************************************************************/
@@ -107,18 +107,18 @@ __EXPORT int board_dma_alloc_init(void)
 
 	} else {
 		dma_heap_inuse = 0;
-		dma_heap_peek_use = 0;
+		dma_heap_peak_use = 0;
 		g_dma_perf = perf_alloc(PC_COUNT, "dma_alloc");
 	}
 
 	return OK;
 }
 
-__EXPORT int board_get_dma_usage(uint16_t *dma_total, uint16_t *dma_used, uint16_t *dma_peek_used)
+__EXPORT int board_get_dma_usage(uint16_t *dma_total, uint16_t *dma_used, uint16_t *dma_peak_used)
 {
 	*dma_total = sizeof(g_dma_heap);
 	*dma_used = dma_heap_inuse;
-	*dma_peek_used = dma_heap_peek_use;
+	*dma_peak_used = dma_heap_peak_use;
 
 	return OK;
 }
@@ -139,8 +139,8 @@ fat_dma_alloc(size_t size)
 	if (rv != NULL) {
 		dma_heap_inuse += size;
 
-		if (dma_heap_inuse > dma_heap_peek_use) {
-			dma_heap_peek_use = dma_heap_inuse;
+		if (dma_heap_inuse > dma_heap_peak_use) {
+			dma_heap_peak_use = dma_heap_inuse;
 		}
 	}
 
diff --git a/src/modules/systemlib/printload.c b/src/modules/systemlib/printload.c
index 6955b4c6ef..fa15ae99fc 100644
--- a/src/modules/systemlib/printload.c
+++ b/src/modules/systemlib/printload.c
@@ -218,14 +218,14 @@ void print_load(uint64_t t, int fd, struct print_load_s *print_state)
 #if defined(BOARD_DMA_ALLOC_POOL_SIZE)
 				uint16_t dma_total;
 				uint16_t dma_used;
-				uint16_t dma_peek_used;
+				uint16_t dma_peak_used;
 
-				if (board_get_dma_usage(&dma_total, &dma_used, &dma_peek_used) >= 0) {
-					dprintf(fd, "%sDMA Memory: %d total, %d used %d peek\n",
+				if (board_get_dma_usage(&dma_total, &dma_used, &dma_peak_used) >= 0) {
+					dprintf(fd, "%sDMA Memory: %d total, %d used %d peak\n",
 						clear_line,
 						dma_total,
 						dma_used,
-						dma_peek_used);
+						dma_peak_used);
 				}
 
 #endif

From 6ac0eabb5a0cb8c0a6e9c617ba9d6450872f4abb Mon Sep 17 00:00:00 2001
From: Mark Charlebois <charlebm@gmail.com>
Date: Tue, 24 May 2016 14:39:17 -0700
Subject: [PATCH 0439/1230] Changes required to support Hexagon SDK 3.0

The inc and lib directories were renamed to incs and libs.

This requires an updated cmake_hexagon and come changes to qurt paths in
PX4.

Signed-off-by: Mark Charlebois <charlebm@gmail.com>
---
 cmake/cmake_hexagon                         | 2 +-
 cmake/configs/qurt_eagle_travis.cmake       | 2 +-
 src/modules/muorb/krait/CMakeLists.txt      | 8 ++++----
 src/platforms/qurt/px4_layer/CMakeLists.txt | 2 +-
 4 files changed, 7 insertions(+), 7 deletions(-)

diff --git a/cmake/cmake_hexagon b/cmake/cmake_hexagon
index 77d487dc07..e099d1f31d 160000
--- a/cmake/cmake_hexagon
+++ b/cmake/cmake_hexagon
@@ -1 +1 @@
-Subproject commit 77d487dc076a8519f81a335a5e4a2040799790de
+Subproject commit e099d1f31de5091e47e54b4215a1680815aa013d
diff --git a/cmake/configs/qurt_eagle_travis.cmake b/cmake/configs/qurt_eagle_travis.cmake
index eee24eda15..f2e8d494e0 100644
--- a/cmake/configs/qurt_eagle_travis.cmake
+++ b/cmake/configs/qurt_eagle_travis.cmake
@@ -15,7 +15,7 @@ else()
 	set(HEXAGON_SDK_ROOT $ENV{HEXAGON_SDK_ROOT})
 endif()
 
-include_directories(${HEXAGON_SDK_ROOT}/lib/common/qurt/ADSPv5MP/include)
+include_directories(${HEXAGON_SDK_ROOT}/libs/common/qurt/ADSPv5MP/include)
 
 set(config_generate_parameters_scope ALL)
 
diff --git a/src/modules/muorb/krait/CMakeLists.txt b/src/modules/muorb/krait/CMakeLists.txt
index fe4c509303..b9e054e3e7 100644
--- a/src/modules/muorb/krait/CMakeLists.txt
+++ b/src/modules/muorb/krait/CMakeLists.txt
@@ -30,11 +30,11 @@
 # POSSIBILITY OF SUCH DAMAGE.
 #
 ############################################################################
+set(CMAKE_MODULE_PATH ${CMAKE_MODULE_PATH} "${CMAKE_SOURCE_DIR}/cmake/cmake_hexagon")
+include(hexagon_sdk)
+
 include_directories(${CMAKE_BINARY_DIR}/src/firmware/posix)
-include_directories(
-	${HEXAGON_SDK_ROOT}/inc/stddef
-	${HEXAGON_SDK_ROOT}/lib/common/rpcmem
-	)
+include_directories(${HEXAGON_SDK_INCLUDES})
 
 px4_add_module(
 	MODULE modules__muorb__krait
diff --git a/src/platforms/qurt/px4_layer/CMakeLists.txt b/src/platforms/qurt/px4_layer/CMakeLists.txt
index 6196d1d43f..283f31d761 100644
--- a/src/platforms/qurt/px4_layer/CMakeLists.txt
+++ b/src/platforms/qurt/px4_layer/CMakeLists.txt
@@ -31,7 +31,7 @@
 #
 ############################################################################
 
-include_directories(${HEXAGON_SDK_ROOT}/lib/common/qurt/ADSPv5MP/include)
+include_directories(${HEXAGON_SDK_ROOT}/libs/common/qurt/ADSPv5MP/include)
 
 set(QURT_LAYER_SRCS
 	px4_qurt_impl.cpp

From 02e3f28d87d5b7fc7f444ed86759f2a7652b4d79 Mon Sep 17 00:00:00 2001
From: Mark Charlebois <charlebm@gmail.com>
Date: Tue, 24 May 2016 16:32:34 -0700
Subject: [PATCH 0440/1230] Added back support for SDK 2.0 as well as 3.0

Signed-off-by: Mark Charlebois <charlebm@gmail.com>
---
 cmake/cmake_hexagon                         | 2 +-
 cmake/configs/qurt_eagle_travis.cmake       | 2 +-
 src/platforms/qurt/px4_layer/CMakeLists.txt | 4 +++-
 3 files changed, 5 insertions(+), 3 deletions(-)

diff --git a/cmake/cmake_hexagon b/cmake/cmake_hexagon
index e099d1f31d..f5722e906c 160000
--- a/cmake/cmake_hexagon
+++ b/cmake/cmake_hexagon
@@ -1 +1 @@
-Subproject commit e099d1f31de5091e47e54b4215a1680815aa013d
+Subproject commit f5722e906c85fe2311c37f0833dff3f7c2f6e5cc
diff --git a/cmake/configs/qurt_eagle_travis.cmake b/cmake/configs/qurt_eagle_travis.cmake
index f2e8d494e0..8f135098b8 100644
--- a/cmake/configs/qurt_eagle_travis.cmake
+++ b/cmake/configs/qurt_eagle_travis.cmake
@@ -15,7 +15,7 @@ else()
 	set(HEXAGON_SDK_ROOT $ENV{HEXAGON_SDK_ROOT})
 endif()
 
-include_directories(${HEXAGON_SDK_ROOT}/libs/common/qurt/ADSPv5MP/include)
+include_directories(${HEXAGON_8074_INCLUDES})
 
 set(config_generate_parameters_scope ALL)
 
diff --git a/src/platforms/qurt/px4_layer/CMakeLists.txt b/src/platforms/qurt/px4_layer/CMakeLists.txt
index 283f31d761..38dbd8a46f 100644
--- a/src/platforms/qurt/px4_layer/CMakeLists.txt
+++ b/src/platforms/qurt/px4_layer/CMakeLists.txt
@@ -30,8 +30,10 @@
 # POSSIBILITY OF SUCH DAMAGE.
 #
 ############################################################################
+set(CMAKE_MODULE_PATH ${CMAKE_MODULE_PATH} "${CMAKE_SOURCE_DIR}/cmake/cmake_hexagon")
+include(hexagon_sdk)
 
-include_directories(${HEXAGON_SDK_ROOT}/libs/common/qurt/ADSPv5MP/include)
+include_directories(${HEXAGON_8074_INCLUDES})
 
 set(QURT_LAYER_SRCS
 	px4_qurt_impl.cpp

From 30a3311fe66d445d837ae100d79f3037e6ed3263 Mon Sep 17 00:00:00 2001
From: Mark Charlebois <charlebm@gmail.com>
Date: Tue, 24 May 2016 16:41:43 -0700
Subject: [PATCH 0441/1230] Fixed qurt_eagle_travis build

Added missing stub function and added SDK 2 and 3 support.

Signed-off-by: Mark Charlebois <charlebm@gmail.com>
---
 cmake/configs/qurt_eagle_travis.cmake  | 1 +
 src/platforms/qurt/stubs/stubs_posix.c | 5 +++++
 2 files changed, 6 insertions(+)

diff --git a/cmake/configs/qurt_eagle_travis.cmake b/cmake/configs/qurt_eagle_travis.cmake
index 8f135098b8..609ae54283 100644
--- a/cmake/configs/qurt_eagle_travis.cmake
+++ b/cmake/configs/qurt_eagle_travis.cmake
@@ -8,6 +8,7 @@ set(QURT_ENABLE_STUBS "1")
 set(CMAKE_TOOLCHAIN_FILE ${CMAKE_SOURCE_DIR}/cmake/cmake_hexagon/toolchain/Toolchain-qurt.cmake)
 
 set(CMAKE_MODULE_PATH ${CMAKE_MODULE_PATH} "${CMAKE_SOURCE_DIR}/cmake/cmake_hexagon")
+include(hexagon_sdk)
 
 if ("$ENV{HEXAGON_SDK_ROOT}" STREQUAL "")
 	message(FATAL_ERROR "Enviroment variable HEXAGON_SDK_ROOT must be set")
diff --git a/src/platforms/qurt/stubs/stubs_posix.c b/src/platforms/qurt/stubs/stubs_posix.c
index 16514aa72f..24b22f13f9 100644
--- a/src/platforms/qurt/stubs/stubs_posix.c
+++ b/src/platforms/qurt/stubs/stubs_posix.c
@@ -148,6 +148,11 @@ int pthread_mutexattr_settype(pthread_mutexattr_t *attr, int type)
 	return -1;
 }
 
+int pthread_condattr_init(pthread_condattr_t *attr)
+{
+	return -1;
+}
+
 int fsync(int fd)
 {
 	return -1;

From bea416e117657b0e165c3368b22694d533293e9b Mon Sep 17 00:00:00 2001
From: Mark Charlebois <charlebm@gmail.com>
Date: Tue, 24 May 2016 16:55:38 -0700
Subject: [PATCH 0442/1230] Updated cmake_hexagon for support of required posix
 build headers

Signed-off-by: Mark Charlebois <charlebm@gmail.com>
---
 cmake/cmake_hexagon | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/cmake/cmake_hexagon b/cmake/cmake_hexagon
index f5722e906c..4b13895e67 160000
--- a/cmake/cmake_hexagon
+++ b/cmake/cmake_hexagon
@@ -1 +1 @@
-Subproject commit f5722e906c85fe2311c37f0833dff3f7c2f6e5cc
+Subproject commit 4b13895e67978edd97482e9170386367a670baa7

From cc1578509114bb6a0ad48976f61b769a62b72d0f Mon Sep 17 00:00:00 2001
From: Mark Charlebois <charlebm@gmail.com>
Date: Tue, 24 May 2016 21:47:10 -0700
Subject: [PATCH 0443/1230] Updated cmake_hexagon

Signed-off-by: Mark Charlebois <charlebm@gmail.com>
---
 cmake/cmake_hexagon | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/cmake/cmake_hexagon b/cmake/cmake_hexagon
index 4b13895e67..09eaa583ae 160000
--- a/cmake/cmake_hexagon
+++ b/cmake/cmake_hexagon
@@ -1 +1 @@
-Subproject commit 4b13895e67978edd97482e9170386367a670baa7
+Subproject commit 09eaa583ae4bcbb7ddb66677acd238f4159ea1d8

From ec620ecc14a723a363f8fb91920e5c6f1a06df9f Mon Sep 17 00:00:00 2001
From: Mark Charlebois <charlebm@gmail.com>
Date: Tue, 24 May 2016 22:00:42 -0700
Subject: [PATCH 0444/1230] Updated cmake_hexagon for SDK 2.0 support

Signed-off-by: Mark Charlebois <charlebm@gmail.com>
---
 cmake/cmake_hexagon | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/cmake/cmake_hexagon b/cmake/cmake_hexagon
index 09eaa583ae..3187dd7648 160000
--- a/cmake/cmake_hexagon
+++ b/cmake/cmake_hexagon
@@ -1 +1 @@
-Subproject commit 09eaa583ae4bcbb7ddb66677acd238f4159ea1d8
+Subproject commit 3187dd7648adcb0abca7904e402507203e4c25a9

From 69976caca902a0270c4db9a232c0c3195ccc561e Mon Sep 17 00:00:00 2001
From: Kabir Mohammed <mhkabir98@gmail.com>
Date: Thu, 2 Jun 2016 10:07:19 +0530
Subject: [PATCH 0445/1230] param : fix shell handler instructions (#4716)

---
 src/systemcmds/param/param.c | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/src/systemcmds/param/param.c b/src/systemcmds/param/param.c
index 19be8fa0d6..12e7cca9a2 100644
--- a/src/systemcmds/param/param.c
+++ b/src/systemcmds/param/param.c
@@ -209,7 +209,7 @@ param_main(int argc, char *argv[])
 		}
 	}
 
-	warnx("expected a command, try 'load', 'import', 'show', 'set', 'compare',\n'index', 'index_used', 'select' or 'save'");
+	warnx("expected a command, try 'load', 'import', 'show', 'set', 'compare',\n'index', 'index_used', 'greater', 'select', 'save', or 'reset' ");
 	return 1;
 }
 

From bdf064fd8f8d5b7d3ef4a937f2dfc9e4e7b26551 Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Thu, 2 Jun 2016 06:45:32 +0200
Subject: [PATCH 0446/1230] Update SITL Gazebo plugin

---
 Tools/sitl_gazebo | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/Tools/sitl_gazebo b/Tools/sitl_gazebo
index 9247cdf57b..e929eb4af0 160000
--- a/Tools/sitl_gazebo
+++ b/Tools/sitl_gazebo
@@ -1 +1 @@
-Subproject commit 9247cdf57b4108b1be3cb4babe8f89e6268a2464
+Subproject commit e929eb4af063800bb772466b5561bb5be12cd81e

From 5b1273e3348ef705abe1c56bd7c7dc035f338f60 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Beat=20K=C3=BCng?= <beat-kueng@gmx.net>
Date: Mon, 2 May 2016 09:58:30 +0200
Subject: [PATCH 0447/1230] orb: add optional queuing of messages

This adds two uORB API calls:
- orb_advertise_queue
- orb_advertise_multi_queue

Both add a queue_size parameter to define a maximum number of buffered
item. The existing orb calls use all a queue size of one and thus their
behavior is unchanged. If a writer publishes too fast, the oldest elements
from the queue are silently dropped.
The returned timestamp is always the one from the latest message in the
queue.

Queue size can be set via ioctl during advertisement phase. After that it
cannot be changed anymore.
---
 src/drivers/drv_orb_dev.h              |  3 ++
 src/modules/uORB/uORB.cpp              | 11 ++++++
 src/modules/uORB/uORB.h                | 12 ++++++
 src/modules/uORB/uORBDevices_nuttx.cpp | 51 ++++++++++++++++++++++----
 src/modules/uORB/uORBDevices_nuttx.hpp | 13 ++++++-
 src/modules/uORB/uORBDevices_posix.cpp | 51 ++++++++++++++++++++++----
 src/modules/uORB/uORBDevices_posix.hpp | 13 ++++++-
 src/modules/uORB/uORBManager.cpp       | 15 ++++++--
 src/modules/uORB/uORBManager.hpp       | 11 ++++--
 9 files changed, 156 insertions(+), 24 deletions(-)

diff --git a/src/drivers/drv_orb_dev.h b/src/drivers/drv_orb_dev.h
index 79a2895fb5..710eb3d175 100644
--- a/src/drivers/drv_orb_dev.h
+++ b/src/drivers/drv_orb_dev.h
@@ -87,4 +87,7 @@
 /** Get the priority for the topic */
 #define ORBIOCGPRIORITY		_ORBIOC(14)
 
+/** Set the queue size of the topic */
+#define ORBIOCSETQUEUESIZE	_ORBIOC(15)
+
 #endif /* _DRV_UORB_H */
diff --git a/src/modules/uORB/uORB.cpp b/src/modules/uORB/uORB.cpp
index 375f261ca8..cf32658673 100644
--- a/src/modules/uORB/uORB.cpp
+++ b/src/modules/uORB/uORB.cpp
@@ -45,12 +45,23 @@ orb_advert_t orb_advertise(const struct orb_metadata *meta, const void *data)
 	return uORB::Manager::get_instance()->orb_advertise(meta, data);
 }
 
+orb_advert_t orb_advertise_queue(const struct orb_metadata *meta, const void *data, int queue_size)
+{
+	return uORB::Manager::get_instance()->orb_advertise(meta, data, queue_size);
+}
+
 orb_advert_t orb_advertise_multi(const struct orb_metadata *meta, const void *data, int *instance,
 				 int priority)
 {
 	return uORB::Manager::get_instance()->orb_advertise_multi(meta, data, instance, priority);
 }
 
+orb_advert_t orb_advertise_multi_queue(const struct orb_metadata *meta, const void *data, int *instance,
+				       int priority, int queue_size)
+{
+	return uORB::Manager::get_instance()->orb_advertise_multi(meta, data, instance, priority, queue_size);
+}
+
 int orb_unadvertise(orb_advert_t handle)
 {
 	return uORB::Manager::get_instance()->orb_unadvertise(handle);
diff --git a/src/modules/uORB/uORB.h b/src/modules/uORB/uORB.h
index 57e12f9236..e189476f32 100644
--- a/src/modules/uORB/uORB.h
+++ b/src/modules/uORB/uORB.h
@@ -140,12 +140,24 @@ typedef void 	*orb_advert_t;
  */
 extern orb_advert_t orb_advertise(const struct orb_metadata *meta, const void *data) __EXPORT;
 
+/**
+ * @see uORB::Manager::orb_advertise()
+ */
+extern orb_advert_t orb_advertise_queue(const struct orb_metadata *meta, const void *data,
+					int queue_size) __EXPORT;
+
 /**
  * @see uORB::Manager::orb_advertise_multi()
  */
 extern orb_advert_t orb_advertise_multi(const struct orb_metadata *meta, const void *data, int *instance,
 					int priority) __EXPORT;
 
+/**
+ * @see uORB::Manager::orb_advertise_multi()
+ */
+extern orb_advert_t orb_advertise_multi_queue(const struct orb_metadata *meta, const void *data, int *instance,
+		int priority, int queue_size) __EXPORT;
+
 /**
  * @see uORB::Manager::orb_unadvertise()
  */
diff --git a/src/modules/uORB/uORBDevices_nuttx.cpp b/src/modules/uORB/uORBDevices_nuttx.cpp
index 7eb6e52fa4..e27f55d297 100644
--- a/src/modules/uORB/uORBDevices_nuttx.cpp
+++ b/src/modules/uORB/uORBDevices_nuttx.cpp
@@ -50,7 +50,8 @@ uORB::DeviceNode::DeviceNode
 	const struct orb_metadata *meta,
 	const char *name,
 	const char *path,
-	int priority
+	int priority,
+	unsigned int queue_size
 ) :
 	CDev(name, path),
 	_meta(meta),
@@ -60,6 +61,7 @@ uORB::DeviceNode::DeviceNode
 	_publisher(0),
 	_priority(priority),
 	_published(false),
+	_queue_size(queue_size),
 	_IsRemoteSubscriberPresent(false),
 	_subscriber_count(0)
 {
@@ -185,13 +187,26 @@ uORB::DeviceNode::read(struct file *filp, char *buffer, size_t buflen)
 	 */
 	irqstate_t flags = px4_enter_critical_section();
 
-	/* if the caller doesn't want the data, don't give it to them */
-	if (nullptr != buffer) {
-		memcpy(buffer, _data, _meta->o_size);
+	if (_generation > sd->generation + _queue_size) {
+		/* Reader is too far behind: some messages are lost */
+		sd->generation = _generation - _queue_size;
 	}
 
-	/* track the last generation that the file has seen */
-	sd->generation = _generation;
+	if (_generation == sd->generation && sd->generation > 0) {
+		/* The subscriber already read the latest message, but nothing new was published yet.
+		 * Return the previous message
+		 */
+		--sd->generation;
+	}
+
+	/* if the caller doesn't want the data, don't give it to them */
+	if (nullptr != buffer) {
+		memcpy(buffer, _data + (_meta->o_size * (sd->generation % _queue_size)), _meta->o_size);
+	}
+
+	if (sd->generation < _generation) {
+		++sd->generation;
+	}
 
 	/* set priority */
 	sd->priority = _priority;
@@ -226,7 +241,7 @@ uORB::DeviceNode::write(struct file *filp, const char *buffer, size_t buflen)
 
 			/* re-check size */
 			if (nullptr == _data) {
-				_data = new uint8_t[_meta->o_size];
+				_data = new uint8_t[_meta->o_size * _queue_size];
 			}
 
 			unlock();
@@ -245,10 +260,11 @@ uORB::DeviceNode::write(struct file *filp, const char *buffer, size_t buflen)
 
 	/* Perform an atomic copy. */
 	irqstate_t flags = px4_enter_critical_section();
-	memcpy(_data, buffer, _meta->o_size);
+	memcpy(_data + (_meta->o_size * (_generation % _queue_size)), buffer, _meta->o_size);
 
 	/* update the timestamp and generation count */
 	_last_update = hrt_absolute_time();
+	/* wrap-around happens after ~49 days, assuming a publisher rate of 1 kHz */
 	_generation++;
 
 	_published = true;
@@ -290,6 +306,11 @@ uORB::DeviceNode::ioctl(struct file *filp, int cmd, unsigned long arg)
 		*(int *)arg = sd->priority;
 		return OK;
 
+	case ORBIOCSETQUEUESIZE:
+		//no need for locking here, since this is used only during the advertisement call,
+		//and only one advertiser is allowed to open the DeviceNode at the same time.
+		return update_queue_size(arg);
+
 	default:
 		/* give it to the superclass */
 		return CDev::ioctl(filp, cmd, arg);
@@ -522,6 +543,20 @@ bool uORB::DeviceNode::is_published()
 	return _published;
 }
 
+int uORB::DeviceNode::update_queue_size(unsigned int queue_size)
+{
+	if (_queue_size == queue_size) {
+		return PX4_OK;
+	}
+
+	if (_data || _queue_size > queue_size) {
+		return ERROR;
+	}
+
+	_queue_size = queue_size;
+	return PX4_OK;
+}
+
 //-----------------------------------------------------------------------------
 //-----------------------------------------------------------------------------
 int16_t uORB::DeviceNode::process_add_subscription(int32_t rateInHz)
diff --git a/src/modules/uORB/uORBDevices_nuttx.hpp b/src/modules/uORB/uORBDevices_nuttx.hpp
index 1519b06a82..2015bff986 100644
--- a/src/modules/uORB/uORBDevices_nuttx.hpp
+++ b/src/modules/uORB/uORBDevices_nuttx.hpp
@@ -61,7 +61,8 @@ public:
 		const struct orb_metadata *meta,
 		const char *name,
 		const char *path,
-		int priority
+		int priority,
+		unsigned int queue_size = 1
 	);
 
 	/**
@@ -168,6 +169,15 @@ public:
 	 * and publish to this node or if another node should be tried. */
 	bool is_published();
 
+	/**
+	 * Try to change the size of the queue. This can only be done as long as nobody published yet.
+	 * This is the case, for example when orb_subscribe was called before an orb_advertise.
+	 * The queue size can only be increased.
+	 * @param queue_size new size of the queue
+	 * @return PX4_OK if queue size successfully set
+	 */
+	int update_queue_size(unsigned int queue_size);
+
 protected:
 	virtual pollevent_t poll_state(struct file *filp);
 	virtual void poll_notify_one(struct pollfd *fds, pollevent_t events);
@@ -190,6 +200,7 @@ private:
 					We allow one publisher to have an open file descriptor at the same time. */
 	const int   _priority;  /**< priority of topic */
 	bool _published;  /**< has ever data been published */
+	unsigned int _queue_size; /**< maximum number of elements in the queue */
 
 private: // private class methods.
 
diff --git a/src/modules/uORB/uORBDevices_posix.cpp b/src/modules/uORB/uORBDevices_posix.cpp
index d641859875..c5cd4b8fbd 100644
--- a/src/modules/uORB/uORBDevices_posix.cpp
+++ b/src/modules/uORB/uORBDevices_posix.cpp
@@ -61,7 +61,8 @@ uORB::DeviceNode::SubscriberData  *uORB::DeviceNode::filp_to_sd(device::file_t *
 	return sd;
 }
 
-uORB::DeviceNode::DeviceNode(const struct orb_metadata *meta, const char *name, const char *path, int priority) :
+uORB::DeviceNode::DeviceNode(const struct orb_metadata *meta, const char *name, const char *path,
+			     int priority, unsigned int queue_size) :
 	VDev(name, path),
 	_meta(meta),
 	_data(nullptr),
@@ -70,6 +71,7 @@ uORB::DeviceNode::DeviceNode(const struct orb_metadata *meta, const char *name,
 	_publisher(0),
 	_priority(priority),
 	_published(false),
+	_queue_size(queue_size),
 	_subscriber_count(0)
 {
 	// enable debug() calls
@@ -198,13 +200,26 @@ uORB::DeviceNode::read(device::file_t *filp, char *buffer, size_t buflen)
 	 */
 	lock();
 
-	/* if the caller doesn't want the data, don't give it to them */
-	if (nullptr != buffer) {
-		memcpy(buffer, _data, _meta->o_size);
+	if (_generation > sd->generation + _queue_size) {
+		/* Reader is too far behind: some messages are lost */
+		sd->generation = _generation - _queue_size;
 	}
 
-	/* track the last generation that the file has seen */
-	sd->generation = _generation;
+	if (_generation == sd->generation && sd->generation > 0) {
+		/* The subscriber already read the latest message, but nothing new was published yet.
+		 * Return the previous message
+		 */
+		--sd->generation;
+	}
+
+	/* if the caller doesn't want the data, don't give it to them */
+	if (nullptr != buffer) {
+		memcpy(buffer, _data + (_meta->o_size * (sd->generation % _queue_size)), _meta->o_size);
+	}
+
+	if (sd->generation < _generation) {
+		++sd->generation;
+	}
 
 	/* set priority */
 	sd->priority = _priority;
@@ -238,7 +253,7 @@ uORB::DeviceNode::write(device::file_t *filp, const char *buffer, size_t buflen)
 
 		/* re-check size */
 		if (nullptr == _data) {
-			_data = new uint8_t[_meta->o_size];
+			_data = new uint8_t[_meta->o_size * _queue_size];
 		}
 
 		unlock();
@@ -255,10 +270,11 @@ uORB::DeviceNode::write(device::file_t *filp, const char *buffer, size_t buflen)
 	}
 
 	lock();
-	memcpy(_data, buffer, _meta->o_size);
+	memcpy(_data + (_meta->o_size * (_generation % _queue_size)), buffer, _meta->o_size);
 
 	/* update the timestamp and generation count */
 	_last_update = hrt_absolute_time();
+	/* wrap-around happens after ~49 days, assuming a publisher rate of 1 kHz */
 	_generation++;
 
 	_published = true;
@@ -305,6 +321,11 @@ uORB::DeviceNode::ioctl(device::file_t *filp, int cmd, unsigned long arg)
 		*(int *)arg = sd->priority;
 		return PX4_OK;
 
+	case ORBIOCSETQUEUESIZE:
+		//no need for locking here, since this is used only during the advertisement call,
+		//and only one advertiser is allowed to open the DeviceNode at the same time.
+		return update_queue_size(arg);
+
 	default:
 		/* give it to the superclass */
 		return VDev::ioctl(filp, cmd, arg);
@@ -527,6 +548,20 @@ bool uORB::DeviceNode::is_published()
 	return _published;
 }
 
+int uORB::DeviceNode::update_queue_size(unsigned int queue_size)
+{
+	if (_queue_size == queue_size) {
+		return PX4_OK;
+	}
+
+	if (_data || _queue_size > queue_size) {
+		return ERROR;
+	}
+
+	_queue_size = queue_size;
+	return PX4_OK;
+}
+
 //-----------------------------------------------------------------------------
 //-----------------------------------------------------------------------------
 int16_t uORB::DeviceNode::process_add_subscription(int32_t rateInHz)
diff --git a/src/modules/uORB/uORBDevices_posix.hpp b/src/modules/uORB/uORBDevices_posix.hpp
index 963ed4a79d..35882accaa 100644
--- a/src/modules/uORB/uORBDevices_posix.hpp
+++ b/src/modules/uORB/uORBDevices_posix.hpp
@@ -48,7 +48,8 @@ class DeviceMaster;
 class uORB::DeviceNode : public device::VDev
 {
 public:
-	DeviceNode(const struct orb_metadata *meta, const char *name, const char *path, int priority);
+	DeviceNode(const struct orb_metadata *meta, const char *name, const char *path,
+		   int priority, unsigned int queue_size = 1);
 	~DeviceNode();
 
 	virtual int   open(device::file_t *filp);
@@ -105,6 +106,15 @@ public:
 	 * and publish to this node or if another node should be tried. */
 	bool is_published();
 
+	/**
+	 * Try to change the size of the queue. This can only be done as long as nobody published yet.
+	 * This is the case, for example when orb_subscribe was called before an orb_advertise.
+	 * The queue size can only be increased.
+	 * @param queue_size new size of the queue
+	 * @return PX4_OK if queue size successfully set
+	 */
+	int update_queue_size(unsigned int queue_size);
+
 protected:
 	virtual pollevent_t poll_state(device::file_t *filp);
 	virtual void    poll_notify_one(px4_pollfd_struct_t *fds, pollevent_t events);
@@ -128,6 +138,7 @@ private:
 					We allow one publisher to have an open file descriptor at the same time. */
 	const int   _priority;  /**< priority of topic */
 	bool _published;  /**< has ever data been published */
+	unsigned int _queue_size; /**< maximum number of elements in the queue */
 
 	static SubscriberData    *filp_to_sd(device::file_t *filp);
 
diff --git a/src/modules/uORB/uORBManager.cpp b/src/modules/uORB/uORBManager.cpp
index c1266ba04a..6c2309b13f 100644
--- a/src/modules/uORB/uORBManager.cpp
+++ b/src/modules/uORB/uORBManager.cpp
@@ -87,14 +87,14 @@ int uORB::Manager::orb_exists(const struct orb_metadata *meta, int instance)
 #endif
 }
 
-orb_advert_t uORB::Manager::orb_advertise(const struct orb_metadata *meta, const void *data)
+orb_advert_t uORB::Manager::orb_advertise(const struct orb_metadata *meta, const void *data, int queue_size)
 {
 	//warnx("orb_advertise meta = %p", meta);
-	return orb_advertise_multi(meta, data, nullptr, ORB_PRIO_DEFAULT);
+	return orb_advertise_multi(meta, data, nullptr, ORB_PRIO_DEFAULT, queue_size);
 }
 
 orb_advert_t uORB::Manager::orb_advertise_multi(const struct orb_metadata *meta, const void *data, int *instance,
-		int priority)
+		int priority, int queue_size)
 {
 	int result, fd;
 	orb_advert_t advertiser;
@@ -109,6 +109,15 @@ orb_advert_t uORB::Manager::orb_advertise_multi(const struct orb_metadata *meta,
 		return nullptr;
 	}
 
+	/* Set the queue size. This must be done before the first publication; thus it fails if
+	 * this is not the first advertiser.
+	 */
+	result = px4_ioctl(fd, ORBIOCSETQUEUESIZE, (unsigned long)queue_size);
+
+	if (result < 0 && queue_size > 1) {
+		PX4_WARN("orb_advertise_multi: failed to set queue size");
+	}
+
 	/* get the advertiser handle and close the node */
 	result = px4_ioctl(fd, ORBIOCGADVERTISER, (unsigned long)&advertiser);
 	px4_close(fd);
diff --git a/src/modules/uORB/uORBManager.hpp b/src/modules/uORB/uORBManager.hpp
index ca5aee846e..21f5777265 100644
--- a/src/modules/uORB/uORBManager.hpp
+++ b/src/modules/uORB/uORBManager.hpp
@@ -96,13 +96,15 @@ public:
 	 * @param data    A pointer to the initial data to be published.
 	 *      For topics updated by interrupt handlers, the advertisement
 	 *      must be performed from non-interrupt context.
+	 * @param queue_size  Maximum number of buffered elements. If this is 1, no queuing is
+	 *      used.
 	 * @return    nullptr on error, otherwise returns an object pointer
 	 *      that can be used to publish to the topic.
 	 *      If the topic in question is not known (due to an
 	 *      ORB_DEFINE with no corresponding ORB_DECLARE)
 	 *      this function will return nullptr and set errno to ENOENT.
 	 */
-	orb_advert_t orb_advertise(const struct orb_metadata *meta, const void *data);
+	orb_advert_t orb_advertise(const struct orb_metadata *meta, const void *data, int queue_size = 1);
 
 	/**
 	 * Advertise as the publisher of a topic.
@@ -130,6 +132,8 @@ public:
 	 *      instances, the priority allows the subscriber to prioritize the best
 	 *      data source as long as its available. The subscriber is responsible to check
 	 *      and handle different priorities (@see orb_priority()).
+	 * @param queue_size  Maximum number of buffered elements. If this is 1, no queuing is
+	 *      used.
 	 * @return    ERROR on error, otherwise returns a handle
 	 *      that can be used to publish to the topic.
 	 *      If the topic in question is not known (due to an
@@ -137,7 +141,7 @@ public:
 	 *      this function will return -1 and set errno to ENOENT.
 	 */
 	orb_advert_t orb_advertise_multi(const struct orb_metadata *meta, const void *data, int *instance,
-					 int priority) ;
+					 int priority, int queue_size = 1) ;
 
 
 	/**
@@ -272,7 +276,8 @@ public:
 	int  orb_check(int handle, bool *updated) ;
 
 	/**
-	 * Return the last time that the topic was updated.
+	 * Return the last time that the topic was updated. If a queue is used, it returns
+	 * the timestamp of the latest element in the queue.
 	 *
 	 * @param handle  A handle returned from orb_subscribe.
 	 * @param time    Returns the absolute time that the topic was updated, or zero if it has

From 79b3766544cc8a1e5c2971afb78c5453cc075e53 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Beat=20K=C3=BCng?= <beat-kueng@gmx.net>
Date: Fri, 20 May 2016 15:29:32 +0200
Subject: [PATCH 0448/1230] orb: add unit tests for queuing, including tests
 with poll & notify interface

Both succeed under Posix & NuttX (Pixracer)
---
 .../uORB/uORB_tests/uORBTest_UnitTest.cpp     | 236 +++++++++++++++++-
 .../uORB/uORB_tests/uORBTest_UnitTest.hpp     |  19 +-
 2 files changed, 251 insertions(+), 4 deletions(-)

diff --git a/src/modules/uORB/uORB_tests/uORBTest_UnitTest.cpp b/src/modules/uORB/uORB_tests/uORBTest_UnitTest.cpp
index 78b2f0e1e0..da8f859422 100644
--- a/src/modules/uORB/uORB_tests/uORBTest_UnitTest.cpp
+++ b/src/modules/uORB/uORB_tests/uORBTest_UnitTest.cpp
@@ -36,6 +36,8 @@
 #include <px4_config.h>
 #include <px4_time.h>
 #include <stdio.h>
+#include <errno.h>
+#include <poll.h>
 
 uORBTest::UnitTest &uORBTest::UnitTest::instance()
 {
@@ -164,7 +166,19 @@ int uORBTest::UnitTest::test()
 		return ret;
 	}
 
-	return test_multi2();
+	ret = test_multi2();
+
+	if (ret != OK) {
+		return ret;
+	}
+
+	ret = test_queue();
+
+	if (ret != OK) {
+		return ret;
+	}
+
+	return test_queue_poll_notify();
 }
 
 int uORBTest::UnitTest::test_unadvertise()
@@ -557,6 +571,226 @@ int uORBTest::UnitTest::test_multi_reversed()
 	return test_note("PASS multi-topic reversed");
 }
 
+int uORBTest::UnitTest::test_queue()
+{
+	test_note("Testing orb queuing");
+
+	struct orb_test_medium t, u;
+	int sfd;
+	orb_advert_t ptopic;
+	bool updated;
+
+	sfd = orb_subscribe(ORB_ID(orb_test_medium_queue));
+
+	if (sfd < 0) {
+		return test_fail("subscribe failed: %d", errno);
+	}
+
+
+	const int queue_size = 11;
+	t.val = 0;
+	ptopic = orb_advertise_queue(ORB_ID(orb_test_medium_queue), &t, queue_size);
+
+	if (ptopic == nullptr) {
+		return test_fail("advertise failed: %d", errno);
+	}
+
+	orb_check(sfd, &updated);
+
+	if (!updated) {
+		return test_fail("update flag not set");
+	}
+
+	if (PX4_OK != orb_copy(ORB_ID(orb_test_medium_queue), sfd, &u)) {
+		return test_fail("copy(1) failed: %d", errno);
+	}
+
+	if (u.val != t.val) {
+		return test_fail("copy(1) mismatch: %d expected %d", u.val, t.val);
+	}
+
+	orb_check(sfd, &updated);
+
+	if (updated) {
+		return test_fail("spurious updated flag");
+	}
+
+#define CHECK_UPDATED(element) \
+	orb_check(sfd, &updated); \
+	if (!updated) { \
+		return test_fail("update flag not set, element %i", element); \
+	}
+#define CHECK_NOT_UPDATED(element) \
+	orb_check(sfd, &updated); \
+	if (updated) { \
+		return test_fail("update flag set, element %i", element); \
+	}
+#define CHECK_COPY(i_got, i_correct) \
+	orb_copy(ORB_ID(orb_test_medium_queue), sfd, &u); \
+	if (i_got != i_correct) { \
+		return test_fail("got wrong element from the queue (got %i, should be %i)", i_got, i_correct); \
+	}
+
+	//no messages in the queue anymore
+
+	test_note("  Testing to write some elements...");
+
+	for (int i = 0; i < queue_size - 2; ++i) {
+		t.val = i;
+		orb_publish(ORB_ID(orb_test_medium_queue), ptopic, &t);
+	}
+
+	for (int i = 0; i < queue_size - 2; ++i) {
+		CHECK_UPDATED(i);
+		CHECK_COPY(u.val, i);
+	}
+
+	CHECK_NOT_UPDATED(queue_size);
+
+	test_note("  Testing overflow...");
+	int overflow_by = 3;
+
+	for (int i = 0; i < queue_size + overflow_by; ++i) {
+		t.val = i;
+		orb_publish(ORB_ID(orb_test_medium_queue), ptopic, &t);
+	}
+
+	for (int i = 0; i < queue_size; ++i) {
+		CHECK_UPDATED(i);
+		CHECK_COPY(u.val, i + overflow_by);
+	}
+
+	CHECK_NOT_UPDATED(queue_size);
+
+	test_note("  Testing underflow...");
+
+	for (int i = 0; i < queue_size; ++i) {
+		CHECK_NOT_UPDATED(i);
+		CHECK_COPY(u.val, queue_size + overflow_by - 1);
+	}
+
+	t.val = 943;
+	orb_publish(ORB_ID(orb_test_medium_queue), ptopic, &t);
+	CHECK_UPDATED(-1);
+	CHECK_COPY(u.val, t.val);
+
+#undef CHECK_COPY
+#undef CHECK_UPDATED
+#undef CHECK_NOT_UPDATED
+
+	orb_unadvertise(ptopic);
+
+	return test_note("PASS orb queuing");
+}
+
+
+int uORBTest::UnitTest::pub_test_queue_entry(char *const argv[])
+{
+	uORBTest::UnitTest &t = uORBTest::UnitTest::instance();
+	return t.pub_test_queue_main();
+}
+
+int uORBTest::UnitTest::pub_test_queue_main()
+{
+	struct orb_test_medium t;
+	orb_advert_t ptopic;
+	const int queue_size = 50;
+	t.val = 0;
+
+	if ((ptopic = orb_advertise_queue(ORB_ID(orb_test_medium_queue_poll), &t, queue_size)) == nullptr) {
+		_thread_should_exit = true;
+		return test_fail("advertise failed: %d", errno);
+	}
+
+	int message_counter = 0, num_messages = 20 * queue_size;
+	++t.val;
+
+	while (message_counter < num_messages) {
+
+		//simulate burst
+		int burst_counter = 0;
+
+		while (burst_counter++ < queue_size / 2 + 7) { //make interval non-boundary aligned
+			orb_publish(ORB_ID(orb_test_medium_queue_poll), ptopic, &t);
+			++t.val;
+		}
+
+		message_counter += burst_counter;
+		usleep(20 * 1000); //give subscriber a chance to catch up
+	}
+
+	_num_messages_sent = t.val;
+	usleep(100 * 1000);
+	_thread_should_exit = true;
+	orb_unadvertise(ptopic);
+
+	return 0;
+}
+
+int uORBTest::UnitTest::test_queue_poll_notify()
+{
+	test_note("Testing orb queuing (poll & notify)");
+
+	struct orb_test_medium t;
+	int sfd;
+
+	if ((sfd = orb_subscribe(ORB_ID(orb_test_medium_queue_poll))) < 0) {
+		return test_fail("subscribe failed: %d", errno);
+	}
+
+	_thread_should_exit = false;
+
+	char *const args[1] = { NULL };
+	int pubsub_task = px4_task_spawn_cmd("uorb_test_queue",
+					     SCHED_DEFAULT,
+					     SCHED_PRIORITY_MIN + 5,
+					     1500,
+					     (px4_main_t)&uORBTest::UnitTest::pub_test_queue_entry,
+					     args);
+
+	if (pubsub_task < 0) {
+		return test_fail("failed launching task");
+	}
+
+	int next_expected_val = 0;
+	px4_pollfd_struct_t fds[1];
+	fds[0].fd = sfd;
+	fds[0].events = POLLIN;
+
+	while (!_thread_should_exit) {
+
+		int poll_ret = px4_poll(fds, 1, 500);
+
+		if (poll_ret == 0) {
+			if (_thread_should_exit) {
+				break;
+			}
+
+			return test_fail("poll timeout");
+
+		} else if (poll_ret < 0) {
+			return test_fail("poll error (%d, %d)", poll_ret, errno);
+		}
+
+		if (fds[0].revents & POLLIN) {
+			orb_copy(ORB_ID(orb_test_medium_queue_poll), sfd, &t);
+
+			if (next_expected_val != t.val) {
+				return test_fail("copy mismatch: %d expected %d", t.val, next_expected_val);
+			}
+
+			++next_expected_val;
+		}
+	}
+	if (_num_messages_sent != next_expected_val) {
+		return test_fail("number of sent and received messages mismatch (sent: %i, received: %i)",
+				_num_messages_sent, next_expected_val);
+	}
+
+	return test_note("PASS orb queuing (poll & notify), got %i messages", next_expected_val);
+}
+
+
 int uORBTest::UnitTest::test_fail(const char *fmt, ...)
 {
 	va_list ap;
diff --git a/src/modules/uORB/uORB_tests/uORBTest_UnitTest.hpp b/src/modules/uORB/uORB_tests/uORBTest_UnitTest.hpp
index cf81d36e23..8016b9c8a2 100644
--- a/src/modules/uORB/uORB_tests/uORBTest_UnitTest.hpp
+++ b/src/modules/uORB/uORB_tests/uORBTest_UnitTest.hpp
@@ -54,7 +54,10 @@ ORB_DEFINE(orb_test_medium, struct orb_test_medium, sizeof(orb_test_medium),
 	   "ORB_TEST_MEDIUM:int val;hrt_abstime time;char[64] junk;");
 ORB_DEFINE(orb_test_medium_multi, struct orb_test_medium, sizeof(orb_test_medium),
 	   "ORB_TEST_MEDIUM_MULTI:int val;hrt_abstime time;char[64] junk;");
-
+ORB_DEFINE(orb_test_medium_queue, struct orb_test_medium, sizeof(orb_test_medium),
+	   "ORB_TEST_MEDIUM_MULTI:int val;hrt_abstime time;char[64] junk;");
+ORB_DEFINE(orb_test_medium_queue_poll, struct orb_test_medium, sizeof(orb_test_medium),
+	   "ORB_TEST_MEDIUM_MULTI:int val;hrt_abstime time;char[64] junk;");
 
 struct orb_test_large {
 	int val;
@@ -98,13 +101,23 @@ private:
 	bool pubsubtest_print;
 	int pubsubtest_res = OK;
 
-	int test_unadvertise();
 	orb_advert_t _pfd[4]; ///< used for test_multi and test_multi_reversed
 
 	int test_single();
+
+	/* These 3 depend on each other and must be called in this order */
 	int test_multi();
-	int test_multi2();
 	int test_multi_reversed();
+	int test_unadvertise();
+
+	int test_multi2();
+
+	/* queuing tests */
+	int test_queue();
+	static int pub_test_queue_entry(char *const argv[]);
+	int pub_test_queue_main();
+	int test_queue_poll_notify();
+	volatile int _num_messages_sent = 0;
 
 	int test_fail(const char *fmt, ...);
 	int test_note(const char *fmt, ...);

From 392c32d31668fce2a2fd63caa23beb6e3a3d91b5 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Beat=20K=C3=BCng?= <beat-kueng@gmx.net>
Date: Fri, 20 May 2016 16:03:07 +0200
Subject: [PATCH 0449/1230] uORBTest_UnitTest.cpp: fix style

---
 src/modules/uORB/uORB_tests/uORBTest_UnitTest.cpp | 3 ++-
 1 file changed, 2 insertions(+), 1 deletion(-)

diff --git a/src/modules/uORB/uORB_tests/uORBTest_UnitTest.cpp b/src/modules/uORB/uORB_tests/uORBTest_UnitTest.cpp
index da8f859422..eaa0221c60 100644
--- a/src/modules/uORB/uORB_tests/uORBTest_UnitTest.cpp
+++ b/src/modules/uORB/uORB_tests/uORBTest_UnitTest.cpp
@@ -782,9 +782,10 @@ int uORBTest::UnitTest::test_queue_poll_notify()
 			++next_expected_val;
 		}
 	}
+
 	if (_num_messages_sent != next_expected_val) {
 		return test_fail("number of sent and received messages mismatch (sent: %i, received: %i)",
-				_num_messages_sent, next_expected_val);
+				 _num_messages_sent, next_expected_val);
 	}
 
 	return test_note("PASS orb queuing (poll & notify), got %i messages", next_expected_val);

From 43d734ef4372d4b502580c78debef32370d4d7c2 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Beat=20K=C3=BCng?= <beat-kueng@gmx.net>
Date: Fri, 27 May 2016 09:40:42 +0200
Subject: [PATCH 0450/1230] orb: consistently use unsigned int for queue_size

---
 src/modules/uORB/uORB.cpp                         |  4 ++--
 src/modules/uORB/uORB.h                           |  4 ++--
 src/modules/uORB/uORBManager.cpp                  |  4 ++--
 src/modules/uORB/uORBManager.hpp                  |  4 ++--
 src/modules/uORB/uORB_tests/uORBTest_UnitTest.cpp | 14 +++++++-------
 5 files changed, 15 insertions(+), 15 deletions(-)

diff --git a/src/modules/uORB/uORB.cpp b/src/modules/uORB/uORB.cpp
index cf32658673..4724502ebc 100644
--- a/src/modules/uORB/uORB.cpp
+++ b/src/modules/uORB/uORB.cpp
@@ -45,7 +45,7 @@ orb_advert_t orb_advertise(const struct orb_metadata *meta, const void *data)
 	return uORB::Manager::get_instance()->orb_advertise(meta, data);
 }
 
-orb_advert_t orb_advertise_queue(const struct orb_metadata *meta, const void *data, int queue_size)
+orb_advert_t orb_advertise_queue(const struct orb_metadata *meta, const void *data, unsigned int queue_size)
 {
 	return uORB::Manager::get_instance()->orb_advertise(meta, data, queue_size);
 }
@@ -57,7 +57,7 @@ orb_advert_t orb_advertise_multi(const struct orb_metadata *meta, const void *da
 }
 
 orb_advert_t orb_advertise_multi_queue(const struct orb_metadata *meta, const void *data, int *instance,
-				       int priority, int queue_size)
+				       int priority, unsigned int queue_size)
 {
 	return uORB::Manager::get_instance()->orb_advertise_multi(meta, data, instance, priority, queue_size);
 }
diff --git a/src/modules/uORB/uORB.h b/src/modules/uORB/uORB.h
index e189476f32..c1b03c0def 100644
--- a/src/modules/uORB/uORB.h
+++ b/src/modules/uORB/uORB.h
@@ -144,7 +144,7 @@ extern orb_advert_t orb_advertise(const struct orb_metadata *meta, const void *d
  * @see uORB::Manager::orb_advertise()
  */
 extern orb_advert_t orb_advertise_queue(const struct orb_metadata *meta, const void *data,
-					int queue_size) __EXPORT;
+					unsigned int queue_size) __EXPORT;
 
 /**
  * @see uORB::Manager::orb_advertise_multi()
@@ -156,7 +156,7 @@ extern orb_advert_t orb_advertise_multi(const struct orb_metadata *meta, const v
  * @see uORB::Manager::orb_advertise_multi()
  */
 extern orb_advert_t orb_advertise_multi_queue(const struct orb_metadata *meta, const void *data, int *instance,
-		int priority, int queue_size) __EXPORT;
+		int priority, unsigned int queue_size) __EXPORT;
 
 /**
  * @see uORB::Manager::orb_unadvertise()
diff --git a/src/modules/uORB/uORBManager.cpp b/src/modules/uORB/uORBManager.cpp
index 6c2309b13f..e5e669527a 100644
--- a/src/modules/uORB/uORBManager.cpp
+++ b/src/modules/uORB/uORBManager.cpp
@@ -87,14 +87,14 @@ int uORB::Manager::orb_exists(const struct orb_metadata *meta, int instance)
 #endif
 }
 
-orb_advert_t uORB::Manager::orb_advertise(const struct orb_metadata *meta, const void *data, int queue_size)
+orb_advert_t uORB::Manager::orb_advertise(const struct orb_metadata *meta, const void *data, unsigned int queue_size)
 {
 	//warnx("orb_advertise meta = %p", meta);
 	return orb_advertise_multi(meta, data, nullptr, ORB_PRIO_DEFAULT, queue_size);
 }
 
 orb_advert_t uORB::Manager::orb_advertise_multi(const struct orb_metadata *meta, const void *data, int *instance,
-		int priority, int queue_size)
+		int priority, unsigned int queue_size)
 {
 	int result, fd;
 	orb_advert_t advertiser;
diff --git a/src/modules/uORB/uORBManager.hpp b/src/modules/uORB/uORBManager.hpp
index 21f5777265..9a6a56c48a 100644
--- a/src/modules/uORB/uORBManager.hpp
+++ b/src/modules/uORB/uORBManager.hpp
@@ -104,7 +104,7 @@ public:
 	 *      ORB_DEFINE with no corresponding ORB_DECLARE)
 	 *      this function will return nullptr and set errno to ENOENT.
 	 */
-	orb_advert_t orb_advertise(const struct orb_metadata *meta, const void *data, int queue_size = 1);
+	orb_advert_t orb_advertise(const struct orb_metadata *meta, const void *data, unsigned int queue_size = 1);
 
 	/**
 	 * Advertise as the publisher of a topic.
@@ -141,7 +141,7 @@ public:
 	 *      this function will return -1 and set errno to ENOENT.
 	 */
 	orb_advert_t orb_advertise_multi(const struct orb_metadata *meta, const void *data, int *instance,
-					 int priority, int queue_size = 1) ;
+					 int priority, unsigned int queue_size = 1) ;
 
 
 	/**
diff --git a/src/modules/uORB/uORB_tests/uORBTest_UnitTest.cpp b/src/modules/uORB/uORB_tests/uORBTest_UnitTest.cpp
index eaa0221c60..7bcf9d7e4f 100644
--- a/src/modules/uORB/uORB_tests/uORBTest_UnitTest.cpp
+++ b/src/modules/uORB/uORB_tests/uORBTest_UnitTest.cpp
@@ -587,7 +587,7 @@ int uORBTest::UnitTest::test_queue()
 	}
 
 
-	const int queue_size = 11;
+	const unsigned int queue_size = 11;
 	t.val = 0;
 	ptopic = orb_advertise_queue(ORB_ID(orb_test_medium_queue), &t, queue_size);
 
@@ -635,12 +635,12 @@ int uORBTest::UnitTest::test_queue()
 
 	test_note("  Testing to write some elements...");
 
-	for (int i = 0; i < queue_size - 2; ++i) {
+	for (unsigned int i = 0; i < queue_size - 2; ++i) {
 		t.val = i;
 		orb_publish(ORB_ID(orb_test_medium_queue), ptopic, &t);
 	}
 
-	for (int i = 0; i < queue_size - 2; ++i) {
+	for (unsigned int i = 0; i < queue_size - 2; ++i) {
 		CHECK_UPDATED(i);
 		CHECK_COPY(u.val, i);
 	}
@@ -650,12 +650,12 @@ int uORBTest::UnitTest::test_queue()
 	test_note("  Testing overflow...");
 	int overflow_by = 3;
 
-	for (int i = 0; i < queue_size + overflow_by; ++i) {
+	for (unsigned int i = 0; i < queue_size + overflow_by; ++i) {
 		t.val = i;
 		orb_publish(ORB_ID(orb_test_medium_queue), ptopic, &t);
 	}
 
-	for (int i = 0; i < queue_size; ++i) {
+	for (unsigned int i = 0; i < queue_size; ++i) {
 		CHECK_UPDATED(i);
 		CHECK_COPY(u.val, i + overflow_by);
 	}
@@ -664,7 +664,7 @@ int uORBTest::UnitTest::test_queue()
 
 	test_note("  Testing underflow...");
 
-	for (int i = 0; i < queue_size; ++i) {
+	for (unsigned int i = 0; i < queue_size; ++i) {
 		CHECK_NOT_UPDATED(i);
 		CHECK_COPY(u.val, queue_size + overflow_by - 1);
 	}
@@ -694,7 +694,7 @@ int uORBTest::UnitTest::pub_test_queue_main()
 {
 	struct orb_test_medium t;
 	orb_advert_t ptopic;
-	const int queue_size = 50;
+	const unsigned int queue_size = 50;
 	t.val = 0;
 
 	if ((ptopic = orb_advertise_queue(ORB_ID(orb_test_medium_queue_poll), &t, queue_size)) == nullptr) {

From 659ac8faf295e478746c0c5afa58e76ac743df48 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Beat=20K=C3=BCng?= <beat-kueng@gmx.net>
Date: Tue, 24 May 2016 14:17:04 +0200
Subject: [PATCH 0451/1230] refactor logger: use static fields & move them to
 source file

avoids multiple declarations of...
---
 src/modules/logger/logger.cpp | 8 ++++++--
 src/modules/logger/logger.h   | 9 +++------
 2 files changed, 9 insertions(+), 8 deletions(-)

diff --git a/src/modules/logger/logger.cpp b/src/modules/logger/logger.cpp
index 9a4eb1553a..a6a2767ac9 100644
--- a/src/modules/logger/logger.cpp
+++ b/src/modules/logger/logger.cpp
@@ -75,6 +75,10 @@
 
 using namespace px4::logger;
 
+static Logger *logger_ptr = nullptr;
+static int logger_task = -1;
+static pthread_t writer_thread;
+
 int logger_main(int argc, char *argv[])
 {
 	// logger currently assumes little endian
@@ -464,7 +468,7 @@ void Logger::run()
 		return;
 	}
 
-	int ret = _writer.thread_start(_writer_thread);
+	int ret = _writer.thread_start(writer_thread);
 
 	if (ret) {
 		PX4_ERR("logger: failed to create writer thread (%i)", ret);
@@ -616,7 +620,7 @@ void Logger::run()
 	_writer.notify();
 
 	// wait for thread to complete
-	ret = pthread_join(_writer_thread, NULL);
+	ret = pthread_join(writer_thread, NULL);
 
 	if (ret) {
 		PX4_WARN("join failed: %d", ret);
diff --git a/src/modules/logger/logger.h b/src/modules/logger/logger.h
index d061b9917f..7f8ae5933c 100644
--- a/src/modules/logger/logger.h
+++ b/src/modules/logger/logger.h
@@ -203,11 +203,8 @@ private:
 	param_t						_log_utc_offset;
 	orb_advert_t					_mavlink_log_pub = nullptr;
 	uint16_t					_next_topic_id; ///< id of next subscribed topic
+
 };
 
-Logger *logger_ptr = nullptr;
-int		logger_task = -1;
-pthread_t _writer_thread;
-
-}
-}
+} //namespace logger
+} //namespace px4

From 069dd01cb0724af40c0a011f0d6229bc67d7517b Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Beat=20K=C3=BCng?= <beat-kueng@gmx.net>
Date: Tue, 24 May 2016 15:05:49 +0200
Subject: [PATCH 0452/1230] logger: subscribe to mavlink_log messages and write
 them to the log

---
 msg/mavlink_log.msg                 |  2 +-
 src/modules/logger/logger.cpp       | 64 +++++++++++++++++++++--------
 src/modules/logger/logger.h         |  7 ++++
 src/modules/logger/messages.h       | 10 +++++
 src/modules/systemlib/mavlink_log.c |  1 +
 src/modules/uORB/Subscription.cpp   |  2 +
 6 files changed, 67 insertions(+), 19 deletions(-)

diff --git a/msg/mavlink_log.msg b/msg/mavlink_log.msg
index d52004fad6..274596adde 100644
--- a/msg/mavlink_log.msg
+++ b/msg/mavlink_log.msg
@@ -1,3 +1,3 @@
 
 uint8[50] text
-uint8 severity
+uint8 severity # log level (same as in the linux kernel, starting with 0)
diff --git a/src/modules/logger/logger.cpp b/src/modules/logger/logger.cpp
index a6a2767ac9..9d1044f6b5 100644
--- a/src/modules/logger/logger.cpp
+++ b/src/modules/logger/logger.cpp
@@ -41,6 +41,8 @@
 
 #include <uORB/uORB.h>
 #include <uORB/uORBTopics.h>
+#include <uORB/Subscription.hpp>
+#include <uORB/topics/mavlink_log.h>
 #include <uORB/topics/parameter_update.h>
 #include <uORB/topics/vehicle_status.h>
 #include <uORB/topics/vehicle_gps_position.h>
@@ -439,6 +441,7 @@ void Logger::run()
 
 	uORB::Subscription<vehicle_status_s> vehicle_status_sub(ORB_ID(vehicle_status));
 	uORB::Subscription<parameter_update_s> parameter_update_sub(ORB_ID(parameter_update));
+	uORB::Subscription<mavlink_log_s> mavlink_log_sub(ORB_ID(mavlink_log));
 
 
 	add_topic("sensor_gyro", 0);
@@ -547,38 +550,37 @@ void Logger::run()
 
 						//PX4_INFO("topic: %s, size = %zu, out_size = %zu", sub.metadata->o_name, sub.metadata->o_size, msg_size);
 
-						if (_writer.write(buffer, msg_size, _dropout_start)) {
+						if (write(buffer, msg_size)) {
 
 #ifdef DBGPRINT
 							total_bytes += msg_size;
 #endif /* DBGPRINT */
 
-							if (_dropout_start) {
-								float dropout_duration = (float)(hrt_elapsed_time(&_dropout_start) / 1000) / 1.e3f;
-
-								if (dropout_duration > _max_dropout_duration) {
-									_max_dropout_duration = dropout_duration;
-								}
-
-								_dropout_start = 0;
-							}
-
 							data_written = true;
 
 						} else {
-
-							if (!_dropout_start) {
-								_dropout_start = hrt_absolute_time();
-								++_write_dropouts;
-								_high_water = 0;
-							}
-
 							break;	// Write buffer overflow, skip this record
 						}
 					}
 				}
 			}
 
+			//check for new mavlink log message
+			if (mavlink_log_sub.check_updated()) {
+				mavlink_log_sub.update();
+				message_logging_s log_msg;
+				log_msg.log_level = mavlink_log_sub.get().severity + '0';
+				log_msg.timestamp = mavlink_log_sub.get().timestamp;
+				const char *message = (const char *)mavlink_log_sub.get().text;
+				int message_len = strlen(message);
+
+				if (message_len > 0) {
+					strncpy(log_msg.message, message, sizeof(log_msg.message));
+					log_msg.msg_size = sizeof(log_msg) - sizeof(log_msg.message) - MSG_HEADER_LEN + message_len;
+					write(&log_msg, log_msg.msg_size + MSG_HEADER_LEN);
+				}
+			}
+
 			if (!_dropout_start && _writer.get_buffer_fill_count() > _high_water) {
 				_high_water = _writer.get_buffer_fill_count();
 			}
@@ -642,6 +644,32 @@ void Logger::run()
 	}
 }
 
+bool Logger::write(void *ptr, size_t size)
+{
+	if (_writer.write(ptr, size, _dropout_start)) {
+
+		if (_dropout_start) {
+			float dropout_duration = (float)(hrt_elapsed_time(&_dropout_start) / 1000) / 1.e3f;
+
+			if (dropout_duration > _max_dropout_duration) {
+				_max_dropout_duration = dropout_duration;
+			}
+
+			_dropout_start = 0;
+		}
+
+		return true;
+	}
+
+	if (!_dropout_start) {
+		_dropout_start = hrt_absolute_time();
+		++_write_dropouts;
+		_high_water = 0;
+	}
+
+	return false;
+}
+
 int Logger::create_log_dir(tm *tt)
 {
 	/* create dir on sdcard if needed */
diff --git a/src/modules/logger/logger.h b/src/modules/logger/logger.h
index 7f8ae5933c..32e1771faf 100644
--- a/src/modules/logger/logger.h
+++ b/src/modules/logger/logger.h
@@ -164,6 +164,13 @@ private:
 	 */
 	bool write_wait(void *ptr, size_t size);
 
+	/**
+	 * Write data to the logger and handle dropouts.
+	 * Must be called with _writer.lock() held.
+	 * @return true if data written, false otherwise (on overflow)
+	 */
+	bool write(void *ptr, size_t size);
+
 	/**
 	 * Get the time for log file name
 	 * @param tt returned time
diff --git a/src/modules/logger/messages.h b/src/modules/logger/messages.h
index 58fdd56a94..b784a2bd12 100644
--- a/src/modules/logger/messages.h
+++ b/src/modules/logger/messages.h
@@ -42,6 +42,7 @@ enum class MessageType : uint8_t {
 	REMOVE_LOGGED_MSG = 'R',
 	SYNC = 'S',
 	DROPOUT = 'O',
+	LOGGING = 'L',
 };
 
 
@@ -108,6 +109,15 @@ struct message_info_header_s {
 	char key[255];
 };
 
+struct message_logging_s {
+	uint16_t msg_size; //size of message - MSG_HEADER_LEN
+	uint8_t msg_type = static_cast<uint8_t>(MessageType::LOGGING);
+
+	uint8_t log_level; //same levels as in the linux kernel
+	uint64_t timestamp;
+	char message[255];
+};
+
 struct message_parameter_header_s {
 	uint16_t msg_size;
 	uint8_t msg_type = static_cast<uint8_t>(MessageType::PARAMETER);
diff --git a/src/modules/systemlib/mavlink_log.c b/src/modules/systemlib/mavlink_log.c
index 4d4b0062aa..bf897b3057 100644
--- a/src/modules/systemlib/mavlink_log.c
+++ b/src/modules/systemlib/mavlink_log.c
@@ -64,6 +64,7 @@ __EXPORT void mavlink_vasprintf(int severity, orb_advert_t *mavlink_log_pub, con
 	struct mavlink_log_s log_msg;
 
 	log_msg.severity = severity;
+	log_msg.timestamp = hrt_absolute_time();
 
 	va_list ap;
 
diff --git a/src/modules/uORB/Subscription.cpp b/src/modules/uORB/Subscription.cpp
index 64067967d1..7ddf234848 100644
--- a/src/modules/uORB/Subscription.cpp
+++ b/src/modules/uORB/Subscription.cpp
@@ -49,6 +49,7 @@
 #include "topics/position_setpoint_triplet.h"
 #include "topics/vehicle_status.h"
 #include "topics/manual_control_setpoint.h"
+#include "topics/mavlink_log.h"
 #include "topics/vehicle_local_position_setpoint.h"
 #include "topics/vehicle_local_position.h"
 #include "topics/vehicle_attitude_setpoint.h"
@@ -165,6 +166,7 @@ template class __EXPORT Subscription<encoders_s>;
 template class __EXPORT Subscription<position_setpoint_triplet_s>;
 template class __EXPORT Subscription<vehicle_status_s>;
 template class __EXPORT Subscription<manual_control_setpoint_s>;
+template class __EXPORT Subscription<mavlink_log_s>;
 template class __EXPORT Subscription<vehicle_local_position_setpoint_s>;
 template class __EXPORT Subscription<vehicle_local_position_s>;
 template class __EXPORT Subscription<vehicle_attitude_setpoint_s>;

From 034772056acd858a29c64f42ecf9503b68f34b92 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Beat=20K=C3=BCng?= <beat-kueng@gmx.net>
Date: Tue, 24 May 2016 15:07:11 +0200
Subject: [PATCH 0453/1230] logger: prepare for replay: add replayed file to
 the log, use _replayed as file name suffix

---
 src/modules/logger/logger.cpp | 29 ++++++++++++++++++++++++++---
 src/modules/logger/logger.h   |  8 ++++++++
 2 files changed, 34 insertions(+), 3 deletions(-)

diff --git a/src/modules/logger/logger.cpp b/src/modules/logger/logger.cpp
index 9d1044f6b5..e73174a81a 100644
--- a/src/modules/logger/logger.cpp
+++ b/src/modules/logger/logger.cpp
@@ -81,6 +81,9 @@ static Logger *logger_ptr = nullptr;
 static int logger_task = -1;
 static pthread_t writer_thread;
 
+
+char *Logger::_replay_file_name = nullptr;
+
 int logger_main(int argc, char *argv[])
 {
 	// logger currently assumes little endian
@@ -735,14 +738,21 @@ int Logger::get_log_file_name(char *file_name, size_t file_name_size)
 		time_ok = get_log_time(&tt, false);
 	}
 
+	const char *replay_suffix = "";
+
+	if (_replay_file_name) {
+		replay_suffix = "_replayed";
+	}
+
+
 	if (time_ok) {
 		if (create_log_dir(&tt)) {
 			return -1;
 		}
 
 		char log_file_name[64] = "";
-		strftime(log_file_name, sizeof(log_file_name), "%H_%M_%S.ulg", &tt);
-		snprintf(file_name, file_name_size, "%s/%s", _log_dir, log_file_name);
+		strftime(log_file_name, sizeof(log_file_name), "%H_%M_%S", &tt);
+		snprintf(file_name, file_name_size, "%s/%s%s.ulg", _log_dir, log_file_name, replay_suffix);
 
 	} else {
 		if (create_log_dir(nullptr)) {
@@ -754,7 +764,7 @@ int Logger::get_log_file_name(char *file_name, size_t file_name_size)
 		/* look for the next file that does not exist */
 		while (file_number <= MAX_NO_LOGFILE) {
 			/* format log file path: e.g. /fs/microsd/sess001/log001.ulg */
-			snprintf(file_name, file_name_size, "%s/log%03u.ulg", _log_dir, file_number);
+			snprintf(file_name, file_name_size, "%s/log%03u%s.ulg", _log_dir, file_number, replay_suffix);
 
 			if (!file_exist(file_name)) {
 				break;
@@ -773,6 +783,15 @@ int Logger::get_log_file_name(char *file_name, size_t file_name_size)
 	return 0;
 }
 
+void Logger::setReplayFile(const char *file_name)
+{
+	if (_replay_file_name) {
+		free(_replay_file_name);
+	}
+
+	_replay_file_name = strdup(file_name);
+}
+
 bool Logger::get_log_time(struct tm *tt, bool boot_time)
 {
 	int vehicle_gps_position_sub = orb_subscribe(ORB_ID(vehicle_gps_position));
@@ -997,6 +1016,10 @@ void Logger::write_version()
 		param_get(_log_utc_offset, &utc_offset);
 		write_info("time_ref_utc", utc_offset * 60);
 	}
+
+	if (_replay_file_name) {
+		write_info("replay", _replay_file_name);
+	}
 }
 
 void Logger::write_parameters()
diff --git a/src/modules/logger/logger.h b/src/modules/logger/logger.h
index 32e1771faf..e01f9eb65e 100644
--- a/src/modules/logger/logger.h
+++ b/src/modules/logger/logger.h
@@ -80,6 +80,13 @@ public:
 
 	~Logger();
 
+	/**
+	 * Tell the logger that we're in replay mode. This must be called
+	 * before starting the logger.
+	 * @param file_name file name of the used log replay file. Will be copied.
+	 */
+	static void setReplayFile(const char *file_name);
+
 	/**
 	 * Add a topic to be logged. This must be called before start_log()
 	 * (because it does not write an ADD_LOGGED_MSG message).
@@ -211,6 +218,7 @@ private:
 	orb_advert_t					_mavlink_log_pub = nullptr;
 	uint16_t					_next_topic_id; ///< id of next subscribed topic
 
+	static char		*_replay_file_name;
 };
 
 } //namespace logger

From 078e79f2949cb0563e05e232ddfd45fcbd4e7359 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Beat=20K=C3=BCng?= <beat-kueng@gmx.net>
Date: Tue, 24 May 2016 15:27:10 +0200
Subject: [PATCH 0454/1230] mavlink_log.c: fix coding style

---
 src/modules/systemlib/mavlink_log.c | 1 +
 1 file changed, 1 insertion(+)

diff --git a/src/modules/systemlib/mavlink_log.c b/src/modules/systemlib/mavlink_log.c
index bf897b3057..311e7d8c91 100644
--- a/src/modules/systemlib/mavlink_log.c
+++ b/src/modules/systemlib/mavlink_log.c
@@ -64,6 +64,7 @@ __EXPORT void mavlink_vasprintf(int severity, orb_advert_t *mavlink_log_pub, con
 	struct mavlink_log_s log_msg;
 
 	log_msg.severity = severity;
+
 	log_msg.timestamp = hrt_absolute_time();
 
 	va_list ap;

From 0e3d660ccd8827df82079c4f4f7433c06b942def Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Beat=20K=C3=BCng?= <beat-kueng@gmx.net>
Date: Tue, 31 May 2016 17:32:13 +0200
Subject: [PATCH 0455/1230] logger refactor: add ulog_ prefix to struct names
 and header length

---
 src/modules/logger/log_writer.cpp |  4 +-
 src/modules/logger/logger.cpp     | 62 +++++++++++++++----------------
 src/modules/logger/messages.h     | 58 ++++++++++++++---------------
 3 files changed, 62 insertions(+), 62 deletions(-)

diff --git a/src/modules/logger/log_writer.cpp b/src/modules/logger/log_writer.cpp
index 1268ecb596..c645b6dbeb 100644
--- a/src/modules/logger/log_writer.cpp
+++ b/src/modules/logger/log_writer.cpp
@@ -250,7 +250,7 @@ bool LogWriter::write(void *ptr, size_t size, uint64_t dropout_start)
 	size_t dropout_size = 0;
 
 	if (dropout_start) {
-		dropout_size = sizeof(message_dropout_s);
+		dropout_size = sizeof(ulog_message_dropout_s);
 	}
 
 	if (size + dropout_size > available) {
@@ -260,7 +260,7 @@ bool LogWriter::write(void *ptr, size_t size, uint64_t dropout_start)
 
 	if (dropout_start) {
 		//write dropout msg
-		message_dropout_s dropout_msg;
+		ulog_message_dropout_s dropout_msg;
 		dropout_msg.duration = (uint16_t)(hrt_elapsed_time(&dropout_start) / 1000);
 		write_no_check(&dropout_msg, sizeof(dropout_msg));
 	}
diff --git a/src/modules/logger/logger.cpp b/src/modules/logger/logger.cpp
index e73174a81a..4e486ab957 100644
--- a/src/modules/logger/logger.cpp
+++ b/src/modules/logger/logger.cpp
@@ -341,9 +341,9 @@ int Logger::add_topic(const orb_metadata *topic)
 
 	size_t fields_len = strlen(topic->o_fields) + strlen(topic->o_name) + 1; //1 for ':'
 
-	if (fields_len > sizeof(message_format_s::format)) {
+	if (fields_len > sizeof(ulog_message_format_s::format)) {
 		PX4_WARN("skip topic %s, format string is too large: %zu (max is %zu)", topic->o_name, fields_len,
-			 sizeof(message_format_s::format));
+			 sizeof(ulog_message_format_s::format));
 
 		return -1;
 	}
@@ -537,18 +537,18 @@ void Logger::run()
 			for (LoggerSubscription &sub : _subscriptions) {
 				/* each message consists of a header followed by an orb data object
 				 */
-				size_t msg_size = sizeof(message_data_header_s) + sub.metadata->o_size_no_padding;
+				size_t msg_size = sizeof(ulog_message_data_header_s) + sub.metadata->o_size_no_padding;
 				uint8_t buffer[msg_size];
 
 				/* if this topic has been updated, copy the new data into the message buffer
 				 * and write a message to the log
 				 */
 				for (uint8_t instance = 0; instance < ORB_MULTI_MAX_INSTANCES; instance++) {
-					if (copy_if_updated_multi(sub, instance, buffer + sizeof(message_data_header_s))) {
+					if (copy_if_updated_multi(sub, instance, buffer + sizeof(ulog_message_data_header_s))) {
 
-						message_data_header_s *header = reinterpret_cast<message_data_header_s *>(buffer);
-						header->msg_type = static_cast<uint8_t>(MessageType::DATA);
-						header->msg_size = static_cast<uint16_t>(msg_size - MSG_HEADER_LEN);
+						ulog_message_data_header_s *header = reinterpret_cast<ulog_message_data_header_s *>(buffer);
+						header->msg_type = static_cast<uint8_t>(ULogMessageType::DATA);
+						header->msg_size = static_cast<uint16_t>(msg_size - ULOG_MSG_HEADER_LEN);
 						header->msg_id = sub.msg_ids[instance];
 
 						//PX4_INFO("topic: %s, size = %zu, out_size = %zu", sub.metadata->o_name, sub.metadata->o_size, msg_size);
@@ -571,7 +571,7 @@ void Logger::run()
 			//check for new mavlink log message
 			if (mavlink_log_sub.check_updated()) {
 				mavlink_log_sub.update();
-				message_logging_s log_msg;
+				ulog_message_logging_s log_msg;
 				log_msg.log_level = mavlink_log_sub.get().severity + '0';
 				log_msg.timestamp = mavlink_log_sub.get().timestamp;
 				const char *message = (const char *)mavlink_log_sub.get().text;
@@ -579,8 +579,8 @@ void Logger::run()
 
 				if (message_len > 0) {
 					strncpy(log_msg.message, message, sizeof(log_msg.message));
-					log_msg.msg_size = sizeof(log_msg) - sizeof(log_msg.message) - MSG_HEADER_LEN + message_len;
-					write(&log_msg, log_msg.msg_size + MSG_HEADER_LEN);
+					log_msg.msg_size = sizeof(log_msg) - sizeof(log_msg.message) - ULOG_MSG_HEADER_LEN + message_len;
+					write(&log_msg, log_msg.msg_size + ULOG_MSG_HEADER_LEN);
 				}
 			}
 
@@ -890,14 +890,14 @@ bool Logger::write_wait(void *ptr, size_t size)
 void Logger::write_formats()
 {
 	_writer.lock();
-	message_format_s msg;
+	ulog_message_format_s msg;
 	const orb_metadata **topics = orb_get_topics();
 
 	//write all known formats
 	for (size_t i = 0; i < orb_topics_count(); i++) {
 		int format_len = snprintf(msg.format, sizeof(msg.format), "%s:%s", topics[i]->o_name, topics[i]->o_fields);
 		size_t msg_size = sizeof(msg) - sizeof(msg.format) + format_len;
-		msg.msg_size = msg_size - MSG_HEADER_LEN;
+		msg.msg_size = msg_size - ULOG_MSG_HEADER_LEN;
 
 		write_wait(&msg, msg_size);
 	}
@@ -922,7 +922,7 @@ void Logger::write_all_add_logged_msg()
 
 void Logger::write_add_logged_msg(LoggerSubscription &subscription, int instance)
 {
-	message_add_logged_s msg;
+	ulog_message_add_logged_s msg;
 
 	msg.multi_id = instance;
 	subscription.msg_ids[instance] = _next_topic_id;
@@ -933,7 +933,7 @@ void Logger::write_add_logged_msg(LoggerSubscription &subscription, int instance
 	memcpy(msg.message_name, subscription.metadata->o_name, message_name_len);
 
 	size_t msg_size = sizeof(msg) - sizeof(msg.message_name) + message_name_len;
-	msg.msg_size = msg_size - MSG_HEADER_LEN;
+	msg.msg_size = msg_size - ULOG_MSG_HEADER_LEN;
 
 	write_wait(&msg, msg_size);
 
@@ -944,9 +944,9 @@ void Logger::write_add_logged_msg(LoggerSubscription &subscription, int instance
 void Logger::write_info(const char *name, const char *value)
 {
 	_writer.lock();
-	uint8_t buffer[sizeof(message_info_header_s)];
-	message_info_header_s *msg = reinterpret_cast<message_info_header_s *>(buffer);
-	msg->msg_type = static_cast<uint8_t>(MessageType::INFO);
+	uint8_t buffer[sizeof(ulog_message_info_header_s)];
+	ulog_message_info_header_s *msg = reinterpret_cast<ulog_message_info_header_s *>(buffer);
+	msg->msg_type = static_cast<uint8_t>(ULogMessageType::INFO);
 
 	/* construct format key (type and name) */
 	size_t vlen = strlen(value);
@@ -958,7 +958,7 @@ void Logger::write_info(const char *name, const char *value)
 		memcpy(&buffer[msg_size], value, vlen);
 		msg_size += vlen;
 
-		msg->msg_size = msg_size - MSG_HEADER_LEN;
+		msg->msg_size = msg_size - ULOG_MSG_HEADER_LEN;
 
 		write_wait(buffer, msg_size);
 	}
@@ -968,9 +968,9 @@ void Logger::write_info(const char *name, const char *value)
 void Logger::write_info(const char *name, int32_t value)
 {
 	_writer.lock();
-	uint8_t buffer[sizeof(message_info_header_s)];
-	message_info_header_s *msg = reinterpret_cast<message_info_header_s *>(buffer);
-	msg->msg_type = static_cast<uint8_t>(MessageType::INFO);
+	uint8_t buffer[sizeof(ulog_message_info_header_s)];
+	ulog_message_info_header_s *msg = reinterpret_cast<ulog_message_info_header_s *>(buffer);
+	msg->msg_type = static_cast<uint8_t>(ULogMessageType::INFO);
 
 	/* construct format key (type and name) */
 	msg->key_len = snprintf(msg->key, sizeof(msg->key), "int32_t %s", name);
@@ -980,7 +980,7 @@ void Logger::write_info(const char *name, int32_t value)
 	memcpy(&buffer[msg_size], &value, sizeof(int32_t));
 	msg_size += sizeof(int32_t);
 
-	msg->msg_size = msg_size - MSG_HEADER_LEN;
+	msg->msg_size = msg_size - ULOG_MSG_HEADER_LEN;
 
 	write_wait(buffer, msg_size);
 
@@ -989,7 +989,7 @@ void Logger::write_info(const char *name, int32_t value)
 
 void Logger::write_header()
 {
-	message_file_header_s header;
+	ulog_file_header_s header;
 	header.magic[0] = 'U';
 	header.magic[1] = 'L';
 	header.magic[2] = 'o';
@@ -1025,10 +1025,10 @@ void Logger::write_version()
 void Logger::write_parameters()
 {
 	_writer.lock();
-	uint8_t buffer[sizeof(message_parameter_header_s) + sizeof(param_value_u)];
-	message_parameter_header_s *msg = reinterpret_cast<message_parameter_header_s *>(buffer);
+	uint8_t buffer[sizeof(ulog_message_parameter_header_s) + sizeof(param_value_u)];
+	ulog_message_parameter_header_s *msg = reinterpret_cast<ulog_message_parameter_header_s *>(buffer);
 
-	msg->msg_type = static_cast<uint8_t>(MessageType::PARAMETER);
+	msg->msg_type = static_cast<uint8_t>(ULogMessageType::PARAMETER);
 	int param_idx = 0;
 	param_t param = 0;
 
@@ -1069,7 +1069,7 @@ void Logger::write_parameters()
 			param_get(param, &buffer[msg_size]);
 			msg_size += value_size;
 
-			msg->msg_size = msg_size - MSG_HEADER_LEN;
+			msg->msg_size = msg_size - ULOG_MSG_HEADER_LEN;
 
 			write_wait(buffer, msg_size);
 		}
@@ -1082,10 +1082,10 @@ void Logger::write_parameters()
 void Logger::write_changed_parameters()
 {
 	_writer.lock();
-	uint8_t buffer[sizeof(message_parameter_header_s) + sizeof(param_value_u)];
-	message_parameter_header_s *msg = reinterpret_cast<message_parameter_header_s *>(buffer);
+	uint8_t buffer[sizeof(ulog_message_parameter_header_s) + sizeof(param_value_u)];
+	ulog_message_parameter_header_s *msg = reinterpret_cast<ulog_message_parameter_header_s *>(buffer);
 
-	msg->msg_type = static_cast<uint8_t>(MessageType::PARAMETER);
+	msg->msg_type = static_cast<uint8_t>(ULogMessageType::PARAMETER);
 	int param_idx = 0;
 	param_t param = 0;
 
@@ -1129,7 +1129,7 @@ void Logger::write_changed_parameters()
 			msg_size += value_size;
 
 			/* msg_size is now 1 (msg_type) + 2 (msg_size) + 1 (key_len) + key_len + value_size */
-			msg->msg_size = msg_size - MSG_HEADER_LEN;
+			msg->msg_size = msg_size - ULOG_MSG_HEADER_LEN;
 
 			write_wait(buffer, msg_size);
 		}
diff --git a/src/modules/logger/messages.h b/src/modules/logger/messages.h
index b784a2bd12..4ce963c1dc 100644
--- a/src/modules/logger/messages.h
+++ b/src/modules/logger/messages.h
@@ -33,7 +33,7 @@
 
 #pragma once
 
-enum class MessageType : uint8_t {
+enum class ULogMessageType : uint8_t {
 	FORMAT = 'F',
 	DATA = 'D',
 	INFO = 'I',
@@ -49,78 +49,78 @@ enum class MessageType : uint8_t {
 /* declare message data structs with byte alignment (no padding) */
 #pragma pack(push, 1)
 
-#define MSG_HEADER_LEN 3 //accounts for msg_size and msg_type
+#define ULOG_MSG_HEADER_LEN 3 //accounts for msg_size and msg_type
 
 /** first bytes of the file */
-struct message_file_header_s {
+struct ulog_file_header_s {
 	uint8_t magic[8];
 	uint64_t timestamp;
 };
 
-struct message_format_s {
-	uint16_t msg_size; //size of message - MSG_HEADER_LEN
-	uint8_t msg_type = static_cast<uint8_t>(MessageType::FORMAT);
+struct ulog_message_format_s {
+	uint16_t msg_size; //size of message - ULOG_MSG_HEADER_LEN
+	uint8_t msg_type = static_cast<uint8_t>(ULogMessageType::FORMAT);
 
 	char format[2096];
 };
 
-struct message_add_logged_s {
-	uint16_t msg_size; //size of message - MSG_HEADER_LEN
-	uint8_t msg_type = static_cast<uint8_t>(MessageType::ADD_LOGGED_MSG);
+struct ulog_message_add_logged_s {
+	uint16_t msg_size; //size of message - ULOG_MSG_HEADER_LEN
+	uint8_t msg_type = static_cast<uint8_t>(ULogMessageType::ADD_LOGGED_MSG);
 
 	uint8_t multi_id;
 	uint16_t msg_id;
 	char message_name[255];
 };
 
-struct message_remove_logged_s {
-	uint16_t msg_size; //size of message - MSG_HEADER_LEN
-	uint8_t msg_type = static_cast<uint8_t>(MessageType::REMOVE_LOGGED_MSG);
+struct ulog_message_remove_logged_s {
+	uint16_t msg_size; //size of message - ULOG_MSG_HEADER_LEN
+	uint8_t msg_type = static_cast<uint8_t>(ULogMessageType::REMOVE_LOGGED_MSG);
 
 	uint16_t msg_id;
 };
 
-struct message_sync_s {
-	uint16_t msg_size; //size of message - MSG_HEADER_LEN
-	uint8_t msg_type = static_cast<uint8_t>(MessageType::SYNC);
+struct ulog_message_sync_s {
+	uint16_t msg_size; //size of message - ULOG_MSG_HEADER_LEN
+	uint8_t msg_type = static_cast<uint8_t>(ULogMessageType::SYNC);
 
 	uint8_t sync_magic[8];
 };
 
-struct message_dropout_s {
-	uint16_t msg_size = sizeof(uint16_t); //size of message - MSG_HEADER_LEN
-	uint8_t msg_type = static_cast<uint8_t>(MessageType::DROPOUT);
+struct ulog_message_dropout_s {
+	uint16_t msg_size = sizeof(uint16_t); //size of message - ULOG_MSG_HEADER_LEN
+	uint8_t msg_type = static_cast<uint8_t>(ULogMessageType::DROPOUT);
 
 	uint16_t duration; //in ms
 };
 
-struct message_data_header_s {
-	uint16_t msg_size; //size of message - MSG_HEADER_LEN
-	uint8_t msg_type = static_cast<uint8_t>(MessageType::DATA);
+struct ulog_message_data_header_s {
+	uint16_t msg_size; //size of message - ULOG_MSG_HEADER_LEN
+	uint8_t msg_type = static_cast<uint8_t>(ULogMessageType::DATA);
 
 	uint16_t msg_id;
 };
 
-struct message_info_header_s {
-	uint16_t msg_size; //size of message - MSG_HEADER_LEN
-	uint8_t msg_type = static_cast<uint8_t>(MessageType::INFO);
+struct ulog_message_info_header_s {
+	uint16_t msg_size; //size of message - ULOG_MSG_HEADER_LEN
+	uint8_t msg_type = static_cast<uint8_t>(ULogMessageType::INFO);
 
 	uint8_t key_len;
 	char key[255];
 };
 
-struct message_logging_s {
-	uint16_t msg_size; //size of message - MSG_HEADER_LEN
-	uint8_t msg_type = static_cast<uint8_t>(MessageType::LOGGING);
+struct ulog_message_logging_s {
+	uint16_t msg_size; //size of message - ULOG_MSG_HEADER_LEN
+	uint8_t msg_type = static_cast<uint8_t>(ULogMessageType::LOGGING);
 
 	uint8_t log_level; //same levels as in the linux kernel
 	uint64_t timestamp;
 	char message[255];
 };
 
-struct message_parameter_header_s {
+struct ulog_message_parameter_header_s {
 	uint16_t msg_size;
-	uint8_t msg_type = static_cast<uint8_t>(MessageType::PARAMETER);
+	uint8_t msg_type = static_cast<uint8_t>(ULogMessageType::PARAMETER);
 
 	uint8_t key_len;
 	char key[255];

From 25cff52019086b9e6e728fdc0489c10b32f250d9 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Beat=20K=C3=BCng?= <beat-kueng@gmx.net>
Date: Tue, 24 May 2016 10:00:59 +0200
Subject: [PATCH 0456/1230] RTCM: use MAVLINK_MSG_ID_GPS_RTCM_DATA mavlink
 message (supports fragmentation)

---
 msg/gps_inject_data.msg                  |  3 ++-
 src/drivers/gps/gps.cpp                  |  5 +++++
 src/modules/mavlink/mavlink_receiver.cpp | 17 +++++++++--------
 src/modules/mavlink/mavlink_receiver.h   |  2 +-
 4 files changed, 17 insertions(+), 10 deletions(-)

diff --git a/msg/gps_inject_data.msg b/msg/gps_inject_data.msg
index 4ba4ebc341..ac44d02dbe 100644
--- a/msg/gps_inject_data.msg
+++ b/msg/gps_inject_data.msg
@@ -1,2 +1,3 @@
 uint8 len			# length of data
-uint8[110] data			# data to write to GPS device
+uint8 flags         		# LSB: 1=fragmented
+uint8[182] data		# data to write to GPS device (RTCM message)
diff --git a/src/drivers/gps/gps.cpp b/src/drivers/gps/gps.cpp
index 926611c7b6..a502548b28 100644
--- a/src/drivers/gps/gps.cpp
+++ b/src/drivers/gps/gps.cpp
@@ -441,6 +441,11 @@ void GPS::handleInjectDataTopic()
 		if (updated) {
 			struct gps_inject_data_s msg;
 			orb_copy(ORB_ID(gps_inject_data), orb_inject_data_cur_fd, &msg);
+
+			/* Write the message to the gps device. Note that the message could be fragmented.
+			 * But as we don't write anywhere else to the device during operation, we don't
+			 * need to assemble the message first.
+			 */
 			injectData(msg.data, msg.len);
 
 			_orb_inject_data_next = (_orb_inject_data_next + 1) % _orb_inject_data_fd_count;
diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp
index 557c5caefd..2671999ab8 100644
--- a/src/modules/mavlink/mavlink_receiver.cpp
+++ b/src/modules/mavlink/mavlink_receiver.cpp
@@ -250,8 +250,8 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg)
 		handle_message_adsb_vehicle(msg);
 		break;
 
-	case MAVLINK_MSG_ID_GPS_INJECT_DATA:
-		handle_message_gps_inject_data(msg);
+	case MAVLINK_MSG_ID_GPS_RTCM_DATA:
+		handle_message_gps_rtcm_data(msg);
 		break;
 
 	default:
@@ -1775,16 +1775,17 @@ void MavlinkReceiver::handle_message_adsb_vehicle(mavlink_message_t *msg)
 	}
 }
 
-void MavlinkReceiver::handle_message_gps_inject_data(mavlink_message_t *msg)
+void MavlinkReceiver::handle_message_gps_rtcm_data(mavlink_message_t *msg)
 {
-	mavlink_gps_inject_data_t gps_inject_data_msg;
+	mavlink_gps_rtcm_data_t gps_rtcm_data_msg;
 	gps_inject_data_s gps_inject_data_topic;
 
-	mavlink_msg_gps_inject_data_decode(msg, &gps_inject_data_msg);
+	mavlink_msg_gps_rtcm_data_decode(msg, &gps_rtcm_data_msg);
 
-	gps_inject_data_topic.len = gps_inject_data_msg.len;
-	memcpy(gps_inject_data_topic.data, gps_inject_data_msg.data,
-	       math::min((int)sizeof(gps_inject_data_topic.data), (int)sizeof(uint8_t) * gps_inject_data_msg.len));
+	gps_inject_data_topic.len = gps_rtcm_data_msg.len;
+	gps_inject_data_topic.flags = gps_rtcm_data_msg.flags;
+	memcpy(gps_inject_data_topic.data, gps_rtcm_data_msg.data,
+	       math::min((int)sizeof(gps_inject_data_topic.data), (int)sizeof(uint8_t) * gps_rtcm_data_msg.len));
 
 	orb_advert_t &pub = _gps_inject_data_pub[_gps_inject_data_next_idx];
 
diff --git a/src/modules/mavlink/mavlink_receiver.h b/src/modules/mavlink/mavlink_receiver.h
index 96a7da80cb..28ee0fa04e 100644
--- a/src/modules/mavlink/mavlink_receiver.h
+++ b/src/modules/mavlink/mavlink_receiver.h
@@ -141,7 +141,7 @@ private:
 	void handle_message_distance_sensor(mavlink_message_t *msg);
 	void handle_message_follow_target(mavlink_message_t *msg);
 	void handle_message_adsb_vehicle(mavlink_message_t *msg);
-	void handle_message_gps_inject_data(mavlink_message_t *msg);
+	void handle_message_gps_rtcm_data(mavlink_message_t *msg);
 
 	void *receive_thread(void *arg);
 

From d35814ed992a0e7182a4abb71b4927391cf402e9 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Beat=20K=C3=BCng?= <beat-kueng@gmx.net>
Date: Tue, 24 May 2016 10:02:57 +0200
Subject: [PATCH 0457/1230] nuttx px4fmu-v4 config: increase
 CONFIG_NFILE_DESCRIPTORS to 52

necessary for mavlink receiver. It had the following output:
 mavlink_rcv_if0: node_open as advertiser failed.
---
 nuttx-configs/px4fmu-v4/nsh/defconfig | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/nuttx-configs/px4fmu-v4/nsh/defconfig b/nuttx-configs/px4fmu-v4/nsh/defconfig
index 0d339753fa..eb4fdc9d20 100644
--- a/nuttx-configs/px4fmu-v4/nsh/defconfig
+++ b/nuttx-configs/px4fmu-v4/nsh/defconfig
@@ -442,7 +442,7 @@ CONFIG_SIG_SIGWORK=4
 CONFIG_MAX_TASKS=32
 CONFIG_MAX_TASK_ARGS=10
 CONFIG_NPTHREAD_KEYS=4
-CONFIG_NFILE_DESCRIPTORS=42
+CONFIG_NFILE_DESCRIPTORS=52
 CONFIG_NFILE_STREAMS=8
 CONFIG_NAME_MAX=32
 CONFIG_PREALLOC_MQ_MSGS=4

From b226f7d13d348ed481f0af0fe33ae28f0b981861 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Beat=20K=C3=BCng?= <beat-kueng@gmx.net>
Date: Tue, 31 May 2016 15:10:22 +0200
Subject: [PATCH 0458/1230] gps: call fsync after injecting gps data

This is to minimize delay
---
 src/drivers/gps/gps.cpp | 4 +++-
 1 file changed, 3 insertions(+), 1 deletion(-)

diff --git a/src/drivers/gps/gps.cpp b/src/drivers/gps/gps.cpp
index a502548b28..3e29be8425 100644
--- a/src/drivers/gps/gps.cpp
+++ b/src/drivers/gps/gps.cpp
@@ -463,7 +463,9 @@ bool GPS::injectData(uint8_t *data, size_t len)
 		}
 	}
 
-	return ::write(_serial_fd, data, len) == len;
+	size_t written = ::write(_serial_fd, data, len);
+	::fsync(_serial_fd);
+	return written == len;
 }
 
 int GPS::setBaudrate(unsigned baud)

From fc2d2c6ce077c8ba2d4df80ecdb0e5ae4ee03f29 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Beat=20K=C3=BCng?= <beat-kueng@gmx.net>
Date: Wed, 1 Jun 2016 16:16:25 +0200
Subject: [PATCH 0459/1230] gps: update drivers submodule

---
 src/drivers/gps/devices | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/src/drivers/gps/devices b/src/drivers/gps/devices
index 8f2d911a52..5c1ae95655 160000
--- a/src/drivers/gps/devices
+++ b/src/drivers/gps/devices
@@ -1 +1 @@
-Subproject commit 8f2d911a5203e7f6d29c1e3cf3405972a87215d8
+Subproject commit 5c1ae956552c3887c5fddd303a8f242efa715333

From f07a4f2f939bdc9c5e5220798d999073dd151674 Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Thu, 2 Jun 2016 09:27:15 +0200
Subject: [PATCH 0460/1230] MAVLink app needs more FDs

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

diff --git a/nuttx-configs/px4fmu-v2/nsh/defconfig b/nuttx-configs/px4fmu-v2/nsh/defconfig
index 193394ef40..2322043b6d 100644
--- a/nuttx-configs/px4fmu-v2/nsh/defconfig
+++ b/nuttx-configs/px4fmu-v2/nsh/defconfig
@@ -442,7 +442,7 @@ CONFIG_SIG_SIGWORK=4
 CONFIG_MAX_TASKS=32
 CONFIG_MAX_TASK_ARGS=10
 CONFIG_NPTHREAD_KEYS=4
-CONFIG_NFILE_DESCRIPTORS=42
+CONFIG_NFILE_DESCRIPTORS=47
 CONFIG_NFILE_STREAMS=8
 CONFIG_NAME_MAX=32
 CONFIG_PREALLOC_MQ_MSGS=4

From fb1fd205bd2beea683611725607eb0efb0cdf76e Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Thu, 2 Jun 2016 09:27:26 +0200
Subject: [PATCH 0461/1230] MAVLink app needs more FDs

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

diff --git a/nuttx-configs/px4fmu-v4/nsh/defconfig b/nuttx-configs/px4fmu-v4/nsh/defconfig
index eb4fdc9d20..ccb6b5c132 100644
--- a/nuttx-configs/px4fmu-v4/nsh/defconfig
+++ b/nuttx-configs/px4fmu-v4/nsh/defconfig
@@ -442,7 +442,7 @@ CONFIG_SIG_SIGWORK=4
 CONFIG_MAX_TASKS=32
 CONFIG_MAX_TASK_ARGS=10
 CONFIG_NPTHREAD_KEYS=4
-CONFIG_NFILE_DESCRIPTORS=52
+CONFIG_NFILE_DESCRIPTORS=47
 CONFIG_NFILE_STREAMS=8
 CONFIG_NAME_MAX=32
 CONFIG_PREALLOC_MQ_MSGS=4

From f06e6f0ce11d22f8eff877bcaaee78839c9c52ef Mon Sep 17 00:00:00 2001
From: BharathR <bharath374@users.noreply.github.com>
Date: Fri, 3 Jun 2016 01:22:09 -0700
Subject: [PATCH 0462/1230] Fixed actuator controls struct in Snapdragon legacy
 uart_esc driver wrapper (#4724)

---
 src/platforms/qurt/fc_addon/uart_esc/uart_esc_main.cpp | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/src/platforms/qurt/fc_addon/uart_esc/uart_esc_main.cpp b/src/platforms/qurt/fc_addon/uart_esc/uart_esc_main.cpp
index ebeefcd893..8890c6d1e9 100644
--- a/src/platforms/qurt/fc_addon/uart_esc/uart_esc_main.cpp
+++ b/src/platforms/qurt/fc_addon/uart_esc/uart_esc_main.cpp
@@ -338,7 +338,7 @@ void task_main(int argc, char *argv[])
 
 				if (_armed.armed) {
 					_outputs.noutputs = mixer->mix(&_outputs.output[0],
-								       actuator_controls_0_s::NUM_ACTUATOR_CONTROLS,
+								       actuator_controls_s::NUM_ACTUATOR_CONTROLS,
 								       NULL);
 
 					// Make sure we support only up to UART_ESC_MAX_MOTORS motors

From de5bf60b07e164c87a004e325a58ffdc118af948 Mon Sep 17 00:00:00 2001
From: Daniel Agar <daniel@agar.ca>
Date: Fri, 3 Jun 2016 16:48:30 -0400
Subject: [PATCH 0463/1230] make submodulesclean sync recursively

---
 Makefile | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/Makefile b/Makefile
index 27eb42e9a8..d4b4cf1afe 100644
--- a/Makefile
+++ b/Makefile
@@ -304,7 +304,7 @@ clean:
 	@(cd NuttX/nuttx && make clean)
 
 submodulesclean:
-	@git submodule sync
+	@git submodule sync --recursive
 	@git submodule deinit -f .
 	@git submodule update --init --recursive --force
 

From 7524474c7b8249a504169e43b3c58d1f0df5a7c8 Mon Sep 17 00:00:00 2001
From: Daniel Agar <daniel@agar.ca>
Date: Fri, 3 Jun 2016 16:50:26 -0400
Subject: [PATCH 0464/1230] make distclean ignore eclipse project files

---
 Makefile | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/Makefile b/Makefile
index d4b4cf1afe..6ec7d72f78 100644
--- a/Makefile
+++ b/Makefile
@@ -309,7 +309,7 @@ submodulesclean:
 	@git submodule update --init --recursive --force
 
 distclean: submodulesclean
-	@git clean -ff -x -d
+	@git clean -ff -x -d -e ".project" -e ".cproject"
 
 # targets handled by cmake
 cmake_targets = test upload package package_source debug debug_tui debug_ddd debug_io debug_io_tui debug_io_ddd check_weak \

From 6f9665d7188f297982685acc35cf8c73199140ed Mon Sep 17 00:00:00 2001
From: bharathr <bharathr@qti.qualcomm.com>
Date: Fri, 3 Jun 2016 19:44:56 -0700
Subject: [PATCH 0465/1230] Restored 210qc snapdragon config files

---
 posix-configs/eagle/210qc/mainapp.config |  7 ++
 posix-configs/eagle/210qc/px4.config     | 93 ++++++++++++++++++++++++
 2 files changed, 100 insertions(+)
 create mode 100644 posix-configs/eagle/210qc/mainapp.config
 create mode 100644 posix-configs/eagle/210qc/px4.config

diff --git a/posix-configs/eagle/210qc/mainapp.config b/posix-configs/eagle/210qc/mainapp.config
new file mode 100644
index 0000000000..b511bb1121
--- /dev/null
+++ b/posix-configs/eagle/210qc/mainapp.config
@@ -0,0 +1,7 @@
+uorb start
+muorb start
+mavlink start -u 14556
+sleep 1
+mavlink stream -u 14556 -s HIGHRES_IMU -r 50
+mavlink stream -u 14556 -s ATTITUDE -r 50
+mavlink boot_complete
diff --git a/posix-configs/eagle/210qc/px4.config b/posix-configs/eagle/210qc/px4.config
new file mode 100644
index 0000000000..945be5e8de
--- /dev/null
+++ b/posix-configs/eagle/210qc/px4.config
@@ -0,0 +1,93 @@
+uorb start
+qshell start
+param set CAL_GYRO0_ID 100
+param set CAL_ACC0_ID 100
+param set CAL_MAG0_ID 101
+param set SYS_AUTOSTART 4001
+param set MAV_TYPE 2
+param set MC_YAW_P 12
+param set MC_YAWRATE_P 0.08
+param set MC_YAWRATE_I 0.0
+param set MC_YAWRATE_D 0
+param set MC_PITCH_P 7.0
+param set MC_PITCHRATE_P 0.08
+param set MC_PITCHRATE_I 0.0
+param set MC_PITCHRATE_D 0.001
+param set MC_ROLL_P 7.0
+param set MC_ROLLRATE_P 0.08
+param set MC_ROLLRATE_I 0.0
+param set MC_ROLLRATE_D 0.001
+param set RC_MAP_THROTTLE 1
+param set RC_MAP_ROLL 2
+param set RC_MAP_PITCH 3
+param set RC_MAP_YAW 4
+param set RC_MAP_MODE_SW 5
+param set RC_MAP_POSCTL_SW 6
+param set RC1_MAX 1900
+param set RC1_MIN 1099
+param set RC1_TRIM 1099
+param set RC1_REV 1
+param set RC2_MAX 1900
+param set RC2_MIN 1099
+param set RC2_TRIM 1500
+param set RC2_REV -1
+param set RC3_MAX 1900
+param set RC3_MIN 1099
+param set RC3_TRIM 1500
+param set RC3_REV 1
+param set RC4_MAX 1900
+param set RC4_MIN 1099
+param set RC4_TRIM 1500
+param set RC4_REV -1
+param set RC5_MAX 1900
+param set RC5_MIN 1099
+param set RC5_TRIM 1500
+param set RC5_REV 1
+param set RC6_MAX 1900
+param set RC6_MIN 1099
+param set RC6_TRIM 1099
+param set RC6_REV 1
+param set ATT_W_MAG 0.00
+param set PE_MAG_NOISE 1.0f
+param set SENS_BOARD_ROT 0
+param set CAL_GYRO0_XOFF -0.0028
+param set CAL_GYRO0_YOFF -0.0047
+param set CAL_GYRO0_ZOFF -0.0034
+param set CAL_GYRO0_XSCALE 1.0000
+param set CAL_GYRO0_YSCALE 1.0000
+param set CAL_GYRO0_ZSCALE 1.0000
+param set CAL_MAG0_XOFF 0.0000
+param set CAL_MAG0_YOFF 0.0000
+param set CAL_MAG0_ZOFF 0.0000
+param set CAL_MAG0_XSCALE 1.0000
+param set CAL_MAG0_YSCALE 1.0000
+param set CAL_MAG0_ZSCALE 1.0000
+param set CAL_ACC0_XOFF -0.0941
+param set CAL_ACC0_YOFF 0.1168
+param set CAL_ACC0_ZOFF -0.0398
+param set CAL_ACC0_XSCALE 1.0004
+param set CAL_ACC0_YSCALE 0.9974
+param set CAL_ACC0_ZSCALE 0.9951
+param set RC_RECEIVER_TYPE 1
+param set UART_ESC_MODEL 2
+param set UART_ESC_BAUDRTE 250000
+param set UART_ESC_MOTOR1 4
+param set UART_ESC_MOTOR2 2
+param set UART_ESC_MOTOR3 1
+param set UART_ESC_MOTOR4 3
+sleep 1
+df_mpu9250_wrapper start
+df_bmp280_wrapper start
+sensors start
+commander start
+ekf2 start
+land_detector start multicopter
+mc_pos_control start
+mc_att_control start
+uart_esc start -D /dev/tty-2
+rc_receiver start -D /dev/tty-1
+sleep 1
+list_devices
+list_files
+list_tasks
+list_topics

From 5e00155e033ba23034a0a78a4eb0e3cd98c39011 Mon Sep 17 00:00:00 2001
From: Kartik Mohta <kartikmohta@gmail.com>
Date: Sat, 4 Jun 2016 01:03:45 -0400
Subject: [PATCH 0466/1230] Update cmake/cmake_hexagon

New updates required to use the Hexagon SDK v3.0 from Qualcomm
---
 cmake/cmake_hexagon | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/cmake/cmake_hexagon b/cmake/cmake_hexagon
index 3187dd7648..c0ba14e598 160000
--- a/cmake/cmake_hexagon
+++ b/cmake/cmake_hexagon
@@ -1 +1 @@
-Subproject commit 3187dd7648adcb0abca7904e402507203e4c25a9
+Subproject commit c0ba14e5983fd197cfff8baa70224d1d67908f0c

From 1880d1f6fd92c3477774df0fb18192677cade59b Mon Sep 17 00:00:00 2001
From: Julian Oes <julian@oes.ch>
Date: Sun, 5 Jun 2016 11:01:38 +0100
Subject: [PATCH 0467/1230] Snapdragon: start sdlog first to log everything
 (#4734)

In order to capture everything for replay logs, start sdlog2 at the very
beginning. Also, wait with starting the sensors for a second, to make
sure sdlog2 has started by then.
---
 posix-configs/eagle/flight/mainapp.config | 2 +-
 posix-configs/eagle/flight/px4.config     | 1 +
 2 files changed, 2 insertions(+), 1 deletion(-)

diff --git a/posix-configs/eagle/flight/mainapp.config b/posix-configs/eagle/flight/mainapp.config
index b2bccc62c4..bafdb746d2 100644
--- a/posix-configs/eagle/flight/mainapp.config
+++ b/posix-configs/eagle/flight/mainapp.config
@@ -1,5 +1,6 @@
 uorb start
 muorb start
+sdlog2 start -r 100 -e -t -a -b 200
 dataman start
 navigator start
 mavlink start -u 14556 -r 1000000
@@ -7,4 +8,3 @@ sleep 1
 mavlink stream -u 14556 -s HIGHRES_IMU -r 50
 mavlink stream -u 14556 -s ATTITUDE -r 50
 mavlink boot_complete
-sdlog2 start -r 100 -e -t -a -b 200
diff --git a/posix-configs/eagle/flight/px4.config b/posix-configs/eagle/flight/px4.config
index 1d1084d36f..acf18e07a8 100644
--- a/posix-configs/eagle/flight/px4.config
+++ b/posix-configs/eagle/flight/px4.config
@@ -1,6 +1,7 @@
 uorb start
 qshell start
 param set SYS_AUTOSTART 4001
+sleep 1
 gps start -d /dev/tty-4
 sleep 1
 param set MAV_TYPE 2

From eea102f63dd4baac1cb186b43df05890e7db60e2 Mon Sep 17 00:00:00 2001
From: Julian Oes <julian@oes.ch>
Date: Sun, 5 Jun 2016 11:02:13 +0100
Subject: [PATCH 0468/1230] cmake: POSIX/RPi build don't need patch/genromfs
 (#4733)

---
 cmake/toolchains/Toolchain-arm-linux-gnueabihf-raspbian.cmake | 3 +--
 cmake/toolchains/Toolchain-native.cmake                       | 2 +-
 2 files changed, 2 insertions(+), 3 deletions(-)

diff --git a/cmake/toolchains/Toolchain-arm-linux-gnueabihf-raspbian.cmake b/cmake/toolchains/Toolchain-arm-linux-gnueabihf-raspbian.cmake
index 3b17c83558..0a9d2b4f3b 100644
--- a/cmake/toolchains/Toolchain-arm-linux-gnueabihf-raspbian.cmake
+++ b/cmake/toolchains/Toolchain-arm-linux-gnueabihf-raspbian.cmake
@@ -7,7 +7,6 @@
 # C_COMPILER
 # CMAKE_SYSTEM_NAME
 # CMAKE_SYSTEM_VERSION
-# GENROMFS
 # LINKER_FLAGS
 # CMAKE_EXE_LINKER_FLAGS
 # CMAKE_FIND_ROOT_PATH
@@ -63,7 +62,7 @@ foreach(tool objcopy nm ld)
 endforeach()
 
 # os tools
-foreach(tool echo patch grep rm mkdir nm genromfs cp touch make unzip)
+foreach(tool echo grep rm mkdir nm cp touch make unzip)
 	string(TOUPPER ${tool} TOOL)
 	find_program(${TOOL} ${tool})
 	if(NOT ${TOOL})
diff --git a/cmake/toolchains/Toolchain-native.cmake b/cmake/toolchains/Toolchain-native.cmake
index 888c73a6d8..2899671043 100644
--- a/cmake/toolchains/Toolchain-native.cmake
+++ b/cmake/toolchains/Toolchain-native.cmake
@@ -8,7 +8,7 @@ foreach(tool nm ld)
 endforeach()
 
 # os tools
-foreach(tool echo patch grep rm mkdir nm genromfs cp touch make unzip)
+foreach(tool echo grep rm mkdir nm cp touch make unzip)
 	string(TOUPPER ${tool} TOOL)
 	find_program(${TOOL} ${tool})
 	if(NOT ${TOOL})

From f8ff667ddeff000d7b7099b83d6a4e801d02c345 Mon Sep 17 00:00:00 2001
From: CAI Dongcai <cia120@163.com>
Date: Sun, 5 Jun 2016 12:08:31 +0200
Subject: [PATCH 0469/1230] pwm steps bugs fixed (#4719)

fix the divide 0 bug in using "pwm steps" in NSH, and add it's
description
---
 src/systemcmds/pwm/pwm.c | 7 +++++--
 1 file changed, 5 insertions(+), 2 deletions(-)

diff --git a/src/systemcmds/pwm/pwm.c b/src/systemcmds/pwm/pwm.c
index 1457e7fe2d..302ed5aaf2 100644
--- a/src/systemcmds/pwm/pwm.c
+++ b/src/systemcmds/pwm/pwm.c
@@ -75,7 +75,7 @@ usage(const char *reason)
 
 	errx(1,
 	     "usage:\n"
-	     "pwm arm|disarm|rate|failsafe|disarmed|min|max|test|info  ...\n"
+	     "pwm arm|disarm|rate|failsafe|disarmed|min|max|test|steps|info  ...\n"
 	     "\n"
 	     "arm\t\t\t\tArm output\n"
 	     "disarm\t\t\t\tDisarm output\n"
@@ -101,6 +101,9 @@ usage(const char *reason)
 	     "\t[-a]\t\t\tConfigure all outputs\n"
 	     "\t-p <pwm value>\t\tPWM value\n"
 	     "\n"
+	     "steps ...\t\t\tRun 5 steps\n"
+	     "\t[-c <channels>]\t\t(e.g. 1234)\n"
+	     "\n"
 	     "info\t\t\t\tPrint information\n"
 	     "\n"
 	     "\t-v\t\t\tVerbose\n"
@@ -663,7 +666,7 @@ pwm_main(int argc, char *argv[])
 
 						} else if (phase == 1) {
 							/* ramp - depending how steep it is this ramp will look instantaneous on the output */
-							val = idle + (full - idle) * (phase_maxcount / (float)phase_counter);
+							val = idle + (full - idle) * ((float)phase_counter / phase_maxcount);
 
 						} else {
 							val = off;

From ff4f27b05e8a32cf30d1cae1a512de632683a3aa Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Sun, 5 Jun 2016 12:39:22 +0200
Subject: [PATCH 0470/1230] MAVLink app: Add option to configure broadcast,
 default to off

---
 src/modules/mavlink/mavlink_main.cpp | 40 +++++++++++++++++++++++-----
 src/modules/mavlink/mavlink_main.h   | 11 +++++++-
 src/modules/mavlink/mavlink_params.c | 12 +++++++++
 3 files changed, 55 insertions(+), 8 deletions(-)

diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp
index c76d39074e..2cdc32e69d 100644
--- a/src/modules/mavlink/mavlink_main.cpp
+++ b/src/modules/mavlink/mavlink_main.cpp
@@ -237,12 +237,15 @@ Mavlink::Mavlink() :
 	_message_buffer_mutex {},
 	_send_mutex {},
 	_param_initialized(false),
-	_param_system_id(0),
-	_param_component_id(0),
-	_param_radio_id(0),
-	_param_system_type(MAV_TYPE_FIXED_WING),
-	_param_use_hil_gps(0),
-	_param_forward_externalsp(0),
+	_logging_enabled(false),
+	_broadcast_mode(Mavlink::BROADCAST_MODE_OFF),
+	_param_system_id(PARAM_INVALID),
+	_param_component_id(PARAM_INVALID),
+	_param_radio_id(PARAM_INVALID),
+	_param_system_type(PARAM_INVALID),
+	_param_use_hil_gps(PARAM_INVALID),
+	_param_forward_externalsp(PARAM_INVALID),
+	_param_broadcast(PARAM_INVALID),
 	_system_type(0),
 
 	/* performance counters */
@@ -539,6 +542,7 @@ void Mavlink::mavlink_update_system(void)
 		_param_system_type = param_find("MAV_TYPE");
 		_param_use_hil_gps = param_find("MAV_USEHILGPS");
 		_param_forward_externalsp = param_find("MAV_FWDEXTSP");
+		_param_broadcast = param_find("MAV_BROADCAST");
 
 		/* test param - needs to be referenced, but is unused */
 		(void)param_find("MAV_TEST_PAR");
@@ -597,6 +601,8 @@ void Mavlink::mavlink_update_system(void)
 	int32_t forward_externalsp;
 	param_get(_param_forward_externalsp, &forward_externalsp);
 
+	param_get(_param_broadcast, &_broadcast_mode);
+
 	_forward_externalsp = (bool)forward_externalsp;
 }
 
@@ -889,7 +895,7 @@ Mavlink::send_packet()
 		struct telemetry_status_s &tstatus = get_rx_status();
 
 		/* resend message via broadcast if no valid connection exists */
-		if ((_mode != MAVLINK_MODE_ONBOARD) &&
+		if ((_mode != MAVLINK_MODE_ONBOARD) && broadcast_enabled() &&
 			(!get_client_source_initialized()
 			|| (hrt_elapsed_time(&tstatus.heartbeat_time) > 3 * 1000 * 1000))) {
 
@@ -2475,6 +2481,26 @@ Mavlink::stream_command(int argc, char *argv[])
 	return OK;
 }
 
+void
+Mavlink::set_boot_complete()
+{
+	_boot_complete = true;
+
+#ifdef __PX4_POSIX
+	Mavlink *inst;
+	LL_FOREACH(::_mavlink_instances, inst) {
+		if ((inst->get_mode() != MAVLINK_MODE_ONBOARD) &&
+			(!inst->broadcast_enabled()) &&
+			((inst->get_protocol() == UDP) || (inst->get_protocol() == TCP))) {
+			printf("\n\n   **********   NETWORK BROADCAST NOT ENABLED   **********\n" \
+				"   The drone will only connect to localhost (excluding VMs).\n" \
+				"   Run 'pxh> param set MAV_BROADCAST 1' to enable.\n");
+		}
+	}
+#endif
+
+}
+
 static void usage()
 {
 	warnx("usage: mavlink {start|stop|stream} [-d device] [-u network_port] [-o remote_port] [-t partner_ip] [-b baudrate]\n\t[-r rate][-m mode] [-s stream] [-f] [-p] [-v] [-w] [-x]");
diff --git a/src/modules/mavlink/mavlink_main.h b/src/modules/mavlink/mavlink_main.h
index d47289a32a..8391bfb6e9 100644
--- a/src/modules/mavlink/mavlink_main.h
+++ b/src/modules/mavlink/mavlink_main.h
@@ -164,6 +164,11 @@ public:
 		MAVLINK_MODE_CONFIG
 	};
 
+	enum BROADCAST_MODE {
+		BROADCAST_MODE_OFF = 0,
+		BROADCAST_MODE_ON
+	};
+
 	void			set_mode(enum MAVLINK_MODE);
 	enum MAVLINK_MODE	get_mode() { return _mode; }
 
@@ -181,13 +186,15 @@ public:
 
 	void			set_config_link_on(bool on) { _config_link_on = on; }
 
+	bool			broadcast_enabled() { return _broadcast_mode > BROADCAST_MODE_OFF; }
+
 	/**
 	 * Set the boot complete flag on all instances
 	 *
 	 * Setting the flag unblocks parameter transmissions, which are gated
 	 * beforehand to ensure that the system is fully initialized.
 	 */
-	static void		set_boot_complete() { _boot_complete = true; }
+	static void		set_boot_complete();
 
 	/**
 	 * Get the free space in the transmit buffer
@@ -504,6 +511,7 @@ private:
 
 	bool			_param_initialized;
 	bool			_logging_enabled;
+	uint32_t		_broadcast_mode;
 
 	param_t			_param_system_id;
 	param_t			_param_component_id;
@@ -512,6 +520,7 @@ private:
 	param_t			_param_system_type;
 	param_t			_param_use_hil_gps;
 	param_t			_param_forward_externalsp;
+	param_t			_param_broadcast;
 
 	unsigned		_system_type;
 	static bool		_config_link_on;
diff --git a/src/modules/mavlink/mavlink_params.c b/src/modules/mavlink/mavlink_params.c
index 44964b9840..c988b9aa0f 100644
--- a/src/modules/mavlink/mavlink_params.c
+++ b/src/modules/mavlink/mavlink_params.c
@@ -99,6 +99,18 @@ PARAM_DEFINE_INT32(MAV_USEHILGPS, 0);
  */
 PARAM_DEFINE_INT32(MAV_FWDEXTSP, 1);
 
+/**
+ * Broadcast heartbeats on local network
+ *
+ * This allows a ground control station to automatically find the drone
+ * on the local network.
+ *
+ * @value 0 Never broadcast
+ * @value 1 Always broadcast
+ * @group MAVLink
+ */
+PARAM_DEFINE_INT32(MAV_BROADCAST, 0);
+
 /**
  * Test parameter
  *

From e19b373347ea68df1cf303f13019b8b85a3211e3 Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Sun, 5 Jun 2016 12:40:25 +0200
Subject: [PATCH 0471/1230] Updatee SITL config rcS_gazebo_iris

---
 posix-configs/SITL/init/rcS_gazebo_iris | 6 +++---
 1 file changed, 3 insertions(+), 3 deletions(-)

diff --git a/posix-configs/SITL/init/rcS_gazebo_iris b/posix-configs/SITL/init/rcS_gazebo_iris
index fdadd243bb..e4d8c958a9 100644
--- a/posix-configs/SITL/init/rcS_gazebo_iris
+++ b/posix-configs/SITL/init/rcS_gazebo_iris
@@ -59,8 +59,8 @@ ekf2 start
 mc_pos_control start
 mc_att_control start
 mixer load /dev/pwm_output0 ../../../../ROMFS/px4fmu_common/mixers/quad_w.main.mix
-mavlink start -u 14556 -r 4000000 -t 127.0.0.1
-mavlink start -u 14557 -r 4000000 -m onboard -o 14540 -t 127.0.0.1
+mavlink start -u 14556 -r 4000000
+mavlink start -u 14557 -r 4000000 -m onboard -o 14540
 mavlink stream -r 50 -s POSITION_TARGET_LOCAL_NED -u 14556
 mavlink stream -r 50 -s LOCAL_POSITION_NED -u 14556
 mavlink stream -r 50 -s GLOBAL_POSITION_INT -u 14556
@@ -71,5 +71,5 @@ mavlink stream -r 50 -s SERVO_OUTPUT_RAW_0 -u 14556
 mavlink stream -r 20 -s RC_CHANNELS -u 14556
 mavlink stream -r 250 -s HIGHRES_IMU -u 14556
 mavlink stream -r 10 -s OPTICAL_FLOW_RAD -u 14556
-mavlink boot_complete
 sdlog2 start -r 100 -e -t -a
+mavlink boot_complete

From b198638a6cc42233f0ca950301ea3f793f3d6255 Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Sun, 5 Jun 2016 12:40:41 +0200
Subject: [PATCH 0472/1230] Update SITL config
 posix-configs/SITL/init/rcS_gazebo_iris_opt_flow

---
 posix-configs/SITL/init/rcS_gazebo_iris_opt_flow | 6 +++---
 1 file changed, 3 insertions(+), 3 deletions(-)

diff --git a/posix-configs/SITL/init/rcS_gazebo_iris_opt_flow b/posix-configs/SITL/init/rcS_gazebo_iris_opt_flow
index 3c6a577bad..59fa38daf3 100644
--- a/posix-configs/SITL/init/rcS_gazebo_iris_opt_flow
+++ b/posix-configs/SITL/init/rcS_gazebo_iris_opt_flow
@@ -58,8 +58,8 @@ position_estimator_inav start
 mc_pos_control start
 mc_att_control start
 mixer load /dev/pwm_output0 ../../../../ROMFS/px4fmu_common/mixers/quad_w.main.mix
-mavlink start -u 14556 -r 2000000 -t 127.0.0.1
-mavlink start -u 14557 -r 2000000 -m onboard -o 14540 -t 127.0.0.1
+mavlink start -u 14556 -r 2000000
+mavlink start -u 14557 -r 2000000 -m onboard -o 14540
 mavlink stream -r 80 -s POSITION_TARGET_LOCAL_NED -u 14556
 mavlink stream -r 80 -s LOCAL_POSITION_NED -u 14556
 mavlink stream -r 80 -s GLOBAL_POSITION_INT -u 14556
@@ -69,5 +69,5 @@ mavlink stream -r 80 -s ATTITUDE_TARGET -u 14556
 mavlink stream -r 20 -s RC_CHANNELS -u 14556
 mavlink stream -r 250 -s HIGHRES_IMU -u 14556
 mavlink stream -r 10 -s OPTICAL_FLOW_RAD -u 14556
-mavlink boot_complete
 sdlog2 start -r 100 -e -t -a
+mavlink boot_complete

From a651f3dae537c1689c9870f8ae4f547ab57970c6 Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Sun, 5 Jun 2016 12:41:43 +0200
Subject: [PATCH 0473/1230] Update SITL config
 posix-configs/SITL/init/rcS_gazebo_plane

---
 posix-configs/SITL/init/rcS_gazebo_plane | 6 +++---
 1 file changed, 3 insertions(+), 3 deletions(-)

diff --git a/posix-configs/SITL/init/rcS_gazebo_plane b/posix-configs/SITL/init/rcS_gazebo_plane
index ffe21a36ac..bcd62ca582 100644
--- a/posix-configs/SITL/init/rcS_gazebo_plane
+++ b/posix-configs/SITL/init/rcS_gazebo_plane
@@ -43,8 +43,8 @@ ekf2 start
 fw_pos_control_l1 start
 fw_att_control start
 mixer load /dev/pwm_output0 ../../../../ROMFS/px4fmu_common/mixers/plane_sitl.main.mix
-mavlink start -u 14556 -r 2000000 -t 127.0.0.1
-mavlink start -u 14557 -r 2000000 -m onboard -o 14540 -t 127.0.0.1
+mavlink start -u 14556 -r 2000000
+mavlink start -u 14557 -r 2000000 -m onboard -o 14540
 mavlink stream -r 80 -s POSITION_TARGET_LOCAL_NED -u 14556
 mavlink stream -r 80 -s LOCAL_POSITION_NED -u 14556
 mavlink stream -r 80 -s GLOBAL_POSITION_INT -u 14556
@@ -54,5 +54,5 @@ mavlink stream -r 80 -s ATTITUDE_TARGET -u 14556
 mavlink stream -r 20 -s RC_CHANNELS -u 14556
 mavlink stream -r 250 -s HIGHRES_IMU -u 14556
 mavlink stream -r 10 -s OPTICAL_FLOW_RAD -u 14556
-mavlink boot_complete
 sdlog2 start -r 200 -e -t -a
+mavlink boot_complete

From da943b000738d2b09b6bffc56cbd84ed77e2d635 Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Sun, 5 Jun 2016 12:41:55 +0200
Subject: [PATCH 0474/1230] Update SITL config
 posix-configs/SITL/init/rcS_gazebo_standard_vtol

---
 posix-configs/SITL/init/rcS_gazebo_standard_vtol | 6 +++---
 1 file changed, 3 insertions(+), 3 deletions(-)

diff --git a/posix-configs/SITL/init/rcS_gazebo_standard_vtol b/posix-configs/SITL/init/rcS_gazebo_standard_vtol
index c9b0296a86..3487aedf0f 100644
--- a/posix-configs/SITL/init/rcS_gazebo_standard_vtol
+++ b/posix-configs/SITL/init/rcS_gazebo_standard_vtol
@@ -59,8 +59,8 @@ mc_att_control start
 fw_pos_control_l1 start
 fw_att_control start
 mixer load /dev/pwm_output0 ../../../../ROMFS/px4fmu_common/mixers/standard_vtol_sitl.main.mix
-mavlink start -u 14556 -r 2000000 -t 127.0.0.1
-mavlink start -u 14557 -r 2000000 -m onboard -o 14540 -t 127.0.0.1
+mavlink start -u 14556 -r 2000000
+mavlink start -u 14557 -r 2000000 -m onboard -o 14540
 mavlink stream -r 80 -s POSITION_TARGET_LOCAL_NED -u 14556
 mavlink stream -r 80 -s LOCAL_POSITION_NED -u 14556
 mavlink stream -r 80 -s GLOBAL_POSITION_INT -u 14556
@@ -70,5 +70,5 @@ mavlink stream -r 80 -s ATTITUDE_TARGET -u 14556
 mavlink stream -r 20 -s RC_CHANNELS -u 14556
 mavlink stream -r 250 -s HIGHRES_IMU -u 14556
 mavlink stream -r 10 -s OPTICAL_FLOW_RAD -u 14556
-mavlink boot_complete
 sdlog2 start -r 200 -e -t -a
+mavlink boot_complete

From e5ba214f34caa44fb5bd4e2042a5be2a29afd1e0 Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Sun, 5 Jun 2016 12:42:08 +0200
Subject: [PATCH 0475/1230] Update SITL config
 posix-configs/SITL/init/rcS_gazebo_tailsitter

---
 posix-configs/SITL/init/rcS_gazebo_tailsitter | 6 +++---
 1 file changed, 3 insertions(+), 3 deletions(-)

diff --git a/posix-configs/SITL/init/rcS_gazebo_tailsitter b/posix-configs/SITL/init/rcS_gazebo_tailsitter
index 57367ce3ff..b4954906e3 100644
--- a/posix-configs/SITL/init/rcS_gazebo_tailsitter
+++ b/posix-configs/SITL/init/rcS_gazebo_tailsitter
@@ -56,8 +56,8 @@ mc_att_control start
 fw_pos_control_l1 start
 fw_att_control start
 mixer load /dev/pwm_output0 ../../../../ROMFS/px4fmu_common/mixers/quad_x_vtol.main.mix
-mavlink start -u 14556 -r 2000000 -t 127.0.0.1
-mavlink start -u 14557 -r 2000000 -m onboard -o 14540 -t 127.0.0.1
+mavlink start -u 14556 -r 2000000
+mavlink start -u 14557 -r 2000000 -m onboard -o 14540
 mavlink stream -r 80 -s POSITION_TARGET_LOCAL_NED -u 14556
 mavlink stream -r 80 -s LOCAL_POSITION_NED -u 14556
 mavlink stream -r 80 -s GLOBAL_POSITION_INT -u 14556
@@ -67,5 +67,5 @@ mavlink stream -r 80 -s ATTITUDE_TARGET -u 14556
 mavlink stream -r 20 -s RC_CHANNELS -u 14556
 mavlink stream -r 250 -s HIGHRES_IMU -u 14556
 mavlink stream -r 10 -s OPTICAL_FLOW_RAD -u 14556
-mavlink boot_complete
 sdlog2 start -r 200 -e -t -a
+mavlink boot_complete

From 17e28ceeb118d00692dfb529d4ab264f8c6c07d8 Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Sun, 5 Jun 2016 12:42:22 +0200
Subject: [PATCH 0476/1230] Update SITL config
 posix-configs/SITL/init/rcS_jmavsim_iris

---
 posix-configs/SITL/init/rcS_jmavsim_iris | 6 +++---
 1 file changed, 3 insertions(+), 3 deletions(-)

diff --git a/posix-configs/SITL/init/rcS_jmavsim_iris b/posix-configs/SITL/init/rcS_jmavsim_iris
index 3f13970349..99501dd819 100644
--- a/posix-configs/SITL/init/rcS_jmavsim_iris
+++ b/posix-configs/SITL/init/rcS_jmavsim_iris
@@ -60,8 +60,8 @@ ekf2 start
 mc_pos_control start
 mc_att_control start
 mixer load /dev/pwm_output0 ../../../../ROMFS/px4fmu_common/mixers/quad_x.main.mix
-mavlink start -u 14556 -r 2000000 -t 127.0.0.1
-mavlink start -u 14557 -r 2000000 -m onboard -o 14540 -t 127.0.0.1
+mavlink start -u 14556 -r 2000000
+mavlink start -u 14557 -r 2000000 -m onboard -o 14540
 mavlink stream -r 80 -s POSITION_TARGET_LOCAL_NED -u 14556
 mavlink stream -r 80 -s LOCAL_POSITION_NED -u 14556
 mavlink stream -r 80 -s GLOBAL_POSITION_INT -u 14556
@@ -72,5 +72,5 @@ mavlink stream -r 20 -s RC_CHANNELS -u 14556
 mavlink stream -r 250 -s HIGHRES_IMU -u 14556
 mavlink stream -r 10 -s OPTICAL_FLOW_RAD -u 14556
 mavlink stream -r 20 -s MANUAL_CONTROL -u 14556
-mavlink boot_complete
 sdlog2 start -r 100 -e -t -a
+mavlink boot_complete

From 50f4a603e8803b496674c0dcc8c6205d640ce1db Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Sun, 5 Jun 2016 12:42:37 +0200
Subject: [PATCH 0477/1230] Update SITL config
 posix-configs/SITL/init/rcS_lpe_gazebo_iris

---
 posix-configs/SITL/init/rcS_lpe_gazebo_iris | 6 +++---
 1 file changed, 3 insertions(+), 3 deletions(-)

diff --git a/posix-configs/SITL/init/rcS_lpe_gazebo_iris b/posix-configs/SITL/init/rcS_lpe_gazebo_iris
index 1de001b619..9ce02acf76 100644
--- a/posix-configs/SITL/init/rcS_lpe_gazebo_iris
+++ b/posix-configs/SITL/init/rcS_lpe_gazebo_iris
@@ -50,8 +50,8 @@ attitude_estimator_q start
 mc_pos_control start
 mc_att_control start
 mixer load /dev/pwm_output0 ../../../../ROMFS/px4fmu_common/mixers/quad_w.main.mix
-mavlink start -u 14556 -r 2000000 -t 127.0.0.1
-mavlink start -u 14557 -r 2000000 -m onboard -o 14540 -t 127.0.0.1
+mavlink start -u 14556 -r 2000000
+mavlink start -u 14557 -r 2000000 -m onboard -o 14540
 mavlink stream -r 80 -s POSITION_TARGET_LOCAL_NED -u 14556
 mavlink stream -r 80 -s LOCAL_POSITION_NED_COV -u 14556
 mavlink stream -r 80 -s GLOBAL_POSITION_INT -u 14556
@@ -61,9 +61,9 @@ mavlink stream -r 80 -s ATTITUDE_TARGET -u 14556
 mavlink stream -r 20 -s RC_CHANNELS -u 14556
 mavlink stream -r 250 -s HIGHRES_IMU -u 14556
 mavlink stream -r 10 -s OPTICAL_FLOW_RAD -u 14556
-mavlink boot_complete
 sdlog2 start -r 100 -e -t -a
 
 # start LPE at end, when we know it is ok to init sensors
 sleep 5
 local_position_estimator start
+mavlink boot_complete

From 53b1bbd49f10e7642fd845676dcf66bf88f4c275 Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Sun, 5 Jun 2016 12:42:52 +0200
Subject: [PATCH 0478/1230] UPpdate SITL config
 posix-configs/SITL/init/rcS_lpe_gazebo_iris_opt_flow

---
 posix-configs/SITL/init/rcS_lpe_gazebo_iris_opt_flow | 6 +++---
 1 file changed, 3 insertions(+), 3 deletions(-)

diff --git a/posix-configs/SITL/init/rcS_lpe_gazebo_iris_opt_flow b/posix-configs/SITL/init/rcS_lpe_gazebo_iris_opt_flow
index 85a3ce7617..bd93519031 100644
--- a/posix-configs/SITL/init/rcS_lpe_gazebo_iris_opt_flow
+++ b/posix-configs/SITL/init/rcS_lpe_gazebo_iris_opt_flow
@@ -54,8 +54,8 @@ attitude_estimator_q start
 mc_pos_control start
 mc_att_control start
 mixer load /dev/pwm_output0 ../../../../ROMFS/px4fmu_common/mixers/quad_w.main.mix
-mavlink start -u 14556 -r 2000000 -t 127.0.0.1
-mavlink start -u 14557 -r 2000000 -m onboard -o 14540 -t 127.0.0.1
+mavlink start -u 14556 -r 2000000
+mavlink start -u 14557 -r 2000000 -m onboard -o 14540
 mavlink stream -r 80 -s POSITION_TARGET_LOCAL_NED -u 14556
 mavlink stream -r 80 -s LOCAL_POSITION_NED_COV -u 14556
 mavlink stream -r 80 -s GLOBAL_POSITION_INT -u 14556
@@ -65,9 +65,9 @@ mavlink stream -r 80 -s ATTITUDE_TARGET -u 14556
 mavlink stream -r 20 -s RC_CHANNELS -u 14556
 mavlink stream -r 250 -s HIGHRES_IMU -u 14556
 mavlink stream -r 10 -s OPTICAL_FLOW_RAD -u 14556
-mavlink boot_complete
 sdlog2 start -r 100 -e -t -a
 
 # start LPE at end, when we know it is ok to init sensors
 sleep 5
 local_position_estimator start
+mavlink boot_complete

From 6608c98192119f364b8d756077d5349e0cebf9e1 Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Sun, 5 Jun 2016 12:43:04 +0200
Subject: [PATCH 0479/1230] Update SITL config
 posix-configs/SITL/init/rcS_lpe_jmavsim_iris

---
 posix-configs/SITL/init/rcS_lpe_jmavsim_iris | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/posix-configs/SITL/init/rcS_lpe_jmavsim_iris b/posix-configs/SITL/init/rcS_lpe_jmavsim_iris
index a12d2e6f47..76685313ed 100644
--- a/posix-configs/SITL/init/rcS_lpe_jmavsim_iris
+++ b/posix-configs/SITL/init/rcS_lpe_jmavsim_iris
@@ -51,7 +51,7 @@ navigator start
 mc_pos_control start
 mc_att_control start
 mixer load /dev/pwm_output0 ../../../../ROMFS/px4fmu_common/mixers/quad_x.main.mix
-mavlink start -u 14556 -r 2000000 -t 127.0.0.1
+mavlink start -u 14556 -r 2000000
 mavlink stream -r 80 -s POSITION_TARGET_LOCAL_NED -u 14556
 mavlink stream -r 80 -s LOCAL_POSITION_NED_COV -u 14556
 mavlink stream -r 80 -s GLOBAL_POSITION_INT -u 14556
@@ -61,7 +61,7 @@ mavlink stream -r 80 -s ATTITUDE_TARGET -u 14556
 mavlink stream -r 20 -s RC_CHANNELS -u 14556
 mavlink stream -r 250 -s HIGHRES_IMU -u 14556
 mavlink stream -r 10 -s OPTICAL_FLOW_RAD -u 14556
-mavlink boot_complete
 sdlog2 start -r 100 -e -t -a
 attitude_estimator_q start
 local_position_estimator start
+mavlink boot_complete

From b0d05c19bc77dcd8baf6f9cc51618a63d3ece846 Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Sun, 5 Jun 2016 12:53:32 +0200
Subject: [PATCH 0480/1230] Update SITL config
 posix-configs/SITL/init/rcS_multiple_gazebo_iris

---
 posix-configs/SITL/init/rcS_multiple_gazebo_iris | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/posix-configs/SITL/init/rcS_multiple_gazebo_iris b/posix-configs/SITL/init/rcS_multiple_gazebo_iris
index 74a01bceee..674ff3f312 100644
--- a/posix-configs/SITL/init/rcS_multiple_gazebo_iris
+++ b/posix-configs/SITL/init/rcS_multiple_gazebo_iris
@@ -59,5 +59,5 @@ mavlink stream -r 80 -s ATTITUDE_TARGET -u _MAVPORT_
 mavlink stream -r 20 -s RC_CHANNELS -u _MAVPORT_
 mavlink stream -r 250 -s HIGHRES_IMU -u _MAVPORT_
 mavlink stream -r 10 -s OPTICAL_FLOW_RAD -u _MAVPORT_
-mavlink boot_complete
 sdlog2 start -r 100 -e -t -a
+mavlink boot_complete

From 6b97a9ece8f7adcd113042728386a3031361ea21 Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Sun, 5 Jun 2016 12:53:44 +0200
Subject: [PATCH 0481/1230] Update Bebop config

---
 posix-configs/bebop/mainapp.config | 1 +
 1 file changed, 1 insertion(+)

diff --git a/posix-configs/bebop/mainapp.config b/posix-configs/bebop/mainapp.config
index bd0cbbf4b7..bd4de20090 100644
--- a/posix-configs/bebop/mainapp.config
+++ b/posix-configs/bebop/mainapp.config
@@ -1,5 +1,6 @@
 uorb start
 param set SYS_AUTOSTART 4001
+param set MAV_BROADCAST 1
 sleep 1
 param set MAV_TYPE 2
 sensors start

From 131a1613afd7151f331d3cc1f28fcd01294439eb Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Sun, 5 Jun 2016 12:53:58 +0200
Subject: [PATCH 0482/1230] Update 200qx config

---
 posix-configs/eagle/200qx/mainapp-calib.config  | 1 +
 posix-configs/eagle/200qx/mainapp-flight.config | 1 +
 2 files changed, 2 insertions(+)

diff --git a/posix-configs/eagle/200qx/mainapp-calib.config b/posix-configs/eagle/200qx/mainapp-calib.config
index 7130d85ded..eba7890720 100644
--- a/posix-configs/eagle/200qx/mainapp-calib.config
+++ b/posix-configs/eagle/200qx/mainapp-calib.config
@@ -1,5 +1,6 @@
 uorb start
 muorb start
+param set MAV_BROADCAST 1
 mavlink start -u 14556
 sleep 1
 mavlink stream -u 14556 -s HIGHRES_IMU -r 50
diff --git a/posix-configs/eagle/200qx/mainapp-flight.config b/posix-configs/eagle/200qx/mainapp-flight.config
index fcad72bfe5..60fe035f7d 100644
--- a/posix-configs/eagle/200qx/mainapp-flight.config
+++ b/posix-configs/eagle/200qx/mainapp-flight.config
@@ -1,5 +1,6 @@
 uorb start
 muorb start
+param set MAV_BROADCAST 1
 mavlink start -u 14556
 sleep 1
 mavlink stream -u 14556 -s HIGHRES_IMU -r 50

From 8a7fd9c8893390cb362e08c529d5ccd0c06cef5f Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Sun, 5 Jun 2016 12:54:12 +0200
Subject: [PATCH 0483/1230] Update 210qc config

---
 posix-configs/eagle/210qc/mainapp.config | 1 +
 1 file changed, 1 insertion(+)

diff --git a/posix-configs/eagle/210qc/mainapp.config b/posix-configs/eagle/210qc/mainapp.config
index b511bb1121..c09015fa7e 100644
--- a/posix-configs/eagle/210qc/mainapp.config
+++ b/posix-configs/eagle/210qc/mainapp.config
@@ -1,5 +1,6 @@
 uorb start
 muorb start
+param set MAV_BROADCAST 1
 mavlink start -u 14556
 sleep 1
 mavlink stream -u 14556 -s HIGHRES_IMU -r 50

From 44c222f6b7faf9f082806d92e4e2d9ab229199d1 Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Sun, 5 Jun 2016 12:54:27 +0200
Subject: [PATCH 0484/1230] Update Eagle flight config

---
 posix-configs/eagle/flight/mainapp.config | 1 +
 1 file changed, 1 insertion(+)

diff --git a/posix-configs/eagle/flight/mainapp.config b/posix-configs/eagle/flight/mainapp.config
index bafdb746d2..685eba34f1 100644
--- a/posix-configs/eagle/flight/mainapp.config
+++ b/posix-configs/eagle/flight/mainapp.config
@@ -1,6 +1,7 @@
 uorb start
 muorb start
 sdlog2 start -r 100 -e -t -a -b 200
+param set MAV_BROADCAST 1
 dataman start
 navigator start
 mavlink start -u 14556 -r 1000000

From 93ef7ea4f6ecd7ff62e3cbc351b487a02e6492fb Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Sun, 5 Jun 2016 12:54:41 +0200
Subject: [PATCH 0485/1230] posix-configs/eagle/hil

---
 posix-configs/eagle/hil/mainapphil.config | 1 +
 1 file changed, 1 insertion(+)

diff --git a/posix-configs/eagle/hil/mainapphil.config b/posix-configs/eagle/hil/mainapphil.config
index 743307e42f..5636501d6e 100644
--- a/posix-configs/eagle/hil/mainapphil.config
+++ b/posix-configs/eagle/hil/mainapphil.config
@@ -1,5 +1,6 @@
 uorb start
 muorb start
+param set MAV_BROADCAST 1
 mavlink start -u 14556
 sleep 1
 mavlink stream -u 14556 -s HIGHRES_IMU -r 50

From c02214f7452e7b76bf58eb809068bbdeece78df3 Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Sun, 5 Jun 2016 12:55:14 +0200
Subject: [PATCH 0486/1230] Update RPI2 config

---
 posix-configs/rpi2/mainapp.config | 1 +
 1 file changed, 1 insertion(+)

diff --git a/posix-configs/rpi2/mainapp.config b/posix-configs/rpi2/mainapp.config
index 28fbb2a67c..3f68fe09e5 100644
--- a/posix-configs/rpi2/mainapp.config
+++ b/posix-configs/rpi2/mainapp.config
@@ -1,5 +1,6 @@
 uorb start
 param set SYS_AUTOSTART 4001
+param set MAV_BROADCAST 1
 sleep 1
 param set MAV_TYPE 2
 df_mpu9250_wrapper start

From 66db577c7d74ee12be71dca32586af46c7421942 Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Sun, 5 Jun 2016 14:37:47 +0200
Subject: [PATCH 0487/1230] Tone down mavlink message

---
 src/modules/mavlink/mavlink_main.cpp | 4 +---
 1 file changed, 1 insertion(+), 3 deletions(-)

diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp
index 2cdc32e69d..c9d0445403 100644
--- a/src/modules/mavlink/mavlink_main.cpp
+++ b/src/modules/mavlink/mavlink_main.cpp
@@ -2492,9 +2492,7 @@ Mavlink::set_boot_complete()
 		if ((inst->get_mode() != MAVLINK_MODE_ONBOARD) &&
 			(!inst->broadcast_enabled()) &&
 			((inst->get_protocol() == UDP) || (inst->get_protocol() == TCP))) {
-			printf("\n\n   **********   NETWORK BROADCAST NOT ENABLED   **********\n" \
-				"   The drone will only connect to localhost (excluding VMs).\n" \
-				"   Run 'pxh> param set MAV_BROADCAST 1' to enable.\n");
+			PX4_INFO("MAVLink only on localhost (set param MAV_BROADCAST = 1 to enable network)");
 		}
 	}
 #endif

From ed0aad61d22cab320789f75be5b1cd2ce7d32777 Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Sun, 5 Jun 2016 15:47:03 +0200
Subject: [PATCH 0488/1230] Updated SITL gazebo, fixes #4723

---
 Tools/sitl_gazebo | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/Tools/sitl_gazebo b/Tools/sitl_gazebo
index e929eb4af0..2b764c71b7 160000
--- a/Tools/sitl_gazebo
+++ b/Tools/sitl_gazebo
@@ -1 +1 @@
-Subproject commit e929eb4af063800bb772466b5561bb5be12cd81e
+Subproject commit 2b764c71b76b7f64f3bf1707d4e7aa4ce4e1a0a0

From f9df60919e4722edade53b066d25248af9d73fd6 Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Sun, 5 Jun 2016 15:51:54 +0200
Subject: [PATCH 0489/1230] Update submodule again

---
 Tools/sitl_gazebo | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/Tools/sitl_gazebo b/Tools/sitl_gazebo
index 2b764c71b7..7196226f3d 160000
--- a/Tools/sitl_gazebo
+++ b/Tools/sitl_gazebo
@@ -1 +1 @@
-Subproject commit 2b764c71b76b7f64f3bf1707d4e7aa4ce4e1a0a0
+Subproject commit 7196226f3dc3b95df7ffd076eb93ee441dc71748

From 5ec7de3a5a4d8fda7528c4f57f018d1423da3067 Mon Sep 17 00:00:00 2001
From: James Goppert <jgoppert@users.noreply.github.com>
Date: Mon, 6 Jun 2016 14:12:18 -0500
Subject: [PATCH 0490/1230] Make LPE est always log. (#4749)

This is a trivial change so I'm going to merge to help address edge cases in users logs.
---
 .../BlockLocalPositionEstimator.cpp           | 53 +++++++++----------
 1 file changed, 24 insertions(+), 29 deletions(-)

diff --git a/src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp b/src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp
index cf7f639f9d..505a039d22 100644
--- a/src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp
+++ b/src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp
@@ -583,37 +583,32 @@ void BlockLocalPositionEstimator::publishLocalPos()
 
 void BlockLocalPositionEstimator::publishEstimatorStatus()
 {
-	if (PX4_ISFINITE(_x(X_x)) &&
-	    PX4_ISFINITE(_x(X_y)) && PX4_ISFINITE(_x(X_z)) &&
-	    PX4_ISFINITE(_x(X_vx)) && PX4_ISFINITE(_x(X_vy))
-	    && PX4_ISFINITE(_x(X_vz))) {
-		_pub_est_status.get().timestamp = _timeStamp;
+	_pub_est_status.get().timestamp = _timeStamp;
 
-		for (int i = 0; i < n_x; i++) {
-			_pub_est_status.get().states[i] = _x(i);
-			_pub_est_status.get().covariances[i] = _P(i, i);
-		}
-
-		_pub_est_status.get().n_states = n_x;
-		_pub_est_status.get().nan_flags = 0;
-		_pub_est_status.get().health_flags =
-			((_baroFault > fault_lvl_disable) << SENSOR_BARO)
-			+ ((_gpsFault > fault_lvl_disable) << SENSOR_GPS)
-			+ ((_lidarFault > fault_lvl_disable) << SENSOR_LIDAR)
-			+ ((_flowFault > fault_lvl_disable) << SENSOR_FLOW)
-			+ ((_sonarFault > fault_lvl_disable) << SENSOR_SONAR)
-			+ ((_visionFault > fault_lvl_disable) << SENSOR_VISION)
-			+ ((_mocapFault > fault_lvl_disable) << SENSOR_MOCAP);
-		_pub_est_status.get().timeout_flags =
-			(_baroInitialized << SENSOR_BARO)
-			+ (_gpsInitialized << SENSOR_GPS)
-			+ (_flowInitialized << SENSOR_FLOW)
-			+ (_lidarInitialized << SENSOR_LIDAR)
-			+ (_sonarInitialized << SENSOR_SONAR)
-			+ (_visionInitialized << SENSOR_VISION)
-			+ (_mocapInitialized << SENSOR_MOCAP);
-		_pub_est_status.update();
+	for (int i = 0; i < n_x; i++) {
+		_pub_est_status.get().states[i] = _x(i);
+		_pub_est_status.get().covariances[i] = _P(i, i);
 	}
+
+	_pub_est_status.get().n_states = n_x;
+	_pub_est_status.get().nan_flags = 0;
+	_pub_est_status.get().health_flags =
+		((_baroFault > fault_lvl_disable) << SENSOR_BARO)
+		+ ((_gpsFault > fault_lvl_disable) << SENSOR_GPS)
+		+ ((_lidarFault > fault_lvl_disable) << SENSOR_LIDAR)
+		+ ((_flowFault > fault_lvl_disable) << SENSOR_FLOW)
+		+ ((_sonarFault > fault_lvl_disable) << SENSOR_SONAR)
+		+ ((_visionFault > fault_lvl_disable) << SENSOR_VISION)
+		+ ((_mocapFault > fault_lvl_disable) << SENSOR_MOCAP);
+	_pub_est_status.get().timeout_flags =
+		(_baroInitialized << SENSOR_BARO)
+		+ (_gpsInitialized << SENSOR_GPS)
+		+ (_flowInitialized << SENSOR_FLOW)
+		+ (_lidarInitialized << SENSOR_LIDAR)
+		+ (_sonarInitialized << SENSOR_SONAR)
+		+ (_visionInitialized << SENSOR_VISION)
+		+ (_mocapInitialized << SENSOR_MOCAP);
+	_pub_est_status.update();
 }
 
 void BlockLocalPositionEstimator::publishGlobalPos()

From 64e5e565c2c8c90830f09b0fbb6c7121b36e8642 Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Mon, 6 Jun 2016 23:11:54 +0200
Subject: [PATCH 0491/1230] Gazebo: Fix Tailsitter model

---
 Tools/sitl_gazebo | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/Tools/sitl_gazebo b/Tools/sitl_gazebo
index 7196226f3d..084592b12b 160000
--- a/Tools/sitl_gazebo
+++ b/Tools/sitl_gazebo
@@ -1 +1 @@
-Subproject commit 7196226f3dc3b95df7ffd076eb93ee441dc71748
+Subproject commit 084592b12b02d616e27d30abfefc8ca563283a4b

From b51ec04938412d01e8ae17ae3a086d3b4525eefb Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Beat=20K=C3=BCng?= <beat-kueng@gmx.net>
Date: Sat, 4 Jun 2016 10:15:03 +0200
Subject: [PATCH 0492/1230] logger: fix alignment issue in data message header

---
 src/modules/logger/logger.cpp | 12 ++++++++----
 1 file changed, 8 insertions(+), 4 deletions(-)

diff --git a/src/modules/logger/logger.cpp b/src/modules/logger/logger.cpp
index 4e486ab957..555245ba41 100644
--- a/src/modules/logger/logger.cpp
+++ b/src/modules/logger/logger.cpp
@@ -546,10 +546,14 @@ void Logger::run()
 				for (uint8_t instance = 0; instance < ORB_MULTI_MAX_INSTANCES; instance++) {
 					if (copy_if_updated_multi(sub, instance, buffer + sizeof(ulog_message_data_header_s))) {
 
-						ulog_message_data_header_s *header = reinterpret_cast<ulog_message_data_header_s *>(buffer);
-						header->msg_type = static_cast<uint8_t>(ULogMessageType::DATA);
-						header->msg_size = static_cast<uint16_t>(msg_size - ULOG_MSG_HEADER_LEN);
-						header->msg_id = sub.msg_ids[instance];
+						uint16_t write_msg_size = static_cast<uint16_t>(msg_size - ULOG_MSG_HEADER_LEN);
+						//write one byte after another (necessary because of alignment)
+						buffer[0] = (uint8_t)write_msg_size;
+						buffer[1] = (uint8_t)(write_msg_size >> 8);
+						buffer[2] = static_cast<uint8_t>(ULogMessageType::DATA);
+						uint16_t write_msg_id = sub.msg_ids[instance];
+						buffer[3] = (uint8_t)write_msg_id;
+						buffer[4] = (uint8_t)(write_msg_id >> 8);
 
 						//PX4_INFO("topic: %s, size = %zu, out_size = %zu", sub.metadata->o_name, sub.metadata->o_size, msg_size);
 

From 8ef493c82d24100ef19229a427a7ef327e871606 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Beat=20K=C3=BCng?= <beat-kueng@gmx.net>
Date: Sat, 4 Jun 2016 10:18:07 +0200
Subject: [PATCH 0493/1230] logger: use local time if
 orb_copy(vehicle_gps_position) fails for -t option

orb_copy can fail if there is no advertiser yet
---
 src/modules/logger/logger.cpp | 22 ++++++++++++----------
 1 file changed, 12 insertions(+), 10 deletions(-)

diff --git a/src/modules/logger/logger.cpp b/src/modules/logger/logger.cpp
index 555245ba41..3ae67f3940 100644
--- a/src/modules/logger/logger.cpp
+++ b/src/modules/logger/logger.cpp
@@ -470,14 +470,14 @@ void Logger::run()
 	add_topic("vehicle_status", 200);
 
 	if (!_writer.init()) {
-		PX4_ERR("logger: init of writer failed");
+		PX4_ERR("init of writer failed (alloc failed)");
 		return;
 	}
 
 	int ret = _writer.thread_start(writer_thread);
 
 	if (ret) {
-		PX4_ERR("logger: failed to create writer thread (%i)", ret);
+		PX4_ERR("failed to create writer thread (%i)", ret);
 		return;
 	}
 
@@ -806,17 +806,20 @@ bool Logger::get_log_time(struct tm *tt, bool boot_time)
 
 	/* Get the latest GPS publication */
 	vehicle_gps_position_s gps_pos;
+	time_t utc_time_sec;
+	bool use_clock_time = true;
 
-	if (orb_copy(ORB_ID(vehicle_gps_position), vehicle_gps_position_sub, &gps_pos) < 0) {
+	if (orb_copy(ORB_ID(vehicle_gps_position), vehicle_gps_position_sub, &gps_pos) == 0) {
+		utc_time_sec = gps_pos.time_utc_usec / 1e6;
 		orb_unsubscribe(vehicle_gps_position_sub);
-		return false;
+
+		if (gps_pos.fix_type >= 2 && utc_time_sec >= GPS_EPOCH_SECS) {
+			use_clock_time = false;
+		}
 	}
 
-	orb_unsubscribe(vehicle_gps_position_sub);
-	time_t utc_time_sec = gps_pos.time_utc_usec / 1e6;
-
-	if (gps_pos.fix_type < 2 || utc_time_sec < GPS_EPOCH_SECS) {
-		//take clock time if there's no fix (yet)
+	if (use_clock_time) {
+		/* take clock time if there's no fix (yet) */
 		struct timespec ts;
 		px4_clock_gettime(CLOCK_REALTIME, &ts);
 		utc_time_sec = ts.tv_sec + (ts.tv_nsec / 1e9);
@@ -1102,7 +1105,6 @@ void Logger::write_changed_parameters()
 
 		// log parameters which are valid AND used AND unsaved
 		if ((param != PARAM_INVALID) && param_value_unsaved(param)) {
-			warnx("logging change to parameter %s", param_name(param));
 
 			/* get parameter type and size */
 			const char *type_str;

From febe75bb12d8c4fc768b48847916bbf395af553b Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Beat=20K=C3=BCng?= <beat-kueng@gmx.net>
Date: Sat, 4 Jun 2016 15:11:18 +0200
Subject: [PATCH 0494/1230] logger: don't use uint8_t buffer[msg_size]; (it's
 C99 not C++)

Also, it's not clear where the allocation was. It looks like it was on
the heap, but the compiler could decide to put it on the stack. This is
very bad for us because we use fixed size stacks with tights bounds. So if
a user specifies a large topic to log, it could have crashed.

Now the allocation is on the heap and the user can specify any size of
topic to log (as long as there is enough memory).
---
 src/modules/logger/logger.cpp | 55 ++++++++++++++++++++++++-----------
 src/modules/logger/logger.h   |  2 ++
 2 files changed, 40 insertions(+), 17 deletions(-)

diff --git a/src/modules/logger/logger.cpp b/src/modules/logger/logger.cpp
index 3ae67f3940..38cd944cf9 100644
--- a/src/modules/logger/logger.cpp
+++ b/src/modules/logger/logger.cpp
@@ -293,8 +293,6 @@ void Logger::run_trampoline(int argc, char *argv[])
 }
 
 
-static constexpr size_t MAX_DATA_SIZE = 740;
-
 Logger::Logger(size_t buffer_size, unsigned log_interval, bool log_on_start,
 	       bool log_until_shutdown, bool log_name_timestamp) :
 	_log_on_start(log_on_start),
@@ -327,18 +325,15 @@ Logger::~Logger()
 			}
 		} while (logger_task != -1);
 	}
+
+	if (_msg_buffer) {
+		delete[](_msg_buffer);
+	}
 }
 
 int Logger::add_topic(const orb_metadata *topic)
 {
 	int fd = -1;
-
-	if (topic->o_size > MAX_DATA_SIZE) {
-		PX4_WARN("skip topic %s, data size is too large: %zu (max is %zu)", topic->o_name, topic->o_size, MAX_DATA_SIZE);
-		return -1;
-
-	}
-
 	size_t fields_len = strlen(topic->o_fields) + strlen(topic->o_name) + 1; //1 for ':'
 
 	if (fields_len > sizeof(ulog_message_format_s::format)) {
@@ -469,6 +464,33 @@ void Logger::run()
 //	add_topic("estimator_status");
 	add_topic("vehicle_status", 200);
 
+	//all topics added. Get required message buffer size
+	int max_msg_size = 0;
+
+	for (const auto &subscription : _subscriptions) {
+		//use o_size, because that's what orb_copy will use
+		if (subscription.metadata->o_size > max_msg_size) {
+			max_msg_size = subscription.metadata->o_size;
+		}
+	}
+
+	max_msg_size += sizeof(ulog_message_data_header_s);
+
+	if (max_msg_size > _msg_buffer_len) {
+		if (_msg_buffer) {
+			delete[](_msg_buffer);
+		}
+
+		_msg_buffer_len = max_msg_size;
+		_msg_buffer = new uint8_t[_msg_buffer_len];
+
+		if (!_msg_buffer) {
+			PX4_ERR("failed to alloc message buffer");
+			return;
+		}
+	}
+
+
 	if (!_writer.init()) {
 		PX4_ERR("init of writer failed (alloc failed)");
 		return;
@@ -538,26 +560,25 @@ void Logger::run()
 				/* each message consists of a header followed by an orb data object
 				 */
 				size_t msg_size = sizeof(ulog_message_data_header_s) + sub.metadata->o_size_no_padding;
-				uint8_t buffer[msg_size];
 
 				/* if this topic has been updated, copy the new data into the message buffer
 				 * and write a message to the log
 				 */
 				for (uint8_t instance = 0; instance < ORB_MULTI_MAX_INSTANCES; instance++) {
-					if (copy_if_updated_multi(sub, instance, buffer + sizeof(ulog_message_data_header_s))) {
+					if (copy_if_updated_multi(sub, instance, _msg_buffer + sizeof(ulog_message_data_header_s))) {
 
 						uint16_t write_msg_size = static_cast<uint16_t>(msg_size - ULOG_MSG_HEADER_LEN);
 						//write one byte after another (necessary because of alignment)
-						buffer[0] = (uint8_t)write_msg_size;
-						buffer[1] = (uint8_t)(write_msg_size >> 8);
-						buffer[2] = static_cast<uint8_t>(ULogMessageType::DATA);
+						_msg_buffer[0] = (uint8_t)write_msg_size;
+						_msg_buffer[1] = (uint8_t)(write_msg_size >> 8);
+						_msg_buffer[2] = static_cast<uint8_t>(ULogMessageType::DATA);
 						uint16_t write_msg_id = sub.msg_ids[instance];
-						buffer[3] = (uint8_t)write_msg_id;
-						buffer[4] = (uint8_t)(write_msg_id >> 8);
+						_msg_buffer[3] = (uint8_t)write_msg_id;
+						_msg_buffer[4] = (uint8_t)(write_msg_id >> 8);
 
 						//PX4_INFO("topic: %s, size = %zu, out_size = %zu", sub.metadata->o_name, sub.metadata->o_size, msg_size);
 
-						if (write(buffer, msg_size)) {
+						if (write(_msg_buffer, msg_size)) {
 
 #ifdef DBGPRINT
 							total_bytes += msg_size;
diff --git a/src/modules/logger/logger.h b/src/modules/logger/logger.h
index e01f9eb65e..4b7c4852a2 100644
--- a/src/modules/logger/logger.h
+++ b/src/modules/logger/logger.h
@@ -195,6 +195,8 @@ private:
 	static constexpr const char 	*LOG_ROOT = PX4_ROOTFSDIR"/fs/microsd/log";
 #endif
 
+	uint8_t						*_msg_buffer = nullptr;
+	int						_msg_buffer_len = 0;
 	bool						_task_should_exit = true;
 	char 						_log_dir[64];
 	bool						_has_log_dir = false;

From 78d357cb0cb3611b0b693abe52a6b94a96e03ae0 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Beat=20K=C3=BCng?= <beat-kueng@gmx.net>
Date: Sat, 4 Jun 2016 17:23:20 +0200
Subject: [PATCH 0495/1230] logger: free up ~200B stack size

we now use the already existing buffer for logging messages, which is
allocated on the heap.
In fact, stack usage was too high before this, now it's ok again.
---
 src/modules/logger/logger.cpp | 20 ++++++++++++++------
 src/modules/logger/messages.h |  2 +-
 2 files changed, 15 insertions(+), 7 deletions(-)

diff --git a/src/modules/logger/logger.cpp b/src/modules/logger/logger.cpp
index 38cd944cf9..f43ada64c9 100644
--- a/src/modules/logger/logger.cpp
+++ b/src/modules/logger/logger.cpp
@@ -476,6 +476,10 @@ void Logger::run()
 
 	max_msg_size += sizeof(ulog_message_data_header_s);
 
+	if (sizeof(ulog_message_logging_s) > max_msg_size) {
+		max_msg_size = sizeof(ulog_message_logging_s);
+	}
+
 	if (max_msg_size > _msg_buffer_len) {
 		if (_msg_buffer) {
 			delete[](_msg_buffer);
@@ -596,16 +600,20 @@ void Logger::run()
 			//check for new mavlink log message
 			if (mavlink_log_sub.check_updated()) {
 				mavlink_log_sub.update();
-				ulog_message_logging_s log_msg;
-				log_msg.log_level = mavlink_log_sub.get().severity + '0';
-				log_msg.timestamp = mavlink_log_sub.get().timestamp;
 				const char *message = (const char *)mavlink_log_sub.get().text;
 				int message_len = strlen(message);
 
 				if (message_len > 0) {
-					strncpy(log_msg.message, message, sizeof(log_msg.message));
-					log_msg.msg_size = sizeof(log_msg) - sizeof(log_msg.message) - ULOG_MSG_HEADER_LEN + message_len;
-					write(&log_msg, log_msg.msg_size + ULOG_MSG_HEADER_LEN);
+					uint16_t write_msg_size = sizeof(ulog_message_logging_s) - sizeof(ulog_message_logging_s::message)
+							- ULOG_MSG_HEADER_LEN + message_len;
+					_msg_buffer[0] = (uint8_t)write_msg_size;
+					_msg_buffer[1] = (uint8_t)(write_msg_size >> 8);
+					_msg_buffer[2] = static_cast<uint8_t>(ULogMessageType::LOGGING);
+					_msg_buffer[3] = mavlink_log_sub.get().severity + '0';
+					memcpy(_msg_buffer+4, &mavlink_log_sub.get().timestamp, sizeof(ulog_message_logging_s::timestamp));
+					strncpy((char*)(_msg_buffer+12), message, sizeof(ulog_message_logging_s::message));
+
+					write(_msg_buffer, write_msg_size + ULOG_MSG_HEADER_LEN);
 				}
 			}
 
diff --git a/src/modules/logger/messages.h b/src/modules/logger/messages.h
index 4ce963c1dc..a38692db0d 100644
--- a/src/modules/logger/messages.h
+++ b/src/modules/logger/messages.h
@@ -115,7 +115,7 @@ struct ulog_message_logging_s {
 
 	uint8_t log_level; //same levels as in the linux kernel
 	uint64_t timestamp;
-	char message[255];
+	char message[200];
 };
 
 struct ulog_message_parameter_header_s {

From 0fb0f17ccbe44ba480000cdaff99d5d3e42053ec Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Beat=20K=C3=BCng?= <beat-kueng@gmx.net>
Date: Sat, 4 Jun 2016 17:24:11 +0200
Subject: [PATCH 0496/1230] logger: reduce memory usage, by limiting the nr of
 added topics to 64 (was 128)

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

diff --git a/src/modules/logger/logger.h b/src/modules/logger/logger.h
index 4b7c4852a2..ea3f56de92 100644
--- a/src/modules/logger/logger.h
+++ b/src/modules/logger/logger.h
@@ -186,7 +186,7 @@ private:
 	 */
 	bool get_log_time(struct tm *tt, bool boot_time = false);
 
-	static constexpr size_t 	MAX_TOPICS_NUM = 128; /**< Maximum number of logged topics */
+	static constexpr size_t 	MAX_TOPICS_NUM = 64; /**< Maximum number of logged topics */
 	static constexpr unsigned	MAX_NO_LOGFOLDER = 999;	/**< Maximum number of log dirs */
 	static constexpr unsigned	MAX_NO_LOGFILE = 999;	/**< Maximum number of log files */
 #ifdef __PX4_POSIX_EAGLE

From e709048fb85278c449938a7c87af3b4c6cb7281e Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Beat=20K=C3=BCng?= <beat-kueng@gmx.net>
Date: Mon, 6 Jun 2016 15:27:29 +0200
Subject: [PATCH 0497/1230] orb: add orb_get_interval to API

---
 src/drivers/drv_orb_dev.h              |  3 +++
 src/modules/uORB/uORB.cpp              |  6 +++++-
 src/modules/uORB/uORB.h                |  5 +++++
 src/modules/uORB/uORBDevices_nuttx.cpp |  4 ++++
 src/modules/uORB/uORBDevices_posix.cpp |  4 ++++
 src/modules/uORB/uORBManager.cpp       |  8 ++++++++
 src/modules/uORB/uORBManager.hpp       | 12 ++++++++++++
 7 files changed, 41 insertions(+), 1 deletion(-)

diff --git a/src/drivers/drv_orb_dev.h b/src/drivers/drv_orb_dev.h
index 710eb3d175..2a0d5ca7d9 100644
--- a/src/drivers/drv_orb_dev.h
+++ b/src/drivers/drv_orb_dev.h
@@ -90,4 +90,7 @@
 /** Set the queue size of the topic */
 #define ORBIOCSETQUEUESIZE	_ORBIOC(15)
 
+/** Get the minimum interval at which the topic can be seen to be updated for this subscription */
+#define ORBIOCGETINTERVAL	_ORBIOC(16)
+
 #endif /* _DRV_UORB_H */
diff --git a/src/modules/uORB/uORB.cpp b/src/modules/uORB/uORB.cpp
index 4724502ebc..8a0bf0bdfc 100644
--- a/src/modules/uORB/uORB.cpp
+++ b/src/modules/uORB/uORB.cpp
@@ -140,8 +140,12 @@ int  orb_priority(int handle, int32_t *priority)
 	return uORB::Manager::get_instance()->orb_priority(handle, priority);
 }
 
-int  orb_set_interval(int handle, unsigned interval)
+int orb_set_interval(int handle, unsigned interval)
 {
 	return uORB::Manager::get_instance()->orb_set_interval(handle, interval);
 }
 
+int orb_get_interval(int handle, unsigned *interval)
+{
+	return uORB::Manager::get_instance()->orb_get_interval(handle, interval);
+}
diff --git a/src/modules/uORB/uORB.h b/src/modules/uORB/uORB.h
index c1b03c0def..f7f1a9dff4 100644
--- a/src/modules/uORB/uORB.h
+++ b/src/modules/uORB/uORB.h
@@ -232,6 +232,11 @@ extern int	orb_priority(int handle, int32_t *priority) __EXPORT;
  */
 extern int	orb_set_interval(int handle, unsigned interval) __EXPORT;
 
+/**
+ * @see uORB::Manager::orb_get_interval()
+ */
+extern int	orb_get_interval(int handle, unsigned *interval) __EXPORT;
+
 __END_DECLS
 
 /* Diverse uORB header defines */ //XXX: move to better location
diff --git a/src/modules/uORB/uORBDevices_nuttx.cpp b/src/modules/uORB/uORBDevices_nuttx.cpp
index e27f55d297..e0f51469a5 100644
--- a/src/modules/uORB/uORBDevices_nuttx.cpp
+++ b/src/modules/uORB/uORBDevices_nuttx.cpp
@@ -311,6 +311,10 @@ uORB::DeviceNode::ioctl(struct file *filp, int cmd, unsigned long arg)
 		//and only one advertiser is allowed to open the DeviceNode at the same time.
 		return update_queue_size(arg);
 
+	case ORBIOCGETINTERVAL:
+		*(unsigned *)arg = sd->update_interval;
+		return OK;
+
 	default:
 		/* give it to the superclass */
 		return CDev::ioctl(filp, cmd, arg);
diff --git a/src/modules/uORB/uORBDevices_posix.cpp b/src/modules/uORB/uORBDevices_posix.cpp
index c5cd4b8fbd..d828431c56 100644
--- a/src/modules/uORB/uORBDevices_posix.cpp
+++ b/src/modules/uORB/uORBDevices_posix.cpp
@@ -326,6 +326,10 @@ uORB::DeviceNode::ioctl(device::file_t *filp, int cmd, unsigned long arg)
 		//and only one advertiser is allowed to open the DeviceNode at the same time.
 		return update_queue_size(arg);
 
+	case ORBIOCGETINTERVAL:
+		*(unsigned *)arg = sd->update_interval;
+		return OK;
+
 	default:
 		/* give it to the superclass */
 		return VDev::ioctl(filp, cmd, arg);
diff --git a/src/modules/uORB/uORBManager.cpp b/src/modules/uORB/uORBManager.cpp
index e5e669527a..d4d958f8e3 100644
--- a/src/modules/uORB/uORBManager.cpp
+++ b/src/modules/uORB/uORBManager.cpp
@@ -204,6 +204,14 @@ int uORB::Manager::orb_set_interval(int handle, unsigned interval)
 	return px4_ioctl(handle, ORBIOCSETINTERVAL, interval * 1000);
 }
 
+int uORB::Manager::orb_get_interval(int handle, unsigned *interval)
+{
+	ASSERT(interval);
+	int ret = px4_ioctl(handle, ORBIOCGETINTERVAL, (unsigned long)interval);
+	*interval /= 1000;
+	return ret;
+}
+
 
 int uORB::Manager::node_advertise
 (
diff --git a/src/modules/uORB/uORBManager.hpp b/src/modules/uORB/uORBManager.hpp
index 9a6a56c48a..e43e57e887 100644
--- a/src/modules/uORB/uORBManager.hpp
+++ b/src/modules/uORB/uORBManager.hpp
@@ -327,6 +327,18 @@ public:
 	 */
 	int  orb_set_interval(int handle, unsigned interval) ;
 
+
+	/**
+	 * Get the minimum interval between which updates are seen for a subscription.
+	 *
+	 * @see orb_set_interval()
+	 *
+	 * @param handle  A handle returned from orb_subscribe.
+	 * @param interval  The returned interval period in milliseconds.
+	 * @return    OK on success, ERROR otherwise with ERRNO set accordingly.
+	 */
+	int	orb_get_interval(int handle, unsigned *interval);
+
 	/**
 	 * Method to set the uORBCommunicator::IChannel instance.
 	 * @param comm_channel

From 76d6ffd4453648272343796fad7527a07f68f41b Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Beat=20K=C3=BCng?= <beat-kueng@gmx.net>
Date: Mon, 6 Jun 2016 15:34:58 +0200
Subject: [PATCH 0498/1230] logger: use the defined interval for all
 multi-instances (& fix code style)

---
 src/modules/logger/logger.cpp | 13 ++++++++++---
 1 file changed, 10 insertions(+), 3 deletions(-)

diff --git a/src/modules/logger/logger.cpp b/src/modules/logger/logger.cpp
index f43ada64c9..5cc66d9c47 100644
--- a/src/modules/logger/logger.cpp
+++ b/src/modules/logger/logger.cpp
@@ -399,6 +399,13 @@ bool Logger::copy_if_updated_multi(LoggerSubscription &sub, int multi_instance,
 
 			/* copy first data */
 			if (handle >= 0) {
+				/* set to the same interval as the first instance */
+				unsigned int interval;
+
+				if (orb_get_interval(sub.fd[0], &interval) == 0 && interval > 0) {
+					orb_set_interval(handle, interval);
+				}
+
 				orb_copy(sub.metadata, handle, buffer);
 				updated = true;
 			}
@@ -605,13 +612,13 @@ void Logger::run()
 
 				if (message_len > 0) {
 					uint16_t write_msg_size = sizeof(ulog_message_logging_s) - sizeof(ulog_message_logging_s::message)
-							- ULOG_MSG_HEADER_LEN + message_len;
+								  - ULOG_MSG_HEADER_LEN + message_len;
 					_msg_buffer[0] = (uint8_t)write_msg_size;
 					_msg_buffer[1] = (uint8_t)(write_msg_size >> 8);
 					_msg_buffer[2] = static_cast<uint8_t>(ULogMessageType::LOGGING);
 					_msg_buffer[3] = mavlink_log_sub.get().severity + '0';
-					memcpy(_msg_buffer+4, &mavlink_log_sub.get().timestamp, sizeof(ulog_message_logging_s::timestamp));
-					strncpy((char*)(_msg_buffer+12), message, sizeof(ulog_message_logging_s::message));
+					memcpy(_msg_buffer + 4, &mavlink_log_sub.get().timestamp, sizeof(ulog_message_logging_s::timestamp));
+					strncpy((char *)(_msg_buffer + 12), message, sizeof(ulog_message_logging_s::message));
 
 					write(_msg_buffer, write_msg_size + ULOG_MSG_HEADER_LEN);
 				}

From 605f731ac428efcef5c5d8b7261badeb78f626e0 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Beat=20K=C3=BCng?= <beat-kueng@gmx.net>
Date: Mon, 6 Jun 2016 15:36:04 +0200
Subject: [PATCH 0499/1230] logger: reduce maximum logged string length to 128
 (use less memory)

---
 src/modules/logger/messages.h | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/src/modules/logger/messages.h b/src/modules/logger/messages.h
index a38692db0d..10e5d01838 100644
--- a/src/modules/logger/messages.h
+++ b/src/modules/logger/messages.h
@@ -115,7 +115,7 @@ struct ulog_message_logging_s {
 
 	uint8_t log_level; //same levels as in the linux kernel
 	uint64_t timestamp;
-	char message[200];
+	char message[128]; //defines the maximum length of a logged message string
 };
 
 struct ulog_message_parameter_header_s {

From 5125fc3a81726aa8c9010b877c00d5bdfd45d3f2 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Beat=20K=C3=BCng?= <beat-kueng@gmx.net>
Date: Mon, 6 Jun 2016 15:39:17 +0200
Subject: [PATCH 0500/1230] uORBDevices::SubscriberData: remove unused member
 poll_priv

---
 src/modules/uORB/uORBDevices_nuttx.hpp | 1 -
 src/modules/uORB/uORBDevices_posix.hpp | 1 -
 2 files changed, 2 deletions(-)

diff --git a/src/modules/uORB/uORBDevices_nuttx.hpp b/src/modules/uORB/uORBDevices_nuttx.hpp
index 2015bff986..e4eedd6003 100644
--- a/src/modules/uORB/uORBDevices_nuttx.hpp
+++ b/src/modules/uORB/uORBDevices_nuttx.hpp
@@ -187,7 +187,6 @@ private:
 		unsigned  generation; /**< last generation the subscriber has seen */
 		unsigned  update_interval; /**< if nonzero minimum interval between updates */
 		struct hrt_call update_call;  /**< deferred wakeup call if update_period is nonzero */
-		void    *poll_priv; /**< saved copy of fds->f_priv while poll is active */
 		bool    update_reported; /**< true if we have reported the update via poll/check */
 		int   priority; /**< priority of publisher */
 	};
diff --git a/src/modules/uORB/uORBDevices_posix.hpp b/src/modules/uORB/uORBDevices_posix.hpp
index 35882accaa..25f241ff58 100644
--- a/src/modules/uORB/uORBDevices_posix.hpp
+++ b/src/modules/uORB/uORBDevices_posix.hpp
@@ -125,7 +125,6 @@ private:
 		unsigned  update_interval; /**< if nonzero minimum interval between updates */
 		uint64_t last_update; /**< time at which the last update was provided, used when update_interval is nonzero */
 		struct hrt_call update_call;  /**< deferred wakeup call if update_period is nonzero */
-		void    *poll_priv; /**< saved copy of fds->f_priv while poll is active */
 		bool    update_reported; /**< true if we have reported the update via poll/check */
 		int   priority; /**< priority of publisher */
 	};

From f5310ca51eaf2794f07c4309672e4b3a51eb827f Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Beat=20K=C3=BCng?= <beat-kueng@gmx.net>
Date: Mon, 6 Jun 2016 15:49:06 +0200
Subject: [PATCH 0501/1230] orb: avoid unnecessary string duplication of
 objname

---
 src/modules/uORB/uORBDevices_nuttx.cpp | 10 +---------
 src/modules/uORB/uORBDevices_posix.cpp | 10 +---------
 2 files changed, 2 insertions(+), 18 deletions(-)

diff --git a/src/modules/uORB/uORBDevices_nuttx.cpp b/src/modules/uORB/uORBDevices_nuttx.cpp
index e0f51469a5..7ae8e3a1ce 100644
--- a/src/modules/uORB/uORBDevices_nuttx.cpp
+++ b/src/modules/uORB/uORBDevices_nuttx.cpp
@@ -673,18 +673,12 @@ uORB::DeviceMaster::ioctl(struct file *filp, int cmd, unsigned long arg)
 					*(adv->instance) = group_tries;
 				}
 
-				/* driver wants a permanent copy of the node name, so make one here */
-				objname = strdup(meta->o_name);
-
-				if (objname == nullptr) {
-					return -ENOMEM;
-				}
+				objname = meta->o_name; //no need for a copy, meta->o_name will never be freed or changed
 
 				/* driver wants a permanent copy of the path, so make one here */
 				devpath = strdup(nodepath);
 
 				if (devpath == nullptr) {
-					free((void *)objname);
 					return -ENOMEM;
 				}
 
@@ -693,7 +687,6 @@ uORB::DeviceMaster::ioctl(struct file *filp, int cmd, unsigned long arg)
 
 				/* if we didn't get a device, that's bad */
 				if (node == nullptr) {
-					free((void *)objname);
 					free((void *)devpath);
 					return -ENOMEM;
 				}
@@ -720,7 +713,6 @@ uORB::DeviceMaster::ioctl(struct file *filp, int cmd, unsigned long arg)
 					}
 
 					/* also discard the name now */
-					free((void *)objname);
 					free((void *)devpath);
 
 				} else {
diff --git a/src/modules/uORB/uORBDevices_posix.cpp b/src/modules/uORB/uORBDevices_posix.cpp
index d828431c56..ac936e45e9 100644
--- a/src/modules/uORB/uORBDevices_posix.cpp
+++ b/src/modules/uORB/uORBDevices_posix.cpp
@@ -679,18 +679,12 @@ uORB::DeviceMaster::ioctl(device::file_t *filp, int cmd, unsigned long arg)
 					*(adv->instance) = group_tries;
 				}
 
-				/* driver wants a permanent copy of the node name, so make one here */
-				objname = strdup(meta->o_name);
-
-				if (objname == nullptr) {
-					return -ENOMEM;
-				}
+				objname = meta->o_name; //no need for a copy, meta->o_name will never be freed or changed
 
 				/* driver wants a permanent copy of the path, so make one here */
 				devpath = strdup(nodepath);
 
 				if (devpath == nullptr) {
-					free((void *)objname);
 					return -ENOMEM;
 				}
 
@@ -699,7 +693,6 @@ uORB::DeviceMaster::ioctl(device::file_t *filp, int cmd, unsigned long arg)
 
 				/* if we didn't get a device, that's bad */
 				if (node == nullptr) {
-					free((void *)objname);
 					free((void *)devpath);
 					return -ENOMEM;
 				}
@@ -726,7 +719,6 @@ uORB::DeviceMaster::ioctl(device::file_t *filp, int cmd, unsigned long arg)
 					}
 
 					/* also discard the name now */
-					free((void *)objname);
 					free((void *)devpath);
 
 				} else {

From c64b0d45734c0eefffbe16c4a898c926de7826a0 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Beat=20K=C3=BCng?= <beat-kueng@gmx.net>
Date: Mon, 6 Jun 2016 16:46:07 +0200
Subject: [PATCH 0502/1230] logger: don't use system time for log file name if
 system time obviously wrong

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

diff --git a/src/modules/logger/logger.cpp b/src/modules/logger/logger.cpp
index 5cc66d9c47..3a6a5b8c56 100644
--- a/src/modules/logger/logger.cpp
+++ b/src/modules/logger/logger.cpp
@@ -859,6 +859,10 @@ bool Logger::get_log_time(struct tm *tt, bool boot_time)
 		struct timespec ts;
 		px4_clock_gettime(CLOCK_REALTIME, &ts);
 		utc_time_sec = ts.tv_sec + (ts.tv_nsec / 1e9);
+
+		if (utc_time_sec < GPS_EPOCH_SECS) {
+			return false;
+		}
 	}
 
 	/* strip the time elapsed since boot */

From 97e6a57501e58b3151300dcaa8d7215d9a47eb9c Mon Sep 17 00:00:00 2001
From: James Goppert <jgoppert@users.noreply.github.com>
Date: Mon, 6 Jun 2016 19:40:41 -0500
Subject: [PATCH 0503/1230] Add more GPS checks for LPE (#4750)

* Added more GPS checks for LPE.

* Add logic to force GPS alt init.
---
 .../BlockLocalPositionEstimator.cpp                 |  1 +
 .../BlockLocalPositionEstimator.hpp                 |  1 +
 src/modules/local_position_estimator/params.c       | 13 ++++++++++++-
 .../local_position_estimator/sensors/baro.cpp       |  3 ++-
 .../local_position_estimator/sensors/gps.cpp        |  9 ++++++++-
 5 files changed, 24 insertions(+), 3 deletions(-)

diff --git a/src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp b/src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp
index 505a039d22..6bbcb284f6 100644
--- a/src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp
+++ b/src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp
@@ -65,6 +65,7 @@ BlockLocalPositionEstimator::BlockLocalPositionEstimator() :
 	_gps_vxy_stddev(this, "GPS_VXY"),
 	_gps_vz_stddev(this, "GPS_VZ"),
 	_gps_eph_max(this, "EPH_MAX"),
+	_gps_epv_max(this, "EPV_MAX"),
 	_vision_xy_stddev(this, "VIS_XY"),
 	_vision_z_stddev(this, "VIS_Z"),
 	_vision_on(this, "VIS_ON"),
diff --git a/src/modules/local_position_estimator/BlockLocalPositionEstimator.hpp b/src/modules/local_position_estimator/BlockLocalPositionEstimator.hpp
index 4ccdb329f4..c32db40066 100644
--- a/src/modules/local_position_estimator/BlockLocalPositionEstimator.hpp
+++ b/src/modules/local_position_estimator/BlockLocalPositionEstimator.hpp
@@ -262,6 +262,7 @@ private:
 	BlockParamFloat  _gps_vxy_stddev;
 	BlockParamFloat  _gps_vz_stddev;
 	BlockParamFloat  _gps_eph_max;
+	BlockParamFloat  _gps_epv_max;
 
 	// vision parameters
 	BlockParamFloat  _vision_xy_stddev;
diff --git a/src/modules/local_position_estimator/params.c b/src/modules/local_position_estimator/params.c
index 590e726ccb..071aabc9fe 100644
--- a/src/modules/local_position_estimator/params.c
+++ b/src/modules/local_position_estimator/params.c
@@ -129,7 +129,7 @@ PARAM_DEFINE_FLOAT(LPE_ACC_Z, 0.0454f);
 PARAM_DEFINE_FLOAT(LPE_BAR_Z, 1.0f);
 
 /**
- * GPS
+ * Enables GPS data, also forces alt init with GPS
  *
  * @group Local Position Estimator
  * @boolean
@@ -203,6 +203,17 @@ PARAM_DEFINE_FLOAT(LPE_GPS_VZ, 0.25f);
  */
 PARAM_DEFINE_FLOAT(LPE_EPH_MAX, 3.0f);
 
+/**
+ * GPS max epv
+ *
+ * @group Local Position Estimator
+ * @unit m
+ * @min 1.0
+ * @max 5.0
+ * @decimal 3
+ */
+PARAM_DEFINE_FLOAT(LPE_EPV_MAX, 5.0f);
+
 /**
  * Vision xy standard deviation.
  *
diff --git a/src/modules/local_position_estimator/sensors/baro.cpp b/src/modules/local_position_estimator/sensors/baro.cpp
index f3de04856e..351a134696 100644
--- a/src/modules/local_position_estimator/sensors/baro.cpp
+++ b/src/modules/local_position_estimator/sensors/baro.cpp
@@ -29,7 +29,8 @@ void BlockLocalPositionEstimator::baroInit()
 		_baroInitialized = true;
 		_baroFault = FAULT_NONE;
 
-		if (!_altHomeInitialized) {
+		// only initialize alt home with baro if gps is disabled
+		if (!_altHomeInitialized && !_gps_on.get()) {
 			_altHomeInitialized = true;
 			_altHome = _baroAltHome;
 		}
diff --git a/src/modules/local_position_estimator/sensors/gps.cpp b/src/modules/local_position_estimator/sensors/gps.cpp
index 5bff337a55..c71512deb9 100644
--- a/src/modules/local_position_estimator/sensors/gps.cpp
+++ b/src/modules/local_position_estimator/sensors/gps.cpp
@@ -51,8 +51,15 @@ int BlockLocalPositionEstimator::gpsMeasure(Vector<double, n_y_gps> &y)
 	// check for good gps signal
 	uint8_t nSat = _sub_gps.get().satellites_used;
 	float eph = _sub_gps.get().eph;
+	float epv = _sub_gps.get().epv;
+	uint8_t fix_type = _sub_gps.get().fix_type;
 
-	if (nSat < 6 || eph > _gps_eph_max.get()) {
+	if (
+		nSat < 6 ||
+		eph > _gps_eph_max.get() ||
+		epv > _gps_epv_max.get() ||
+		fix_type < 3
+	) {
 		return -1;
 	}
 

From cefc00af39dccfe8ff3ec2155a271ec93ff57661 Mon Sep 17 00:00:00 2001
From: James Goppert <james.goppert@gmail.com>
Date: Mon, 6 Jun 2016 19:51:07 -0500
Subject: [PATCH 0504/1230] Changed baro init count for LPE.

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

diff --git a/src/modules/local_position_estimator/sensors/baro.cpp b/src/modules/local_position_estimator/sensors/baro.cpp
index 351a134696..fb69d68681 100644
--- a/src/modules/local_position_estimator/sensors/baro.cpp
+++ b/src/modules/local_position_estimator/sensors/baro.cpp
@@ -6,7 +6,7 @@ extern orb_advert_t mavlink_log_pub;
 
 // required number of samples for sensor
 // to initialize
-static const uint32_t 		REQ_BARO_INIT_COUNT = 100;
+static const uint32_t 		REQ_BARO_INIT_COUNT = 500; // require 5 seconds of data
 static const uint32_t 		BARO_TIMEOUT =   	100000;	// 0.1 s
 
 void BlockLocalPositionEstimator::baroInit()

From 9fedda43c6a501575773d83d715c36c46ac05f6a Mon Sep 17 00:00:00 2001
From: James Goppert <james.goppert@gmail.com>
Date: Mon, 6 Jun 2016 20:02:45 -0500
Subject: [PATCH 0505/1230] Revert "Changed baro init count for LPE."

This reverts commit cefc00af39dccfe8ff3ec2155a271ec93ff57661.
---
 src/modules/local_position_estimator/sensors/baro.cpp | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/src/modules/local_position_estimator/sensors/baro.cpp b/src/modules/local_position_estimator/sensors/baro.cpp
index fb69d68681..351a134696 100644
--- a/src/modules/local_position_estimator/sensors/baro.cpp
+++ b/src/modules/local_position_estimator/sensors/baro.cpp
@@ -6,7 +6,7 @@ extern orb_advert_t mavlink_log_pub;
 
 // required number of samples for sensor
 // to initialize
-static const uint32_t 		REQ_BARO_INIT_COUNT = 500; // require 5 seconds of data
+static const uint32_t 		REQ_BARO_INIT_COUNT = 100;
 static const uint32_t 		BARO_TIMEOUT =   	100000;	// 0.1 s
 
 void BlockLocalPositionEstimator::baroInit()

From dc3bd31e590852c3da4b5c16718786e4074da9ba Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Beat=20K=C3=BCng?= <beat-kueng@gmx.net>
Date: Mon, 6 Jun 2016 16:16:14 +0200
Subject: [PATCH 0506/1230] pwm_out_sim: fix orb handle == 0 is valid & use
 orb_unsubscribe instead of px4_close

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

diff --git a/src/drivers/pwm_out_sim/pwm_out_sim.cpp b/src/drivers/pwm_out_sim/pwm_out_sim.cpp
index 7be5a82e60..c1c3c51ae5 100644
--- a/src/drivers/pwm_out_sim/pwm_out_sim.cpp
+++ b/src/drivers/pwm_out_sim/pwm_out_sim.cpp
@@ -358,11 +358,11 @@ PWMSim::subscribe()
 
 		if (unsub_groups & (1 << i)) {
 			PX4_DEBUG("unsubscribe from actuator_controls_%d", i);
-			px4_close(_control_subs[i]);
+			orb_unsubscribe(_control_subs[i]);
 			_control_subs[i] = -1;
 		}
 
-		if (_control_subs[i] > 0) {
+		if (_control_subs[i] >= 0) {
 			_poll_fds[_poll_fds_num].fd = _control_subs[i];
 			_poll_fds[_poll_fds_num].events = POLLIN;
 			_poll_fds_num++;

From d0ffb1acb88b34511c1bdcbdf5bb8e99ba296715 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Beat=20K=C3=BCng?= <beat-kueng@gmx.net>
Date: Mon, 6 Jun 2016 16:18:19 +0200
Subject: [PATCH 0507/1230] fix resource leak in MavlinkReceiver: call
 orb_unsubscribe

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

diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp
index 2671999ab8..57b92b6f61 100644
--- a/src/modules/mavlink/mavlink_receiver.cpp
+++ b/src/modules/mavlink/mavlink_receiver.cpp
@@ -150,6 +150,7 @@ MavlinkReceiver::MavlinkReceiver(Mavlink *parent) :
 
 MavlinkReceiver::~MavlinkReceiver()
 {
+	orb_unsubscribe(_control_mode_sub);
 }
 
 void

From 0eb22823a6aaa4cf8eac56af0d9e2a6bfb9ee34c Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Beat=20K=C3=BCng?= <beat-kueng@gmx.net>
Date: Mon, 6 Jun 2016 16:19:00 +0200
Subject: [PATCH 0508/1230] fix resource leak in MavlinkParametersManager: call
 orb_unsubscribe

---
 src/modules/mavlink/mavlink_parameters.cpp | 9 +++++++++
 src/modules/mavlink/mavlink_parameters.h   | 1 +
 2 files changed, 10 insertions(+)

diff --git a/src/modules/mavlink/mavlink_parameters.cpp b/src/modules/mavlink/mavlink_parameters.cpp
index 99f887a5b9..d6b7ef090c 100644
--- a/src/modules/mavlink/mavlink_parameters.cpp
+++ b/src/modules/mavlink/mavlink_parameters.cpp
@@ -57,6 +57,15 @@ MavlinkParametersManager::MavlinkParametersManager(Mavlink *mavlink) : MavlinkSt
 	_uavcan_parameter_value_sub(-1)
 {
 }
+MavlinkParametersManager::~MavlinkParametersManager()
+{
+	if (_uavcan_parameter_value_sub >= 0) {
+		orb_unsubscribe(_uavcan_parameter_value_sub);
+	}
+	if (_uavcan_parameter_request_pub) {
+		orb_unadvertise(_uavcan_parameter_request_pub);
+	}
+}
 
 unsigned
 MavlinkParametersManager::get_size()
diff --git a/src/modules/mavlink/mavlink_parameters.h b/src/modules/mavlink/mavlink_parameters.h
index 1dcd073203..d0a14d1cd8 100644
--- a/src/modules/mavlink/mavlink_parameters.h
+++ b/src/modules/mavlink/mavlink_parameters.h
@@ -84,6 +84,7 @@ private:
 
 protected:
 	explicit MavlinkParametersManager(Mavlink *mavlink);
+	~MavlinkParametersManager();
 
 	void send(const hrt_abstime t);
 

From d8fbd8a6f6f8ce1458ed330fff664b4787f06df3 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Beat=20K=C3=BCng?= <beat-kueng@gmx.net>
Date: Mon, 6 Jun 2016 16:20:05 +0200
Subject: [PATCH 0509/1230] fix MavlinkOrbSubscription: use orb_unsubscribe
 instead of close()

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

diff --git a/src/modules/mavlink/mavlink_orb_subscription.cpp b/src/modules/mavlink/mavlink_orb_subscription.cpp
index 631461a454..66d309a156 100644
--- a/src/modules/mavlink/mavlink_orb_subscription.cpp
+++ b/src/modules/mavlink/mavlink_orb_subscription.cpp
@@ -58,7 +58,7 @@ MavlinkOrbSubscription::MavlinkOrbSubscription(const orb_id_t topic, int instanc
 
 MavlinkOrbSubscription::~MavlinkOrbSubscription()
 {
-	close(_fd);
+	orb_unsubscribe(_fd);
 }
 
 orb_id_t

From 1f55e238275a044cdf1860fee11df0c52d92bb58 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Beat=20K=C3=BCng?= <beat-kueng@gmx.net>
Date: Mon, 6 Jun 2016 16:48:16 +0200
Subject: [PATCH 0510/1230] fix resource leak in mavlink_main.cpp: close the
 socket

---
 src/modules/mavlink/mavlink_main.cpp | 5 +++++
 1 file changed, 5 insertions(+)

diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp
index c9d0445403..b837e43665 100644
--- a/src/modules/mavlink/mavlink_main.cpp
+++ b/src/modules/mavlink/mavlink_main.cpp
@@ -2249,6 +2249,11 @@ Mavlink::task_main(int argc, char *argv[])
 		::close(_uart_fd);
 	}
 
+	if (_socket_fd >= 0) {
+		close(_socket_fd);
+		_socket_fd = -1;
+	}
+
 	if (_forwarding_on || _ftp_on) {
 		message_buffer_destroy();
 		pthread_mutex_destroy(&_message_buffer_mutex);

From 1be3c0fe48c08bdc677de3e04a2db217fc14c2cd Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Beat=20K=C3=BCng?= <beat-kueng@gmx.net>
Date: Mon, 6 Jun 2016 16:57:24 +0200
Subject: [PATCH 0511/1230] mavlink_main.cpp: fix race conditions in 'mavlink
 stop-all'

This had multiple issues:
- linked list was modified while other instances were still running and
  accessing it (the used linked list is NOT thread-safe).
- Mavlink instance was deleted, but it was still in the linked list, and
  thus could still be dereferenced by other threads
- the instance was deleted, but it was still accessed by the 'stop-all'
  calling thread

What we do now is:
- wait for all threads to exit
- then remove the instances from the linked list and delete them
---
 src/modules/mavlink/mavlink_main.cpp | 23 +++++++++++------------
 1 file changed, 11 insertions(+), 12 deletions(-)

diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp
index b837e43665..08d85cbacb 100644
--- a/src/modules/mavlink/mavlink_main.cpp
+++ b/src/modules/mavlink/mavlink_main.cpp
@@ -323,10 +323,6 @@ Mavlink::~Mavlink()
 			}
 		} while (_task_running);
 	}
-
-	if (_mavlink_instances) {
-		LL_DELETE(_mavlink_instances, this);
-	}
 }
 
 void
@@ -370,13 +366,10 @@ Mavlink *
 Mavlink::get_instance(unsigned instance)
 {
 	Mavlink *inst;
-	unsigned inst_index = 0;
 	LL_FOREACH(::_mavlink_instances, inst) {
-		if (instance == inst_index) {
+		if (instance == inst->get_instance_id()) {
 			return inst;
 		}
-
-		inst_index++;
 	}
 
 	return nullptr;
@@ -439,6 +432,14 @@ Mavlink::destroy_all_instances()
 				return ERROR;
 			}
 		}
+
+	}
+
+	//we know all threads have exited, so it's safe to manipulate the linked list and delete objects.
+	while (_mavlink_instances) {
+		inst_to_del = _mavlink_instances;
+		LL_DELETE(_mavlink_instances, inst_to_del);
+		delete (inst_to_del);
 	}
 
 	printf("\n");
@@ -2259,8 +2260,7 @@ Mavlink::task_main(int argc, char *argv[])
 		pthread_mutex_destroy(&_message_buffer_mutex);
 	}
 
-	warnx("exiting");
-	_task_running = false;
+	warnx("exiting channel %i", (int)_channel);
 
 	return OK;
 }
@@ -2281,9 +2281,8 @@ int Mavlink::start_helper(int argc, char *argv[])
 	} else {
 		/* this will actually only return once MAVLink exits */
 		res = instance->task_main(argc, argv);
+		instance->_task_running = false;
 
-		/* delete instance on main thread end */
-		delete instance;
 	}
 
 	return res;

From d9267a4db5e434f131d148aba98a815eb50b702c Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Beat=20K=C3=BCng?= <beat-kueng@gmx.net>
Date: Tue, 7 Jun 2016 10:49:07 +0200
Subject: [PATCH 0512/1230] pwm_out_sim.cpp: fix initialization of
 _control_subs

Note: {-1} initializes only the first element, and sets the others to 0.
---
 src/drivers/pwm_out_sim/pwm_out_sim.cpp | 15 +++++++++------
 1 file changed, 9 insertions(+), 6 deletions(-)

diff --git a/src/drivers/pwm_out_sim/pwm_out_sim.cpp b/src/drivers/pwm_out_sim/pwm_out_sim.cpp
index c1c3c51ae5..320910fcfc 100644
--- a/src/drivers/pwm_out_sim/pwm_out_sim.cpp
+++ b/src/drivers/pwm_out_sim/pwm_out_sim.cpp
@@ -182,7 +182,6 @@ PWMSim::PWMSim() :
 	_mode(MODE_NONE),
 	_update_rate(50),
 	_current_update_rate(0),
-	_control_subs { -1},
 	_poll_fds{},
 	_poll_fds_num(0),
 	_armed_sub(-1),
@@ -201,6 +200,10 @@ PWMSim::PWMSim() :
 	_control_topics[1] = ORB_ID(actuator_controls_1);
 	_control_topics[2] = ORB_ID(actuator_controls_2);
 	_control_topics[3] = ORB_ID(actuator_controls_3);
+
+	for (unsigned i = 0; i < actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS; i++) {
+		_control_subs[i] = -1;
+	}
 }
 
 PWMSim::~PWMSim()
@@ -402,7 +405,7 @@ PWMSim::task_main()
 			}
 
 			for (unsigned i = 0; i < actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS; i++) {
-				if (_control_subs[i] > 0) {
+				if (_control_subs[i] >= 0) {
 					orb_set_interval(_control_subs[i], update_rate_in_ms);
 				}
 			}
@@ -441,7 +444,7 @@ PWMSim::task_main()
 		unsigned poll_id = 0;
 
 		for (unsigned i = 0; i < actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS; i++) {
-			if (_control_subs[i] > 0) {
+			if (_control_subs[i] >= 0) {
 				if (_poll_fds[poll_id].revents & POLLIN) {
 					orb_copy(_control_topics[i], _control_subs[i], &_controls[i]);
 				}
@@ -527,12 +530,12 @@ PWMSim::task_main()
 	}
 
 	for (unsigned i = 0; i < actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS; i++) {
-		if (_control_subs[i] > 0) {
-			px4_close(_control_subs[i]);
+		if (_control_subs[i] >= 0) {
+			orb_unsubscribe(_control_subs[i]);
 		}
 	}
 
-	px4_close(_armed_sub);
+	orb_unsubscribe(_armed_sub);
 
 	/* make sure servos are off */
 	// up_pwm_servo_deinit();

From 224b20b3e41c42ea5d72318bdfaf068f66194c02 Mon Sep 17 00:00:00 2001
From: Julian Oes <julian@oes.ch>
Date: Tue, 7 Jun 2016 11:19:44 +0200
Subject: [PATCH 0513/1230] cmake_hexagon: updated submodule (#4756)

This contains the latest fixes for the SDK 3.0.
---
 cmake/cmake_hexagon | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/cmake/cmake_hexagon b/cmake/cmake_hexagon
index c0ba14e598..eb1d242e57 160000
--- a/cmake/cmake_hexagon
+++ b/cmake/cmake_hexagon
@@ -1 +1 @@
-Subproject commit c0ba14e5983fd197cfff8baa70224d1d67908f0c
+Subproject commit eb1d242e57b34a28ba4b8ab4c04901bd85540d07

From 294c05d60796bf89ca9ecfc715fa7e2a7c062d49 Mon Sep 17 00:00:00 2001
From: Julian Oes <julian@oes.ch>
Date: Sun, 5 Jun 2016 22:04:35 +0200
Subject: [PATCH 0514/1230] DriverFramework: update to latest master

---
 src/lib/DriverFramework | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/src/lib/DriverFramework b/src/lib/DriverFramework
index 791c584969..851c7116e2 160000
--- a/src/lib/DriverFramework
+++ b/src/lib/DriverFramework
@@ -1 +1 @@
-Subproject commit 791c584969b2f84b2921c8c8c373962c677cefdb
+Subproject commit 851c7116e235867e2923bd22507359310569efa7

From ba7d35d9d06ca5dd1caebde9b929f5ede562be58 Mon Sep 17 00:00:00 2001
From: Julian Oes <julian@oes.ch>
Date: Sun, 5 Jun 2016 14:34:46 +0200
Subject: [PATCH 0515/1230] commander: fix convoluted auto disarm

---
 src/modules/commander/commander.cpp | 19 ++++++-------------
 1 file changed, 6 insertions(+), 13 deletions(-)

diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp
index c8cc22ffab..4ddea3450d 100644
--- a/src/modules/commander/commander.cpp
+++ b/src/modules/commander/commander.cpp
@@ -1950,15 +1950,12 @@ int commander_thread_main(int argc, char *argv[])
 			    &(status_flags.condition_local_altitude_valid), &status_changed);
 
 		/* Update land detector */
-		static bool check_for_disarming = false;
 		orb_check(land_detector_sub, &updated);
 		if (updated) {
 			orb_copy(ORB_ID(vehicle_land_detected), land_detector_sub, &land_detector);
-		}
 
-		if ((updated && status_flags.condition_local_altitude_valid) || check_for_disarming) {
 			if (was_landed != land_detector.landed) {
-				if (land_detector.landed && armed.armed) {
+				if (land_detector.landed) {
 					mavlink_and_console_log_info(&mavlink_log_pub, "LANDING DETECTED");
 				} else {
 					mavlink_and_console_log_info(&mavlink_log_pub, "TAKEOFF DETECTED");
@@ -1973,20 +1970,18 @@ int commander_thread_main(int argc, char *argv[])
 
 			if (disarm_when_landed > 0) {
 				if (land_detector.landed) {
-					if (!check_for_disarming && _inair_last_time > 0) {
-						_inair_last_time = land_detector.timestamp;
-						check_for_disarming = true;
-					}
-
-					if (_inair_last_time > 0 && ((hrt_absolute_time() - _inair_last_time) > (hrt_abstime)disarm_when_landed * 1000 * 1000)) {
+					if (_inair_last_time > 0 &&
+					    (hrt_elapsed_time(&_inair_last_time) > (hrt_abstime)disarm_when_landed * 1000 * 1000)) {
 						arm_disarm(false, &mavlink_log_pub, "auto disarm on land");
 						_inair_last_time = 0;
-						check_for_disarming = false;
 					}
 				} else {
 					_inair_last_time = land_detector.timestamp;
 				}
 			}
+
+			was_landed = land_detector.landed;
+			was_falling = land_detector.freefall;
 		}
 
 		if (!rtl_on) {
@@ -2678,8 +2673,6 @@ int commander_thread_main(int argc, char *argv[])
 			commander_set_home_position(home_pub, _home, local_position, global_position, attitude);
 		}
 
-		was_landed = land_detector.landed;
-		was_falling = land_detector.freefall;
 		was_armed = armed.armed;
 
 		/* print new state */

From b2719cf439497b66d91f5d540ac65b289091ef59 Mon Sep 17 00:00:00 2001
From: Julian Oes <julian@oes.ch>
Date: Sun, 5 Jun 2016 14:36:01 +0200
Subject: [PATCH 0516/1230] land_detector: publish when there is proper data

Instead of publishing before even having done update(), let's wait for a
result and then advertise.
---
 src/modules/land_detector/LandDetector.cpp | 20 +++++++++-----------
 1 file changed, 9 insertions(+), 11 deletions(-)

diff --git a/src/modules/land_detector/LandDetector.cpp b/src/modules/land_detector/LandDetector.cpp
index f55d5b83d7..484ebd6d19 100644
--- a/src/modules/land_detector/LandDetector.cpp
+++ b/src/modules/land_detector/LandDetector.cpp
@@ -49,7 +49,7 @@ namespace landdetection
 {
 
 LandDetector::LandDetector() :
-	_landDetectedPub(0),
+	_landDetectedPub(nullptr),
 	_landDetected{0, false, false},
 	_arming_time(0),
 	_taskShouldExit(false),
@@ -93,7 +93,6 @@ void LandDetector::cycle()
 		_landDetected.timestamp = hrt_absolute_time();
 		_landDetected.landed = false;
 		_landDetected.freefall = false;
-		_landDetectedPub = orb_advertise(ORB_ID(vehicle_land_detected), &_landDetected);
 
 		// initialize land detection algorithm
 		initialize();
@@ -104,18 +103,17 @@ void LandDetector::cycle()
 	}
 
 	LandDetectionResult current_state = update();
-	bool landDetected = (current_state == LANDDETECTION_RES_LANDED);
-	bool freefallDetected = (current_state == LANDDETECTION_RES_FREEFALL);
 
-	// publish if land detection state has changed
-	if ((_landDetected.landed != landDetected) || (_landDetected.freefall != freefallDetected)) {
-		_landDetected.timestamp = hrt_absolute_time();
-		_landDetected.landed = landDetected;
-		_landDetected.freefall = freefallDetected;
+	_landDetected.timestamp = hrt_absolute_time();
+	_landDetected.landed = (current_state == LANDDETECTION_RES_LANDED);
+	_landDetected.freefall = (current_state == LANDDETECTION_RES_FREEFALL);
 
-		// publish the land detected broadcast
+	// publish the land detected broadcast
+	if (_landDetectedPub == nullptr) {
+		_landDetectedPub = orb_advertise(ORB_ID(vehicle_land_detected), &_landDetected);
+
+	} else {
 		orb_publish(ORB_ID(vehicle_land_detected), (orb_advert_t)_landDetectedPub, &_landDetected);
-		//warnx("in air status changed: %s", (_landDetected.landed) ? "LANDED" : "TAKEOFF");
 	}
 
 	if (!_taskShouldExit) {

From f56e951f00eba7dd003e36332eb47195a307dd9f Mon Sep 17 00:00:00 2001
From: Julian Oes <julian@oes.ch>
Date: Sun, 5 Jun 2016 14:36:43 +0200
Subject: [PATCH 0517/1230] land_detector: don't estimate freefall on 0 data

---
 src/modules/land_detector/MulticopterLandDetector.cpp | 5 +++++
 1 file changed, 5 insertions(+)

diff --git a/src/modules/land_detector/MulticopterLandDetector.cpp b/src/modules/land_detector/MulticopterLandDetector.cpp
index a87c3a6658..43b3a935b2 100644
--- a/src/modules/land_detector/MulticopterLandDetector.cpp
+++ b/src/modules/land_detector/MulticopterLandDetector.cpp
@@ -135,6 +135,11 @@ bool MulticopterLandDetector::get_freefall_state()
 
 	const uint64_t now = hrt_absolute_time();
 
+	if (_ctrl_state.timestamp == 0) {
+		// _ctrl_state is not valid yet, we have to assume we're not falling.
+		return false;
+	}
+
 	float acc_norm = _ctrl_state.x_acc * _ctrl_state.x_acc
 			 + _ctrl_state.y_acc * _ctrl_state.y_acc
 			 + _ctrl_state.z_acc * _ctrl_state.z_acc;

From 7c5769675250304804174182818bf5d1b7bcb5d2 Mon Sep 17 00:00:00 2001
From: Julian Oes <julian@oes.ch>
Date: Sat, 4 Jun 2016 21:59:14 +0200
Subject: [PATCH 0518/1230] df_bmp280_wrapper: don't advertise garbage

---
 .../df_bmp280_wrapper/df_bmp280_wrapper.cpp       | 15 ++++-----------
 1 file changed, 4 insertions(+), 11 deletions(-)

diff --git a/src/platforms/posix/drivers/df_bmp280_wrapper/df_bmp280_wrapper.cpp b/src/platforms/posix/drivers/df_bmp280_wrapper/df_bmp280_wrapper.cpp
index 0d580b19f9..124de2a0e3 100644
--- a/src/platforms/posix/drivers/df_bmp280_wrapper/df_bmp280_wrapper.cpp
+++ b/src/platforms/posix/drivers/df_bmp280_wrapper/df_bmp280_wrapper.cpp
@@ -115,16 +115,6 @@ DfBmp280Wrapper::~DfBmp280Wrapper()
 
 int DfBmp280Wrapper::start()
 {
-	// TODO: don't publish garbage here
-	baro_report baro_report = {};
-	_baro_topic = orb_advertise_multi(ORB_ID(sensor_baro), &baro_report,
-					  &_baro_orb_class_instance, ORB_PRIO_DEFAULT);
-
-	if (_baro_topic == nullptr) {
-		PX4_ERR("sensor_baro advert fail");
-		return -1;
-	}
-
 	/* Init device and start sensor. */
 	int ret = init();
 
@@ -197,7 +187,10 @@ int DfBmp280Wrapper::_publish(struct baro_sensor_data &data)
 	// TODO: when is this ever blocked?
 	if (!(m_pub_blocked)) {
 
-		if (_baro_topic != nullptr) {
+		if (_baro_topic == nullptr) {
+			_baro_topic = orb_advertise_multi(ORB_ID(sensor_baro), &baro_report,
+							  &_baro_orb_class_instance, ORB_PRIO_DEFAULT);
+		} else {
 			orb_publish(ORB_ID(sensor_baro), _baro_topic, &baro_report);
 		}
 	}

From 40d6a4e5fc88661afa6ad57f40736f93d0cf9a77 Mon Sep 17 00:00:00 2001
From: Julian Oes <julian@oes.ch>
Date: Sat, 4 Jun 2016 21:59:43 +0200
Subject: [PATCH 0519/1230] df_isl29501_wrapper: don't advertise garbage

---
 .../df_isl29501_wrapper/df_isl29501_wrapper.cpp  | 16 ++++++----------
 1 file changed, 6 insertions(+), 10 deletions(-)

diff --git a/src/platforms/posix/drivers/df_isl29501_wrapper/df_isl29501_wrapper.cpp b/src/platforms/posix/drivers/df_isl29501_wrapper/df_isl29501_wrapper.cpp
index 43bb25205e..43198be6d8 100644
--- a/src/platforms/posix/drivers/df_isl29501_wrapper/df_isl29501_wrapper.cpp
+++ b/src/platforms/posix/drivers/df_isl29501_wrapper/df_isl29501_wrapper.cpp
@@ -118,11 +118,6 @@ DfISL29501Wrapper::~DfISL29501Wrapper()
 
 int DfISL29501Wrapper::start()
 {
-
-	struct distance_sensor_s d;
-	_range_topic = orb_advertise_multi(ORB_ID(distance_sensor), &d,
-					   &_orb_class_instance, ORB_PRIO_DEFAULT);
-
 	int ret;
 	ret = ISL29501::init();
 
@@ -156,10 +151,6 @@ int DfISL29501Wrapper::stop()
 
 int DfISL29501Wrapper::_publish(struct range_sensor_data &data)
 {
-	if (!_range_topic) {
-		return 1;
-	}
-
 	struct distance_sensor_s d;
 
 	memset(&d, 0, sizeof(d));
@@ -178,7 +169,12 @@ int DfISL29501Wrapper::_publish(struct range_sensor_data &data)
 
 	d.covariance = 0.0f;
 
-	orb_publish(ORB_ID(distance_sensor), _range_topic, &d);
+	if (_range_topic == nullptr) {
+		_range_topic = orb_advertise_multi(ORB_ID(distance_sensor), &d,
+						   &_orb_class_instance, ORB_PRIO_DEFAULT);
+	} else {
+		orb_publish(ORB_ID(distance_sensor), _range_topic, &d);
+	}
 
 	/* Notify anyone waiting for data. */
 	DevMgr::updateNotify(*this);

From 44207a3a5c6311dfca7120270e01bdcfd8a80d3e Mon Sep 17 00:00:00 2001
From: Julian Oes <julian@oes.ch>
Date: Sat, 4 Jun 2016 22:00:01 +0200
Subject: [PATCH 0520/1230] df_trone_wrapper: don't advertise garbage

---
 .../drivers/df_trone_wrapper/df_trone_wrapper.cpp | 15 ++++++---------
 1 file changed, 6 insertions(+), 9 deletions(-)

diff --git a/src/platforms/posix/drivers/df_trone_wrapper/df_trone_wrapper.cpp b/src/platforms/posix/drivers/df_trone_wrapper/df_trone_wrapper.cpp
index 4bc4fdf531..1abade2e8f 100644
--- a/src/platforms/posix/drivers/df_trone_wrapper/df_trone_wrapper.cpp
+++ b/src/platforms/posix/drivers/df_trone_wrapper/df_trone_wrapper.cpp
@@ -117,10 +117,6 @@ DfTROneWrapper::~DfTROneWrapper()
 
 int DfTROneWrapper::start()
 {
-	struct distance_sensor_s d;
-	_range_topic = orb_advertise_multi(ORB_ID(distance_sensor), &d,
-					   &_orb_class_instance, ORB_PRIO_DEFAULT);
-
 	int ret;
 
 	/* Init device and start sensor. */
@@ -156,10 +152,6 @@ int DfTROneWrapper::stop()
 
 int DfTROneWrapper::_publish(struct range_sensor_data &data)
 {
-	if (!_range_topic) {
-		return 1;
-	}
-
 	struct distance_sensor_s d;
 
 	memset(&d, 0, sizeof(d));
@@ -180,7 +172,12 @@ int DfTROneWrapper::_publish(struct range_sensor_data &data)
 
 	d.covariance = 0.0f;
 
-	orb_publish(ORB_ID(distance_sensor), _range_topic, &d);
+	if (_range_topic == nullptr) {
+		_range_topic = orb_advertise_multi(ORB_ID(distance_sensor), &d,
+						   &_orb_class_instance, ORB_PRIO_DEFAULT);
+	} else {
+		orb_publish(ORB_ID(distance_sensor), _range_topic, &d);
+	}
 
 	/* Notify anyone waiting for data. */
 	DevMgr::updateNotify(*this);

From 354b31e1b1938de4dd8e743cee1b7543718df448 Mon Sep 17 00:00:00 2001
From: Julian Oes <julian@oes.ch>
Date: Sat, 4 Jun 2016 22:14:04 +0200
Subject: [PATCH 0521/1230] DF driver wrappers: astyle

---
 .../posix/drivers/df_bmp280_wrapper/df_bmp280_wrapper.cpp        | 1 +
 .../posix/drivers/df_isl29501_wrapper/df_isl29501_wrapper.cpp    | 1 +
 .../posix/drivers/df_trone_wrapper/df_trone_wrapper.cpp          | 1 +
 3 files changed, 3 insertions(+)

diff --git a/src/platforms/posix/drivers/df_bmp280_wrapper/df_bmp280_wrapper.cpp b/src/platforms/posix/drivers/df_bmp280_wrapper/df_bmp280_wrapper.cpp
index 124de2a0e3..b773a225a2 100644
--- a/src/platforms/posix/drivers/df_bmp280_wrapper/df_bmp280_wrapper.cpp
+++ b/src/platforms/posix/drivers/df_bmp280_wrapper/df_bmp280_wrapper.cpp
@@ -190,6 +190,7 @@ int DfBmp280Wrapper::_publish(struct baro_sensor_data &data)
 		if (_baro_topic == nullptr) {
 			_baro_topic = orb_advertise_multi(ORB_ID(sensor_baro), &baro_report,
 							  &_baro_orb_class_instance, ORB_PRIO_DEFAULT);
+
 		} else {
 			orb_publish(ORB_ID(sensor_baro), _baro_topic, &baro_report);
 		}
diff --git a/src/platforms/posix/drivers/df_isl29501_wrapper/df_isl29501_wrapper.cpp b/src/platforms/posix/drivers/df_isl29501_wrapper/df_isl29501_wrapper.cpp
index 43198be6d8..3aa4fb88e2 100644
--- a/src/platforms/posix/drivers/df_isl29501_wrapper/df_isl29501_wrapper.cpp
+++ b/src/platforms/posix/drivers/df_isl29501_wrapper/df_isl29501_wrapper.cpp
@@ -172,6 +172,7 @@ int DfISL29501Wrapper::_publish(struct range_sensor_data &data)
 	if (_range_topic == nullptr) {
 		_range_topic = orb_advertise_multi(ORB_ID(distance_sensor), &d,
 						   &_orb_class_instance, ORB_PRIO_DEFAULT);
+
 	} else {
 		orb_publish(ORB_ID(distance_sensor), _range_topic, &d);
 	}
diff --git a/src/platforms/posix/drivers/df_trone_wrapper/df_trone_wrapper.cpp b/src/platforms/posix/drivers/df_trone_wrapper/df_trone_wrapper.cpp
index 1abade2e8f..af90241849 100644
--- a/src/platforms/posix/drivers/df_trone_wrapper/df_trone_wrapper.cpp
+++ b/src/platforms/posix/drivers/df_trone_wrapper/df_trone_wrapper.cpp
@@ -175,6 +175,7 @@ int DfTROneWrapper::_publish(struct range_sensor_data &data)
 	if (_range_topic == nullptr) {
 		_range_topic = orb_advertise_multi(ORB_ID(distance_sensor), &d,
 						   &_orb_class_instance, ORB_PRIO_DEFAULT);
+
 	} else {
 		orb_publish(ORB_ID(distance_sensor), _range_topic, &d);
 	}

From ac9a81c0c72270d6c346b71a1de8c9fc80b3e2da Mon Sep 17 00:00:00 2001
From: Roman <bapstr@ethz.ch>
Date: Sat, 28 May 2016 19:52:20 +0200
Subject: [PATCH 0522/1230] standard vtol pusher fix: - pusher code now works
 with the new manual setpoint generation

---
 src/modules/vtol_att_control/standard.cpp | 70 +++++++++++++++--------
 1 file changed, 47 insertions(+), 23 deletions(-)

diff --git a/src/modules/vtol_att_control/standard.cpp b/src/modules/vtol_att_control/standard.cpp
index a4a09ed414..d3e3d57e7d 100644
--- a/src/modules/vtol_att_control/standard.cpp
+++ b/src/modules/vtol_att_control/standard.cpp
@@ -298,32 +298,56 @@ void Standard::update_mc_state()
 		return;
 	}
 
-	// get projection of thrust vector on body x axis. This is used to
-	// determine the desired forward acceleration which we want to achieve with the pusher
-	math::Matrix<3, 3> R(&_v_att->R[0]);
-	math::Matrix<3, 3> R_sp(&_v_att_sp->R_body[0]);
-	math::Vector<3> thrust_sp_axis(-R_sp(0, 2), -R_sp(1, 2), -R_sp(2, 2));
-	math::Vector<3> euler = R.to_euler();
-	R.from_euler(0, 0, euler(2));
-	math::Vector<3> body_x_zero_tilt(R(0, 0), R(1, 0), R(2, 0));
+	matrix::Dcmf R(matrix::Quatf(_v_att->q));
+	matrix::Dcmf R_sp(&_v_att_sp->R_body[0]);
+	matrix::Eulerf euler(R);
+	matrix::Eulerf euler_sp(R_sp);
+	_pusher_throttle = 0.0f;
+
+	// direction of desired body z axis represented in earth frame
+	matrix::Vector3f body_z_sp(R_sp(0, 2), R_sp(1, 2), R_sp(2, 2));
+
+	// rotate desired body z axis into new frame which is rotated in z by the current
+	// heading of the vehicle. we refer to this as the heading frame.
+	matrix::Dcmf R_yaw = matrix::Eulerf(0.0f, 0.0f, -euler(2));
+	body_z_sp = R_yaw * body_z_sp;
+	body_z_sp.normalize();
+	
+	// calculate the desired pitch seen in the heading frame
+	// this value corresponds to the amount the vehicle would try to pitch forward
+	float pitch_forward = asinf(body_z_sp(0));
+	// only allow pitching forward up to threshold, the rest of the desired
+	// forward acceleration will be compensated by the pusher
+	if (pitch_forward < -_params_standard.down_pitch_max) {
+		// desired roll angle in heading frame stays the same
+		float roll_new = -atan2f(body_z_sp(1), body_z_sp(2));
+
+		_pusher_throttle = (sinf(-pitch_forward) - sinf(_params_standard.down_pitch_max)) * _v_att_sp->thrust;
+
+		// limit desired pitch
+		float pitch_new = -_params_standard.down_pitch_max;
+
+		// create corrected desired body z axis in heading frame
+		matrix::Dcmf R_tmp = matrix::Eulerf(roll_new, pitch_new, 0.0f);
+		matrix::Vector3f tilt_new(R_tmp(0, 2), R_tmp(1, 2), R_tmp(2, 2));
+
+		// rotate the vector into a new frame which is rotated in z by the desired heading
+		// with respect to the earh frame.
+		float yaw_error = _wrap_pi(euler_sp(2) - euler(2));
+		matrix::Dcmf R_yaw_correction = matrix::Eulerf(0.0f, 0.0f, -yaw_error);
+		tilt_new = R_yaw_correction * tilt_new;
+
+		// now extract roll and pitch setpoints
+		float pitch = asinf(tilt_new(0));
+		float roll = -atan2f(tilt_new(1), tilt_new(2));
+		R_sp = matrix::Eulerf(roll, pitch, euler_sp(2));
+		matrix::Quatf q_sp(R_sp);
+		memcpy(&_v_att_sp->R_body[0], &R_sp._data[0], sizeof(_v_att_sp->R_body));
+		memcpy(&_v_att_sp->q_d[0], &q_sp._data[0], sizeof(_v_att_sp->q_d));
+	}
 
-	// we are using a parameter to scale the thrust value in order to compensate for highly over/under-powered motors
-	_pusher_throttle = body_x_zero_tilt * thrust_sp_axis * _v_att_sp->thrust * _params_standard.forward_thurst_scale;
 	_pusher_throttle = _pusher_throttle < 0.0f ? 0.0f : _pusher_throttle;
 
-	float pitch_sp_corrected = _v_att_sp->pitch_body < -_params_standard.down_pitch_max ? -_params_standard.down_pitch_max :
-				   _v_att_sp->pitch_body;
-
-	// compute new desired rotation matrix with corrected pitch angle
-	// and copy data to attitude setpoint topic
-	euler = R_sp.to_euler();
-	euler(1) = pitch_sp_corrected;
-	R_sp.from_euler(euler(0), euler(1), euler(2));
-	memcpy(&_v_att_sp->R_body[0], R_sp.data, sizeof(_v_att_sp->R_body));
-	_v_att_sp->pitch_body = pitch_sp_corrected;
-	math::Quaternion q_sp;
-	q_sp.from_dcm(R_sp);
-	memcpy(&_v_att_sp->q_d[0], &q_sp.data[0], sizeof(_v_att_sp->q_d));
 }
 
 void Standard::update_fw_state()

From 9ec9210a082ed5ee5faf8c3afbb793af8571e52d Mon Sep 17 00:00:00 2001
From: Andreas Antener <antener_a@gmx.ch>
Date: Thu, 2 Jun 2016 13:22:00 +0200
Subject: [PATCH 0523/1230] fixed code style in standard.cpp

---
 src/modules/vtol_att_control/standard.cpp | 3 ++-
 1 file changed, 2 insertions(+), 1 deletion(-)

diff --git a/src/modules/vtol_att_control/standard.cpp b/src/modules/vtol_att_control/standard.cpp
index d3e3d57e7d..9109a82f90 100644
--- a/src/modules/vtol_att_control/standard.cpp
+++ b/src/modules/vtol_att_control/standard.cpp
@@ -312,10 +312,11 @@ void Standard::update_mc_state()
 	matrix::Dcmf R_yaw = matrix::Eulerf(0.0f, 0.0f, -euler(2));
 	body_z_sp = R_yaw * body_z_sp;
 	body_z_sp.normalize();
-	
+
 	// calculate the desired pitch seen in the heading frame
 	// this value corresponds to the amount the vehicle would try to pitch forward
 	float pitch_forward = asinf(body_z_sp(0));
+
 	// only allow pitching forward up to threshold, the rest of the desired
 	// forward acceleration will be compensated by the pusher
 	if (pitch_forward < -_params_standard.down_pitch_max) {

From fb7f0747ca8f9a2fbc7a036d779cbdf883df1da0 Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Tue, 7 Jun 2016 15:20:00 +0200
Subject: [PATCH 0524/1230] Update sitl_gazebo with Solo model

---
 Tools/sitl_gazebo | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/Tools/sitl_gazebo b/Tools/sitl_gazebo
index 084592b12b..eea655d25d 160000
--- a/Tools/sitl_gazebo
+++ b/Tools/sitl_gazebo
@@ -1 +1 @@
-Subproject commit 084592b12b02d616e27d30abfefc8ca563283a4b
+Subproject commit eea655d25d405aae29e2e8a1e914b3579076193e

From 3f027023cf827d5a3103f03e857627e1c77ffc0d Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Tue, 7 Jun 2016 19:30:07 +0200
Subject: [PATCH 0525/1230] Update Solo model

---
 Tools/sitl_gazebo | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/Tools/sitl_gazebo b/Tools/sitl_gazebo
index eea655d25d..459a0c9335 160000
--- a/Tools/sitl_gazebo
+++ b/Tools/sitl_gazebo
@@ -1 +1 @@
-Subproject commit eea655d25d405aae29e2e8a1e914b3579076193e
+Subproject commit 459a0c933598249fdf30d8162c5b9ec7d6620258

From ec15130a80b702762a58e3309c49bce4b0b62f32 Mon Sep 17 00:00:00 2001
From: sander <sander@droneslab.com>
Date: Fri, 13 May 2016 01:46:24 +0200
Subject: [PATCH 0526/1230] Minimum altitude for FW flight in standard VTOL

---
 src/modules/vtol_att_control/standard.cpp      |  8 ++++++++
 src/modules/vtol_att_control/standard.h        |  2 ++
 src/modules/vtol_att_control/standard_params.c | 12 ++++++++++++
 3 files changed, 22 insertions(+)

diff --git a/src/modules/vtol_att_control/standard.cpp b/src/modules/vtol_att_control/standard.cpp
index 9109a82f90..1815676321 100644
--- a/src/modules/vtol_att_control/standard.cpp
+++ b/src/modules/vtol_att_control/standard.cpp
@@ -68,6 +68,7 @@ Standard::Standard(VtolAttitudeControl *attc) :
 	_params_handles_standard.front_trans_time_min = param_find("VT_TRANS_MIN_TM");
 	_params_handles_standard.down_pitch_max = param_find("VT_DWN_PITCH_MAX");
 	_params_handles_standard.forward_thurst_scale = param_find("VT_FWD_THRUST_SC");
+	_params_handles_standard.fw_minimum_altitude = param_find("VT_FW_MIN_ALT");
 }
 
 Standard::~Standard()
@@ -114,6 +115,9 @@ Standard::parameters_update()
 	/* scale for fixed wing thrust used for forward acceleration in multirotor mode */
 	param_get(_params_handles_standard.forward_thurst_scale, &_params_standard.forward_thurst_scale);
 
+	/* QuadChute; minimum altitude for fixed wing flight */
+	param_get(_params_handles_standard.fw_minimum_altitude, &_params_standard.fw_minimum_altitude);
+
 
 	return OK;
 }
@@ -361,6 +365,10 @@ void Standard::update_fw_state()
 		set_idle_fw();  // force them to stop, not just idle
 		_flag_enable_mc_motors = true;
 	}
+
+	if(_local_pos->dist_bottom < _params_standard.fw_minimum_altitude){
+		_attc->abort_front_transition();
+	}
 }
 
 /**
diff --git a/src/modules/vtol_att_control/standard.h b/src/modules/vtol_att_control/standard.h
index b4ab84d59b..3a3e22e64c 100644
--- a/src/modules/vtol_att_control/standard.h
+++ b/src/modules/vtol_att_control/standard.h
@@ -76,6 +76,7 @@ private:
 		float front_trans_time_min;
 		float down_pitch_max;
 		float forward_thurst_scale;
+		float fw_minimum_altitude;
 	} _params_standard;
 
 	struct {
@@ -88,6 +89,7 @@ private:
 		param_t front_trans_time_min;
 		param_t down_pitch_max;
 		param_t forward_thurst_scale;
+		param_t fw_minimum_altitude;
 	} _params_handles_standard;
 
 	enum vtol_mode {
diff --git a/src/modules/vtol_att_control/standard_params.c b/src/modules/vtol_att_control/standard_params.c
index b4ab3e4c42..114dc17474 100644
--- a/src/modules/vtol_att_control/standard_params.c
+++ b/src/modules/vtol_att_control/standard_params.c
@@ -72,3 +72,15 @@ PARAM_DEFINE_FLOAT(VT_DWN_PITCH_MAX, 5.0f);
  * @group VTOL Attitude Control
  */
 PARAM_DEFINE_FLOAT(VT_FWD_THRUST_SC, 0.0f);
+
+
+/**
+ * QuadChute
+ *
+ * Minimum altitude for fixed wing flight, when in fixed wing the altitude drops below this altitude
+ * the vehicle will transition back to MC mode and enter failsafe RTL
+ * @min 0.0
+ * @max 200.0
+ * @group VTOL Attitude Control
+ */
+PARAM_DEFINE_FLOAT(VT_FW_MIN_ALT, 0.0f);

From 7cea813d55107f5d3905400ac426876e84a31f7d Mon Sep 17 00:00:00 2001
From: sander <sander@droneslab.com>
Date: Fri, 13 May 2016 01:58:05 +0200
Subject: [PATCH 0527/1230] Sanity check and quadchute during front transition

---
 src/modules/vtol_att_control/standard.cpp | 15 +++++++++++++--
 1 file changed, 13 insertions(+), 2 deletions(-)

diff --git a/src/modules/vtol_att_control/standard.cpp b/src/modules/vtol_att_control/standard.cpp
index 1815676321..78c8bb119c 100644
--- a/src/modules/vtol_att_control/standard.cpp
+++ b/src/modules/vtol_att_control/standard.cpp
@@ -262,6 +262,14 @@ void Standard::update_transition_state()
 			}
 		}
 
+		// quadchute
+		if(_params_standard.fw_minimum_altitude > FLT_EPSILON){
+			if(_local_pos->dist_bottom < _params_standard.fw_minimum_altitude){
+				_attc->abort_front_transition();
+			}
+		}
+
+
 	} else if (_vtol_schedule.flight_mode == TRANSITION_TO_MC) {
 		// continually increase mc attitude control as we transition back to mc mode
 		if (_params_standard.back_trans_dur > 0.0f) {
@@ -366,8 +374,11 @@ void Standard::update_fw_state()
 		_flag_enable_mc_motors = true;
 	}
 
-	if(_local_pos->dist_bottom < _params_standard.fw_minimum_altitude){
-		_attc->abort_front_transition();
+	// quadchute
+	if(_params_standard.fw_minimum_altitude > FLT_EPSILON){
+		if(_local_pos->dist_bottom < _params_standard.fw_minimum_altitude){
+			_attc->abort_front_transition();
+		}
 	}
 }
 

From bb0b7f26320444b564e686d32ca43b634cceff09 Mon Sep 17 00:00:00 2001
From: sander <sander@droneslab.com>
Date: Fri, 13 May 2016 02:08:13 +0200
Subject: [PATCH 0528/1230] Sanitize mavlink message

---
 src/modules/vtol_att_control/vtol_att_control_main.cpp | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/src/modules/vtol_att_control/vtol_att_control_main.cpp b/src/modules/vtol_att_control/vtol_att_control_main.cpp
index 3e86813861..fb6154e963 100644
--- a/src/modules/vtol_att_control/vtol_att_control_main.cpp
+++ b/src/modules/vtol_att_control/vtol_att_control_main.cpp
@@ -508,8 +508,8 @@ VtolAttitudeControl::is_fixed_wing_requested()
 void
 VtolAttitudeControl::abort_front_transition()
 {
-	if (!_abort_front_transition) {
-		mavlink_log_critical(&_mavlink_log_pub, "Front transition timeout occured, aborting");
+	if(!_abort_front_transition) {
+		mavlink_log_critical(&_mavlink_log_pub, "Transition timeout or FW min alt occured, aborting");
 		_abort_front_transition = true;
 		_vtol_vehicle_status.vtol_transition_failsafe = true;
 	}

From 832e1003c289953d28859bec200b442934ef7238 Mon Sep 17 00:00:00 2001
From: sander <sander@droneslab.com>
Date: Sat, 21 May 2016 13:49:14 +0000
Subject: [PATCH 0529/1230] Base QuadChute on local_pos.z and only when armed

---
 src/modules/vtol_att_control/standard.cpp | 9 +++++----
 1 file changed, 5 insertions(+), 4 deletions(-)

diff --git a/src/modules/vtol_att_control/standard.cpp b/src/modules/vtol_att_control/standard.cpp
index 78c8bb119c..83efa6fb59 100644
--- a/src/modules/vtol_att_control/standard.cpp
+++ b/src/modules/vtol_att_control/standard.cpp
@@ -263,8 +263,8 @@ void Standard::update_transition_state()
 		}
 
 		// quadchute
-		if(_params_standard.fw_minimum_altitude > FLT_EPSILON){
-			if(_local_pos->dist_bottom < _params_standard.fw_minimum_altitude){
+		if(_params_standard.fw_minimum_altitude > FLT_EPSILON && _armed->armed){
+			if(-(_local_pos->z) < _params_standard.fw_minimum_altitude){
 				_attc->abort_front_transition();
 			}
 		}
@@ -375,8 +375,9 @@ void Standard::update_fw_state()
 	}
 
 	// quadchute
-	if(_params_standard.fw_minimum_altitude > FLT_EPSILON){
-		if(_local_pos->dist_bottom < _params_standard.fw_minimum_altitude){
+	if(_params_standard.fw_minimum_altitude > FLT_EPSILON && _armed->armed){
+		printf("z: %f min %f\n", (double)_local_pos->z, (double)_params_standard.fw_minimum_altitude);
+		if(-(_local_pos->z) < _params_standard.fw_minimum_altitude){
 			_attc->abort_front_transition();
 		}
 	}

From 2d61eddebf8ac3a78ab1053b31e7028befe58b9e Mon Sep 17 00:00:00 2001
From: sander <sander@droneslab.com>
Date: Sat, 21 May 2016 13:50:59 +0000
Subject: [PATCH 0530/1230] Remove debug info

---
 src/modules/vtol_att_control/standard.cpp | 1 -
 1 file changed, 1 deletion(-)

diff --git a/src/modules/vtol_att_control/standard.cpp b/src/modules/vtol_att_control/standard.cpp
index 83efa6fb59..97a1e0020d 100644
--- a/src/modules/vtol_att_control/standard.cpp
+++ b/src/modules/vtol_att_control/standard.cpp
@@ -376,7 +376,6 @@ void Standard::update_fw_state()
 
 	// quadchute
 	if(_params_standard.fw_minimum_altitude > FLT_EPSILON && _armed->armed){
-		printf("z: %f min %f\n", (double)_local_pos->z, (double)_params_standard.fw_minimum_altitude);
 		if(-(_local_pos->z) < _params_standard.fw_minimum_altitude){
 			_attc->abort_front_transition();
 		}

From cdf98644289359e19d9bd6cee844c47ba81fa64b Mon Sep 17 00:00:00 2001
From: sander <sander@droneslab.com>
Date: Sat, 28 May 2016 04:44:11 +0200
Subject: [PATCH 0531/1230] QuadChute moved to VtolType

---
 src/modules/vtol_att_control/standard.cpp     | 17 -----------------
 src/modules/vtol_att_control/standard.h       |  4 +---
 .../vtol_att_control_main.cpp                 |  6 ++++++
 .../vtol_att_control/vtol_att_control_main.h  |  1 +
 src/modules/vtol_att_control/vtol_type.cpp    | 19 +++++++++++++++++++
 src/modules/vtol_att_control/vtol_type.h      |  1 +
 6 files changed, 28 insertions(+), 20 deletions(-)

diff --git a/src/modules/vtol_att_control/standard.cpp b/src/modules/vtol_att_control/standard.cpp
index 97a1e0020d..a0e9692444 100644
--- a/src/modules/vtol_att_control/standard.cpp
+++ b/src/modules/vtol_att_control/standard.cpp
@@ -68,7 +68,6 @@ Standard::Standard(VtolAttitudeControl *attc) :
 	_params_handles_standard.front_trans_time_min = param_find("VT_TRANS_MIN_TM");
 	_params_handles_standard.down_pitch_max = param_find("VT_DWN_PITCH_MAX");
 	_params_handles_standard.forward_thurst_scale = param_find("VT_FWD_THRUST_SC");
-	_params_handles_standard.fw_minimum_altitude = param_find("VT_FW_MIN_ALT");
 }
 
 Standard::~Standard()
@@ -115,9 +114,6 @@ Standard::parameters_update()
 	/* scale for fixed wing thrust used for forward acceleration in multirotor mode */
 	param_get(_params_handles_standard.forward_thurst_scale, &_params_standard.forward_thurst_scale);
 
-	/* QuadChute; minimum altitude for fixed wing flight */
-	param_get(_params_handles_standard.fw_minimum_altitude, &_params_standard.fw_minimum_altitude);
-
 
 	return OK;
 }
@@ -262,12 +258,6 @@ void Standard::update_transition_state()
 			}
 		}
 
-		// quadchute
-		if(_params_standard.fw_minimum_altitude > FLT_EPSILON && _armed->armed){
-			if(-(_local_pos->z) < _params_standard.fw_minimum_altitude){
-				_attc->abort_front_transition();
-			}
-		}
 
 
 	} else if (_vtol_schedule.flight_mode == TRANSITION_TO_MC) {
@@ -373,13 +363,6 @@ void Standard::update_fw_state()
 		set_idle_fw();  // force them to stop, not just idle
 		_flag_enable_mc_motors = true;
 	}
-
-	// quadchute
-	if(_params_standard.fw_minimum_altitude > FLT_EPSILON && _armed->armed){
-		if(-(_local_pos->z) < _params_standard.fw_minimum_altitude){
-			_attc->abort_front_transition();
-		}
-	}
 }
 
 /**
diff --git a/src/modules/vtol_att_control/standard.h b/src/modules/vtol_att_control/standard.h
index 3a3e22e64c..ecddb92fcd 100644
--- a/src/modules/vtol_att_control/standard.h
+++ b/src/modules/vtol_att_control/standard.h
@@ -75,8 +75,7 @@ private:
 		float front_trans_timeout;
 		float front_trans_time_min;
 		float down_pitch_max;
-		float forward_thurst_scale;
-		float fw_minimum_altitude;
+		float forward_thurst_scale;		
 	} _params_standard;
 
 	struct {
@@ -89,7 +88,6 @@ private:
 		param_t front_trans_time_min;
 		param_t down_pitch_max;
 		param_t forward_thurst_scale;
-		param_t fw_minimum_altitude;
 	} _params_handles_standard;
 
 	enum vtol_mode {
diff --git a/src/modules/vtol_att_control/vtol_att_control_main.cpp b/src/modules/vtol_att_control/vtol_att_control_main.cpp
index fb6154e963..5cfd3da4b9 100644
--- a/src/modules/vtol_att_control/vtol_att_control_main.cpp
+++ b/src/modules/vtol_att_control/vtol_att_control_main.cpp
@@ -134,6 +134,7 @@ VtolAttitudeControl::VtolAttitudeControl() :
 	_params_handles.arsp_lp_gain = param_find("VT_ARSP_LP_GAIN");
 	_params_handles.vtol_type = param_find("VT_TYPE");
 	_params_handles.elevons_mc_lock = param_find("VT_ELEV_MC_LOCK");
+	_params_handles.fw_min_alt = param_find("VT_FW_MIN_ALT");
 
 	/* fetch initial parameter values */
 	parameters_update();
@@ -567,6 +568,11 @@ VtolAttitudeControl::parameters_update()
 	param_get(_params_handles.elevons_mc_lock, &l);
 	_params.elevons_mc_lock = l;
 
+	/* minimum relative altitude for FW mode (QuadChute) */
+	param_get(_params_handles.fw_min_alt, &v);
+	_params.fw_min_alt = v;
+
+
 	return OK;
 }
 
diff --git a/src/modules/vtol_att_control/vtol_att_control_main.h b/src/modules/vtol_att_control/vtol_att_control_main.h
index e2a63993d7..90a36e652f 100644
--- a/src/modules/vtol_att_control/vtol_att_control_main.h
+++ b/src/modules/vtol_att_control/vtol_att_control_main.h
@@ -209,6 +209,7 @@ private:
 		param_t arsp_lp_gain;
 		param_t vtol_type;
 		param_t elevons_mc_lock;
+		param_t fw_min_alt;
 	} _params_handles;
 
 	/* for multicopters it is usual to have a non-zero idle speed of the engines
diff --git a/src/modules/vtol_att_control/vtol_type.cpp b/src/modules/vtol_att_control/vtol_type.cpp
index c9e69d7b62..73d90fb3b3 100644
--- a/src/modules/vtol_att_control/vtol_type.cpp
+++ b/src/modules/vtol_att_control/vtol_type.cpp
@@ -177,6 +177,25 @@ void VtolType::update_fw_state()
 	if (!_tecs_running || (_tecs_running && _fw_virtual_att_sp->timestamp <= _tecs_running_ts)) {
 		waiting_on_tecs();
 	}
+
+	// quadchute
+	if(_params->fw_min_alt > FLT_EPSILON && _armed->armed){
+		if(-(_local_pos->z) < _params->fw_min_alt){
+			_attc->abort_front_transition();
+		}
+	}
+
+}
+
+void VtolType::update_transition_state()
+{
+	// quadchute
+	if(_params->fw_min_alt > FLT_EPSILON && _armed->armed){
+		if(-(_local_pos->z) < _params->fw_min_alt){
+			_attc->abort_front_transition();
+		}
+	}
+
 }
 
 bool VtolType::can_transition_on_ground()
diff --git a/src/modules/vtol_att_control/vtol_type.h b/src/modules/vtol_att_control/vtol_type.h
index 9a15ff8cfa..d755fb43e4 100644
--- a/src/modules/vtol_att_control/vtol_type.h
+++ b/src/modules/vtol_att_control/vtol_type.h
@@ -59,6 +59,7 @@ struct Params {
 	float arsp_lp_gain;			// total airspeed estimate low pass gain
 	int vtol_type;
 	int elevons_mc_lock;		// lock elevons in multicopter mode
+	float fw_min_alt;			// minimum relative altitude for FW mode (QuadChute)
 };
 
 enum mode {

From cd574c523bc54a8956bbc0cbd5dc1b67484f91a0 Mon Sep 17 00:00:00 2001
From: tumbili <roman@px4.io>
Date: Tue, 7 Jun 2016 15:48:39 +0200
Subject: [PATCH 0532/1230] fixed whitespaces

---
 src/modules/vtol_att_control/standard.cpp | 2 --
 src/modules/vtol_att_control/standard.h   | 2 +-
 2 files changed, 1 insertion(+), 3 deletions(-)

diff --git a/src/modules/vtol_att_control/standard.cpp b/src/modules/vtol_att_control/standard.cpp
index a0e9692444..9109a82f90 100644
--- a/src/modules/vtol_att_control/standard.cpp
+++ b/src/modules/vtol_att_control/standard.cpp
@@ -258,8 +258,6 @@ void Standard::update_transition_state()
 			}
 		}
 
-
-
 	} else if (_vtol_schedule.flight_mode == TRANSITION_TO_MC) {
 		// continually increase mc attitude control as we transition back to mc mode
 		if (_params_standard.back_trans_dur > 0.0f) {
diff --git a/src/modules/vtol_att_control/standard.h b/src/modules/vtol_att_control/standard.h
index ecddb92fcd..b4ab84d59b 100644
--- a/src/modules/vtol_att_control/standard.h
+++ b/src/modules/vtol_att_control/standard.h
@@ -75,7 +75,7 @@ private:
 		float front_trans_timeout;
 		float front_trans_time_min;
 		float down_pitch_max;
-		float forward_thurst_scale;		
+		float forward_thurst_scale;
 	} _params_standard;
 
 	struct {

From ff58d31348cc6ad4f2e54691f1c45002caf9acbd Mon Sep 17 00:00:00 2001
From: Roman <bapstr@ethz.ch>
Date: Tue, 7 Jun 2016 20:00:41 +0200
Subject: [PATCH 0533/1230] fixed code style

---
 src/modules/vtol_att_control/vtol_att_control_main.cpp | 2 +-
 src/modules/vtol_att_control/vtol_type.cpp             | 8 ++++----
 2 files changed, 5 insertions(+), 5 deletions(-)

diff --git a/src/modules/vtol_att_control/vtol_att_control_main.cpp b/src/modules/vtol_att_control/vtol_att_control_main.cpp
index 5cfd3da4b9..57e0a98170 100644
--- a/src/modules/vtol_att_control/vtol_att_control_main.cpp
+++ b/src/modules/vtol_att_control/vtol_att_control_main.cpp
@@ -509,7 +509,7 @@ VtolAttitudeControl::is_fixed_wing_requested()
 void
 VtolAttitudeControl::abort_front_transition()
 {
-	if(!_abort_front_transition) {
+	if (!_abort_front_transition) {
 		mavlink_log_critical(&_mavlink_log_pub, "Transition timeout or FW min alt occured, aborting");
 		_abort_front_transition = true;
 		_vtol_vehicle_status.vtol_transition_failsafe = true;
diff --git a/src/modules/vtol_att_control/vtol_type.cpp b/src/modules/vtol_att_control/vtol_type.cpp
index 73d90fb3b3..83e331a581 100644
--- a/src/modules/vtol_att_control/vtol_type.cpp
+++ b/src/modules/vtol_att_control/vtol_type.cpp
@@ -179,8 +179,8 @@ void VtolType::update_fw_state()
 	}
 
 	// quadchute
-	if(_params->fw_min_alt > FLT_EPSILON && _armed->armed){
-		if(-(_local_pos->z) < _params->fw_min_alt){
+	if (_params->fw_min_alt > FLT_EPSILON && _armed->armed) {
+		if (-(_local_pos->z) < _params->fw_min_alt) {
 			_attc->abort_front_transition();
 		}
 	}
@@ -190,8 +190,8 @@ void VtolType::update_fw_state()
 void VtolType::update_transition_state()
 {
 	// quadchute
-	if(_params->fw_min_alt > FLT_EPSILON && _armed->armed){
-		if(-(_local_pos->z) < _params->fw_min_alt){
+	if (_params->fw_min_alt > FLT_EPSILON && _armed->armed) {
+		if (-(_local_pos->z) < _params->fw_min_alt) {
 			_attc->abort_front_transition();
 		}
 	}

From 7a6ff4742d0baa71e5a2749206222931737d64a2 Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lorenz@px4.io>
Date: Tue, 7 Jun 2016 21:25:35 +0200
Subject: [PATCH 0534/1230] Add Solo sim model (#4761)

---
 Makefile                                |  2 +-
 posix-configs/SITL/init/rcS_gazebo_solo | 75 +++++++++++++++++++++++++
 src/firmware/posix/CMakeLists.txt       |  2 +-
 3 files changed, 77 insertions(+), 2 deletions(-)
 create mode 100644 posix-configs/SITL/init/rcS_gazebo_solo

diff --git a/Makefile b/Makefile
index 6ec7d72f78..26aad575a8 100644
--- a/Makefile
+++ b/Makefile
@@ -315,7 +315,7 @@ distclean: submodulesclean
 cmake_targets = test upload package package_source debug debug_tui debug_ddd debug_io debug_io_tui debug_io_ddd check_weak \
 	run_cmake_config config gazebo gazebo_gdb gazebo_lldb jmavsim replay \
 	jmavsim_gdb jmavsim_lldb gazebo_gdb_iris gazebo_lldb_tailsitter gazebo_iris gazebo_iris_opt_flow gazebo_tailsitter \
-	gazebo_gdb_standard_vtol gazebo_lldb_standard_vtol gazebo_standard_vtol gazebo_plane
+	gazebo_gdb_standard_vtol gazebo_lldb_standard_vtol gazebo_standard_vtol gazebo_plane gazebo_solo
 $(foreach targ,$(cmake_targets),$(eval $(call cmake-targ,$(targ))))
 
 .PHONY: clean
diff --git a/posix-configs/SITL/init/rcS_gazebo_solo b/posix-configs/SITL/init/rcS_gazebo_solo
new file mode 100644
index 0000000000..e4d8c958a9
--- /dev/null
+++ b/posix-configs/SITL/init/rcS_gazebo_solo
@@ -0,0 +1,75 @@
+uorb start
+param load
+param set MAV_TYPE 2
+param set SYS_AUTOSTART 4010
+param set SYS_RESTART_TYPE 2
+dataman start
+param set CAL_GYRO0_ID 2293768
+param set CAL_ACC0_ID 1376264
+param set CAL_ACC1_ID 1310728
+param set CAL_MAG0_ID 196616
+param set CAL_GYRO0_XOFF 0.01
+param set CAL_ACC0_XOFF 0.01
+param set CAL_ACC0_YOFF -0.01
+param set CAL_ACC0_ZOFF 0.01
+param set CAL_ACC0_XSCALE 1.01
+param set CAL_ACC0_YSCALE 1.01
+param set CAL_ACC0_ZSCALE 1.01
+param set CAL_ACC1_XOFF 0.01
+param set CAL_MAG0_XOFF 0.01
+param set SENS_BOARD_ROT 8
+param set SENS_BOARD_X_OFF 0.000001
+param set COM_RC_IN_MODE 1
+param set NAV_DLL_ACT 2
+param set COM_DISARM_LAND 3
+param set NAV_ACC_RAD 2.0
+param set RTL_RETURN_ALT 30.0
+param set RTL_DESCEND_ALT 10.0
+param set RTL_LAND_DELAY 0
+param set MIS_TAKEOFF_ALT 2.5
+param set MC_ROLLRATE_P 0.5
+param set MC_PITCHRATE_P 0.5
+param set MC_PITCH_P 5
+param set MC_ROLL_P 5
+param set MC_ROLLRATE_I 0.1
+param set MC_PITCHRATE_I 0.1
+param set MPC_HOLD_MAX_Z 2.0
+param set MPC_HOLD_XY_DZ 0.1
+param set MPC_Z_VEL_P 0.8
+param set MPC_Z_VEL_I 0.15
+param set MPC_XY_VEL_P 0.15
+param set MPC_XY_VEL_I 0.2
+param set EKF2_GBIAS_INIT 0.01
+param set EKF2_ANGERR_INIT 0.01
+simulator start -s
+rgbledsim start
+tone_alarm start
+gyrosim start
+accelsim start
+barosim start
+adcsim start
+gpssim start
+pwm_out_sim mode_pwm
+sleep 1
+sensors start
+commander start
+land_detector start multicopter
+navigator start
+ekf2 start
+mc_pos_control start
+mc_att_control start
+mixer load /dev/pwm_output0 ../../../../ROMFS/px4fmu_common/mixers/quad_w.main.mix
+mavlink start -u 14556 -r 4000000
+mavlink start -u 14557 -r 4000000 -m onboard -o 14540
+mavlink stream -r 50 -s POSITION_TARGET_LOCAL_NED -u 14556
+mavlink stream -r 50 -s LOCAL_POSITION_NED -u 14556
+mavlink stream -r 50 -s GLOBAL_POSITION_INT -u 14556
+mavlink stream -r 50 -s ATTITUDE -u 14556
+mavlink stream -r 50 -s ATTITUDE_QUATERNION -u 14556
+mavlink stream -r 50 -s ATTITUDE_TARGET -u 14556
+mavlink stream -r 50 -s SERVO_OUTPUT_RAW_0 -u 14556
+mavlink stream -r 20 -s RC_CHANNELS -u 14556
+mavlink stream -r 250 -s HIGHRES_IMU -u 14556
+mavlink stream -r 10 -s OPTICAL_FLOW_RAD -u 14556
+sdlog2 start -r 100 -e -t -a
+mavlink boot_complete
diff --git a/src/firmware/posix/CMakeLists.txt b/src/firmware/posix/CMakeLists.txt
index 0da29d151f..a1c5f1c374 100644
--- a/src/firmware/posix/CMakeLists.txt
+++ b/src/firmware/posix/CMakeLists.txt
@@ -128,7 +128,7 @@ add_dependencies(run_config mainapp)
 
 foreach(viewer none jmavsim gazebo replay)
 	foreach(debugger none gdb lldb ddd valgrind)
-		foreach(model none iris iris_opt_flow tailsitter standard_vtol plane)
+		foreach(model none iris iris_opt_flow tailsitter standard_vtol plane solo)
 			if (debugger STREQUAL "none")
 				if (model STREQUAL "none")
 					set(_targ_name "${viewer}")

From a39124a10d54b200823150c9173b1ca6df8e2dc9 Mon Sep 17 00:00:00 2001
From: Julian Oes <julian@oes.ch>
Date: Wed, 1 Jun 2016 13:58:55 +0100
Subject: [PATCH 0535/1230] dataman: make it less chatty

- Removed a couple of unneeded printfs.
- On POSIX it shouldn't warn if it's not a multiple of the
  block size.
---
 src/modules/dataman/dataman.c | 55 +++++++++++++++++------------------
 1 file changed, 27 insertions(+), 28 deletions(-)

diff --git a/src/modules/dataman/dataman.c b/src/modules/dataman/dataman.c
index 356a722027..8a22c62d23 100644
--- a/src/modules/dataman/dataman.c
+++ b/src/modules/dataman/dataman.c
@@ -715,6 +715,11 @@ task_main(int argc, char *argv[])
 	g_task_fd = open(k_data_manager_device_path, O_RDONLY | O_BINARY);
 
 	if (g_task_fd >= 0) {
+
+#ifndef __PX4_POSIX
+		// XXX on Mac OS and Linux the file is not a multiple of the sector sizes
+		// this might need further inspection.
+
 		/* File exists, check its size */
 		int file_size = lseek(g_task_fd, 0, SEEK_END);
 
@@ -722,54 +727,47 @@ task_main(int argc, char *argv[])
 			PX4_WARN("Incompatible data manager file %s, resetting it", k_data_manager_device_path);
 			PX4_WARN("Size: %u, sector size: %d", file_size, k_sector_size);
 			close(g_task_fd);
-#ifndef __PX4_POSIX
-			// XXX on Mac OS and Linux the file is not a multiple of the sector sizes
-			// this might need further inspection
 			unlink(k_data_manager_device_path);
+		}
+
+#else
+		close(g_task_fd);
 #endif
 
-		} else {
-			close(g_task_fd);
-		}
 	}
 
 	/* Open or create the data manager file */
 	g_task_fd = open(k_data_manager_device_path, O_RDWR | O_CREAT | O_BINARY, PX4_O_MODE_666);
 
 	if (g_task_fd < 0) {
-		warnx("Could not open data manager file %s", k_data_manager_device_path);
+		PX4_WARN("Could not open data manager file %s", k_data_manager_device_path);
 		px4_sem_post(&g_init_sema); /* Don't want to hang startup */
 		return -1;
 	}
 
 	if ((unsigned)lseek(g_task_fd, max_offset, SEEK_SET) != max_offset) {
 		close(g_task_fd);
-		warnx("Could not seek data manager file %s", k_data_manager_device_path);
+		PX4_WARN("Could not seek data manager file %s", k_data_manager_device_path);
 		px4_sem_post(&g_init_sema); /* Don't want to hang startup */
 		return -1;
 	}
 
 	fsync(g_task_fd);
 
-	printf("dataman: ");
 	/* see if we need to erase any items based on restart type */
 	int sys_restart_val;
 
+	const char *restart_type_str = "Unkown restart";
+
 	if (param_get(param_find("SYS_RESTART_TYPE"), &sys_restart_val) == OK) {
 		if (sys_restart_val == DM_INIT_REASON_POWER_ON) {
-			printf("Power on restart");
+			restart_type_str = "Power on restart";
 			_restart(DM_INIT_REASON_POWER_ON);
 
 		} else if (sys_restart_val == DM_INIT_REASON_IN_FLIGHT) {
-			printf("In flight restart");
+			restart_type_str = "In flight restart";
 			_restart(DM_INIT_REASON_IN_FLIGHT);
-
-		} else {
-			printf("Unknown restart");
 		}
-
-	} else {
-		printf("Unknown restart");
 	}
 
 	/* We use two file descriptors, one for the caller context and one for the worker thread */
@@ -777,7 +775,8 @@ task_main(int argc, char *argv[])
 	/* worker thread is shutting down but still processing requests */
 	g_fd = g_task_fd;
 
-	printf(", data manager file '%s' size is %d bytes\n", k_data_manager_device_path, max_offset);
+	PX4_INFO("%s, data manager file '%s' size is %d bytes",
+		 restart_type_str, k_data_manager_device_path, max_offset);
 
 	/* Tell startup that the worker thread has completed its initialization */
 	px4_sem_post(&g_init_sema);
@@ -885,11 +884,11 @@ static void
 status(void)
 {
 	/* display usage statistics */
-	warnx("Writes   %d", g_func_counts[dm_write_func]);
-	warnx("Reads    %d", g_func_counts[dm_read_func]);
-	warnx("Clears   %d", g_func_counts[dm_clear_func]);
-	warnx("Restarts %d", g_func_counts[dm_restart_func]);
-	warnx("Max Q lengths work %d, free %d", g_work_q.max_size, g_free_q.max_size);
+	PX4_INFO("Writes   %d", g_func_counts[dm_write_func]);
+	PX4_INFO("Reads    %d", g_func_counts[dm_read_func]);
+	PX4_INFO("Clears   %d", g_func_counts[dm_clear_func]);
+	PX4_INFO("Restarts %d", g_func_counts[dm_restart_func]);
+	PX4_INFO("Max Q lengths work %d, free %d", g_work_q.max_size, g_free_q.max_size);
 }
 
 static void
@@ -903,7 +902,7 @@ stop(void)
 static void
 usage(void)
 {
-	warnx("usage: dataman {start [-f datafile]|stop|status|poweronrestart|inflightrestart}");
+	PX4_INFO("usage: dataman {start [-f datafile]|stop|status|poweronrestart|inflightrestart}");
 }
 
 int
@@ -917,13 +916,13 @@ dataman_main(int argc, char *argv[])
 	if (!strcmp(argv[1], "start")) {
 
 		if (g_fd >= 0) {
-			warnx("dataman already running");
+			PX4_WARN("dataman already running");
 			return -1;
 		}
 
 		if (argc == 4 && strcmp(argv[2], "-f") == 0) {
 			k_data_manager_device_path = strdup(argv[3]);
-			warnx("dataman file set to: %s\n", k_data_manager_device_path);
+			PX4_INFO("dataman file set to: %s", k_data_manager_device_path);
 
 		} else {
 			k_data_manager_device_path = strdup(default_device_path);
@@ -932,7 +931,7 @@ dataman_main(int argc, char *argv[])
 		start();
 
 		if (g_fd < 0) {
-			warnx("dataman start failed");
+			PX4_ERR("dataman start failed");
 			free(k_data_manager_device_path);
 			k_data_manager_device_path = NULL;
 			return -1;
@@ -943,7 +942,7 @@ dataman_main(int argc, char *argv[])
 
 	/* Worker thread should be running for all other commands */
 	if (g_fd < 0) {
-		warnx("dataman worker thread not running");
+		PX4_WARN("dataman worker thread not running");
 		usage();
 		return -1;
 	}

From ec5b2adfc0701a284258e33bb441d6e0acde6baf Mon Sep 17 00:00:00 2001
From: Julian Oes <julian@oes.ch>
Date: Wed, 1 Jun 2016 14:00:03 +0100
Subject: [PATCH 0536/1230] mavlink: don't try broadcast 0, and less printfs

This removes a bunch of unneeded printfs and prevents broadcasting
packets of size 0 which just trigger a warning.
---
 src/modules/mavlink/mavlink_main.cpp | 46 ++++++++++++++++------------
 src/modules/mavlink/mavlink_main.h   | 20 +++++++++++-
 2 files changed, 46 insertions(+), 20 deletions(-)

diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp
index 08d85cbacb..c3d7aa07a9 100644
--- a/src/modules/mavlink/mavlink_main.cpp
+++ b/src/modules/mavlink/mavlink_main.cpp
@@ -224,7 +224,6 @@ Mavlink::Mavlink() :
 	_src_addr_initialized(false),
 	_broadcast_address_found(false),
 	_broadcast_address_not_found_warned(false),
-	_sendto_result(1),
 	_network_buf{},
 	_network_buf_len(0),
 #endif
@@ -890,34 +889,42 @@ Mavlink::send_packet()
 
 #ifdef __PX4_POSIX
 
+	/* Only send packets if there is something in the buffer. */
+	if (_network_buf_len == 0) {
+		return 0;
+	}
+
 	if (get_protocol() == UDP) {
-		ret = sendto(_socket_fd, _network_buf, _network_buf_len, 0, (struct sockaddr *)&_src_addr, sizeof(_src_addr));
+
+
+		ret = sendto(_socket_fd, _network_buf, _network_buf_len, 0,
+			     (struct sockaddr *)&_src_addr, sizeof(_src_addr));
 
 		struct telemetry_status_s &tstatus = get_rx_status();
 
 		/* resend message via broadcast if no valid connection exists */
 		if ((_mode != MAVLINK_MODE_ONBOARD) && broadcast_enabled() &&
-			(!get_client_source_initialized()
-			|| (hrt_elapsed_time(&tstatus.heartbeat_time) > 3 * 1000 * 1000))) {
+		    (!get_client_source_initialized()
+		    || (hrt_elapsed_time(&tstatus.heartbeat_time) > 3 * 1000 * 1000))) {
 
 			if (!_broadcast_address_found) {
 				find_broadcast_address();
 			}
 
-			if (_broadcast_address_found) {
+			if (_broadcast_address_found && _network_buf_len > 0) {
 
-				int bret = sendto(_socket_fd, _network_buf, _network_buf_len, 0, (struct sockaddr *)&_bcast_addr, sizeof(_bcast_addr));
+				int bret = sendto(_socket_fd, _network_buf, _network_buf_len, 0,
+						  (struct sockaddr *)&_bcast_addr, sizeof(_bcast_addr));
 
-				if (bret <= 0 && _sendto_result > 0) {
+				if (bret <= 0) {
 					PX4_ERR("sending broadcast failed, errno: %d: %s", errno, strerror(errno));
 				}
-				_sendto_result = bret;
 			}
 		}
 
 	} else if (get_protocol() == TCP) {
 		/* not implemented, but possible to do so */
-		warnx("TCP transport pending implementation");
+		PX4_ERR("TCP transport pending implementation");
 	}
 
 	_network_buf_len = 0;
@@ -1179,7 +1186,7 @@ Mavlink::init_udp()
 {
 #if defined (__PX4_LINUX) || defined (__PX4_DARWIN)
 
-	PX4_INFO("Setting up UDP w/port %d", _network_port);
+	PX4_DEBUG("Setting up UDP with port %d", _network_port);
 
 	_myaddr.sin_family = AF_INET;
 	_myaddr.sin_addr.s_addr = htonl(INADDR_ANY);
@@ -1792,7 +1799,8 @@ Mavlink::task_main(int argc, char *argv[])
 			return ERROR;
 		}
 
-		warnx("mode: %u, data rate: %d B/s on %s @ %dB", _mode, _datarate, _device_name, _baudrate);
+		PX4_INFO("mode: %s, data rate: %d B/s on %s @ %dB",
+			 mavlink_mode_str(_mode), _datarate, _device_name, _baudrate);
 
 		/* flush stdout in case MAVLink is about to take it over */
 		fflush(stdout);
@@ -1814,7 +1822,8 @@ Mavlink::task_main(int argc, char *argv[])
 			return ERROR;
 		}
 
-		warnx("mode: %u, data rate: %d B/s on udp port %hu", _mode, _datarate, _network_port);
+		PX4_INFO("mode: %s, data rate: %d B/s on udp port %hu",
+			 mavlink_mode_str(_mode), _datarate, _network_port);
 	}
 
 	/* initialize send mutex */
@@ -2015,7 +2024,6 @@ Mavlink::task_main(int argc, char *argv[])
 
 	/* init socket if necessary */
 	if (get_protocol() == UDP) {
-		find_broadcast_address();
 		init_udp();
 	}
 
@@ -2111,18 +2119,18 @@ Mavlink::task_main(int argc, char *argv[])
 			if (OK == configure_stream(_subscribe_to_stream, _subscribe_to_stream_rate)) {
 				if (_subscribe_to_stream_rate > 0.0f) {
 					if ( get_protocol() == SERIAL ) {
-						PX4_INFO("stream %s on device %s enabled with rate %.1f Hz", _subscribe_to_stream, _device_name,
-							(double)_subscribe_to_stream_rate);
+						PX4_DEBUG("stream %s on device %s enabled with rate %.1f Hz", _subscribe_to_stream, _device_name,
+							  (double)_subscribe_to_stream_rate);
 					} else if ( get_protocol() == UDP ) {
-						PX4_INFO("stream %s on UDP port %d enabled with rate %.1f Hz", _subscribe_to_stream, _network_port,
-							(double)_subscribe_to_stream_rate);
+						PX4_DEBUG("stream %s on UDP port %d enabled with rate %.1f Hz", _subscribe_to_stream, _network_port,
+							  (double)_subscribe_to_stream_rate);
 					}
 
 				} else {
 					if ( get_protocol() == SERIAL ) {
-						PX4_WARN("stream %s on device %s disabled", _subscribe_to_stream, _device_name);
+						PX4_INFO("stream %s on device %s disabled", _subscribe_to_stream, _device_name);
 					} else if ( get_protocol() == UDP ) {
-						PX4_WARN("stream %s on UDP port %d disabled", _subscribe_to_stream, _network_port);
+						PX4_INFO("stream %s on UDP port %d disabled", _subscribe_to_stream, _network_port);
 					}
 				}
 
diff --git a/src/modules/mavlink/mavlink_main.h b/src/modules/mavlink/mavlink_main.h
index 8391bfb6e9..6ae5636317 100644
--- a/src/modules/mavlink/mavlink_main.h
+++ b/src/modules/mavlink/mavlink_main.h
@@ -169,6 +169,25 @@ public:
 		BROADCAST_MODE_ON
 	};
 
+	static const char *mavlink_mode_str(enum MAVLINK_MODE mode) {
+		switch (mode) {
+			case MAVLINK_MODE_NORMAL:
+				return "Normal";
+			case MAVLINK_MODE_CUSTOM:
+				return "Custom";
+			case MAVLINK_MODE_ONBOARD:
+				return "Onboard";
+			case MAVLINK_MODE_OSD:
+				return "OSD";
+			case MAVLINK_MODE_MAGIC:
+				return "Magic";
+			case MAVLINK_MODE_CONFIG:
+				return "Config";
+			default:
+				return "Unknown";
+		}
+	}
+
 	void			set_mode(enum MAVLINK_MODE);
 	enum MAVLINK_MODE	get_mode() { return _mode; }
 
@@ -486,7 +505,6 @@ private:
 	bool _src_addr_initialized;
 	bool _broadcast_address_found;
 	bool _broadcast_address_not_found_warned;
-	int _sendto_result;
 	uint8_t _network_buf[MAVLINK_MAX_PACKET_LEN];
 	unsigned _network_buf_len;
 #endif

From 67b2f68abdfdf54a3270d93dce3bcbe439a26011 Mon Sep 17 00:00:00 2001
From: Julian Oes <julian@oes.ch>
Date: Wed, 1 Jun 2016 14:01:00 +0100
Subject: [PATCH 0537/1230] mavlink: improved, updated usage

---
 src/modules/mavlink/mavlink_main.cpp | 23 +++++++++++++++++++----
 1 file changed, 19 insertions(+), 4 deletions(-)

diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp
index c3d7aa07a9..1e3dcc288d 100644
--- a/src/modules/mavlink/mavlink_main.cpp
+++ b/src/modules/mavlink/mavlink_main.cpp
@@ -2486,7 +2486,7 @@ Mavlink::stream_command(int argc, char *argv[])
 		}
 
 	} else {
-		warnx("usage: mavlink stream [-d device] [-u network_port] -s stream -r rate");
+		PX4_INFO("usage: mavlink stream [-d device] [-u network_port] -s stream -r rate");
 		return 1;
 	}
 
@@ -2513,7 +2513,22 @@ Mavlink::set_boot_complete()
 
 static void usage()
 {
-	warnx("usage: mavlink {start|stop|stream} [-d device] [-u network_port] [-o remote_port] [-t partner_ip] [-b baudrate]\n\t[-r rate][-m mode] [-s stream] [-f] [-p] [-v] [-w] [-x]");
+	PX4_INFO("usage: mavlink {start|status|stream|stop-all|boot_complete}");
+	PX4_INFO("    [-d device]");
+#ifdef __PX4_POSIX
+	PX4_INFO("    [-u network_port]");
+	PX4_INFO("    [-o remote_port]");
+	PX4_INFO("    [-t partner_ip]");
+#endif
+	PX4_INFO("    [-b baudrate]");
+	PX4_INFO("    [-r rate]");
+	PX4_INFO("    [-m mode]");
+	PX4_INFO("    [-s stream]");
+	PX4_INFO("    [-f]");
+	PX4_INFO("    [-p]");
+	PX4_INFO("    [-v]");
+	PX4_INFO("    [-w]");
+	PX4_INFO("    [-x]");
 }
 
 int mavlink_main(int argc, char *argv[])
@@ -2527,11 +2542,11 @@ int mavlink_main(int argc, char *argv[])
 		return Mavlink::start(argc, argv);
 
 	} else if (!strcmp(argv[1], "stop")) {
-		warnx("mavlink stop is deprecated, use stop-all instead");
+		PX4_WARN("mavlink stop is deprecated, use stop-all instead");
 		usage();
 		return 1;
 
-	} else if (!strcmp(argv[1], "stop") || !strcmp(argv[1], "stop-all") ) {
+	} else if (!strcmp(argv[1], "stop-all") ) {
 		return Mavlink::destroy_all_instances();
 
 	} else if (!strcmp(argv[1], "status")) {

From 1dea9433b7850381f578297ce27894287107d2fb Mon Sep 17 00:00:00 2001
From: Julian Oes <julian@oes.ch>
Date: Wed, 1 Jun 2016 14:01:24 +0100
Subject: [PATCH 0538/1230] mavlink: don't complain if there is no mission

---
 src/modules/mavlink/mavlink_mission.cpp | 11 ++++++++---
 1 file changed, 8 insertions(+), 3 deletions(-)

diff --git a/src/modules/mavlink/mavlink_mission.cpp b/src/modules/mavlink/mavlink_mission.cpp
index 976f4e94a1..0327f94ec7 100644
--- a/src/modules/mavlink/mavlink_mission.cpp
+++ b/src/modules/mavlink/mavlink_mission.cpp
@@ -122,16 +122,21 @@ MavlinkMissionManager::init_offboard_mission()
 	mission_s mission_state;
 	if (!_dataman_init) {
 		_dataman_init = true;
-		if (dm_read(DM_KEY_MISSION_STATE, 0, &mission_state, sizeof(mission_s)) == sizeof(mission_s)) {
+		int ret = dm_read(DM_KEY_MISSION_STATE, 0, &mission_state, sizeof(mission_s)) == sizeof(mission_s);
+		if (ret > 0) {
 			_dataman_id = mission_state.dataman_id;
 			_count = mission_state.count;
 			_current_seq = mission_state.current_seq;
 
-		} else {
+		} else if (ret == 0) {
+			_dataman_id = 0;
+			_count = 0;
+			_current_seq = 0;
+		} else {
+			PX4_WARN("offboard mission init failed");
 			_dataman_id = 0;
 			_count = 0;
 			_current_seq = 0;
-			warnx("offboard mission init failed");
 		}
 	}
 	_my_dataman_id = _dataman_id;

From 00bffeaf72ecb5e9f231d6e2cff98477f9918310 Mon Sep 17 00:00:00 2001
From: Julian Oes <julian@oes.ch>
Date: Wed, 1 Jun 2016 14:01:59 +0100
Subject: [PATCH 0539/1230] mavlink: whitespace only

---
 src/modules/mavlink/mavlink_mission.cpp | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/src/modules/mavlink/mavlink_mission.cpp b/src/modules/mavlink/mavlink_mission.cpp
index 0327f94ec7..088ba47ac0 100644
--- a/src/modules/mavlink/mavlink_mission.cpp
+++ b/src/modules/mavlink/mavlink_mission.cpp
@@ -584,7 +584,7 @@ MavlinkMissionManager::handle_mission_count(const mavlink_message_t *msg)
 	if (CHECK_SYSID_COMPID_MISSION(wpc)) {
 		if (_state == MAVLINK_WPM_STATE_IDLE) {
 			_time_last_recv = hrt_absolute_time();
-			
+
 			if(_transfer_in_progress)
 			{
 				send_mission_ack(_transfer_partner_sysid, _transfer_partner_compid, MAV_MISSION_ERROR);
@@ -793,7 +793,7 @@ MavlinkMissionManager::parse_mavlink_mission_item(const mavlink_mission_item_t *
 		mission_item->params[5] = mavlink_mission_item->y;
 		mission_item->params[6] = mavlink_mission_item->z;
 		break;
-			
+
 	default:
 		return MAV_MISSION_UNSUPPORTED_FRAME;
 	}

From c95c13985c4afd49ff497375a8cce1dc198f0e25 Mon Sep 17 00:00:00 2001
From: Julian Oes <julian@oes.ch>
Date: Wed, 1 Jun 2016 14:02:15 +0100
Subject: [PATCH 0540/1230] mavlink: better printf

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

diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp
index 57b92b6f61..a769cb0315 100644
--- a/src/modules/mavlink/mavlink_receiver.cpp
+++ b/src/modules/mavlink/mavlink_receiver.cpp
@@ -2079,7 +2079,7 @@ MavlinkReceiver::receive_thread(void *arg)
 					srcaddr_last->sin_addr.s_addr = srcaddr.sin_addr.s_addr;
 					srcaddr_last->sin_port = srcaddr.sin_port;
 					_mavlink->set_client_source_initialized();
-					warnx("changing partner IP to: %s", inet_ntoa(srcaddr.sin_addr));
+					PX4_INFO("partner IP: %s", inet_ntoa(srcaddr.sin_addr));
 				}
 			}
 

From f8020a40639ed0735e41f4097e0a830392e0088f Mon Sep 17 00:00:00 2001
From: Julian Oes <julian@oes.ch>
Date: Wed, 1 Jun 2016 14:03:04 +0100
Subject: [PATCH 0541/1230] muorb: removed printf

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

diff --git a/src/modules/muorb/krait/px4muorb_KraitRpcWrapper.cpp b/src/modules/muorb/krait/px4muorb_KraitRpcWrapper.cpp
index 00fd0e3286..b9fb8511e8 100644
--- a/src/modules/muorb/krait/px4muorb_KraitRpcWrapper.cpp
+++ b/src/modules/muorb/krait/px4muorb_KraitRpcWrapper.cpp
@@ -219,7 +219,7 @@ bool px4muorb::KraitRpcWrapper::Initialize()
 
 	int diff = (time_dsp - time_appsproc);
 
-	PX4_INFO("time_dsp: %llu us, time appsproc: %llu us, diff: %d us", time_dsp, time_appsproc, diff);
+	PX4_DEBUG("time_dsp: %llu us, time appsproc: %llu us, diff: %d us", time_dsp, time_appsproc, diff);
 
 	_Initialized = true;
 	return rc;

From 4827f0d93138a2e937e608895452ec11311b8ac2 Mon Sep 17 00:00:00 2001
From: Julian Oes <julian@oes.ch>
Date: Wed, 1 Jun 2016 14:03:22 +0100
Subject: [PATCH 0542/1230] sdlog2: removed a printf

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

diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c
index aef45b0377..3d63da3e2d 100644
--- a/src/modules/sdlog2/sdlog2.c
+++ b/src/modules/sdlog2/sdlog2.c
@@ -1110,7 +1110,7 @@ int sdlog2_thread_main(int argc, char *argv[])
 	}
 
 	/* initialize log buffer with specified size */
-	PX4_WARN("log buffer size: %i bytes", log_buffer_size);
+	PX4_DEBUG("log buffer size: %i bytes", log_buffer_size);
 
 	if (OK != logbuffer_init(&lb, log_buffer_size)) {
 		PX4_WARN("can't allocate log buffer, exiting");

From 93ada40bf96605aadce798e391f7f9772d4b1dba Mon Sep 17 00:00:00 2001
From: Julian Oes <julian@oes.ch>
Date: Wed, 1 Jun 2016 14:03:37 +0100
Subject: [PATCH 0543/1230] param_shmem: removed a printf

---
 src/modules/systemlib/param/param_shmem.c | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/src/modules/systemlib/param/param_shmem.c b/src/modules/systemlib/param/param_shmem.c
index 5803b292bb..c64d754536 100644
--- a/src/modules/systemlib/param/param_shmem.c
+++ b/src/modules/systemlib/param/param_shmem.c
@@ -938,7 +938,7 @@ param_load_default_no_notify(void)
 
 	close(fd_load);
 
-	PX4_INFO("param loading done");
+	PX4_DEBUG("param loading done");
 
 	if (result != 0) {
 		PX4_WARN("error reading parameters from '%s'", param_get_default_file());

From 9c9f22fd4050f59c52f08073eb8e1b8b08bada1f Mon Sep 17 00:00:00 2001
From: Julian Oes <julian@oes.ch>
Date: Wed, 1 Jun 2016 14:04:01 +0100
Subject: [PATCH 0544/1230] main: don't add a newline after every line

---
 src/platforms/posix/main.cpp | 2 --
 1 file changed, 2 deletions(-)

diff --git a/src/platforms/posix/main.cpp b/src/platforms/posix/main.cpp
index 278257fce6..2f86a4e00e 100644
--- a/src/platforms/posix/main.cpp
+++ b/src/platforms/posix/main.cpp
@@ -109,8 +109,6 @@ static void run_cmd(const vector<string> &appargs, bool exit_on_fail)
 
 		arg[i] = (char *)0;
 
-		cout << endl;
-
 		int retval = apps[command](i, (char **)arg);
 
 		if (retval) {

From 9079b5400140fde6a2acd32ab213475c03c56df2 Mon Sep 17 00:00:00 2001
From: Julian Oes <julian@oes.ch>
Date: Wed, 1 Jun 2016 14:04:58 +0100
Subject: [PATCH 0545/1230] POSIX: be less verbose on startup

---
 src/platforms/posix/px4_layer/px4_posix_impl.cpp | 8 +++-----
 1 file changed, 3 insertions(+), 5 deletions(-)

diff --git a/src/platforms/posix/px4_layer/px4_posix_impl.cpp b/src/platforms/posix/px4_layer/px4_posix_impl.cpp
index 6adeb39731..7f8a08a5d1 100644
--- a/src/platforms/posix/px4_layer/px4_posix_impl.cpp
+++ b/src/platforms/posix/px4_layer/px4_posix_impl.cpp
@@ -72,20 +72,19 @@ void init_once(void);
 void init_once(void)
 {
 	_shell_task_id = pthread_self();
-	printf("[init] shell id: %lu\n", (unsigned long)_shell_task_id);
+	//printf("[init] shell id: %lu\n", (unsigned long)_shell_task_id);
 	work_queues_init();
 	hrt_work_queue_init();
 	hrt_init();
 
 #ifdef CONFIG_SHMEM
-	PX4_INFO("Syncing params to shared memory\n");
+	PX4_DEBUG("Syncing params to shared memory\n");
 	init_params();
 #endif
 }
 
 void init(int argc, char *argv[], const char *app_name)
 {
-	printf("[init] task name: %s\n", app_name);
 	printf("\n");
 	printf("______  __   __    ___ \n");
 	printf("| ___ \\ \\ \\ / /   /   |\n");
@@ -94,8 +93,7 @@ void init(int argc, char *argv[], const char *app_name)
 	printf("| |     / /^\\ \\ \\___  |\n");
 	printf("\\_|     \\/   \\/     |_/\n");
 	printf("\n");
-	printf("Ready to fly.\n");
-	printf("\n");
+	printf("%s starting.\n", app_name);
 	printf("\n");
 
 	// set the threads name

From 51defb9de291422bca29570da71a9c5eb24c8a32 Mon Sep 17 00:00:00 2001
From: Julian Oes <julian@oes.ch>
Date: Wed, 1 Jun 2016 15:35:36 +0100
Subject: [PATCH 0546/1230] px4.config: remove unneeded commands

---
 posix-configs/eagle/flight/px4.config | 6 ------
 1 file changed, 6 deletions(-)

diff --git a/posix-configs/eagle/flight/px4.config b/posix-configs/eagle/flight/px4.config
index acf18e07a8..c22796cbe0 100644
--- a/posix-configs/eagle/flight/px4.config
+++ b/posix-configs/eagle/flight/px4.config
@@ -3,7 +3,6 @@ qshell start
 param set SYS_AUTOSTART 4001
 sleep 1
 gps start -d /dev/tty-4
-sleep 1
 param set MAV_TYPE 2
 df_hmc5883_wrapper start
 df_mpu9250_wrapper start
@@ -17,8 +16,3 @@ land_detector start multicopter
 mc_pos_control start
 mc_att_control start
 pwm_out_rc_in start -d /dev/tty-2
-sleep 1
-list_devices
-list_files
-list_tasks
-list_topics

From 9186a95b00d10b6e0eb3cb77566a5dee845597ce Mon Sep 17 00:00:00 2001
From: Julian Oes <julian@oes.ch>
Date: Wed, 1 Jun 2016 15:35:55 +0100
Subject: [PATCH 0547/1230] pwm_out_rc_in: say a tiny bit less

---
 src/drivers/pwm_out_rc_in/pwm_out_rc_in.cpp | 3 +--
 1 file changed, 1 insertion(+), 2 deletions(-)

diff --git a/src/drivers/pwm_out_rc_in/pwm_out_rc_in.cpp b/src/drivers/pwm_out_rc_in/pwm_out_rc_in.cpp
index ffecef0915..e7756c3d8f 100644
--- a/src/drivers/pwm_out_rc_in/pwm_out_rc_in.cpp
+++ b/src/drivers/pwm_out_rc_in/pwm_out_rc_in.cpp
@@ -168,7 +168,7 @@ int initialize_mixer(const char *mixer_filename)
 		}
 
 	} else {
-		PX4_WARN("Unable to open mixer config file, try default mixer");
+		PX4_WARN("No mixer config file found, using default mixer.");
 
 		/* Mixer file loading failed, fall back to default mixer configuration for
 		* QUAD_X airframe. */
@@ -186,7 +186,6 @@ int initialize_mixer(const char *mixer_filename)
 			return -1;
 		}
 
-		PX4_INFO("Successfully initialized default quad x mixer.");
 		return 0;
 	}
 }

From 5b4f01635724f117b520f14e80f5f35c7b29d289 Mon Sep 17 00:00:00 2001
From: Julian Oes <julian@oes.ch>
Date: Wed, 1 Jun 2016 15:38:33 +0100
Subject: [PATCH 0548/1230] qshell: swap INFO and DEBUG

---
 src/drivers/qshell/qurt/qshell_start_qurt.cpp | 14 +++++++-------
 1 file changed, 7 insertions(+), 7 deletions(-)

diff --git a/src/drivers/qshell/qurt/qshell_start_qurt.cpp b/src/drivers/qshell/qurt/qshell_start_qurt.cpp
index b769147817..ca6119fe54 100644
--- a/src/drivers/qshell/qurt/qshell_start_qurt.cpp
+++ b/src/drivers/qshell/qurt/qshell_start_qurt.cpp
@@ -56,17 +56,17 @@ int qshell_entry(int argc, char **argv)
 {
 	//px4::init(argc, argv, "qshell");
 
-	PX4_INFO("qshell entry.....");
+	PX4_DEBUG("qshell entry.....");
 	QShell qshell;
 	qshell.main();
 
-	PX4_INFO("goodbye");
+	PX4_DEBUG("goodbye");
 	return 0;
 }
 
 static void usage()
 {
-	PX4_DEBUG("usage: qshell {start|stop|status}");
+	PX4_INFO("usage: qshell {start|stop|status}");
 }
 int qshell_main(int argc, char *argv[])
 {
@@ -78,12 +78,12 @@ int qshell_main(int argc, char *argv[])
 	if (!strcmp(argv[1], "start")) {
 
 		if (QShell::appState.isRunning()) {
-			PX4_DEBUG("already running");
+			PX4_INFO("already running");
 			/* this is not an error */
 			return 0;
 		}
 
-		PX4_INFO("before starting the qshell_entry task");
+		PX4_DEBUG("before starting the qshell_entry task");
 
 		daemon_task = px4_task_spawn_cmd("qshell",
 						 SCHED_DEFAULT,
@@ -102,10 +102,10 @@ int qshell_main(int argc, char *argv[])
 
 	if (!strcmp(argv[1], "status")) {
 		if (QShell::appState.isRunning()) {
-			PX4_DEBUG("is running");
+			PX4_INFO("is running");
 
 		} else {
-			PX4_DEBUG("not started");
+			PX4_INFO("not started");
 		}
 
 		return 0;

From a88c3c2dbeb37ae734e970eadcfb54c19d026bef Mon Sep 17 00:00:00 2001
From: Julian Oes <julian@oes.ch>
Date: Wed, 1 Jun 2016 15:39:23 +0100
Subject: [PATCH 0549/1230] commander: printf fine tuning

---
 src/modules/commander/PreflightCheck.cpp | 2 +-
 src/modules/commander/commander.cpp      | 2 +-
 2 files changed, 2 insertions(+), 2 deletions(-)

diff --git a/src/modules/commander/PreflightCheck.cpp b/src/modules/commander/PreflightCheck.cpp
index 5fdd5b7f4c..3141d6d0b3 100644
--- a/src/modules/commander/PreflightCheck.cpp
+++ b/src/modules/commander/PreflightCheck.cpp
@@ -389,7 +389,7 @@ bool preflightCheck(orb_advert_t *mavlink_log_pub, bool checkMag, bool checkAcc,
 #ifdef __PX4_QURT
 	// WARNING: Preflight checks are important and should be added back when
 	// all the sensors are supported
-	PX4_WARN("WARNING: Preflight checks PASS always.");
+	PX4_WARN("Preflight checks always pass on Snapdragon.");
 	return true;
 #endif
 
diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp
index 4ddea3450d..6cf33daca2 100644
--- a/src/modules/commander/commander.cpp
+++ b/src/modules/commander/commander.cpp
@@ -3606,7 +3606,7 @@ void *commander_low_prio_loop(void *arg)
 					if (ret != OK) {
 						mavlink_and_console_log_critical(&mavlink_log_pub, "settings auto save error");
 					} else {
-						PX4_INFO("commander: settings saved.");
+						PX4_DEBUG("commander: settings saved.");
 					}
 
 					need_param_autosave = false;

From 11cc17b63a07e28d61d5d4f9af67bdb972e27f55 Mon Sep 17 00:00:00 2001
From: Julian Oes <julian@oes.ch>
Date: Wed, 1 Jun 2016 15:39:45 +0100
Subject: [PATCH 0550/1230] param_shmem: be less verbose

---
 src/modules/systemlib/param/param_shmem.c | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/src/modules/systemlib/param/param_shmem.c b/src/modules/systemlib/param/param_shmem.c
index c64d754536..5ff934b043 100644
--- a/src/modules/systemlib/param/param_shmem.c
+++ b/src/modules/systemlib/param/param_shmem.c
@@ -878,7 +878,7 @@ exit:
 	}
 
 	if (res == OK) {
-		PX4_INFO("saving params completed successfully");
+		PX4_DEBUG("saving params completed successfully");
 	}
 
 	return res;

From 6b1bcef644b37f67489fe0a9fa72c7940f53d8fe Mon Sep 17 00:00:00 2001
From: Julian Oes <julian@oes.ch>
Date: Wed, 1 Jun 2016 15:40:11 +0100
Subject: [PATCH 0551/1230] uORBManager: removed printfs

---
 src/modules/uORB/uORBManager.cpp | 16 ++++++++--------
 1 file changed, 8 insertions(+), 8 deletions(-)

diff --git a/src/modules/uORB/uORBManager.cpp b/src/modules/uORB/uORBManager.cpp
index d4d958f8e3..ce9ae40f8c 100644
--- a/src/modules/uORB/uORBManager.cpp
+++ b/src/modules/uORB/uORBManager.cpp
@@ -352,8 +352,8 @@ uORBCommunicator::IChannel *uORB::Manager::get_uorb_communicator(void)
 int16_t uORB::Manager::process_add_subscription(const char *messageName,
 		int32_t msgRateInHz)
 {
-	warnx("[posix-uORB::Manager::process_add_subscription(%d)] entering Manager_process_add_subscription: name: %s",
-	      __LINE__, messageName);
+	PX4_DEBUG("[posix-uORB::Manager::process_add_subscription(%d)] entering Manager_process_add_subscription: name: %s",
+		  __LINE__, messageName);
 	int16_t rc = 0;
 	_remote_subscriber_topics.insert(messageName);
 	char nodepath[orb_maxpath];
@@ -364,8 +364,8 @@ int16_t uORB::Manager::process_add_subscription(const char *messageName,
 		uORB::DeviceNode *node = uORB::DeviceMaster::GetDeviceNode(nodepath);
 
 		if (node == nullptr) {
-			warnx("[posix-uORB::Manager::process_add_subscription(%d)]DeviceNode(%s) not created yet",
-			      __LINE__, messageName);
+			PX4_DEBUG("[posix-uORB::Manager::process_add_subscription(%d)]DeviceNode(%s) not created yet",
+				  __LINE__, messageName);
 
 		} else {
 			// node is present.
@@ -396,8 +396,8 @@ int16_t uORB::Manager::process_remove_subscription(
 
 		// get the node name.
 		if (node == nullptr) {
-			warnx("[posix-uORB::Manager::process_remove_subscription(%d)]Error No existing subscriber found for message: [%s]",
-			      __LINE__, messageName);
+			PX4_DEBUG("[posix-uORB::Manager::process_remove_subscription(%d)]Error No existing subscriber found for message: [%s]",
+				  __LINE__, messageName);
 
 		} else {
 			// node is present.
@@ -425,8 +425,8 @@ int16_t uORB::Manager::process_received_message(const char *messageName,
 
 		// get the node name.
 		if (node == nullptr) {
-			warnx("[uORB::Manager::process_received_message(%d)]Error No existing subscriber found for message: [%s] nodepath:[%s]",
-			      __LINE__, messageName, nodepath);
+			PX4_DEBUG("[uORB::Manager::process_received_message(%d)]Error No existing subscriber found for message: [%s] nodepath:[%s]",
+				  __LINE__, messageName, nodepath);
 
 		} else {
 			// node is present.

From a381c6cea58919e41d118b1c7ef8d007182cc2d1 Mon Sep 17 00:00:00 2001
From: Julian Oes <julian@oes.ch>
Date: Wed, 1 Jun 2016 15:41:30 +0100
Subject: [PATCH 0552/1230] df_mpu9250_wrapper: removed a debug printf

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

diff --git a/src/platforms/posix/drivers/df_mpu9250_wrapper/df_mpu9250_wrapper.cpp b/src/platforms/posix/drivers/df_mpu9250_wrapper/df_mpu9250_wrapper.cpp
index 54b16d91b2..a730dd60be 100644
--- a/src/platforms/posix/drivers/df_mpu9250_wrapper/df_mpu9250_wrapper.cpp
+++ b/src/platforms/posix/drivers/df_mpu9250_wrapper/df_mpu9250_wrapper.cpp
@@ -288,7 +288,7 @@ int DfMpu9250Wrapper::start()
 		return ret;
 	}
 
-	PX4_INFO("MPU9250 device id is: %d", m_id.dev_id);
+	PX4_DEBUG("MPU9250 device id is: %d", m_id.dev_id);
 
 	/* Force getting the calibration values. */
 	_update_accel_calibration();

From 4cb073a48b4fffb6d0f344a1789ac1a1fc0d3920 Mon Sep 17 00:00:00 2001
From: Julian Oes <julian@oes.ch>
Date: Wed, 1 Jun 2016 15:42:00 +0100
Subject: [PATCH 0553/1230] POSIX main: get the newlines right

---
 src/platforms/posix/main.cpp | 5 +----
 1 file changed, 1 insertion(+), 4 deletions(-)

diff --git a/src/platforms/posix/main.cpp b/src/platforms/posix/main.cpp
index 2f86a4e00e..7b1c4dec7f 100644
--- a/src/platforms/posix/main.cpp
+++ b/src/platforms/posix/main.cpp
@@ -146,10 +146,6 @@ static void usage()
 
 static void process_line(string &line, bool exit_on_fail)
 {
-	if (line.length() == 0) {
-		printf("\n");
-	}
-
 	vector<string> appargs(10);
 
 	stringstream(line) >> appargs[0] >> appargs[1] >> appargs[2] >> appargs[3] >> appargs[4] >> appargs[5] >> appargs[6] >>
@@ -340,6 +336,7 @@ int main(int argc, char **argv)
 				}
 
 				process_line(mystr, false);
+				cout << endl;
 				mystr = "";
 				buf_ptr_read = buf_ptr_write;
 

From 7a29cf0f13a6b277b3c014ce263d70226c8c618f Mon Sep 17 00:00:00 2001
From: Julian Oes <julian@oes.ch>
Date: Wed, 1 Jun 2016 15:42:42 +0100
Subject: [PATCH 0554/1230] QURT main: less debug printfs

---
 src/platforms/qurt/px4_layer/main.cpp | 6 +++---
 1 file changed, 3 insertions(+), 3 deletions(-)

diff --git a/src/platforms/qurt/px4_layer/main.cpp b/src/platforms/qurt/px4_layer/main.cpp
index 6b424d8239..d4c316d553 100644
--- a/src/platforms/qurt/px4_layer/main.cpp
+++ b/src/platforms/qurt/px4_layer/main.cpp
@@ -82,7 +82,7 @@ static void run_cmd(map<string, px4_main_t> &apps, const vector<string> &appargs
 
 			while (i < appargs.size() && appargs[i].c_str()[0] != '\0') {
 				arg[i] = (char *)appargs[i].c_str();
-				PX4_WARN("  arg%d = '%s'\n", i, arg[i]);
+				PX4_DEBUG("  arg%d = '%s'\n", i, arg[i]);
 				++i;
 			}
 
@@ -128,10 +128,10 @@ static void process_commands(map<string, px4_main_t> &apps, const char *cmds)
 
 			// If we have a command to run
 			if (appargs.size() > 0) {
-				PX4_WARN("Processing command: %s", appargs[0].c_str());
+				PX4_DEBUG("Processing command: %s", appargs[0].c_str());
 
 				for (int ai = 1; ai < (int)appargs.size(); ai++) {
-					PX4_WARN("   > arg: %s", appargs[ai].c_str());
+					PX4_DEBUG("   > arg: %s", appargs[ai].c_str());
 				}
 
 				run_cmd(apps, appargs);

From 8e47f612fd02abbbd245b0697abe7b5bbb091c15 Mon Sep 17 00:00:00 2001
From: Julian Oes <julian@oes.ch>
Date: Wed, 1 Jun 2016 15:43:12 +0100
Subject: [PATCH 0555/1230] QURT main: more precise printfs

---
 src/platforms/qurt/px4_layer/main.cpp | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/src/platforms/qurt/px4_layer/main.cpp b/src/platforms/qurt/px4_layer/main.cpp
index d4c316d553..0e6953afdf 100644
--- a/src/platforms/qurt/px4_layer/main.cpp
+++ b/src/platforms/qurt/px4_layer/main.cpp
@@ -211,7 +211,7 @@ const char *get_commands()
 
 int dspal_entry(int argc, char *argv[])
 {
-	PX4_INFO("In main\n");
+	PX4_INFO("In dspal_entry");
 	map<string, px4_main_t> apps;
 	init_app_map(apps);
 	DriverFramework::Framework::initialize();
@@ -233,7 +233,7 @@ int dspal_entry(int argc, char *argv[])
 
 static void usage()
 {
-	PX4_WARN("Usage: dspal {start |stop}");
+	PX4_INFO("Usage: dspal {start |stop}");
 }
 
 

From a2865cd5c1e4b3b8690bdec93acb26e2bb2abf98 Mon Sep 17 00:00:00 2001
From: Julian Oes <julian@oes.ch>
Date: Wed, 1 Jun 2016 15:43:52 +0100
Subject: [PATCH 0556/1230] QURT px4_layer: removed debug printfs

---
 src/platforms/qurt/px4_layer/px4_qurt_impl.cpp  | 3 ---
 src/platforms/qurt/px4_layer/px4_qurt_tasks.cpp | 4 ++--
 2 files changed, 2 insertions(+), 5 deletions(-)

diff --git a/src/platforms/qurt/px4_layer/px4_qurt_impl.cpp b/src/platforms/qurt/px4_layer/px4_qurt_impl.cpp
index b905ac439c..e956b9da94 100644
--- a/src/platforms/qurt/px4_layer/px4_qurt_impl.cpp
+++ b/src/platforms/qurt/px4_layer/px4_qurt_impl.cpp
@@ -99,16 +99,13 @@ void init_once(void)
 {
 	// Required for QuRT
 	//_posix_init();
-	PX4_WARN("Before calling work_queue_init");
 
 //	_shell_task_id = pthread_self();
 //	PX4_INFO("Shell id is %lu", _shell_task_id);
 
 	work_queues_init();
-	PX4_WARN("Before calling hrt_init");
 	hrt_work_queue_init();
 	hrt_init();
-	PX4_WARN("after calling hrt_init");
 
 	/* Shared memory param sync*/
 	init_params();
diff --git a/src/platforms/qurt/px4_layer/px4_qurt_tasks.cpp b/src/platforms/qurt/px4_layer/px4_qurt_tasks.cpp
index 5f4ebc3ea3..ebd039b65d 100644
--- a/src/platforms/qurt/px4_layer/px4_qurt_tasks.cpp
+++ b/src/platforms/qurt/px4_layer/px4_qurt_tasks.cpp
@@ -197,11 +197,11 @@ px4_task_t px4_task_spawn_cmd(const char *name, int scheduler, int priority, int
 #endif
 	size_t fixed_stacksize = -1;
 	pthread_attr_getstacksize(&attr, &fixed_stacksize);
-	PX4_INFO("stack size: %d passed stacksize(%d)", fixed_stacksize, stack_size);
+	PX4_DEBUG("stack size: %d passed stacksize(%d)", fixed_stacksize, stack_size);
 	fixed_stacksize = 8 * 1024;
 	fixed_stacksize = (fixed_stacksize < (size_t)stack_size) ? (size_t)stack_size : fixed_stacksize;
 
-	PX4_INFO("setting the thread[%s] stack size to[%d]", name, fixed_stacksize);
+	PX4_DEBUG("setting the thread[%s] stack size to[%d]", name, fixed_stacksize);
 	pthread_attr_setstacksize(&attr, fixed_stacksize);
 
 	PX4_DEBUG("stack address after pthread_attr_setstacksize: 0x%X", attr.stackaddr);

From 953984dd48d3b425840e8346b728cfe4eab8bdc4 Mon Sep 17 00:00:00 2001
From: Julian Oes <julian@oes.ch>
Date: Wed, 1 Jun 2016 15:44:18 +0100
Subject: [PATCH 0557/1230] QURT px4_layer: getpid can shut up

---
 src/platforms/qurt/px4_layer/px4_qurt_tasks.cpp | 3 +++
 1 file changed, 3 insertions(+)

diff --git a/src/platforms/qurt/px4_layer/px4_qurt_tasks.cpp b/src/platforms/qurt/px4_layer/px4_qurt_tasks.cpp
index ebd039b65d..40897c8ff0 100644
--- a/src/platforms/qurt/px4_layer/px4_qurt_tasks.cpp
+++ b/src/platforms/qurt/px4_layer/px4_qurt_tasks.cpp
@@ -340,7 +340,10 @@ unsigned long px4_getpid()
 		}
 	}
 
+#ifndef __PX4_QURT
+	// XXX This doesn't seem to be working on QURT
 	PX4_ERR("px4_getpid() called from unknown thread context!");
+#endif
 	return ~0;
 }
 

From a6e46a7e6fb1652fa1e72e2cada20b2ce2c6a900 Mon Sep 17 00:00:00 2001
From: Julian Oes <julian@oes.ch>
Date: Wed, 1 Jun 2016 15:45:19 +0100
Subject: [PATCH 0558/1230] commander: whitespace

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

diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp
index 6cf33daca2..2fef8ddfbd 100644
--- a/src/modules/commander/commander.cpp
+++ b/src/modules/commander/commander.cpp
@@ -1161,7 +1161,7 @@ int commander_thread_main(int argc, char *argv[])
 		if (!strcmp(argv[2],"-hil")) {
 			startup_in_hil = true;
 		} else {
-			PX4_ERR("Argument %s not supported, abort.", argv[2]);	
+			PX4_ERR("Argument %s not supported, abort.", argv[2]);
 			thread_should_exit = true;
 		}
 	}

From 750b40f96284ee60968af4df627cb99e983a8b04 Mon Sep 17 00:00:00 2001
From: Julian Oes <julian@oes.ch>
Date: Wed, 1 Jun 2016 17:02:19 +0100
Subject: [PATCH 0559/1230] pwm_out_sim: don't complain without waiting

---
 src/drivers/pwm_out_sim/pwm_out_sim.cpp | 17 +++++------------
 1 file changed, 5 insertions(+), 12 deletions(-)

diff --git a/src/drivers/pwm_out_sim/pwm_out_sim.cpp b/src/drivers/pwm_out_sim/pwm_out_sim.cpp
index 320910fcfc..243c43dc84 100644
--- a/src/drivers/pwm_out_sim/pwm_out_sim.cpp
+++ b/src/drivers/pwm_out_sim/pwm_out_sim.cpp
@@ -414,21 +414,14 @@ PWMSim::task_main()
 			_current_update_rate = _update_rate;
 		}
 
-		/* sleep waiting for data, but no more than a second */
-		int ret = 0;
-
 		if (_poll_fds_num == 0) {
-			usleep(1000 * 1000);
-
-			/* this can happen during boot, but after the sleep its likely resolved */
-			if (_poll_fds_num == 0) {
-				PX4_WARN("pwm_out_sim: No valid fds");
-			}
-
-		} else {
-			ret = px4_poll(&_poll_fds[0], _poll_fds_num, 1000);
+			PX4_DEBUG("no valid fds");
+			continue;
 		}
 
+		/* sleep waiting for data, but no more than a second */
+		int ret = px4_poll(&_poll_fds[0], _poll_fds_num, 1000);
+
 		/* this would be bad... */
 		if (ret < 0) {
 			DEVICE_LOG("poll error %d", errno);

From cdd45a7b2ddeb3dcb5867e95d3e158702d48ca4e Mon Sep 17 00:00:00 2001
From: Julian Oes <julian@oes.ch>
Date: Thu, 2 Jun 2016 11:11:04 +0100
Subject: [PATCH 0560/1230] cmake: add define for module name

---
 cmake/common/px4_base.cmake | 5 ++++-
 1 file changed, 4 insertions(+), 1 deletion(-)

diff --git a/cmake/common/px4_base.cmake b/cmake/common/px4_base.cmake
index 1b721b1401..6c0f2abac1 100644
--- a/cmake/common/px4_base.cmake
+++ b/cmake/common/px4_base.cmake
@@ -303,6 +303,9 @@ function(px4_add_module)
 	if(MAIN)
 		set_target_properties(${MODULE} PROPERTIES
 			COMPILE_DEFINITIONS PX4_MAIN=${MAIN}_app_main)
+		add_definitions(-DMODULE_NAME="${MAIN}")
+	else()
+		add_definitions(-DMODULE_NAME="unknown")
 	endif()
 
 	if(INCLUDES)
@@ -400,7 +403,7 @@ function(px4_generate_messages)
 		list(APPEND msg_source_files_out ${msg_source_out_path}/${msg}.cpp)
 	endforeach()
 	add_custom_command(OUTPUT ${msg_source_files_out}
-		COMMAND ${PYTHON_EXECUTABLE} 
+		COMMAND ${PYTHON_EXECUTABLE}
 			Tools/px_generate_uorb_topic_files.py
 			--sources
 			${QUIET}

From 49930d64ad677af8f762d915ba4de9c22e06aeba Mon Sep 17 00:00:00 2001
From: Julian Oes <julian@oes.ch>
Date: Thu, 2 Jun 2016 15:11:43 +0100
Subject: [PATCH 0561/1230] romfs_pruner: delete hidden files, remove tabs

If a text editor creates hidden save files, those will get copied into
the ROMFS. This is now fixed by deleting hidden files.

Also, the there was some available potential by removing the leading
whitespace.
---
 Tools/px_romfs_pruner.py | 29 ++++++++++++++++++-----------
 1 file changed, 18 insertions(+), 11 deletions(-)

diff --git a/Tools/px_romfs_pruner.py b/Tools/px_romfs_pruner.py
index 5ac8fc2801..fa48019bd0 100644
--- a/Tools/px_romfs_pruner.py
+++ b/Tools/px_romfs_pruner.py
@@ -1,7 +1,7 @@
 #!/usr/bin/env python
 ############################################################################
 #
-#   Copyright (C) 2014-2015 PX4 Development Team. All rights reserved.
+#   Copyright (C) 2014-2016 PX4 Development Team. All rights reserved.
 
 # Redistribution and use in source and binary forms, with or without
 # modification, are permitted provided that the following conditions
@@ -35,7 +35,12 @@
 
 """
 px_romfs_pruner.py:
-Delete all comments and newlines before ROMFS is converted to an image
+Try to keep size of ROMFS minimal.
+
+This script goes through the temporarily copied ROMFS data and deletes all
+comments, empty lines and leading whitespace.
+It also deletes hidden files such as auto-saved backups that a text editor
+might have left in the tree.
 
 @author: Julian Oes <julian@oes.ch>
 """
@@ -53,18 +58,20 @@ def main():
                         help="ROMFS scratch folder.")
     args = parser.parse_args()
 
-    #print("Pruning ROMFS files.")
-
-    # go through
+    # go through temp folder
     for (root, dirs, files) in os.walk(args.folder):
         for file in files:
-            # only prune text files
-            if ".zip" in file or ".bin" in file or ".swp" in file \
-                    or ".data" in file or ".DS_Store" in file \
-                    or file.startswith("."):
+            file_path = os.path.join(root, file)
+
+            # delete hidden files
+            if file.startswith("."):
+                os.remove(file_path)
                 continue
 
-            file_path = os.path.join(root, file)
+            # only prune text files
+            if ".zip" in file or ".bin" in file or ".swp" in file \
+                    or ".data" in file or ".DS_Store" in file:
+                continue
 
             # read file line by line
             pruned_content = ""
@@ -77,7 +84,7 @@ def main():
                     else:
                         if not line.isspace() \
                                 and not line.strip().startswith("#"):
-                            pruned_content += line
+                            pruned_content += line.strip() + "\n"
             # overwrite old scratch file
             with open(file_path, "wb") as f:
                 pruned_content = re.sub("\r\n", "\n", pruned_content)

From f68a6eb42cde25a9c41592f5ea4975cca7464930 Mon Sep 17 00:00:00 2001
From: Julian Oes <julian@oes.ch>
Date: Thu, 2 Jun 2016 15:19:21 +0100
Subject: [PATCH 0562/1230] err/px4_log: switch everything to static function

Instead of having separate log functions for NuttX and POSIX, this now
switches everything to px4_log.h and PX4_INFO/WARN/ERR/DEBUG.

Also, the call mostly used is now a static inline function instead of a
macro which lead to a big increase in flash size for STM32.
---
 src/modules/systemlib/err.c |  3 ++
 src/modules/systemlib/err.h | 24 +++++++++++-----
 src/platforms/px4_log.h     | 57 +++++++++++++++++++++++++++++++------
 3 files changed, 69 insertions(+), 15 deletions(-)

diff --git a/src/modules/systemlib/err.c b/src/modules/systemlib/err.c
index b7b6f0a9c3..7765395256 100644
--- a/src/modules/systemlib/err.c
+++ b/src/modules/systemlib/err.c
@@ -60,6 +60,8 @@ extern int lib_lowvprintf(const char *fmt, va_list ap);
 #  warning Cannot output without one of CONFIG_NFILE_STREAMS or CONFIG_ARCH_LOWPUTC
 #endif
 
+// XXX not used anymore
+#if 0
 const char *
 getprogname(void)
 {
@@ -201,3 +203,4 @@ vwarnx(const char *fmt, va_list args)
 {
 	warnerr_core(NOCODE, fmt, args);
 }
+#endif
diff --git a/src/modules/systemlib/err.h b/src/modules/systemlib/err.h
index 49f181e9b2..036f7c8740 100644
--- a/src/modules/systemlib/err.h
+++ b/src/modules/systemlib/err.h
@@ -67,23 +67,33 @@
 
 #include <px4_log.h>
 #include <stdarg.h>
+#include <errno.h>
 #include "visibility.h"
 
 __BEGIN_DECLS
 
 __EXPORT const char *getprogname(void);
 
-#ifdef __PX4_POSIX
 
 #include <errno.h>
 #include <px4_tasks.h>
-#define err(eval, ...)					do { PX4_ERR(__VA_ARGS__); PX4_ERR("Task exited with errno=%i\n", errno); \
-		px4_task_exit(eval); } while(0)
-#define errx(eval, ...)				do { PX4_ERR(__VA_ARGS__); px4_task_exit(eval); } while(0)
-#define warn(...) 					PX4_WARN(__VA_ARGS__)
-#define warnx(...) 					PX4_WARN(__VA_ARGS__)
 
-#else
+#define err(eval, ...)		do { \
+					PX4_ERR(__VA_ARGS__); \
+					PX4_ERR("Task exited with errno=%i\n", errno); \
+					px4_task_exit(eval); } \
+				while(0)
+
+#define errx(eval, ...)		do { \
+					PX4_ERR(__VA_ARGS__); \
+					px4_task_exit(eval); \
+				} while(0)
+
+#define warn(...) 		PX4_WARN(__VA_ARGS__)
+#define warnx(...) 		PX4_WARN(__VA_ARGS__)
+
+// XXX not used anymore
+#if 0
 __EXPORT void	err(int eval, const char *fmt, ...)		__attribute__((noreturn, format(printf, 2, 3)));
 __EXPORT void	verr(int eval, const char *fmt, va_list)	__attribute__((noreturn, format(printf, 2, 0)));
 __EXPORT void	errc(int eval, int code, const char *fmt, ...)	__attribute__((noreturn, format(printf, 3, 4)));
diff --git a/src/platforms/px4_log.h b/src/platforms/px4_log.h
index 953b766f4d..ddb58e3b0a 100644
--- a/src/platforms/px4_log.h
+++ b/src/platforms/px4_log.h
@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (C) 2015 Mark Charlebois. All rights reserved.
+ *   Copyright (C) 2015-2016 PX4 Development Team. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -50,6 +50,7 @@ static inline void do_nothing(int level, ...)
 	(void)level;
 }
 
+
 /****************************************************************************
  * __px4_log_omit:
  * Compile out the message
@@ -150,17 +151,23 @@ __END_DECLS
 #define __px4__log_printcond(cond, ...)	    if (cond) printf(__VA_ARGS__)
 #define __px4__log_printline(level, ...)    if (level <= __px4_log_level_current) printf(__VA_ARGS__)
 
+
+#ifndef MODULE_NAME
+#define MODULE_NAME "Unknown"
+#endif
+
 #define __px4__log_timestamp_fmt	"%-10" PRIu64 " "
 #define __px4__log_timestamp_arg 	,hrt_absolute_time()
 #define __px4__log_level_fmt		"%-5s "
 #define __px4__log_level_arg(level)	,__px4_log_level_str[level]
 #define __px4__log_thread_fmt		"%#X "
 #define __px4__log_thread_arg		,(unsigned int)pthread_self()
+#define __px4__log_modulename_fmt	"%-10s "
+#define __px4__log_modulename_arg	,"[" MODULE_NAME "]"
 
 #define __px4__log_file_and_line_fmt 	" (file %s line %u)"
 #define __px4__log_file_and_line_arg 	, __FILE__, __LINE__
 #define __px4__log_end_fmt 		"\n"
-#define __px4__log_endline 		)
 
 /****************************************************************************
  * Output format macros
@@ -199,6 +206,40 @@ __END_DECLS
 			     __px4__log_level_arg(level), ##__VA_ARGS__\
 			    )
 
+/****************************************************************************
+ * __px4_log_modulename:
+ * Convert a message in the form:
+ * 	PX4_WARN("val is %d", val);
+ * to
+ * 	printf("%-5s [%-10s] val is %d\n", __px4_log_level_str[3],
+ *		MODULENAME, val);
+ ****************************************************************************/
+
+/* It turns out the macro below uses a lot more flash space than a static
+ * inline function. */
+#if 0
+#define __px4_log_modulename(level, FMT, ...) \
+	__px4__log_printline(level,\
+			     __px4__log_level_fmt\
+			     __px4__log_modulename_fmt\
+			     FMT\
+			     __px4__log_end_fmt\
+			     __px4__log_level_arg(level)\
+			     __px4__log_modulename_arg\
+			     , ##__VA_ARGS__\
+			    )
+#endif
+
+static inline void __px4_log_modulename(int level, const char *fmt, ...)
+{
+	if (level <= __px4_log_level_current) {
+		printf(__px4__log_level_fmt __px4__log_level_arg(level));
+		printf(__px4__log_modulename_fmt __px4__log_modulename_arg);
+		printf(fmt);
+		printf("\n");
+	}
+}
+
 /****************************************************************************
  * __px4_log_timestamp:
  * Convert a message in the form:
@@ -338,7 +379,7 @@ __END_DECLS
  * Messages that should never be filtered or compiled out
  ****************************************************************************/
 #define PX4_LOG(FMT, ...) 	__px4_log(_PX4_LOG_LEVEL_ALWAYS, FMT, ##__VA_ARGS__)
-#define PX4_INFO(FMT, ...) 	__px4_log(_PX4_LOG_LEVEL_ALWAYS, FMT, ##__VA_ARGS__)
+#define PX4_INFO(FMT, ...) 	__px4_log_modulename(_PX4_LOG_LEVEL_ALWAYS, FMT, ##__VA_ARGS__)
 
 #if defined(TRACE_BUILD)
 /****************************************************************************
@@ -362,8 +403,8 @@ __END_DECLS
 /****************************************************************************
  * Non-verbose settings for a Release build to minimize strings in build
  ****************************************************************************/
-#define PX4_PANIC(FMT, ...)	__px4_log(_PX4_LOG_LEVEL_PANIC, FMT, ##__VA_ARGS__)
-#define PX4_ERR(FMT, ...)	__px4_log(_PX4_LOG_LEVEL_ERROR, FMT, ##__VA_ARGS__)
+#define PX4_PANIC(FMT, ...)	__px4_log_modulename(_PX4_LOG_LEVEL_PANIC, FMT, ##__VA_ARGS__)
+#define PX4_ERR(FMT, ...)	__px4_log_modulename(_PX4_LOG_LEVEL_ERROR, FMT, ##__VA_ARGS__)
 #define PX4_WARN(FMT, ...) 	__px4_log_omit(_PX4_LOG_LEVEL_WARN,  FMT, ##__VA_ARGS__)
 #define PX4_DEBUG(FMT, ...) 	__px4_log_omit(_PX4_LOG_LEVEL_DEBUG, FMT, ##__VA_ARGS__)
 
@@ -371,9 +412,9 @@ __END_DECLS
 /****************************************************************************
  * Medium verbose settings for a default build
  ****************************************************************************/
-#define PX4_PANIC(FMT, ...)	__px4_log(_PX4_LOG_LEVEL_PANIC, FMT, ##__VA_ARGS__)
-#define PX4_ERR(FMT, ...)	__px4_log(_PX4_LOG_LEVEL_ERROR, FMT, ##__VA_ARGS__)
-#define PX4_WARN(FMT, ...) 	__px4_log(_PX4_LOG_LEVEL_WARN,  FMT, ##__VA_ARGS__)
+#define PX4_PANIC(FMT, ...)	__px4_log_modulename(_PX4_LOG_LEVEL_PANIC, FMT, ##__VA_ARGS__)
+#define PX4_ERR(FMT, ...)	__px4_log_modulename(_PX4_LOG_LEVEL_ERROR, FMT, ##__VA_ARGS__)
+#define PX4_WARN(FMT, ...) 	__px4_log_modulename(_PX4_LOG_LEVEL_WARN,  FMT, ##__VA_ARGS__)
 #define PX4_DEBUG(FMT, ...) 	__px4_log_omit(_PX4_LOG_LEVEL_DEBUG, FMT, ##__VA_ARGS__)
 
 #endif

From a6fd71853031867d27d4c38ca96d86723474a2d3 Mon Sep 17 00:00:00 2001
From: Julian Oes <julian@oes.ch>
Date: Thu, 2 Jun 2016 15:22:23 +0100
Subject: [PATCH 0563/1230] px4_log: use proper ROS printfs

---
 src/platforms/px4_log.h | 8 ++++----
 1 file changed, 4 insertions(+), 4 deletions(-)

diff --git a/src/platforms/px4_log.h b/src/platforms/px4_log.h
index ddb58e3b0a..565458b1e5 100644
--- a/src/platforms/px4_log.h
+++ b/src/platforms/px4_log.h
@@ -60,11 +60,11 @@ static inline void do_nothing(int level, ...)
 #if defined(__PX4_ROS)
 
 #include <ros/console.h>
-#define PX4_PANIC(...)	ROS_WARN(__VA_ARGS__)
-#define PX4_ERR(...)	ROS_WARN(__VA_ARGS__)
+#define PX4_PANIC(...)	ROS_FATAL(__VA_ARGS__)
+#define PX4_ERR(...)	ROS_ERROR(__VA_ARGS__)
 #define PX4_WARN(...) 	ROS_WARN(__VA_ARGS__)
-#define PX4_INFO(...) 	ROS_WARN(__VA_ARGS__)
-#define PX4_DEBUG(...)
+#define PX4_INFO(...) 	ROS_INFO(__VA_ARGS__)
+#define PX4_DEBUG(...)	ROS_DEBUG(__VA_ARGS__)
 #define PX4_BACKTRACE()
 
 #elif defined(__PX4_QURT)

From 6898e2dcf4bee9ecd300f98a0e6debf0e3e5ede3 Mon Sep 17 00:00:00 2001
From: Julian Oes <julian@oes.ch>
Date: Thu, 2 Jun 2016 15:22:39 +0100
Subject: [PATCH 0564/1230] commander: don't shout please

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

diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp
index 2fef8ddfbd..22e659ea41 100644
--- a/src/modules/commander/commander.cpp
+++ b/src/modules/commander/commander.cpp
@@ -1956,15 +1956,15 @@ int commander_thread_main(int argc, char *argv[])
 
 			if (was_landed != land_detector.landed) {
 				if (land_detector.landed) {
-					mavlink_and_console_log_info(&mavlink_log_pub, "LANDING DETECTED");
+					mavlink_and_console_log_info(&mavlink_log_pub, "Landing detected");
 				} else {
-					mavlink_and_console_log_info(&mavlink_log_pub, "TAKEOFF DETECTED");
+					mavlink_and_console_log_info(&mavlink_log_pub, "Takeoff detected");
 				}
 			}
 
 			if (was_falling != land_detector.freefall) {
 				if (land_detector.freefall) {
-					mavlink_and_console_log_info(&mavlink_log_pub, "FREEFALL DETECTED");
+					mavlink_and_console_log_info(&mavlink_log_pub, "Freefall detected");
 				}
 			}
 

From c916ee7357f668d231fa35783f14097499b7c172 Mon Sep 17 00:00:00 2001
From: Julian Oes <julian@oes.ch>
Date: Thu, 2 Jun 2016 15:24:09 +0100
Subject: [PATCH 0565/1230] err: astyle prefers it different

---
 src/modules/systemlib/err.h | 14 +++++++-------
 1 file changed, 7 insertions(+), 7 deletions(-)

diff --git a/src/modules/systemlib/err.h b/src/modules/systemlib/err.h
index 036f7c8740..d8f08fb4d5 100644
--- a/src/modules/systemlib/err.h
+++ b/src/modules/systemlib/err.h
@@ -79,15 +79,15 @@ __EXPORT const char *getprogname(void);
 #include <px4_tasks.h>
 
 #define err(eval, ...)		do { \
-					PX4_ERR(__VA_ARGS__); \
-					PX4_ERR("Task exited with errno=%i\n", errno); \
-					px4_task_exit(eval); } \
-				while(0)
+		PX4_ERR(__VA_ARGS__); \
+		PX4_ERR("Task exited with errno=%i\n", errno); \
+		px4_task_exit(eval); } \
+	while(0)
 
 #define errx(eval, ...)		do { \
-					PX4_ERR(__VA_ARGS__); \
-					px4_task_exit(eval); \
-				} while(0)
+		PX4_ERR(__VA_ARGS__); \
+		px4_task_exit(eval); \
+	} while(0)
 
 #define warn(...) 		PX4_WARN(__VA_ARGS__)
 #define warnx(...) 		PX4_WARN(__VA_ARGS__)

From de4ae580197f9d644d1d7d26aaf8d1d67caeeba4 Mon Sep 17 00:00:00 2001
From: Julian Oes <julian@oes.ch>
Date: Thu, 2 Jun 2016 15:56:22 +0100
Subject: [PATCH 0566/1230] px4_log: get the printf of fmt/args right

---
 src/platforms/px4_log.h | 7 ++++++-
 1 file changed, 6 insertions(+), 1 deletion(-)

diff --git a/src/platforms/px4_log.h b/src/platforms/px4_log.h
index 565458b1e5..f9e47196e2 100644
--- a/src/platforms/px4_log.h
+++ b/src/platforms/px4_log.h
@@ -123,6 +123,8 @@ static inline void do_nothing(int level, ...)
 #include <stdint.h>
 #include <sys/cdefs.h>
 #include <stdio.h>
+#include <stdarg.h>
+
 #include <px4_defines.h>
 
 __BEGIN_DECLS
@@ -235,7 +237,10 @@ static inline void __px4_log_modulename(int level, const char *fmt, ...)
 	if (level <= __px4_log_level_current) {
 		printf(__px4__log_level_fmt __px4__log_level_arg(level));
 		printf(__px4__log_modulename_fmt __px4__log_modulename_arg);
-		printf(fmt);
+		va_list argptr;
+		va_start(argptr, fmt);
+		vprintf(fmt, argptr);
+		va_end(argptr);
 		printf("\n");
 	}
 }

From be4819e8b170a79fe089ace5566b5aca717bb1b1 Mon Sep 17 00:00:00 2001
From: Julian Oes <julian@oes.ch>
Date: Thu, 2 Jun 2016 16:32:21 +0100
Subject: [PATCH 0567/1230] ecl: update submodule

This moves printfs to PX4_INFOs.
---
 src/lib/ecl | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/src/lib/ecl b/src/lib/ecl
index 654093cae1..ab0ac05b47 160000
--- a/src/lib/ecl
+++ b/src/lib/ecl
@@ -1 +1 @@
-Subproject commit 654093cae1af0f76fa1ebf42a706d69043476f95
+Subproject commit ab0ac05b47f63401833da650984accde9b6dab05

From b24eded7a00e52afaa695d71e38eedc7ac10066a Mon Sep 17 00:00:00 2001
From: Julian Oes <julian@oes.ch>
Date: Thu, 2 Jun 2016 16:43:25 +0100
Subject: [PATCH 0568/1230] cmake: use module name if no main is available

---
 cmake/common/px4_base.cmake | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/cmake/common/px4_base.cmake b/cmake/common/px4_base.cmake
index 6c0f2abac1..014fdda23a 100644
--- a/cmake/common/px4_base.cmake
+++ b/cmake/common/px4_base.cmake
@@ -305,7 +305,7 @@ function(px4_add_module)
 			COMPILE_DEFINITIONS PX4_MAIN=${MAIN}_app_main)
 		add_definitions(-DMODULE_NAME="${MAIN}")
 	else()
-		add_definitions(-DMODULE_NAME="unknown")
+		add_definitions(-DMODULE_NAME="${MODULE}")
 	endif()
 
 	if(INCLUDES)

From c87f9a1f8a8cbb61220082996b7b822351fe3a03 Mon Sep 17 00:00:00 2001
From: Julian Oes <julian@oes.ch>
Date: Thu, 2 Jun 2016 17:25:02 +0100
Subject: [PATCH 0569/1230] init: make echo consistent with printfs

---
 ROMFS/px4fmu_common/init.d/rc.interface | 16 ++++----
 ROMFS/px4fmu_common/init.d/rcS          | 54 ++++++++++++-------------
 2 files changed, 35 insertions(+), 35 deletions(-)

diff --git a/ROMFS/px4fmu_common/init.d/rc.interface b/ROMFS/px4fmu_common/init.d/rc.interface
index a8c17ad2c0..3ec0cd0d95 100644
--- a/ROMFS/px4fmu_common/init.d/rc.interface
+++ b/ROMFS/px4fmu_common/init.d/rc.interface
@@ -45,10 +45,10 @@ then
 
 	if mixer load $OUTPUT_DEV $MIXER_FILE
 	then
-		echo "[i] Mixer: $MIXER_FILE on $OUTPUT_DEV"
+		echo "INFO  [init] Mixer: $MIXER_FILE on $OUTPUT_DEV"
 	else
-		echo "[i] Error loading mixer: $MIXER_FILE"
-		echo "ERROR: Could not load mixer: $MIXER_FILE" >> $LOG_FILE
+		echo "ERROR [init] Error loading mixer: $MIXER_FILE"
+		echo "ERROR:[init] Could not load mixer: $MIXER_FILE" >> $LOG_FILE
 		tone_alarm $TUNE_ERR
 	fi
 
@@ -56,8 +56,8 @@ then
 else
 	if [ $MIXER != skip ]
 	then
-		echo "[i] Mixer not defined"
-		echo "ERROR: Mixer not defined" >> $LOG_FILE
+		echo "ERROR [init] Mixer not defined"
+		echo "ERROR [init] Mixer not defined" >> $LOG_FILE
 		tone_alarm $TUNE_ERR
 	fi
 fi
@@ -137,10 +137,10 @@ then
 			then
 				if mixer load $OUTPUT_AUX_DEV $MIXER_AUX_FILE
 				then
-					echo "[i] Mixer: $MIXER_AUX_FILE on $OUTPUT_AUX_DEV"
+					echo "INFO  [init] Mixer: $MIXER_AUX_FILE on $OUTPUT_AUX_DEV"
 				else
-					echo "[i] Error loading mixer: $MIXER_AUX_FILE"
-					echo "ERROR: Could not load mixer: $MIXER_AUX_FILE" >> $LOG_FILE
+					echo "ERROR [init] Error loading mixer: $MIXER_AUX_FILE"
+					echo "ERROR [init] Could not load mixer: $MIXER_AUX_FILE" >> $LOG_FILE
 				fi
 			else
 				set PWM_AUX_OUT none
diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS
index 7b01c11c78..dda3ffdd79 100644
--- a/ROMFS/px4fmu_common/init.d/rcS
+++ b/ROMFS/px4fmu_common/init.d/rcS
@@ -32,9 +32,9 @@ else
 	then
 		if mount -t vfat /dev/mmcsd0 /fs/microsd
 		then
-			echo "[i] microSD card formatted"
+			echo "INFO  [init] MicroSD card formatted"
 		else
-			echo "[i] format failed"
+			echo "ERROR [init] Format failed"
 			tone_alarm MNBG
 			set LOG_FILE /dev/null
 		fi
@@ -50,7 +50,7 @@ fi
 set FRC /fs/microsd/etc/rc.txt
 if [ -f $FRC ]
 then
-	echo "[i] Executing script: $FRC"
+	echo "INFO  [init] Executing script: $FRC"
 	sh $FRC
 	set MODE custom
 fi
@@ -173,7 +173,7 @@ then
 	#
 	if param compare SYS_AUTOSTART 0
 	then
-		echo "[i] No autostart"
+		echo "INFO  [init] No autostart"
 	else
 		sh /etc/init.d/rc.autostart
 	fi
@@ -192,7 +192,7 @@ then
 	set FCONFIG /fs/microsd/etc/config.txt
 	if [ -f $FCONFIG ]
 	then
-		echo "[i] Custom: $FCONFIG"
+		echo "INFO  [init] Custom: $FCONFIG"
 		sh $FCONFIG
 	fi
 	unset FCONFIG
@@ -223,7 +223,7 @@ then
 
 		if px4io checkcrc ${IO_FILE}
 		then
-			echo "PX4IO CRC OK" >> $LOG_FILE
+			echo "INFO  [init] PX4IO CRC OK" >> $LOG_FILE
 
 			set IO_PRESENT yes
 		else
@@ -246,16 +246,16 @@ then
 				usleep 500000
 				if px4io checkcrc $IO_FILE
 				then
-					echo "PX4IO CRC OK after updating" >> $LOG_FILE
+					echo "INFO  [init] PX4IO CRC OK after updating" >> $LOG_FILE
 					tone_alarm MLL8CDE
 
 					set IO_PRESENT yes
 				else
-					echo "PX4IO update failed" >> $LOG_FILE
+					echo "ERROR [init] PX4IO update failed" >> $LOG_FILE
 					tone_alarm $TUNE_ERR
 				fi
 			else
-				echo "PX4IO update failed" >> $LOG_FILE
+				echo "ERROR [init] PX4IO update failed" >> $LOG_FILE
 				tone_alarm $TUNE_ERR
 			fi
 		fi
@@ -263,7 +263,7 @@ then
 
 		if [ $IO_PRESENT == no ]
 		then
-			echo "ERROR: PX4IO not found" >> $LOG_FILE
+			echo "ERROR [init] PX4IO not found" >> $LOG_FILE
 			tone_alarm $TUNE_ERR
 		fi
 	fi
@@ -355,7 +355,7 @@ then
 		then
 			if param compare UAVCAN_ENABLE 0
 			then
-				echo "[i] OVERRIDING UAVCAN_ENABLE = 1"
+				echo "INFO  [init] OVERRIDING UAVCAN_ENABLE = 1" >> $LOG_FILE
 				param set UAVCAN_ENABLE 1
 			fi
 		fi
@@ -366,7 +366,7 @@ then
 			then
 				sh /etc/init.d/rc.io
 			else
-				echo "PX4IO start failed" >> $LOG_FILE
+				echo "INFO  [init] PX4IO start failed" >> $LOG_FILE
 				tone_alarm $TUNE_ERR
 			fi
 		fi
@@ -376,7 +376,7 @@ then
 			if fmu mode_$FMU_MODE
 			then
 			else
-				echo "FMU start failed" >> $LOG_FILE
+				echo "ERR  [init] FMU start failed" >> $LOG_FILE
 				tone_alarm $TUNE_ERR
 			fi
 
@@ -408,7 +408,7 @@ then
 			if mkblctrl $MKBLCTRL_ARG
 			then
 			else
-				echo "ERROR: MK start failed" >> $LOG_FILE
+				echo "ERROR [init] MK start failed" >> $LOG_FILE
 				tone_alarm $TUNE_ERR
 			fi
 			unset MKBLCTRL_ARG
@@ -420,7 +420,7 @@ then
 			if pwm_out_sim mode_port2_pwm8
 			then
 			else
-				echo "PWM SIM start failed" >> $LOG_FILE
+				echo "ERROR [init] PWM SIM start failed" >> $LOG_FILE
 				tone_alarm $TUNE_ERR
 			fi
 		fi
@@ -436,7 +436,7 @@ then
 				then
 					sh /etc/init.d/rc.io
 				else
-					echo "PX4IO start failed" >> $LOG_FILE
+					echo "ERROR [init] PX4IO start failed" >> $LOG_FILE
 					tone_alarm $TUNE_ERR
 				fi
 			fi
@@ -446,7 +446,7 @@ then
 				if fmu mode_$FMU_MODE
 				then
 				else
-					echo "FMU mode_$FMU_MODE start failed" >> $LOG_FILE
+					echo "ERROR [init] FMU mode_$FMU_MODE start failed" >> $LOG_FILE
 					tone_alarm $TUNE_ERR
 				fi
 
@@ -658,7 +658,7 @@ then
 	#
 	if [ $VEHICLE_TYPE == fw ]
 	then
-		echo "FIXED WING"
+		echo "INFO  [init] Fixedwing"
 
 		if [ $MIXER == none ]
 		then
@@ -686,11 +686,11 @@ then
 	#
 	if [ $VEHICLE_TYPE == mc ]
 	then
-		echo "MULTICOPTER"
+		echo "INFO  [init] Multicopter"
 
 		if [ $MIXER == none ]
 		then
-			echo "Mixer undefined"
+			echo "INFO  [init] Mixer undefined"
 		fi
 
 		if [ $MAV_TYPE == none ]
@@ -737,7 +737,7 @@ then
 		# Still no MAV_TYPE found
 		if [ $MAV_TYPE == none ]
 		then
-			echo "Unknown MAV_TYPE"
+			echo "WARN  [init] Unknown MAV_TYPE"
 			param set MAV_TYPE 2
 		else
 			param set MAV_TYPE $MAV_TYPE
@@ -755,11 +755,11 @@ then
 	#
 	if [ $VEHICLE_TYPE == vtol ]
 	then
-		echo "VTOL"
+		echo "INFO  [init] VTOL"
 
 		if [ $MIXER == none ]
 		then
-			echo "VTOL mixer undefined"
+			echo "WARN  [init] VTOL mixer undefined"
 		fi
 
 		if [ $MAV_TYPE == none ]
@@ -782,7 +782,7 @@ then
 		# Still no MAV_TYPE found
 		if [ $MAV_TYPE == none ]
 		then
-			echo "Unknown MAV_TYPE"
+			echo "WARN  [init] Unknown MAV_TYPE"
 			param set MAV_TYPE 19
 		else
 			param set MAV_TYPE $MAV_TYPE
@@ -866,14 +866,14 @@ then
 	#
 	if [ $VEHICLE_TYPE == none ]
 	then
-		echo "[i] No autostart ID found"
+		echo "WARN  [init] No autostart ID found"
 	fi
 
 	# Start any custom addons
 	set FEXTRAS /fs/microsd/etc/extras.txt
 	if [ -f $FEXTRAS ]
 	then
-		echo "[i] Addons script: $FEXTRAS"
+		echo "INFO  [init] Addons script: $FEXTRAS"
 		sh $FEXTRAS
 	fi
 	unset FEXTRAS
@@ -897,7 +897,7 @@ mavlink boot_complete
 
 if [ $EXIT_ON_END == yes ]
 then
-	echo "NSH EXIT"
+	echo "INFO  [init] NSH exit"
 	exit
 fi
 unset EXIT_ON_END

From 4a6b845ef635812dc057efad07cfcec406c01969 Mon Sep 17 00:00:00 2001
From: Julian Oes <julian@oes.ch>
Date: Thu, 2 Jun 2016 17:25:27 +0100
Subject: [PATCH 0570/1230] px4io: nicer printf

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

diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp
index 61335a3eeb..e3c1b1e6d3 100644
--- a/src/drivers/px4io/px4io.cpp
+++ b/src/drivers/px4io/px4io.cpp
@@ -894,7 +894,7 @@ PX4IO::init()
 	ret = register_driver(PWM_OUTPUT0_DEVICE_PATH, &fops, 0666, (void *)this);
 
 	if (ret == OK) {
-		DEVICE_LOG("default PWM output device");
+		PX4_INFO("default PWM output device");
 		_primary_pwm_device = true;
 	}
 

From 8d491077cef680e5e470e71a69ac110fbbda57b0 Mon Sep 17 00:00:00 2001
From: Julian Oes <julian@oes.ch>
Date: Thu, 2 Jun 2016 17:25:45 +0100
Subject: [PATCH 0571/1230] adc: removed unneeded printf

---
 src/drivers/stm32/adc/adc.cpp | 2 --
 1 file changed, 2 deletions(-)

diff --git a/src/drivers/stm32/adc/adc.cpp b/src/drivers/stm32/adc/adc.cpp
index 43d2dad252..3151f5ac3a 100644
--- a/src/drivers/stm32/adc/adc.cpp
+++ b/src/drivers/stm32/adc/adc.cpp
@@ -251,8 +251,6 @@ ADC::init()
 	}
 
 
-	DEVICE_DEBUG("init done");
-
 	/* create the device node */
 	return CDev::init();
 }

From 0acf0b3c69686bab9df7c6b57c74620b83745a03 Mon Sep 17 00:00:00 2001
From: Julian Oes <julian@oes.ch>
Date: Thu, 2 Jun 2016 17:26:23 +0100
Subject: [PATCH 0572/1230] param: be less verbose on startup

---
 src/systemcmds/param/param.c | 6 +++---
 1 file changed, 3 insertions(+), 3 deletions(-)

diff --git a/src/systemcmds/param/param.c b/src/systemcmds/param/param.c
index 12e7cca9a2..f3c4172b19 100644
--- a/src/systemcmds/param/param.c
+++ b/src/systemcmds/param/param.c
@@ -525,7 +525,7 @@ do_compare(const char *name, char *vals[], unsigned comparisons, enum COMPARE_OP
 
 				if (((cmp_op == COMPARE_OPERATOR_EQUAL) && (i == j)) ||
 				    ((cmp_op == COMPARE_OPERATOR_GREATER) && (i > j))) {
-					printf(" %ld: ", (long)i);
+					PX4_DEBUG(" %ld: ", (long)i);
 					ret = 0;
 				}
 			}
@@ -545,7 +545,7 @@ do_compare(const char *name, char *vals[], unsigned comparisons, enum COMPARE_OP
 
 				if (((cmp_op == COMPARE_OPERATOR_EQUAL) && (fabsf(f - g) < 1e-7f)) ||
 				    ((cmp_op == COMPARE_OPERATOR_GREATER) && (f > g))) {
-					printf(" %4.4f: ", (double)f);
+					PX4_DEBUG(" %4.4f: ", (double)f);
 					ret = 0;
 				}
 			}
@@ -559,7 +559,7 @@ do_compare(const char *name, char *vals[], unsigned comparisons, enum COMPARE_OP
 	}
 
 	if (ret == 0) {
-		printf("%c %s: match\n",
+		PX4_DEBUG("%c %s: match\n",
 		       param_value_unsaved(param) ? '*' : (param_value_is_default(param) ? ' ' : '+'),
 		       param_name(param));
 	}

From 5f3332e429cba1292b4253500a816c2b5395792d Mon Sep 17 00:00:00 2001
From: Julian Oes <julian@oes.ch>
Date: Thu, 2 Jun 2016 17:26:44 +0100
Subject: [PATCH 0573/1230] ver: less verbose on startup

---
 src/systemcmds/ver/ver.c | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/src/systemcmds/ver/ver.c b/src/systemcmds/ver/ver.c
index df041f018f..f4d34719b0 100644
--- a/src/systemcmds/ver/ver.c
+++ b/src/systemcmds/ver/ver.c
@@ -135,7 +135,7 @@ int ver_main(int argc, char *argv[])
 					ret = strncmp(HW_ARCH, argv[2], strlen(HW_ARCH));
 
 					if (ret == 0) {
-						printf("ver hwcmp match: %s\n", HW_ARCH);
+						PX4_INFO("match: %s", HW_ARCH);
 					}
 
 					return ret;

From f24ca14122d5d25f64a4a63d1ba54a2663f9160d Mon Sep 17 00:00:00 2001
From: Julian Oes <julian@oes.ch>
Date: Thu, 2 Jun 2016 17:27:16 +0100
Subject: [PATCH 0574/1230] param: whitespace

---
 src/systemcmds/param/param.c | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/src/systemcmds/param/param.c b/src/systemcmds/param/param.c
index f3c4172b19..ea698ae5a1 100644
--- a/src/systemcmds/param/param.c
+++ b/src/systemcmds/param/param.c
@@ -560,8 +560,8 @@ do_compare(const char *name, char *vals[], unsigned comparisons, enum COMPARE_OP
 
 	if (ret == 0) {
 		PX4_DEBUG("%c %s: match\n",
-		       param_value_unsaved(param) ? '*' : (param_value_is_default(param) ? ' ' : '+'),
-		       param_name(param));
+			  param_value_unsaved(param) ? '*' : (param_value_is_default(param) ? ' ' : '+'),
+			  param_name(param));
 	}
 
 	return ret;

From 01f6d713b4478c5db022b38e6e8b0e0de3bf1495 Mon Sep 17 00:00:00 2001
From: Julian Oes <julian@oes.ch>
Date: Fri, 3 Jun 2016 14:00:34 +0100
Subject: [PATCH 0575/1230] pwm_out_sim: use PX4_INFO

---
 src/drivers/pwm_out_sim/pwm_out_sim.cpp | 20 ++++++++++----------
 1 file changed, 10 insertions(+), 10 deletions(-)

diff --git a/src/drivers/pwm_out_sim/pwm_out_sim.cpp b/src/drivers/pwm_out_sim/pwm_out_sim.cpp
index 243c43dc84..d3cb5b2542 100644
--- a/src/drivers/pwm_out_sim/pwm_out_sim.cpp
+++ b/src/drivers/pwm_out_sim/pwm_out_sim.cpp
@@ -260,7 +260,7 @@ PWMSim::init()
 				   nullptr);
 
 	if (_task < 0) {
-		DEVICE_DEBUG("task start failed: %d", errno);
+		PX4_INFO("task start failed: %d", errno);
 		return -errno;
 	}
 
@@ -285,42 +285,42 @@ PWMSim::set_mode(Mode mode)
 	 */
 	switch (mode) {
 	case MODE_2PWM:
-		DEVICE_DEBUG("MODE_2PWM");
+		PX4_INFO("MODE_2PWM");
 		/* multi-port with flow control lines as PWM */
 		_update_rate = 50;	/* default output rate */
 		_num_outputs = 2;
 		break;
 
 	case MODE_4PWM:
-		DEVICE_DEBUG("MODE_4PWM");
+		PX4_INFO("MODE_4PWM");
 		/* multi-port as 4 PWM outs */
 		_update_rate = 50;	/* default output rate */
 		_num_outputs = 4;
 		break;
 
 	case MODE_8PWM:
-		DEVICE_DEBUG("MODE_8PWM");
+		PX4_INFO("MODE_8PWM");
 		/* multi-port as 8 PWM outs */
 		_update_rate = 50;	/* default output rate */
 		_num_outputs = 8;
 		break;
 
 	case MODE_12PWM:
-		DEVICE_DEBUG("MODE_12PWM");
+		PX4_INFO("MODE_12PWM");
 		/* multi-port as 12 PWM outs */
 		_update_rate = 50;	/* default output rate */
 		_num_outputs = 12;
 		break;
 
 	case MODE_16PWM:
-		DEVICE_DEBUG("MODE_16PWM");
+		PX4_INFO("MODE_16PWM");
 		/* multi-port as 16 PWM outs */
 		_update_rate = 50;	/* default output rate */
 		_num_outputs = 16;
 		break;
 
 	case MODE_NONE:
-		DEVICE_DEBUG("MODE_NONE");
+		PX4_INFO("MODE_NONE");
 		/* disable servo outputs and set a very low update rate */
 		_update_rate = 10;
 		_num_outputs = 0;
@@ -575,7 +575,7 @@ PWMSim::ioctl(device::file_t *filp, int cmd, unsigned long arg)
 
 	default:
 		ret = -ENOTTY;
-		DEVICE_DEBUG("not in a PWM mode");
+		PX4_INFO("not in a PWM mode");
 		break;
 	}
 
@@ -789,7 +789,7 @@ PWMSim::pwm_ioctl(device::file_t *filp, int cmd, unsigned long arg)
 				ret = _mixers->load_from_buf(buf, buflen);
 
 				if (ret != 0) {
-					DEVICE_DEBUG("mixer load failed with %d", ret);
+					PX4_ERR("mixer load failed with %d", ret);
 					delete _mixers;
 					_mixers = nullptr;
 					_groups_required = 0;
@@ -949,7 +949,7 @@ extern "C" __EXPORT int pwm_out_sim_main(int argc, char *argv[]);
 static void
 usage()
 {
-	PX4_WARN("pwm_out_sim: unrecognized command, try:");
+	PX4_WARN("unrecognized command, try:");
 	PX4_WARN("  mode_pwm, mode_gpio_serial, mode_pwm_serial, mode_pwm_gpio, mode_port2_pwm8, mode_port2_pwm12, mode_port2_pwm16");
 }
 

From 69bb60229105955f8eb8946586562024c234cbb4 Mon Sep 17 00:00:00 2001
From: Julian Oes <julian@oes.ch>
Date: Fri, 3 Jun 2016 14:03:29 +0100
Subject: [PATCH 0576/1230] gyrosim: be less verbose

---
 src/platforms/posix/drivers/gyrosim/gyrosim.cpp | 8 ++++----
 1 file changed, 4 insertions(+), 4 deletions(-)

diff --git a/src/platforms/posix/drivers/gyrosim/gyrosim.cpp b/src/platforms/posix/drivers/gyrosim/gyrosim.cpp
index 012db15942..6fb35a7dbe 100644
--- a/src/platforms/posix/drivers/gyrosim/gyrosim.cpp
+++ b/src/platforms/posix/drivers/gyrosim/gyrosim.cpp
@@ -388,7 +388,7 @@ GYROSIM::~GYROSIM()
 int
 GYROSIM::init()
 {
-	PX4_INFO("GYROSIM::init");
+	PX4_DEBUG("init");
 	int ret = 1;
 
 	struct accel_report arp = {};
@@ -535,7 +535,7 @@ GYROSIM::transfer(uint8_t *send, uint8_t *recv, unsigned len)
 void
 GYROSIM::_set_sample_rate(unsigned desired_sample_rate_hz)
 {
-	PX4_INFO("GYROSIM::_set_sample_rate %u Hz", desired_sample_rate_hz);
+	PX4_DEBUG("_set_sample_rate %u Hz", desired_sample_rate_hz);
 
 	if (desired_sample_rate_hz == 0 ||
 	    desired_sample_rate_hz == GYRO_SAMPLERATE_DEFAULT ||
@@ -554,7 +554,7 @@ GYROSIM::_set_sample_rate(unsigned desired_sample_rate_hz)
 	write_reg(MPUREG_SMPLRT_DIV, div - 1);
 
 	unsigned sample_rate = 1000 / div;
-	PX4_INFO("GYROSIM: Changed sample rate to %uHz", sample_rate);
+	PX4_DEBUG("Changed sample rate to %uHz", sample_rate);
 	setSampleInterval(1000000 / sample_rate);
 	_gyro->setSampleInterval(1000000 / sample_rate);
 }
@@ -1213,7 +1213,7 @@ int
 GYROSIM_gyro::init()
 {
 	int ret = VirtDevObj::init();
-	PX4_INFO("GYROSIM_gyro::init base class ret: %d", ret);
+	PX4_DEBUG("GYROSIM_gyro::init base class ret: %d", ret);
 	return ret;
 }
 

From 5e4e896dad12789c8b816ece8170cfb2b9f6a9ed Mon Sep 17 00:00:00 2001
From: Julian Oes <julian@oes.ch>
Date: Fri, 3 Jun 2016 14:03:46 +0100
Subject: [PATCH 0577/1230] tone_alrmsim: add missing string for home set tone

---
 src/platforms/posix/drivers/tonealrmsim/tone_alarm.cpp | 1 +
 1 file changed, 1 insertion(+)

diff --git a/src/platforms/posix/drivers/tonealrmsim/tone_alarm.cpp b/src/platforms/posix/drivers/tonealrmsim/tone_alarm.cpp
index cc05e73fcb..44f4168c4a 100644
--- a/src/platforms/posix/drivers/tonealrmsim/tone_alarm.cpp
+++ b/src/platforms/posix/drivers/tonealrmsim/tone_alarm.cpp
@@ -249,6 +249,7 @@ ToneAlarm::ToneAlarm() :
 	_tune_names[TONE_EKF_WARNING_TUNE] = "ekf_warning";				// ekf warning
 	_tune_names[TONE_BARO_WARNING_TUNE] = "baro_warning";			// baro warning
 	_tune_names[TONE_SINGLE_BEEP_TUNE] = "beep";                    // single beep
+	_tune_names[TONE_HOME_SET] = "home_set";
 }
 
 ToneAlarm::~ToneAlarm()

From a548b5bd5e4913b9800205a472b0a7db11466154 Mon Sep 17 00:00:00 2001
From: Julian Oes <julian@oes.ch>
Date: Fri, 3 Jun 2016 14:04:14 +0100
Subject: [PATCH 0578/1230] tonealrmsim: better output of tone_alarm in SITL

---
 src/platforms/posix/drivers/tonealrmsim/tone_alarm.cpp | 3 +--
 1 file changed, 1 insertion(+), 2 deletions(-)

diff --git a/src/platforms/posix/drivers/tonealrmsim/tone_alarm.cpp b/src/platforms/posix/drivers/tonealrmsim/tone_alarm.cpp
index 44f4168c4a..961ece334d 100644
--- a/src/platforms/posix/drivers/tonealrmsim/tone_alarm.cpp
+++ b/src/platforms/posix/drivers/tonealrmsim/tone_alarm.cpp
@@ -657,8 +657,6 @@ ToneAlarm::devIOCTL(unsigned long cmd, unsigned long arg)
 	/* decide whether to increase the alarm level to cmd or leave it alone */
 	switch (cmd) {
 	case TONE_SET_ALARM:
-		PX4_INFO("TONE_SET_ALARM %lu", arg);
-
 		if (arg < TONE_NUMBER_OF_TUNES) {
 			if (arg == TONE_STOP_TUNE) {
 				// stop the tune
@@ -673,6 +671,7 @@ ToneAlarm::devIOCTL(unsigned long cmd, unsigned long arg)
 					/* play the selected tune */
 					_default_tune_number = arg;
 					start_tune(_default_tunes[arg]);
+					PX4_INFO("%s", _tune_names[arg]);
 				}
 			}
 

From 04083cff3d9aee6a4178783a928969de1a370eab Mon Sep 17 00:00:00 2001
From: Julian Oes <julian@oes.ch>
Date: Tue, 7 Jun 2016 09:41:57 +0200
Subject: [PATCH 0579/1230] err: we need to use exit on NuttX

px4_task_exit doesn't seem to be available for NuttX, so it had no
effect and broke the init because the return values of the tasks were
wrong.
---
 src/modules/systemlib/err.h | 15 ++++++++++-----
 1 file changed, 10 insertions(+), 5 deletions(-)

diff --git a/src/modules/systemlib/err.h b/src/modules/systemlib/err.h
index d8f08fb4d5..76f59a8e1c 100644
--- a/src/modules/systemlib/err.h
+++ b/src/modules/systemlib/err.h
@@ -66,6 +66,7 @@
 #define _SYSTEMLIB_ERR_H
 
 #include <px4_log.h>
+#include <px4_tasks.h>
 #include <stdarg.h>
 #include <errno.h>
 #include "visibility.h"
@@ -75,18 +76,22 @@ __BEGIN_DECLS
 __EXPORT const char *getprogname(void);
 
 
-#include <errno.h>
-#include <px4_tasks.h>
+#ifdef __PX4_NUTTX
+#define EXIT(eval) exit(eval)
+#else
+#define EXIT(eval) px4_task_exit(eval)
+#endif
+
 
 #define err(eval, ...)		do { \
 		PX4_ERR(__VA_ARGS__); \
 		PX4_ERR("Task exited with errno=%i\n", errno); \
-		px4_task_exit(eval); } \
-	while(0)
+		EXIT(eval); \
+	} while(0)
 
 #define errx(eval, ...)		do { \
 		PX4_ERR(__VA_ARGS__); \
-		px4_task_exit(eval); \
+		EXIT(eval); \
 	} while(0)
 
 #define warn(...) 		PX4_WARN(__VA_ARGS__)

From 3b5feafef4692b8153475a27a82f7a0d06f8576c Mon Sep 17 00:00:00 2001
From: Julian Oes <julian@oes.ch>
Date: Tue, 7 Jun 2016 10:06:36 +0200
Subject: [PATCH 0580/1230] err: add missing include for exit

---
 src/modules/systemlib/err.h | 1 +
 1 file changed, 1 insertion(+)

diff --git a/src/modules/systemlib/err.h b/src/modules/systemlib/err.h
index 76f59a8e1c..72167bec54 100644
--- a/src/modules/systemlib/err.h
+++ b/src/modules/systemlib/err.h
@@ -69,6 +69,7 @@
 #include <px4_tasks.h>
 #include <stdarg.h>
 #include <errno.h>
+#include <stdlib.h>
 #include "visibility.h"
 
 __BEGIN_DECLS

From e1805dfe0c94f082d96842e46d30dc76c3748314 Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Sun, 22 May 2016 14:09:29 +0200
Subject: [PATCH 0581/1230] Sensors app: Use two parameters to convert battery
 current and voltage to make configuring custom frames easier

---
 src/modules/sensors/sensor_params.c | 48 ++++++++++++++--
 src/modules/sensors/sensors.cpp     | 85 ++++++++++++++++++-----------
 2 files changed, 94 insertions(+), 39 deletions(-)

diff --git a/src/modules/sensors/sensor_params.c b/src/modules/sensors/sensor_params.c
index 29439d61d4..2de4b4216f 100644
--- a/src/modules/sensors/sensor_params.c
+++ b/src/modules/sensors/sensor_params.c
@@ -1964,28 +1964,64 @@ PARAM_DEFINE_INT32(RC_DSM_BIND, -1);
 PARAM_DEFINE_INT32(BAT_V_SCALE_IO, 10000);
 
 /**
- * Scaling factor for battery voltage sensor on FMU v2.
+ * Scaling from ADC counts to volt on the ADC input (battery voltage)
+ *
+ * This is not the battery voltage, but the intermediate ADC voltage.
+ * A value of -1 signifies that the board defaults are used, which is
+ * highly recommended.
  *
  * @group Battery Calibration
  * @decimal 8
  */
-PARAM_DEFINE_FLOAT(BAT_V_SCALING, -1.0f);
+PARAM_DEFINE_FLOAT(BAT_CNT_V_VOLT, -1.0f);
 
 /**
- * Scaling factor for battery current sensor.
+ * Scaling from ADC counts to volt on the ADC input (battery current)
+ *
+ * This is not the battery current, but the intermediate ADC voltage.
+ * A value of -1 signifies that the board defaults are used, which is
+ * highly recommended.
  *
  * @group Battery Calibration
  * @decimal 8
  */
-PARAM_DEFINE_FLOAT(BAT_C_SCALING, -1.0);
+PARAM_DEFINE_FLOAT(BAT_CNT_V_CURR, -1.0);
 
 /**
- * Offset for battery current sensor.
+ * Offset in volt as seen by the ADC input of the current sensor.
+ *
+ * This offset will be subtracted before calculating the battery
+ * current based on the voltage.
  *
  * @group Battery Calibration
  * @decimal 8
  */
-PARAM_DEFINE_FLOAT(BAT_C_OFFSET, -1.0);
+PARAM_DEFINE_FLOAT(BAT_V_OFFS_CURR, 0.0);
+
+/**
+ * Battery voltage divider (V divider)
+ *
+ * This is the divider from battery voltage to 3.3V ADC voltage.
+ * If using e.g. Mauch power modules the value from the datasheet
+ * can be applied straight here. A value of -1 means to use
+ * the board default.
+ *
+ * @group Battery Calibration
+ * @decimal 8
+ */
+PARAM_DEFINE_FLOAT(BAT_V_DIV, -1.0);
+
+/**
+ * Battery current per volt (A/V)
+ *
+ * The voltage seen by the 3.3V ADC multiplied by this factor
+ * will determine the battery current. A value of -1 means to use
+ * the board default.
+ *
+ * @group Battery Calibration
+ * @decimal 8
+ */
+PARAM_DEFINE_FLOAT(BAT_A_PER_V, -1.0);
 
 
 /**
diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp
index 116911bb3a..9eb02d46b9 100644
--- a/src/modules/sensors/sensors.cpp
+++ b/src/modules/sensors/sensors.cpp
@@ -317,6 +317,8 @@ private:
 		float battery_voltage_scaling;
 		float battery_current_scaling;
 		float battery_current_offset;
+		float battery_v_div;
+		float battery_a_per_v;
 
 		float baro_qnh;
 
@@ -377,6 +379,8 @@ private:
 		param_t battery_voltage_scaling;
 		param_t battery_current_scaling;
 		param_t battery_current_offset;
+		param_t battery_v_div;
+		param_t battery_a_per_v;
 
 		param_t board_rotation;
 
@@ -644,9 +648,11 @@ Sensors::Sensors() :
 	_parameter_handles.diff_pres_offset_pa = param_find("SENS_DPRES_OFF");
 	_parameter_handles.diff_pres_analog_scale = param_find("SENS_DPRES_ANSC");
 
-	_parameter_handles.battery_voltage_scaling = param_find("BAT_V_SCALING");
-	_parameter_handles.battery_current_scaling = param_find("BAT_C_SCALING");
-	_parameter_handles.battery_current_offset = param_find("BAT_C_OFFSET");
+	_parameter_handles.battery_voltage_scaling = param_find("BAT_CNT_V_VOLT");
+	_parameter_handles.battery_current_scaling = param_find("BAT_CNT_V_CURR");
+	_parameter_handles.battery_current_offset = param_find("BAT_V_OFFS_CURR");
+	_parameter_handles.battery_v_div = param_find("BAT_V_DIV");
+	_parameter_handles.battery_a_per_v = param_find("BAT_A_PER_V");
 
 	/* rotations */
 	_parameter_handles.board_rotation = param_find("SENS_BOARD_ROT");
@@ -887,18 +893,7 @@ Sensors::parameters_update()
 
 	} else if (_parameters.battery_voltage_scaling < 0.0f) {
 		/* apply scaling according to defaults if set to default */
-#if defined (CONFIG_ARCH_BOARD_PX4FMU_V4)
-		_parameters.battery_voltage_scaling = 0.011f;
-#elif defined (CONFIG_ARCH_BOARD_PX4FMU_V2) || defined ( CONFIG_ARCH_BOARD_MINDPX_V2 )
-		_parameters.battery_voltage_scaling = 0.0082f;
-#elif defined (CONFIG_ARCH_BOARD_AEROCORE)
-		_parameters.battery_voltage_scaling = 0.0063f;
-#elif defined (CONFIG_ARCH_BOARD_PX4FMU_V1)
-		_parameters.battery_voltage_scaling = 0.00459340659f;
-#else
-		/* ensure a missing default trips a low voltage lockdown */
-		_parameters.battery_voltage_scaling = 0.00001f;
-#endif
+		_parameters.battery_voltage_scaling = (3.3f / 4096);
 	}
 
 	/* scaling of ADC ticks to battery current */
@@ -907,27 +902,50 @@ Sensors::parameters_update()
 
 	} else if (_parameters.battery_current_scaling < 0.0f) {
 		/* apply scaling according to defaults if set to default */
-#if defined (CONFIG_ARCH_BOARD_PX4FMU_V4)
-		/* current scaling for ACSP4 */
-		_parameters.battery_current_scaling = 0.0293f;
-#elif defined (CONFIG_ARCH_BOARD_PX4FMU_V2) || defined ( CONFIG_ARCH_BOARD_MINDPX_V2 )
-		/* current scaling for 3DR power brick */
-		_parameters.battery_current_scaling = 0.0124f;
-#elif defined (CONFIG_ARCH_BOARD_AEROCORE)
-		_parameters.battery_current_scaling = 0.0124f;
-#elif defined (CONFIG_ARCH_BOARD_PX4FMU_V1)
-		_parameters.battery_current_scaling = 0.0124f;
-#else
-		/* ensure a missing default leads to an unrealistic current value */
-		_parameters.battery_current_scaling = 0.00001f;
-#endif
+		_parameters.battery_current_scaling = (3.3f / 4096);
 	}
 
 	if (param_get(_parameter_handles.battery_current_offset, &(_parameters.battery_current_offset)) != OK) {
 		warnx("%s", paramerr);
 
-	} else if (_parameters.battery_current_offset < 0.0f) {
-		_parameters.battery_current_offset = 0.0f;
+	}
+
+	if (param_get(_parameter_handles.battery_v_div, &(_parameters.battery_v_div)) != OK) {
+		warnx("%s", paramerr);
+		_parameters.battery_v_div = 100.0f;
+
+	} else if (_parameters.battery_v_div < 0.0f) {
+		/* apply scaling according to defaults if set to default */
+#if defined (CONFIG_ARCH_BOARD_PX4FMU_V4)
+		_parameters.battery_v_div = 13.653333333f;
+#elif defined (CONFIG_ARCH_BOARD_PX4FMU_V2) || defined ( CONFIG_ARCH_BOARD_MINDPX_V2 )
+		_parameters.battery_v_div = 10.177939394f;
+#elif defined (CONFIG_ARCH_BOARD_AEROCORE)
+		_parameters.battery_v_div = 7.8196363636f;
+#elif defined (CONFIG_ARCH_BOARD_PX4FMU_V1)
+		_parameters.battery_v_div = 5.7013919372f;
+#else
+		/* ensure a missing default trips a low voltage lockdown */
+		_parameters.battery_v_div = 100.0f;
+#endif
+	}
+
+	if (param_get(_parameter_handles.battery_a_per_v, &(_parameters.battery_a_per_v)) != OK) {
+		warnx("%s", paramerr);
+		_parameters.battery_a_per_v = 100.0f;
+
+	} else if (_parameters.battery_a_per_v < 0.0f) {
+		/* apply scaling according to defaults if set to default */
+#if defined (CONFIG_ARCH_BOARD_PX4FMU_V4)
+		/* current scaling for ACSP4 */
+		_parameters.battery_a_per_v = 36.367515152f;
+#elif defined (CONFIG_ARCH_BOARD_PX4FMU_V2) || defined (CONFIG_ARCH_BOARD_MINDPX_V2) || defined (CONFIG_ARCH_BOARD_AEROCORE) || defined (CONFIG_ARCH_BOARD_PX4FMU_V1)
+		/* current scaling for 3DR power brick */
+		_parameters.battery_a_per_v = 15.391030303f;
+#else
+		/* ensure a missing default leads to an unrealistic current value */
+		_parameters.battery_a_per_v = 100.0f;
+#endif
 	}
 
 	param_get(_parameter_handles.board_rotation, &(_parameters.board_rotation));
@@ -1655,14 +1673,15 @@ Sensors::adc_poll(struct sensor_combined_s &raw)
 				/* look for specific channels and process the raw voltage to measurement data */
 				if (ADC_BATTERY_VOLTAGE_CHANNEL == buf_adc[i].am_channel) {
 					/* Voltage in volts */
-					bat_voltage_v = (buf_adc[i].am_data * _parameters.battery_voltage_scaling);
+					bat_voltage_v = (buf_adc[i].am_data * _parameters.battery_voltage_scaling) * _parameters.battery_v_div;
 
 					if (bat_voltage_v > 0.5f) {
 						updated_battery = true;
 					}
 
 				} else if (ADC_BATTERY_CURRENT_CHANNEL == buf_adc[i].am_channel) {
-					bat_current_a = (buf_adc[i].am_data * _parameters.battery_current_scaling);
+					bat_current_a = ((buf_adc[i].am_data * _parameters.battery_current_scaling)
+							 - _parameters.battery_current_offset) * _parameters.battery_a_per_v;
 
 #ifdef ADC_AIRSPEED_VOLTAGE_CHANNEL
 

From 98eeb25e45271dc524fca50900b7bd729579cd04 Mon Sep 17 00:00:00 2001
From: Andreas Antener <antener_a@gmx.ch>
Date: Thu, 2 Jun 2016 14:04:15 +0200
Subject: [PATCH 0582/1230] produce 0 voltage/current if no suitable default
 could be set

---
 src/modules/sensors/sensors.cpp | 8 ++++----
 1 file changed, 4 insertions(+), 4 deletions(-)

diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp
index 9eb02d46b9..4998a72f16 100644
--- a/src/modules/sensors/sensors.cpp
+++ b/src/modules/sensors/sensors.cpp
@@ -912,7 +912,7 @@ Sensors::parameters_update()
 
 	if (param_get(_parameter_handles.battery_v_div, &(_parameters.battery_v_div)) != OK) {
 		warnx("%s", paramerr);
-		_parameters.battery_v_div = 100.0f;
+		_parameters.battery_v_div = 0.0f;
 
 	} else if (_parameters.battery_v_div < 0.0f) {
 		/* apply scaling according to defaults if set to default */
@@ -926,13 +926,13 @@ Sensors::parameters_update()
 		_parameters.battery_v_div = 5.7013919372f;
 #else
 		/* ensure a missing default trips a low voltage lockdown */
-		_parameters.battery_v_div = 100.0f;
+		_parameters.battery_v_div = 0.0f;
 #endif
 	}
 
 	if (param_get(_parameter_handles.battery_a_per_v, &(_parameters.battery_a_per_v)) != OK) {
 		warnx("%s", paramerr);
-		_parameters.battery_a_per_v = 100.0f;
+		_parameters.battery_a_per_v = 0.0f;
 
 	} else if (_parameters.battery_a_per_v < 0.0f) {
 		/* apply scaling according to defaults if set to default */
@@ -944,7 +944,7 @@ Sensors::parameters_update()
 		_parameters.battery_a_per_v = 15.391030303f;
 #else
 		/* ensure a missing default leads to an unrealistic current value */
-		_parameters.battery_a_per_v = 100.0f;
+		_parameters.battery_a_per_v = 0.0f;
 #endif
 	}
 

From 58f6835b19c5c32d698dcb6df0c523016bc411d3 Mon Sep 17 00:00:00 2001
From: Miguel Arroyo <miguel@cs.columbia.edu>
Date: Tue, 7 Jun 2016 16:49:27 -0400
Subject: [PATCH 0583/1230] Fixes Function signature MPU9250

---
 src/lib/DriverFramework                                         | 2 +-
 .../posix/drivers/df_mpu9250_wrapper/df_mpu9250_wrapper.cpp     | 2 +-
 2 files changed, 2 insertions(+), 2 deletions(-)

diff --git a/src/lib/DriverFramework b/src/lib/DriverFramework
index 851c7116e2..0624230124 160000
--- a/src/lib/DriverFramework
+++ b/src/lib/DriverFramework
@@ -1 +1 @@
-Subproject commit 851c7116e235867e2923bd22507359310569efa7
+Subproject commit 062423012432f563da95b492804cc2a1c51fedb4
diff --git a/src/platforms/posix/drivers/df_mpu9250_wrapper/df_mpu9250_wrapper.cpp b/src/platforms/posix/drivers/df_mpu9250_wrapper/df_mpu9250_wrapper.cpp
index a730dd60be..368e120031 100644
--- a/src/platforms/posix/drivers/df_mpu9250_wrapper/df_mpu9250_wrapper.cpp
+++ b/src/platforms/posix/drivers/df_mpu9250_wrapper/df_mpu9250_wrapper.cpp
@@ -760,7 +760,7 @@ namespace df_mpu9250_wrapper
 
 DfMpu9250Wrapper *g_dev = nullptr;
 
-int start(enum Rotation rotation);
+int start(bool mag_enabled, enum Rotation rotation);
 int stop();
 int info();
 void usage();

From a9a0759d91209c8a8839feba62e58a90c3e00e6e Mon Sep 17 00:00:00 2001
From: Julian Oes <julian@oes.ch>
Date: Wed, 8 Jun 2016 10:55:56 +0200
Subject: [PATCH 0584/1230] sitl_gazebo: update submodule

This only removes one printfs, nothing fancy.
---
 Tools/sitl_gazebo | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/Tools/sitl_gazebo b/Tools/sitl_gazebo
index 459a0c9335..33342fa4bb 160000
--- a/Tools/sitl_gazebo
+++ b/Tools/sitl_gazebo
@@ -1 +1 @@
-Subproject commit 459a0c933598249fdf30d8162c5b9ec7d6620258
+Subproject commit 33342fa4bba54486109149c39284cb0e959c82a0

From 847cf684db03696d3a6769b1f3c2162f73eaeae3 Mon Sep 17 00:00:00 2001
From: Julian Oes <julian@oes.ch>
Date: Wed, 8 Jun 2016 12:38:50 +0200
Subject: [PATCH 0585/1230] Revert "sitl_gazebo: update submodule"

This reverts commit a9a0759d91209c8a8839feba62e58a90c3e00e6e.
---
 Tools/sitl_gazebo | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/Tools/sitl_gazebo b/Tools/sitl_gazebo
index 33342fa4bb..459a0c9335 160000
--- a/Tools/sitl_gazebo
+++ b/Tools/sitl_gazebo
@@ -1 +1 @@
-Subproject commit 33342fa4bba54486109149c39284cb0e959c82a0
+Subproject commit 459a0c933598249fdf30d8162c5b9ec7d6620258

From 9c170f7fae5b482db7ee5a27bb52894e772329cb Mon Sep 17 00:00:00 2001
From: CarlOlsson <carlolsson.co@gmail.com>
Date: Mon, 6 Jun 2016 16:53:02 +0200
Subject: [PATCH 0586/1230] added parameter which defines threshold for
 airspeed given to the filter remove unnecessary replay fields

---
 msg/ekf2_replay.msg                          |  3 --
 src/modules/ekf2/ekf2_main.cpp               | 33 ++++++++++++++------
 src/modules/ekf2/ekf2_params.c               | 11 +++++++
 src/modules/ekf2_replay/ekf2_replay_main.cpp | 22 +++++++++++--
 src/modules/sdlog2/sdlog2.c                  |  6 ++--
 src/modules/sdlog2/sdlog2_messages.h         |  8 ++---
 6 files changed, 58 insertions(+), 25 deletions(-)

diff --git a/msg/ekf2_replay.msg b/msg/ekf2_replay.msg
index e19daae59b..834230da1a 100644
--- a/msg/ekf2_replay.msg
+++ b/msg/ekf2_replay.msg
@@ -43,9 +43,6 @@ uint8 flow_quality			# Quality of accumulated optical flow data (0 - 255)
 # airspeed
 float32 indicated_airspeed_m_s		# indicated airspeed in meters per second, -1 if unknown
 float32 true_airspeed_m_s		# true filtered airspeed in meters per second, -1 if unknown
-float32 true_airspeed_unfiltered_m_s	# true airspeed in meters per second, -1 if unknown
-float32 air_temperature_celsius		# air temperature in degrees celsius, -1000 if unknown
-float32 confidence			# confidence value from 0 to 1 for this sensor
 
 # external vision measurements
 float32[3] pos_ev			# position in earth (NED) frame (metres)
diff --git a/src/modules/ekf2/ekf2_main.cpp b/src/modules/ekf2/ekf2_main.cpp
index 26c59cf93d..113f8d840e 100644
--- a/src/modules/ekf2/ekf2_main.cpp
+++ b/src/modules/ekf2/ekf2_main.cpp
@@ -80,6 +80,7 @@
 #include <uORB/topics/optical_flow.h>
 #include <uORB/topics/distance_sensor.h>
 #include <uORB/topics/vehicle_land_detected.h>
+#include <uORB/topics/vehicle_status.h>
 
 #include <ecl/EKF/ekf.h>
 
@@ -143,6 +144,7 @@ private:
 	int 	_ev_pos_sub = -1;
 	int	_actuator_armed_sub = -1;
 	int	_vehicle_land_detected_sub = -1;
+	int _status_sub = -1;
 
 	bool	_prev_landed = true;	// landed status from the previous frame
 
@@ -255,7 +257,10 @@ private:
 	control::BlockParamFloat _ev_pos_x;	// X position of VI sensor focal point in body frame (m)
 	control::BlockParamFloat _ev_pos_y;	// Y position of VI sensor focal point in body frame (m)
 	control::BlockParamFloat _ev_pos_z;	// Z position of VI sensor focal point in body frame (m)
-
+	// control of airspeed and sideslip fusion
+ 	control::BlockParamFloat _arspFusionThreshold; 	// a value of zero will disabled airspeed fusion. Any another positive value will determine
+ 													// the minimum airspeed which will still be fused
+ 	
 	// output predictor filter time constants
 	control::BlockParamFloat _tau_vel;	// time constant used by the output velocity complementary filter (s)
 	control::BlockParamFloat _tau_pos;	// time constant used by the output position complementary filter (s)
@@ -355,6 +360,7 @@ Ekf2::Ekf2():
 	_ev_pos_x(this, "EKF2_EV_POS_X", false, &_params->ev_pos_body(0)),
 	_ev_pos_y(this, "EKF2_EV_POS_Y", false, &_params->ev_pos_body(1)),
 	_ev_pos_z(this, "EKF2_EV_POS_Z", false, &_params->ev_pos_body(2)),
+	_arspFusionThreshold(this, "EKF2_ARSP_THR", false),
 	_tau_vel(this, "EKF2_TAU_VEL", false, &_params->vel_Tau),
 	_tau_pos(this, "EKF2_TAU_POS", false, &_params->pos_Tau),
 	_gyr_bias_init(this, "EKF2_GBIAS_INIT", false, &_params->switch_on_gyro_bias),
@@ -386,6 +392,7 @@ void Ekf2::task_main()
 	_range_finder_sub = orb_subscribe(ORB_ID(distance_sensor));
 	_ev_pos_sub = orb_subscribe(ORB_ID(vision_position_estimate));
 	_vehicle_land_detected_sub = orb_subscribe(ORB_ID(vehicle_land_detected));
+	_status_sub = orb_subscribe(ORB_ID(vehicle_status));
 
 	px4_pollfd_struct_t fds[2] = {};
 	fds[0].fd = _sensors_sub;
@@ -406,6 +413,7 @@ void Ekf2::task_main()
 	distance_sensor_s range_finder = {};
 	vehicle_land_detected_s vehicle_land_detected = {};
 	vision_position_estimate_s ev = {};
+	vehicle_status_s _vehicle_status = {};
 
 	while (!_task_should_exit) {
 		int ret = px4_poll(fds, sizeof(fds) / sizeof(fds[0]), 1000);
@@ -440,9 +448,17 @@ void Ekf2::task_main()
 		bool range_finder_updated = false;
 		bool vehicle_land_detected_updated = false;
 		bool vision_position_updated = false;
+		bool vehicle_status_updated = false;
 
 		orb_copy(ORB_ID(sensor_combined), _sensors_sub, &sensors);
 		// update all other topics if they have new data
+
+ 		orb_check(_status_sub, &vehicle_status_updated);
+
+		if (vehicle_status_updated) {
+ 			orb_copy(ORB_ID(vehicle_status), _status_sub, &_vehicle_status);
+ 		}
+
 		orb_check(_gps_sub, &gps_updated);
 
 		if (gps_updated) {
@@ -517,12 +533,12 @@ void Ekf2::task_main()
 			_ekf.setGpsData(gps.timestamp_position, &gps_msg);
 		}
 
-		// read airspeed data if available
-		float eas2tas = airspeed.true_airspeed_m_s / airspeed.indicated_airspeed_m_s;
-
-		if (airspeed_updated && airspeed.true_airspeed_m_s > 7.0f) {
+		// only set airspeed data if condition for airspeed fusion are met
+ 		bool fuse_airspeed = airspeed_updated && !_vehicle_status.is_rotary_wing && _arspFusionThreshold.get() <= airspeed.true_airspeed_m_s;
+ 		if (fuse_airspeed) {
+ 			float eas2tas = airspeed.true_airspeed_m_s / airspeed.indicated_airspeed_m_s;
 			_ekf.setAirspeedData(airspeed.timestamp, &airspeed.true_airspeed_m_s, &eas2tas);
-		}
+  		}
 
 		if (optical_flow_updated) {
 			flow_message flow;
@@ -907,13 +923,10 @@ void Ekf2::task_main()
 				replay.rng_timestamp = 0;
 			}
 
-			if (airspeed_updated) {
+			if (fuse_airspeed) {
 				replay.asp_timestamp = airspeed.timestamp;
 				replay.indicated_airspeed_m_s = airspeed.indicated_airspeed_m_s;
 				replay.true_airspeed_m_s = airspeed.true_airspeed_m_s;
-				replay.true_airspeed_unfiltered_m_s = airspeed.true_airspeed_unfiltered_m_s;
-				replay.air_temperature_celsius = airspeed.air_temperature_celsius;
-				replay.confidence = airspeed.confidence;
 
 			} else {
 				replay.asp_timestamp = 0;
diff --git a/src/modules/ekf2/ekf2_params.c b/src/modules/ekf2/ekf2_params.c
index f316495926..6a9eab4770 100644
--- a/src/modules/ekf2/ekf2_params.c
+++ b/src/modules/ekf2/ekf2_params.c
@@ -782,6 +782,17 @@ PARAM_DEFINE_FLOAT(EKF2_EV_POS_Y, 0.0f);
  */
 PARAM_DEFINE_FLOAT(EKF2_EV_POS_Z, 0.0f);
 
+ /**
+ * Airspeed fusion threshold. A value of zero will deactivate airspeed fusion. Any other positive
+ * value will determine the minimum airspeed which will still be fused.
+ *
+ * @group EKF2
+ * @min 0.0
+ * @unit m/s
+ * @decimal 1
+ */
+ PARAM_DEFINE_FLOAT(EKF2_ARSP_THR, 0.0f);
+
 /**
 
  * Time constant of the velocity output prediction and smoothing filter
diff --git a/src/modules/ekf2_replay/ekf2_replay_main.cpp b/src/modules/ekf2_replay/ekf2_replay_main.cpp
index c6ff14e86a..df74663ee7 100644
--- a/src/modules/ekf2_replay/ekf2_replay_main.cpp
+++ b/src/modules/ekf2_replay/ekf2_replay_main.cpp
@@ -137,6 +137,7 @@ private:
 	orb_advert_t _range_pub;
 	orb_advert_t _airspeed_pub;
 	orb_advert_t _ev_pub;
+	orb_advert_t _vehicle_status_pub;
 
 	int _att_sub;
 	int _estimator_status_sub;
@@ -154,6 +155,7 @@ private:
 	struct distance_sensor_s _range;
 	struct airspeed_s _airspeed;
 	struct vision_position_estimate_s _ev;
+	struct vehicle_status_s _vehicle_status;
 
 	unsigned _message_counter; // counter which will increase with every message read from the log
 	unsigned _part1_counter_ref;		// this is the value of _message_counter when the part1 of the replay message is read (imu data)
@@ -211,6 +213,7 @@ Ekf2Replay::Ekf2Replay(char *logfile) :
 	_range_pub(nullptr),
 	_airspeed_pub(nullptr),
 	_ev_pub(nullptr),
+	_vehicle_status_pub(nullptr),
 	_att_sub(-1),
 	_estimator_status_sub(-1),
 	_innov_sub(-1),
@@ -222,6 +225,8 @@ Ekf2Replay::Ekf2Replay(char *logfile) :
 	_land_detected{},
 	_flow{},
 	_range{},
+	_airspeed{},
+ 	_vehicle_status{},
 	_message_counter(0),
 	_part1_counter_ref(0),
 	_read_part2(false),
@@ -364,6 +369,7 @@ void Ekf2Replay::setEstimatorInput(uint8_t *data, uint8_t type)
 	struct log_RPL6_s replay_part6 = {};
 	struct log_RPL5_s replay_part5 = {};
 	struct log_LAND_s vehicle_landed = {};
+	struct log_STAT_s vehicle_status = {};
 
 	if (type == LOG_RPL1_MSG) {
 
@@ -431,9 +437,6 @@ void Ekf2Replay::setEstimatorInput(uint8_t *data, uint8_t type)
 		_airspeed.timestamp = replay_part6.time_airs_usec;
 		_airspeed.indicated_airspeed_m_s = replay_part6.indicated_airspeed_m_s;
 		_airspeed.true_airspeed_m_s = replay_part6.true_airspeed_m_s;
-		_airspeed.true_airspeed_unfiltered_m_s = replay_part6.true_airspeed_unfiltered_m_s;
-		_airspeed.air_temperature_celsius = replay_part6.air_temperature_celsius;
-		_airspeed.confidence = replay_part6.confidence;
 		_read_part6 = true;
 
 	} else if (type == LOG_RPL5_MSG) {
@@ -464,6 +467,19 @@ void Ekf2Replay::setEstimatorInput(uint8_t *data, uint8_t type)
 			orb_publish(ORB_ID(vehicle_land_detected), _landed_pub, &_land_detected);
 		}
 	}
+
+	else if (type == LOG_STAT_MSG) {
+ 		uint8_t *dest_ptr = (uint8_t *)&vehicle_status.main_state;
+ 		parseMessage(data, dest_ptr, type);
+ 		_vehicle_status.is_rotary_wing = vehicle_status.is_rot_wing;
+ 
+ 		if (_vehicle_status_pub == nullptr) {
+ 			_vehicle_status_pub = orb_advertise(ORB_ID(vehicle_status), &_vehicle_status);
+ 
+ 		} else if (_vehicle_status_pub != nullptr) {
+ 			orb_publish(ORB_ID(vehicle_status), _vehicle_status_pub, &_vehicle_status);
+ 		}
+ 	}
 }
 
 void Ekf2Replay::writeMessage(int &fd, void *data, size_t size)
diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c
index 3d63da3e2d..ceab974656 100644
--- a/src/modules/sdlog2/sdlog2.c
+++ b/src/modules/sdlog2/sdlog2.c
@@ -1517,6 +1517,7 @@ int sdlog2_thread_main(int argc, char *argv[])
 			log_msg.body.log_STAT.nav_state = buf_status.nav_state;
 			log_msg.body.log_STAT.arming_state = buf_status.arming_state;
 			log_msg.body.log_STAT.failsafe = (uint8_t) buf_status.failsafe;
+			log_msg.body.log_STAT.is_rot_wing = (uint8_t)buf_status.is_rotary_wing;
 			LOGBUFFER_WRITE_AND_COUNT(STAT);
 		}
 
@@ -1600,10 +1601,7 @@ int sdlog2_thread_main(int argc, char *argv[])
 					log_msg.msg_type = LOG_RPL6_MSG;
 					log_msg.body.log_RPL6.time_airs_usec = buf.replay.asp_timestamp;
 					log_msg.body.log_RPL6.indicated_airspeed_m_s = buf.replay.indicated_airspeed_m_s;
-					log_msg.body.log_RPL6.true_airspeed_m_s = buf.replay.true_airspeed_m_s;
-					log_msg.body.log_RPL6.true_airspeed_unfiltered_m_s = buf.replay.true_airspeed_unfiltered_m_s;
-					log_msg.body.log_RPL6.air_temperature_celsius = buf.replay.air_temperature_celsius;
-					log_msg.body.log_RPL6.confidence = buf.replay.confidence;
+					log_msg.body.log_RPL6.true_airspeed_m_s = buf.replay.true_airspeed_m_s;;
 					LOGBUFFER_WRITE_AND_COUNT(RPL6);
 				}
 
diff --git a/src/modules/sdlog2/sdlog2_messages.h b/src/modules/sdlog2/sdlog2_messages.h
index 48d22b1c94..a6a253f921 100644
--- a/src/modules/sdlog2/sdlog2_messages.h
+++ b/src/modules/sdlog2/sdlog2_messages.h
@@ -182,6 +182,7 @@ struct log_STAT_s {
 	uint8_t nav_state;
 	uint8_t arming_state;
 	uint8_t failsafe;
+	uint8_t is_rot_wing;
 };
 
 /* --- RC - RC INPUT CHANNELS --- */
@@ -600,9 +601,6 @@ struct log_RPL6_s {
 	uint64_t time_airs_usec;
 	float indicated_airspeed_m_s;
 	float true_airspeed_m_s;
-	float true_airspeed_unfiltered_m_s;
-	float air_temperature_celsius;
-	float confidence;
 };
 
 /* --- EKF2 REPLAY Part 5 --- */
@@ -668,7 +666,7 @@ static const struct log_format_s log_formats[] = {
 	LOG_FORMAT_S(DGPS, GPS,	 "QBffLLfffffBHHH",	"GPSTime,Fix,EPH,EPV,Lat,Lon,Alt,VelN,VelE,VelD,Cog,nSat,SNR,N,J"),
 	LOG_FORMAT_S(ATTC, ATTC, "ffff",		"Roll,Pitch,Yaw,Thrust"),
 	LOG_FORMAT_S(ATC1, ATTC, "ffff",		"Roll,Pitch,Yaw,Thrust"),
-	LOG_FORMAT(STAT, "BBBB",		"MainState,NavState,ArmS,Failsafe"),
+	LOG_FORMAT(STAT, "BBBBB",		"MainState,NavState,ArmS,Failsafe,IsRotWing"),
 	LOG_FORMAT(VTOL, "fBBB",		"Arsp,RwMode,TransMode,Failsafe"),
 	LOG_FORMAT(CTS, "fffffff", "Vx_b,Vy_b,Vz_b,Vinf,P,Q,R"),
 	LOG_FORMAT(RC, "ffffffffffffBBBL",		"C0,C1,C2,C3,C4,C5,C6,C7,C8,C9,C10,C11,RSSI,CNT,Lost,Drop"),
@@ -712,7 +710,7 @@ static const struct log_format_s log_formats[] = {
 	LOG_FORMAT(RPL3, "QffffIB", "Tflow,fx,fy,gx,gy,delT,qual"),
 	LOG_FORMAT(RPL4, "Qf", "Trng,rng"),
 	LOG_FORMAT(RPL5, "Qfffffffff", "Tev,x,y,z,q0,q1,q2,q3,posErr,angErr"),
-	LOG_FORMAT(RPL6, "Qfffff", "Tasp,inAsp,trAsp,ufAsp,tpAsp,confAsp"),
+	LOG_FORMAT(RPL6, "Qff", "Tasp,inAsp,trAsp"),
 	LOG_FORMAT(LAND, "B", "Landed"),
 	LOG_FORMAT(LOAD, "f", "CPU"),
 	/* system-level messages, ID >= 0x80 */

From acea2f98d5df5e1b64da9a38c186f1ff65963ebc Mon Sep 17 00:00:00 2001
From: CarlOlsson <carlolsson.co@gmail.com>
Date: Tue, 7 Jun 2016 14:02:24 +0200
Subject: [PATCH 0587/1230] log airspeed in rpl mode even if it is not fused

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

diff --git a/src/modules/ekf2/ekf2_main.cpp b/src/modules/ekf2/ekf2_main.cpp
index 113f8d840e..bc5b81a758 100644
--- a/src/modules/ekf2/ekf2_main.cpp
+++ b/src/modules/ekf2/ekf2_main.cpp
@@ -923,7 +923,7 @@ void Ekf2::task_main()
 				replay.rng_timestamp = 0;
 			}
 
-			if (fuse_airspeed) {
+			if (airspeed_updated) {
 				replay.asp_timestamp = airspeed.timestamp;
 				replay.indicated_airspeed_m_s = airspeed.indicated_airspeed_m_s;
 				replay.true_airspeed_m_s = airspeed.true_airspeed_m_s;

From b9e9f62bee8f9787d75eaf69df6b8bd2e040fba9 Mon Sep 17 00:00:00 2001
From: tumbili <roman@px4.io>
Date: Wed, 8 Jun 2016 17:14:33 +0200
Subject: [PATCH 0588/1230] code style formatting

---
 src/modules/ekf2/ekf2_main.cpp               | 29 +++++++++++++-------
 src/modules/ekf2_replay/ekf2_replay_main.cpp | 27 +++++++++---------
 2 files changed, 33 insertions(+), 23 deletions(-)

diff --git a/src/modules/ekf2/ekf2_main.cpp b/src/modules/ekf2/ekf2_main.cpp
index bc5b81a758..ea333769bc 100644
--- a/src/modules/ekf2/ekf2_main.cpp
+++ b/src/modules/ekf2/ekf2_main.cpp
@@ -258,9 +258,10 @@ private:
 	control::BlockParamFloat _ev_pos_y;	// Y position of VI sensor focal point in body frame (m)
 	control::BlockParamFloat _ev_pos_z;	// Z position of VI sensor focal point in body frame (m)
 	// control of airspeed and sideslip fusion
- 	control::BlockParamFloat _arspFusionThreshold; 	// a value of zero will disabled airspeed fusion. Any another positive value will determine
- 													// the minimum airspeed which will still be fused
- 	
+	control::BlockParamFloat
+	_arspFusionThreshold; 	// a value of zero will disabled airspeed fusion. Any another positive value will determine
+	// the minimum airspeed which will still be fused
+
 	// output predictor filter time constants
 	control::BlockParamFloat _tau_vel;	// time constant used by the output velocity complementary filter (s)
 	control::BlockParamFloat _tau_pos;	// time constant used by the output position complementary filter (s)
@@ -453,11 +454,11 @@ void Ekf2::task_main()
 		orb_copy(ORB_ID(sensor_combined), _sensors_sub, &sensors);
 		// update all other topics if they have new data
 
- 		orb_check(_status_sub, &vehicle_status_updated);
+		orb_check(_status_sub, &vehicle_status_updated);
 
 		if (vehicle_status_updated) {
- 			orb_copy(ORB_ID(vehicle_status), _status_sub, &_vehicle_status);
- 		}
+			orb_copy(ORB_ID(vehicle_status), _status_sub, &_vehicle_status);
+		}
 
 		orb_check(_gps_sub, &gps_updated);
 
@@ -534,11 +535,13 @@ void Ekf2::task_main()
 		}
 
 		// only set airspeed data if condition for airspeed fusion are met
- 		bool fuse_airspeed = airspeed_updated && !_vehicle_status.is_rotary_wing && _arspFusionThreshold.get() <= airspeed.true_airspeed_m_s;
- 		if (fuse_airspeed) {
- 			float eas2tas = airspeed.true_airspeed_m_s / airspeed.indicated_airspeed_m_s;
+		bool fuse_airspeed = airspeed_updated && !_vehicle_status.is_rotary_wing
+				     && _arspFusionThreshold.get() <= airspeed.true_airspeed_m_s;
+
+		if (fuse_airspeed) {
+			float eas2tas = airspeed.true_airspeed_m_s / airspeed.indicated_airspeed_m_s;
 			_ekf.setAirspeedData(airspeed.timestamp, &airspeed.true_airspeed_m_s, &eas2tas);
-  		}
+		}
 
 		if (optical_flow_updated) {
 			flow_message flow;
@@ -569,18 +572,23 @@ void Ekf2::task_main()
 			ev_data.posNED(2) = ev.z;
 			Quaternion q(ev.q);
 			ev_data.quat = q;
+
 			// position measurement error
 			if (ev.pos_err >= 0.001f) {
 				ev_data.posErr = ev.pos_err;
+
 			} else {
 				ev_data.posErr = _default_ev_pos_noise;
 			}
+
 			// angle measurement error
 			if (ev.ang_err >= 0.001f) {
 				ev_data.angErr = ev.ang_err;
+
 			} else {
 				ev_data.angErr = _default_ev_ang_noise;
 			}
+
 			// use timestamp from external computer - requires clocks to be synchronised so may not be a good idea
 			_ekf.setExtVisionData(ev.timestamp_computer, &ev_data);
 		}
@@ -861,6 +869,7 @@ void Ekf2::task_main()
 			_ekf.copy_mag_decl_deg(&decl_deg);
 			_mag_declination_deg.set(decl_deg);
 		}
+
 		_prev_landed = vehicle_land_detected.landed;
 
 		// publish replay message if in replay mode
diff --git a/src/modules/ekf2_replay/ekf2_replay_main.cpp b/src/modules/ekf2_replay/ekf2_replay_main.cpp
index df74663ee7..b8de2c70b1 100644
--- a/src/modules/ekf2_replay/ekf2_replay_main.cpp
+++ b/src/modules/ekf2_replay/ekf2_replay_main.cpp
@@ -226,7 +226,7 @@ Ekf2Replay::Ekf2Replay(char *logfile) :
 	_flow{},
 	_range{},
 	_airspeed{},
- 	_vehicle_status{},
+	_vehicle_status{},
 	_message_counter(0),
 	_part1_counter_ref(0),
 	_read_part2(false),
@@ -299,6 +299,7 @@ void Ekf2Replay::publishEstimatorInput()
 
 	if (_airspeed_pub == nullptr && _read_part6) {
 		_airspeed_pub = orb_advertise(ORB_ID(airspeed), &_airspeed);
+
 	} else if (_airspeed_pub != nullptr) {
 		orb_publish(ORB_ID(airspeed), _airspeed_pub, &_airspeed);
 	}
@@ -431,7 +432,7 @@ void Ekf2Replay::setEstimatorInput(uint8_t *data, uint8_t type)
 		_range.current_distance = replay_part4.range_to_ground;
 		_read_part4 = true;
 
-	} else if (type == LOG_RPL6_MSG){
+	} else if (type == LOG_RPL6_MSG) {
 		uint8_t *dest_ptr = (uint8_t *)&replay_part6.time_airs_usec;
 		parseMessage(data, dest_ptr, type);
 		_airspeed.timestamp = replay_part6.time_airs_usec;
@@ -469,17 +470,17 @@ void Ekf2Replay::setEstimatorInput(uint8_t *data, uint8_t type)
 	}
 
 	else if (type == LOG_STAT_MSG) {
- 		uint8_t *dest_ptr = (uint8_t *)&vehicle_status.main_state;
- 		parseMessage(data, dest_ptr, type);
- 		_vehicle_status.is_rotary_wing = vehicle_status.is_rot_wing;
- 
- 		if (_vehicle_status_pub == nullptr) {
- 			_vehicle_status_pub = orb_advertise(ORB_ID(vehicle_status), &_vehicle_status);
- 
- 		} else if (_vehicle_status_pub != nullptr) {
- 			orb_publish(ORB_ID(vehicle_status), _vehicle_status_pub, &_vehicle_status);
- 		}
- 	}
+		uint8_t *dest_ptr = (uint8_t *)&vehicle_status.main_state;
+		parseMessage(data, dest_ptr, type);
+		_vehicle_status.is_rotary_wing = vehicle_status.is_rot_wing;
+
+		if (_vehicle_status_pub == nullptr) {
+			_vehicle_status_pub = orb_advertise(ORB_ID(vehicle_status), &_vehicle_status);
+
+		} else if (_vehicle_status_pub != nullptr) {
+			orb_publish(ORB_ID(vehicle_status), _vehicle_status_pub, &_vehicle_status);
+		}
+	}
 }
 
 void Ekf2Replay::writeMessage(int &fd, void *data, size_t size)

From 9f5adb2356ec6eccba354c10bcc37b75fc275d2b Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Beat=20K=C3=BCng?= <beat-kueng@gmx.net>
Date: Wed, 8 Jun 2016 12:58:23 +0200
Subject: [PATCH 0589/1230] getprogname: re-enable on NuttX & move to
 px4_tasks.h

---
 src/modules/systemlib/err.c                     | 11 -----------
 src/modules/systemlib/err.h                     |  2 --
 src/platforms/nuttx/px4_layer/px4_nuttx_tasks.c | 13 +++++++++++++
 src/platforms/px4_tasks.h                       |  2 ++
 4 files changed, 15 insertions(+), 13 deletions(-)

diff --git a/src/modules/systemlib/err.c b/src/modules/systemlib/err.c
index 7765395256..5ad31330af 100644
--- a/src/modules/systemlib/err.c
+++ b/src/modules/systemlib/err.c
@@ -62,17 +62,6 @@ extern int lib_lowvprintf(const char *fmt, va_list ap);
 
 // XXX not used anymore
 #if 0
-const char *
-getprogname(void)
-{
-#if CONFIG_TASK_NAME_SIZE > 0
-	FAR struct tcb_s	*thisproc = sched_self();
-
-	return thisproc->name;
-#else
-	return "app";
-#endif
-}
 
 static void
 warnerr_core(int errcode, const char *fmt, va_list args)
diff --git a/src/modules/systemlib/err.h b/src/modules/systemlib/err.h
index 72167bec54..7c09bf7b0f 100644
--- a/src/modules/systemlib/err.h
+++ b/src/modules/systemlib/err.h
@@ -74,8 +74,6 @@
 
 __BEGIN_DECLS
 
-__EXPORT const char *getprogname(void);
-
 
 #ifdef __PX4_NUTTX
 #define EXIT(eval) exit(eval)
diff --git a/src/platforms/nuttx/px4_layer/px4_nuttx_tasks.c b/src/platforms/nuttx/px4_layer/px4_nuttx_tasks.c
index bb4dea7d4a..4cefa0c716 100644
--- a/src/platforms/nuttx/px4_layer/px4_nuttx_tasks.c
+++ b/src/platforms/nuttx/px4_layer/px4_nuttx_tasks.c
@@ -38,6 +38,7 @@
  */
 
 #include <px4_config.h>
+#include <px4_tasks.h>
 #include <unistd.h>
 #include <stdio.h>
 #include <fcntl.h>
@@ -106,3 +107,15 @@ int px4_task_delete(int pid)
 {
 	return task_delete(pid);
 }
+
+const char *getprogname(void)
+{
+#if CONFIG_TASK_NAME_SIZE > 0
+	FAR struct tcb_s	*thisproc = sched_self();
+
+	return thisproc->name;
+#else
+	return "app";
+#endif
+}
+
diff --git a/src/platforms/px4_tasks.h b/src/platforms/px4_tasks.h
index 6b6fdbddf4..0ce9e2e4a3 100644
--- a/src/platforms/px4_tasks.h
+++ b/src/platforms/px4_tasks.h
@@ -134,5 +134,7 @@ __EXPORT bool px4_task_is_running(const char *taskname);
 __EXPORT int px4_prctl(int option, const char *arg2, unsigned pid);
 #endif
 
+__EXPORT const char *getprogname(void);
+
 __END_DECLS
 

From fcab80bfee34e5029b1c5d21faaf9311452f598f Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Beat=20K=C3=BCng?= <beat-kueng@gmx.net>
Date: Wed, 8 Jun 2016 13:04:54 +0200
Subject: [PATCH 0590/1230] getprogname: rename to px4_get_taskname

---
 src/modules/systemlib/err.c                       | 4 ++--
 src/platforms/nuttx/px4_layer/px4_nuttx_tasks.c   | 2 +-
 src/platforms/posix/px4_layer/px4_posix_tasks.cpp | 2 +-
 src/platforms/px4_tasks.h                         | 3 ++-
 src/platforms/qurt/px4_layer/px4_qurt_tasks.cpp   | 2 +-
 5 files changed, 7 insertions(+), 6 deletions(-)

diff --git a/src/modules/systemlib/err.c b/src/modules/systemlib/err.c
index 5ad31330af..78e3058068 100644
--- a/src/modules/systemlib/err.c
+++ b/src/modules/systemlib/err.c
@@ -67,7 +67,7 @@ static void
 warnerr_core(int errcode, const char *fmt, va_list args)
 {
 #if CONFIG_NFILE_STREAMS > 0
-	fprintf(stderr, "%s: ", getprogname());
+	fprintf(stderr, "%s: ", px4_get_taskname());
 	vfprintf(stderr, fmt, args);
 
 	/* convenience as many parts of NuttX use negative errno */
@@ -81,7 +81,7 @@ warnerr_core(int errcode, const char *fmt, va_list args)
 
 	fprintf(stderr, "\n");
 #elif CONFIG_ARCH_LOWPUTC
-	lowsyslog("%s: ", getprogname());
+	lowsyslog("%s: ", px4_get_taskname());
 	lowvsyslog(fmt, args);
 
 	/* convenience as many parts of NuttX use negative errno */
diff --git a/src/platforms/nuttx/px4_layer/px4_nuttx_tasks.c b/src/platforms/nuttx/px4_layer/px4_nuttx_tasks.c
index 4cefa0c716..84eed74b53 100644
--- a/src/platforms/nuttx/px4_layer/px4_nuttx_tasks.c
+++ b/src/platforms/nuttx/px4_layer/px4_nuttx_tasks.c
@@ -108,7 +108,7 @@ int px4_task_delete(int pid)
 	return task_delete(pid);
 }
 
-const char *getprogname(void)
+const char *px4_get_taskname(void)
 {
 #if CONFIG_TASK_NAME_SIZE > 0
 	FAR struct tcb_s	*thisproc = sched_self();
diff --git a/src/platforms/posix/px4_layer/px4_posix_tasks.cpp b/src/platforms/posix/px4_layer/px4_posix_tasks.cpp
index ffcd74f5e1..53cc821b2f 100644
--- a/src/platforms/posix/px4_layer/px4_posix_tasks.cpp
+++ b/src/platforms/posix/px4_layer/px4_posix_tasks.cpp
@@ -388,7 +388,7 @@ unsigned long px4_getpid()
 	return (unsigned long)pthread_self();
 }
 
-const char *getprogname()
+const char *px4_get_taskname()
 {
 	pthread_t pid = pthread_self();
 	const char *prog_name = "UnknownApp";
diff --git a/src/platforms/px4_tasks.h b/src/platforms/px4_tasks.h
index 0ce9e2e4a3..6fa3619ae5 100644
--- a/src/platforms/px4_tasks.h
+++ b/src/platforms/px4_tasks.h
@@ -134,7 +134,8 @@ __EXPORT bool px4_task_is_running(const char *taskname);
 __EXPORT int px4_prctl(int option, const char *arg2, unsigned pid);
 #endif
 
-__EXPORT const char *getprogname(void);
+/** return the name of the current task */
+__EXPORT const char *px4_get_taskname(void);
 
 __END_DECLS
 
diff --git a/src/platforms/qurt/px4_layer/px4_qurt_tasks.cpp b/src/platforms/qurt/px4_layer/px4_qurt_tasks.cpp
index 40897c8ff0..6cf342076a 100644
--- a/src/platforms/qurt/px4_layer/px4_qurt_tasks.cpp
+++ b/src/platforms/qurt/px4_layer/px4_qurt_tasks.cpp
@@ -348,7 +348,7 @@ unsigned long px4_getpid()
 }
 
 
-const char *getprogname()
+const char *px4_get_taskname()
 {
 	pthread_t pid = pthread_self();
 

From a7c7a46be87c0f81a3ae5b887a60bbba0a329ae7 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Beat=20K=C3=BCng?= <beat-kueng@gmx.net>
Date: Wed, 8 Jun 2016 17:58:30 +0200
Subject: [PATCH 0591/1230] posix console: output newline before process_line

Before the output looked like this:
pxh> logger statusINFO  [logger]   Running
INFO  [logger]   Wrote 0.13 MiB (avg  0.72 KiB/s)
INFO  [logger]   Since last status: dropouts: 0 (max len: 0.000 s), max used buffer: 7176 / 12288 B
---
 src/platforms/posix/main.cpp | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/src/platforms/posix/main.cpp b/src/platforms/posix/main.cpp
index 7b1c4dec7f..ca0a471703 100644
--- a/src/platforms/posix/main.cpp
+++ b/src/platforms/posix/main.cpp
@@ -128,7 +128,7 @@ static void run_cmd(const vector<string> &appargs, bool exit_on_fail)
 		// Do nothing
 
 	} else {
-		cout << endl << "Invalid command: " << command << "\ntype 'help' for a list of commands" << endl;
+		cout << "Invalid command: " << command << "\ntype 'help' for a list of commands" << endl;
 
 	}
 }
@@ -335,8 +335,8 @@ int main(int argc, char **argv)
 					}
 				}
 
-				process_line(mystr, false);
 				cout << endl;
+				process_line(mystr, false);
 				mystr = "";
 				buf_ptr_read = buf_ptr_write;
 

From 5b811facabcbdabef87c50e48e3e8c230d0df0b5 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Beat=20K=C3=BCng?= <beat-kueng@gmx.net>
Date: Wed, 8 Jun 2016 17:59:07 +0200
Subject: [PATCH 0592/1230] posix console/scripts: treat lines starting with #
 as comment

---
 src/platforms/posix/main.cpp | 4 +---
 1 file changed, 1 insertion(+), 3 deletions(-)

diff --git a/src/platforms/posix/main.cpp b/src/platforms/posix/main.cpp
index ca0a471703..4d6791fdf4 100644
--- a/src/platforms/posix/main.cpp
+++ b/src/platforms/posix/main.cpp
@@ -119,12 +119,10 @@ static void run_cmd(const vector<string> &appargs, bool exit_on_fail)
 			}
 		}
 
-
-
 	} else if (command.compare("help") == 0) {
 		list_builtins();
 
-	} else if (command.length() == 0) {
+	} else if (command.length() == 0 || command[0] == '#') {
 		// Do nothing
 
 	} else {

From 1b5edcabec0ce8931c581884b25606b92ac72885 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Beat=20K=C3=BCng?= <beat-kueng@gmx.net>
Date: Thu, 9 Jun 2016 11:08:04 +0200
Subject: [PATCH 0593/1230] posix shell: avoid printing non-printable
 characters (like Ctrl-C)

---
 src/platforms/posix/main.cpp | 6 ++++--
 1 file changed, 4 insertions(+), 2 deletions(-)

diff --git a/src/platforms/posix/main.cpp b/src/platforms/posix/main.cpp
index 4d6791fdf4..fb5b7fe751 100644
--- a/src/platforms/posix/main.cpp
+++ b/src/platforms/posix/main.cpp
@@ -373,8 +373,10 @@ int main(int argc, char **argv)
 				}
 
 			default:	// any other input
-				cout << c;
-				mystr += c;
+				if (c > 3) {
+					cout << c;
+					mystr += c;
+				}
 				break;
 			}
 		}

From 03f7ee9b1218ae8b87966dbbd91c73bb516e50d3 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Beat=20K=C3=BCng?= <beat-kueng@gmx.net>
Date: Thu, 9 Jun 2016 11:12:07 +0200
Subject: [PATCH 0594/1230] posix shell: do not complaining about non-existing
 command muorb on SITL exit

Namely the following output after Ctrl-C:
Invalid command: muorb
type 'help' for a list of commands
---
 src/platforms/posix/main.cpp | 6 +++---
 1 file changed, 3 insertions(+), 3 deletions(-)

diff --git a/src/platforms/posix/main.cpp b/src/platforms/posix/main.cpp
index fb5b7fe751..6d9d33e7e2 100644
--- a/src/platforms/posix/main.cpp
+++ b/src/platforms/posix/main.cpp
@@ -92,7 +92,7 @@ static void print_prompt()
 	cout.flush();
 }
 
-static void run_cmd(const vector<string> &appargs, bool exit_on_fail)
+static void run_cmd(const vector<string> &appargs, bool exit_on_fail, bool silently_fail = false)
 {
 	// command is appargs[0]
 	string command = appargs[0];
@@ -125,7 +125,7 @@ static void run_cmd(const vector<string> &appargs, bool exit_on_fail)
 	} else if (command.length() == 0 || command[0] == '#') {
 		// Do nothing
 
-	} else {
+	} else if (!silently_fail) {
 		cout << "Invalid command: " << command << "\ntype 'help' for a list of commands" << endl;
 
 	}
@@ -392,7 +392,7 @@ int main(int argc, char **argv)
 		//if (px4_task_is_running("muorb")) {
 		// sending muorb stop is needed if it is running to exit cleanly
 		vector<string> muorb_stop_cmd = { "muorb", "stop" };
-		run_cmd(muorb_stop_cmd, !daemon_mode);
+		run_cmd(muorb_stop_cmd, !daemon_mode, true);
 	}
 
 	vector<string> shutdown_cmd = { "shutdown" };

From 513900fb16b41026613725194871392b01ba5b09 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Beat=20K=C3=BCng?= <beat-kueng@gmx.net>
Date: Thu, 9 Jun 2016 12:51:04 +0200
Subject: [PATCH 0595/1230] printf log: colorize output under POSIX & tty
 output according to the log level

---
 src/platforms/posix/main.cpp            |  1 +
 src/platforms/posix/px4_layer/px4_log.c |  2 ++
 src/platforms/px4_log.h                 | 39 ++++++++++++++++++++++++-
 3 files changed, 41 insertions(+), 1 deletion(-)

diff --git a/src/platforms/posix/main.cpp b/src/platforms/posix/main.cpp
index 6d9d33e7e2..ccb9aaf494 100644
--- a/src/platforms/posix/main.cpp
+++ b/src/platforms/posix/main.cpp
@@ -377,6 +377,7 @@ int main(int argc, char **argv)
 					cout << c;
 					mystr += c;
 				}
+
 				break;
 			}
 		}
diff --git a/src/platforms/posix/px4_layer/px4_log.c b/src/platforms/posix/px4_layer/px4_log.c
index 074ae8b466..88406a8854 100644
--- a/src/platforms/posix/px4_layer/px4_log.c
+++ b/src/platforms/posix/px4_layer/px4_log.c
@@ -7,6 +7,8 @@
 __EXPORT int __px4_log_level_current = PX4_LOG_LEVEL_AT_RUN_TIME;
 
 __EXPORT const char *__px4_log_level_str[_PX4_LOG_LEVEL_PANIC + 1] = { "INFO", "DEBUG", "WARN", "ERROR", "PANIC" };
+__EXPORT const char *__px4_log_level_color[_PX4_LOG_LEVEL_PANIC + 1] =
+	{ PX4_ANSI_COLOR_RESET, PX4_ANSI_COLOR_GREEN, PX4_ANSI_COLOR_YELLOW, PX4_ANSI_COLOR_RED, PX4_ANSI_COLOR_RED };
 
 void px4_backtrace()
 {
diff --git a/src/platforms/px4_log.h b/src/platforms/px4_log.h
index f9e47196e2..b912f7dad3 100644
--- a/src/platforms/px4_log.h
+++ b/src/platforms/px4_log.h
@@ -130,7 +130,8 @@ static inline void do_nothing(int level, ...)
 __BEGIN_DECLS
 __EXPORT extern uint64_t hrt_absolute_time(void);
 
-__EXPORT extern const char *__px4_log_level_str[5];
+__EXPORT extern const char *__px4_log_level_str[_PX4_LOG_LEVEL_PANIC + 1];
+__EXPORT extern const char *__px4_log_level_color[_PX4_LOG_LEVEL_PANIC + 1];
 __EXPORT extern int __px4_log_level_current;
 __EXPORT extern void px4_backtrace(void);
 __END_DECLS
@@ -171,6 +172,38 @@ __END_DECLS
 #define __px4__log_file_and_line_arg 	, __FILE__, __LINE__
 #define __px4__log_end_fmt 		"\n"
 
+#define PX4_ANSI_COLOR_RED     "\x1b[31m"
+#define PX4_ANSI_COLOR_GREEN   "\x1b[32m"
+#define PX4_ANSI_COLOR_YELLOW  "\x1b[33m"
+#define PX4_ANSI_COLOR_BLUE    "\x1b[34m"
+#define PX4_ANSI_COLOR_MAGENTA "\x1b[35m"
+#define PX4_ANSI_COLOR_CYAN    "\x1b[36m"
+#define PX4_ANSI_COLOR_GRAY    "\x1B[37m"
+#define PX4_ANSI_COLOR_RESET   "\x1b[0m"
+
+#ifdef __PX4_POSIX
+#define PX4_LOG_COLORIZED_OUTPUT //if defined and output is a tty, colorize the output according to the log level
+#endif /* __PX4_POSIX */
+
+
+#ifdef PX4_LOG_COLORIZED_OUTPUT
+#include <unistd.h>
+#define PX4_LOG_COLOR_START \
+	int use_color = isatty(STDOUT_FILENO); \
+	if (use_color) printf("%s", __px4_log_level_color[level]);
+#define PX4_LOG_COLOR_MODULE \
+	if (use_color) printf(PX4_ANSI_COLOR_GRAY);
+#define PX4_LOG_COLOR_MESSAGE \
+	if (use_color) printf("%s", __px4_log_level_color[level]);
+#define PX4_LOG_COLOR_END \
+	if (use_color) printf(PX4_ANSI_COLOR_RESET);
+#else
+#define PX4_LOG_COLOR_START
+#define PX4_LOG_COLOR_MODULE
+#define PX4_LOG_COLOR_MESSAGE
+#define PX4_LOG_COLOR_END
+#endif /* PX4_LOG_COLORIZED_OUTPUT */
+
 /****************************************************************************
  * Output format macros
  * Use these to implement the code level macros below
@@ -235,12 +268,16 @@ __END_DECLS
 static inline void __px4_log_modulename(int level, const char *fmt, ...)
 {
 	if (level <= __px4_log_level_current) {
+		PX4_LOG_COLOR_START
 		printf(__px4__log_level_fmt __px4__log_level_arg(level));
+		PX4_LOG_COLOR_MODULE
 		printf(__px4__log_modulename_fmt __px4__log_modulename_arg);
+		PX4_LOG_COLOR_MESSAGE
 		va_list argptr;
 		va_start(argptr, fmt);
 		vprintf(fmt, argptr);
 		va_end(argptr);
+		PX4_LOG_COLOR_END
 		printf("\n");
 	}
 }

From 29eac9b7cbf66553e94d348a40afd7da7aa17ccf Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Beat=20K=C3=BCng?= <beat-kueng@gmx.net>
Date: Thu, 9 Jun 2016 13:16:04 +0200
Subject: [PATCH 0596/1230] px4_log.c: fix code style

---
 src/platforms/posix/px4_layer/px4_log.c | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/src/platforms/posix/px4_layer/px4_log.c b/src/platforms/posix/px4_layer/px4_log.c
index 88406a8854..37d08564b1 100644
--- a/src/platforms/posix/px4_layer/px4_log.c
+++ b/src/platforms/posix/px4_layer/px4_log.c
@@ -8,7 +8,7 @@ __EXPORT int __px4_log_level_current = PX4_LOG_LEVEL_AT_RUN_TIME;
 
 __EXPORT const char *__px4_log_level_str[_PX4_LOG_LEVEL_PANIC + 1] = { "INFO", "DEBUG", "WARN", "ERROR", "PANIC" };
 __EXPORT const char *__px4_log_level_color[_PX4_LOG_LEVEL_PANIC + 1] =
-	{ PX4_ANSI_COLOR_RESET, PX4_ANSI_COLOR_GREEN, PX4_ANSI_COLOR_YELLOW, PX4_ANSI_COLOR_RED, PX4_ANSI_COLOR_RED };
+{ PX4_ANSI_COLOR_RESET, PX4_ANSI_COLOR_GREEN, PX4_ANSI_COLOR_YELLOW, PX4_ANSI_COLOR_RED, PX4_ANSI_COLOR_RED };
 
 void px4_backtrace()
 {

From 039d5528ddfba211d07d3e970d0e6d2922ea1765 Mon Sep 17 00:00:00 2001
From: Stephen Street <sgstreet@users.noreply.github.com>
Date: Thu, 9 Jun 2016 12:08:57 -0700
Subject: [PATCH 0597/1230] Change prefix construction to allow correct
 building of scope tag in parameters.xml when building out-of-tree (#4781)

Fixes issue #4767
---
 Tools/px4params/srcscanner.py | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/Tools/px4params/srcscanner.py b/Tools/px4params/srcscanner.py
index 2b5f5f3fb0..085c956139 100644
--- a/Tools/px4params/srcscanner.py
+++ b/Tools/px4params/srcscanner.py
@@ -33,7 +33,7 @@ class SourceScanner(object):
         Scans provided file and passes its contents to the parser using
         parser.Parse method.
         """
-        prefix = ".." + os.path.sep + "src" + os.path.sep
+        prefix = "^.*" + os.path.sep + "src" + os.path.sep
         scope = re.sub(prefix.replace("\\", "/"), "", os.path.dirname(os.path.relpath(path)).replace("\\", "/"))
 
         with codecs.open(path, 'r', 'utf-8') as f:

From 0a27d14f6cce7812755765c9ec0b9f256516abed Mon Sep 17 00:00:00 2001
From: Mark Whitehorn <kd0aij@gmail.com>
Date: Thu, 9 Jun 2016 15:33:57 -0600
Subject: [PATCH 0598/1230] robustify S.port and D telemetry detection (#4731)

---
 src/drivers/frsky_telemetry/frsky_telemetry.c | 94 ++++++++++++++-----
 1 file changed, 70 insertions(+), 24 deletions(-)

diff --git a/src/drivers/frsky_telemetry/frsky_telemetry.c b/src/drivers/frsky_telemetry/frsky_telemetry.c
index 77507d2e6b..ad0e6a5a8c 100644
--- a/src/drivers/frsky_telemetry/frsky_telemetry.c
+++ b/src/drivers/frsky_telemetry/frsky_telemetry.c
@@ -67,8 +67,8 @@
 static volatile bool thread_should_exit = false;
 static volatile bool thread_running = false;
 static int frsky_task;
-typedef enum { IDLE, SPORT, DTYPE } frsky_state_t;
-static frsky_state_t frsky_state = IDLE;
+typedef enum { SCANNING, SPORT, DTYPE } frsky_state_t;
+static frsky_state_t frsky_state = SCANNING;
 
 /* functions */
 static int sPort_open_uart(const char *uart_name, struct termios *uart_config, struct termios *uart_config_original);
@@ -77,6 +77,7 @@ static void usage(void);
 static int frsky_telemetry_thread_main(int argc, char *argv[]);
 __EXPORT int frsky_telemetry_main(int argc, char *argv[]);
 
+#define DEBUG
 
 /**
  * Opens the UART device and sets all required serial parameters.
@@ -106,7 +107,7 @@ static int sPort_open_uart(const char *uart_name, struct termios *uart_config, s
 	uart_config->c_oflag &= ~OPOST;
 
 	/* Set baud rate */
-	static const speed_t speed = B57600;
+	static const speed_t speed = B9600;
 
 	if (cfsetispeed(uart_config, speed) < 0 || cfsetospeed(uart_config, speed) < 0) {
 		warnx("ERR: %s: %d (cfsetispeed, cfsetospeed)\n", uart_name, termios_state);
@@ -175,7 +176,7 @@ static int frsky_telemetry_thread_main(int argc, char *argv[])
 		}
 	}
 
-	/* Open UART assuming SmartPort telemetry */
+	/* Open UART assuming D type telemetry */
 	struct termios uart_config_original;
 	struct termios uart_config;
 	const int uart = sPort_open_uart(device_name, &uart_config, &uart_config_original);
@@ -194,9 +195,10 @@ static int frsky_telemetry_thread_main(int argc, char *argv[])
 
 	/* Main thread loop */
 	char sbuf[20];
-	frsky_state = IDLE;
+	frsky_state = SCANNING;
+	frsky_state_t baudRate = DTYPE;
 
-	while (!thread_should_exit && frsky_state == IDLE) {
+	while (!thread_should_exit && frsky_state == SCANNING) {
 		/* 2 byte polling frames indicate SmartPort telemetry
 		 * 11 byte packets indicate D type telemetry
 		 */
@@ -204,18 +206,64 @@ static int frsky_telemetry_thread_main(int argc, char *argv[])
 
 		if (status > 0) {
 			/* traffic on the port, D type is 11 bytes per frame, SmartPort is only 2
-			 * allow a little time to receive the entire packet
+			 * Wait long enough for 11 bytes at 9600 baud
 			 */
-			usleep(5000);
-			status = read(uart, &sbuf[0], sizeof(sbuf));
-		}
+			usleep(12000);
+			int nbytes = read(uart, &sbuf[0], sizeof(sbuf));
+			PX4_DEBUG("frsky input: %d bytes: %x %x, speed: %d", nbytes, sbuf[0], sbuf[1], baudRate);
 
-		/* received some data; check size of packet */
-		if (status > 0 && status < 3) {
-			frsky_state = SPORT;
+			// look for valid header byte
+			if (nbytes > 10) {
+				if (baudRate == DTYPE) {
+					// see if we got a valid D-type hostframe
+					struct adc_linkquality host_frame;
+
+					if (frsky_parse_host((uint8_t *)&sbuf[0], nbytes, &host_frame)) {
+						frsky_state = baudRate;
+						break;
+					}
+
+				} else {
+					// check for alternating S.port start bytes
+					int index = 0;
+
+					while (index < 2 && sbuf[index] != 0x7E) { index++; }
+
+					if (index < 2) {
+
+						int success = 1;
+
+						for (int i = index + 2; i < nbytes; i += 2) {
+							if (sbuf[i] != 0x7E) { success = 0; break; }
+						}
+
+						if (success) {
+							frsky_state = baudRate;
+							break;
+						}
+					}
+				}
+
+			}
+
+			// alternate between S.port and D-type baud rates
+			if (baudRate == SPORT) {
+				PX4_DEBUG("setting baud rate to %d", 9600);
+				set_uart_speed(uart, &uart_config, B9600);
+				baudRate = DTYPE;
+
+			} else {
+				PX4_DEBUG("setting baud rate to %d", 57600);
+				set_uart_speed(uart, &uart_config, B57600);
+				baudRate = SPORT;
+
+			}
+
+			// wait a second
+			usleep(1000000);
+			// flush buffer
+			read(uart, &sbuf[0], sizeof(sbuf));
 
-		} else if (status > 0) {
-			frsky_state = DTYPE;
 		}
 	}
 
@@ -225,7 +273,7 @@ static int frsky_telemetry_thread_main(int argc, char *argv[])
 			err(1, "could not allocate memory");
 		}
 
-		warnx("sending FrSky SmartPort telemetry");
+		PX4_INFO("sending FrSky SmartPort telemetry");
 
 		struct sensor_baro_s *sensor_baro = malloc(sizeof(struct sensor_baro_s));
 
@@ -434,7 +482,7 @@ static int frsky_telemetry_thread_main(int argc, char *argv[])
 			}
 		}
 
-		warnx("freeing sPort memory");
+		PX4_DEBUG("freeing sPort memory");
 		sPort_deinit();
 		free(sensor_baro);
 
@@ -442,11 +490,11 @@ static int frsky_telemetry_thread_main(int argc, char *argv[])
 
 	} else if (frsky_state == DTYPE) {
 		/* detected D type telemetry: reconfigure UART */
-		warnx("sending FrSky D type telemetry");
+		PX4_INFO("sending FrSky D type telemetry");
 		int status = set_uart_speed(uart, &uart_config, B9600);
 
 		if (status < 0) {
-			warnx("error setting speed for %s, quitting", device_name);
+			PX4_DEBUG("error setting speed for %s, quitting", device_name);
 			/* Reset the UART flags to original state */
 			tcsetattr(uart, TCSANOW, &uart_config_original);
 			close(uart);
@@ -464,8 +512,6 @@ static int frsky_telemetry_thread_main(int argc, char *argv[])
 
 		struct adc_linkquality host_frame;
 
-//		uint8_t dbuf[45];
-
 		/* send D8 mode telemetry */
 		while (!thread_should_exit) {
 
@@ -506,7 +552,7 @@ static int frsky_telemetry_thread_main(int argc, char *argv[])
 
 //		/* TODO: flush the input buffer if in full duplex mode */
 //		read(uart, &sbuf[0], sizeof(sbuf));
-		warnx("freeing frsky memory");
+		PX4_DEBUG("freeing frsky memory");
 		frsky_deinit();
 
 	}
@@ -573,8 +619,8 @@ int frsky_telemetry_main(int argc, char *argv[])
 	if (!strcmp(argv[1], "status")) {
 		if (thread_running) {
 			switch (frsky_state) {
-			case IDLE:
-				errx(0, "running: IDLE");
+			case SCANNING:
+				errx(0, "running: SCANNING");
 				break;
 
 			case SPORT:

From f4f0892b25447d3e7a20312ebee429b4f8d06fe1 Mon Sep 17 00:00:00 2001
From: Julian Oes <julian@oes.ch>
Date: Fri, 10 Jun 2016 07:48:37 +0100
Subject: [PATCH 0599/1230] sdlog2: no new sessXXX folder on every arm (#4775)

Previously, if no time was set, sdlog2 created a folder like sess001,
sess002 for every logfile. The logfiles would then always be named
log001.px4log and their numbering would not actually increase.

This is now fixed and a new folder is only created per boot.
---
 src/modules/sdlog2/sdlog2.c | 11 +++++++++--
 1 file changed, 9 insertions(+), 2 deletions(-)

diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c
index ceab974656..7543560eac 100644
--- a/src/modules/sdlog2/sdlog2.c
+++ b/src/modules/sdlog2/sdlog2.c
@@ -196,6 +196,10 @@ static pthread_attr_t logwriter_attr;
 
 static perf_counter_t perf_write;
 
+/* Keep track if we've already created a folder named sessXXX because
+ * we don't want to create yet another one. */
+static bool sess_folder_created = false;
+
 /**
  * Log buffer writing thread. Open and close file here.
  */
@@ -452,13 +456,16 @@ int create_log_dir()
 		}
 
 	} else {
-		/* look for the next dir that does not exist */
-		while (dir_number <= MAX_NO_LOGFOLDER) {
+		/* Look for the next dir that does not exist.
+		 * However, if we've already crated a sessXXX folder in this session
+		 * let's re-use it. */
+		while (dir_number <= MAX_NO_LOGFOLDER && !sess_folder_created) {
 			/* format log dir: e.g. /fs/microsd/sess001 */
 			sprintf(log_dir, "%s/sess%03u", log_root, dir_number);
 			mkdir_ret = mkdir(log_dir, S_IRWXU | S_IRWXG | S_IRWXO);
 
 			if (mkdir_ret == 0) {
+				sess_folder_created = true;
 				break;
 
 			} else if (errno != EEXIST) {

From f2509117769d92b26fd84f7e36b35e52f9649e85 Mon Sep 17 00:00:00 2001
From: Mark Whitehorn <kd0aij@gmail.com>
Date: Mon, 30 May 2016 20:30:04 -0600
Subject: [PATCH 0600/1230] read topics to log from a text file

---
 .../px4fmu_common/logging/default_topics.txt  | 21 +++++
 src/modules/logger/logger.cpp                 | 76 +++++++++++++------
 src/modules/logger/logger.h                   |  7 ++
 3 files changed, 82 insertions(+), 22 deletions(-)
 create mode 100644 ROMFS/px4fmu_common/logging/default_topics.txt

diff --git a/ROMFS/px4fmu_common/logging/default_topics.txt b/ROMFS/px4fmu_common/logging/default_topics.txt
new file mode 100644
index 0000000000..cb06411e21
--- /dev/null
+++ b/ROMFS/px4fmu_common/logging/default_topics.txt
@@ -0,0 +1,21 @@
+sensor_gyro
+sensor_accel
+vehicle_rates_setpoint, 10
+vehicle_attitude_setpoint, 10
+vehicle_attitude
+actuator_outputs, 50
+battery_status, 100
+vehicle_command, 100
+actuator_controls, 10
+vehicle_local_position_setpoint, 200
+rc_channels, 20
+#ekf2_innovations, 20
+commander_state, 100
+vehicle_local_position, 200
+vehicle_global_position, 200
+system_power, 100
+servorail_status, 200
+mc_att_ctrl_status, 50
+#control_state
+#estimator_status
+vehicle_status, 200
diff --git a/src/modules/logger/logger.cpp b/src/modules/logger/logger.cpp
index 3a6a5b8c56..fce47c44c5 100644
--- a/src/modules/logger/logger.cpp
+++ b/src/modules/logger/logger.cpp
@@ -422,6 +422,59 @@ bool Logger::copy_if_updated_multi(LoggerSubscription &sub, int multi_instance,
 	return updated;
 }
 
+int Logger::add_topics_from_file(const char *fname)
+{
+	FILE		*fp;
+	char		line[80];
+	char		topic_name[40];
+	unsigned	interval;
+	int			ntopics = 0;
+
+	/* open the topic list file */
+	fp = fopen(fname, "r");
+
+	if (fp == NULL) {
+		warnx("file not found");
+		return -1;
+	}
+
+	/* call add_topic for each topic line in the file */
+	// format is TOPIC_NAME, [interval]
+	for (;;) {
+
+		/* get a line, bail on error/EOF */
+		line[0] = '\0';
+
+		if (fgets(line, sizeof(line), fp) == NULL) {
+			break;
+		}
+
+		/* skip comment lines */
+		if ((strlen(line) < 2) || (line[1] == '#')) {
+			continue;
+		}
+
+		int nfields = sscanf(line, "%s, %u", &topic_name[0], &interval);
+		switch (nfields) {
+		case 1:
+			/* add topic with default interval */
+			add_topic(&topic_name[0], 0);
+			ntopics++;
+			break;
+		case 2:
+			/* add topic with specified interval */
+			add_topic(&topic_name[0], interval);
+			ntopics++;
+			break;
+		default:
+			break;
+		}
+	}
+
+	fclose(fp);
+	return ntopics;
+}
+
 void Logger::run()
 {
 #ifdef DBGPRINT
@@ -448,28 +501,7 @@ void Logger::run()
 	uORB::Subscription<parameter_update_s> parameter_update_sub(ORB_ID(parameter_update));
 	uORB::Subscription<mavlink_log_s> mavlink_log_sub(ORB_ID(mavlink_log));
 
-
-	add_topic("sensor_gyro", 0);
-	add_topic("sensor_accel", 0);
-	add_topic("vehicle_rates_setpoint", 10);
-	add_topic("vehicle_attitude_setpoint", 10);
-	add_topic("vehicle_attitude", 0);
-	add_topic("actuator_outputs", 50);
-	add_topic("battery_status", 100);
-	add_topic("vehicle_command", 100);
-	add_topic("actuator_controls", 10);
-	add_topic("vehicle_local_position_setpoint", 200);
-	add_topic("rc_channels", 20);
-//	add_topic("ekf2_innovations", 20);
-	add_topic("commander_state", 100);
-	add_topic("vehicle_local_position", 200);
-	add_topic("vehicle_global_position", 200);
-	add_topic("system_power", 100);
-	add_topic("servorail_status", 200);
-	add_topic("mc_att_ctrl_status", 50);
-//	add_topic("control_state");
-//	add_topic("estimator_status");
-	add_topic("vehicle_status", 200);
+	add_topics_from_file("/etc/logging/default_topics.txt");
 
 	//all topics added. Get required message buffer size
 	int max_msg_size = 0;
diff --git a/src/modules/logger/logger.h b/src/modules/logger/logger.h
index ea3f56de92..0890ec6962 100644
--- a/src/modules/logger/logger.h
+++ b/src/modules/logger/logger.h
@@ -186,6 +186,13 @@ private:
 	 */
 	bool get_log_time(struct tm *tt, bool boot_time = false);
 
+	/**
+	 * Parse a file containing a list of uORB topics to log, calling add_topic for each
+	 * @param fname name of file
+	 * @return number of topics added
+	 */
+	int add_topics_from_file(const char *fname);
+
 	static constexpr size_t 	MAX_TOPICS_NUM = 64; /**< Maximum number of logged topics */
 	static constexpr unsigned	MAX_NO_LOGFOLDER = 999;	/**< Maximum number of log dirs */
 	static constexpr unsigned	MAX_NO_LOGFILE = 999;	/**< Maximum number of log files */

From 2bd15f169894abf19bc98abb56022ba1c5860e0f Mon Sep 17 00:00:00 2001
From: Mark Whitehorn <kd0aij@gmail.com>
Date: Tue, 31 May 2016 07:03:08 -0600
Subject: [PATCH 0601/1230] fix string handling issues and simplify parser

---
 src/modules/logger/logger.cpp | 21 +++++++--------------
 1 file changed, 7 insertions(+), 14 deletions(-)

diff --git a/src/modules/logger/logger.cpp b/src/modules/logger/logger.cpp
index fce47c44c5..9ccb5b9600 100644
--- a/src/modules/logger/logger.cpp
+++ b/src/modules/logger/logger.cpp
@@ -426,7 +426,7 @@ int Logger::add_topics_from_file(const char *fname)
 {
 	FILE		*fp;
 	char		line[80];
-	char		topic_name[40];
+	char		topic_name[80];
 	unsigned	interval;
 	int			ntopics = 0;
 
@@ -450,24 +450,17 @@ int Logger::add_topics_from_file(const char *fname)
 		}
 
 		/* skip comment lines */
-		if ((strlen(line) < 2) || (line[1] == '#')) {
+		if ((strlen(line) < 2) || (line[0] == '#')) {
 			continue;
 		}
 
-		int nfields = sscanf(line, "%s, %u", &topic_name[0], &interval);
-		switch (nfields) {
-		case 1:
-			/* add topic with default interval */
-			add_topic(&topic_name[0], 0);
-			ntopics++;
-			break;
-		case 2:
+		// default interval to zero
+		interval = 0;
+		int nfields = sscanf(line, "%s, %u", topic_name, &interval);
+		if (nfields > 0) {
 			/* add topic with specified interval */
-			add_topic(&topic_name[0], interval);
+			add_topic(topic_name, interval);
 			ntopics++;
-			break;
-		default:
-			break;
 		}
 	}
 

From 0ca63fa37913f5adff53673226fa168fc3f1704f Mon Sep 17 00:00:00 2001
From: Mark Whitehorn <kd0aij@gmail.com>
Date: Tue, 31 May 2016 09:24:19 -0600
Subject: [PATCH 0602/1230] move default topics into logger.cpp; add on/off
 command

look for alternate topic list at
/fs/microsd/etc/logging/logger_topics.txt
on and off commands have the same effect as arm/disarm
---
 src/modules/logger/logger.cpp | 55 +++++++++++++++++++++++++++++++++--
 src/modules/logger/logger.h   |  6 ++++
 2 files changed, 58 insertions(+), 3 deletions(-)

diff --git a/src/modules/logger/logger.cpp b/src/modules/logger/logger.cpp
index 9ccb5b9600..d9a67a3524 100644
--- a/src/modules/logger/logger.cpp
+++ b/src/modules/logger/logger.cpp
@@ -120,6 +120,24 @@ int logger_main(int argc, char *argv[])
 		return 0;
 	}
 
+	if (!strcmp(argv[1], "on")) {
+		if (logger_ptr != nullptr) {
+			logger_ptr->set_arm_override(true);
+			return 0;
+		}
+
+		return 1;
+	}
+
+	if (!strcmp(argv[1], "off")) {
+		if (logger_ptr != nullptr) {
+			logger_ptr->set_arm_override(false);
+			return 0;
+		}
+
+		return 1;
+	}
+
 	if (!strcmp(argv[1], "stop")) {
 		if (logger_ptr == nullptr) {
 			PX4_INFO("not running");
@@ -157,7 +175,7 @@ void Logger::usage(const char *reason)
 		PX4_WARN("%s\n", reason);
 	}
 
-	PX4_INFO("usage: logger {start|stop|status} [-r <log rate>] [-b <buffer size>] -e -a -t -x\n"
+	PX4_INFO("usage: logger {start|stop|on|off|status} [-r <log rate>] [-b <buffer size>] -e -a -t -x\n"
 		 "\t-r\tLog rate in Hz, 0 means unlimited rate\n"
 		 "\t-b\tLog buffer size in KiB, default is 12\n"
 		 "\t-e\tEnable logging right after start until disarm (otherwise only when armed)\n"
@@ -295,6 +313,7 @@ void Logger::run_trampoline(int argc, char *argv[])
 
 Logger::Logger(size_t buffer_size, unsigned log_interval, bool log_on_start,
 	       bool log_until_shutdown, bool log_name_timestamp) :
+	_arm_override(false),
 	_log_on_start(log_on_start),
 	_log_until_shutdown(log_until_shutdown),
 	_log_name_timestamp(log_name_timestamp),
@@ -422,6 +441,28 @@ bool Logger::copy_if_updated_multi(LoggerSubscription &sub, int multi_instance,
 	return updated;
 }
 
+void Logger::add_default_topics()
+{
+	add_topic("sensor_gyro", 0);
+	add_topic("sensor_accel", 0);
+	add_topic("vehicle_rates_setpoint", 10);
+	add_topic("vehicle_attitude_setpoint", 10);
+	add_topic("vehicle_attitude", 0);
+	add_topic("actuator_outputs", 50);
+	add_topic("battery_status", 100);
+	add_topic("vehicle_command", 100);
+	add_topic("actuator_controls", 10);
+	add_topic("vehicle_local_position_setpoint", 200);
+	add_topic("rc_channels", 20);
+	add_topic("commander_state", 100);
+	add_topic("vehicle_local_position", 200);
+	add_topic("vehicle_global_position", 200);
+	add_topic("system_power", 100);
+	add_topic("servorail_status", 200);
+	add_topic("mc_att_ctrl_status", 50);
+	add_topic("vehicle_status", 200);
+}
+
 int Logger::add_topics_from_file(const char *fname)
 {
 	FILE		*fp;
@@ -457,6 +498,7 @@ int Logger::add_topics_from_file(const char *fname)
 		// default interval to zero
 		interval = 0;
 		int nfields = sscanf(line, "%s, %u", topic_name, &interval);
+
 		if (nfields > 0) {
 			/* add topic with specified interval */
 			add_topic(topic_name, interval);
@@ -494,7 +536,13 @@ void Logger::run()
 	uORB::Subscription<parameter_update_s> parameter_update_sub(ORB_ID(parameter_update));
 	uORB::Subscription<mavlink_log_s> mavlink_log_sub(ORB_ID(mavlink_log));
 
-	add_topics_from_file("/etc/logging/default_topics.txt");
+	int ntopics = add_topics_from_file("/fs/microsd/etc/logging/logger_topics.txt");
+	warnx("logging %d topics from logger_topics.txt", ntopics);
+
+	if (ntopics < 1) {
+		warnx("logging default topics");
+		add_default_topics();
+	}
 
 	//all topics added. Get required message buffer size
 	int max_msg_size = 0;
@@ -559,7 +607,8 @@ void Logger::run()
 		if (vehicle_status_sub.check_updated()) {
 			vehicle_status_sub.update();
 			bool armed = (vehicle_status_sub.get().arming_state == vehicle_status_s::ARMING_STATE_ARMED) ||
-				     (vehicle_status_sub.get().arming_state == vehicle_status_s::ARMING_STATE_ARMED_ERROR);
+				     (vehicle_status_sub.get().arming_state == vehicle_status_s::ARMING_STATE_ARMED_ERROR) ||
+				     _arm_override;
 
 			if (_was_armed != armed && !_log_until_shutdown) {
 				_was_armed = armed;
diff --git a/src/modules/logger/logger.h b/src/modules/logger/logger.h
index 0890ec6962..d22e141123 100644
--- a/src/modules/logger/logger.h
+++ b/src/modules/logger/logger.h
@@ -107,6 +107,8 @@ public:
 
 	void status();
 
+	void set_arm_override(bool override) { _arm_override = override; }
+
 private:
 	static void run_trampoline(int argc, char *argv[]);
 
@@ -193,6 +195,8 @@ private:
 	 */
 	int add_topics_from_file(const char *fname);
 
+	void add_default_topics();
+
 	static constexpr size_t 	MAX_TOPICS_NUM = 64; /**< Maximum number of logged topics */
 	static constexpr unsigned	MAX_NO_LOGFOLDER = 999;	/**< Maximum number of log dirs */
 	static constexpr unsigned	MAX_NO_LOGFILE = 999;	/**< Maximum number of log files */
@@ -209,6 +213,8 @@ private:
 	bool						_has_log_dir = false;
 	bool						_enabled = false;
 	bool						_was_armed = false;
+	bool						_arm_override;
+
 
 	// statistics
 	hrt_abstime					_start_time; ///< Time when logging started (not the logger thread)

From f129b86d1b5446d88321efc6838226deba290e0b Mon Sep 17 00:00:00 2001
From: Mark Whitehorn <kd0aij@gmail.com>
Date: Tue, 31 May 2016 09:24:45 -0600
Subject: [PATCH 0603/1230] reduce logbuffer size to 12K due to lack of RAM

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

diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS
index dda3ffdd79..e6d95e59f7 100644
--- a/ROMFS/px4fmu_common/init.d/rcS
+++ b/ROMFS/px4fmu_common/init.d/rcS
@@ -638,7 +638,7 @@ then
 				then
 				fi
 			else
-				if logger start -b 9
+				if logger start -b 12
 				then
 				fi
 			fi

From 0bf7c39356c4259b24a093a67c7717d22c3c6775 Mon Sep 17 00:00:00 2001
From: Mark Whitehorn <kd0aij@gmail.com>
Date: Thu, 2 Jun 2016 06:28:27 -0600
Subject: [PATCH 0604/1230] remove extraneous topic list file

---
 .../px4fmu_common/logging/default_topics.txt  | 21 -------------------
 1 file changed, 21 deletions(-)
 delete mode 100644 ROMFS/px4fmu_common/logging/default_topics.txt

diff --git a/ROMFS/px4fmu_common/logging/default_topics.txt b/ROMFS/px4fmu_common/logging/default_topics.txt
deleted file mode 100644
index cb06411e21..0000000000
--- a/ROMFS/px4fmu_common/logging/default_topics.txt
+++ /dev/null
@@ -1,21 +0,0 @@
-sensor_gyro
-sensor_accel
-vehicle_rates_setpoint, 10
-vehicle_attitude_setpoint, 10
-vehicle_attitude
-actuator_outputs, 50
-battery_status, 100
-vehicle_command, 100
-actuator_controls, 10
-vehicle_local_position_setpoint, 200
-rc_channels, 20
-#ekf2_innovations, 20
-commander_state, 100
-vehicle_local_position, 200
-vehicle_global_position, 200
-system_power, 100
-servorail_status, 200
-mc_att_ctrl_status, 50
-#control_state
-#estimator_status
-vehicle_status, 200

From 3bfedfff19e408e0c7201902388260e4a46872d8 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Beat=20K=C3=BCng?= <beat-kueng@gmx.net>
Date: Fri, 10 Jun 2016 08:50:16 +0200
Subject: [PATCH 0605/1230] logger: printf cleanup, output statistics when
 stopping the logger

---
 src/modules/logger/logger.cpp | 48 ++++++++++++++++++++---------------
 src/modules/logger/logger.h   |  1 +
 2 files changed, 29 insertions(+), 20 deletions(-)

diff --git a/src/modules/logger/logger.cpp b/src/modules/logger/logger.cpp
index d9a67a3524..f86d9eae8c 100644
--- a/src/modules/logger/logger.cpp
+++ b/src/modules/logger/logger.cpp
@@ -144,6 +144,7 @@ int logger_main(int argc, char *argv[])
 			return 1;
 		}
 
+		logger_ptr->print_statistics();
 		delete logger_ptr;
 		logger_ptr = nullptr;
 		return 0;
@@ -211,19 +212,26 @@ void Logger::status()
 
 	} else {
 		PX4_INFO("Running");
-
-		float kibibytes = _writer.get_total_written() / 1024.0f;
-		float mebibytes = kibibytes / 1024.0f;
-		float seconds = ((float)(hrt_absolute_time() - _start_time)) / 1000000.0f;
-
-		PX4_INFO("Wrote %4.2f MiB (avg %5.2f KiB/s)", (double)mebibytes, (double)(kibibytes / seconds));
-		PX4_INFO("Since last status: dropouts: %zu (max len: %.3f s), max used buffer: %zu / %zu B",
-			 _write_dropouts, (double)_max_dropout_duration, _high_water, _writer.get_buffer_size());
-		_high_water = 0;
-		_write_dropouts = 0;
-		_max_dropout_duration = 0.f;
+		print_statistics();
 	}
 }
+void Logger::print_statistics()
+{
+	if (!_enabled) {
+		return;
+	}
+
+	float kibibytes = _writer.get_total_written() / 1024.0f;
+	float mebibytes = kibibytes / 1024.0f;
+	float seconds = ((float)(hrt_absolute_time() - _start_time)) / 1000000.0f;
+
+	PX4_INFO("Wrote %4.2f MiB (avg %5.2f KiB/s)", (double)mebibytes, (double)(kibibytes / seconds));
+	PX4_INFO("Since last status: dropouts: %zu (max len: %.3f s), max used buffer: %zu / %zu B",
+		 _write_dropouts, (double)_max_dropout_duration, _high_water, _writer.get_buffer_size());
+	_high_water = 0;
+	_write_dropouts = 0;
+	_max_dropout_duration = 0.f;
+}
 
 void Logger::run_trampoline(int argc, char *argv[])
 {
@@ -296,12 +304,12 @@ void Logger::run_trampoline(int argc, char *argv[])
 
 #if defined(DBGPRINT) && defined(__PX4_NUTTX)
 	struct mallinfo alloc_info = mallinfo();
-	warnx("largest free chunk: %d bytes", alloc_info.mxordblk);
-	warnx("remaining free heap: %d bytes", alloc_info.fordblks);
+	PX4_INFO("largest free chunk: %d bytes", alloc_info.mxordblk);
+	PX4_INFO("remaining free heap: %d bytes", alloc_info.fordblks);
 #endif /* DBGPRINT */
 
 	if (logger_ptr == nullptr) {
-		PX4_WARN("alloc failed");
+		PX4_ERR("alloc failed");
 
 	} else {
 		logger_ptr->run();
@@ -386,7 +394,7 @@ int Logger::add_topic(const char *name, unsigned interval = 0)
 	for (size_t i = 0; i < orb_topics_count(); i++) {
 		if (strcmp(name, topics[i]->o_name) == 0) {
 			fd = add_topic(topics[i]);
-			PX4_INFO("logging topic: %zu, %s", i, topics[i]->o_name);
+			PX4_DEBUG("logging topic: %s, interval: %i", topics[i]->o_name, interval);
 			break;
 		}
 	}
@@ -475,7 +483,6 @@ int Logger::add_topics_from_file(const char *fname)
 	fp = fopen(fname, "r");
 
 	if (fp == NULL) {
-		warnx("file not found");
 		return -1;
 	}
 
@@ -536,11 +543,12 @@ void Logger::run()
 	uORB::Subscription<parameter_update_s> parameter_update_sub(ORB_ID(parameter_update));
 	uORB::Subscription<mavlink_log_s> mavlink_log_sub(ORB_ID(mavlink_log));
 
-	int ntopics = add_topics_from_file("/fs/microsd/etc/logging/logger_topics.txt");
-	warnx("logging %d topics from logger_topics.txt", ntopics);
+	int ntopics = add_topics_from_file(PX4_ROOTFSDIR "/fs/microsd/etc/logging/logger_topics.txt");
 
-	if (ntopics < 1) {
-		warnx("logging default topics");
+	if (ntopics > 0) {
+		PX4_INFO("logging %d topics from logger_topics.txt", ntopics);
+
+	} else {
 		add_default_topics();
 	}
 
diff --git a/src/modules/logger/logger.h b/src/modules/logger/logger.h
index d22e141123..d1e96658fe 100644
--- a/src/modules/logger/logger.h
+++ b/src/modules/logger/logger.h
@@ -106,6 +106,7 @@ public:
 	static void usage(const char *reason);
 
 	void status();
+	void print_statistics();
 
 	void set_arm_override(bool override) { _arm_override = override; }
 

From da0813b84258335b291b3623db7a0db3169f6b99 Mon Sep 17 00:00:00 2001
From: Julian Oes <julian@oes.ch>
Date: Wed, 8 Jun 2016 14:20:36 +0200
Subject: [PATCH 0606/1230] Snapdragon: make RC calibration easier

20 Hz of RC_CHANNELS makes the RC calibration in QGC a more pleasant
thing.
---
 posix-configs/eagle/flight/mainapp.config | 1 +
 1 file changed, 1 insertion(+)

diff --git a/posix-configs/eagle/flight/mainapp.config b/posix-configs/eagle/flight/mainapp.config
index 685eba34f1..b7441d911a 100644
--- a/posix-configs/eagle/flight/mainapp.config
+++ b/posix-configs/eagle/flight/mainapp.config
@@ -8,4 +8,5 @@ mavlink start -u 14556 -r 1000000
 sleep 1
 mavlink stream -u 14556 -s HIGHRES_IMU -r 50
 mavlink stream -u 14556 -s ATTITUDE -r 50
+mavlink stream -u 14556 -s RC_CHANNELS -r 20
 mavlink boot_complete

From e322359b27a01256e7aa23be689773f42ef70077 Mon Sep 17 00:00:00 2001
From: Daniel Agar <daniel@agar.ca>
Date: Fri, 10 Jun 2016 10:33:56 -0400
Subject: [PATCH 0607/1230] get_mavlink_mode_state remove unused pos_sp_triplet
 (#4613)

---
 src/modules/mavlink/mavlink_messages.cpp | 29 ++++++------------------
 1 file changed, 7 insertions(+), 22 deletions(-)

diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp
index 5bafb057ce..28f61da576 100644
--- a/src/modules/mavlink/mavlink_messages.cpp
+++ b/src/modules/mavlink/mavlink_messages.cpp
@@ -90,8 +90,8 @@
 #include "mavlink_main.h"
 
 static uint16_t cm_uint16_from_m_float(float m);
-static void get_mavlink_mode_state(struct vehicle_status_s *status, struct position_setpoint_triplet_s *pos_sp_triplet,
-				   uint8_t *mavlink_state, uint8_t *mavlink_base_mode, uint32_t *mavlink_custom_mode);
+static void get_mavlink_mode_state(struct vehicle_status_s *status, uint8_t *mavlink_state,
+								   uint8_t *mavlink_base_mode, uint32_t *mavlink_custom_mode);
 
 uint16_t
 cm_uint16_from_m_float(float m)
@@ -106,8 +106,8 @@ cm_uint16_from_m_float(float m)
 	return (uint16_t)(m * 100.0f);
 }
 
-void get_mavlink_mode_state(struct vehicle_status_s *status, struct position_setpoint_triplet_s *pos_sp_triplet,
-			    uint8_t *mavlink_state, uint8_t *mavlink_base_mode, uint32_t *mavlink_custom_mode)
+void get_mavlink_mode_state(struct vehicle_status_s *status, uint8_t *mavlink_state,
+							uint8_t *mavlink_base_mode, uint32_t *mavlink_custom_mode)
 {
 	*mavlink_state = 0;
 	*mavlink_base_mode = 0;
@@ -305,7 +305,6 @@ public:
 
 private:
 	MavlinkOrbSubscription *_status_sub;
-	MavlinkOrbSubscription *_pos_sp_triplet_sub;
 
 	/* do not allow top copying this class */
 	MavlinkStreamHeartbeat(MavlinkStreamHeartbeat &);
@@ -313,14 +312,12 @@ private:
 
 protected:
 	explicit MavlinkStreamHeartbeat(Mavlink *mavlink) : MavlinkStream(mavlink),
-		_status_sub(_mavlink->add_orb_subscription(ORB_ID(vehicle_status))),
-		_pos_sp_triplet_sub(_mavlink->add_orb_subscription(ORB_ID(position_setpoint_triplet)))
+		_status_sub(_mavlink->add_orb_subscription(ORB_ID(vehicle_status)))
 	{}
 
 	void send(const hrt_abstime t)
 	{
 		struct vehicle_status_s status;
-		struct position_setpoint_triplet_s pos_sp_triplet;
 
 		/* always send the heartbeat, independent of the update status of the topics */
 		if (!_status_sub->update(&status)) {
@@ -328,15 +325,10 @@ protected:
 			memset(&status, 0, sizeof(status));
 		}
 
-		if (!_pos_sp_triplet_sub->update(&pos_sp_triplet)) {
-			/* if topic update failed fill it with defaults */
-			memset(&pos_sp_triplet, 0, sizeof(pos_sp_triplet));
-		}
-
 		uint8_t base_mode = 0;
 		uint32_t custom_mode = 0;
 		uint8_t system_status = 0;
-		get_mavlink_mode_state(&status, &pos_sp_triplet, &system_status, &base_mode, &custom_mode);
+		get_mavlink_mode_state(&status, &system_status, &base_mode, &custom_mode);
 
 		mavlink_msg_heartbeat_send(_mavlink->get_channel(), _mavlink->get_system_type(), MAV_AUTOPILOT_PX4,
 			base_mode, custom_mode, system_status);
@@ -1948,9 +1940,6 @@ private:
 	MavlinkOrbSubscription *_status_sub;
 	uint64_t _status_time;
 
-	MavlinkOrbSubscription *_pos_sp_triplet_sub;
-	uint64_t _pos_sp_triplet_time;
-
 	MavlinkOrbSubscription *_act_sub;
 	uint64_t _act_time;
 
@@ -1962,8 +1951,6 @@ protected:
 	explicit MavlinkStreamHILControls(Mavlink *mavlink) : MavlinkStream(mavlink),
 		_status_sub(_mavlink->add_orb_subscription(ORB_ID(vehicle_status))),
 		_status_time(0),
-		_pos_sp_triplet_sub(_mavlink->add_orb_subscription(ORB_ID(position_setpoint_triplet))),
-		_pos_sp_triplet_time(0),
 		_act_sub(_mavlink->add_orb_subscription(ORB_ID(actuator_outputs))),
 		_act_time(0)
 	{}
@@ -1971,11 +1958,9 @@ protected:
 	void send(const hrt_abstime t)
 	{
 		struct vehicle_status_s status;
-		struct position_setpoint_triplet_s pos_sp_triplet;
 		struct actuator_outputs_s act;
 
 		bool updated = _act_sub->update(&_act_time, &act);
-		updated |= _pos_sp_triplet_sub->update(&_pos_sp_triplet_time, &pos_sp_triplet);
 		updated |= _status_sub->update(&_status_time, &status);
 
 		if (updated && (status.arming_state == vehicle_status_s::ARMING_STATE_ARMED)) {
@@ -1983,7 +1968,7 @@ protected:
 			uint8_t mavlink_state;
 			uint8_t mavlink_base_mode;
 			uint32_t mavlink_custom_mode;
-			get_mavlink_mode_state(&status, &pos_sp_triplet, &mavlink_state, &mavlink_base_mode, &mavlink_custom_mode);
+			get_mavlink_mode_state(&status, &mavlink_state, &mavlink_base_mode, &mavlink_custom_mode);
 
 			float out[8];
 

From a19a6be05e487c3d9fd94dfbde04352fb24deea8 Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Wed, 8 Jun 2016 09:32:23 +0200
Subject: [PATCH 0608/1230] Update jMAVSim

---
 Tools/jMAVSim | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/Tools/jMAVSim b/Tools/jMAVSim
index 9155c8f955..39cee41574 160000
--- a/Tools/jMAVSim
+++ b/Tools/jMAVSim
@@ -1 +1 @@
-Subproject commit 9155c8f955e05c969bcd932b705b98d98845f228
+Subproject commit 39cee41574520c4151e318925c6982f3269b790e

From 8ecada3f8142c326edf875119ce6235a37511a08 Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Fri, 10 Jun 2016 21:28:05 +0200
Subject: [PATCH 0609/1230] Sensors: Allow more headroom for stack

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

diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp
index 4998a72f16..7041d5a5a0 100644
--- a/src/modules/sensors/sensors.cpp
+++ b/src/modules/sensors/sensors.cpp
@@ -2292,7 +2292,7 @@ Sensors::start()
 	_sensors_task = px4_task_spawn_cmd("sensors",
 					   SCHED_DEFAULT,
 					   SCHED_PRIORITY_MAX - 5,
-					   2000,
+					   2200,
 					   (px4_main_t)&Sensors::task_main_trampoline,
 					   nullptr);
 

From 8a5e2a308555d8bf260150a07f99c797ffc7902c Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Sun, 12 Jun 2016 09:29:14 +0200
Subject: [PATCH 0610/1230] Updated IRIS params

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

diff --git a/ROMFS/px4fmu_common/init.d/10016_3dr_iris b/ROMFS/px4fmu_common/init.d/10016_3dr_iris
index 30af86730e..a9f65a6479 100644
--- a/ROMFS/px4fmu_common/init.d/10016_3dr_iris
+++ b/ROMFS/px4fmu_common/init.d/10016_3dr_iris
@@ -25,8 +25,8 @@ then
 	param set MC_YAWRATE_I 0.25
 	param set MC_YAWRATE_D 0.0
 
-	param set BAT_V_SCALING 0.00989
-	param set BAT_C_SCALING 0.0124
+	param set BAT_V_DIV 12.27559
+	param set BAT_A_PER_V 15.39103
 fi
 
 set MIXER quad_w

From 4cca4ea9540c6a3e9902c68b79a3650331c68624 Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Sun, 12 Jun 2016 11:53:55 +0200
Subject: [PATCH 0611/1230] Add DOI

---
 README.md | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/README.md b/README.md
index 70a2c6a84b..176bafd5d1 100644
--- a/README.md
+++ b/README.md
@@ -1,6 +1,6 @@
 ## PX4 Pro Drone Autopilot ##
 
-[![Build Status](https://travis-ci.org/PX4/Firmware.svg?branch=master)](https://travis-ci.org/PX4/Firmware) [![Coverity Scan](https://scan.coverity.com/projects/3966/badge.svg?flat=1)](https://scan.coverity.com/projects/3966?tab=overview)
+[![DOI](https://zenodo.org/badge/22634/PX4/Firmware.svg)](https://zenodo.org/badge/latestdoi/22634/PX4/Firmware) [![Build Status](https://travis-ci.org/PX4/Firmware.svg?branch=master)](https://travis-ci.org/PX4/Firmware) [![Coverity Scan](https://scan.coverity.com/projects/3966/badge.svg?flat=1)](https://scan.coverity.com/projects/3966?tab=overview)
 
 [![Gitter](https://badges.gitter.im/Join%20Chat.svg)](https://gitter.im/PX4/Firmware?utm_source=badge&utm_medium=badge&utm_campaign=pr-badge&utm_content=badge)
 

From 6f7e978f33fd7ce261dbeaa34cd593dd3ffe1b6d Mon Sep 17 00:00:00 2001
From: Daniel Agar <daniel@agar.ca>
Date: Mon, 13 Jun 2016 17:39:30 -0400
Subject: [PATCH 0612/1230] initial circle-ci configuration (#4807)

---
 circle.yml | 19 +++++++++++++++++++
 1 file changed, 19 insertions(+)
 create mode 100644 circle.yml

diff --git a/circle.yml b/circle.yml
new file mode 100644
index 0000000000..7f87ee6381
--- /dev/null
+++ b/circle.yml
@@ -0,0 +1,19 @@
+machine:
+  services:
+    - docker
+
+checkout:
+  post:
+    - git submodule sync
+    - git submodule update --init --recursive
+
+## Customize dependencies
+dependencies:
+  pre:
+    - docker pull px4io/px4-dev-base
+    - docker info
+
+test:
+  override:
+    #- sudo docker run --rm -v `pwd`:`pwd`:rw -v $HOME/.ccache:$HOME/.ccache:rw -e CCACHE_DIR=$HOME/.ccache -w=`pwd` --user=$UID -it px4io/px4-dev-base /bin/bash -c "make"
+    - docker run -v `pwd`:`pwd`:rw -w=`pwd` --user=$UID -it px4io/px4-dev-base /bin/bash -c "make"

From 2f5357be7ad44aa4d0471232fb2100ca02d2d7eb Mon Sep 17 00:00:00 2001
From: Daniel Agar <daniel@agar.ca>
Date: Tue, 14 Jun 2016 01:06:43 -0400
Subject: [PATCH 0613/1230] travis-ci use px4io docker images (#4696)

---
 .travis.yml              | 145 ++++++++++++++++-----------------------
 Makefile                 |  15 ++--
 unittests/CMakeLists.txt |   5 +-
 3 files changed, 72 insertions(+), 93 deletions(-)

diff --git a/.travis.yml b/.travis.yml
index f2da8bcf96..3272b1bfc7 100644
--- a/.travis.yml
+++ b/.travis.yml
@@ -7,11 +7,15 @@ matrix:
   fast_finish: true
   include:
     - os: linux
-      sudo: false
-      env: GCC_VER=4.8
+      sudo: required
+      env: GCC_VER=4.8 DOCKER_REPO="px4io/px4-dev-base"
+      services:
+        - docker
     - os: linux
-      sudo: false
-      env: GCC_VER=4.9
+      sudo: required
+      env: GCC_VER=4.9 DOCKER_REPO="px4io/px4-dev-nuttx-gcc4.9"
+      services:
+        - docker
     - os: osx
       osx_image: xcode7
       sudo: true
@@ -20,49 +24,10 @@ cache:
   directories:
     - $HOME/.ccache
 
-addons:
-  apt:
-    sources:
-    - kubuntu-backports
-    - ubuntu-toolchain-r-test
-    - george-edison55-precise-backports
-    packages:
-    - build-essential
-    - ccache
-    - clang-3.5
-    - cmake
-    - g++-4.9
-    - gcc-4.9
-    - genromfs
-    - libc6-i386
-    - libncurses5-dev
-    - ninja-build
-    - python-argparse
-    - python-empy
-    - s3cmd
-    - texinfo
-    - zlib1g-dev
-
 before_install:
-  - cd ${TRAVIS_BUILD_DIR} && git fetch --unshallow && git fetch --all --tags
+  - cd ${TRAVIS_BUILD_DIR} && git fetch --unshallow && git fetch --all --tags && git submodule update --quiet --init --recursive
   - if [ "${TRAVIS_OS_NAME}" = "linux" ]; then
-         pushd .
-      && cd ~ && mkdir gcc && cd gcc
-      && if [ "$GCC_VER" = "4.8" ]; then GCC_URL="https://launchpadlibrarian.net/186124160/gcc-arm-none-eabi-4_8-2014q3-20140805-linux.tar.bz2" ; fi
-      && if [ "$GCC_VER" = "4.9" ]; then GCC_URL="https://launchpad.net/gcc-arm-embedded/4.9/4.9-2014-q4-major/+download/gcc-arm-none-eabi-4_9-2014q4-20141203-linux.tar.bz2" ; fi
-      && wget -O gcc.tar.bz2 ${GCC_URL}
-      && tar -jxf gcc.tar.bz2 --strip 1
-      && exportline="export PATH=$HOME/gcc/bin:\$PATH"
-      && if grep -Fxq "$exportline" ~/.profile; then echo nothing to do ; else echo $exportline >> ~/.profile; fi
-      && . ~/.profile
-      && popd
-      && git clone git://github.com/PX4/CI-Tools.git
-      && ./CI-Tools/s3cmd-configure
-      && mkdir -p ~/bin
-      && wget -O ~/bin/astyle https://github.com/PX4/astyle/releases/download/2.05.1/astyle-linux && chmod +x ~/bin/astyle
-      && astyle --version
-      && if [ "$CXX" = "g++" ]; then export CXX="g++-4.9" CC="gcc-4.9"; fi
-      ;
+      docker pull ${DOCKER_REPO};
     elif [ "${TRAVIS_OS_NAME}" = "osx" ]; then
          brew tap PX4/homebrew-px4
       && brew update; brew update
@@ -73,24 +38,8 @@ before_install:
       ;
     fi
 
-before_script:
-# setup ccache
-  - mkdir -p ~/bin
-  - ln -s /usr/bin/ccache ~/bin/arm-none-eabi-g++
-  - ln -s /usr/bin/ccache ~/bin/arm-none-eabi-gcc
-  - ln -s /usr/bin/ccache ~/bin/arm-none-eabi-size
-  - ln -s /usr/bin/ccache ~/bin/arm-none-eabi-objcopy
-  - ln -s /usr/bin/ccache ~/bin/clang++
-  - ln -s /usr/bin/ccache ~/bin/clang++-3.4
-  - ln -s /usr/bin/ccache ~/bin/clang++-3.5
-  - ln -s /usr/bin/ccache ~/bin/clang
-  - ln -s /usr/bin/ccache ~/bin/clang-3.4
-  - ln -s /usr/bin/ccache ~/bin/clang-3.5
-  - export PATH=~/bin:$PATH
-
 env:
   global:
-    - NINJA_BUILD=1
 # AWS KEY: $PX4_AWS_KEY
     - secure: "XknnZHWBbpHbN4f3fuAVwUztdLIu8ej4keC3aQSDofo3uw8AFEzojfsQsN9u77ShWSIV4iYJWh9C9ALkCx7TocJ+xYjiboo10YhM9lH/8u+EXjYWG6GHS8ua0wkir+cViSxoLNaMtmcb/rPTicJecAGANxLsIHyBAgTL3fkbLSA="
 # AWS SECRET: $PX4_AWS_SECRET
@@ -98,39 +47,63 @@ env:
     - PX4_AWS_BUCKET=px4-travis
 
 script:
-  - git submodule update --quiet --init --recursive
   - if [ "${TRAVIS_OS_NAME}" = "linux" ]; then
-      arm-none-eabi-gcc --version && make check VECTORCONTROL=1;
+      docker run --rm -v `pwd`:`pwd`:rw -v $HOME/.ccache:$HOME/.ccache:rw -e CCACHE_DIR=$HOME/.ccache -w=`pwd` --user=$UID -it ${DOCKER_REPO} /bin/bash -c "ccache -z; make check VECTORCONTROL=1; ccache -s";
     elif [ "${TRAVIS_OS_NAME}" = "osx" ]; then
       make check_posix_sitl_default;
     fi
 
 after_success:
-  - if [[ "${TRAVIS_OS_NAME}" = "linux" && "${GCC_VER}" = "4.8" ]]; then
-         make package_firmware
-      && find . -name \*.px4 -exec cp "{}" . \;
-      && find . -maxdepth 1 -mindepth 1 -type f -name 'nuttx-*-default.px4' | sed 's/.\/nuttx-//' | sed 's/-default.px4//' | xargs -I{} mv nuttx-{}-default.px4 {}_default.px4
-      && ./CI-Tools/s3cmd-put `find . -maxdepth 1 -mindepth 1 -type f -name '*_default.px4'` build_px4fmu-v2_default/parameters.xml build_px4fmu-v2_default/airframes.xml CI-Tools/directory/index.html Firmware/$TRAVIS_BRANCH/
-      && ./CI-Tools/s3cmd-put Firmware.zip archives/Firmware/$TRAVIS_BRANCH/$TRAVIS_BUILD_ID/
-      && ./CI-Tools/s3cmd-put CI-Tools/directory/index.html archives/Firmware/$TRAVIS_BRANCH/
-      && ./CI-Tools/s3cmd-put CI-Tools/index.html index.html
-      && ./CI-Tools/s3cmd-put CI-Tools/timestamp.html timestamp.html
-      && echo ""
-      && echo "Binaries have been posted to:" https://px4-travis.s3.amazonaws.com/archives/Firmware/$TRAVIS_BRANCH/$TRAVIS_BUILD_ID/Firmware.zip
-      ;
-    fi
+  - make package_firmware && mkdir s3deploy-archive && cp Firmware.zip s3deploy-archive/
+# find all px4 firmware (*.px4) and rename
+  - find . -type f -name 'nuttx-*-default.px4' -exec cp "{}" . \;
+  - find . -maxdepth 1 -mindepth 1 -type f -name 'nuttx-*-default.px4' | sed 's/.\/nuttx-//' | sed 's/-default.px4//' | xargs -I{} mv nuttx-{}-default.px4 {}_default.px4
+  - mkdir s3deploy-branch && mv *_default.px4 build_px4fmu-v2_default/parameters.xml build_px4fmu-v2_default/airframes.xml s3deploy-branch/
 
 deploy:
-  provider: releases
-  api_key:
-    secure: cdHWLRBxA5UlYpOS0Sp891QK7PFmMgQ5ZWs1aPt+sw0rIrowyWMHCwXNBEdUqaExHYNYgXCUDI0EzNgfB7ZcR63Qv1MQeoyamV4jsxlyAqDqmxNtWO82S6RhHGeMLk26VgFKzynVcEk1IYlQP2nqzMQLdu+jTrngERuAIrCdRuc=
-  file: "Firmware.zip"
-  skip_cleanup: true
-  on:
-    tags: true
-    all_branches: true
-    repo: PX4/Firmware
-    condition: $GCC_VER = 4.8
+# deploy *.px4 to S3 px4-travis/Firmware/$TRAVIS_BRANCH
+  - provider: s3
+    access_key_id: $PX4_AWS_KEY
+    secret_access_key:
+      secure: $PX4_AWS_SECRET
+    bucket: px4-travis
+    local_dir: s3deploy-branch
+    upload-dir: Firmware/$TRAVIS_BRANCH
+    acl: public_read
+    skip_cleanup: true
+    on:
+      branch: master
+      branch: beta
+      branch: stable
+      condition: $GCC_VER = 4.8
+
+# deploy Firmware.zip to S3 px4-travis/archives/Firmware/$TRAVIS_BRANCH/$TRAVIS_BUILD_ID
+  - provider: s3
+    access_key_id: $PX4_AWS_KEY
+    secret_access_key:
+      secure: $PX4_AWS_SECRET
+    bucket: px4-travis
+    local_dir: s3deploy-archive
+    upload-dir: archives/Firmware/$TRAVIS_BRANCH/$TRAVIS_BUILD_ID
+    acl: public_read
+    skip_cleanup: true
+    on:
+      branch: master
+      branch: beta
+      branch: stable
+      condition: $GCC_VER = 4.8
+
+# on tags deploy Firmware.zip to Github releases
+  - provider: releases
+    api_key: 
+      secure: cdHWLRBxA5UlYpOS0Sp891QK7PFmMgQ5ZWs1aPt+sw0rIrowyWMHCwXNBEdUqaExHYNYgXCUDI0EzNgfB7ZcR63Qv1MQeoyamV4jsxlyAqDqmxNtWO82S6RhHGeMLk26VgFKzynVcEk1IYlQP2nqzMQLdu+jTrngERuAIrCdRuc=
+    file: "Firmware.zip"
+    skip_cleanup: true
+    on:
+      tags: true
+      all_branches: true
+      repo: PX4/Firmware
+      condition: $GCC_VER = 4.8
 
 notifications:
   webhooks:
diff --git a/Makefile b/Makefile
index 26aad575a8..bd6533e3e7 100644
--- a/Makefile
+++ b/Makefile
@@ -223,8 +223,8 @@ run_sitl_ros: sitl_deprecation
 # Other targets
 # --------------------------------------------------------------------
 
-.PHONY: gazebo_build uavcan_firmware check check_format unittest tests package_firmware clean submodulesclean distclean
-.NOTPARALLEL: gazebo_build uavcan_firmware check check_format unittest tests package_firmware clean submodulesclean distclean
+.PHONY: gazebo_build uavcan_firmware check check_format unittest tests qgc_firmware package_firmware clean submodulesclean distclean
+.NOTPARALLEL: gazebo_build uavcan_firmware check check_format unittest tests qgc_firmware package_firmware clean submodulesclean distclean
 
 gazebo_build:
 	@mkdir -p build_gazebo
@@ -287,15 +287,20 @@ ifeq ($(VECTORCONTROL),1)
 endif
 
 unittest: posix_sitl_test
-	@export CC=clang
-	@export CXX=clang++
-	@export ASAN_OPTIONS=symbolize=1
 	$(call cmake-build-other,unittest, ../unittests)
 	@(cd build_unittest && ctest -j2 --output-on-failure)
 	
 test_onboard_sitl:
 	@HEADLESS=1 make posix_sitl_test gazebo_iris
 
+
+# QGroundControl flashable firmware
+qgc_firmware: \
+	check_px4fmu-v1_default \
+	check_px4fmu-v2_default \
+	check_mindpx-v2_default \
+	check_px4fmu-v4_default_and_uavcan
+
 package_firmware:
 	@zip --junk-paths Firmware.zip `find . -name \*.px4`
 
diff --git a/unittests/CMakeLists.txt b/unittests/CMakeLists.txt
index 5d39cc4bb7..2a628bb1f1 100644
--- a/unittests/CMakeLists.txt
+++ b/unittests/CMakeLists.txt
@@ -1,8 +1,8 @@
 cmake_minimum_required(VERSION 2.8)
 
 include(CMakeForceCompiler)
-CMAKE_FORCE_C_COMPILER(clang Clang)
-CMAKE_FORCE_CXX_COMPILER(clang++ Clang)
+#CMAKE_FORCE_C_COMPILER(clang Clang)
+#CMAKE_FORCE_CXX_COMPILER(clang++ Clang)
 
 if("${CMAKE_C_COMPILER_ID}" STREQUAL "Clang")
 	add_compile_options(-Qunused-arguments )
@@ -28,6 +28,7 @@ endif()
 set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -std=gnu99 -g")
 
 set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -g3 -fsanitize=address -fno-omit-frame-pointer")
+set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wno-sign-compare -Wno-unused-but-set-variable")
 
 set(GTEST_DIR ${CMAKE_SOURCE_DIR}/googletest)
 add_subdirectory(${GTEST_DIR})

From f13b75a6d2ab6f2dcb69da79464aab2de5f04689 Mon Sep 17 00:00:00 2001
From: Daniel Agar <daniel@agar.ca>
Date: Tue, 14 Jun 2016 01:56:42 -0400
Subject: [PATCH 0614/1230] travis-ci fix s3 deploy conditions (#4810)

---
 .travis.yml | 16 ++++++++--------
 1 file changed, 8 insertions(+), 8 deletions(-)

diff --git a/.travis.yml b/.travis.yml
index 3272b1bfc7..0e0ec4bfd5 100644
--- a/.travis.yml
+++ b/.travis.yml
@@ -59,6 +59,10 @@ after_success:
   - find . -type f -name 'nuttx-*-default.px4' -exec cp "{}" . \;
   - find . -maxdepth 1 -mindepth 1 -type f -name 'nuttx-*-default.px4' | sed 's/.\/nuttx-//' | sed 's/-default.px4//' | xargs -I{} mv nuttx-{}-default.px4 {}_default.px4
   - mkdir s3deploy-branch && mv *_default.px4 build_px4fmu-v2_default/parameters.xml build_px4fmu-v2_default/airframes.xml s3deploy-branch/
+# only deploy GCC 4.8 builds on master/beta/stable
+  - if [[ "$GCC_VER" == "4.8" && ( "$TRAVIS_BRANCH" == "master" || "$TRAVIS_BRANCH" == "beta" || "$TRAVIS_BRANCH" == "stable" ) ]]; then
+      export PX4_S3_DEPLOY=1
+    fi
 
 deploy:
 # deploy *.px4 to S3 px4-travis/Firmware/$TRAVIS_BRANCH
@@ -72,10 +76,8 @@ deploy:
     acl: public_read
     skip_cleanup: true
     on:
-      branch: master
-      branch: beta
-      branch: stable
-      condition: $GCC_VER = 4.8
+      all_branches: true
+      condition: $PX4_S3_DEPLOY = 1
 
 # deploy Firmware.zip to S3 px4-travis/archives/Firmware/$TRAVIS_BRANCH/$TRAVIS_BUILD_ID
   - provider: s3
@@ -88,10 +90,8 @@ deploy:
     acl: public_read
     skip_cleanup: true
     on:
-      branch: master
-      branch: beta
-      branch: stable
-      condition: $GCC_VER = 4.8
+      all_branches: true
+      condition: $PX4_S3_DEPLOY = 1
 
 # on tags deploy Firmware.zip to Github releases
   - provider: releases

From 2eac57731cbb61f7e18dbc8310c431570c14241a Mon Sep 17 00:00:00 2001
From: Daniel Agar <daniel@agar.ca>
Date: Tue, 14 Jun 2016 02:18:22 -0400
Subject: [PATCH 0615/1230] travis-ci fix missing semicolon (#4812)

---
 .travis.yml | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/.travis.yml b/.travis.yml
index 0e0ec4bfd5..12f57ed6f7 100644
--- a/.travis.yml
+++ b/.travis.yml
@@ -61,7 +61,7 @@ after_success:
   - mkdir s3deploy-branch && mv *_default.px4 build_px4fmu-v2_default/parameters.xml build_px4fmu-v2_default/airframes.xml s3deploy-branch/
 # only deploy GCC 4.8 builds on master/beta/stable
   - if [[ "$GCC_VER" == "4.8" && ( "$TRAVIS_BRANCH" == "master" || "$TRAVIS_BRANCH" == "beta" || "$TRAVIS_BRANCH" == "stable" ) ]]; then
-      export PX4_S3_DEPLOY=1
+      export PX4_S3_DEPLOY=1;
     fi
 
 deploy:

From b731a9e96c8c84305b4310282120ceb1f53fbfa3 Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Tue, 14 Jun 2016 08:42:12 +0200
Subject: [PATCH 0616/1230] Fix AR Drone scaling param

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

diff --git a/ROMFS/px4fmu_common/init.d/4008_ardrone b/ROMFS/px4fmu_common/init.d/4008_ardrone
index 9d3e4427be..6849e1e827 100644
--- a/ROMFS/px4fmu_common/init.d/4008_ardrone
+++ b/ROMFS/px4fmu_common/init.d/4008_ardrone
@@ -29,7 +29,7 @@ then
     param set MC_YAWRATE_D 0.0
     param set MC_YAW_FF 0.8
 
-    param set BAT_V_SCALING 0.00838095238
+    param set BAT_V_DIV 34.32838
 fi
 
 set OUTPUT_MODE ardrone

From d66af65a9265faf6684d0d1909e2c4cfcbda4d14 Mon Sep 17 00:00:00 2001
From: CarlOlsson <carlolsson.co@gmail.com>
Date: Wed, 8 Jun 2016 19:48:01 +0200
Subject: [PATCH 0617/1230] ekf2: fixed airspeed thr bug

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

diff --git a/src/modules/ekf2/ekf2_main.cpp b/src/modules/ekf2/ekf2_main.cpp
index ea333769bc..8ab9faaac8 100644
--- a/src/modules/ekf2/ekf2_main.cpp
+++ b/src/modules/ekf2/ekf2_main.cpp
@@ -536,7 +536,7 @@ void Ekf2::task_main()
 
 		// only set airspeed data if condition for airspeed fusion are met
 		bool fuse_airspeed = airspeed_updated && !_vehicle_status.is_rotary_wing
-				     && _arspFusionThreshold.get() <= airspeed.true_airspeed_m_s;
+				     && _arspFusionThreshold.get() <= airspeed.true_airspeed_m_s && _arspFusionThreshold.get() >= 0.1f;
 
 		if (fuse_airspeed) {
 			float eas2tas = airspeed.true_airspeed_m_s / airspeed.indicated_airspeed_m_s;

From 2b85c594b34052e50f861f9329796f40b7c96876 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Beat=20K=C3=BCng?= <beat-kueng@gmx.net>
Date: Wed, 15 Jun 2016 09:53:37 +0200
Subject: [PATCH 0618/1230] fix px4_getopt: correctly handle options that take
 an argument, but no argument is given

This lead to a segfault, for example 'logger start -r'
---
 src/platforms/common/px4_getopt.c | 3 +++
 1 file changed, 3 insertions(+)

diff --git a/src/platforms/common/px4_getopt.c b/src/platforms/common/px4_getopt.c
index 863b5816b4..a242e66e83 100644
--- a/src/platforms/common/px4_getopt.c
+++ b/src/platforms/common/px4_getopt.c
@@ -165,6 +165,9 @@ __EXPORT int px4_getopt(int argc, char *argv[], const char *options, int *myopti
 
 		if (takesarg) {
 			*myoptarg = argv[*myoptind];
+			if (!*myoptarg) { //Error: option takes an argument, but there is no more argument
+				return -1;
+			}
 			*myoptind += 1;
 		}
 

From 574a67b93db0a436c11c8c85d31ff05b673c100c Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Beat=20K=C3=BCng?= <beat-kueng@gmx.net>
Date: Wed, 15 Jun 2016 09:55:25 +0200
Subject: [PATCH 0619/1230] fix px4_getopt: ensure progress in case of unknown
 options

Previously sdlog2 got stuck in an endless loop if an unknown argument was
given
---
 src/platforms/common/px4_getopt.c | 5 ++++-
 1 file changed, 4 insertions(+), 1 deletion(-)

diff --git a/src/platforms/common/px4_getopt.c b/src/platforms/common/px4_getopt.c
index a242e66e83..6b0427b936 100644
--- a/src/platforms/common/px4_getopt.c
+++ b/src/platforms/common/px4_getopt.c
@@ -143,10 +143,12 @@ __EXPORT int px4_getopt(int argc, char *argv[], const char *options, int *myopti
 	char c;
 	int takesarg;
 
-	if (*myoptind == 1)
+	if (*myoptind == 1) {
 		if (reorder(argc, argv, options) != 0) {
+			*myoptind += 1;
 			return (int)'?';
 		}
+	}
 
 	p = argv[*myoptind];
 
@@ -158,6 +160,7 @@ __EXPORT int px4_getopt(int argc, char *argv[], const char *options, int *myopti
 		c = isvalidopt(p[1], options, &takesarg);
 
 		if (c == '?') {
+			*myoptind += 1;
 			return (int)c;
 		}
 

From 26b7ac388444ccc0e96876233fa85a2094f0ccab Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Beat=20K=C3=BCng?= <beat-kueng@gmx.net>
Date: Wed, 15 Jun 2016 09:56:34 +0200
Subject: [PATCH 0620/1230] mavlink: remove an UDP remote port warning, print
 remote port in mode info

---
 src/modules/mavlink/mavlink_main.cpp | 5 ++---
 1 file changed, 2 insertions(+), 3 deletions(-)

diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp
index 1e3dcc288d..125ed23a5d 100644
--- a/src/modules/mavlink/mavlink_main.cpp
+++ b/src/modules/mavlink/mavlink_main.cpp
@@ -1701,7 +1701,6 @@ Mavlink::task_main(int argc, char *argv[])
 		case 'o':
 			temp_int_arg = strtoul(myoptarg, &eptr, 10);
 			if ( *eptr == '\0' ) {
-				warnx("set remote port %d", temp_int_arg);
 				_remote_port = temp_int_arg;
 				set_protocol(UDP);
 			} else {
@@ -1822,8 +1821,8 @@ Mavlink::task_main(int argc, char *argv[])
 			return ERROR;
 		}
 
-		PX4_INFO("mode: %s, data rate: %d B/s on udp port %hu",
-			 mavlink_mode_str(_mode), _datarate, _network_port);
+		PX4_INFO("mode: %s, data rate: %d B/s on udp port %hu remote port %hu",
+			 mavlink_mode_str(_mode), _datarate, _network_port, _remote_port);
 	}
 
 	/* initialize send mutex */

From d9d5d9d9e47a173b6bb9456d7ec65a9a30ecf9f2 Mon Sep 17 00:00:00 2001
From: Julian Oes <julian@oes.ch>
Date: Fri, 10 Jun 2016 14:19:12 +0100
Subject: [PATCH 0621/1230] mc_pos_control: run the loop without position data

By running the loop in mc_pos_control without successfully polling on
position data, we still copy the manual input over so that
mc_att_control can consume it. This allows to fly in manual mode without
GPS and no position estimate.
---
 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 5d9d6ec1ae..7438a4ac91 100644
--- a/src/modules/mc_pos_control/mc_pos_control_main.cpp
+++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp
@@ -1231,12 +1231,12 @@ MulticopterPositionControl::task_main()
 	fds[0].events = POLLIN;
 
 	while (!_task_should_exit) {
-		/* wait for up to 500ms for data */
-		int pret = px4_poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 500);
+		/* wait for up to 20ms for data */
+		int pret = px4_poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 20);
 
 		/* timed out - periodic check for _task_should_exit */
 		if (pret == 0) {
-			continue;
+			// Go through the loop anyway to copy manual input at 50 Hz.
 		}
 
 		/* this is undesirable but not much we can do */

From a76eab367cbc394fd58653b3cd6e30da40d64e3c Mon Sep 17 00:00:00 2001
From: Julian Oes <julian@oes.ch>
Date: Mon, 13 Jun 2016 17:33:10 +0100
Subject: [PATCH 0622/1230] DriverFramework: update submodule

This brings various PRs in:
- RPi build fixes
- unit tests on CI and on Mac
- some cmake cleanup
- CI style check for DF
---
 src/lib/DriverFramework | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/src/lib/DriverFramework b/src/lib/DriverFramework
index 0624230124..e2865fb56c 160000
--- a/src/lib/DriverFramework
+++ b/src/lib/DriverFramework
@@ -1 +1 @@
-Subproject commit 062423012432f563da95b492804cc2a1c51fedb4
+Subproject commit e2865fb56c4f8e061b75fc3f7b306ab549458f6e

From 2a26611cf5be85d8475ac08e387530b847e93fbf Mon Sep 17 00:00:00 2001
From: James Goppert <jgoppert@users.noreply.github.com>
Date: Wed, 15 Jun 2016 12:37:52 -0400
Subject: [PATCH 0623/1230] Make LPE sonar reading more robust. (#4806)

---
 .../BlockLocalPositionEstimator.cpp           |  1 +
 .../BlockLocalPositionEstimator.hpp           |  1 +
 src/modules/local_position_estimator/params.c | 10 +++---
 .../local_position_estimator/sensors/flow.cpp | 15 ++------
 .../sensors/sonar.cpp                         | 34 +++++++++++++------
 5 files changed, 34 insertions(+), 27 deletions(-)

diff --git a/src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp b/src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp
index 6bbcb284f6..12360a18a2 100644
--- a/src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp
+++ b/src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp
@@ -113,6 +113,7 @@ BlockLocalPositionEstimator::BlockLocalPositionEstimator() :
 	_time_last_gps(0),
 	_time_last_lidar(0),
 	_time_last_sonar(0),
+	_time_init_sonar(0),
 	_time_last_vision_p(0),
 	_time_last_mocap(0),
 
diff --git a/src/modules/local_position_estimator/BlockLocalPositionEstimator.hpp b/src/modules/local_position_estimator/BlockLocalPositionEstimator.hpp
index c32db40066..eba9d61355 100644
--- a/src/modules/local_position_estimator/BlockLocalPositionEstimator.hpp
+++ b/src/modules/local_position_estimator/BlockLocalPositionEstimator.hpp
@@ -318,6 +318,7 @@ private:
 	uint64_t _time_last_gps;
 	uint64_t _time_last_lidar;
 	uint64_t _time_last_sonar;
+	uint64_t _time_init_sonar;
 	uint64_t _time_last_vision_p;
 	uint64_t _time_last_mocap;
 
diff --git a/src/modules/local_position_estimator/params.c b/src/modules/local_position_estimator/params.c
index 071aabc9fe..4b85db9447 100644
--- a/src/modules/local_position_estimator/params.c
+++ b/src/modules/local_position_estimator/params.c
@@ -126,7 +126,7 @@ PARAM_DEFINE_FLOAT(LPE_ACC_Z, 0.0454f);
  * @max 3
  * @decimal 2
  */
-PARAM_DEFINE_FLOAT(LPE_BAR_Z, 1.0f);
+PARAM_DEFINE_FLOAT(LPE_BAR_Z, 3.0f);
 
 /**
  * Enables GPS data, also forces alt init with GPS
@@ -264,7 +264,7 @@ PARAM_DEFINE_FLOAT(LPE_VIC_P, 0.05f);
  * @max 1
  * @decimal 8
  */
-PARAM_DEFINE_FLOAT(LPE_PN_P, 0.1f);
+PARAM_DEFINE_FLOAT(LPE_PN_P, 0.0f);
 
 /**
  * Velocity propagation noise density
@@ -275,7 +275,7 @@ PARAM_DEFINE_FLOAT(LPE_PN_P, 0.1f);
  * @max 1
  * @decimal 8
  */
-PARAM_DEFINE_FLOAT(LPE_PN_V, 0.1f);
+PARAM_DEFINE_FLOAT(LPE_PN_V, 0.0f);
 
 /**
  * Accel bias propagation noise density
@@ -289,7 +289,7 @@ PARAM_DEFINE_FLOAT(LPE_PN_V, 0.1f);
 PARAM_DEFINE_FLOAT(LPE_PN_B, 1e-3f);
 
 /**
- * Terrain random walk noise density
+ * Terrain random walk noise density, hilly/outdoor (1e-1), flat/Indoor (1e-3)
  *
  * @group Local Position Estimator
  * @unit m/s/sqrt(Hz)
@@ -297,7 +297,7 @@ PARAM_DEFINE_FLOAT(LPE_PN_B, 1e-3f);
  * @max 1
  * @decimal 3
  */
-PARAM_DEFINE_FLOAT(LPE_PN_T, 1e-3f);
+PARAM_DEFINE_FLOAT(LPE_PN_T, 1e-1f);
 
 /**
  * Flow gyro high pass filter cut off frequency
diff --git a/src/modules/local_position_estimator/sensors/flow.cpp b/src/modules/local_position_estimator/sensors/flow.cpp
index 331123ab2f..31f29a1487 100644
--- a/src/modules/local_position_estimator/sensors/flow.cpp
+++ b/src/modules/local_position_estimator/sensors/flow.cpp
@@ -49,21 +49,12 @@ int BlockLocalPositionEstimator::flowMeasure(Vector<float, n_y_flow> &y)
 	}
 
 	// calculate range to center of image for flow
-	float d = 0;
-
-	if (_lidarInitialized && (_lidarFault < fault_lvl_disable)) {
-		d = _sub_lidar->get().current_distance
-		    + (_lidar_z_offset.get() - _flow_z_offset.get());
-
-	} else if (_sonarInitialized && (_sonarFault < fault_lvl_disable)) {
-		d = _sub_sonar->get().current_distance
-		    + (_sonar_z_offset.get() - _flow_z_offset.get());
-
-	} else {
-		// no valid distance data
+	if (!_canEstimateT) {
 		return -1;
 	}
 
+	float d = agl() * cosf(_sub_att.get().roll) * cosf(_sub_att.get().pitch);
+
 	// check for global accuracy
 	if (_gpsInitialized) {
 		double  lat = _sub_gps.get().lat * 1.0e-7;
diff --git a/src/modules/local_position_estimator/sensors/sonar.cpp b/src/modules/local_position_estimator/sensors/sonar.cpp
index 5e6349b671..d95ad5e346 100644
--- a/src/modules/local_position_estimator/sensors/sonar.cpp
+++ b/src/modules/local_position_estimator/sensors/sonar.cpp
@@ -7,33 +7,47 @@ extern orb_advert_t mavlink_log_pub;
 // required number of samples for sensor
 // to initialize
 static const int 		REQ_SONAR_INIT_COUNT = 10;
-static const uint32_t 	SONAR_TIMEOUT =   1000000; // 1.0 s
+static const uint32_t 	SONAR_TIMEOUT =   5000000; // 2.0 s
+static const float  	SONAR_MAX_INIT_STD =   0.3f; // meters
 
 void BlockLocalPositionEstimator::sonarInit()
 {
 	// measure
 	Vector<float, n_y_sonar> y;
 
+	if (_sonarStats.getCount() == 0) {
+		_time_init_sonar = _timeStamp;
+	}
+
 	if (sonarMeasure(y) != OK) {
-		_sonarStats.reset();
 		return;
 	}
 
 	// if finished
 	if (_sonarStats.getCount() > REQ_SONAR_INIT_COUNT) {
-		mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] sonar init "
-					     "mean %d cm std %d cm",
-					     int(100 * _sonarStats.getMean()(0)),
-					     int(100 * _sonarStats.getStdDev()(0)));
-		_sonarInitialized = true;
-		_sonarFault = FAULT_NONE;
+		if (_sonarStats.getStdDev()(0) > SONAR_MAX_INIT_STD) {
+			mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] sonar init std > min");
+			_sonarStats.reset();
+
+		} else if ((_timeStamp - _time_init_sonar) > SONAR_TIMEOUT) {
+			mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] sonar init timeout ");
+			_sonarStats.reset();
+
+		} else {
+			mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] sonar init "
+						     "mean %d cm std %d cm",
+						     int(100 * _sonarStats.getMean()(0)),
+						     int(100 * _sonarStats.getStdDev()(0)));
+			_sonarInitialized = true;
+			_sonarFault = FAULT_NONE;
+		}
 	}
 }
 
 int BlockLocalPositionEstimator::sonarMeasure(Vector<float, n_y_sonar> &y)
 {
 	// measure
-	float d = _sub_sonar->get().current_distance + _sonar_z_offset.get();
+	float d = _sub_sonar->get().current_distance;
 	float eps = 0.01f;
 	float min_dist = _sub_sonar->get().min_distance + eps;
 	float max_dist = _sub_sonar->get().max_distance - eps;
@@ -47,7 +61,7 @@ int BlockLocalPositionEstimator::sonarMeasure(Vector<float, n_y_sonar> &y)
 	_sonarStats.update(Scalarf(d));
 	_time_last_sonar = _timeStamp;
 	y.setZero();
-	y(0) = d *
+	y(0) = (d + _sonar_z_offset.get()) *
 	       cosf(_sub_att.get().roll) *
 	       cosf(_sub_att.get().pitch);
 	return OK;

From d16daf5ba4c7d8c08ce48b6169c31ec1754e3ada Mon Sep 17 00:00:00 2001
From: Daniel Agar <daniel@agar.ca>
Date: Wed, 15 Jun 2016 14:29:19 -0400
Subject: [PATCH 0624/1230] pwm_out_sim sleep if no fds (#4829)

-fixes #4828
---
 src/drivers/pwm_out_sim/pwm_out_sim.cpp | 3 +++
 1 file changed, 3 insertions(+)

diff --git a/src/drivers/pwm_out_sim/pwm_out_sim.cpp b/src/drivers/pwm_out_sim/pwm_out_sim.cpp
index d3cb5b2542..af264d966c 100644
--- a/src/drivers/pwm_out_sim/pwm_out_sim.cpp
+++ b/src/drivers/pwm_out_sim/pwm_out_sim.cpp
@@ -414,7 +414,10 @@ PWMSim::task_main()
 			_current_update_rate = _update_rate;
 		}
 
+		/* this can happen during boot, but after the sleep its likely resolved */
 		if (_poll_fds_num == 0) {
+			usleep(1000 * 1000);
+
 			PX4_DEBUG("no valid fds");
 			continue;
 		}

From 7bbfa5d94b2468fe78384930fd184368140851f0 Mon Sep 17 00:00:00 2001
From: Daniel Agar <daniel@agar.ca>
Date: Tue, 14 Jun 2016 13:30:59 -0400
Subject: [PATCH 0625/1230] cleanup px4_custom_mode

---
 src/modules/commander/commander.cpp      | 34 +------------
 src/modules/commander/px4_custom_mode.h  |  5 +-
 src/modules/mavlink/mavlink_messages.cpp | 61 ++++++++++--------------
 3 files changed, 29 insertions(+), 71 deletions(-)

diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp
index 22e659ea41..a1ea14e2fe 100644
--- a/src/modules/commander/commander.cpp
+++ b/src/modules/commander/commander.cpp
@@ -60,7 +60,6 @@
 #include <systemlib/err.h>
 #include <systemlib/circuit_breaker.h>
 #include <systemlib/mavlink_log.h>
-//#include <debug.h>
 #include <sys/stat.h>
 #include <string.h>
 #include <math.h>
@@ -123,12 +122,6 @@
 #include "esc_calibration.h"
 #include "PreflightCheck.h"
 
-/* oddly, ERROR is not defined for c++ */
-#ifdef ERROR
-# undef ERROR
-#endif
-static const int ERROR = -1;
-
 static constexpr uint8_t COMMANDER_MAX_GPS_NOISE = 60;		/**< Maximum percentage signal to noise ratio allowed for GPS reception */
 
 /* Decouple update interval and hysteresis counters, all depends on intervals */
@@ -154,19 +147,6 @@ static constexpr uint8_t COMMANDER_MAX_GPS_NOISE = 60;		/**< Maximum percentage
 #define HIL_ID_MIN 1000
 #define HIL_ID_MAX 1999
 
-enum MAV_MODE_FLAG {
-	MAV_MODE_FLAG_CUSTOM_MODE_ENABLED = 1, /* 0b00000001 Reserved for future use. | */
-	MAV_MODE_FLAG_TEST_ENABLED = 2, /* 0b00000010 system has a test mode enabled. This flag is intended for temporary system tests and should not be used for stable implementations. | */
-	MAV_MODE_FLAG_AUTO_ENABLED = 4, /* 0b00000100 autonomous mode enabled, system finds its own goal positions. Guided flag can be set or not, depends on the actual implementation. | */
-	MAV_MODE_FLAG_GUIDED_ENABLED = 8, /* 0b00001000 guided mode enabled, system flies MISSIONs / mission items. | */
-	MAV_MODE_FLAG_STABILIZE_ENABLED = 16, /* 0b00010000 system stabilizes electronically its attitude (and optionally position). It needs however further control inputs to move around. | */
-	MAV_MODE_FLAG_HIL_ENABLED = 32, /* 0b00100000 hardware in the loop simulation. All motors / actuators are blocked, but internal software is full operational. | */
-	MAV_MODE_FLAG_MANUAL_INPUT_ENABLED = 64, /* 0b01000000 remote control input is enabled. | */
-	MAV_MODE_FLAG_SAFETY_ARMED = 128, /* 0b10000000 MAV safety set to armed. Motors are enabled / running / can start. Ready to fly. | */
-	MAV_MODE_FLAG_ENUM_END = 129, /*  | */
-};
-
-
 /* Mavlink log uORB handle */
 static orb_advert_t mavlink_log_pub = 0;
 
@@ -268,9 +248,6 @@ void print_reject_arm(const char *msg);
 
 void print_status();
 
-transition_result_t check_navigation_state_machine(struct vehicle_status_s *status,
-		struct vehicle_control_mode_s *control_mode, struct vehicle_local_position_s *local_pos);
-
 transition_result_t arm_disarm(bool arm, orb_advert_t *mavlink_log_pub, const char *armedBy);
 
 /**
@@ -745,7 +722,7 @@ bool handle_command(struct vehicle_status_s *status_local, const struct safety_s
 						case PX4_CUSTOM_SUB_MODE_AUTO_LAND:
 							main_ret = main_state_transition(status_local, commander_state_s::MAIN_STATE_AUTO_LAND, main_state_prev, &status_flags, &internal_state);
 							break;
-						case PX4_CUSTOM_SUB_MODE_FOLLOW_TARGET:
+						case PX4_CUSTOM_SUB_MODE_AUTO_FOLLOW_TARGET:
 							main_ret = main_state_transition(status_local, commander_state_s::MAIN_STATE_AUTO_FOLLOW_TARGET, main_state_prev, &status_flags, &internal_state);
 							break;
 
@@ -1209,15 +1186,6 @@ int commander_thread_main(int argc, char *argv[])
 	// main_states_str[commander_state_s::MAIN_STATE_STAB]			= "STAB";
 	// main_states_str[commander_state_s::MAIN_STATE_OFFBOARD]			= "OFFBOARD";
 
-	// const char *arming_states_str[vehicle_status_s::ARMING_STATE_MAX];
-	// arming_states_str[vehicle_status_s::ARMING_STATE_INIT]			= "INIT";
-	// arming_states_str[vehicle_status_s::ARMING_STATE_STANDBY]			= "STANDBY";
-	// arming_states_str[vehicle_status_s::ARMING_STATE_ARMED]			= "ARMED";
-	// arming_states_str[vehicle_status_s::ARMING_STATE_ARMED_ERROR]		= "ARMED_ERROR";
-	// arming_states_str[vehicle_status_s::ARMING_STATE_STANDBY_ERROR]		= "STANDBY_ERROR";
-	// arming_states_str[vehicle_status_s::ARMING_STATE_REBOOT]			= "REBOOT";
-	// arming_states_str[vehicle_status_s::ARMING_STATE_IN_AIR_RESTORE]		= "IN_AIR_RESTORE";
-
 	// const char *nav_states_str[vehicle_status_s::NAVIGATION_STATE_MAX];
 	// nav_states_str[vehicle_status_s::NAVIGATION_STATE_MANUAL]			= "MANUAL";
 	// nav_states_str[vehicle_status_s::NAVIGATION_STATE_STAB]				= "STAB";
diff --git a/src/modules/commander/px4_custom_mode.h b/src/modules/commander/px4_custom_mode.h
index 85f2ef590e..b3e9de3aba 100644
--- a/src/modules/commander/px4_custom_mode.h
+++ b/src/modules/commander/px4_custom_mode.h
@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2013-2015 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2013-2016 PX4 Development Team. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -35,7 +35,6 @@
  * @file px4_custom_mode.h
  * PX4 custom flight modes
  *
- * @author Anton Babushkin <anton@px4.io>
  */
 
 #ifndef PX4_CUSTOM_MODE_H_
@@ -62,7 +61,7 @@ enum PX4_CUSTOM_SUB_MODE_AUTO {
 	PX4_CUSTOM_SUB_MODE_AUTO_RTL,
 	PX4_CUSTOM_SUB_MODE_AUTO_LAND,
 	PX4_CUSTOM_SUB_MODE_AUTO_RTGS,
-	PX4_CUSTOM_SUB_MODE_FOLLOW_TARGET
+	PX4_CUSTOM_SUB_MODE_AUTO_FOLLOW_TARGET
 };
 
 union px4_custom_mode {
diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp
index 28f61da576..8839c91b2a 100644
--- a/src/modules/mavlink/mavlink_messages.cpp
+++ b/src/modules/mavlink/mavlink_messages.cpp
@@ -130,11 +130,14 @@ void get_mavlink_mode_state(struct vehicle_status_s *status, uint8_t *mavlink_st
 	union px4_custom_mode custom_mode;
 	custom_mode.data = 0;
 
-	switch (status->nav_state) {
+	const uint8_t auto_mode_flags	= MAV_MODE_FLAG_AUTO_ENABLED
+									| MAV_MODE_FLAG_STABILIZE_ENABLED
+									| MAV_MODE_FLAG_GUIDED_ENABLED;
 
+	switch (status->nav_state) {
 		case vehicle_status_s::NAVIGATION_STATE_MANUAL:
-			*mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED
-			                      | (status->is_rotary_wing ? MAV_MODE_FLAG_STABILIZE_ENABLED : 0);
+			*mavlink_base_mode	|= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED
+			    				| (status->is_rotary_wing ? MAV_MODE_FLAG_STABILIZE_ENABLED : 0);
 			custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_MANUAL;
 			break;
 
@@ -149,60 +152,52 @@ void get_mavlink_mode_state(struct vehicle_status_s *status, uint8_t *mavlink_st
 			break;
 
 		case vehicle_status_s::NAVIGATION_STATE_STAB:
-			*mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED
-								  | MAV_MODE_FLAG_STABILIZE_ENABLED;
+			*mavlink_base_mode	|= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED
+								| MAV_MODE_FLAG_STABILIZE_ENABLED;
 			custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_STABILIZED;
 			break;
 
 		case vehicle_status_s::NAVIGATION_STATE_ALTCTL:
-			*mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED
-			                      | MAV_MODE_FLAG_STABILIZE_ENABLED;
+			*mavlink_base_mode	|= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED
+								| MAV_MODE_FLAG_STABILIZE_ENABLED;
 			custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_ALTCTL;
 			break;
 
 		case vehicle_status_s::NAVIGATION_STATE_POSCTL:
-			*mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED
-			                      | MAV_MODE_FLAG_STABILIZE_ENABLED
-					      | MAV_MODE_FLAG_GUIDED_ENABLED;
+			*mavlink_base_mode	|= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED
+								| MAV_MODE_FLAG_STABILIZE_ENABLED
+								| MAV_MODE_FLAG_GUIDED_ENABLED; // TODO: is POSCTL GUIDED?
 			custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_POSCTL;
 			break;
 
 		case vehicle_status_s::NAVIGATION_STATE_AUTO_TAKEOFF:
-			*mavlink_base_mode |= MAV_MODE_FLAG_AUTO_ENABLED
-			                      | MAV_MODE_FLAG_STABILIZE_ENABLED
-					      | MAV_MODE_FLAG_GUIDED_ENABLED;
+			*mavlink_base_mode |= auto_mode_flags;
 			custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_AUTO;
 			custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_TAKEOFF;
 			break;
 
 		case vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION:
-			*mavlink_base_mode |= MAV_MODE_FLAG_AUTO_ENABLED
-			                      | MAV_MODE_FLAG_STABILIZE_ENABLED
-					      | MAV_MODE_FLAG_GUIDED_ENABLED;
+			*mavlink_base_mode |= auto_mode_flags;
 			custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_AUTO;
 			custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_MISSION;
 			break;
 
 		case vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER:
-			*mavlink_base_mode |= MAV_MODE_FLAG_AUTO_ENABLED
-			                      | MAV_MODE_FLAG_STABILIZE_ENABLED
-					      | MAV_MODE_FLAG_GUIDED_ENABLED;
+			*mavlink_base_mode |= auto_mode_flags;
 			custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_AUTO;
 			custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_LOITER;
 			break;
+
 		case vehicle_status_s::NAVIGATION_STATE_AUTO_FOLLOW_TARGET:
-			*mavlink_base_mode 	|= MAV_MODE_FLAG_AUTO_ENABLED
-								| MAV_MODE_FLAG_STABILIZE_ENABLED
-								| MAV_MODE_FLAG_GUIDED_ENABLED;
+			*mavlink_base_mode |= auto_mode_flags;
 			custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_AUTO;
-			custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_FOLLOW_TARGET;
+			custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_FOLLOW_TARGET;
 			break;
+
 		case vehicle_status_s::NAVIGATION_STATE_AUTO_RTL:
 			/* fallthrough */
 		case vehicle_status_s::NAVIGATION_STATE_AUTO_RCRECOVER:
-			*mavlink_base_mode |= MAV_MODE_FLAG_AUTO_ENABLED
-			                      | MAV_MODE_FLAG_STABILIZE_ENABLED
-					      | MAV_MODE_FLAG_GUIDED_ENABLED;
+			*mavlink_base_mode |= auto_mode_flags;
 			custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_AUTO;
 			custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_RTL;
 			break;
@@ -212,17 +207,13 @@ void get_mavlink_mode_state(struct vehicle_status_s *status, uint8_t *mavlink_st
 		case vehicle_status_s::NAVIGATION_STATE_AUTO_LANDGPSFAIL:
 			/* fallthrough */
 		case vehicle_status_s::NAVIGATION_STATE_DESCEND:
-			*mavlink_base_mode |= MAV_MODE_FLAG_AUTO_ENABLED
-			                      | MAV_MODE_FLAG_STABILIZE_ENABLED
-					      | MAV_MODE_FLAG_GUIDED_ENABLED;
+			*mavlink_base_mode |= auto_mode_flags;
 			custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_AUTO;
 			custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_LAND;
 			break;
 
 		case vehicle_status_s::NAVIGATION_STATE_AUTO_RTGS:
-			*mavlink_base_mode |= MAV_MODE_FLAG_AUTO_ENABLED
-			                      | MAV_MODE_FLAG_STABILIZE_ENABLED
-					      | MAV_MODE_FLAG_GUIDED_ENABLED;
+			*mavlink_base_mode |= auto_mode_flags;
 			custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_AUTO;
 			custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_RTGS;
 			break;
@@ -233,9 +224,9 @@ void get_mavlink_mode_state(struct vehicle_status_s *status, uint8_t *mavlink_st
 			break;
 
 		case vehicle_status_s::NAVIGATION_STATE_OFFBOARD:
-			*mavlink_base_mode |= MAV_MODE_FLAG_AUTO_ENABLED
-			                      | MAV_MODE_FLAG_STABILIZE_ENABLED
-					      | MAV_MODE_FLAG_GUIDED_ENABLED;
+			*mavlink_base_mode	|= MAV_MODE_FLAG_AUTO_ENABLED
+								| MAV_MODE_FLAG_STABILIZE_ENABLED
+								| MAV_MODE_FLAG_GUIDED_ENABLED;
 			custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_OFFBOARD;
 			break;
 

From c22a9137dd63b4e228856c1fbee8e840773772a7 Mon Sep 17 00:00:00 2001
From: Daniel Agar <daniel@agar.ca>
Date: Tue, 14 Jun 2016 13:42:49 -0400
Subject: [PATCH 0626/1230] commander cleanup headers

---
 src/modules/commander/commander.cpp        | 141 ++++++++++-----------
 src/modules/commander/commander_helper.cpp |   2 +-
 2 files changed, 66 insertions(+), 77 deletions(-)

diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp
index a1ea14e2fe..d04ec441df 100644
--- a/src/modules/commander/commander.cpp
+++ b/src/modules/commander/commander.cpp
@@ -44,83 +44,74 @@
  * @author Sander Smeets	<sander@droneslab.com>
  */
 
-#include <px4_config.h>
-#include <px4_posix.h>
-#include <px4_time.h>
-#include <px4_tasks.h>
-#include <pthread.h>
-#include <stdio.h>
-#include <sys/stat.h>
-#include <stdlib.h>
-#include <stdbool.h>
-#include <string.h>
-#include <unistd.h>
-#include <fcntl.h>
-#include <errno.h>
-#include <systemlib/err.h>
-#include <systemlib/circuit_breaker.h>
-#include <systemlib/mavlink_log.h>
-#include <sys/stat.h>
-#include <string.h>
-#include <math.h>
-#include <poll.h>
-#include <float.h>
-
-#include <uORB/uORB.h>
-#include <uORB/topics/sensor_combined.h>
-#include <uORB/topics/battery_status.h>
-#include <uORB/topics/manual_control_setpoint.h>
-#include <uORB/topics/offboard_control_mode.h>
-#include <uORB/topics/home_position.h>
-#include <uORB/topics/vehicle_global_position.h>
-#include <uORB/topics/vehicle_local_position.h>
-#include <uORB/topics/vehicle_attitude.h>
-#include <uORB/topics/position_setpoint_triplet.h>
-#include <uORB/topics/vehicle_gps_position.h>
-#include <uORB/topics/vehicle_command.h>
-#include <uORB/topics/subsystem_info.h>
-#include <uORB/topics/actuator_controls.h>
-#include <uORB/topics/actuator_armed.h>
-#include <uORB/topics/parameter_update.h>
-#include <uORB/topics/differential_pressure.h>
-#include <uORB/topics/safety.h>
-#include <uORB/topics/system_power.h>
-#include <uORB/topics/mission.h>
-#include <uORB/topics/mission_result.h>
-#include <uORB/topics/geofence_result.h>
-#include <uORB/topics/telemetry_status.h>
-#include <uORB/topics/vtol_vehicle_status.h>
-#include <uORB/topics/vehicle_land_detected.h>
-#include <uORB/topics/input_rc.h>
-#include <uORB/topics/vehicle_command_ack.h>
-#include <uORB/topics/mavlink_log.h>
-#include <uORB/topics/cpuload.h>
-
-#include <drivers/drv_led.h>
-#include <drivers/drv_hrt.h>
-#include <drivers/drv_tone_alarm.h>
-
-#include <systemlib/param/param.h>
-#include <systemlib/systemlib.h>
-#include <systemlib/err.h>
-#include <systemlib/rc_check.h>
-#include <geo/geo.h>
-#include <systemlib/state_table.h>
-#include <dataman/dataman.h>
-#include <navigator/navigation.h>
-
-#include "px4_custom_mode.h"
-#include "commander_helper.h"
-#include "state_machine_helper.h"
-#include "calibration_routines.h"
+/* commander module headers */
 #include "accelerometer_calibration.h"
+#include "airspeed_calibration.h"
+#include "baro_calibration.h"
+#include "calibration_routines.h"
+#include "commander_helper.h"
+#include "esc_calibration.h"
 #include "gyro_calibration.h"
 #include "mag_calibration.h"
-#include "baro_calibration.h"
-#include "rc_calibration.h"
-#include "airspeed_calibration.h"
-#include "esc_calibration.h"
 #include "PreflightCheck.h"
+#include "px4_custom_mode.h"
+#include "rc_calibration.h"
+#include "state_machine_helper.h"
+
+/* PX4 headers */
+#include <dataman/dataman.h>
+#include <drivers/drv_hrt.h>
+#include <drivers/drv_led.h>
+#include <drivers/drv_tone_alarm.h>
+#include <geo/geo.h>
+#include <navigator/navigation.h>
+#include <px4_config.h>
+#include <px4_posix.h>
+#include <px4_tasks.h>
+#include <px4_time.h>
+#include <systemlib/circuit_breaker.h>
+#include <systemlib/err.h>
+#include <systemlib/mavlink_log.h>
+#include <systemlib/param/param.h>
+#include <systemlib/rc_check.h>
+#include <systemlib/state_table.h>
+#include <systemlib/systemlib.h>
+#include <uORB/topics/actuator_armed.h>
+#include <uORB/topics/actuator_controls.h>
+#include <uORB/topics/battery_status.h>
+#include <uORB/topics/cpuload.h>
+#include <uORB/topics/differential_pressure.h>
+#include <uORB/topics/geofence_result.h>
+#include <uORB/topics/home_position.h>
+#include <uORB/topics/input_rc.h>
+#include <uORB/topics/manual_control_setpoint.h>
+#include <uORB/topics/mavlink_log.h>
+#include <uORB/topics/mission.h>
+#include <uORB/topics/mission_result.h>
+#include <uORB/topics/offboard_control_mode.h>
+#include <uORB/topics/parameter_update.h>
+#include <uORB/topics/position_setpoint_triplet.h>
+#include <uORB/topics/safety.h>
+#include <uORB/topics/sensor_combined.h>
+#include <uORB/topics/subsystem_info.h>
+#include <uORB/topics/system_power.h>
+#include <uORB/topics/telemetry_status.h>
+#include <uORB/topics/vehicle_attitude.h>
+#include <uORB/topics/vehicle_command.h>
+#include <uORB/topics/vehicle_command_ack.h>
+#include <uORB/topics/vehicle_global_position.h>
+#include <uORB/topics/vehicle_gps_position.h>
+#include <uORB/topics/vehicle_land_detected.h>
+#include <uORB/topics/vehicle_local_position.h>
+#include <uORB/topics/vtol_vehicle_status.h>
+#include <uORB/uORB.h>
+#include <v2.0/common/mavlink.h>
+
+/* oddly, ERROR is not defined for c++ */
+#ifdef ERROR
+# undef ERROR
+#endif
+static const int ERROR = -1;
 
 static constexpr uint8_t COMMANDER_MAX_GPS_NOISE = 60;		/**< Maximum percentage signal to noise ratio allowed for GPS reception */
 
@@ -269,9 +260,7 @@ void answer_command(struct vehicle_command_s &cmd, unsigned result,
 /**
  * check whether autostart ID is in the reserved range for HIL setups
  */
-bool is_hil_setup(int id);
-
-bool is_hil_setup(int id) {
+static bool is_hil_setup(int id) {
 	return (id >= HIL_ID_MIN) && (id <= HIL_ID_MAX);
 }
 
diff --git a/src/modules/commander/commander_helper.cpp b/src/modules/commander/commander_helper.cpp
index 812e2ed09d..0fbee034f7 100644
--- a/src/modules/commander/commander_helper.cpp
+++ b/src/modules/commander/commander_helper.cpp
@@ -62,7 +62,7 @@
 #include <drivers/drv_led.h>
 #include <drivers/drv_rgbled.h>
 
-#include <v1.0/common/mavlink.h> // For MAV_TYPE
+#include <v2.0/common/mavlink.h> // For MAV_TYPE
 
 #include "commander_helper.h"
 #include "DevMgr.hpp"

From 4ab8ddec53696e1135cc590ea1a7dffe1715734a Mon Sep 17 00:00:00 2001
From: Daniel Agar <daniel@agar.ca>
Date: Tue, 14 Jun 2016 15:09:07 -0400
Subject: [PATCH 0627/1230] rename cb_usb -> circuit_breaker_engaged_usb_check

---
 src/modules/commander/commander.cpp            | 2 +-
 src/modules/commander/state_machine_helper.cpp | 2 +-
 src/modules/commander/state_machine_helper.h   | 2 +-
 3 files changed, 3 insertions(+), 3 deletions(-)

diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp
index d04ec441df..5b9079a0df 100644
--- a/src/modules/commander/commander.cpp
+++ b/src/modules/commander/commander.cpp
@@ -2806,7 +2806,7 @@ void
 get_circuit_breaker_params()
 {
 	status_flags.circuit_breaker_engaged_power_check = circuit_breaker_enabled("CBRK_SUPPLY_CHK", CBRK_SUPPLY_CHK_KEY);
-	status_flags.cb_usb = circuit_breaker_enabled("CBRK_USB_CHK", CBRK_USB_CHK_KEY);
+	status_flags.circuit_breaker_engaged_usb_check = circuit_breaker_enabled("CBRK_USB_CHK", CBRK_USB_CHK_KEY);
 	status_flags.circuit_breaker_engaged_airspd_check = circuit_breaker_enabled("CBRK_AIRSPD_CHK", CBRK_AIRSPD_CHK_KEY);
 	status_flags.circuit_breaker_engaged_enginefailure_check = circuit_breaker_enabled("CBRK_ENGINEFAIL", CBRK_ENGINEFAIL_KEY);
 	status_flags.circuit_breaker_engaged_gpsfailure_check = circuit_breaker_enabled("CBRK_GPSFAIL", CBRK_GPSFAIL_KEY);
diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp
index 72b8551834..88f3e5d6ec 100644
--- a/src/modules/commander/state_machine_helper.cpp
+++ b/src/modules/commander/state_machine_helper.cpp
@@ -886,7 +886,7 @@ int preflight_check(struct vehicle_status_s *status, orb_advert_t *mavlink_log_p
 				checkAirspeed, (status->rc_input_mode == vehicle_status_s::RC_IN_MODE_DEFAULT),
 				!status_flags->circuit_breaker_engaged_gpsfailure_check, true, reportFailures);
 
-	if (!status_flags->cb_usb && status_flags->usb_connected && prearm) {
+	if (!status_flags->circuit_breaker_engaged_usb_check && status_flags->usb_connected && prearm) {
 		preflight_ok = false;
 		if (reportFailures) {
 			mavlink_and_console_log_critical(mavlink_log_pub, "ARMING DENIED: Flying with USB is not safe");
diff --git a/src/modules/commander/state_machine_helper.h b/src/modules/commander/state_machine_helper.h
index 8a951a0375..f3302eba08 100644
--- a/src/modules/commander/state_machine_helper.h
+++ b/src/modules/commander/state_machine_helper.h
@@ -76,7 +76,7 @@ struct status_flags_s {
 	bool circuit_breaker_engaged_airspd_check;
 	bool circuit_breaker_engaged_enginefailure_check;
 	bool circuit_breaker_engaged_gpsfailure_check;
-	bool cb_usb;
+	bool circuit_breaker_engaged_usb_check;
 	bool offboard_control_signal_found_once;
 	bool offboard_control_signal_lost;
 	bool offboard_control_signal_weak;

From 64d9b8eefdb671fec4eafeb8b9312e8758bbd963 Mon Sep 17 00:00:00 2001
From: Daniel Agar <daniel@agar.ca>
Date: Tue, 14 Jun 2016 16:14:45 -0400
Subject: [PATCH 0628/1230] fix COM_FLTMODE5 metadata typo

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

diff --git a/src/modules/commander/commander_params.c b/src/modules/commander/commander_params.c
index 46acadfe8f..0402554493 100644
--- a/src/modules/commander/commander_params.c
+++ b/src/modules/commander/commander_params.c
@@ -369,7 +369,7 @@ PARAM_DEFINE_INT32(COM_FLTMODE3, -1);
 PARAM_DEFINE_INT32(COM_FLTMODE4, -1);
 
 /**
- * Fift flightmode slot (1640-1800)
+ * Fifth flightmode slot (1640-1800)
  *
  * If the main switch channel is in this range the
  * selected flight mode will be applied.

From e2801d35e4de21cf21b2e1a67cf5b74ecd1c44ed Mon Sep 17 00:00:00 2001
From: Daniel Agar <daniel@agar.ca>
Date: Wed, 15 Jun 2016 11:11:31 -0400
Subject: [PATCH 0629/1230] fix comment spelling

---
 nuttx-configs/px4fmu-v2/include/board.h | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/nuttx-configs/px4fmu-v2/include/board.h b/nuttx-configs/px4fmu-v2/include/board.h
index 52668cacd2..e84a5cb517 100755
--- a/nuttx-configs/px4fmu-v2/include/board.h
+++ b/nuttx-configs/px4fmu-v2/include/board.h
@@ -294,7 +294,7 @@ extern "C" {
  *
  * Description:
  *   All STM32 architectures must provide the following entry point.  This entry point
- *   is called early in the intitialization -- after all memory has been configured
+ *   is called early in the initialization -- after all memory has been configured
  *   and mapped but before any devices have been initialized.
  *
  ************************************************************************************/

From 538d9ada2500c7a38a306a8fda6c842404539524 Mon Sep 17 00:00:00 2001
From: David Sidrane <david_s5@nscdg.com>
Date: Wed, 15 Jun 2016 09:51:53 -1000
Subject: [PATCH 0630/1230] Needed to avoid name collsion in  upcomming Nuttx
 logger changes (#4830)

---
 src/modules/systemlib/mixer/mixer.h          |  2 +-
 src/modules/systemlib/mixer/mixer_simple.cpp | 34 ++++++++++----------
 2 files changed, 18 insertions(+), 18 deletions(-)

diff --git a/src/modules/systemlib/mixer/mixer.h b/src/modules/systemlib/mixer/mixer.h
index f23a9dd4e3..2f7f1befed 100644
--- a/src/modules/systemlib/mixer/mixer.h
+++ b/src/modules/systemlib/mixer/mixer.h
@@ -427,7 +427,7 @@ public:
 protected:
 
 private:
-	mixer_simple_s			*_info;
+	mixer_simple_s			*_pinfo;
 
 	static int			parse_output_scaler(const char *buf, unsigned &buflen, mixer_scaler_s &scaler);
 	static int			parse_control_scaler(const char *buf,
diff --git a/src/modules/systemlib/mixer/mixer_simple.cpp b/src/modules/systemlib/mixer/mixer_simple.cpp
index 4ad20c8aee..31108c1a67 100644
--- a/src/modules/systemlib/mixer/mixer_simple.cpp
+++ b/src/modules/systemlib/mixer/mixer_simple.cpp
@@ -61,14 +61,14 @@ SimpleMixer::SimpleMixer(ControlCallback control_cb,
 			 uintptr_t cb_handle,
 			 mixer_simple_s *mixinfo) :
 	Mixer(control_cb, cb_handle),
-	_info(mixinfo)
+	_pinfo(mixinfo)
 {
 }
 
 SimpleMixer::~SimpleMixer()
 {
-	if (_info != nullptr) {
-		free(_info);
+	if (_pinfo != nullptr) {
+		free(_pinfo);
 	}
 }
 
@@ -279,7 +279,7 @@ SimpleMixer::mix(float *outputs, unsigned space, uint16_t *status_reg)
 {
 	float		sum = 0.0f;
 
-	if (_info == nullptr) {
+	if (_pinfo == nullptr) {
 		return 0;
 	}
 
@@ -287,26 +287,26 @@ SimpleMixer::mix(float *outputs, unsigned space, uint16_t *status_reg)
 		return 0;
 	}
 
-	for (unsigned i = 0; i < _info->control_count; i++) {
+	for (unsigned i = 0; i < _pinfo->control_count; i++) {
 		float input;
 
 		_control_cb(_cb_handle,
-			    _info->controls[i].control_group,
-			    _info->controls[i].control_index,
+			    _pinfo->controls[i].control_group,
+			    _pinfo->controls[i].control_index,
 			    input);
 
-		sum += scale(_info->controls[i].scaler, input);
+		sum += scale(_pinfo->controls[i].scaler, input);
 	}
 
-	*outputs = scale(_info->output_scaler, sum);
+	*outputs = scale(_pinfo->output_scaler, sum);
 	return 1;
 }
 
 void
 SimpleMixer::groups_required(uint32_t &groups)
 {
-	for (unsigned i = 0; i < _info->control_count; i++) {
-		groups |= 1 << _info->controls[i].control_group;
+	for (unsigned i = 0; i < _pinfo->control_count; i++) {
+		groups |= 1 << _pinfo->controls[i].control_group;
 	}
 }
 
@@ -318,30 +318,30 @@ SimpleMixer::check()
 
 	/* sanity that presumes that a mixer includes a control no more than once */
 	/* max of 32 groups due to groups_required API */
-	if (_info->control_count > 32) {
+	if (_pinfo->control_count > 32) {
 		return -2;
 	}
 
 	/* validate the output scaler */
-	ret = scale_check(_info->output_scaler);
+	ret = scale_check(_pinfo->output_scaler);
 
 	if (ret != 0) {
 		return ret;
 	}
 
 	/* validate input scalers */
-	for (unsigned i = 0; i < _info->control_count; i++) {
+	for (unsigned i = 0; i < _pinfo->control_count; i++) {
 
 		/* verify that we can fetch the control */
 		if (_control_cb(_cb_handle,
-				_info->controls[i].control_group,
-				_info->controls[i].control_index,
+				_pinfo->controls[i].control_group,
+				_pinfo->controls[i].control_index,
 				junk) != 0) {
 			return -3;
 		}
 
 		/* validate the scaler */
-		ret = scale_check(_info->controls[i].scaler);
+		ret = scale_check(_pinfo->controls[i].scaler);
 
 		if (ret != 0) {
 			return (10 * i + ret);

From bada390ddebb65562cb306fa0c98ee78ee5e3446 Mon Sep 17 00:00:00 2001
From: Daniel Agar <daniel@agar.ca>
Date: Thu, 16 Jun 2016 15:02:28 -0400
Subject: [PATCH 0631/1230] fw_pos_control_l1 fix FW_LND_FLALT usage (#4835)

-fixes #4745
---
 src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp | 2 +-
 src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c | 2 +-
 2 files changed, 2 insertions(+), 2 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 c510c4e322..01f378b970 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
@@ -614,7 +614,7 @@ FixedwingPositionControl::FixedwingPositionControl() :
 
 	_parameter_handles.land_slope_angle = param_find("FW_LND_ANG");
 	_parameter_handles.land_H1_virt = param_find("FW_LND_HVIRT");
-	_parameter_handles.land_flare_alt_relative = param_find("FW_LND_FL_ALT");
+	_parameter_handles.land_flare_alt_relative = param_find("FW_LND_FLALT");
 	_parameter_handles.land_flare_pitch_min_deg = param_find("FW_LND_FL_PMIN");
 	_parameter_handles.land_flare_pitch_max_deg = param_find("FW_LND_FL_PMAX");
 	_parameter_handles.land_thrust_lim_alt_relative = param_find("FW_LND_TLALT");
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 42af0d36fa..561d4e4349 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
@@ -198,7 +198,7 @@ PARAM_DEFINE_FLOAT(FW_THR_IDLE, 0.15f);
  * Throttle limit value before flare
  *
  * This throttle value will be set as throttle limit at FW_LND_TLALT,
- * before arcraft will flare.
+ * before aircraft will flare.
  *
  * @unit norm
  * @min 0.0

From c4463047514790cf34bfe720e3e7e9bdf9dc77b6 Mon Sep 17 00:00:00 2001
From: Eric Ye <ericye16@gmail.com>
Date: Fri, 17 Jun 2016 07:47:06 -0700
Subject: [PATCH 0632/1230] Update makefile to ask for cmake 3.4.3 (#4831)

---
 Makefile | 10 +++++-----
 1 file changed, 5 insertions(+), 5 deletions(-)

diff --git a/Makefile b/Makefile
index bd6533e3e7..c14c2fc411 100644
--- a/Makefile
+++ b/Makefile
@@ -50,11 +50,11 @@ ifneq ($(CMAKE_VER),0)
     $(warning sudo apt-get install cmake)
     $(warning )
     $(warning Official website:)
-    $(warning wget https://cmake.org/files/v3.3/cmake-3.3.2-Linux-x86_64.sh)
-    $(warning chmod +x cmake-3.3.2-Linux-x86_64.sh)
-    $(warning sudo mkdir /opt/cmake-3.3.2)
-    $(warning sudo ./cmake-3.3.2-Linux-x86_64.sh --prefix=/opt/cmake-3.3.2 --exclude-subdir)
-    $(warning export PATH=/opt/cmake-3.3.2/bin:$$PATH)
+    $(warning wget https://cmake.org/files/v3.4/cmake-3.4.3-Linux-x86_64.sh)
+    $(warning chmod +x cmake-3.4.3-Linux-x86_64.sh)
+    $(warning sudo mkdir /opt/cmake-3.4.3)
+    $(warning sudo ./cmake-3.4.3-Linux-x86_64.sh --prefix=/opt/cmake-3.4.3 --exclude-subdir)
+    $(warning export PATH=/opt/cmake-3.4.3/bin:$$PATH)
     $(warning )
     $(error Fatal)
 endif

From 1dec6e83c6fe1abf31e0ad09d14e65abfc74b965 Mon Sep 17 00:00:00 2001
From: Julian Oes <julian@oes.ch>
Date: Fri, 17 Jun 2016 21:16:54 +0200
Subject: [PATCH 0633/1230] ecl: update submodule (#4839)

---
 src/lib/ecl | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/src/lib/ecl b/src/lib/ecl
index ab0ac05b47..74a77b45b2 160000
--- a/src/lib/ecl
+++ b/src/lib/ecl
@@ -1 +1 @@
-Subproject commit ab0ac05b47f63401833da650984accde9b6dab05
+Subproject commit 74a77b45b2b51cb9a2f2a05e0b356d87cac0d9a8

From b31c346ea7dba6c3e0998a8e1e91d38c1a064709 Mon Sep 17 00:00:00 2001
From: Kelly Steich <kelly.steich@wingtra.com>
Date: Thu, 9 Jun 2016 18:03:36 +0200
Subject: [PATCH 0634/1230] new file structure for the camera_trigger driver

Conflicts:
	PX4/cmake/configs/nuttx_px4fmu-v2_default.cmake
---
 src/drivers/camera_trigger/CMakeLists.txt                      | 3 +++
 src/drivers/camera_trigger/interfaces/src/camera_interface.cpp | 0
 src/drivers/camera_trigger/interfaces/src/camera_interface.h   | 0
 src/drivers/camera_trigger/interfaces/src/pwm.cpp              | 0
 src/drivers/camera_trigger/interfaces/src/pwm.h                | 0
 src/drivers/camera_trigger/interfaces/src/relay.cpp            | 0
 src/drivers/camera_trigger/interfaces/src/relay.h              | 0
 7 files changed, 3 insertions(+)
 create mode 100644 src/drivers/camera_trigger/interfaces/src/camera_interface.cpp
 create mode 100644 src/drivers/camera_trigger/interfaces/src/camera_interface.h
 create mode 100644 src/drivers/camera_trigger/interfaces/src/pwm.cpp
 create mode 100644 src/drivers/camera_trigger/interfaces/src/pwm.h
 create mode 100644 src/drivers/camera_trigger/interfaces/src/relay.cpp
 create mode 100644 src/drivers/camera_trigger/interfaces/src/relay.h

diff --git a/src/drivers/camera_trigger/CMakeLists.txt b/src/drivers/camera_trigger/CMakeLists.txt
index 8674c222d7..1a1491dbe3 100644
--- a/src/drivers/camera_trigger/CMakeLists.txt
+++ b/src/drivers/camera_trigger/CMakeLists.txt
@@ -39,6 +39,9 @@ px4_add_module(
 	SRCS
 		camera_trigger.cpp
 		camera_trigger_params.c
+                interfaces/src/camera_interface.cpp
+                interfaces/src/pwm.cpp
+                interfaces/src/relay.cpp
 	DEPENDS
 		platforms__common
 	)
diff --git a/src/drivers/camera_trigger/interfaces/src/camera_interface.cpp b/src/drivers/camera_trigger/interfaces/src/camera_interface.cpp
new file mode 100644
index 0000000000..e69de29bb2
diff --git a/src/drivers/camera_trigger/interfaces/src/camera_interface.h b/src/drivers/camera_trigger/interfaces/src/camera_interface.h
new file mode 100644
index 0000000000..e69de29bb2
diff --git a/src/drivers/camera_trigger/interfaces/src/pwm.cpp b/src/drivers/camera_trigger/interfaces/src/pwm.cpp
new file mode 100644
index 0000000000..e69de29bb2
diff --git a/src/drivers/camera_trigger/interfaces/src/pwm.h b/src/drivers/camera_trigger/interfaces/src/pwm.h
new file mode 100644
index 0000000000..e69de29bb2
diff --git a/src/drivers/camera_trigger/interfaces/src/relay.cpp b/src/drivers/camera_trigger/interfaces/src/relay.cpp
new file mode 100644
index 0000000000..e69de29bb2
diff --git a/src/drivers/camera_trigger/interfaces/src/relay.h b/src/drivers/camera_trigger/interfaces/src/relay.h
new file mode 100644
index 0000000000..e69de29bb2

From 29f31ae6ac8b625422d93873e11e490d0041b5b5 Mon Sep 17 00:00:00 2001
From: Kelly Steich <kelly.steich@wingtra.com>
Date: Fri, 10 Jun 2016 11:23:09 +0200
Subject: [PATCH 0635/1230] fixed the triggering function logic

Conflicts:
	PX4/src/drivers/camera_trigger/camera_trigger.cpp
	PX4/src/drivers/camera_trigger/interfaces/src/camera_interface.h
	PX4/src/drivers/camera_trigger/interfaces/src/pwm.cpp
	PX4/src/drivers/camera_trigger/interfaces/src/pwm.h
	PX4/src/drivers/camera_trigger/interfaces/src/relay.cpp
	PX4/src/drivers/camera_trigger/interfaces/src/relay.h
---
 src/drivers/camera_trigger/camera_trigger.cpp | 10 +++
 .../interfaces/src/camera_interface.h         | 54 +++++++++++++
 .../camera_trigger/interfaces/src/pwm.cpp     | 30 ++++++++
 .../camera_trigger/interfaces/src/pwm.h       | 27 +++++++
 .../camera_trigger/interfaces/src/relay.cpp   | 75 +++++++++++++++++++
 .../camera_trigger/interfaces/src/relay.h     | 44 +++++++++++
 6 files changed, 240 insertions(+)

diff --git a/src/drivers/camera_trigger/camera_trigger.cpp b/src/drivers/camera_trigger/camera_trigger.cpp
index fff4acd3da..195d431c90 100644
--- a/src/drivers/camera_trigger/camera_trigger.cpp
+++ b/src/drivers/camera_trigger/camera_trigger.cpp
@@ -426,7 +426,11 @@ CameraTrigger::engage(void *arg)
 	/* set timestamp the instant before the trigger goes off */
 	report.timestamp = hrt_absolute_time();
 
+<<<<<<< HEAD
 	CameraTrigger::trigger(trig, trig->_polarity);
+=======
+	trig->_camera_interface->trigger(true);
+>>>>>>> fb669fc... fixed the triggering function logic
 
 	report.seq = trig->_trigger_seq++;
 
@@ -436,10 +440,16 @@ CameraTrigger::engage(void *arg)
 void
 CameraTrigger::disengage(void *arg)
 {
+<<<<<<< HEAD
 
 	CameraTrigger *trig = reinterpret_cast<CameraTrigger *>(arg);
 
 	CameraTrigger::trigger(trig, !(trig->_polarity));
+=======
+	CameraTrigger *trig = reinterpret_cast<CameraTrigger *>(arg);
+
+	trig->_camera_interface->trigger(false);
+>>>>>>> fb669fc... fixed the triggering function logic
 }
 
 void
diff --git a/src/drivers/camera_trigger/interfaces/src/camera_interface.h b/src/drivers/camera_trigger/interfaces/src/camera_interface.h
index e69de29bb2..5554fc3d6c 100644
--- a/src/drivers/camera_trigger/interfaces/src/camera_interface.h
+++ b/src/drivers/camera_trigger/interfaces/src/camera_interface.h
@@ -0,0 +1,54 @@
+/**
+ * @file camera_interface.h
+ */
+
+#pragma once
+
+class CameraInterface
+{
+public:
+
+	/**
+	 * Constructor
+	 */
+	CameraInterface();
+
+	/**
+	 * Destructor.
+	 */
+	virtual ~CameraInterface();
+
+	/**
+	 * setup the interface
+	 */
+	virtual void setup() {};
+
+	/**
+	 * trigger the camera
+	 * @param trigger:
+	 */
+	virtual void trigger(bool enable) {};
+
+
+	/**
+	 * Display info.
+	 */
+	virtual void info() {};
+
+	/**
+	 * Power on the camera
+	 * @return 0 on success, <0 on error
+	 */
+	virtual int powerOn() { return -1; }
+
+	/**
+	 * Power off the camera
+	 * @return 0 on success, <0 on error
+	 */
+	virtual int powerOff() { return -1; }
+
+
+protected:
+
+
+};
diff --git a/src/drivers/camera_trigger/interfaces/src/pwm.cpp b/src/drivers/camera_trigger/interfaces/src/pwm.cpp
index e69de29bb2..67eedaf471 100644
--- a/src/drivers/camera_trigger/interfaces/src/pwm.cpp
+++ b/src/drivers/camera_trigger/interfaces/src/pwm.cpp
@@ -0,0 +1,30 @@
+#include "pwm.h"
+
+CameraInterfacePWM::CameraInterfacePWM():
+	CameraInterface()
+{
+}
+
+CameraInterfacePWM::~CameraInterfacePWM()
+{
+}
+
+void CameraInterfacePWM::setup()
+{
+
+}
+
+void CameraInterfacePWM::trigger(bool enable)
+{
+
+}
+
+int CameraInterfacePWM::powerOn()
+{
+	return 0;
+}
+
+int CameraInterfacePWM::powerOff()
+{
+	return 0;
+}
diff --git a/src/drivers/camera_trigger/interfaces/src/pwm.h b/src/drivers/camera_trigger/interfaces/src/pwm.h
index e69de29bb2..7632feb119 100644
--- a/src/drivers/camera_trigger/interfaces/src/pwm.h
+++ b/src/drivers/camera_trigger/interfaces/src/pwm.h
@@ -0,0 +1,27 @@
+/**
+ * @file pwm.h
+ *
+ * Interface with cameras via pwm.
+ *
+ */
+#pragma once
+
+#include "camera_interface.h"
+
+
+class CameraInterfacePWM : public CameraInterface
+{
+public:
+	CameraInterfacePWM();
+	virtual ~CameraInterfacePWM();
+
+	void setup();
+
+	void trigger(bool enable);
+
+	int powerOn();
+	int powerOff();
+
+private:
+
+};
diff --git a/src/drivers/camera_trigger/interfaces/src/relay.cpp b/src/drivers/camera_trigger/interfaces/src/relay.cpp
index e69de29bb2..c07215bba5 100644
--- a/src/drivers/camera_trigger/interfaces/src/relay.cpp
+++ b/src/drivers/camera_trigger/interfaces/src/relay.cpp
@@ -0,0 +1,75 @@
+#include "relay.h"
+
+constexpr uint32_t CameraInterfaceRelay::_gpios[6];
+
+CameraInterfaceRelay::CameraInterfaceRelay():
+	CameraInterface(),
+	_pins{},
+	_polarity(0)
+{
+	_p_pin = param_find("TRIG_PINS");
+	_p_polarity = param_find("TRIG_POLARITY");
+
+	int pin_list;
+	param_get(_p_pin, &pin_list);
+	param_get(_p_polarity, &_polarity);
+
+	// Set all pins as invalid
+	for (unsigned i = 0; i < sizeof(_pins) / sizeof(_pins[0]); i++) {
+		_pins[i] = -1;
+	}
+
+	// Convert number to individual channels
+	unsigned i = 0;
+	int single_pin;
+
+	while ((single_pin = pin_list % 10)) {
+
+		_pins[i] = single_pin - 1;
+
+		if (_pins[i] < 0 || _pins[i] >= static_cast<int>(sizeof(_gpios) / sizeof(_gpios[0]))) {
+			_pins[i] = -1;
+		}
+
+		pin_list /= 10;
+		i++;
+	}
+}
+
+CameraInterfaceRelay::~CameraInterfaceRelay()
+{
+}
+
+void CameraInterfaceRelay::setup()
+{
+	for (unsigned i = 0; i < sizeof(_pins) / sizeof(_pins[0]); i++) {
+		stm32_configgpio(_gpios[_pins[i]]);
+		stm32_gpiowrite(_gpios[_pins[i]], !_polarity);
+	}
+}
+
+void CameraInterfaceRelay::trigger(bool enable)
+{
+	if (enable) {
+		for (unsigned i = 0; i < sizeof(_pins) / sizeof(_pins[0]); i++) {
+			if (_pins[i] >= 0) {
+				// ACTIVE_LOW == 1
+				stm32_gpiowrite(_gpios[_pins[i]], _polarity);
+			}
+		}
+
+	} else {
+		for (unsigned i = 0; i < sizeof(_pins) / sizeof(_pins[0]); i++) {
+			if (_pins[i] >= 0) {
+				// ACTIVE_LOW == 1
+				stm32_gpiowrite(_gpios[_pins[i]], !_polarity);
+			}
+		}
+	}
+}
+
+void CameraInterfaceRelay::info()
+{
+	warnx("pins 1-3 : %d,%d,%d polarity : %s", _pins[0], _pins[1], _pins[2],
+	      _polarity ? "ACTIVE_HIGH" : "ACTIVE_LOW");
+}
diff --git a/src/drivers/camera_trigger/interfaces/src/relay.h b/src/drivers/camera_trigger/interfaces/src/relay.h
index e69de29bb2..371ae9511f 100644
--- a/src/drivers/camera_trigger/interfaces/src/relay.h
+++ b/src/drivers/camera_trigger/interfaces/src/relay.h
@@ -0,0 +1,44 @@
+/**
+ * @file relay.h
+ *
+ * Interface with cameras via FMU auxiliary pins.
+ *
+ */
+#pragma once
+
+#include <systemlib/err.h>
+#include <systemlib/param/param.h>
+#include <board_config.h>
+
+#include "camera_interface.h"
+
+
+class CameraInterfaceRelay : public CameraInterface
+{
+public:
+	CameraInterfaceRelay();
+	virtual ~CameraInterfaceRelay();
+
+	void setup();
+
+	void trigger(bool enable);
+
+	void info();
+
+	int _pins[6];
+	int _polarity;
+
+private:
+	param_t _p_pin;
+	param_t _p_polarity;
+
+	static constexpr uint32_t _gpios[6] = {
+		GPIO_GPIO0_OUTPUT,
+		GPIO_GPIO1_OUTPUT,
+		GPIO_GPIO2_OUTPUT,
+		GPIO_GPIO3_OUTPUT,
+		GPIO_GPIO4_OUTPUT,
+		GPIO_GPIO5_OUTPUT
+	};
+
+};

From 3671bfb743d29792a44f19676343927f3461af64 Mon Sep 17 00:00:00 2001
From: Kelly Steich <kelly.steich@wingtra.com>
Date: Fri, 10 Jun 2016 11:29:41 +0200
Subject: [PATCH 0636/1230] added the camera interface info method to the
 camera trigger info method

---
 src/drivers/camera_trigger/camera_trigger.cpp | 1 +
 1 file changed, 1 insertion(+)

diff --git a/src/drivers/camera_trigger/camera_trigger.cpp b/src/drivers/camera_trigger/camera_trigger.cpp
index 195d431c90..b68e482e2b 100644
--- a/src/drivers/camera_trigger/camera_trigger.cpp
+++ b/src/drivers/camera_trigger/camera_trigger.cpp
@@ -472,6 +472,7 @@ CameraTrigger::info()
 	warnx("mode : %i", _mode);
 	warnx("interval : %.2f", (double)_interval);
 	warnx("distance : %.2f", (double)_distance);
+	_camera_interface->info();
 }
 
 static void usage()

From f83c53c27448fc052d0b426c032570e959fb04e8 Mon Sep 17 00:00:00 2001
From: Andreas Bircher <bircher@gmx.ch>
Date: Fri, 10 Jun 2016 12:11:56 +0200
Subject: [PATCH 0637/1230] adding the initial pwm trigger logic

---
 .../camera_trigger/interfaces/src/pwm.cpp     | 107 +++++++++++++++++-
 .../camera_trigger/interfaces/src/pwm.h       |  19 ++++
 2 files changed, 125 insertions(+), 1 deletion(-)

diff --git a/src/drivers/camera_trigger/interfaces/src/pwm.cpp b/src/drivers/camera_trigger/interfaces/src/pwm.cpp
index 67eedaf471..0aae31a2e8 100644
--- a/src/drivers/camera_trigger/interfaces/src/pwm.cpp
+++ b/src/drivers/camera_trigger/interfaces/src/pwm.cpp
@@ -1,8 +1,33 @@
 #include "pwm.h"
 
 CameraInterfacePWM::CameraInterfacePWM():
-	CameraInterface()
+	CameraInterface(),
+	_camera_is_on(false)
 {
+	_p_pin = param_find("TRIG_PINS");
+	int pin_list;
+	param_get(_p_pin, &pin_list);
+
+	// Set all pins as invalid
+	for (unsigned i = 0; i < sizeof(_pins) / sizeof(_pins[0]); i++) {
+		_pins[i] = -1;
+	}
+
+	// Convert number to individual channels
+	unsigned i = 0;
+	int single_pin;
+
+	while ((single_pin = pin_list % 10)) {
+
+		_pins[i] = single_pin - 1;
+
+		if (_pins[i] < 0 || _pins[i] >= static_cast<int>(sizeof(_gpios) / sizeof(_gpios[0]))) {
+			_pins[i] = -1;
+		}
+
+		pin_list /= 10;
+		i++;
+	}
 }
 
 CameraInterfacePWM::~CameraInterfacePWM()
@@ -11,20 +36,100 @@ CameraInterfacePWM::~CameraInterfacePWM()
 
 void CameraInterfacePWM::setup()
 {
+	_pwm_dev = PWM_DEVICE_PATH;  // Used for direct pwm output without mixer
+	_pwm_fd = open(_pwm_dev, 0);  // open pwm device
 
+	if (_pwm_fd < 0) {
+		err(1, "can't open %s", _pwm_dev);
+	}
+
+	for (unsigned i = 0; i < sizeof(_pins) / sizeof(_pins[0]); i++) {
+		if (_pin[i] >= 0) {
+			int ret_camera_pwm = px4_ioctl(_pwm_fd, PWM_CAMERA_SET(_pin[i]), math::constrain(PWM_CAMERA_DISARMED, 1000, 2000));
+		}
+	}
 }
 
 void CameraInterfacePWM::trigger(bool enable)
 {
+	// Check if armed, otherwise don't send high PWM values
+	if (_disarmed) {
+		for (unsigned i = 0; i < sizeof(_pins) / sizeof(_pins[0]); i++) {
+			if (_pin[i] >= 0) {
+				int ret_camera_pwm = px4_ioctl(_pwm_fd, PWM_CAMERA_SET(_pin[i]), math::constrain(PWM_CAMERA_DISARMED, 1000, 2000));
+			}
+		}
 
+	} else {
+		if (!_camera_is_on) {
+			// Turn camera on and give time to start up
+			powerOn();
+			return;
+		}
+
+		if (trig) {
+			// Set all valid pins to shoot level
+			for (unsigned i = 0; i < sizeof(_pins) / sizeof(_pins[0]); i++) {
+				if (_pin[i] >= 0) {
+					int ret_camera_pwm = px4_ioctl(_pwm_fd, PWM_CAMERA_SET(_pin[i]), math::constrain(PWM_CAMERA_INSTANT_SHOOT, 1000, 2000));
+				}
+			}
+
+		} else {
+			// Set all valid pins back to neutral level
+			for (unsigned i = 0; i < sizeof(_pins) / sizeof(_pins[0]); i++) {
+				if (_pin[i] >= 0) {
+					int ret_camera_pwm = px4_ioctl(_pwm_fd, PWM_CAMERA_SET(_pin[i]), math::constrain(PWM_CAMERA_NEUTRAL, 1000, 2000));
+				}
+			}
+		}
+	}
 }
 
 int CameraInterfacePWM::powerOn()
 {
+	// Check if armed, otherwise don't send high PWM values
+	if (_disarmed) {
+		for (unsigned i = 0; i < sizeof(_pins) / sizeof(_pins[0]); i++) {
+			if (_pin[i] >= 0) {
+				int ret_camera_pwm = px4_ioctl(_pwm_fd, PWM_CAMERA_SET(_pin[i]), math::constrain(PWM_CAMERA_DISARMED, 1000, 2000));
+			}
+		}
+
+	} else {
+		// Set all valid pins to turn on level
+		for (unsigned i = 0; i < sizeof(_pins) / sizeof(_pins[0]); i++) {
+			if (_pin[i] >= 0) {
+				int ret_camera_pwm = px4_ioctl(_pwm_fd, PWM_CAMERA_SET(_pin[i]), math::constrain(PWM_CAMERA_ON, 1000, 2000));
+			}
+		}
+
+		_camera_is_on = true;
+	}
+
 	return 0;
 }
 
 int CameraInterfacePWM::powerOff()
 {
+	// Check if armed, otherwise don't send high PWM values
+	if (_disarmed) {
+		for (unsigned i = 0; i < sizeof(_pins) / sizeof(_pins[0]); i++) {
+			if (_pin[i] >= 0) {
+				int ret_camera_pwm = px4_ioctl(_pwm_fd, PWM_CAMERA_SET(_pin[i]), math::constrain(PWM_CAMERA_DISARMED, 1000, 2000));
+			}
+		}
+
+	} else {
+		// Set all valid pins to turn off level
+		for (unsigned i = 0; i < sizeof(_pins) / sizeof(_pins[0]); i++) {
+			if (_pin[i] >= 0) {
+				int ret_camera_pwm = px4_ioctl(_pwm_fd, PWM_CAMERA_SET(_pin[i]), math::constrain(PWM_CAMERA_OFF, 1000, 2000));
+			}
+		}
+
+		_camera_is_on = false;
+	}
+
 	return 0;
 }
diff --git a/src/drivers/camera_trigger/interfaces/src/pwm.h b/src/drivers/camera_trigger/interfaces/src/pwm.h
index 7632feb119..4cd21c29b9 100644
--- a/src/drivers/camera_trigger/interfaces/src/pwm.h
+++ b/src/drivers/camera_trigger/interfaces/src/pwm.h
@@ -8,6 +8,19 @@
 
 #include "camera_interface.h"
 
+// TODO(birchera): check if this is the right device and addresses
+#define PWM_DEVICE_PATH /dev/pwm_output0
+#define PWM_CAMERA_BASE		0x2a00
+#define PWM_CAMERA_SET(_pin)	_PX4_IOC(_PWM_CAMERA_BASE, 0x30 + _pin)
+// PWM levels of the interface to seagull MAP converter to
+// Multiport (http://www.seagulluav.com/manuals/Seagull_MAP2-Manual.pdf)
+#define PWM_CAMERA_DISARMED 90 // TODO(birchera): check here value
+#define PWM_CAMERA_ON 1100
+#define PWM_CAMERA_AUTOFOCUS_SHOOT 1300
+#define PWM_CAMERA_NEUTRAL 1500
+#define PWM_CAMERA_INSTANT_SHOOT 1700
+#define PWM_CAMERA_OFF 1900
+
 
 class CameraInterfacePWM : public CameraInterface
 {
@@ -22,6 +35,12 @@ public:
 	int powerOn();
 	int powerOff();
 
+	int _pins[6];
 private:
 
+	param_t _p_pin;
+	const char *_pwm_dev;
+	int _pwm_fd;
+	bool _camera_is_on;
+
 };

From 4c5f32ab16857555c770a1426088c0604826e678 Mon Sep 17 00:00:00 2001
From: Kelly Steich <kelly.steich@wingtra.com>
Date: Fri, 10 Jun 2016 14:20:26 +0200
Subject: [PATCH 0638/1230] delete the camera interface object in camera
 trigger destructor

---
 src/drivers/camera_trigger/camera_trigger.cpp | 2 ++
 1 file changed, 2 insertions(+)

diff --git a/src/drivers/camera_trigger/camera_trigger.cpp b/src/drivers/camera_trigger/camera_trigger.cpp
index b68e482e2b..7ce77a2642 100644
--- a/src/drivers/camera_trigger/camera_trigger.cpp
+++ b/src/drivers/camera_trigger/camera_trigger.cpp
@@ -236,6 +236,8 @@ CameraTrigger::CameraTrigger() :
 
 CameraTrigger::~CameraTrigger()
 {
+	delete(_camera_interface);
+
 	camera_trigger::g_camera_trigger = nullptr;
 }
 

From adffb859627779712b62da118f0a402e6c2f16e7 Mon Sep 17 00:00:00 2001
From: Andreas Bircher <bircher@gmx.ch>
Date: Fri, 10 Jun 2016 14:54:12 +0200
Subject: [PATCH 0639/1230] adding arming check before setting PWM

---
 .../camera_trigger/interfaces/src/pwm.cpp     | 67 +++++++++++++------
 .../camera_trigger/interfaces/src/pwm.h       | 24 ++++---
 2 files changed, 62 insertions(+), 29 deletions(-)

diff --git a/src/drivers/camera_trigger/interfaces/src/pwm.cpp b/src/drivers/camera_trigger/interfaces/src/pwm.cpp
index 0aae31a2e8..c5df0e489e 100644
--- a/src/drivers/camera_trigger/interfaces/src/pwm.cpp
+++ b/src/drivers/camera_trigger/interfaces/src/pwm.cpp
@@ -1,7 +1,12 @@
+#include <px4.h>
+#include <sys/ioctl.h>
+#include <lib/helpfuncs/helpfuncs.h>
+
 #include "pwm.h"
 
 CameraInterfacePWM::CameraInterfacePWM():
 	CameraInterface(),
+	_vehicle_status_sub(-1),
 	_camera_is_on(false)
 {
 	_p_pin = param_find("TRIG_PINS");
@@ -21,7 +26,7 @@ CameraInterfacePWM::CameraInterfacePWM():
 
 		_pins[i] = single_pin - 1;
 
-		if (_pins[i] < 0 || _pins[i] >= static_cast<int>(sizeof(_gpios) / sizeof(_gpios[0]))) {
+		if (_pins[i] < 0) {
 			_pins[i] = -1;
 		}
 
@@ -44,8 +49,9 @@ void CameraInterfacePWM::setup()
 	}
 
 	for (unsigned i = 0; i < sizeof(_pins) / sizeof(_pins[0]); i++) {
-		if (_pin[i] >= 0) {
-			int ret_camera_pwm = px4_ioctl(_pwm_fd, PWM_CAMERA_SET(_pin[i]), math::constrain(PWM_CAMERA_DISARMED, 1000, 2000));
+		if (_pins[i] >= 0) {
+			px4_ioctl(_pwm_fd, PWM_CAMERA_SET(_pins[i]), math::constrain(PWM_CAMERA_DISARMED, 1000,
+					2000)); // TODO(birchera): use return value to make sure everything is fine
 		}
 	}
 }
@@ -53,10 +59,17 @@ void CameraInterfacePWM::setup()
 void CameraInterfacePWM::trigger(bool enable)
 {
 	// Check if armed, otherwise don't send high PWM values
-	if (_disarmed) {
+	if (_vehicle_status_sub < 0) {
+		_vehicle_status_sub = orb_subscribe(ORB_ID(vehicle_status));
+	}
+
+	orb_copy(ORB_ID(vehicle_status), _vehicle_status_sub, &_vehicle_status);
+
+	if (_vehicle_status.arming_state != _vehicle_status.ARMING_STATE_ARMED
+	    || !_vehicle_status.no_pwm) {
 		for (unsigned i = 0; i < sizeof(_pins) / sizeof(_pins[0]); i++) {
-			if (_pin[i] >= 0) {
-				int ret_camera_pwm = px4_ioctl(_pwm_fd, PWM_CAMERA_SET(_pin[i]), math::constrain(PWM_CAMERA_DISARMED, 1000, 2000));
+			if (_pins[i] >= 0) {
+				px4_ioctl(_pwm_fd, PWM_CAMERA_SET(_pins[i]), math::constrain(PWM_CAMERA_DISARMED, 1000, 2000));
 			}
 		}
 
@@ -70,16 +83,16 @@ void CameraInterfacePWM::trigger(bool enable)
 		if (trig) {
 			// Set all valid pins to shoot level
 			for (unsigned i = 0; i < sizeof(_pins) / sizeof(_pins[0]); i++) {
-				if (_pin[i] >= 0) {
-					int ret_camera_pwm = px4_ioctl(_pwm_fd, PWM_CAMERA_SET(_pin[i]), math::constrain(PWM_CAMERA_INSTANT_SHOOT, 1000, 2000));
+				if (_pins[i] >= 0) {
+					px4_ioctl(_pwm_fd, PWM_CAMERA_SET(_pins[i]), math::constrain(PWM_CAMERA_INSTANT_SHOOT, 1000, 2000));
 				}
 			}
 
 		} else {
 			// Set all valid pins back to neutral level
 			for (unsigned i = 0; i < sizeof(_pins) / sizeof(_pins[0]); i++) {
-				if (_pin[i] >= 0) {
-					int ret_camera_pwm = px4_ioctl(_pwm_fd, PWM_CAMERA_SET(_pin[i]), math::constrain(PWM_CAMERA_NEUTRAL, 1000, 2000));
+				if (_pins[i] >= 0) {
+					px4_ioctl(_pwm_fd, PWM_CAMERA_SET(_pins[i]), math::constrain(PWM_CAMERA_NEUTRAL, 1000, 2000));
 				}
 			}
 		}
@@ -89,18 +102,25 @@ void CameraInterfacePWM::trigger(bool enable)
 int CameraInterfacePWM::powerOn()
 {
 	// Check if armed, otherwise don't send high PWM values
-	if (_disarmed) {
+	if (_vehicle_status_sub < 0) {
+		_vehicle_status_sub = orb_subscribe(ORB_ID(vehicle_status));
+	}
+
+	orb_copy(ORB_ID(vehicle_status), _vehicle_status_sub, &_vehicle_status);
+
+	if (_vehicle_status.arming_state != _vehicle_status.ARMING_STATE_ARMED
+	    || !_vehicle_status.no_pwm) {
 		for (unsigned i = 0; i < sizeof(_pins) / sizeof(_pins[0]); i++) {
-			if (_pin[i] >= 0) {
-				int ret_camera_pwm = px4_ioctl(_pwm_fd, PWM_CAMERA_SET(_pin[i]), math::constrain(PWM_CAMERA_DISARMED, 1000, 2000));
+			if (_pins[i] >= 0) {
+				px4_ioctl(_pwm_fd, PWM_CAMERA_SET(_pins[i]), math::constrain(PWM_CAMERA_DISARMED, 1000, 2000));
 			}
 		}
 
 	} else {
 		// Set all valid pins to turn on level
 		for (unsigned i = 0; i < sizeof(_pins) / sizeof(_pins[0]); i++) {
-			if (_pin[i] >= 0) {
-				int ret_camera_pwm = px4_ioctl(_pwm_fd, PWM_CAMERA_SET(_pin[i]), math::constrain(PWM_CAMERA_ON, 1000, 2000));
+			if (_pins[i] >= 0) {
+				px4_ioctl(_pwm_fd, PWM_CAMERA_SET(_pins[i]), math::constrain(PWM_CAMERA_ON, 1000, 2000));
 			}
 		}
 
@@ -113,18 +133,25 @@ int CameraInterfacePWM::powerOn()
 int CameraInterfacePWM::powerOff()
 {
 	// Check if armed, otherwise don't send high PWM values
-	if (_disarmed) {
+	if (_vehicle_status_sub < 0) {
+		_vehicle_status_sub = orb_subscribe(ORB_ID(vehicle_status));
+	}
+
+	orb_copy(ORB_ID(vehicle_status), _vehicle_status_sub, &_vehicle_status);
+
+	if (_vehicle_status.arming_state != _vehicle_status.ARMING_STATE_ARMED
+	    || !_vehicle_status.no_pwm) {
 		for (unsigned i = 0; i < sizeof(_pins) / sizeof(_pins[0]); i++) {
-			if (_pin[i] >= 0) {
-				int ret_camera_pwm = px4_ioctl(_pwm_fd, PWM_CAMERA_SET(_pin[i]), math::constrain(PWM_CAMERA_DISARMED, 1000, 2000));
+			if (_pins[i] >= 0) {
+				px4_ioctl(_pwm_fd, PWM_CAMERA_SET(_pins[i]), math::constrain(PWM_CAMERA_DISARMED, 1000, 2000));
 			}
 		}
 
 	} else {
 		// Set all valid pins to turn off level
 		for (unsigned i = 0; i < sizeof(_pins) / sizeof(_pins[0]); i++) {
-			if (_pin[i] >= 0) {
-				int ret_camera_pwm = px4_ioctl(_pwm_fd, PWM_CAMERA_SET(_pin[i]), math::constrain(PWM_CAMERA_OFF, 1000, 2000));
+			if (_pins[i] >= 0) {
+				px4_ioctl(_pwm_fd, PWM_CAMERA_SET(_pins[i]), math::constrain(PWM_CAMERA_OFF, 1000, 2000));
 			}
 		}
 
diff --git a/src/drivers/camera_trigger/interfaces/src/pwm.h b/src/drivers/camera_trigger/interfaces/src/pwm.h
index 4cd21c29b9..5dceba0a35 100644
--- a/src/drivers/camera_trigger/interfaces/src/pwm.h
+++ b/src/drivers/camera_trigger/interfaces/src/pwm.h
@@ -6,20 +6,23 @@
  */
 #pragma once
 
+#include <systemlib/param/param.h>
+
+#include <uORB/topics/vehicle_status.h>
 #include "camera_interface.h"
 
 // TODO(birchera): check if this is the right device and addresses
-#define PWM_DEVICE_PATH /dev/pwm_output0
-#define PWM_CAMERA_BASE		0x2a00
-#define PWM_CAMERA_SET(_pin)	_PX4_IOC(_PWM_CAMERA_BASE, 0x30 + _pin)
+#define PWM_DEVICE_PATH			"/dev/pwm_output0"
+#define PWM_CAMERA_BASE			0x2a00
+#define PWM_CAMERA_SET(_pin)	_PX4_IOC(PWM_CAMERA_BASE, 0x30 + _pin)
 // PWM levels of the interface to seagull MAP converter to
 // Multiport (http://www.seagulluav.com/manuals/Seagull_MAP2-Manual.pdf)
-#define PWM_CAMERA_DISARMED 90 // TODO(birchera): check here value
-#define PWM_CAMERA_ON 1100
-#define PWM_CAMERA_AUTOFOCUS_SHOOT 1300
-#define PWM_CAMERA_NEUTRAL 1500
-#define PWM_CAMERA_INSTANT_SHOOT 1700
-#define PWM_CAMERA_OFF 1900
+#define PWM_CAMERA_DISARMED			90 // TODO(birchera): check here value
+#define PWM_CAMERA_ON				1100
+#define PWM_CAMERA_AUTOFOCUS_SHOOT	1300
+#define PWM_CAMERA_NEUTRAL			1500
+#define PWM_CAMERA_INSTANT_SHOOT	1700
+#define PWM_CAMERA_OFF				1900
 
 
 class CameraInterfacePWM : public CameraInterface
@@ -38,6 +41,9 @@ public:
 	int _pins[6];
 private:
 
+	int _vehicle_status_sub;
+	struct vehicle_status_s _vehicle_status;
+
 	param_t _p_pin;
 	const char *_pwm_dev;
 	int _pwm_fd;

From 2ec1e508d2ca82c54a1228f30fbbf78901d04f90 Mon Sep 17 00:00:00 2001
From: Kelly Steich <kelly.steich@wingtra.com>
Date: Fri, 10 Jun 2016 15:29:52 +0200
Subject: [PATCH 0640/1230] added setup method to constructor of camera
 interfaces

Conflicts:
	PX4/src/drivers/camera_trigger/interfaces/src/pwm.cpp
---
 .../camera_trigger/interfaces/src/camera_interface.h     | 9 ++++-----
 src/drivers/camera_trigger/interfaces/src/pwm.cpp        | 1 +
 src/drivers/camera_trigger/interfaces/src/pwm.h          | 3 +--
 src/drivers/camera_trigger/interfaces/src/relay.cpp      | 2 ++
 src/drivers/camera_trigger/interfaces/src/relay.h        | 5 +++--
 5 files changed, 11 insertions(+), 9 deletions(-)

diff --git a/src/drivers/camera_trigger/interfaces/src/camera_interface.h b/src/drivers/camera_trigger/interfaces/src/camera_interface.h
index 5554fc3d6c..e6b848884c 100644
--- a/src/drivers/camera_trigger/interfaces/src/camera_interface.h
+++ b/src/drivers/camera_trigger/interfaces/src/camera_interface.h
@@ -18,11 +18,6 @@ public:
 	 */
 	virtual ~CameraInterface();
 
-	/**
-	 * setup the interface
-	 */
-	virtual void setup() {};
-
 	/**
 	 * trigger the camera
 	 * @param trigger:
@@ -50,5 +45,9 @@ public:
 
 protected:
 
+	/**
+	 * setup the interface
+	 */
+	virtual void setup() {};
 
 };
diff --git a/src/drivers/camera_trigger/interfaces/src/pwm.cpp b/src/drivers/camera_trigger/interfaces/src/pwm.cpp
index c5df0e489e..4fcbcf8ac3 100644
--- a/src/drivers/camera_trigger/interfaces/src/pwm.cpp
+++ b/src/drivers/camera_trigger/interfaces/src/pwm.cpp
@@ -33,6 +33,7 @@ CameraInterfacePWM::CameraInterfacePWM():
 		pin_list /= 10;
 		i++;
 	}
+	setup();
 }
 
 CameraInterfacePWM::~CameraInterfacePWM()
diff --git a/src/drivers/camera_trigger/interfaces/src/pwm.h b/src/drivers/camera_trigger/interfaces/src/pwm.h
index 5dceba0a35..3eab5d8de2 100644
--- a/src/drivers/camera_trigger/interfaces/src/pwm.h
+++ b/src/drivers/camera_trigger/interfaces/src/pwm.h
@@ -31,8 +31,6 @@ public:
 	CameraInterfacePWM();
 	virtual ~CameraInterfacePWM();
 
-	void setup();
-
 	void trigger(bool enable);
 
 	int powerOn();
@@ -40,6 +38,7 @@ public:
 
 	int _pins[6];
 private:
+	void setup();
 
 	int _vehicle_status_sub;
 	struct vehicle_status_s _vehicle_status;
diff --git a/src/drivers/camera_trigger/interfaces/src/relay.cpp b/src/drivers/camera_trigger/interfaces/src/relay.cpp
index c07215bba5..a8efb6d813 100644
--- a/src/drivers/camera_trigger/interfaces/src/relay.cpp
+++ b/src/drivers/camera_trigger/interfaces/src/relay.cpp
@@ -34,6 +34,8 @@ CameraInterfaceRelay::CameraInterfaceRelay():
 		pin_list /= 10;
 		i++;
 	}
+
+	setup();
 }
 
 CameraInterfaceRelay::~CameraInterfaceRelay()
diff --git a/src/drivers/camera_trigger/interfaces/src/relay.h b/src/drivers/camera_trigger/interfaces/src/relay.h
index 371ae9511f..5a63251f48 100644
--- a/src/drivers/camera_trigger/interfaces/src/relay.h
+++ b/src/drivers/camera_trigger/interfaces/src/relay.h
@@ -19,8 +19,6 @@ public:
 	CameraInterfaceRelay();
 	virtual ~CameraInterfaceRelay();
 
-	void setup();
-
 	void trigger(bool enable);
 
 	void info();
@@ -29,6 +27,9 @@ public:
 	int _polarity;
 
 private:
+
+	void setup();
+
 	param_t _p_pin;
 	param_t _p_polarity;
 

From c49a2da2614d5d4cfbf395e23048afcbdeac4b65 Mon Sep 17 00:00:00 2001
From: Andreas Bircher <bircher@gmx.ch>
Date: Fri, 10 Jun 2016 14:54:12 +0200
Subject: [PATCH 0641/1230] adding arming check before setting PWM

---
 src/drivers/camera_trigger/interfaces/src/pwm.h | 3 +++
 1 file changed, 3 insertions(+)

diff --git a/src/drivers/camera_trigger/interfaces/src/pwm.h b/src/drivers/camera_trigger/interfaces/src/pwm.h
index 3eab5d8de2..205ee7f4f8 100644
--- a/src/drivers/camera_trigger/interfaces/src/pwm.h
+++ b/src/drivers/camera_trigger/interfaces/src/pwm.h
@@ -43,6 +43,9 @@ private:
 	int _vehicle_status_sub;
 	struct vehicle_status_s _vehicle_status;
 
+	int _vehicle_status_sub;
+	struct vehicle_status_s _vehicle_status;
+
 	param_t _p_pin;
 	const char *_pwm_dev;
 	int _pwm_fd;

From eed968979ff3780b78cba1e0fcc01b563e8ecb11 Mon Sep 17 00:00:00 2001
From: Kelly Steich <kelly.steich@wingtra.com>
Date: Tue, 14 Jun 2016 16:21:11 +0200
Subject: [PATCH 0642/1230] added parameter for choosing the camera interface
 mode

Conflicts:
	PX4/src/drivers/camera_trigger/camera_trigger.cpp
---
 src/drivers/camera_trigger/camera_trigger.cpp | 54 +++++++++----------
 1 file changed, 27 insertions(+), 27 deletions(-)

diff --git a/src/drivers/camera_trigger/camera_trigger.cpp b/src/drivers/camera_trigger/camera_trigger.cpp
index 7ce77a2642..1f17877144 100644
--- a/src/drivers/camera_trigger/camera_trigger.cpp
+++ b/src/drivers/camera_trigger/camera_trigger.cpp
@@ -73,7 +73,7 @@ public:
 	/**
 	 * Constructor
 	 */
-	CameraTrigger();
+	CameraTrigger(camera_interface_mode_t camera_interface_mode);
 
 	/**
 	 * Destructor, also kills task.
@@ -137,14 +137,8 @@ private:
 	param_t _p_distance;
 	param_t _p_pin;
 
-	static constexpr uint32_t _gpios[6] = {
-		GPIO_GPIO0_OUTPUT,
-		GPIO_GPIO1_OUTPUT,
-		GPIO_GPIO2_OUTPUT,
-		GPIO_GPIO3_OUTPUT,
-		GPIO_GPIO4_OUTPUT,
-		GPIO_GPIO5_OUTPUT
-	};
+	camera_interface_mode_t _camera_interface_mode;
+	CameraInterface *_camera_interface;  ///< instance of camera interface
 
 	/**
 	 * Vehicle command handler
@@ -172,8 +166,7 @@ namespace camera_trigger
 CameraTrigger	*g_camera_trigger;
 }
 
-CameraTrigger::CameraTrigger() :
-	_pins{},
+CameraTrigger::CameraTrigger(camera_interface_mode_t camera_interface_mode) :
 	_engagecall {},
 	_disengagecall {},
 	_gpio_fd(-1),
@@ -188,7 +181,9 @@ CameraTrigger::CameraTrigger() :
 	_valid_position(false),
 	_vcommand_sub(-1),
 	_vlposition_sub(-1),
-	_trigger_pub(nullptr)
+	_trigger_pub(nullptr),
+	_camera_interface_mode(camera_interface_mode),
+	_camera_interface(nullptr)
 {
 	memset(&_work, 0, sizeof(_work));
 
@@ -428,11 +423,7 @@ CameraTrigger::engage(void *arg)
 	/* set timestamp the instant before the trigger goes off */
 	report.timestamp = hrt_absolute_time();
 
-<<<<<<< HEAD
-	CameraTrigger::trigger(trig, trig->_polarity);
-=======
 	trig->_camera_interface->trigger(true);
->>>>>>> fb669fc... fixed the triggering function logic
 
 	report.seq = trig->_trigger_seq++;
 
@@ -442,16 +433,9 @@ CameraTrigger::engage(void *arg)
 void
 CameraTrigger::disengage(void *arg)
 {
-<<<<<<< HEAD
-
-	CameraTrigger *trig = reinterpret_cast<CameraTrigger *>(arg);
-
-	CameraTrigger::trigger(trig, !(trig->_polarity));
-=======
 	CameraTrigger *trig = reinterpret_cast<CameraTrigger *>(arg);
 
 	trig->_camera_interface->trigger(false);
->>>>>>> fb669fc... fixed the triggering function logic
 }
 
 void
@@ -472,14 +456,15 @@ CameraTrigger::info()
 	warnx("pins 1-3 : %d,%d,%d polarity : %s", _pins[0], _pins[1], _pins[2],
 	      _polarity ? "ACTIVE_HIGH" : "ACTIVE_LOW");
 	warnx("mode : %i", _mode);
-	warnx("interval : %.2f", (double)_interval);
-	warnx("distance : %.2f", (double)_distance);
+	warnx("interval : %.2f [ms]", (double)_interval);
+	warnx("distance : %.2f [m]", (double)_distance);
+	warnx("activation time : %.2f [ms]", (double)_activation_time);
 	_camera_interface->info();
 }
 
 static void usage()
 {
-	errx(1, "usage: camera_trigger {start|stop|info} [-p <n>]\n");
+	errx(1, "usage: camera_trigger {start {--relay|--pwm}|stop|info}\n");
 }
 
 int camera_trigger_main(int argc, char *argv[])
@@ -494,7 +479,22 @@ int camera_trigger_main(int argc, char *argv[])
 			errx(0, "already running");
 		}
 
-		camera_trigger::g_camera_trigger = new CameraTrigger;
+		if (argc >= 3) {
+			if (!strcmp(argv[2], "--relay")) {
+				camera_trigger::g_camera_trigger = new CameraTrigger(CAMERA_INTERFACE_MODE_RELAY);
+
+			} else if (!strcmp(argv[2], "--pwm")) {
+				camera_trigger::g_camera_trigger = new CameraTrigger(CAMERA_INTERFACE_MODE_PWM);
+
+			} else {
+				usage();
+				return 0;
+			}
+
+		} else {
+			camera_trigger::g_camera_trigger = new CameraTrigger(CAMERA_INTERFACE_MODE_RELAY);
+			warnx("started with default camera interface mode : relay");
+		}
 
 		if (camera_trigger::g_camera_trigger == nullptr) {
 			errx(1, "alloc failed");

From f038b167347877b1bcf11ff57efd55d024843eeb Mon Sep 17 00:00:00 2001
From: Andreas Bircher <bircher@gmx.ch>
Date: Fri, 10 Jun 2016 14:54:12 +0200
Subject: [PATCH 0643/1230] adding arming check before setting PWM

---
 src/drivers/camera_trigger/interfaces/src/pwm.h | 3 +++
 1 file changed, 3 insertions(+)

diff --git a/src/drivers/camera_trigger/interfaces/src/pwm.h b/src/drivers/camera_trigger/interfaces/src/pwm.h
index 205ee7f4f8..8e96e92b9a 100644
--- a/src/drivers/camera_trigger/interfaces/src/pwm.h
+++ b/src/drivers/camera_trigger/interfaces/src/pwm.h
@@ -46,6 +46,9 @@ private:
 	int _vehicle_status_sub;
 	struct vehicle_status_s _vehicle_status;
 
+	int _vehicle_status_sub;
+	struct vehicle_status_s _vehicle_status;
+
 	param_t _p_pin;
 	const char *_pwm_dev;
 	int _pwm_fd;

From e951a356fec50f3f725a36ed07d350abefdf0333 Mon Sep 17 00:00:00 2001
From: Andreas Bircher <bircher@gmx.ch>
Date: Thu, 16 Jun 2016 11:25:06 +0200
Subject: [PATCH 0644/1230] fixing the driver interface

Conflicts:
	PX4/src/drivers/camera_trigger/interfaces/src/pwm.cpp
	PX4/src/drivers/camera_trigger/interfaces/src/pwm.h
---
 ROMFS/px4fmu_common/init.d/rcS                |   4 +-
 .../camera_trigger/interfaces/src/pwm.cpp     | 124 ++++++------------
 .../camera_trigger/interfaces/src/pwm.h       |  11 --
 3 files changed, 42 insertions(+), 97 deletions(-)

diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS
index e6d95e59f7..c21f80edfc 100644
--- a/ROMFS/px4fmu_common/init.d/rcS
+++ b/ROMFS/px4fmu_common/init.d/rcS
@@ -537,7 +537,7 @@ then
 			# Get FMU driver out of the way
 			set MIXER_AUX none
 			set AUX_MODE none
-			camera_trigger start
+			camera_trigger start --pwm
 		fi
 	fi
 
@@ -606,7 +606,7 @@ then
 		# Try to get an USB console
 		nshterm /dev/ttyACM0 &
 	else
-		mavlink start -r 800000 -d /dev/ttyACM0 -m config -x
+		mavlink start -r 80000 -d /dev/ttyACM0 -m config -x
 	fi
 
 	#
diff --git a/src/drivers/camera_trigger/interfaces/src/pwm.cpp b/src/drivers/camera_trigger/interfaces/src/pwm.cpp
index 4fcbcf8ac3..cd92402643 100644
--- a/src/drivers/camera_trigger/interfaces/src/pwm.cpp
+++ b/src/drivers/camera_trigger/interfaces/src/pwm.cpp
@@ -4,9 +4,17 @@
 
 #include "pwm.h"
 
+// PWM levels of the interface to seagull MAP converter to
+// Multiport (http://www.seagulluav.com/manuals/Seagull_MAP2-Manual.pdf)
+#define PWM_CAMERA_DISARMED			90 // TODO(birchera): check here value
+#define PWM_CAMERA_ON				1100
+#define PWM_CAMERA_AUTOFOCUS_SHOOT	1300
+#define PWM_CAMERA_NEUTRAL			1500
+#define PWM_CAMERA_INSTANT_SHOOT	1700
+#define PWM_CAMERA_OFF				1900
+
 CameraInterfacePWM::CameraInterfacePWM():
 	CameraInterface(),
-	_vehicle_status_sub(-1),
 	_camera_is_on(false)
 {
 	_p_pin = param_find("TRIG_PINS");
@@ -42,59 +50,37 @@ CameraInterfacePWM::~CameraInterfacePWM()
 
 void CameraInterfacePWM::setup()
 {
-	_pwm_dev = PWM_DEVICE_PATH;  // Used for direct pwm output without mixer
-	_pwm_fd = open(_pwm_dev, 0);  // open pwm device
-
-	if (_pwm_fd < 0) {
-		err(1, "can't open %s", _pwm_dev);
-	}
-
 	for (unsigned i = 0; i < sizeof(_pins) / sizeof(_pins[0]); i++) {
 		if (_pins[i] >= 0) {
-			px4_ioctl(_pwm_fd, PWM_CAMERA_SET(_pins[i]), math::constrain(PWM_CAMERA_DISARMED, 1000,
-					2000)); // TODO(birchera): use return value to make sure everything is fine
+			// TODO(birchera): use return value to make sure everything is fine
+			up_pwm_servo_set(_pins[i], math::constrain(PWM_CAMERA_DISARMED, PWM_CAMERA_DISARMED, 2000));
 		}
 	}
 }
 
 void CameraInterfacePWM::trigger(bool enable)
 {
-	// Check if armed, otherwise don't send high PWM values
-	if (_vehicle_status_sub < 0) {
-		_vehicle_status_sub = orb_subscribe(ORB_ID(vehicle_status));
+	// This only starts working upon prearming
+
+	if (!_camera_is_on) {
+		// Turn camera on and give time to start up
+		powerOn();
+		return;
 	}
 
-	orb_copy(ORB_ID(vehicle_status), _vehicle_status_sub, &_vehicle_status);
-
-	if (_vehicle_status.arming_state != _vehicle_status.ARMING_STATE_ARMED
-	    || !_vehicle_status.no_pwm) {
+	if (enable) {
+		// Set all valid pins to shoot level
 		for (unsigned i = 0; i < sizeof(_pins) / sizeof(_pins[0]); i++) {
 			if (_pins[i] >= 0) {
-				px4_ioctl(_pwm_fd, PWM_CAMERA_SET(_pins[i]), math::constrain(PWM_CAMERA_DISARMED, 1000, 2000));
+				up_pwm_servo_set(_pins[i], math::constrain(PWM_CAMERA_INSTANT_SHOOT, 1000, 2000));
 			}
 		}
 
 	} else {
-		if (!_camera_is_on) {
-			// Turn camera on and give time to start up
-			powerOn();
-			return;
-		}
-
-		if (trig) {
-			// Set all valid pins to shoot level
-			for (unsigned i = 0; i < sizeof(_pins) / sizeof(_pins[0]); i++) {
-				if (_pins[i] >= 0) {
-					px4_ioctl(_pwm_fd, PWM_CAMERA_SET(_pins[i]), math::constrain(PWM_CAMERA_INSTANT_SHOOT, 1000, 2000));
-				}
-			}
-
-		} else {
-			// Set all valid pins back to neutral level
-			for (unsigned i = 0; i < sizeof(_pins) / sizeof(_pins[0]); i++) {
-				if (_pins[i] >= 0) {
-					px4_ioctl(_pwm_fd, PWM_CAMERA_SET(_pins[i]), math::constrain(PWM_CAMERA_NEUTRAL, 1000, 2000));
-				}
+		// Set all valid pins back to neutral level
+		for (unsigned i = 0; i < sizeof(_pins) / sizeof(_pins[0]); i++) {
+			if (_pins[i] >= 0) {
+				up_pwm_servo_set(_pins[i], math::constrain(PWM_CAMERA_NEUTRAL, 1000, 2000));
 			}
 		}
 	}
@@ -102,62 +88,32 @@ void CameraInterfacePWM::trigger(bool enable)
 
 int CameraInterfacePWM::powerOn()
 {
-	// Check if armed, otherwise don't send high PWM values
-	if (_vehicle_status_sub < 0) {
-		_vehicle_status_sub = orb_subscribe(ORB_ID(vehicle_status));
+	// This only starts working upon prearming
+
+	// Set all valid pins to turn on level
+	for (unsigned i = 0; i < sizeof(_pins) / sizeof(_pins[0]); i++) {
+		if (_pins[i] >= 0) {
+			up_pwm_servo_set(_pins[i], math::constrain(PWM_CAMERA_ON, 1000, 2000));
+		}
 	}
 
-	orb_copy(ORB_ID(vehicle_status), _vehicle_status_sub, &_vehicle_status);
-
-	if (_vehicle_status.arming_state != _vehicle_status.ARMING_STATE_ARMED
-	    || !_vehicle_status.no_pwm) {
-		for (unsigned i = 0; i < sizeof(_pins) / sizeof(_pins[0]); i++) {
-			if (_pins[i] >= 0) {
-				px4_ioctl(_pwm_fd, PWM_CAMERA_SET(_pins[i]), math::constrain(PWM_CAMERA_DISARMED, 1000, 2000));
-			}
-		}
-
-	} else {
-		// Set all valid pins to turn on level
-		for (unsigned i = 0; i < sizeof(_pins) / sizeof(_pins[0]); i++) {
-			if (_pins[i] >= 0) {
-				px4_ioctl(_pwm_fd, PWM_CAMERA_SET(_pins[i]), math::constrain(PWM_CAMERA_ON, 1000, 2000));
-			}
-		}
-
-		_camera_is_on = true;
-	}
+	_camera_is_on = true;
 
 	return 0;
 }
 
 int CameraInterfacePWM::powerOff()
 {
-	// Check if armed, otherwise don't send high PWM values
-	if (_vehicle_status_sub < 0) {
-		_vehicle_status_sub = orb_subscribe(ORB_ID(vehicle_status));
+	// This only starts working upon prearming
+
+	// Set all valid pins to turn off level
+	for (unsigned i = 0; i < sizeof(_pins) / sizeof(_pins[0]); i++) {
+		if (_pins[i] >= 0) {
+			up_pwm_servo_set(_pins[i], math::constrain(PWM_CAMERA_OFF, 1000, 2000));
+		}
 	}
 
-	orb_copy(ORB_ID(vehicle_status), _vehicle_status_sub, &_vehicle_status);
-
-	if (_vehicle_status.arming_state != _vehicle_status.ARMING_STATE_ARMED
-	    || !_vehicle_status.no_pwm) {
-		for (unsigned i = 0; i < sizeof(_pins) / sizeof(_pins[0]); i++) {
-			if (_pins[i] >= 0) {
-				px4_ioctl(_pwm_fd, PWM_CAMERA_SET(_pins[i]), math::constrain(PWM_CAMERA_DISARMED, 1000, 2000));
-			}
-		}
-
-	} else {
-		// Set all valid pins to turn off level
-		for (unsigned i = 0; i < sizeof(_pins) / sizeof(_pins[0]); i++) {
-			if (_pins[i] >= 0) {
-				px4_ioctl(_pwm_fd, PWM_CAMERA_SET(_pins[i]), math::constrain(PWM_CAMERA_OFF, 1000, 2000));
-			}
-		}
-
-		_camera_is_on = false;
-	}
+	_camera_is_on = false;
 
 	return 0;
 }
diff --git a/src/drivers/camera_trigger/interfaces/src/pwm.h b/src/drivers/camera_trigger/interfaces/src/pwm.h
index 8e96e92b9a..0c9d3850c2 100644
--- a/src/drivers/camera_trigger/interfaces/src/pwm.h
+++ b/src/drivers/camera_trigger/interfaces/src/pwm.h
@@ -40,18 +40,7 @@ public:
 private:
 	void setup();
 
-	int _vehicle_status_sub;
-	struct vehicle_status_s _vehicle_status;
-
-	int _vehicle_status_sub;
-	struct vehicle_status_s _vehicle_status;
-
-	int _vehicle_status_sub;
-	struct vehicle_status_s _vehicle_status;
-
 	param_t _p_pin;
-	const char *_pwm_dev;
-	int _pwm_fd;
 	bool _camera_is_on;
 
 };

From 6bd17c7ba4e36d53702de95c460e89f27c4e88e8 Mon Sep 17 00:00:00 2001
From: Andreas Bircher <bircher@gmx.ch>
Date: Thu, 16 Jun 2016 13:39:55 +0200
Subject: [PATCH 0645/1230] adding user info

---
 src/drivers/camera_trigger/camera_trigger.cpp       | 2 ++
 src/drivers/camera_trigger/interfaces/src/pwm.cpp   | 5 +++++
 src/drivers/camera_trigger/interfaces/src/pwm.h     | 2 ++
 src/drivers/camera_trigger/interfaces/src/relay.cpp | 2 +-
 4 files changed, 10 insertions(+), 1 deletion(-)

diff --git a/src/drivers/camera_trigger/camera_trigger.cpp b/src/drivers/camera_trigger/camera_trigger.cpp
index 1f17877144..5b1a739620 100644
--- a/src/drivers/camera_trigger/camera_trigger.cpp
+++ b/src/drivers/camera_trigger/camera_trigger.cpp
@@ -482,9 +482,11 @@ int camera_trigger_main(int argc, char *argv[])
 		if (argc >= 3) {
 			if (!strcmp(argv[2], "--relay")) {
 				camera_trigger::g_camera_trigger = new CameraTrigger(CAMERA_INTERFACE_MODE_RELAY);
+				warnx("started with camera interface mode : relay");
 
 			} else if (!strcmp(argv[2], "--pwm")) {
 				camera_trigger::g_camera_trigger = new CameraTrigger(CAMERA_INTERFACE_MODE_PWM);
+				warnx("started with camera interface mode : pwm");
 
 			} else {
 				usage();
diff --git a/src/drivers/camera_trigger/interfaces/src/pwm.cpp b/src/drivers/camera_trigger/interfaces/src/pwm.cpp
index cd92402643..c42faf79da 100644
--- a/src/drivers/camera_trigger/interfaces/src/pwm.cpp
+++ b/src/drivers/camera_trigger/interfaces/src/pwm.cpp
@@ -117,3 +117,8 @@ int CameraInterfacePWM::powerOff()
 
 	return 0;
 }
+
+void CameraInterfaceRelay::info()
+{
+	warnx("PWM - camera triggering, pins 1-3 : %d,%d,%d", _pins[0], _pins[1], _pins[2]);
+}
diff --git a/src/drivers/camera_trigger/interfaces/src/pwm.h b/src/drivers/camera_trigger/interfaces/src/pwm.h
index 0c9d3850c2..43182e2a9f 100644
--- a/src/drivers/camera_trigger/interfaces/src/pwm.h
+++ b/src/drivers/camera_trigger/interfaces/src/pwm.h
@@ -36,6 +36,8 @@ public:
 	int powerOn();
 	int powerOff();
 
+	void info();
+
 	int _pins[6];
 private:
 	void setup();
diff --git a/src/drivers/camera_trigger/interfaces/src/relay.cpp b/src/drivers/camera_trigger/interfaces/src/relay.cpp
index a8efb6d813..0922777f64 100644
--- a/src/drivers/camera_trigger/interfaces/src/relay.cpp
+++ b/src/drivers/camera_trigger/interfaces/src/relay.cpp
@@ -72,6 +72,6 @@ void CameraInterfaceRelay::trigger(bool enable)
 
 void CameraInterfaceRelay::info()
 {
-	warnx("pins 1-3 : %d,%d,%d polarity : %s", _pins[0], _pins[1], _pins[2],
+	warnx("Relay - camera triggering, pins 1-3 : %d,%d,%d polarity : %s", _pins[0], _pins[1], _pins[2],
 	      _polarity ? "ACTIVE_HIGH" : "ACTIVE_LOW");
 }

From 46ec1e6b95a23f2f3577ff47f96e22c5c47e013c Mon Sep 17 00:00:00 2001
From: Andreas Bircher <bircher@gmx.ch>
Date: Thu, 16 Jun 2016 15:40:01 +0200
Subject: [PATCH 0646/1230] fixing cherry-picking divergences

---
 ROMFS/px4fmu_common/init.d/rcS                |  4 +-
 src/drivers/camera_trigger/camera_trigger.cpp | 86 +++++++------------
 .../interfaces/src/camera_interface.cpp       | 14 +++
 .../camera_trigger/interfaces/src/pwm.cpp     |  6 +-
 .../camera_trigger/interfaces/src/pwm.h       | 14 ---
 .../camera_trigger/interfaces/src/relay.cpp   |  8 +-
 6 files changed, 57 insertions(+), 75 deletions(-)

diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS
index c21f80edfc..e6d95e59f7 100644
--- a/ROMFS/px4fmu_common/init.d/rcS
+++ b/ROMFS/px4fmu_common/init.d/rcS
@@ -537,7 +537,7 @@ then
 			# Get FMU driver out of the way
 			set MIXER_AUX none
 			set AUX_MODE none
-			camera_trigger start --pwm
+			camera_trigger start
 		fi
 	fi
 
@@ -606,7 +606,7 @@ then
 		# Try to get an USB console
 		nshterm /dev/ttyACM0 &
 	else
-		mavlink start -r 80000 -d /dev/ttyACM0 -m config -x
+		mavlink start -r 800000 -d /dev/ttyACM0 -m config -x
 	fi
 
 	#
diff --git a/src/drivers/camera_trigger/camera_trigger.cpp b/src/drivers/camera_trigger/camera_trigger.cpp
index 5b1a739620..8bb18e3466 100644
--- a/src/drivers/camera_trigger/camera_trigger.cpp
+++ b/src/drivers/camera_trigger/camera_trigger.cpp
@@ -36,7 +36,11 @@
  *
  * External camera-IMU synchronisation and triggering via FMU auxiliary pins.
  *
+ * Support for camera manipulation via PWM signal over servo pins.
+ *
  * @author Mohammed Kabir <mhkabir98@gmail.com>
+ * @author Kelly Steich <kelly.steich@wingtra.com>
+ * @author Andreas Bircher <andreas@wingtra.com>
  */
 
 #include <stdio.h>
@@ -63,10 +67,19 @@
 #include <drivers/drv_hrt.h>
 #include <board_config.h>
 
+#include "interfaces/src/pwm.h"
+#include "interfaces/src/relay.h"
+
 #define TRIGGER_PIN_DEFAULT 1
 
 extern "C" __EXPORT int camera_trigger_main(int argc, char *argv[]);
 
+typedef enum {
+	CAMERA_INTERFACE_MODE_NONE = 0,
+	CAMERA_INTERFACE_MODE_RELAY,
+	CAMERA_INTERFACE_MODE_PWM
+} camera_interface_mode_t;
+
 class CameraTrigger
 {
 public:
@@ -105,8 +118,6 @@ public:
 	 */
 	void		info();
 
-	int			_pins[6];
-
 private:
 
 	struct hrt_call		_engagecall;
@@ -115,7 +126,6 @@ private:
 
 	int 			_gpio_fd;
 
-	int 			_polarity;
 	int				_mode;
 	float 			_activation_time;
 	float  			_interval;
@@ -130,7 +140,6 @@ private:
 
 	orb_advert_t		_trigger_pub;
 
-	param_t _p_polarity;
 	param_t _p_mode;
 	param_t _p_activation_time;
 	param_t _p_interval;
@@ -153,12 +162,9 @@ private:
 	 */
 	static void	disengage(void *arg);
 
-	static void trigger(CameraTrigger *trig, bool trigger);
-
 };
 
 struct work_s CameraTrigger::_work;
-constexpr uint32_t CameraTrigger::_gpios[6];
 
 namespace camera_trigger
 {
@@ -170,7 +176,6 @@ CameraTrigger::CameraTrigger(camera_interface_mode_t camera_interface_mode) :
 	_engagecall {},
 	_disengagecall {},
 	_gpio_fd(-1),
-	_polarity(0),
 	_mode(0),
 	_activation_time(0.5f /* ms */),
 	_interval(100.0f /* ms */),
@@ -185,44 +190,38 @@ CameraTrigger::CameraTrigger(camera_interface_mode_t camera_interface_mode) :
 	_camera_interface_mode(camera_interface_mode),
 	_camera_interface(nullptr)
 {
+	//Initiate Camera interface basedon camera_interface_mode
+	if (_camera_interface != nullptr) {
+		delete(_camera_interface);
+		/* set to zero to ensure parser is not used while not instantiated */
+		_camera_interface = nullptr;
+	}
+
+	switch (_camera_interface_mode) {
+	case CAMERA_INTERFACE_MODE_RELAY:
+		_camera_interface = new CameraInterfaceRelay;
+		break;
+
+	case CAMERA_INTERFACE_MODE_PWM:
+		_camera_interface = new CameraInterfacePWM;
+		break;
+
+	default:
+		break;
+	}
+
 	memset(&_work, 0, sizeof(_work));
 
 	// Parameters
-	_p_polarity = param_find("TRIG_POLARITY");
 	_p_interval = param_find("TRIG_INTERVAL");
 	_p_distance = param_find("TRIG_DISTANCE");
 	_p_activation_time = param_find("TRIG_ACT_TIME");
 	_p_mode = param_find("TRIG_MODE");
-	_p_pin = param_find("TRIG_PINS");
 
-	param_get(_p_polarity, &_polarity);
 	param_get(_p_activation_time, &_activation_time);
 	param_get(_p_interval, &_interval);
 	param_get(_p_distance, &_distance);
 	param_get(_p_mode, &_mode);
-	int pin_list;
-	param_get(_p_pin, &pin_list);
-
-	// Set all pins as invalid
-	for (unsigned i = 0; i < sizeof(_pins) / sizeof(_pins[0]); i++) {
-		_pins[i] = -1;
-	}
-
-	// Convert number to individual channels
-	unsigned i = 0;
-	int single_pin;
-
-	while ((single_pin = pin_list % 10)) {
-
-		_pins[i] = single_pin - 1;
-
-		if (_pins[i] < 0 || _pins[i] >= static_cast<int>(sizeof(_gpios) / sizeof(_gpios[0]))) {
-			_pins[i] = -1;
-		}
-
-		pin_list /= 10;
-		i++;
-	}
 
 	struct camera_trigger_s	camera_trigger = {};
 
@@ -278,12 +277,6 @@ CameraTrigger::shootOnce()
 void
 CameraTrigger::start()
 {
-
-	for (unsigned i = 0; i < sizeof(_pins) / sizeof(_pins[0]); i++) {
-		px4_arch_configgpio(_gpios[_pins[i]]);
-		px4_arch_gpiowrite(_gpios[_pins[i]], !_polarity);
-	}
-
 	// enable immediate if configured that way
 	if (_mode == 2) {
 		control(true);
@@ -438,23 +431,10 @@ CameraTrigger::disengage(void *arg)
 	trig->_camera_interface->trigger(false);
 }
 
-void
-CameraTrigger::trigger(CameraTrigger *trig, bool trigger)
-{
-	for (unsigned i = 0; i < sizeof(trig->_pins) / sizeof(trig->_pins[0]); i++) {
-		if (trig->_pins[i] >= 0) {
-			// ACTIVE_LOW == 1
-			px4_arch_gpiowrite(trig->_gpios[trig->_pins[i]], trigger);
-		}
-	}
-}
-
 void
 CameraTrigger::info()
 {
 	warnx("state : %s", _trigger_enabled ? "enabled" : "disabled");
-	warnx("pins 1-3 : %d,%d,%d polarity : %s", _pins[0], _pins[1], _pins[2],
-	      _polarity ? "ACTIVE_HIGH" : "ACTIVE_LOW");
 	warnx("mode : %i", _mode);
 	warnx("interval : %.2f [ms]", (double)_interval);
 	warnx("distance : %.2f [m]", (double)_distance);
diff --git a/src/drivers/camera_trigger/interfaces/src/camera_interface.cpp b/src/drivers/camera_trigger/interfaces/src/camera_interface.cpp
index e69de29bb2..d464296dda 100644
--- a/src/drivers/camera_trigger/interfaces/src/camera_interface.cpp
+++ b/src/drivers/camera_trigger/interfaces/src/camera_interface.cpp
@@ -0,0 +1,14 @@
+#include "camera_interface.h"
+
+/**
+ * @file camera_interface.cpp
+ *
+ */
+
+CameraInterface::CameraInterface()
+{
+}
+
+CameraInterface::~CameraInterface()
+{
+}
diff --git a/src/drivers/camera_trigger/interfaces/src/pwm.cpp b/src/drivers/camera_trigger/interfaces/src/pwm.cpp
index c42faf79da..502f57713c 100644
--- a/src/drivers/camera_trigger/interfaces/src/pwm.cpp
+++ b/src/drivers/camera_trigger/interfaces/src/pwm.cpp
@@ -1,7 +1,8 @@
 #include <px4.h>
 #include <sys/ioctl.h>
-#include <lib/helpfuncs/helpfuncs.h>
+#include <lib/mathlib/mathlib.h>
 
+#include "drivers/drv_pwm_output.h"
 #include "pwm.h"
 
 // PWM levels of the interface to seagull MAP converter to
@@ -41,6 +42,7 @@ CameraInterfacePWM::CameraInterfacePWM():
 		pin_list /= 10;
 		i++;
 	}
+
 	setup();
 }
 
@@ -118,7 +120,7 @@ int CameraInterfacePWM::powerOff()
 	return 0;
 }
 
-void CameraInterfaceRelay::info()
+void CameraInterfacePWM::info()
 {
 	warnx("PWM - camera triggering, pins 1-3 : %d,%d,%d", _pins[0], _pins[1], _pins[2]);
 }
diff --git a/src/drivers/camera_trigger/interfaces/src/pwm.h b/src/drivers/camera_trigger/interfaces/src/pwm.h
index 43182e2a9f..7310be8b9e 100644
--- a/src/drivers/camera_trigger/interfaces/src/pwm.h
+++ b/src/drivers/camera_trigger/interfaces/src/pwm.h
@@ -11,20 +11,6 @@
 #include <uORB/topics/vehicle_status.h>
 #include "camera_interface.h"
 
-// TODO(birchera): check if this is the right device and addresses
-#define PWM_DEVICE_PATH			"/dev/pwm_output0"
-#define PWM_CAMERA_BASE			0x2a00
-#define PWM_CAMERA_SET(_pin)	_PX4_IOC(PWM_CAMERA_BASE, 0x30 + _pin)
-// PWM levels of the interface to seagull MAP converter to
-// Multiport (http://www.seagulluav.com/manuals/Seagull_MAP2-Manual.pdf)
-#define PWM_CAMERA_DISARMED			90 // TODO(birchera): check here value
-#define PWM_CAMERA_ON				1100
-#define PWM_CAMERA_AUTOFOCUS_SHOOT	1300
-#define PWM_CAMERA_NEUTRAL			1500
-#define PWM_CAMERA_INSTANT_SHOOT	1700
-#define PWM_CAMERA_OFF				1900
-
-
 class CameraInterfacePWM : public CameraInterface
 {
 public:
diff --git a/src/drivers/camera_trigger/interfaces/src/relay.cpp b/src/drivers/camera_trigger/interfaces/src/relay.cpp
index 0922777f64..d67903bd29 100644
--- a/src/drivers/camera_trigger/interfaces/src/relay.cpp
+++ b/src/drivers/camera_trigger/interfaces/src/relay.cpp
@@ -45,8 +45,8 @@ CameraInterfaceRelay::~CameraInterfaceRelay()
 void CameraInterfaceRelay::setup()
 {
 	for (unsigned i = 0; i < sizeof(_pins) / sizeof(_pins[0]); i++) {
-		stm32_configgpio(_gpios[_pins[i]]);
-		stm32_gpiowrite(_gpios[_pins[i]], !_polarity);
+		px4_arch_configgpio(_gpios[_pins[i]]);
+		px4_arch_gpiowrite(_gpios[_pins[i]], !_polarity);
 	}
 }
 
@@ -56,7 +56,7 @@ void CameraInterfaceRelay::trigger(bool enable)
 		for (unsigned i = 0; i < sizeof(_pins) / sizeof(_pins[0]); i++) {
 			if (_pins[i] >= 0) {
 				// ACTIVE_LOW == 1
-				stm32_gpiowrite(_gpios[_pins[i]], _polarity);
+				px4_arch_gpiowrite(_gpios[_pins[i]], _polarity);
 			}
 		}
 
@@ -64,7 +64,7 @@ void CameraInterfaceRelay::trigger(bool enable)
 		for (unsigned i = 0; i < sizeof(_pins) / sizeof(_pins[0]); i++) {
 			if (_pins[i] >= 0) {
 				// ACTIVE_LOW == 1
-				stm32_gpiowrite(_gpios[_pins[i]], !_polarity);
+				px4_arch_gpiowrite(_gpios[_pins[i]], !_polarity);
 			}
 		}
 	}

From f11d42aab365809be3c765927392987fc6aca27e Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Wed, 8 Jun 2016 09:34:20 +0200
Subject: [PATCH 0647/1230] Simulator MAVLink: Forward port cleanly

---
 src/modules/simulator/simulator_mavlink.cpp | 66 ++++++---------------
 1 file changed, 19 insertions(+), 47 deletions(-)

diff --git a/src/modules/simulator/simulator_mavlink.cpp b/src/modules/simulator/simulator_mavlink.cpp
index 87b773c651..2c3fe4f155 100644
--- a/src/modules/simulator/simulator_mavlink.cpp
+++ b/src/modules/simulator/simulator_mavlink.cpp
@@ -72,60 +72,41 @@ static socklen_t _addrlen = sizeof(_srcaddr);
 static bool actuators_on = false;
 static hrt_abstime batt_sim_start = 0;
 
+const unsigned mode_flag_armed = 128; // following MAVLink spec
+const unsigned mode_flag_custom = 1;
+
 using namespace simulator;
 
 void Simulator::pack_actuator_message(mavlink_hil_controls_t &actuator_msg)
 {
+	// reset state
+	memset(&actuator_msg, 0, sizeof(actuator_msg));
+	actuator_msg.time_usec = hrt_absolute_time();
+
 	float out[8] = {};
+	bool armed = (_vehicle_status.arming_state == vehicle_status_s::ARMING_STATE_ARMED);
 
 	const float pwm_center = (PWM_DEFAULT_MAX + PWM_DEFAULT_MIN) / 2;
 
-	// for now we only support quadrotors
-	unsigned n = 4;
+	for (unsigned i = 0; i < (sizeof(out) / sizeof(out[0])); i++) {
+		// scale PWM out 900..2100 us to -1..1 */
+		out[i] = (_actuators.output[i] - pwm_center) / ((PWM_DEFAULT_MAX - PWM_DEFAULT_MIN) / 2);
 
-	if (_vehicle_status.is_rotary_wing || _vehicle_status.is_vtol) {
-		for (unsigned i = 0; i < 8; i++) {
-			if (_actuators.output[i] > PWM_DEFAULT_MIN / 2) {
-				if (i < n) {
-					// scale PWM out 900..2100 us to 0..1 for rotors */
-					out[i] = (_actuators.output[i] - PWM_DEFAULT_MIN) / (PWM_DEFAULT_MAX - PWM_DEFAULT_MIN);
-
-				} else {
-					// scale PWM out 900..2100 us to -1..1 for other channels */
-					out[i] = (_actuators.output[i] - pwm_center) / ((PWM_DEFAULT_MAX - PWM_DEFAULT_MIN) / 2);
-				}
-
-			} else {
-				// send 0 when disarmed and for disabled channels */
-				out[i] = 0.0f;
-			}
-		}
-
-	} else {
-		// convert back to range [-1, 1]
-		for (unsigned i = 0; i < 8; i++) {
-			out[i] = (_actuators.output[i] - 1500) / 600.0f;
+		if (!PX4_ISFINITE(out[i])) {
+			out[i] = -1.0f;
 		}
 	}
 
-	// if vehicle status has not yet been updated, set actuator commands to zero
-	// this is to prevent the simulation getting into a bad state
-	if (_vehicle_status.timestamp == 0) {
-		memset(out, 0, sizeof(out));
-	}
-
-	actuators_on = (out[3] > 0.1f);
-
-	actuator_msg.time_usec = hrt_absolute_time();
 	actuator_msg.roll_ailerons = out[0];
-	actuator_msg.pitch_elevator = (_vehicle_status.is_rotary_wing || _vehicle_status.is_vtol)  ? out[1] : -out[1];
+	actuator_msg.pitch_elevator = out[1];
 	actuator_msg.yaw_rudder = out[2];
 	actuator_msg.throttle = out[3];
 	actuator_msg.aux1 = out[4];
 	actuator_msg.aux2 = out[5];
-	actuator_msg.aux3 = _actuators.output[6] > PWM_DEFAULT_MIN / 2 ? out[6] : -1.0f;;
+	actuator_msg.aux3 = out[6];
 	actuator_msg.aux4 = out[7];
-	actuator_msg.mode = 0; // need to put something here
+	actuator_msg.mode = mode_flag_custom;
+	actuator_msg.mode |= (armed) ? mode_flag_armed : 0;
 	actuator_msg.nav_mode = 0;
 }
 
@@ -143,17 +124,6 @@ static void fill_rc_input_msg(struct rc_input_values *rc, mavlink_rc_channels_t
 	rc->channel_count = rc_channels->chancount;
 	rc->rssi = rc_channels->rssi;
 
-	/*	PX4_WARN("RC: %d, %d, %d, %d, %d, %d, %d, %d",
-			 rc_channels->chan1_raw,
-			 rc_channels->chan2_raw,
-			 rc_channels->chan3_raw,
-			 rc_channels->chan4_raw,
-			 rc_channels->chan5_raw,
-			 rc_channels->chan6_raw,
-			 rc_channels->chan7_raw,
-			 rc_channels->chan8_raw);
-	*/
-
 	rc->values[0] = rc_channels->chan1_raw;
 	rc->values[1] = rc_channels->chan2_raw;
 	rc->values[2] = rc_channels->chan3_raw;
@@ -552,6 +522,8 @@ void Simulator::pollForMAVLinkMessages(bool publish, int udp_port)
 			len = recvfrom(_fd, _buf, sizeof(_buf), 0, (struct sockaddr *)&_srcaddr, &_addrlen);
 			// send hearbeat
 			mavlink_heartbeat_t hb = {};
+			hb.autopilot = 12;
+			hb.base_mode |= (_vehicle_status.arming_state == vehicle_status_s::ARMING_STATE_ARMED) ? 128 : 0;
 			send_mavlink_message(MAVLINK_MSG_ID_HEARTBEAT, &hb, 200);
 
 			if (len > 0) {

From 184da9e7435f7c638a4ff67f24a2f28817a0eb5a Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Wed, 8 Jun 2016 09:46:41 +0200
Subject: [PATCH 0648/1230] Remove null mixers from plane now that we are doing
 it correctly

---
 ROMFS/px4fmu_common/mixers/plane_sitl.main.mix | 10 +---------
 1 file changed, 1 insertion(+), 9 deletions(-)

diff --git a/ROMFS/px4fmu_common/mixers/plane_sitl.main.mix b/ROMFS/px4fmu_common/mixers/plane_sitl.main.mix
index 387fbe10c6..a46fae9483 100644
--- a/ROMFS/px4fmu_common/mixers/plane_sitl.main.mix
+++ b/ROMFS/px4fmu_common/mixers/plane_sitl.main.mix
@@ -1,14 +1,6 @@
-Mixer for SITL plane, using the VTOL airframe for now
+Mixer for SITL plane
 =========================================================
 
-Z:
-
-Z:
-
-Z:
-
-Z:
-
 # mixer for the elevons
 M: 2
 O:      10000  10000   0 -10000  10000

From 982c25b7da7471462508b22b2b00ddb009a9651f Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Wed, 8 Jun 2016 10:19:45 +0200
Subject: [PATCH 0649/1230] Update SITL Gazebo with new consistent interface

---
 Tools/sitl_gazebo | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/Tools/sitl_gazebo b/Tools/sitl_gazebo
index 459a0c9335..7c979a59ac 160000
--- a/Tools/sitl_gazebo
+++ b/Tools/sitl_gazebo
@@ -1 +1 @@
-Subproject commit 459a0c933598249fdf30d8162c5b9ec7d6620258
+Subproject commit 7c979a59ac3cbd4bcaf8848d99ca3c55db6bbf32

From afa9467dad7b1664762d92a23c4defd525e6b500 Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Wed, 8 Jun 2016 13:17:04 +0200
Subject: [PATCH 0650/1230] Final plane mixer

---
 .../px4fmu_common/mixers/plane_sitl.main.mix  | 36 +++++++++++++------
 1 file changed, 25 insertions(+), 11 deletions(-)

diff --git a/ROMFS/px4fmu_common/mixers/plane_sitl.main.mix b/ROMFS/px4fmu_common/mixers/plane_sitl.main.mix
index a46fae9483..15cab32411 100644
--- a/ROMFS/px4fmu_common/mixers/plane_sitl.main.mix
+++ b/ROMFS/px4fmu_common/mixers/plane_sitl.main.mix
@@ -1,18 +1,32 @@
 Mixer for SITL plane
 =========================================================
 
-# mixer for the elevons
-M: 2
-O:      10000  10000   0 -10000  10000
-S: 0 0  5000   5000    0 -10000  10000
-S: 0 1 -5000 -5000     0 -10000  10000
-
-M: 2
-O:       10000   10000   0 -10000  10000
-S: 0 0   5000    5000    0 -10000  10000
-S: 0 1   5000    5000    0 -10000  10000
-
 # mixer for the pusher/puller throttle
 M: 1
 O:      10000  10000      0 -10000  10000
 S: 0 3      0  20000 -10000 -10000  10000
+
+# mixer for the left aileron
+M: 1
+O:      10000  10000   0 -10000  10000
+S: 0 0  -10000  -10000    0 -10000  10000
+
+# mixer for the right aileron
+M: 1
+O:      10000  10000   0 -10000  10000
+S: 0 0  10000  10000    0 -10000  10000
+
+# mixer for the elevator
+M: 1
+O:       10000   10000   0 -10000  10000
+S: 0 1   10000   10000    0 -10000  10000
+
+# mixer for the rudder
+M: 1
+O:       10000   10000   0 -10000  10000
+S: 0 2   10000   10000    0 -10000  10000
+
+# mixer for the flaps
+M: 1
+O:       10000   10000   0 -10000  10000
+S: 0 4   10000   10000    0 -10000  10000

From 413233341edf3630fbf00567734e13d72c87a2e0 Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Sat, 11 Jun 2016 17:25:27 +0200
Subject: [PATCH 0651/1230] Update SITL Gazebo model

---
 .../mixers/standard_vtol_sitl.main.mix        | 30 ++++++++++---------
 Tools/sitl_gazebo                             |  2 +-
 2 files changed, 17 insertions(+), 15 deletions(-)

diff --git a/ROMFS/px4fmu_common/mixers/standard_vtol_sitl.main.mix b/ROMFS/px4fmu_common/mixers/standard_vtol_sitl.main.mix
index ffa5a024bc..6e84c2814b 100644
--- a/ROMFS/px4fmu_common/mixers/standard_vtol_sitl.main.mix
+++ b/ROMFS/px4fmu_common/mixers/standard_vtol_sitl.main.mix
@@ -1,24 +1,26 @@
 Mixer for standard vtol plane (SITL) with motor x configuration
 =========================================================
 
-This file defines a single mixer for a standard vtol plane (SITL gazebo) with motors in X configuration.
-The plane has two ailerons and one elevator. The ailerons and elevator are treated as elevons
-in order to make the standard vtol simulation compatible with the tailsitter simulation.
+This file defines a single mixer for a standard vtol plane (SITL gazebo) with motors in X configuration. The plane has two ailerons and one elevator.
 
 R: 4x 10000 10000 10000 0
 
-# mixer for the elevons
-M: 2
-O:      10000  10000   0 -10000  10000
-S: 1 0  5000   5000    0 -10000  10000
-S: 1 1 -5000 -5000     0 -10000  10000
-
-M: 2
-O:       10000   10000   0 -10000  10000
-S: 1 0   5000    5000    0 -10000  10000
-S: 1 1   5000    5000    0 -10000  10000
-
 # mixer for the pusher/puller throttle
 M: 1
 O:      10000  10000      0 -10000  10000
 S: 1 3      0  20000 -10000 -10000  10000
+
+# mixer for the left aileron
+M: 1
+O:      10000  10000   0 -10000  10000
+S: 1 0  10000  10000    0 -10000  10000
+
+# mixer for the right aileron
+M: 1
+O:      10000  10000   0 -10000  10000
+S: 1 0  -10000  -10000    0 -10000  10000
+
+# mixer for the elevator
+M: 1
+O:       10000   10000   0 -10000  10000
+S: 1 1   10000   10000    0 -10000  10000
diff --git a/Tools/sitl_gazebo b/Tools/sitl_gazebo
index 7c979a59ac..b35852694c 160000
--- a/Tools/sitl_gazebo
+++ b/Tools/sitl_gazebo
@@ -1 +1 @@
-Subproject commit 7c979a59ac3cbd4bcaf8848d99ca3c55db6bbf32
+Subproject commit b35852694cb0157a136f8decfd8fecfe4f15ee03

From acd7c370571a94d5823f8976c28eb9cfcc463cec Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Sat, 11 Jun 2016 22:26:56 +0200
Subject: [PATCH 0652/1230] Get closer to correct tailsitter mixer, still
 incomplete

---
 ROMFS/px4fmu_common/mixers/quad_x_vtol.main.mix | 10 +++++++++-
 1 file changed, 9 insertions(+), 1 deletion(-)

diff --git a/ROMFS/px4fmu_common/mixers/quad_x_vtol.main.mix b/ROMFS/px4fmu_common/mixers/quad_x_vtol.main.mix
index bd332a2688..12bd885dce 100644
--- a/ROMFS/px4fmu_common/mixers/quad_x_vtol.main.mix
+++ b/ROMFS/px4fmu_common/mixers/quad_x_vtol.main.mix
@@ -6,13 +6,21 @@ are mixed 100%.
 
 R: 4x 10000 10000 10000 0
 
-#mixer for the elevons
+Z:
+
+# left elevon
 M: 2
 O:      10000  10000      0 -10000  10000
 S: 1 0  5000  5000      0 -10000  10000
 S: 1 1  5000  5000      0 -10000  10000
 
+# right elevon
 M: 2
 O:       10000   10000    0 -10000  10000
 S: 1 0   5000   5000    0 -10000  10000
 S: 1 1  -5000  -5000    0 -10000  10000
+
+# mixer for the virtual elevator
+M: 1
+O:       10000   10000   0 -10000  10000
+S: 1 1   10000   10000    0 -10000  10000

From 96387ed8247f0e1a4a90496452db9148d2113860 Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Sat, 11 Jun 2016 22:27:07 +0200
Subject: [PATCH 0653/1230] Update SITL gazebo

---
 Tools/sitl_gazebo | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/Tools/sitl_gazebo b/Tools/sitl_gazebo
index b35852694c..96225f024e 160000
--- a/Tools/sitl_gazebo
+++ b/Tools/sitl_gazebo
@@ -1 +1 @@
-Subproject commit b35852694cb0157a136f8decfd8fecfe4f15ee03
+Subproject commit 96225f024e0da3efa8231f1e99b56d523845bb5a

From c248adb18de10dfa7b8d6ece5351b23973a4563e Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Sat, 11 Jun 2016 22:39:23 +0200
Subject: [PATCH 0654/1230] Fix tuning gains for Solo and tailsitter

---
 posix-configs/SITL/init/rcS_gazebo_solo       | 4 ++--
 posix-configs/SITL/init/rcS_gazebo_tailsitter | 8 ++++----
 2 files changed, 6 insertions(+), 6 deletions(-)

diff --git a/posix-configs/SITL/init/rcS_gazebo_solo b/posix-configs/SITL/init/rcS_gazebo_solo
index e4d8c958a9..5707b3fc64 100644
--- a/posix-configs/SITL/init/rcS_gazebo_solo
+++ b/posix-configs/SITL/init/rcS_gazebo_solo
@@ -27,8 +27,8 @@ param set RTL_RETURN_ALT 30.0
 param set RTL_DESCEND_ALT 10.0
 param set RTL_LAND_DELAY 0
 param set MIS_TAKEOFF_ALT 2.5
-param set MC_ROLLRATE_P 0.5
-param set MC_PITCHRATE_P 0.5
+param set MC_ROLLRATE_P 0.25
+param set MC_PITCHRATE_P 0.25
 param set MC_PITCH_P 5
 param set MC_ROLL_P 5
 param set MC_ROLLRATE_I 0.1
diff --git a/posix-configs/SITL/init/rcS_gazebo_tailsitter b/posix-configs/SITL/init/rcS_gazebo_tailsitter
index b4954906e3..ebaf0bc136 100644
--- a/posix-configs/SITL/init/rcS_gazebo_tailsitter
+++ b/posix-configs/SITL/init/rcS_gazebo_tailsitter
@@ -1,10 +1,10 @@
 uorb start
 param load
 param set MAV_TYPE 20
-param set MC_PITCHRATE_P 0.3
-param set MC_ROLLRATE_P 0.3
-param set MC_YAW_P 2.0
-param set MC_YAWRATE_P 0.35
+param set MC_PITCHRATE_P 0.25
+param set MC_ROLLRATE_P 0.25
+param set MC_PITCH_P 5
+param set MC_ROLL_P 5
 param set VT_TYPE 0
 param set SYS_AUTOSTART 4010
 param set SYS_RESTART_TYPE 2

From d50e30724310e137e3d7b5610c40098efa383615 Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Sat, 11 Jun 2016 22:39:59 +0200
Subject: [PATCH 0655/1230] Update Tailsitter model

---
 Tools/sitl_gazebo | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/Tools/sitl_gazebo b/Tools/sitl_gazebo
index 96225f024e..f6832e0f2b 160000
--- a/Tools/sitl_gazebo
+++ b/Tools/sitl_gazebo
@@ -1 +1 @@
-Subproject commit 96225f024e0da3efa8231f1e99b56d523845bb5a
+Subproject commit f6832e0f2b960fdce8c378c45d69c1b31a539750

From 3770bfd6414ebc6081da3b195ba14d97c9a65d06 Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Sat, 11 Jun 2016 23:26:39 +0200
Subject: [PATCH 0656/1230] Update Gazebo model for plane

---
 Tools/sitl_gazebo | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/Tools/sitl_gazebo b/Tools/sitl_gazebo
index f6832e0f2b..38f2538f58 160000
--- a/Tools/sitl_gazebo
+++ b/Tools/sitl_gazebo
@@ -1 +1 @@
-Subproject commit f6832e0f2b960fdce8c378c45d69c1b31a539750
+Subproject commit 38f2538f58becbe74fc24d10a788f8f86cc64c5c

From 825ba912c3a02621c542f082b537128c3902ab16 Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Sat, 11 Jun 2016 23:27:10 +0200
Subject: [PATCH 0657/1230] Update plane mixer

---
 .../px4fmu_common/mixers/plane_sitl.main.mix  | 24 +++++++++++--------
 1 file changed, 14 insertions(+), 10 deletions(-)

diff --git a/ROMFS/px4fmu_common/mixers/plane_sitl.main.mix b/ROMFS/px4fmu_common/mixers/plane_sitl.main.mix
index 15cab32411..f694702765 100644
--- a/ROMFS/px4fmu_common/mixers/plane_sitl.main.mix
+++ b/ROMFS/px4fmu_common/mixers/plane_sitl.main.mix
@@ -1,6 +1,20 @@
 Mixer for SITL plane
 =========================================================
 
+Z:
+
+Z:
+
+# mixer for the rudder
+M: 1
+O:       10000   10000   0 -10000  10000
+S: 0 2   10000   10000    0 -10000  10000
+
+# mixer for the flaps
+M: 1
+O:       10000   10000   0 -10000  10000
+S: 0 4   10000   10000    0 -10000  10000
+
 # mixer for the pusher/puller throttle
 M: 1
 O:      10000  10000      0 -10000  10000
@@ -20,13 +34,3 @@ S: 0 0  10000  10000    0 -10000  10000
 M: 1
 O:       10000   10000   0 -10000  10000
 S: 0 1   10000   10000    0 -10000  10000
-
-# mixer for the rudder
-M: 1
-O:       10000   10000   0 -10000  10000
-S: 0 2   10000   10000    0 -10000  10000
-
-# mixer for the flaps
-M: 1
-O:       10000   10000   0 -10000  10000
-S: 0 4   10000   10000    0 -10000  10000

From 55267b9ad3abed3e5d90ae3be11bcb6d9363e7ec Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Sun, 12 Jun 2016 10:55:13 +0200
Subject: [PATCH 0658/1230] Add new airframe

---
 Tools/sitl_gazebo | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/Tools/sitl_gazebo b/Tools/sitl_gazebo
index 38f2538f58..76de55d2ca 160000
--- a/Tools/sitl_gazebo
+++ b/Tools/sitl_gazebo
@@ -1 +1 @@
-Subproject commit 38f2538f58becbe74fc24d10a788f8f86cc64c5c
+Subproject commit 76de55d2ca66ecfa9f4209aac32f48b2e1d0b9e7

From dff50072e9a1810081938ac1dbd24c6441fc9f05 Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Sun, 12 Jun 2016 10:55:56 +0200
Subject: [PATCH 0659/1230] Add new airframe meta files

---
 Makefile                                      |  2 +-
 .../SITL/init/rcS_gazebo_typhoon_h480         | 75 +++++++++++++++++++
 src/firmware/posix/CMakeLists.txt             |  2 +-
 3 files changed, 77 insertions(+), 2 deletions(-)
 create mode 100644 posix-configs/SITL/init/rcS_gazebo_typhoon_h480

diff --git a/Makefile b/Makefile
index c14c2fc411..05426971a7 100644
--- a/Makefile
+++ b/Makefile
@@ -320,7 +320,7 @@ distclean: submodulesclean
 cmake_targets = test upload package package_source debug debug_tui debug_ddd debug_io debug_io_tui debug_io_ddd check_weak \
 	run_cmake_config config gazebo gazebo_gdb gazebo_lldb jmavsim replay \
 	jmavsim_gdb jmavsim_lldb gazebo_gdb_iris gazebo_lldb_tailsitter gazebo_iris gazebo_iris_opt_flow gazebo_tailsitter \
-	gazebo_gdb_standard_vtol gazebo_lldb_standard_vtol gazebo_standard_vtol gazebo_plane gazebo_solo
+	gazebo_gdb_standard_vtol gazebo_lldb_standard_vtol gazebo_standard_vtol gazebo_plane gazebo_solo gazebo_typhoon_h480
 $(foreach targ,$(cmake_targets),$(eval $(call cmake-targ,$(targ))))
 
 .PHONY: clean
diff --git a/posix-configs/SITL/init/rcS_gazebo_typhoon_h480 b/posix-configs/SITL/init/rcS_gazebo_typhoon_h480
new file mode 100644
index 0000000000..6a47ce7ff5
--- /dev/null
+++ b/posix-configs/SITL/init/rcS_gazebo_typhoon_h480
@@ -0,0 +1,75 @@
+uorb start
+param load
+param set MAV_TYPE 13
+param set SYS_AUTOSTART 6001
+param set SYS_RESTART_TYPE 2
+dataman start
+param set CAL_GYRO0_ID 2293768
+param set CAL_ACC0_ID 1376264
+param set CAL_ACC1_ID 1310728
+param set CAL_MAG0_ID 196616
+param set CAL_GYRO0_XOFF 0.01
+param set CAL_ACC0_XOFF 0.01
+param set CAL_ACC0_YOFF -0.01
+param set CAL_ACC0_ZOFF 0.01
+param set CAL_ACC0_XSCALE 1.01
+param set CAL_ACC0_YSCALE 1.01
+param set CAL_ACC0_ZSCALE 1.01
+param set CAL_ACC1_XOFF 0.01
+param set CAL_MAG0_XOFF 0.01
+param set SENS_BOARD_ROT 8
+param set SENS_BOARD_X_OFF 0.000001
+param set COM_RC_IN_MODE 1
+param set NAV_DLL_ACT 2
+param set COM_DISARM_LAND 3
+param set NAV_ACC_RAD 2.0
+param set RTL_RETURN_ALT 30.0
+param set RTL_DESCEND_ALT 10.0
+param set RTL_LAND_DELAY 0
+param set MIS_TAKEOFF_ALT 2.5
+param set MC_ROLLRATE_P 0.25
+param set MC_PITCHRATE_P 0.25
+param set MC_PITCH_P 5
+param set MC_ROLL_P 5
+param set MC_ROLLRATE_I 0.1
+param set MC_PITCHRATE_I 0.1
+param set MPC_HOLD_MAX_Z 2.0
+param set MPC_HOLD_XY_DZ 0.1
+param set MPC_Z_VEL_P 0.8
+param set MPC_Z_VEL_I 0.15
+param set MPC_XY_VEL_P 0.15
+param set MPC_XY_VEL_I 0.2
+param set EKF2_GBIAS_INIT 0.01
+param set EKF2_ANGERR_INIT 0.01
+simulator start -s
+rgbledsim start
+tone_alarm start
+gyrosim start
+accelsim start
+barosim start
+adcsim start
+gpssim start
+pwm_out_sim mode_pwm
+sleep 1
+sensors start
+commander start
+land_detector start multicopter
+navigator start
+ekf2 start
+mc_pos_control start
+mc_att_control start
+mixer load /dev/pwm_output0 ../../../../ROMFS/px4fmu_common/mixers/hexa_x.main.mix
+mavlink start -u 14556 -r 4000000
+mavlink start -u 14557 -r 4000000 -m onboard -o 14540
+mavlink stream -r 50 -s POSITION_TARGET_LOCAL_NED -u 14556
+mavlink stream -r 50 -s LOCAL_POSITION_NED -u 14556
+mavlink stream -r 50 -s GLOBAL_POSITION_INT -u 14556
+mavlink stream -r 50 -s ATTITUDE -u 14556
+mavlink stream -r 50 -s ATTITUDE_QUATERNION -u 14556
+mavlink stream -r 50 -s ATTITUDE_TARGET -u 14556
+mavlink stream -r 50 -s SERVO_OUTPUT_RAW_0 -u 14556
+mavlink stream -r 20 -s RC_CHANNELS -u 14556
+mavlink stream -r 250 -s HIGHRES_IMU -u 14556
+mavlink stream -r 10 -s OPTICAL_FLOW_RAD -u 14556
+sdlog2 start -r 100 -e -t -a
+mavlink boot_complete
diff --git a/src/firmware/posix/CMakeLists.txt b/src/firmware/posix/CMakeLists.txt
index a1c5f1c374..74d4025a7a 100644
--- a/src/firmware/posix/CMakeLists.txt
+++ b/src/firmware/posix/CMakeLists.txt
@@ -128,7 +128,7 @@ add_dependencies(run_config mainapp)
 
 foreach(viewer none jmavsim gazebo replay)
 	foreach(debugger none gdb lldb ddd valgrind)
-		foreach(model none iris iris_opt_flow tailsitter standard_vtol plane solo)
+		foreach(model none iris iris_opt_flow tailsitter standard_vtol plane solo typhoon_h480)
 			if (debugger STREQUAL "none")
 				if (model STREQUAL "none")
 					set(_targ_name "${viewer}")

From 0126e49841dc1e3f4258ba9955d90abfc98ce547 Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Sun, 12 Jun 2016 10:56:09 +0200
Subject: [PATCH 0660/1230] Solo: Use the right mixer

---
 posix-configs/SITL/init/rcS_gazebo_solo | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/posix-configs/SITL/init/rcS_gazebo_solo b/posix-configs/SITL/init/rcS_gazebo_solo
index 5707b3fc64..198bb2815d 100644
--- a/posix-configs/SITL/init/rcS_gazebo_solo
+++ b/posix-configs/SITL/init/rcS_gazebo_solo
@@ -58,7 +58,7 @@ navigator start
 ekf2 start
 mc_pos_control start
 mc_att_control start
-mixer load /dev/pwm_output0 ../../../../ROMFS/px4fmu_common/mixers/quad_w.main.mix
+mixer load /dev/pwm_output0 ../../../../ROMFS/px4fmu_common/mixers/quad_x.main.mix
 mavlink start -u 14556 -r 4000000
 mavlink start -u 14557 -r 4000000 -m onboard -o 14540
 mavlink stream -r 50 -s POSITION_TARGET_LOCAL_NED -u 14556

From e6b82898fb54bc9b9379ed135f72e095ef112433 Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Sun, 12 Jun 2016 15:30:50 +0200
Subject: [PATCH 0661/1230] Add airframe model

---
 Tools/sitl_gazebo | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/Tools/sitl_gazebo b/Tools/sitl_gazebo
index 76de55d2ca..682656decb 160000
--- a/Tools/sitl_gazebo
+++ b/Tools/sitl_gazebo
@@ -1 +1 @@
-Subproject commit 76de55d2ca66ecfa9f4209aac32f48b2e1d0b9e7
+Subproject commit 682656decbc93026c888a37a88202eb1f389437c

From e8274d3ddb42599e930b5072058af3ec7e196e61 Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Mon, 13 Jun 2016 19:28:28 +0200
Subject: [PATCH 0662/1230] SITL: Add gimbal meshes

---
 Tools/sitl_gazebo | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/Tools/sitl_gazebo b/Tools/sitl_gazebo
index 682656decb..80139a2b75 160000
--- a/Tools/sitl_gazebo
+++ b/Tools/sitl_gazebo
@@ -1 +1 @@
-Subproject commit 682656decbc93026c888a37a88202eb1f389437c
+Subproject commit 80139a2b75b789f51cab3d6cbad6d916a8c6d7f1

From 9230688f541a0e169b89100213635b339c55ba8f Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Wed, 15 Jun 2016 20:42:56 +0200
Subject: [PATCH 0663/1230] Commander: Add transition command

---
 src/modules/commander/commander.cpp | 25 ++++++++++++++++++++++++-
 1 file changed, 24 insertions(+), 1 deletion(-)

diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp
index 5b9079a0df..afb8a11f7b 100644
--- a/src/modules/commander/commander.cpp
+++ b/src/modules/commander/commander.cpp
@@ -441,6 +441,29 @@ int commander_main(int argc, char *argv[])
 		return 0;
 	}
 
+	if (!strcmp(argv[1], "transition")) {
+
+		vehicle_command_s cmd = {};
+		cmd.target_system = status.system_id;
+		cmd.target_component = status.component_id;
+
+		cmd.command = vehicle_command_s::VEHICLE_CMD_DO_VTOL_TRANSITION;
+		/* transition to the other mode */
+		cmd.param1 = (status.is_rotary_wing) ? vtol_vehicle_status_s::VEHICLE_VTOL_STATE_FW : vtol_vehicle_status_s::VEHICLE_VTOL_STATE_MC;
+		/* param 2-3 unused */
+		cmd.param2 = NAN;
+		cmd.param3 = NAN;
+		cmd.param4 = NAN;
+		cmd.param5 = NAN;
+		cmd.param6 = NAN;
+		cmd.param7 = NAN;
+
+		orb_advert_t h = orb_advertise(ORB_ID(vehicle_command), &cmd);
+		(void)orb_unadvertise(h);
+
+		return 0;
+	}
+
 	if (!strcmp(argv[1], "mode")) {
 		if (argc > 2) {
 			uint8_t new_main_state = commander_state_s::MAIN_STATE_MAX;
@@ -513,7 +536,7 @@ void usage(const char *reason)
 		PX4_INFO("%s", reason);
 	}
 
-	PX4_INFO("usage: commander {start|stop|status|calibrate|check|arm|disarm|takeoff|land|mode}\n");
+	PX4_INFO("usage: commander {start|stop|status|calibrate|check|arm|disarm|takeoff|land|transition|mode}\n");
 }
 
 void print_status()

From c285e231b7cc08f00a3c7c7e164bde54fc9c6c26 Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Thu, 16 Jun 2016 23:34:35 +0200
Subject: [PATCH 0664/1230] Update jMAVSim

---
 Tools/jMAVSim | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/Tools/jMAVSim b/Tools/jMAVSim
index 39cee41574..a5b0f1f089 160000
--- a/Tools/jMAVSim
+++ b/Tools/jMAVSim
@@ -1 +1 @@
-Subproject commit 39cee41574520c4151e318925c6982f3269b790e
+Subproject commit a5b0f1f0896c9372c62222e88117c6ced2c6282b

From 85e3073f1487b08ed9f8fbfc42abbc53801df30e Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Sat, 18 Jun 2016 01:50:42 +0200
Subject: [PATCH 0665/1230] Simulator: Send all actuator output groups

---
 src/modules/simulator/simulator.h           | 14 ++++++---
 src/modules/simulator/simulator_mavlink.cpp | 35 ++++++++++++++-------
 2 files changed, 33 insertions(+), 16 deletions(-)

diff --git a/src/modules/simulator/simulator.h b/src/modules/simulator/simulator.h
index 6111af7c88..8acb377db8 100644
--- a/src/modules/simulator/simulator.h
+++ b/src/modules/simulator/simulator.h
@@ -243,7 +243,7 @@ private:
 #ifndef __PX4_QURT
 		,
 		_rc_channels_pub(nullptr),
-		_actuator_outputs_sub(-1),
+		_actuator_outputs_sub{},
 		_vehicle_attitude_sub(-1),
 		_manual_sub(-1),
 		_vehicle_status_sub(-1),
@@ -253,7 +253,11 @@ private:
 		_manual{},
 		_vehicle_status{}
 #endif
-	{}
+	{
+		for (unsigned i = 0; i < (sizeof(_actuator_outputs_sub) / sizeof(_actuator_outputs_sub[0])); i++) {
+			_actuator_outputs_sub[i] = -1;
+		}
+	}
 	~Simulator() { _instance = NULL; }
 
 	void initializeSensorData();
@@ -299,14 +303,14 @@ private:
 	orb_advert_t _rc_channels_pub;
 
 	// uORB subscription handlers
-	int _actuator_outputs_sub;
+	int _actuator_outputs_sub[ORB_MULTI_MAX_INSTANCES];
 	int _vehicle_attitude_sub;
 	int _manual_sub;
 	int _vehicle_status_sub;
 
 	// uORB data containers
 	struct rc_input_values _rc_input;
-	struct actuator_outputs_s _actuators;
+	struct actuator_outputs_s _actuators[ORB_MULTI_MAX_INSTANCES];
 	struct vehicle_attitude_s _attitude;
 	struct manual_control_setpoint_s _manual;
 	struct vehicle_status_s _vehicle_status;
@@ -316,7 +320,7 @@ private:
 	void send_controls();
 	void pollForMAVLinkMessages(bool publish, int udp_port);
 
-	void pack_actuator_message(mavlink_hil_controls_t &actuator_msg);
+	void pack_actuator_message(mavlink_hil_controls_t &actuator_msg, unsigned index);
 	void send_mavlink_message(const uint8_t msgid, const void *msg, uint8_t component_ID);
 	void update_sensors(mavlink_hil_sensor_t *imu);
 	void update_gps(mavlink_hil_gps_t *gps_sim);
diff --git a/src/modules/simulator/simulator_mavlink.cpp b/src/modules/simulator/simulator_mavlink.cpp
index 2c3fe4f155..2bd62d8a62 100644
--- a/src/modules/simulator/simulator_mavlink.cpp
+++ b/src/modules/simulator/simulator_mavlink.cpp
@@ -77,7 +77,7 @@ const unsigned mode_flag_custom = 1;
 
 using namespace simulator;
 
-void Simulator::pack_actuator_message(mavlink_hil_controls_t &actuator_msg)
+void Simulator::pack_actuator_message(mavlink_hil_controls_t &actuator_msg, unsigned index)
 {
 	// reset state
 	memset(&actuator_msg, 0, sizeof(actuator_msg));
@@ -90,7 +90,7 @@ void Simulator::pack_actuator_message(mavlink_hil_controls_t &actuator_msg)
 
 	for (unsigned i = 0; i < (sizeof(out) / sizeof(out[0])); i++) {
 		// scale PWM out 900..2100 us to -1..1 */
-		out[i] = (_actuators.output[i] - pwm_center) / ((PWM_DEFAULT_MAX - PWM_DEFAULT_MIN) / 2);
+		out[i] = (_actuators[index].output[i] - pwm_center) / ((PWM_DEFAULT_MAX - PWM_DEFAULT_MIN) / 2);
 
 		if (!PX4_ISFINITE(out[i])) {
 			out[i] = -1.0f;
@@ -107,14 +107,21 @@ void Simulator::pack_actuator_message(mavlink_hil_controls_t &actuator_msg)
 	actuator_msg.aux4 = out[7];
 	actuator_msg.mode = mode_flag_custom;
 	actuator_msg.mode |= (armed) ? mode_flag_armed : 0;
-	actuator_msg.nav_mode = 0;
+	actuator_msg.nav_mode = index; // XXX this indicates the output group in our use of the message
 }
 
 void Simulator::send_controls()
 {
-	mavlink_hil_controls_t msg;
-	pack_actuator_message(msg);
-	send_mavlink_message(MAVLINK_MSG_ID_HIL_CONTROLS, &msg, 200);
+	for (unsigned i = 0; i < (sizeof(_actuator_outputs_sub) / sizeof(_actuator_outputs_sub[0])); i++) {
+
+		if (_actuator_outputs_sub[i] < 0 || _actuators[i].timestamp == 0) {
+			continue;
+		}
+
+		mavlink_hil_controls_t msg;
+		pack_actuator_message(msg, i);
+		send_mavlink_message(MAVLINK_MSG_ID_HIL_CONTROLS, &msg, 200);
+	}
 }
 
 static void fill_rc_input_msg(struct rc_input_values *rc, mavlink_rc_channels_t *rc_channels)
@@ -341,10 +348,14 @@ void Simulator::poll_topics()
 {
 	// copy new actuator data if available
 	bool updated;
-	orb_check(_actuator_outputs_sub, &updated);
 
-	if (updated) {
-		orb_copy(ORB_ID(actuator_outputs), _actuator_outputs_sub, &_actuators);
+	for (unsigned i = 0; i < (sizeof(_actuator_outputs_sub) / sizeof(_actuator_outputs_sub[0])); i++) {
+
+		orb_check(_actuator_outputs_sub[i], &updated);
+
+		if (updated) {
+			orb_copy(ORB_ID(actuator_outputs), _actuator_outputs_sub[i], &_actuators[i]);
+		}
 	}
 
 	orb_check(_vehicle_status_sub, &updated);
@@ -363,7 +374,7 @@ void *Simulator::sending_trampoline(void *)
 void Simulator::send()
 {
 	px4_pollfd_struct_t fds[1] = {};
-	fds[0].fd = _actuator_outputs_sub;
+	fds[0].fd = _actuator_outputs_sub[0];
 	fds[0].events = POLLIN;
 
 
@@ -554,7 +565,9 @@ void Simulator::pollForMAVLinkMessages(bool publish, int udp_port)
 	(void)hrt_reset();
 
 	// subscribe to topics
-	_actuator_outputs_sub = orb_subscribe_multi(ORB_ID(actuator_outputs), 0);
+	for (unsigned i = 0; i < (sizeof(_actuator_outputs_sub) / sizeof(_actuator_outputs_sub[0])); i++) {
+		_actuator_outputs_sub[i] = orb_subscribe_multi(ORB_ID(actuator_outputs), i);
+	}
 	_vehicle_status_sub = orb_subscribe(ORB_ID(vehicle_status));
 
 	// got data from simulator, now activate the sending thread

From 51df9771b3795544d73256edda5ecb138595846d Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Sat, 18 Jun 2016 01:54:31 +0200
Subject: [PATCH 0666/1230] Add support for more than 8 outputs, but do not
 enable it yet

---
 Tools/sitl_gazebo | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/Tools/sitl_gazebo b/Tools/sitl_gazebo
index 80139a2b75..f43eb236fd 160000
--- a/Tools/sitl_gazebo
+++ b/Tools/sitl_gazebo
@@ -1 +1 @@
-Subproject commit 80139a2b75b789f51cab3d6cbad6d916a8c6d7f1
+Subproject commit f43eb236fd4e042b423b4c9ad5bbf7549df99c99

From 5d18ea75022cb61709c756710de59c87b1604ff2 Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Sat, 18 Jun 2016 10:40:34 +0200
Subject: [PATCH 0667/1230] Update models

---
 Tools/sitl_gazebo | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/Tools/sitl_gazebo b/Tools/sitl_gazebo
index f43eb236fd..15acf497c9 160000
--- a/Tools/sitl_gazebo
+++ b/Tools/sitl_gazebo
@@ -1 +1 @@
-Subproject commit f43eb236fd4e042b423b4c9ad5bbf7549df99c99
+Subproject commit 15acf497c92242d027f2e6c70bd6e5edb7989b20

From fd768fe5ac080a2392f16aab9c6c2bee397ce3f7 Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Sat, 18 Jun 2016 10:41:35 +0200
Subject: [PATCH 0668/1230] Update models

---
 Tools/sitl_gazebo | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/Tools/sitl_gazebo b/Tools/sitl_gazebo
index 15acf497c9..1a294d7e15 160000
--- a/Tools/sitl_gazebo
+++ b/Tools/sitl_gazebo
@@ -1 +1 @@
-Subproject commit 15acf497c92242d027f2e6c70bd6e5edb7989b20
+Subproject commit 1a294d7e1569aa68d95e6a77cedbf1decaeb31db

From 7b109bacf2e4b80856a542c666dbe87943a5fef1 Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Sat, 18 Jun 2016 12:08:45 +0200
Subject: [PATCH 0669/1230] Update Gazebo models

---
 Tools/sitl_gazebo | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/Tools/sitl_gazebo b/Tools/sitl_gazebo
index 1a294d7e15..81e62925ad 160000
--- a/Tools/sitl_gazebo
+++ b/Tools/sitl_gazebo
@@ -1 +1 @@
-Subproject commit 1a294d7e1569aa68d95e6a77cedbf1decaeb31db
+Subproject commit 81e62925ad86640a5b12fe6a66d6aaadd611f124

From eabb50444591943baf617ffc9c9838d1a3811c92 Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Sat, 18 Jun 2016 13:16:05 +0200
Subject: [PATCH 0670/1230] Compress Iris Gazebo model

---
 Tools/sitl_gazebo | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/Tools/sitl_gazebo b/Tools/sitl_gazebo
index 81e62925ad..0956c05f4d 160000
--- a/Tools/sitl_gazebo
+++ b/Tools/sitl_gazebo
@@ -1 +1 @@
-Subproject commit 81e62925ad86640a5b12fe6a66d6aaadd611f124
+Subproject commit 0956c05f4d97ac65ef4bbbd61989241d47c4217e

From 2242331f08063e46ca112b0e3bbe5a7e8b069ff7 Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Sat, 18 Jun 2016 15:34:37 +0200
Subject: [PATCH 0671/1230] Update SITL Gazebo

---
 Tools/sitl_gazebo | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/Tools/sitl_gazebo b/Tools/sitl_gazebo
index 0956c05f4d..def1d4791a 160000
--- a/Tools/sitl_gazebo
+++ b/Tools/sitl_gazebo
@@ -1 +1 @@
-Subproject commit 0956c05f4d97ac65ef4bbbd61989241d47c4217e
+Subproject commit def1d4791a9e73fbc556b7cf936c684c95ac3f58

From afdc8cdf550078a029002bf30f61240a36ea575c Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Sat, 18 Jun 2016 16:29:28 +0200
Subject: [PATCH 0672/1230] VTOL: Use standard attitude gains

---
 posix-configs/SITL/init/rcS_gazebo_standard_vtol | 10 ++++++----
 1 file changed, 6 insertions(+), 4 deletions(-)

diff --git a/posix-configs/SITL/init/rcS_gazebo_standard_vtol b/posix-configs/SITL/init/rcS_gazebo_standard_vtol
index 3487aedf0f..908ab2209a 100644
--- a/posix-configs/SITL/init/rcS_gazebo_standard_vtol
+++ b/posix-configs/SITL/init/rcS_gazebo_standard_vtol
@@ -1,10 +1,6 @@
 uorb start
 param load
 param set MAV_TYPE 20
-param set MC_PITCHRATE_P 0.3
-param set MC_ROLLRATE_P 0.3
-param set MC_YAW_P 2.0
-param set MC_YAWRATE_P 0.35
 param set VT_TYPE 2
 param set SYS_AUTOSTART 4010
 param set SYS_RESTART_TYPE 2
@@ -22,6 +18,12 @@ param set CAL_ACC0_YSCALE 1.01
 param set CAL_ACC0_ZSCALE 1.01
 param set CAL_ACC1_XOFF 0.01
 param set CAL_MAG0_XOFF 0.01
+param set MC_ROLLRATE_P 0.25
+param set MC_PITCHRATE_P 0.25
+param set MC_PITCH_P 5
+param set MC_ROLL_P 5
+param set MC_ROLLRATE_I 0.1
+param set MC_PITCHRATE_I 0.1
 param set MPC_XY_P 0.15
 param set MPC_XY_VEL_P 0.05
 param set MPC_XY_VEL_D 0.005

From 03813cc46b7622e2d7958034cb82a5c3a90eb607 Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Sat, 18 Jun 2016 16:29:39 +0200
Subject: [PATCH 0673/1230] Update fixed VTOL model

---
 Tools/sitl_gazebo | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/Tools/sitl_gazebo b/Tools/sitl_gazebo
index def1d4791a..15bd1bd855 160000
--- a/Tools/sitl_gazebo
+++ b/Tools/sitl_gazebo
@@ -1 +1 @@
-Subproject commit def1d4791a9e73fbc556b7cf936c684c95ac3f58
+Subproject commit 15bd1bd8556b773166a2759694bf364a46aeab40

From 3c9f5694e70ce2559307549a0385dfcc8f201034 Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Sat, 18 Jun 2016 16:38:49 +0200
Subject: [PATCH 0674/1230] Fix Gazebo models for VTOL and planes

---
 Tools/sitl_gazebo | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/Tools/sitl_gazebo b/Tools/sitl_gazebo
index 15bd1bd855..f8f4c7f8ed 160000
--- a/Tools/sitl_gazebo
+++ b/Tools/sitl_gazebo
@@ -1 +1 @@
-Subproject commit 15bd1bd8556b773166a2759694bf364a46aeab40
+Subproject commit f8f4c7f8ed0a2cb457e544fcd16efa6d4d9e8fcb

From 3ef6ee056f5fbae679417897e9da3d32c17a48b7 Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Sat, 18 Jun 2016 16:58:04 +0200
Subject: [PATCH 0675/1230] Sync rate control gains

---
 posix-configs/SITL/init/rcS_gazebo_iris         | 8 ++++----
 posix-configs/SITL/init/rcS_gazebo_typhoon_h480 | 8 ++++----
 2 files changed, 8 insertions(+), 8 deletions(-)

diff --git a/posix-configs/SITL/init/rcS_gazebo_iris b/posix-configs/SITL/init/rcS_gazebo_iris
index e4d8c958a9..72d205447d 100644
--- a/posix-configs/SITL/init/rcS_gazebo_iris
+++ b/posix-configs/SITL/init/rcS_gazebo_iris
@@ -27,10 +27,10 @@ param set RTL_RETURN_ALT 30.0
 param set RTL_DESCEND_ALT 10.0
 param set RTL_LAND_DELAY 0
 param set MIS_TAKEOFF_ALT 2.5
-param set MC_ROLLRATE_P 0.5
-param set MC_PITCHRATE_P 0.5
-param set MC_PITCH_P 5
-param set MC_ROLL_P 5
+param set MC_ROLLRATE_P 0.3
+param set MC_PITCHRATE_P 0.3
+param set MC_PITCH_P 5.5
+param set MC_ROLL_P 5.5
 param set MC_ROLLRATE_I 0.1
 param set MC_PITCHRATE_I 0.1
 param set MPC_HOLD_MAX_Z 2.0
diff --git a/posix-configs/SITL/init/rcS_gazebo_typhoon_h480 b/posix-configs/SITL/init/rcS_gazebo_typhoon_h480
index 6a47ce7ff5..3c40019721 100644
--- a/posix-configs/SITL/init/rcS_gazebo_typhoon_h480
+++ b/posix-configs/SITL/init/rcS_gazebo_typhoon_h480
@@ -27,10 +27,10 @@ param set RTL_RETURN_ALT 30.0
 param set RTL_DESCEND_ALT 10.0
 param set RTL_LAND_DELAY 0
 param set MIS_TAKEOFF_ALT 2.5
-param set MC_ROLLRATE_P 0.25
-param set MC_PITCHRATE_P 0.25
-param set MC_PITCH_P 5
-param set MC_ROLL_P 5
+param set MC_ROLLRATE_P 0.3
+param set MC_PITCHRATE_P 0.3
+param set MC_PITCH_P 5.5
+param set MC_ROLL_P 5.5
 param set MC_ROLLRATE_I 0.1
 param set MC_PITCHRATE_I 0.1
 param set MPC_HOLD_MAX_Z 2.0

From e7d02f627261d3d446db5f6589721a6c157c5e39 Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Sat, 18 Jun 2016 16:59:46 +0200
Subject: [PATCH 0676/1230] Update VTOL gains

---
 Tools/sitl_gazebo | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/Tools/sitl_gazebo b/Tools/sitl_gazebo
index f8f4c7f8ed..dd8fbb671c 160000
--- a/Tools/sitl_gazebo
+++ b/Tools/sitl_gazebo
@@ -1 +1 @@
-Subproject commit f8f4c7f8ed0a2cb457e544fcd16efa6d4d9e8fcb
+Subproject commit dd8fbb671c34ed799d3da6f29fff31563ac25658

From 229208610efb2163eebef64b38992b15ebb11b40 Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Sat, 18 Jun 2016 17:03:11 +0200
Subject: [PATCH 0677/1230] Fix simulator code style

---
 src/modules/simulator/simulator.h           | 3 ++-
 src/modules/simulator/simulator_mavlink.cpp | 1 +
 2 files changed, 3 insertions(+), 1 deletion(-)

diff --git a/src/modules/simulator/simulator.h b/src/modules/simulator/simulator.h
index 8acb377db8..104101060f 100644
--- a/src/modules/simulator/simulator.h
+++ b/src/modules/simulator/simulator.h
@@ -254,7 +254,8 @@ private:
 		_vehicle_status{}
 #endif
 	{
-		for (unsigned i = 0; i < (sizeof(_actuator_outputs_sub) / sizeof(_actuator_outputs_sub[0])); i++) {
+		for (unsigned i = 0; i < (sizeof(_actuator_outputs_sub) / sizeof(_actuator_outputs_sub[0])); i++)
+		{
 			_actuator_outputs_sub[i] = -1;
 		}
 	}
diff --git a/src/modules/simulator/simulator_mavlink.cpp b/src/modules/simulator/simulator_mavlink.cpp
index 2bd62d8a62..d3b6e53761 100644
--- a/src/modules/simulator/simulator_mavlink.cpp
+++ b/src/modules/simulator/simulator_mavlink.cpp
@@ -568,6 +568,7 @@ void Simulator::pollForMAVLinkMessages(bool publish, int udp_port)
 	for (unsigned i = 0; i < (sizeof(_actuator_outputs_sub) / sizeof(_actuator_outputs_sub[0])); i++) {
 		_actuator_outputs_sub[i] = orb_subscribe_multi(ORB_ID(actuator_outputs), i);
 	}
+
 	_vehicle_status_sub = orb_subscribe(ORB_ID(vehicle_status));
 
 	// got data from simulator, now activate the sending thread

From d860bdcdc08ac29c708bcd8a3de086f41f0fe45e Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Sat, 18 Jun 2016 17:03:49 +0200
Subject: [PATCH 0678/1230] Update VTOL gains

---
 posix-configs/SITL/init/rcS_gazebo_standard_vtol | 8 ++++----
 1 file changed, 4 insertions(+), 4 deletions(-)

diff --git a/posix-configs/SITL/init/rcS_gazebo_standard_vtol b/posix-configs/SITL/init/rcS_gazebo_standard_vtol
index 908ab2209a..6e7861b896 100644
--- a/posix-configs/SITL/init/rcS_gazebo_standard_vtol
+++ b/posix-configs/SITL/init/rcS_gazebo_standard_vtol
@@ -18,10 +18,10 @@ param set CAL_ACC0_YSCALE 1.01
 param set CAL_ACC0_ZSCALE 1.01
 param set CAL_ACC1_XOFF 0.01
 param set CAL_MAG0_XOFF 0.01
-param set MC_ROLLRATE_P 0.25
-param set MC_PITCHRATE_P 0.25
-param set MC_PITCH_P 5
-param set MC_ROLL_P 5
+param set MC_ROLLRATE_P 0.3
+param set MC_PITCHRATE_P 0.3
+param set MC_PITCH_P 5.5
+param set MC_ROLL_P 5.5
 param set MC_ROLLRATE_I 0.1
 param set MC_PITCHRATE_I 0.1
 param set MPC_XY_P 0.15

From c647b6db4dd80f4a8a75809284000ded025b68b6 Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Sat, 18 Jun 2016 17:36:48 +0200
Subject: [PATCH 0679/1230] Update gains

---
 posix-configs/SITL/init/rcS_gazebo_iris              |  4 ++--
 posix-configs/SITL/init/rcS_gazebo_iris_opt_flow     |  8 ++++----
 posix-configs/SITL/init/rcS_gazebo_solo              |  8 ++++----
 posix-configs/SITL/init/rcS_gazebo_tailsitter        | 10 ++++++----
 posix-configs/SITL/init/rcS_jmavsim_iris             | 12 ++++++------
 posix-configs/SITL/init/rcS_lpe_gazebo_iris          | 10 ++++++----
 posix-configs/SITL/init/rcS_lpe_gazebo_iris_opt_flow | 10 ++++++----
 posix-configs/SITL/init/rcS_lpe_jmavsim_iris         | 12 ++++++------
 posix-configs/SITL/init/rcS_multiple_gazebo_iris     | 10 ++++++----
 9 files changed, 46 insertions(+), 38 deletions(-)

diff --git a/posix-configs/SITL/init/rcS_gazebo_iris b/posix-configs/SITL/init/rcS_gazebo_iris
index 72d205447d..5978c609de 100644
--- a/posix-configs/SITL/init/rcS_gazebo_iris
+++ b/posix-configs/SITL/init/rcS_gazebo_iris
@@ -29,8 +29,8 @@ param set RTL_LAND_DELAY 0
 param set MIS_TAKEOFF_ALT 2.5
 param set MC_ROLLRATE_P 0.3
 param set MC_PITCHRATE_P 0.3
-param set MC_PITCH_P 5.5
-param set MC_ROLL_P 5.5
+param set MC_PITCH_P 7
+param set MC_ROLL_P 7
 param set MC_ROLLRATE_I 0.1
 param set MC_PITCHRATE_I 0.1
 param set MPC_HOLD_MAX_Z 2.0
diff --git a/posix-configs/SITL/init/rcS_gazebo_iris_opt_flow b/posix-configs/SITL/init/rcS_gazebo_iris_opt_flow
index 59fa38daf3..7aea1f3f40 100644
--- a/posix-configs/SITL/init/rcS_gazebo_iris_opt_flow
+++ b/posix-configs/SITL/init/rcS_gazebo_iris_opt_flow
@@ -27,10 +27,10 @@ param set RTL_RETURN_ALT 30.0
 param set RTL_DESCEND_ALT 10.0
 param set RTL_LAND_DELAY 0
 param set MIS_TAKEOFF_ALT 2.5
-param set MC_ROLLRATE_P 0.5
-param set MC_PITCHRATE_P 0.5
-param set MC_PITCH_P 5
-param set MC_ROLL_P 5
+param set MC_ROLLRATE_P 0.3
+param set MC_PITCHRATE_P 0.3
+param set MC_PITCH_P 5.5
+param set MC_ROLL_P 5.5
 param set MC_ROLLRATE_I 0.1
 param set MC_PITCHRATE_I 0.1
 param set MPC_HOLD_MAX_Z 2.0
diff --git a/posix-configs/SITL/init/rcS_gazebo_solo b/posix-configs/SITL/init/rcS_gazebo_solo
index 198bb2815d..ded76b8505 100644
--- a/posix-configs/SITL/init/rcS_gazebo_solo
+++ b/posix-configs/SITL/init/rcS_gazebo_solo
@@ -27,10 +27,10 @@ param set RTL_RETURN_ALT 30.0
 param set RTL_DESCEND_ALT 10.0
 param set RTL_LAND_DELAY 0
 param set MIS_TAKEOFF_ALT 2.5
-param set MC_ROLLRATE_P 0.25
-param set MC_PITCHRATE_P 0.25
-param set MC_PITCH_P 5
-param set MC_ROLL_P 5
+param set MC_ROLLRATE_P 0.3
+param set MC_PITCHRATE_P 0.3
+param set MC_PITCH_P 7
+param set MC_ROLL_P 7
 param set MC_ROLLRATE_I 0.1
 param set MC_PITCHRATE_I 0.1
 param set MPC_HOLD_MAX_Z 2.0
diff --git a/posix-configs/SITL/init/rcS_gazebo_tailsitter b/posix-configs/SITL/init/rcS_gazebo_tailsitter
index ebaf0bc136..5d3d44e633 100644
--- a/posix-configs/SITL/init/rcS_gazebo_tailsitter
+++ b/posix-configs/SITL/init/rcS_gazebo_tailsitter
@@ -1,10 +1,6 @@
 uorb start
 param load
 param set MAV_TYPE 20
-param set MC_PITCHRATE_P 0.25
-param set MC_ROLLRATE_P 0.25
-param set MC_PITCH_P 5
-param set MC_ROLL_P 5
 param set VT_TYPE 0
 param set SYS_AUTOSTART 4010
 param set SYS_RESTART_TYPE 2
@@ -22,6 +18,12 @@ param set CAL_ACC0_YSCALE 1.01
 param set CAL_ACC0_ZSCALE 1.01
 param set CAL_ACC1_XOFF 0.01
 param set CAL_MAG0_XOFF 0.01
+param set MC_ROLLRATE_P 0.3
+param set MC_PITCHRATE_P 0.3
+param set MC_PITCH_P 5.5
+param set MC_ROLL_P 5.5
+param set MC_ROLLRATE_I 0.1
+param set MC_PITCHRATE_I 0.1
 param set MPC_XY_P 0.15
 param set MPC_XY_VEL_P 0.05
 param set MPC_XY_VEL_D 0.005
diff --git a/posix-configs/SITL/init/rcS_jmavsim_iris b/posix-configs/SITL/init/rcS_jmavsim_iris
index 99501dd819..dbcc533a81 100644
--- a/posix-configs/SITL/init/rcS_jmavsim_iris
+++ b/posix-configs/SITL/init/rcS_jmavsim_iris
@@ -1,12 +1,6 @@
 uorb start
 param load
 param set MAV_TYPE 2
-param set MC_PITCHRATE_P 0.15
-param set MC_PITCH_P 7
-param set MC_ROLL_P 7
-param set MC_ROLLRATE_P 0.15
-param set MC_YAW_P 2.8
-param set MC_YAWRATE_P 0.35
 param set SYS_AUTOSTART 4010
 param set SYS_RESTART_TYPE 2
 dataman start
@@ -36,6 +30,12 @@ param set RTL_RETURN_ALT 30.0
 param set RTL_DESCEND_ALT 10.0
 param set RTL_LAND_DELAY 0
 param set MIS_TAKEOFF_ALT 2.5
+param set MC_ROLLRATE_P 0.3
+param set MC_PITCHRATE_P 0.3
+param set MC_PITCH_P 7
+param set MC_ROLL_P 7
+param set MC_ROLLRATE_I 0.1
+param set MC_PITCHRATE_I 0.1
 param set MPC_HOLD_MAX_Z 2.0
 param set MPC_HOLD_XY_DZ 0.1
 param set MPC_Z_VEL_MAX 2.0
diff --git a/posix-configs/SITL/init/rcS_lpe_gazebo_iris b/posix-configs/SITL/init/rcS_lpe_gazebo_iris
index 9ce02acf76..514ad7668a 100644
--- a/posix-configs/SITL/init/rcS_lpe_gazebo_iris
+++ b/posix-configs/SITL/init/rcS_lpe_gazebo_iris
@@ -1,10 +1,6 @@
 uorb start
 param load
 param set MAV_TYPE 2
-param set MC_PITCHRATE_P 0.3
-param set MC_ROLLRATE_P 0.3
-param set MC_YAW_P 2.0
-param set MC_YAWRATE_P 0.35
 param set SYS_AUTOSTART 4010
 param set SYS_RESTART_TYPE 2
 dataman start
@@ -21,6 +17,12 @@ param set CAL_ACC0_YSCALE 1.01
 param set CAL_ACC0_ZSCALE 1.01
 param set CAL_ACC1_XOFF 0.01
 param set CAL_MAG0_XOFF 0.01
+param set MC_ROLLRATE_P 0.3
+param set MC_PITCHRATE_P 0.3
+param set MC_PITCH_P 5.5
+param set MC_ROLL_P 5.5
+param set MC_ROLLRATE_I 0.1
+param set MC_PITCHRATE_I 0.1
 param set MPC_XY_P 0.15
 param set MPC_XY_VEL_P 0.05
 param set MPC_XY_VEL_D 0.005
diff --git a/posix-configs/SITL/init/rcS_lpe_gazebo_iris_opt_flow b/posix-configs/SITL/init/rcS_lpe_gazebo_iris_opt_flow
index bd93519031..085dc77a7b 100644
--- a/posix-configs/SITL/init/rcS_lpe_gazebo_iris_opt_flow
+++ b/posix-configs/SITL/init/rcS_lpe_gazebo_iris_opt_flow
@@ -1,10 +1,6 @@
 uorb start
 param load
 param set MAV_TYPE 2
-param set MC_PITCHRATE_P 0.3
-param set MC_ROLLRATE_P 0.3
-param set MC_YAW_P 2.0
-param set MC_YAWRATE_P 0.35
 param set SYS_AUTOSTART 4010
 param set SYS_RESTART_TYPE 2
 dataman start
@@ -21,6 +17,12 @@ param set CAL_ACC0_YSCALE 1.01
 param set CAL_ACC0_ZSCALE 1.01
 param set CAL_ACC1_XOFF 0.01
 param set CAL_MAG0_XOFF 0.01
+param set MC_ROLLRATE_P 0.3
+param set MC_PITCHRATE_P 0.3
+param set MC_PITCH_P 5.5
+param set MC_ROLL_P 5.5
+param set MC_ROLLRATE_I 0.1
+param set MC_PITCHRATE_I 0.1
 param set MPC_XY_P 0.15
 param set MPC_XY_VEL_P 0.05
 param set MPC_XY_VEL_D 0.005
diff --git a/posix-configs/SITL/init/rcS_lpe_jmavsim_iris b/posix-configs/SITL/init/rcS_lpe_jmavsim_iris
index 76685313ed..4de8c96030 100644
--- a/posix-configs/SITL/init/rcS_lpe_jmavsim_iris
+++ b/posix-configs/SITL/init/rcS_lpe_jmavsim_iris
@@ -1,12 +1,6 @@
 uorb start
 param load
 param set MAV_TYPE 2
-param set MC_PITCHRATE_P 0.15
-param set MC_PITCH_P 7
-param set MC_ROLL_P 7
-param set MC_ROLLRATE_P 0.15
-param set MC_YAW_P 2.8
-param set MC_YAWRATE_P 0.35
 param set SYS_AUTOSTART 4010
 param set SYS_RESTART_TYPE 2
 dataman start
@@ -23,6 +17,12 @@ param set CAL_ACC0_YSCALE 1.01
 param set CAL_ACC0_ZSCALE 1.01
 param set CAL_ACC1_XOFF 0.01
 param set CAL_MAG0_XOFF 0.01
+param set MC_ROLLRATE_P 0.3
+param set MC_PITCHRATE_P 0.3
+param set MC_PITCH_P 5.5
+param set MC_ROLL_P 5.5
+param set MC_ROLLRATE_I 0.1
+param set MC_PITCHRATE_I 0.1
 param set MPC_XY_P 0.4
 param set MPC_XY_VEL_P 0.2
 param set MPC_XY_VEL_D 0.005
diff --git a/posix-configs/SITL/init/rcS_multiple_gazebo_iris b/posix-configs/SITL/init/rcS_multiple_gazebo_iris
index 674ff3f312..a791aa8174 100644
--- a/posix-configs/SITL/init/rcS_multiple_gazebo_iris
+++ b/posix-configs/SITL/init/rcS_multiple_gazebo_iris
@@ -2,10 +2,6 @@ uorb start
 simulator start -s -u _SIMPORT_
 param load
 param set MAV_TYPE 2
-param set MC_PITCHRATE_P 0.3
-param set MC_ROLLRATE_P 0.3
-param set MC_YAW_P 2.0
-param set MC_YAWRATE_P 0.35
 param set SYS_AUTOSTART 4010
 param set SYS_RESTART_TYPE 2
 dataman start
@@ -22,6 +18,12 @@ param set CAL_ACC0_YSCALE 1.01
 param set CAL_ACC0_ZSCALE 1.01
 param set CAL_ACC1_XOFF 0.01
 param set CAL_MAG0_XOFF 0.01
+param set MC_ROLLRATE_P 0.3
+param set MC_PITCHRATE_P 0.3
+param set MC_PITCH_P 5.5
+param set MC_ROLL_P 5.5
+param set MC_ROLLRATE_I 0.1
+param set MC_PITCHRATE_I 0.1
 param set MPC_XY_P 0.15
 param set MPC_XY_VEL_P 0.05
 param set MPC_XY_VEL_D 0.005

From 95430180f0f1d79e9678f6310c92e8a10039ae02 Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Sat, 18 Jun 2016 17:37:24 +0200
Subject: [PATCH 0680/1230] Update gazebo

---
 Tools/sitl_gazebo | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/Tools/sitl_gazebo b/Tools/sitl_gazebo
index dd8fbb671c..4c3933fa5c 160000
--- a/Tools/sitl_gazebo
+++ b/Tools/sitl_gazebo
@@ -1 +1 @@
-Subproject commit dd8fbb671c34ed799d3da6f29fff31563ac25658
+Subproject commit 4c3933fa5cf8a966e841849f9d6a006f4f8e5162

From a812224103325e91e44eca7d802e4d9a0de2cc81 Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Sat, 18 Jun 2016 18:18:00 +0200
Subject: [PATCH 0681/1230] Update standard VTOL

---
 Tools/sitl_gazebo | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/Tools/sitl_gazebo b/Tools/sitl_gazebo
index 4c3933fa5c..e7829617e6 160000
--- a/Tools/sitl_gazebo
+++ b/Tools/sitl_gazebo
@@ -1 +1 @@
-Subproject commit 4c3933fa5cf8a966e841849f9d6a006f4f8e5162
+Subproject commit e7829617e6e89412035b3e7ad2287ab397aa657c

From 97f6ad4e533d59aec61413375b7c7638f46f30de Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Sat, 18 Jun 2016 19:28:48 +0200
Subject: [PATCH 0682/1230] Even better Gazebo models

---
 Tools/sitl_gazebo | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/Tools/sitl_gazebo b/Tools/sitl_gazebo
index e7829617e6..8b5465b2af 160000
--- a/Tools/sitl_gazebo
+++ b/Tools/sitl_gazebo
@@ -1 +1 @@
-Subproject commit e7829617e6e89412035b3e7ad2287ab397aa657c
+Subproject commit 8b5465b2af52eeee121426f287daf4070fe63bcb

From 146c8ddbc38d041f2751f414c692285737193415 Mon Sep 17 00:00:00 2001
From: Julian Oes <julian@oes.ch>
Date: Mon, 20 Jun 2016 09:37:43 +0200
Subject: [PATCH 0683/1230] px4_getopt: astyle

---
 src/platforms/common/px4_getopt.c | 2 ++
 1 file changed, 2 insertions(+)

diff --git a/src/platforms/common/px4_getopt.c b/src/platforms/common/px4_getopt.c
index 6b0427b936..d446a1fadc 100644
--- a/src/platforms/common/px4_getopt.c
+++ b/src/platforms/common/px4_getopt.c
@@ -168,9 +168,11 @@ __EXPORT int px4_getopt(int argc, char *argv[], const char *options, int *myopti
 
 		if (takesarg) {
 			*myoptarg = argv[*myoptind];
+
 			if (!*myoptarg) { //Error: option takes an argument, but there is no more argument
 				return -1;
 			}
+
 			*myoptind += 1;
 		}
 

From b2cfe05881c697ae96c2d6c643a6585c1b919531 Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Mon, 20 Jun 2016 12:54:22 +0200
Subject: [PATCH 0684/1230] Adjust Typhoon H480 gains

---
 posix-configs/SITL/init/rcS_gazebo_typhoon_h480 | 5 +++--
 1 file changed, 3 insertions(+), 2 deletions(-)

diff --git a/posix-configs/SITL/init/rcS_gazebo_typhoon_h480 b/posix-configs/SITL/init/rcS_gazebo_typhoon_h480
index 3c40019721..33ca3f67b7 100644
--- a/posix-configs/SITL/init/rcS_gazebo_typhoon_h480
+++ b/posix-configs/SITL/init/rcS_gazebo_typhoon_h480
@@ -29,10 +29,11 @@ param set RTL_LAND_DELAY 0
 param set MIS_TAKEOFF_ALT 2.5
 param set MC_ROLLRATE_P 0.3
 param set MC_PITCHRATE_P 0.3
-param set MC_PITCH_P 5.5
-param set MC_ROLL_P 5.5
+param set MC_PITCH_P 7
+param set MC_ROLL_P 7
 param set MC_ROLLRATE_I 0.1
 param set MC_PITCHRATE_I 0.1
+param set MC_YAWRATE_P 0.08
 param set MPC_HOLD_MAX_Z 2.0
 param set MPC_HOLD_XY_DZ 0.1
 param set MPC_Z_VEL_P 0.8

From 9933494d535ba07e83f1105f7ec6230bfb13ada5 Mon Sep 17 00:00:00 2001
From: Michael Schaeuble <schaeuble.michael@gmail.com>
Date: Thu, 26 May 2016 10:28:36 +0200
Subject: [PATCH 0685/1230] Add parrot bebop build structure

---
 cmake/configs/posix_bebop_default.cmake | 5 +++++
 src/lib/DriverFramework                 | 2 +-
 2 files changed, 6 insertions(+), 1 deletion(-)

diff --git a/cmake/configs/posix_bebop_default.cmake b/cmake/configs/posix_bebop_default.cmake
index fdb1d3eb8c..7689f6d379 100644
--- a/cmake/configs/posix_bebop_default.cmake
+++ b/cmake/configs/posix_bebop_default.cmake
@@ -93,4 +93,9 @@ set(config_module_list
 	platforms/common
 	platforms/posix/px4_layer
 	platforms/posix/work_queue
+	
+  # 
+  # libraries
+  #
+	lib/DriverFramework/framework
 )
diff --git a/src/lib/DriverFramework b/src/lib/DriverFramework
index e2865fb56c..36335412b2 160000
--- a/src/lib/DriverFramework
+++ b/src/lib/DriverFramework
@@ -1 +1 @@
-Subproject commit e2865fb56c4f8e061b75fc3f7b306ab549458f6e
+Subproject commit 36335412b2c461de08696d224b8ae1149fa5ae28

From 696a378120666588b719580155cc03d6402fa033 Mon Sep 17 00:00:00 2001
From: Michael Schaeuble <schaeuble.michael@gmail.com>
Date: Thu, 26 May 2016 15:42:35 +0200
Subject: [PATCH 0686/1230] Add modules and commands to bebop build

---
 cmake/configs/posix_bebop_default.cmake | 5 -----
 posix-configs/bebop/mainapp.config      | 1 -
 2 files changed, 6 deletions(-)

diff --git a/cmake/configs/posix_bebop_default.cmake b/cmake/configs/posix_bebop_default.cmake
index 7689f6d379..fdb1d3eb8c 100644
--- a/cmake/configs/posix_bebop_default.cmake
+++ b/cmake/configs/posix_bebop_default.cmake
@@ -93,9 +93,4 @@ set(config_module_list
 	platforms/common
 	platforms/posix/px4_layer
 	platforms/posix/work_queue
-	
-  # 
-  # libraries
-  #
-	lib/DriverFramework/framework
 )
diff --git a/posix-configs/bebop/mainapp.config b/posix-configs/bebop/mainapp.config
index bd4de20090..bd0cbbf4b7 100644
--- a/posix-configs/bebop/mainapp.config
+++ b/posix-configs/bebop/mainapp.config
@@ -1,6 +1,5 @@
 uorb start
 param set SYS_AUTOSTART 4001
-param set MAV_BROADCAST 1
 sleep 1
 param set MAV_TYPE 2
 sensors start

From 52d8723d551d2c486101140bc24a39453aa0fbe5 Mon Sep 17 00:00:00 2001
From: Michael Schaeuble <schaeuble.michael@gmail.com>
Date: Wed, 1 Jun 2016 17:07:52 +0200
Subject: [PATCH 0687/1230] Add df_ms5607_wrapper (a renamed copy from BMP280)

---
 cmake/configs/posix_bebop_default.cmake       |   5 +
 .../drivers/df_ms5607_wrapper/CMakeLists.txt  |  46 +++
 .../df_ms5607_wrapper/df_ms5607_wrapper.cpp   | 328 ++++++++++++++++++
 3 files changed, 379 insertions(+)
 create mode 100644 src/platforms/posix/drivers/df_ms5607_wrapper/CMakeLists.txt
 create mode 100644 src/platforms/posix/drivers/df_ms5607_wrapper/df_ms5607_wrapper.cpp

diff --git a/cmake/configs/posix_bebop_default.cmake b/cmake/configs/posix_bebop_default.cmake
index fdb1d3eb8c..4ec9de8ae4 100644
--- a/cmake/configs/posix_bebop_default.cmake
+++ b/cmake/configs/posix_bebop_default.cmake
@@ -20,6 +20,7 @@ set(config_module_list
 	#
 	drivers/device
 	modules/sensors
+	platforms/posix/drivers/df_ms5607_wrapper
 
 	#
 	# System commands
@@ -94,3 +95,7 @@ set(config_module_list
 	platforms/posix/px4_layer
 	platforms/posix/work_queue
 )
+
+set(config_df_driver_list
+  ms5607
+)
diff --git a/src/platforms/posix/drivers/df_ms5607_wrapper/CMakeLists.txt b/src/platforms/posix/drivers/df_ms5607_wrapper/CMakeLists.txt
new file mode 100644
index 0000000000..d75c46884d
--- /dev/null
+++ b/src/platforms/posix/drivers/df_ms5607_wrapper/CMakeLists.txt
@@ -0,0 +1,46 @@
+############################################################################
+#
+#   Copyright (c) 2016 PX4 Development Team. All rights reserved.
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions
+# are met:
+#
+# 1. Redistributions of source code must retain the above copyright
+#    notice, this list of conditions and the following disclaimer.
+# 2. Redistributions in binary form must reproduce the above copyright
+#    notice, this list of conditions and the following disclaimer in
+#    the documentation and/or other materials provided with the
+#    distribution.
+# 3. Neither the name PX4 nor the names of its contributors may be
+#    used to endorse or promote products derived from this software
+#    without specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.
+#
+############################################################################
+
+include_directories(../../../../lib/DriverFramework/drivers)
+
+px4_add_module(
+	MODULE platforms__posix__drivers__df_ms5607_wrapper
+	MAIN df_ms5607_wrapper
+	SRCS
+		df_ms5607_wrapper.cpp
+	DEPENDS
+		platforms__common
+		df_driver_framework
+		df_ms5607
+	)
+# vim: set noet ft=cmake fenc=utf-8 ff=unix :
diff --git a/src/platforms/posix/drivers/df_ms5607_wrapper/df_ms5607_wrapper.cpp b/src/platforms/posix/drivers/df_ms5607_wrapper/df_ms5607_wrapper.cpp
new file mode 100644
index 0000000000..51fbb8a160
--- /dev/null
+++ b/src/platforms/posix/drivers/df_ms5607_wrapper/df_ms5607_wrapper.cpp
@@ -0,0 +1,328 @@
+/****************************************************************************
+ *
+ * Copyright (c) 2016 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file df_ms5607_wrapper.cpp
+ * Lightweight driver to access the MS5607 of the DriverFramework.
+ */
+
+#include <px4_config.h>
+
+#include <sys/types.h>
+#include <sys/stat.h>
+#include <stdint.h>
+#include <stddef.h>
+#include <stdlib.h>
+#include <string.h>
+#include <math.h>
+#include <unistd.h>
+#include <px4_getopt.h>
+#include <errno.h>
+
+#include <systemlib/perf_counter.h>
+#include <systemlib/err.h>
+
+#include <drivers/drv_baro.h>
+
+#include <board_config.h>
+
+#include <ms5607/MS5607.hpp>
+#include <DevMgr.hpp>
+
+
+extern "C" { __EXPORT int df_ms5607_wrapper_main(int argc, char *argv[]); }
+
+using namespace DriverFramework;
+
+
+class DfMS5607Wrapper : public MS5607
+{
+public:
+	DfMS5607Wrapper();
+	~DfMS5607Wrapper();
+
+
+	/**
+	 * Start automatic measurement.
+	 *
+	 * @return 0 on success
+	 */
+	int		start();
+
+	/**
+	 * Stop automatic measurement.
+	 *
+	 * @return 0 on success
+	 */
+	int		stop();
+
+private:
+	int _publish(struct baro_sensor_data &data);
+
+	orb_advert_t		_baro_topic;
+
+	int			_baro_orb_class_instance;
+
+	perf_counter_t		_baro_sample_perf;
+
+};
+
+DfMS5607Wrapper::DfMS5607Wrapper() :
+	MS5607(BARO_DEVICE_PATH),
+	_baro_topic(nullptr),
+	_baro_orb_class_instance(-1),
+	_baro_sample_perf(perf_alloc(PC_ELAPSED, "df_baro_read"))
+{
+}
+
+DfMS5607Wrapper::~DfMS5607Wrapper()
+{
+	perf_free(_baro_sample_perf);
+}
+
+int DfMS5607Wrapper::start()
+{
+	// TODO: don't publish garbage here
+	baro_report baro_report = {};
+	_baro_topic = orb_advertise_multi(ORB_ID(sensor_baro), &baro_report,
+					  &_baro_orb_class_instance, ORB_PRIO_DEFAULT);
+
+	if (_baro_topic == nullptr) {
+		PX4_ERR("sensor_baro advert fail");
+		return -1;
+	}
+
+	/* Init device and start sensor. */
+	int ret = init();
+
+	if (ret != 0) {
+		PX4_ERR("MS5607 init fail: %d", ret);
+		return ret;
+	}
+
+	ret = MS5607::start();
+
+	if (ret != 0) {
+		PX4_ERR("MS5607 start fail: %d", ret);
+		return ret;
+	}
+
+	return 0;
+}
+
+int DfMS5607Wrapper::stop()
+{
+	/* Stop sensor. */
+	int ret = MS5607::stop();
+
+	if (ret != 0) {
+		PX4_ERR("MS5607 stop fail: %d", ret);
+		return ret;
+	}
+
+	return 0;
+}
+
+int DfMS5607Wrapper::_publish(struct baro_sensor_data &data)
+{
+	perf_begin(_baro_sample_perf);
+
+	baro_report baro_report = {};
+	baro_report.timestamp = hrt_absolute_time();
+
+	baro_report.pressure = data.pressure_pa;
+	baro_report.temperature = data.temperature_c;
+
+	// TODO: verify this, it's just copied from the MS5611 driver.
+
+	// Constant for now
+	const double MSL_PRESSURE_KPA = 101325.0 / 1000.0;
+
+	/* tropospheric properties (0-11km) for standard atmosphere */
+	const double T1 = 15.0 + 273.15;	/* temperature at base height in Kelvin */
+	const double a  = -6.5 / 1000;	/* temperature gradient in degrees per metre */
+	const double g  = 9.80665;	/* gravity constant in m/s/s */
+	const double R  = 287.05;	/* ideal gas constant in J/kg/K */
+
+	/* current pressure at MSL in kPa */
+	double p1 = MSL_PRESSURE_KPA;
+
+	/* measured pressure in kPa */
+	double p = static_cast<double>(data.pressure_pa) / 1000.0;
+
+	/*
+	 * Solve:
+	 *
+	 *     /        -(aR / g)     \
+	 *    | (p / p1)          . T1 | - T1
+	 *     \                      /
+	 * h = -------------------------------  + h1
+	 *                   a
+	 */
+	baro_report.altitude = (((pow((p / p1), (-(a * R) / g))) * T1) - T1) / a;
+
+	// TODO: when is this ever blocked?
+	if (!(m_pub_blocked)) {
+
+		if (_baro_topic != nullptr) {
+			orb_publish(ORB_ID(sensor_baro), _baro_topic, &baro_report);
+		}
+	}
+
+	/* Notify anyone waiting for data. */
+	DevMgr::updateNotify(*this);
+
+	perf_end(_baro_sample_perf);
+
+	return 0;
+};
+
+
+namespace df_ms5607_wrapper
+{
+
+DfMS5607Wrapper *g_dev = nullptr;
+
+int start(/* enum Rotation rotation */);
+int stop();
+int info();
+void usage();
+
+int start(/*enum Rotation rotation*/)
+{
+	g_dev = new DfMS5607Wrapper(/*rotation*/);
+
+	if (g_dev == nullptr) {
+		PX4_ERR("failed instantiating DfMS5607Wrapper object");
+		return -1;
+	}
+
+	int ret = g_dev->start();
+
+	if (ret != 0) {
+		PX4_ERR("DfMS5607Wrapper start failed");
+		return ret;
+	}
+
+	// Open the IMU sensor
+	DevHandle h;
+	DevMgr::getHandle(BARO_DEVICE_PATH, h);
+
+	if (!h.isValid()) {
+		DF_LOG_INFO("Error: unable to obtain a valid handle for the receiver at: %s (%d)",
+			    BARO_DEVICE_PATH, h.getError());
+		return -1;
+	}
+
+	DevMgr::releaseHandle(h);
+
+	return 0;
+}
+
+int stop()
+{
+	if (g_dev == nullptr) {
+		PX4_ERR("driver not running");
+		return 1;
+	}
+
+	int ret = g_dev->stop();
+
+	if (ret != 0) {
+		PX4_ERR("driver could not be stopped");
+		return ret;
+	}
+
+	delete g_dev;
+	g_dev = nullptr;
+	return 0;
+}
+
+/**
+ * Print a little info about the driver.
+ */
+int
+info()
+{
+	if (g_dev == nullptr) {
+		PX4_ERR("driver not running");
+		return 1;
+	}
+
+	PX4_DEBUG("state @ %p", g_dev);
+
+	return 0;
+}
+
+void
+usage()
+{
+	PX4_WARN("Usage: df_ms5607_wrapper 'start', 'info', 'stop'");
+}
+
+} // namespace df_ms5607_wrapper
+
+
+int
+df_ms5607_wrapper_main(int argc, char *argv[])
+{
+	int ret = 0;
+	int myoptind = 1;
+
+	if (argc <= 1) {
+		df_ms5607_wrapper::usage();
+		return 1;
+	}
+
+	const char *verb = argv[myoptind];
+
+
+	if (!strcmp(verb, "start")) {
+		ret = df_ms5607_wrapper::start();
+	}
+
+	else if (!strcmp(verb, "stop")) {
+		ret = df_ms5607_wrapper::stop();
+	}
+
+	else if (!strcmp(verb, "info")) {
+		ret = df_ms5607_wrapper::info();
+	}
+
+	else {
+		df_ms5607_wrapper::usage();
+		return 1;
+	}
+
+	return ret;
+}

From 47613fefa07a8ccdffec3d903453f54acfef52e8 Mon Sep 17 00:00:00 2001
From: Michael Schaeuble <schaeuble.michael@gmail.com>
Date: Wed, 8 Jun 2016 10:37:14 +0200
Subject: [PATCH 0688/1230] Don't advertise garbage, apply same fix from #4735

---
 .../df_ms5607_wrapper/df_ms5607_wrapper.cpp      | 16 +++++-----------
 1 file changed, 5 insertions(+), 11 deletions(-)

diff --git a/src/platforms/posix/drivers/df_ms5607_wrapper/df_ms5607_wrapper.cpp b/src/platforms/posix/drivers/df_ms5607_wrapper/df_ms5607_wrapper.cpp
index 51fbb8a160..9c36f3cab4 100644
--- a/src/platforms/posix/drivers/df_ms5607_wrapper/df_ms5607_wrapper.cpp
+++ b/src/platforms/posix/drivers/df_ms5607_wrapper/df_ms5607_wrapper.cpp
@@ -112,16 +112,6 @@ DfMS5607Wrapper::~DfMS5607Wrapper()
 
 int DfMS5607Wrapper::start()
 {
-	// TODO: don't publish garbage here
-	baro_report baro_report = {};
-	_baro_topic = orb_advertise_multi(ORB_ID(sensor_baro), &baro_report,
-					  &_baro_orb_class_instance, ORB_PRIO_DEFAULT);
-
-	if (_baro_topic == nullptr) {
-		PX4_ERR("sensor_baro advert fail");
-		return -1;
-	}
-
 	/* Init device and start sensor. */
 	int ret = init();
 
@@ -194,7 +184,11 @@ int DfMS5607Wrapper::_publish(struct baro_sensor_data &data)
 	// TODO: when is this ever blocked?
 	if (!(m_pub_blocked)) {
 
-		if (_baro_topic != nullptr) {
+		if (_baro_topic == nullptr) {
+		  _baro_topic = orb_advertise_multi(ORB_ID(sensor_baro), &baro_report,
+		                                    &_baro_orb_class_instance, ORB_PRIO_DEFAULT);
+
+		} else {
 			orb_publish(ORB_ID(sensor_baro), _baro_topic, &baro_report);
 		}
 	}

From 847562f5d70f7c31b9d5651118109d0f715d778e Mon Sep 17 00:00:00 2001
From: Michael Schaeuble <schaeuble.michael@gmail.com>
Date: Wed, 8 Jun 2016 10:38:47 +0200
Subject: [PATCH 0689/1230] Don't build px4_simple_app anymore

---
 cmake/configs/posix_bebop_default.cmake | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/cmake/configs/posix_bebop_default.cmake b/cmake/configs/posix_bebop_default.cmake
index 4ec9de8ae4..a8e1d21674 100644
--- a/cmake/configs/posix_bebop_default.cmake
+++ b/cmake/configs/posix_bebop_default.cmake
@@ -13,7 +13,7 @@ set(CMAKE_PROGRAM_PATH
 
 set(config_module_list
 
-  examples/px4_simple_app
+  # examples/px4_simple_app
 
 	#
 	# Board support modules

From 0d9c031a2c4e0bd410f0c719a6e3b54b8cd56ccc Mon Sep 17 00:00:00 2001
From: Michael Schaeuble <schaeuble.michael@gmail.com>
Date: Wed, 15 Jun 2016 20:02:55 +0200
Subject: [PATCH 0690/1230] Check for bebop ip as environment variable

---
 Tools/adb_upload_to_bebop.sh | 8 +++++++-
 1 file changed, 7 insertions(+), 1 deletion(-)

diff --git a/Tools/adb_upload_to_bebop.sh b/Tools/adb_upload_to_bebop.sh
index 64e87b6712..bf133d6b1e 100755
--- a/Tools/adb_upload_to_bebop.sh
+++ b/Tools/adb_upload_to_bebop.sh
@@ -1,6 +1,12 @@
 #!/bin/bash
 
-ip=192.168.42.1
+if [ -z ${BEBOP_IP+x} ]; then 
+  ip=192.168.42.1
+  echo "\$BEBOP_IP is not set (use default: $ip)"
+else 
+  ip=$BEBOP_IP
+  echo "\$BEBOP_IP is set to $ip"
+fi
 port=9050
 
 echo "Connecting to bebop: $ip:$port"

From cc0d28e59b969113498dada56b85bf85b4ec4e21 Mon Sep 17 00:00:00 2001
From: Michael Schaeuble <schaeuble.michael@gmail.com>
Date: Sun, 19 Jun 2016 14:13:36 +0200
Subject: [PATCH 0691/1230] Update DF to include the MS5607 driver

---
 cmake/configs/posix_bebop_default.cmake | 1 +
 1 file changed, 1 insertion(+)

diff --git a/cmake/configs/posix_bebop_default.cmake b/cmake/configs/posix_bebop_default.cmake
index a8e1d21674..959ae09467 100644
--- a/cmake/configs/posix_bebop_default.cmake
+++ b/cmake/configs/posix_bebop_default.cmake
@@ -4,6 +4,7 @@ set(CMAKE_TOOLCHAIN_FILE ${CMAKE_SOURCE_DIR}/cmake/toolchains/Toolchain-arm-linu
 
 add_definitions(
   -D__PX4_POSIX_BEBOP
+  -D__BEBOP
 	)
 
 set(CMAKE_PROGRAM_PATH

From 83ec092b46dff17c61de918dcad423a98ca10b9d Mon Sep 17 00:00:00 2001
From: Michael Schaeuble <schaeuble.michael@gmail.com>
Date: Sun, 19 Jun 2016 16:55:06 +0200
Subject: [PATCH 0692/1230] Update bebop configuration and use parameters

---
 Tools/adb_upload_to_bebop.sh       |  1 +
 posix-configs/bebop/mainapp.config | 13 +++++--------
 src/platforms/px4_defines.h        |  2 ++
 3 files changed, 8 insertions(+), 8 deletions(-)

diff --git a/Tools/adb_upload_to_bebop.sh b/Tools/adb_upload_to_bebop.sh
index bf133d6b1e..ca3eb84837 100755
--- a/Tools/adb_upload_to_bebop.sh
+++ b/Tools/adb_upload_to_bebop.sh
@@ -31,6 +31,7 @@ echo "Connection successfully established"
 sleep 1
 
 adb shell mount -o remount,rw /
+adb shell touch /home/root/parameters
 
 ../Tools/adb_upload.sh $@
 
diff --git a/posix-configs/bebop/mainapp.config b/posix-configs/bebop/mainapp.config
index bd0cbbf4b7..3c6bffdc80 100644
--- a/posix-configs/bebop/mainapp.config
+++ b/posix-configs/bebop/mainapp.config
@@ -1,15 +1,12 @@
 uorb start
+param select /home/root/parameters
+param load
 param set SYS_AUTOSTART 4001
+param set MAV_BROADCAST 1
+df_ms5607_wrapper start
 sleep 1
-param set MAV_TYPE 2
-sensors start
-commander start
-ekf2 start
-land_detector start multicopter
-mc_pos_control start
-mc_att_control start
 mavlink start -u 14556 -r 1000000
 sleep 1
-mavlink stream -u 14556 -s HIGHRES_IMU -r 50
 mavlink stream -u 14556 -s ATTITUDE -r 50
+mavlink stream -u 14556 -s ALTITUDE -r 1
 mavlink boot_complete
diff --git a/src/platforms/px4_defines.h b/src/platforms/px4_defines.h
index ee721b8221..1861d27ede 100644
--- a/src/platforms/px4_defines.h
+++ b/src/platforms/px4_defines.h
@@ -162,6 +162,8 @@ __END_DECLS
 #define PX4_ROOTFSDIR
 #elif defined(__PX4_POSIX_EAGLE)
 #define PX4_ROOTFSDIR "/home/linaro"
+#elif defined(__PX4_POSIX_BEBOP)
+#define PX4_ROOTFSDIR "/home/root"
 #else
 #define PX4_ROOTFSDIR "rootfs"
 #endif

From f95f37cb7bfd65652635fca3497e93d17d69b537 Mon Sep 17 00:00:00 2001
From: Michael Schaeuble <schaeuble.michael@gmail.com>
Date: Mon, 20 Jun 2016 11:34:16 +0200
Subject: [PATCH 0693/1230] Update to DF commit with the MS5607 driver

---
 cmake/configs/posix_bebop_default.cmake | 2 +-
 src/lib/DriverFramework                 | 2 +-
 2 files changed, 2 insertions(+), 2 deletions(-)

diff --git a/cmake/configs/posix_bebop_default.cmake b/cmake/configs/posix_bebop_default.cmake
index 959ae09467..a2296222ad 100644
--- a/cmake/configs/posix_bebop_default.cmake
+++ b/cmake/configs/posix_bebop_default.cmake
@@ -4,7 +4,7 @@ set(CMAKE_TOOLCHAIN_FILE ${CMAKE_SOURCE_DIR}/cmake/toolchains/Toolchain-arm-linu
 
 add_definitions(
   -D__PX4_POSIX_BEBOP
-  -D__BEBOP
+  -D__LINUX
 	)
 
 set(CMAKE_PROGRAM_PATH
diff --git a/src/lib/DriverFramework b/src/lib/DriverFramework
index 36335412b2..03115219f2 160000
--- a/src/lib/DriverFramework
+++ b/src/lib/DriverFramework
@@ -1 +1 @@
-Subproject commit 36335412b2c461de08696d224b8ae1149fa5ae28
+Subproject commit 03115219f20761682f50f201feba47eb642b6ae3

From d92496a7f73b80db2f71461ed03c7ef862b56c84 Mon Sep 17 00:00:00 2001
From: Julian Oes <julian@oes.ch>
Date: Mon, 20 Jun 2016 15:29:30 +0200
Subject: [PATCH 0694/1230] df_ms5607_wrapper: astyle (#4853)

---
 .../posix/drivers/df_ms5607_wrapper/df_ms5607_wrapper.cpp     | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/src/platforms/posix/drivers/df_ms5607_wrapper/df_ms5607_wrapper.cpp b/src/platforms/posix/drivers/df_ms5607_wrapper/df_ms5607_wrapper.cpp
index 9c36f3cab4..c02c708f5d 100644
--- a/src/platforms/posix/drivers/df_ms5607_wrapper/df_ms5607_wrapper.cpp
+++ b/src/platforms/posix/drivers/df_ms5607_wrapper/df_ms5607_wrapper.cpp
@@ -185,8 +185,8 @@ int DfMS5607Wrapper::_publish(struct baro_sensor_data &data)
 	if (!(m_pub_blocked)) {
 
 		if (_baro_topic == nullptr) {
-		  _baro_topic = orb_advertise_multi(ORB_ID(sensor_baro), &baro_report,
-		                                    &_baro_orb_class_instance, ORB_PRIO_DEFAULT);
+			_baro_topic = orb_advertise_multi(ORB_ID(sensor_baro), &baro_report,
+							  &_baro_orb_class_instance, ORB_PRIO_DEFAULT);
 
 		} else {
 			orb_publish(ORB_ID(sensor_baro), _baro_topic, &baro_report);

From e42b5804a0c166f883da50d91aefa0060606e1fb Mon Sep 17 00:00:00 2001
From: Daniel Agar <daniel@agar.ca>
Date: Mon, 20 Jun 2016 10:03:37 -0400
Subject: [PATCH 0695/1230] travis-ci only run make check within docker (#4854)

 * otherwise the build environment doesn't get the proper return code
---
 .travis.yml | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/.travis.yml b/.travis.yml
index 12f57ed6f7..6e8742e8ab 100644
--- a/.travis.yml
+++ b/.travis.yml
@@ -48,7 +48,7 @@ env:
 
 script:
   - if [ "${TRAVIS_OS_NAME}" = "linux" ]; then
-      docker run --rm -v `pwd`:`pwd`:rw -v $HOME/.ccache:$HOME/.ccache:rw -e CCACHE_DIR=$HOME/.ccache -w=`pwd` --user=$UID -it ${DOCKER_REPO} /bin/bash -c "ccache -z; make check VECTORCONTROL=1; ccache -s";
+      docker run --rm -v `pwd`:`pwd`:rw -v $HOME/.ccache:$HOME/.ccache:rw -e CCACHE_DIR=$HOME/.ccache -w=`pwd` --user=$UID -it ${DOCKER_REPO} /bin/bash -c "make check VECTORCONTROL=1";
     elif [ "${TRAVIS_OS_NAME}" = "osx" ]; then
       make check_posix_sitl_default;
     fi

From ced83762689d900a448074b65b25e2344a17f1e9 Mon Sep 17 00:00:00 2001
From: Andreas Antener <antener_a@gmx.ch>
Date: Thu, 26 Nov 2015 17:45:20 +0100
Subject: [PATCH 0696/1230] added offboard lost actions with additional timeout

---
 posix-configs/SITL/init/rcS_gazebo_iris       |  6 +-
 src/modules/commander/commander.cpp           | 25 ++++++-
 src/modules/commander/commander_params.c      | 12 +++
 .../commander/state_machine_helper.cpp        | 73 +++++++++++++++++--
 src/modules/commander/state_machine_helper.h  |  3 +-
 src/modules/navigator/navigator_params.c      | 30 ++++++++
 6 files changed, 138 insertions(+), 11 deletions(-)

diff --git a/posix-configs/SITL/init/rcS_gazebo_iris b/posix-configs/SITL/init/rcS_gazebo_iris
index 5978c609de..46ab10febb 100644
--- a/posix-configs/SITL/init/rcS_gazebo_iris
+++ b/posix-configs/SITL/init/rcS_gazebo_iris
@@ -23,9 +23,11 @@ param set COM_RC_IN_MODE 1
 param set NAV_DLL_ACT 2
 param set COM_DISARM_LAND 3
 param set NAV_ACC_RAD 2.0
+param set COM_OF_LOSS_T 5
 param set RTL_RETURN_ALT 30.0
-param set RTL_DESCEND_ALT 10.0
-param set RTL_LAND_DELAY 0
+param set RTL_DESCEND_ALT 5.0
+param set RTL_LAND_DELAY 5
+param set COM_DISARM_LAND 5
 param set MIS_TAKEOFF_ALT 2.5
 param set MC_ROLLRATE_P 0.3
 param set MC_PITCHRATE_P 0.3
diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp
index afb8a11f7b..c010b7c6b2 100644
--- a/src/modules/commander/commander.cpp
+++ b/src/modules/commander/commander.cpp
@@ -1161,6 +1161,8 @@ int commander_thread_main(int argc, char *argv[])
 	param_t _param_component_id = param_find("MAV_COMP_ID");
 	param_t _param_enable_datalink_loss = param_find("NAV_DLL_ACT");
 	param_t _param_enable_rc_loss = param_find("NAV_RCL_ACT");
+	param_t _param_offboard_loss_act = param_find("NAV_OBL_ACT");
+	param_t _param_offboard_loss_rc_act = param_find("NAV_OBL_RC_ACT");
 	param_t _param_datalink_loss_timeout = param_find("COM_DL_LOSS_T");
 	param_t _param_rc_loss_timeout = param_find("COM_RC_LOSS_T");
 	param_t _param_datalink_regain_timeout = param_find("COM_DL_REG_T");
@@ -1176,6 +1178,7 @@ int commander_thread_main(int argc, char *argv[])
 	param_t _param_geofence_action = param_find("GF_ACTION");
 	param_t _param_disarm_land = param_find("COM_DISARM_LAND");
 	param_t _param_low_bat_act = param_find("COM_LOW_BAT_ACT");
+	param_t _param_offboard_loss_timeout = param_find("COM_OF_LOSS_T");
 
 	param_t _param_fmode_1 = param_find("COM_FLTMODE1");
 	param_t _param_fmode_2 = param_find("COM_FLTMODE2");
@@ -1256,6 +1259,7 @@ int commander_thread_main(int argc, char *argv[])
 	status.rc_signal_lost = true;
 	status_flags.offboard_control_signal_lost = true;
 	status.data_link_lost = true;
+	status_flags.offboard_control_loss_timeout = false;
 
 	status_flags.condition_system_prearm_error_reported = false;
 	status_flags.condition_system_hotplug_timeout = false;
@@ -1515,6 +1519,9 @@ int commander_thread_main(int argc, char *argv[])
 	int32_t datalink_loss_timeout = 10;
 	float rc_loss_timeout = 0.5;
 	int32_t datalink_regain_timeout = 0;
+	float offboard_loss_timeout = 0.0f;
+	int32_t offboard_loss_act = 0;
+	int32_t offboard_loss_rc_act = 0;
 
 	int32_t geofence_action = 0;
 
@@ -1609,6 +1616,9 @@ int commander_thread_main(int argc, char *argv[])
 			param_get(_param_geofence_action, &geofence_action);
 			param_get(_param_disarm_land, &disarm_when_landed);
 			param_get(_param_low_bat_act, &low_bat_action);
+			param_get(_param_offboard_loss_timeout, &offboard_loss_timeout);
+			param_set(_param_offboard_loss_act, &offboard_loss_act);
+			param_set(_param_offboard_loss_rc_act, &offboard_loss_rc_act);
 
 			/* Autostart id */
 			param_get(_param_autostart_id, &autostart_id);
@@ -1651,12 +1661,23 @@ int commander_thread_main(int argc, char *argv[])
 		    offboard_control_mode.timestamp + OFFBOARD_TIMEOUT > hrt_absolute_time()) {
 			if (status_flags.offboard_control_signal_lost) {
 				status_flags.offboard_control_signal_lost = false;
+				status_flags.offboard_control_loss_timeout = false;
 				status_changed = true;
 			}
 
 		} else {
 			if (!status_flags.offboard_control_signal_lost) {
 				status_flags.offboard_control_signal_lost = true;
+
+				if (offboard_loss_timeout < FLT_EPSILON) {
+					/* execute loss action immediately */
+					status_flags.offboard_control_loss_timeout = true;
+
+				} else {
+					/* wait for timeout if set */
+					status_flags.offboard_control_loss_timeout = offboard_control_mode.timestamp +
+						OFFBOARD_TIMEOUT + offboard_loss_timeout * 1e6f > hrt_absolute_time();
+				}
 				status_changed = true;
 			}
 		}
@@ -2669,7 +2690,9 @@ int commander_thread_main(int argc, char *argv[])
 						       mission_result.stay_in_failsafe,
 						       &status_flags,
 						       land_detector.landed,
-						       (rc_loss_enabled > 0));
+						       (rc_loss_enabled > 0),
+						       offboard_loss_act,
+						       offboard_loss_rc_act);
 
 		if (status.failsafe != failsafe_old) {
 			status_changed = true;
diff --git a/src/modules/commander/commander_params.c b/src/modules/commander/commander_params.c
index 0402554493..3ca993071b 100644
--- a/src/modules/commander/commander_params.c
+++ b/src/modules/commander/commander_params.c
@@ -413,3 +413,15 @@ PARAM_DEFINE_INT32(COM_FLTMODE5, -1);
  * @value 12 Follow Me
  */
 PARAM_DEFINE_INT32(COM_FLTMODE6, -1);
+
+/**
+ * Time-out to wait when offboard connection is lost before triggering offboard lost action.
+ * See NAV_OBL_ACT and NAV_OBL_RC_ACT to configure action.
+ *
+ * @group Commander
+ * @unit second
+ * @min 0
+ * @max 60
+ * @increment 1
+ */
+PARAM_DEFINE_FLOAT(COM_OF_LOSS_T, 0.0f);
diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp
index 88f3e5d6ec..8eb5d4fc3e 100644
--- a/src/modules/commander/state_machine_helper.cpp
+++ b/src/modules/commander/state_machine_helper.cpp
@@ -588,7 +588,8 @@ transition_result_t hil_state_transition(hil_state_t new_state, orb_advert_t sta
  */
 bool set_nav_state(struct vehicle_status_s *status, struct commander_state_s *internal_state,
 		   const bool data_link_loss_enabled, const bool mission_finished,
-		   const bool stay_in_failsafe, status_flags_s *status_flags, bool landed, const bool rc_loss_enabled)
+		   const bool stay_in_failsafe, status_flags_s *status_flags, bool landed, const bool rc_loss_enabled,
+		   const int offb_loss_act, const int offb_loss_rc_act)
 {
 	navigation_state_t nav_state_old = status->nav_state;
 
@@ -847,17 +848,75 @@ bool set_nav_state(struct vehicle_status_s *status, struct commander_state_s *in
 		if (status_flags->offboard_control_signal_lost && !status->rc_signal_lost) {
 			status->failsafe = true;
 
-			status->nav_state = vehicle_status_s::NAVIGATION_STATE_POSCTL;
+			if (status_flags->offboard_control_loss_timeout && offb_loss_rc_act < 5 && offb_loss_rc_act >= 0) {
+				if (status_flags->condition_global_position_valid && status_flags->condition_home_position_valid
+					&& offb_loss_rc_act == 3) {
+					status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_RTL;
+
+				} else if (status_flags->condition_global_position_valid && offb_loss_rc_act == 0) {
+					status->nav_state = vehicle_status_s::NAVIGATION_STATE_POSCTL;
+
+				} else if (status_flags->condition_local_altitude_valid && offb_loss_rc_act == 1) {
+					status->nav_state = vehicle_status_s::NAVIGATION_STATE_ALTCTL;
+
+				} else if (offb_loss_rc_act == 2) {
+					status->nav_state = vehicle_status_s::NAVIGATION_STATE_MANUAL;
+
+				} else if (status_flags->condition_global_position_valid && offb_loss_rc_act == 4) {
+					status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LAND;
+
+				} else if (status_flags->condition_local_altitude_valid) {
+					status->nav_state = vehicle_status_s::NAVIGATION_STATE_DESCEND;
+
+				} else {
+					status->nav_state = vehicle_status_s::NAVIGATION_STATE_TERMINATION;
+				}
+
+			} else {
+				if (status_flags->condition_global_position_valid) {
+					status->nav_state = vehicle_status_s::NAVIGATION_STATE_POSCTL;
+
+				} else if (status_flags->condition_local_altitude_valid) {
+					status->nav_state = vehicle_status_s::NAVIGATION_STATE_ALTCTL;
+
+				} else {
+					status->nav_state = vehicle_status_s::NAVIGATION_STATE_TERMINATION;
+				}
+			}
+
 		} else if (status_flags->offboard_control_signal_lost && status->rc_signal_lost) {
 			status->failsafe = true;
 
-			if (status_flags->condition_local_position_valid) {
-				status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LAND;
-			} else if (status_flags->condition_local_altitude_valid) {
-				status->nav_state = vehicle_status_s::NAVIGATION_STATE_DESCEND;
+			if (status_flags->offboard_control_loss_timeout && offb_loss_act < 3 && offb_loss_act >= 0) {
+				if (status_flags->condition_global_position_valid && status_flags->condition_home_position_valid
+					&& offb_loss_rc_act == 2) {
+					status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_RTL;
+
+				} else if (status_flags->condition_global_position_valid && offb_loss_rc_act == 1) {
+					status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER;
+
+				} else if (status_flags->condition_global_position_valid && offb_loss_rc_act == 0) {
+					status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LAND;
+
+				} else if (status_flags->condition_local_altitude_valid) {
+					status->nav_state = vehicle_status_s::NAVIGATION_STATE_DESCEND;
+
+				} else {
+					status->nav_state = vehicle_status_s::NAVIGATION_STATE_TERMINATION;
+				}
+
 			} else {
-				status->nav_state = vehicle_status_s::NAVIGATION_STATE_TERMINATION;
+				if (status_flags->condition_global_position_valid) {
+					status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER;
+
+				} else if (status_flags->condition_local_altitude_valid) {
+					status->nav_state = vehicle_status_s::NAVIGATION_STATE_DESCEND;
+
+				} else {
+					status->nav_state = vehicle_status_s::NAVIGATION_STATE_TERMINATION;
+				}
 			}
+
 		} else {
 			status->nav_state = vehicle_status_s::NAVIGATION_STATE_OFFBOARD;
 		}
diff --git a/src/modules/commander/state_machine_helper.h b/src/modules/commander/state_machine_helper.h
index f3302eba08..4012c0e89f 100644
--- a/src/modules/commander/state_machine_helper.h
+++ b/src/modules/commander/state_machine_helper.h
@@ -81,6 +81,7 @@ struct status_flags_s {
 	bool offboard_control_signal_lost;
 	bool offboard_control_signal_weak;
 	bool offboard_control_set_by_command;		// true if the offboard mode was set by a mavlink command and should not be overridden by RC
+	bool offboard_control_loss_timeout;		// true if offboard is lost for a certain amount of time
 	bool rc_signal_found_once;
 	bool rc_signal_lost_cmd;			// true if RC lost mode is commanded
 	bool rc_input_blocked;				// set if RC input should be ignored temporarily
@@ -113,7 +114,7 @@ transition_result_t hil_state_transition(hil_state_t new_state, orb_advert_t sta
 bool set_nav_state(struct vehicle_status_s *status, struct commander_state_s *internal_state,
 		   const bool data_link_loss_enabled, const bool mission_finished,
 		   const bool stay_in_failsafe, status_flags_s *status_flags, bool landed,
-		   const bool rc_loss_enabled);
+		   const bool rc_loss_enabled, const int offb_loss_act, const int offb_loss_rc_act);
 
 int preflight_check(struct vehicle_status_s *status, orb_advert_t *mavlink_log_pub, bool prearm, bool force_report, status_flags_s *status_flags, battery_status_s *battery);
 
diff --git a/src/modules/navigator/navigator_params.c b/src/modules/navigator/navigator_params.c
index 523f03b42e..ab923c4850 100644
--- a/src/modules/navigator/navigator_params.c
+++ b/src/modules/navigator/navigator_params.c
@@ -104,6 +104,36 @@ PARAM_DEFINE_INT32(NAV_DLL_ACT, 0);
  */
 PARAM_DEFINE_INT32(NAV_RCL_ACT, 0);
 
+/**
+ * Set offboard loss failsafe mode
+ *
+ * The offboard loss failsafe will only be entered after a timeout,
+ * set by COM_OF_LOSS_T in seconds.
+ *
+ * @value 0 Land at current position
+ * @value 1 Loiter
+ * @value 2 Return to Land
+ *
+ * @group Mission
+ */
+PARAM_DEFINE_INT32(NAV_OBL_ACT, 0);
+
+/**
+ * Set offboard loss failsafe mode when RC is available
+ *
+ * The offboard loss failsafe will only be entered after a timeout,
+ * set by COM_OF_LOSS_T in seconds.
+ *
+ * @value 0 Position control
+ * @value 1 Altitude control
+ * @value 2 Manual
+ * @value 3 Return to Land
+ * @value 4 Land at current position
+ *
+ * @group Mission
+ */
+PARAM_DEFINE_INT32(NAV_OBL_RC_ACT, 0);
+
 /**
  * Airfield home Lat
  *

From fedb9de6ef5b91b103784a2a2cf41f3ea696ffa3 Mon Sep 17 00:00:00 2001
From: Andreas Antener <antener_a@gmx.ch>
Date: Thu, 2 Jun 2016 20:43:47 +0200
Subject: [PATCH 0697/1230] fixed offboard loss timing handling

---
 src/modules/commander/commander.cpp | 11 +++++++++--
 1 file changed, 9 insertions(+), 2 deletions(-)

diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp
index c010b7c6b2..7ea3aacf9e 100644
--- a/src/modules/commander/commander.cpp
+++ b/src/modules/commander/commander.cpp
@@ -1668,7 +1668,11 @@ int commander_thread_main(int argc, char *argv[])
 		} else {
 			if (!status_flags.offboard_control_signal_lost) {
 				status_flags.offboard_control_signal_lost = true;
+				status_changed = true;
+			}
 
+			/* check timer if offboard was there but now lost */
+			if (!status_flags.offboard_control_loss_timeout && offboard_control_mode.timestamp != 0) {
 				if (offboard_loss_timeout < FLT_EPSILON) {
 					/* execute loss action immediately */
 					status_flags.offboard_control_loss_timeout = true;
@@ -1676,9 +1680,12 @@ int commander_thread_main(int argc, char *argv[])
 				} else {
 					/* wait for timeout if set */
 					status_flags.offboard_control_loss_timeout = offboard_control_mode.timestamp +
-						OFFBOARD_TIMEOUT + offboard_loss_timeout * 1e6f > hrt_absolute_time();
+						OFFBOARD_TIMEOUT + offboard_loss_timeout * 1e6f < hrt_absolute_time();
+				}
+
+				if (status_flags.offboard_control_loss_timeout) {
+					status_changed = true;
 				}
-				status_changed = true;
 			}
 		}
 

From 095997ca59e261964e9a7054a4b5370f689da752 Mon Sep 17 00:00:00 2001
From: Andreas Antener <antener_a@gmx.ch>
Date: Thu, 2 Jun 2016 21:00:15 +0200
Subject: [PATCH 0698/1230] changed order of arguments for readability

---
 src/modules/commander/state_machine_helper.cpp | 18 +++++++++---------
 1 file changed, 9 insertions(+), 9 deletions(-)

diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp
index 8eb5d4fc3e..8cfc38b109 100644
--- a/src/modules/commander/state_machine_helper.cpp
+++ b/src/modules/commander/state_machine_helper.cpp
@@ -849,20 +849,20 @@ bool set_nav_state(struct vehicle_status_s *status, struct commander_state_s *in
 			status->failsafe = true;
 
 			if (status_flags->offboard_control_loss_timeout && offb_loss_rc_act < 5 && offb_loss_rc_act >= 0) {
-				if (status_flags->condition_global_position_valid && status_flags->condition_home_position_valid
-					&& offb_loss_rc_act == 3) {
+				if (offb_loss_rc_act == 3 && status_flags->condition_global_position_valid
+						&& status_flags->condition_home_position_valid) {
 					status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_RTL;
 
-				} else if (status_flags->condition_global_position_valid && offb_loss_rc_act == 0) {
+				} else if (offb_loss_rc_act == 0 && status_flags->condition_global_position_valid) {
 					status->nav_state = vehicle_status_s::NAVIGATION_STATE_POSCTL;
 
-				} else if (status_flags->condition_local_altitude_valid && offb_loss_rc_act == 1) {
+				} else if (offb_loss_rc_act == 1 && status_flags->condition_local_altitude_valid) {
 					status->nav_state = vehicle_status_s::NAVIGATION_STATE_ALTCTL;
 
 				} else if (offb_loss_rc_act == 2) {
 					status->nav_state = vehicle_status_s::NAVIGATION_STATE_MANUAL;
 
-				} else if (status_flags->condition_global_position_valid && offb_loss_rc_act == 4) {
+				} else if (offb_loss_rc_act == 4 && status_flags->condition_global_position_valid) {
 					status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LAND;
 
 				} else if (status_flags->condition_local_altitude_valid) {
@@ -888,14 +888,14 @@ bool set_nav_state(struct vehicle_status_s *status, struct commander_state_s *in
 			status->failsafe = true;
 
 			if (status_flags->offboard_control_loss_timeout && offb_loss_act < 3 && offb_loss_act >= 0) {
-				if (status_flags->condition_global_position_valid && status_flags->condition_home_position_valid
-					&& offb_loss_rc_act == 2) {
+				if (offb_loss_act == 2 && status_flags->condition_global_position_valid
+						&& status_flags->condition_home_position_valid) {
 					status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_RTL;
 
-				} else if (status_flags->condition_global_position_valid && offb_loss_rc_act == 1) {
+				} else if (offb_loss_act == 1 && status_flags->condition_global_position_valid) {
 					status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER;
 
-				} else if (status_flags->condition_global_position_valid && offb_loss_rc_act == 0) {
+				} else if (offb_loss_act == 0 && status_flags->condition_global_position_valid) {
 					status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LAND;
 
 				} else if (status_flags->condition_local_altitude_valid) {

From d3d9f013f48fd3a5da0b4bebd5bdbe89c0b84e4f Mon Sep 17 00:00:00 2001
From: Andreas Antener <antener_a@gmx.ch>
Date: Fri, 3 Jun 2016 11:05:33 +0200
Subject: [PATCH 0699/1230] set OBL parameters for iris

---
 posix-configs/SITL/init/rcS_gazebo_iris | 3 ++-
 1 file changed, 2 insertions(+), 1 deletion(-)

diff --git a/posix-configs/SITL/init/rcS_gazebo_iris b/posix-configs/SITL/init/rcS_gazebo_iris
index 46ab10febb..8e915cea66 100644
--- a/posix-configs/SITL/init/rcS_gazebo_iris
+++ b/posix-configs/SITL/init/rcS_gazebo_iris
@@ -24,10 +24,11 @@ param set NAV_DLL_ACT 2
 param set COM_DISARM_LAND 3
 param set NAV_ACC_RAD 2.0
 param set COM_OF_LOSS_T 5
+param set NAV_OBL_ACT 2
+param set NAV_OBL_RC_ACT 0
 param set RTL_RETURN_ALT 30.0
 param set RTL_DESCEND_ALT 5.0
 param set RTL_LAND_DELAY 5
-param set COM_DISARM_LAND 5
 param set MIS_TAKEOFF_ALT 2.5
 param set MC_ROLLRATE_P 0.3
 param set MC_PITCHRATE_P 0.3

From 8727295f8e4e5775bb643b0cace1cfc79585fd2e Mon Sep 17 00:00:00 2001
From: Andreas Antener <antener_a@gmx.ch>
Date: Wed, 8 Jun 2016 13:21:17 +0200
Subject: [PATCH 0700/1230] fix OBL parameters in commander

---
 src/modules/commander/commander.cpp | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp
index 7ea3aacf9e..5f813ec138 100644
--- a/src/modules/commander/commander.cpp
+++ b/src/modules/commander/commander.cpp
@@ -1617,8 +1617,8 @@ int commander_thread_main(int argc, char *argv[])
 			param_get(_param_disarm_land, &disarm_when_landed);
 			param_get(_param_low_bat_act, &low_bat_action);
 			param_get(_param_offboard_loss_timeout, &offboard_loss_timeout);
-			param_set(_param_offboard_loss_act, &offboard_loss_act);
-			param_set(_param_offboard_loss_rc_act, &offboard_loss_rc_act);
+			param_get(_param_offboard_loss_act, &offboard_loss_act);
+			param_get(_param_offboard_loss_rc_act, &offboard_loss_rc_act);
 
 			/* Autostart id */
 			param_get(_param_autostart_id, &autostart_id);

From d1b27ab056d35f695b33d1a021d8900a4d69ba61 Mon Sep 17 00:00:00 2001
From: Andreas Antener <antener_a@gmx.ch>
Date: Mon, 20 Jun 2016 19:23:11 +0200
Subject: [PATCH 0701/1230] moved params NAV_OBL to COM_OBL

---
 posix-configs/SITL/init/rcS_gazebo_iris  |  4 +-
 src/modules/commander/commander.cpp      |  4 +-
 src/modules/commander/commander_params.c | 54 ++++++++++++++++++------
 src/modules/navigator/navigator_params.c | 30 -------------
 4 files changed, 46 insertions(+), 46 deletions(-)

diff --git a/posix-configs/SITL/init/rcS_gazebo_iris b/posix-configs/SITL/init/rcS_gazebo_iris
index 8e915cea66..47f862fb04 100644
--- a/posix-configs/SITL/init/rcS_gazebo_iris
+++ b/posix-configs/SITL/init/rcS_gazebo_iris
@@ -24,8 +24,8 @@ param set NAV_DLL_ACT 2
 param set COM_DISARM_LAND 3
 param set NAV_ACC_RAD 2.0
 param set COM_OF_LOSS_T 5
-param set NAV_OBL_ACT 2
-param set NAV_OBL_RC_ACT 0
+param set COM_OBL_ACT 2
+param set COM_OBL_RC_ACT 0
 param set RTL_RETURN_ALT 30.0
 param set RTL_DESCEND_ALT 5.0
 param set RTL_LAND_DELAY 5
diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp
index 5f813ec138..c9b50d162a 100644
--- a/src/modules/commander/commander.cpp
+++ b/src/modules/commander/commander.cpp
@@ -1161,8 +1161,8 @@ int commander_thread_main(int argc, char *argv[])
 	param_t _param_component_id = param_find("MAV_COMP_ID");
 	param_t _param_enable_datalink_loss = param_find("NAV_DLL_ACT");
 	param_t _param_enable_rc_loss = param_find("NAV_RCL_ACT");
-	param_t _param_offboard_loss_act = param_find("NAV_OBL_ACT");
-	param_t _param_offboard_loss_rc_act = param_find("NAV_OBL_RC_ACT");
+	param_t _param_offboard_loss_act = param_find("COM_OBL_ACT");
+	param_t _param_offboard_loss_rc_act = param_find("COM_OBL_RC_ACT");
 	param_t _param_datalink_loss_timeout = param_find("COM_DL_LOSS_T");
 	param_t _param_rc_loss_timeout = param_find("COM_RC_LOSS_T");
 	param_t _param_datalink_regain_timeout = param_find("COM_DL_REG_T");
diff --git a/src/modules/commander/commander_params.c b/src/modules/commander/commander_params.c
index 3ca993071b..fc9fcf5372 100644
--- a/src/modules/commander/commander_params.c
+++ b/src/modules/commander/commander_params.c
@@ -276,6 +276,48 @@ PARAM_DEFINE_INT32(COM_DISARM_LAND, 0);
  */
 PARAM_DEFINE_INT32(COM_LOW_BAT_ACT, 0);
 
+/**
+ * Time-out to wait when offboard connection is lost before triggering offboard lost action.
+ * See COM_OBL_ACT and COM_OBL_RC_ACT to configure action.
+ *
+ * @group Commander
+ * @unit second
+ * @min 0
+ * @max 60
+ * @increment 1
+ */
+PARAM_DEFINE_FLOAT(COM_OF_LOSS_T, 0.0f);
+
+/**
+ * Set offboard loss failsafe mode
+ *
+ * The offboard loss failsafe will only be entered after a timeout,
+ * set by COM_OF_LOSS_T in seconds.
+ *
+ * @value 0 Land at current position
+ * @value 1 Loiter
+ * @value 2 Return to Land
+ *
+ * @group Mission
+ */
+PARAM_DEFINE_INT32(COM_OBL_ACT, 0);
+
+/**
+ * Set offboard loss failsafe mode when RC is available
+ *
+ * The offboard loss failsafe will only be entered after a timeout,
+ * set by COM_OF_LOSS_T in seconds.
+ *
+ * @value 0 Position control
+ * @value 1 Altitude control
+ * @value 2 Manual
+ * @value 3 Return to Land
+ * @value 4 Land at current position
+ *
+ * @group Mission
+ */
+PARAM_DEFINE_INT32(COM_OBL_RC_ACT, 0);
+
 /**
  * First flightmode slot (1000-1160)
  *
@@ -413,15 +455,3 @@ PARAM_DEFINE_INT32(COM_FLTMODE5, -1);
  * @value 12 Follow Me
  */
 PARAM_DEFINE_INT32(COM_FLTMODE6, -1);
-
-/**
- * Time-out to wait when offboard connection is lost before triggering offboard lost action.
- * See NAV_OBL_ACT and NAV_OBL_RC_ACT to configure action.
- *
- * @group Commander
- * @unit second
- * @min 0
- * @max 60
- * @increment 1
- */
-PARAM_DEFINE_FLOAT(COM_OF_LOSS_T, 0.0f);
diff --git a/src/modules/navigator/navigator_params.c b/src/modules/navigator/navigator_params.c
index ab923c4850..523f03b42e 100644
--- a/src/modules/navigator/navigator_params.c
+++ b/src/modules/navigator/navigator_params.c
@@ -104,36 +104,6 @@ PARAM_DEFINE_INT32(NAV_DLL_ACT, 0);
  */
 PARAM_DEFINE_INT32(NAV_RCL_ACT, 0);
 
-/**
- * Set offboard loss failsafe mode
- *
- * The offboard loss failsafe will only be entered after a timeout,
- * set by COM_OF_LOSS_T in seconds.
- *
- * @value 0 Land at current position
- * @value 1 Loiter
- * @value 2 Return to Land
- *
- * @group Mission
- */
-PARAM_DEFINE_INT32(NAV_OBL_ACT, 0);
-
-/**
- * Set offboard loss failsafe mode when RC is available
- *
- * The offboard loss failsafe will only be entered after a timeout,
- * set by COM_OF_LOSS_T in seconds.
- *
- * @value 0 Position control
- * @value 1 Altitude control
- * @value 2 Manual
- * @value 3 Return to Land
- * @value 4 Land at current position
- *
- * @group Mission
- */
-PARAM_DEFINE_INT32(NAV_OBL_RC_ACT, 0);
-
 /**
  * Airfield home Lat
  *

From 1aeb1391575532986e704d320b22d663d9b1bada Mon Sep 17 00:00:00 2001
From: Andreas Antener <antener_a@gmx.ch>
Date: Thu, 7 Apr 2016 20:17:51 +0200
Subject: [PATCH 0702/1230] added motor ramp app, draft impl

---
 cmake/configs/posix_sitl_default.cmake   |   1 +
 src/systemcmds/motor_ramp/CMakeLists.txt |  44 ++++
 src/systemcmds/motor_ramp/motor_ramp.cpp | 299 +++++++++++++++++++++++
 3 files changed, 344 insertions(+)
 create mode 100644 src/systemcmds/motor_ramp/CMakeLists.txt
 create mode 100644 src/systemcmds/motor_ramp/motor_ramp.cpp

diff --git a/cmake/configs/posix_sitl_default.cmake b/cmake/configs/posix_sitl_default.cmake
index 3adc2a425c..6457ea5a2b 100644
--- a/cmake/configs/posix_sitl_default.cmake
+++ b/cmake/configs/posix_sitl_default.cmake
@@ -30,6 +30,7 @@ set(config_module_list
 	systemcmds/topic_listener
 	systemcmds/ver
 	systemcmds/top
+	systemcmds/motor_ramp
 
 	modules/attitude_estimator_ekf
 	modules/attitude_estimator_q
diff --git a/src/systemcmds/motor_ramp/CMakeLists.txt b/src/systemcmds/motor_ramp/CMakeLists.txt
new file mode 100644
index 0000000000..601d4a9199
--- /dev/null
+++ b/src/systemcmds/motor_ramp/CMakeLists.txt
@@ -0,0 +1,44 @@
+############################################################################
+#
+#   Copyright (c) 2015 PX4 Development Team. All rights reserved.
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions
+# are met:
+#
+# 1. Redistributions of source code must retain the above copyright
+#    notice, this list of conditions and the following disclaimer.
+# 2. Redistributions in binary form must reproduce the above copyright
+#    notice, this list of conditions and the following disclaimer in
+#    the documentation and/or other materials provided with the
+#    distribution.
+# 3. Neither the name PX4 nor the names of its contributors may be
+#    used to endorse or promote products derived from this software
+#    without specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.
+#
+############################################################################
+px4_add_module(
+    MODULE systemcmds__motor_ramp
+    MAIN motor_ramp
+    STACK 1200
+    COMPILE_FLAGS
+        -Wno-write-strings
+    SRCS
+        motor_ramp.cpp
+    DEPENDS
+        platforms__common
+    )
+# vim: set noet ft=cmake fenc=utf-8 ff=unix : 
diff --git a/src/systemcmds/motor_ramp/motor_ramp.cpp b/src/systemcmds/motor_ramp/motor_ramp.cpp
new file mode 100644
index 0000000000..bdaa319b68
--- /dev/null
+++ b/src/systemcmds/motor_ramp/motor_ramp.cpp
@@ -0,0 +1,299 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2012-2015 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file motor_ramp.cpp
+ * Application to test motor ramp up.
+ *
+ * @author Andreas Antener <andreas@uaventure.com>
+ */
+
+#include <px4_config.h>
+#include <px4_defines.h>
+#include <px4_tasks.h>
+#include <px4_posix.h>
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+#include <unistd.h>
+#include <errno.h>
+#include <math.h>
+#include <poll.h>
+
+#include <arch/board/board.h>
+#include <drivers/drv_hrt.h>
+#include <drivers/drv_pwm_output.h>
+#include <uORB/uORB.h>
+#include <uORB/topics/actuator_outputs.h>
+#include <uORB/topics/control_state.h>
+#include <platforms/px4_defines.h>
+
+#include "systemlib/systemlib.h"
+#include "systemlib/err.h"
+
+static bool thread_should_exit = false;		/**< motor_ramp exit flag */
+static bool thread_running = false;		/**< motor_ramp status flag */
+static int motor_ramp_task;				/**< Handle of motor_ramp task / thread */
+
+enum RampState {
+	RAMP_INIT,
+	RAMP_MIN,
+	RAMP_RAMP,
+	RAMP_WAIT
+};
+
+/**
+ * motor_ramp management function.
+ */
+extern "C" __EXPORT int motor_ramp_main(int argc, char *argv[]);
+
+/**
+ * Mainloop of motor_ramp.
+ */
+int motor_ramp_thread_main(int argc, char *argv[]);
+
+/**
+ * Print the correct usage.
+ */
+static void usage(const char *reason);
+
+static void
+usage(const char *reason)
+{
+	if (reason) {
+		PX4_WARN("%s\n", reason);
+	}
+
+	PX4_WARN("usage: motor_ramp {start|stop|status} [-p <additional params>]\n\n");
+}
+
+/**
+ * The motor_ramp app only briefly exists to start
+ * the background job. The stack size assigned in the
+ * Makefile does only apply to this management task.
+ *
+ * The actual stack size should be set in the call
+ * to task_create().
+ */
+int motor_ramp_main(int argc, char *argv[])
+{
+	if (argc < 2) {
+		usage("missing command");
+		return 1;
+	}
+
+	if (!strcmp(argv[1], "start")) {
+
+		if (thread_running) {
+			PX4_WARN("motor_ramp already running\n");
+			/* this is not an error */
+			return 0;
+		}
+
+		thread_should_exit = false;
+		motor_ramp_task = px4_task_spawn_cmd("motor_ramp",
+						     SCHED_DEFAULT,
+						     SCHED_PRIORITY_DEFAULT,
+						     2000,
+						     motor_ramp_thread_main,
+						     (argv) ? (char *const *)&argv[2] : (char *const *)NULL);
+		return 0;
+	}
+
+	if (!strcmp(argv[1], "stop")) {
+		thread_should_exit = true;
+		return 0;
+	}
+
+	if (!strcmp(argv[1], "status")) {
+		if (thread_running) {
+			PX4_WARN("\trunning\n");
+
+		} else {
+			PX4_WARN("\tnot started\n");
+		}
+
+		return 0;
+	}
+
+	usage("unrecognized command");
+	return 1;
+}
+
+void set_min_pwm(unsigned pwm_value)
+{
+	int ret;
+	unsigned servo_count;
+	char *dev = PWM_OUTPUT0_DEVICE_PATH;
+	int fd = px4_open(dev, 0);
+
+	if (fd < 0) {PX4_WARN("can't open %s", dev);}
+
+	ret = px4_ioctl(fd, PWM_SERVO_GET_COUNT, (unsigned long)&servo_count);
+	struct pwm_output_values pwm_values;
+	memset(&pwm_values, 0, sizeof(pwm_values));
+
+	pwm_values.channel_count = servo_count;
+
+	for (int i = 0; i < servo_count; i++) {
+		pwm_values.values[i] = pwm_value;
+	}
+
+	ret = px4_ioctl(fd, PWM_SERVO_SET_MIN_PWM, (long unsigned int)&pwm_values);
+
+	if (ret != OK) {PX4_WARN("failed setting min values");}
+
+	px4_close(fd);
+}
+
+void publish(orb_advert_t actuator_outputs_pub, struct actuator_outputs_s &actuator_outputs, float output)
+{
+	//PX4_WARN("[motor_ramp] publish %f", (double)output);
+	for (int i = 0; i < actuator_outputs_s::NUM_ACTUATOR_OUTPUTS; i++) {
+		actuator_outputs.output[i] = output;
+	}
+	
+	orb_publish(ORB_ID(actuator_outputs), actuator_outputs_pub, &actuator_outputs);
+}
+
+int motor_ramp_thread_main(int argc, char *argv[])
+{
+	PX4_WARN("[motor_ramp] starting");
+
+	thread_running = true;
+
+	struct actuator_outputs_s actuator_outputs = {};
+	orb_advert_t actuator_outputs_pub = orb_advertise(ORB_ID(actuator_outputs), &actuator_outputs);
+	int ctrl_state_sub = orb_subscribe(ORB_ID(control_state));
+
+	/* wakeup source */
+	px4_pollfd_struct_t fds[1];
+
+	/* Setup of loop */
+	fds[0].fd = ctrl_state_sub;
+	fds[0].events = POLLIN;
+
+	float dt = 0.01f; // prevent division with 0
+	float timer = 0.0f;
+	hrt_abstime start = 0;
+	hrt_abstime last_run = 0;
+
+	enum RampState ramp_state = RAMP_INIT;
+	unsigned min_pwm = 1100;
+	float ramp_time = 0.5f;
+	float output = 0.0f;
+
+	publish(actuator_outputs_pub, actuator_outputs, 0.0f);
+
+	while (!thread_should_exit) {
+		if (last_run > 0) {
+			dt = hrt_elapsed_time(&last_run) * 1e-6;
+
+		} else {
+			start = hrt_absolute_time();
+		}
+
+		last_run = hrt_absolute_time();
+		timer = hrt_elapsed_time(&start) * 1e-6;
+
+		/* wait for up to 500ms for data */
+		int pret = px4_poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 100);
+
+		/* timed out - periodic check for _task_should_exit, etc. */
+		if (pret == 0) {
+			continue;
+		}
+
+		/* this is undesirable but not much we can do - might want to flag unhappy status */
+		if (pret < 0) {
+			PX4_WARN("[motor_ramp] poll error %d", pret);
+			continue;
+		}
+
+		switch (ramp_state) {
+		case RAMP_INIT: {
+				if (min_pwm > 1350) {
+					thread_should_exit = true;
+					PX4_WARN("[motor_ramp] pwm min too high %d", min_pwm);
+					break;
+				}
+
+				PX4_WARN("[motor_ramp] setting pwm min %d", min_pwm);
+				set_min_pwm(min_pwm);
+				ramp_state = RAMP_MIN;
+				break;
+			}
+
+		case RAMP_MIN: {
+				if (timer > 3.0f) {
+					PX4_WARN("[motor_ramp] min set, starting ramp");
+					start = hrt_absolute_time();
+					ramp_state = RAMP_RAMP;
+				}
+
+				break;
+			}
+
+		case RAMP_RAMP: {
+				output += dt / ramp_time;
+
+				if (output > 1.0f) {
+					output = 1.0f;
+					start = hrt_absolute_time();
+					ramp_state = RAMP_WAIT;
+					PX4_WARN("[motor_ramp] ramp finished, waiting");
+				}
+
+				publish(actuator_outputs_pub, actuator_outputs, output);
+				break;
+			}
+
+		case RAMP_WAIT: {
+				if (timer > 3.0f) {
+					thread_should_exit = true;
+					break;
+				}
+
+				break;
+			}
+		}
+	}
+
+	publish(actuator_outputs_pub, actuator_outputs, 0.0f);
+
+	PX4_WARN("[motor_ramp] exiting");
+
+	thread_running = false;
+
+	return 0;
+}

From 0b930a36b9e5d62584f4c14900273be77e964e79 Mon Sep 17 00:00:00 2001
From: Andreas Antener <antener_a@gmx.ch>
Date: Tue, 12 Apr 2016 21:07:09 +0200
Subject: [PATCH 0703/1230] added parameters to motor_ramp

---
 src/systemcmds/motor_ramp/motor_ramp.cpp | 121 ++++++++++++-----------
 1 file changed, 66 insertions(+), 55 deletions(-)

diff --git a/src/systemcmds/motor_ramp/motor_ramp.cpp b/src/systemcmds/motor_ramp/motor_ramp.cpp
index bdaa319b68..62d5cadefc 100644
--- a/src/systemcmds/motor_ramp/motor_ramp.cpp
+++ b/src/systemcmds/motor_ramp/motor_ramp.cpp
@@ -61,9 +61,12 @@
 #include "systemlib/systemlib.h"
 #include "systemlib/err.h"
 
-static bool thread_should_exit = false;		/**< motor_ramp exit flag */
-static bool thread_running = false;		/**< motor_ramp status flag */
-static int motor_ramp_task;				/**< Handle of motor_ramp task / thread */
+static bool _thread_should_exit = false;		/**< motor_ramp exit flag */
+static bool _thread_running = false;		/**< motor_ramp status flag */
+static int _motor_ramp_task;				/**< Handle of motor_ramp task / thread */
+static float _ramp_time;
+static int _min_pwm;
+static orb_advert_t _actuator_outputs_pub = nullptr;
 
 enum RampState {
 	RAMP_INIT,
@@ -82,6 +85,12 @@ extern "C" __EXPORT int motor_ramp_main(int argc, char *argv[]);
  */
 int motor_ramp_thread_main(int argc, char *argv[]);
 
+bool min_pwm_valid(unsigned pwm_value);
+
+void set_min_pwm(unsigned pwm_value);
+
+void publish(struct actuator_outputs_s &actuator_outputs, float output);
+
 /**
  * Print the correct usage.
  */
@@ -91,10 +100,11 @@ static void
 usage(const char *reason)
 {
 	if (reason) {
-		PX4_WARN("%s\n", reason);
+		PX4_WARN("[motor_ramp] %s", reason);
 	}
 
-	PX4_WARN("usage: motor_ramp {start|stop|status} [-p <additional params>]\n\n");
+	PX4_WARN("[motor_ramp] usage:   motor_ramp <min_pwm> <ramp_time>");
+	PX4_WARN("[motor_ramp] example: motor_ramp 1100 0.5\n");
 }
 
 /**
@@ -107,49 +117,48 @@ usage(const char *reason)
  */
 int motor_ramp_main(int argc, char *argv[])
 {
-	if (argc < 2) {
-		usage("missing command");
+	if (argc < 3) {
+		usage("missing parameters");
 		return 1;
 	}
 
-	if (!strcmp(argv[1], "start")) {
-
-		if (thread_running) {
-			PX4_WARN("motor_ramp already running\n");
-			/* this is not an error */
-			return 0;
-		}
-
-		thread_should_exit = false;
-		motor_ramp_task = px4_task_spawn_cmd("motor_ramp",
-						     SCHED_DEFAULT,
-						     SCHED_PRIORITY_DEFAULT,
-						     2000,
-						     motor_ramp_thread_main,
-						     (argv) ? (char *const *)&argv[2] : (char *const *)NULL);
+	if (_thread_running) {
+		PX4_WARN("motor_ramp already running\n");
+		/* this is not an error */
 		return 0;
 	}
 
-	if (!strcmp(argv[1], "stop")) {
-		thread_should_exit = true;
-		return 0;
+	_min_pwm = atoi(argv[1]);
+
+	if (!min_pwm_valid(_min_pwm)) {
+		usage("min pwm not in range");
+		return 1;
 	}
 
-	if (!strcmp(argv[1], "status")) {
-		if (thread_running) {
-			PX4_WARN("\trunning\n");
+	_ramp_time = strtof(argv[2], NULL);
 
-		} else {
-			PX4_WARN("\tnot started\n");
-		}
-
-		return 0;
+	if (!(_ramp_time > 0)) {
+		usage("ramp time must be greater than 0");
+		return 1;
 	}
 
+	_thread_should_exit = false;
+	_motor_ramp_task = px4_task_spawn_cmd("motor_ramp",
+					     SCHED_DEFAULT,
+					     SCHED_PRIORITY_DEFAULT,
+					     2000,
+					     motor_ramp_thread_main,
+					     (argv) ? (char *const *)&argv[2] : (char *const *)NULL);
+	return 0;
+
 	usage("unrecognized command");
 	return 1;
 }
 
+bool min_pwm_valid(unsigned pwm_value) {
+	return pwm_value > 900 && pwm_value < 1300;
+}
+
 void set_min_pwm(unsigned pwm_value)
 {
 	int ret;
@@ -157,7 +166,7 @@ void set_min_pwm(unsigned pwm_value)
 	char *dev = PWM_OUTPUT0_DEVICE_PATH;
 	int fd = px4_open(dev, 0);
 
-	if (fd < 0) {PX4_WARN("can't open %s", dev);}
+	if (fd < 0) {PX4_WARN("[motor_ramp] can't open %s", dev);}
 
 	ret = px4_ioctl(fd, PWM_SERVO_GET_COUNT, (unsigned long)&servo_count);
 	struct pwm_output_values pwm_values;
@@ -171,29 +180,35 @@ void set_min_pwm(unsigned pwm_value)
 
 	ret = px4_ioctl(fd, PWM_SERVO_SET_MIN_PWM, (long unsigned int)&pwm_values);
 
-	if (ret != OK) {PX4_WARN("failed setting min values");}
+	if (ret != OK) {PX4_WARN("[motor_ramp] failed setting min values");}
 
 	px4_close(fd);
 }
 
-void publish(orb_advert_t actuator_outputs_pub, struct actuator_outputs_s &actuator_outputs, float output)
+void publish(struct actuator_outputs_s &actuator_outputs, float output)
 {
 	//PX4_WARN("[motor_ramp] publish %f", (double)output);
 	for (int i = 0; i < actuator_outputs_s::NUM_ACTUATOR_OUTPUTS; i++) {
 		actuator_outputs.output[i] = output;
 	}
-	
-	orb_publish(ORB_ID(actuator_outputs), actuator_outputs_pub, &actuator_outputs);
+
+	if (_actuator_outputs_pub == nullptr) {
+		_actuator_outputs_pub = orb_advertise(ORB_ID(actuator_outputs), &actuator_outputs);
+
+	} else {
+		orb_publish(ORB_ID(actuator_outputs), _actuator_outputs_pub, &actuator_outputs);
+	}
 }
 
 int motor_ramp_thread_main(int argc, char *argv[])
 {
 	PX4_WARN("[motor_ramp] starting");
 
-	thread_running = true;
+	_thread_running = true;
 
 	struct actuator_outputs_s actuator_outputs = {};
-	orb_advert_t actuator_outputs_pub = orb_advertise(ORB_ID(actuator_outputs), &actuator_outputs);
+	publish(actuator_outputs, 0.0f);
+
 	int ctrl_state_sub = orb_subscribe(ORB_ID(control_state));
 
 	/* wakeup source */
@@ -209,13 +224,9 @@ int motor_ramp_thread_main(int argc, char *argv[])
 	hrt_abstime last_run = 0;
 
 	enum RampState ramp_state = RAMP_INIT;
-	unsigned min_pwm = 1100;
-	float ramp_time = 0.5f;
 	float output = 0.0f;
 
-	publish(actuator_outputs_pub, actuator_outputs, 0.0f);
-
-	while (!thread_should_exit) {
+	while (!_thread_should_exit) {
 		if (last_run > 0) {
 			dt = hrt_elapsed_time(&last_run) * 1e-6;
 
@@ -242,14 +253,14 @@ int motor_ramp_thread_main(int argc, char *argv[])
 
 		switch (ramp_state) {
 		case RAMP_INIT: {
-				if (min_pwm > 1350) {
-					thread_should_exit = true;
-					PX4_WARN("[motor_ramp] pwm min too high %d", min_pwm);
+				if (!min_pwm_valid(_min_pwm)) {
+					_thread_should_exit = true;
+					PX4_WARN("[motor_ramp] min pwm not in range %d", _min_pwm);
 					break;
 				}
 
-				PX4_WARN("[motor_ramp] setting pwm min %d", min_pwm);
-				set_min_pwm(min_pwm);
+				PX4_WARN("[motor_ramp] setting pwm min %d", _min_pwm);
+				set_min_pwm(_min_pwm);
 				ramp_state = RAMP_MIN;
 				break;
 			}
@@ -265,7 +276,7 @@ int motor_ramp_thread_main(int argc, char *argv[])
 			}
 
 		case RAMP_RAMP: {
-				output += dt / ramp_time;
+				output += dt / _ramp_time;
 
 				if (output > 1.0f) {
 					output = 1.0f;
@@ -274,13 +285,13 @@ int motor_ramp_thread_main(int argc, char *argv[])
 					PX4_WARN("[motor_ramp] ramp finished, waiting");
 				}
 
-				publish(actuator_outputs_pub, actuator_outputs, output);
+				publish(actuator_outputs, output);
 				break;
 			}
 
 		case RAMP_WAIT: {
 				if (timer > 3.0f) {
-					thread_should_exit = true;
+					_thread_should_exit = true;
 					break;
 				}
 
@@ -289,11 +300,11 @@ int motor_ramp_thread_main(int argc, char *argv[])
 		}
 	}
 
-	publish(actuator_outputs_pub, actuator_outputs, 0.0f);
+	publish(actuator_outputs, 0.0f);
 
 	PX4_WARN("[motor_ramp] exiting");
 
-	thread_running = false;
+	_thread_running = false;
 
 	return 0;
 }

From fabb37975d703565d3d7261da073b316c9adb82a Mon Sep 17 00:00:00 2001
From: Andreas Antener <antener_a@gmx.ch>
Date: Tue, 12 Apr 2016 21:24:32 +0200
Subject: [PATCH 0704/1230] added motor_ramp to v2 config

---
 cmake/configs/nuttx_px4fmu-v2_default.cmake | 1 +
 src/systemcmds/motor_ramp/motor_ramp.cpp    | 2 +-
 2 files changed, 2 insertions(+), 1 deletion(-)

diff --git a/cmake/configs/nuttx_px4fmu-v2_default.cmake b/cmake/configs/nuttx_px4fmu-v2_default.cmake
index 8f5f7f5e7e..1699e6df9d 100644
--- a/cmake/configs/nuttx_px4fmu-v2_default.cmake
+++ b/cmake/configs/nuttx_px4fmu-v2_default.cmake
@@ -68,6 +68,7 @@ set(config_module_list
 	systemcmds/ver
 	#systemcmds/sd_bench
 	#systemcmds/tests
+	systemcmds/motor_ramp
 
 	#
 	# General system control
diff --git a/src/systemcmds/motor_ramp/motor_ramp.cpp b/src/systemcmds/motor_ramp/motor_ramp.cpp
index 62d5cadefc..95df5fdf54 100644
--- a/src/systemcmds/motor_ramp/motor_ramp.cpp
+++ b/src/systemcmds/motor_ramp/motor_ramp.cpp
@@ -135,7 +135,7 @@ int motor_ramp_main(int argc, char *argv[])
 		return 1;
 	}
 
-	_ramp_time = strtof(argv[2], NULL);
+	_ramp_time = atof(argv[2]);
 
 	if (!(_ramp_time > 0)) {
 		usage("ramp time must be greater than 0");

From ede032c55703072501a0c2136e743b699b300f23 Mon Sep 17 00:00:00 2001
From: Andreas Antener <antener_a@gmx.ch>
Date: Tue, 12 Apr 2016 23:09:28 +0200
Subject: [PATCH 0705/1230] ouput pwm values directly

---
 src/systemcmds/motor_ramp/motor_ramp.cpp | 145 +++++++++++++----------
 1 file changed, 83 insertions(+), 62 deletions(-)

diff --git a/src/systemcmds/motor_ramp/motor_ramp.cpp b/src/systemcmds/motor_ramp/motor_ramp.cpp
index 95df5fdf54..076e6e1e47 100644
--- a/src/systemcmds/motor_ramp/motor_ramp.cpp
+++ b/src/systemcmds/motor_ramp/motor_ramp.cpp
@@ -53,9 +53,6 @@
 #include <arch/board/board.h>
 #include <drivers/drv_hrt.h>
 #include <drivers/drv_pwm_output.h>
-#include <uORB/uORB.h>
-#include <uORB/topics/actuator_outputs.h>
-#include <uORB/topics/control_state.h>
 #include <platforms/px4_defines.h>
 
 #include "systemlib/systemlib.h"
@@ -66,7 +63,6 @@ static bool _thread_running = false;		/**< motor_ramp status flag */
 static int _motor_ramp_task;				/**< Handle of motor_ramp task / thread */
 static float _ramp_time;
 static int _min_pwm;
-static orb_advert_t _actuator_outputs_pub = nullptr;
 
 enum RampState {
 	RAMP_INIT,
@@ -87,9 +83,11 @@ int motor_ramp_thread_main(int argc, char *argv[]);
 
 bool min_pwm_valid(unsigned pwm_value);
 
-void set_min_pwm(unsigned pwm_value);
+int set_min_pwm(int fd, unsigned long max_channels, unsigned pwm_value);
 
-void publish(struct actuator_outputs_s &actuator_outputs, float output);
+int set_out(int fd, unsigned long max_channels, float output);
+
+int prepare(int fd, unsigned long *max_channels);
 
 /**
  * Print the correct usage.
@@ -144,60 +142,92 @@ int motor_ramp_main(int argc, char *argv[])
 
 	_thread_should_exit = false;
 	_motor_ramp_task = px4_task_spawn_cmd("motor_ramp",
-					     SCHED_DEFAULT,
-					     SCHED_PRIORITY_DEFAULT,
-					     2000,
-					     motor_ramp_thread_main,
-					     (argv) ? (char *const *)&argv[2] : (char *const *)NULL);
+					      SCHED_DEFAULT,
+					      SCHED_PRIORITY_DEFAULT,
+					      2000,
+					      motor_ramp_thread_main,
+					      (argv) ? (char *const *)&argv[2] : (char *const *)NULL);
 	return 0;
 
 	usage("unrecognized command");
 	return 1;
 }
 
-bool min_pwm_valid(unsigned pwm_value) {
+bool min_pwm_valid(unsigned pwm_value)
+{
 	return pwm_value > 900 && pwm_value < 1300;
 }
 
-void set_min_pwm(unsigned pwm_value)
+int set_min_pwm(int fd, unsigned long max_channels, unsigned pwm_value)
 {
 	int ret;
-	unsigned servo_count;
-	char *dev = PWM_OUTPUT0_DEVICE_PATH;
-	int fd = px4_open(dev, 0);
 
-	if (fd < 0) {PX4_WARN("[motor_ramp] can't open %s", dev);}
-
-	ret = px4_ioctl(fd, PWM_SERVO_GET_COUNT, (unsigned long)&servo_count);
 	struct pwm_output_values pwm_values;
 	memset(&pwm_values, 0, sizeof(pwm_values));
 
-	pwm_values.channel_count = servo_count;
+	pwm_values.channel_count = max_channels;
 
-	for (int i = 0; i < servo_count; i++) {
+	for (int i = 0; i < max_channels; i++) {
 		pwm_values.values[i] = pwm_value;
 	}
 
 	ret = px4_ioctl(fd, PWM_SERVO_SET_MIN_PWM, (long unsigned int)&pwm_values);
 
-	if (ret != OK) {PX4_WARN("[motor_ramp] failed setting min values");}
+	if (ret != OK) {
+		PX4_ERR("[motor_ramp] failed setting min values");
+		return 1;
+	}
 
-	px4_close(fd);
+	return 0;
 }
 
-void publish(struct actuator_outputs_s &actuator_outputs, float output)
+int set_out(int fd, unsigned long max_channels, float output)
 {
-	//PX4_WARN("[motor_ramp] publish %f", (double)output);
-	for (int i = 0; i < actuator_outputs_s::NUM_ACTUATOR_OUTPUTS; i++) {
-		actuator_outputs.output[i] = output;
+	int ret;
+	int pwm = (2000 - _min_pwm) * output + _min_pwm;
+
+	for (unsigned i = 0; i < max_channels; i++) {
+
+		ret = ioctl(fd, PWM_SERVO_SET(i), pwm);
+
+		if (ret != OK) {
+			PX4_ERR("PWM_SERVO_SET(%d), value: %d", i, pwm);
+			return 1;
+		}
 	}
 
-	if (_actuator_outputs_pub == nullptr) {
-		_actuator_outputs_pub = orb_advertise(ORB_ID(actuator_outputs), &actuator_outputs);
+	return 0;
+}
 
-	} else {
-		orb_publish(ORB_ID(actuator_outputs), _actuator_outputs_pub, &actuator_outputs);
+int prepare(int fd, unsigned long *max_channels)
+{
+	int ret;
+
+	/* get number of channels available on the device */
+	ret = ioctl(fd, PWM_SERVO_GET_COUNT, (unsigned long)max_channels);
+
+	if (ret != OK) {
+		PX4_ERR("[motor_ramp] PWM_SERVO_GET_COUNT");
+		return 1;
 	}
+
+	/* tell IO/FMU that its ok to disable its safety with the switch */
+	ret = ioctl(fd, PWM_SERVO_SET_ARM_OK, 0);
+
+	if (ret != OK) {
+		PX4_ERR("[motor_ramp] PWM_SERVO_SET_ARM_OK");
+		return 1;
+	}
+
+	/* tell IO/FMU that the system is armed (it will output values if safety is off) */
+	ret = ioctl(fd, PWM_SERVO_ARM, 0);
+
+	if (ret != OK) {
+		PX4_ERR("[motor_ramp] PWM_SERVO_ARM");
+		return 1;
+	}
+
+	return 0;
 }
 
 int motor_ramp_thread_main(int argc, char *argv[])
@@ -206,17 +236,22 @@ int motor_ramp_thread_main(int argc, char *argv[])
 
 	_thread_running = true;
 
-	struct actuator_outputs_s actuator_outputs = {};
-	publish(actuator_outputs, 0.0f);
+	char *dev = PWM_OUTPUT0_DEVICE_PATH;
+	unsigned long max_channels = 0;
 
-	int ctrl_state_sub = orb_subscribe(ORB_ID(control_state));
+	int fd = px4_open(dev, 0);
 
-	/* wakeup source */
-	px4_pollfd_struct_t fds[1];
+	if (fd < 0) {
+		PX4_ERR("[motor_ramp] can't open %s", dev);
+	}
 
-	/* Setup of loop */
-	fds[0].fd = ctrl_state_sub;
-	fds[0].events = POLLIN;
+	if (prepare(fd, &max_channels) != OK) {
+		_thread_should_exit = true;
+	}
+
+	PX4_WARN("[motor_ramp] max chan %d", max_channels);
+
+	set_out(fd, max_channels, 0.0f);
 
 	float dt = 0.01f; // prevent division with 0
 	float timer = 0.0f;
@@ -237,30 +272,10 @@ int motor_ramp_thread_main(int argc, char *argv[])
 		last_run = hrt_absolute_time();
 		timer = hrt_elapsed_time(&start) * 1e-6;
 
-		/* wait for up to 500ms for data */
-		int pret = px4_poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 100);
-
-		/* timed out - periodic check for _task_should_exit, etc. */
-		if (pret == 0) {
-			continue;
-		}
-
-		/* this is undesirable but not much we can do - might want to flag unhappy status */
-		if (pret < 0) {
-			PX4_WARN("[motor_ramp] poll error %d", pret);
-			continue;
-		}
-
 		switch (ramp_state) {
 		case RAMP_INIT: {
-				if (!min_pwm_valid(_min_pwm)) {
-					_thread_should_exit = true;
-					PX4_WARN("[motor_ramp] min pwm not in range %d", _min_pwm);
-					break;
-				}
-
 				PX4_WARN("[motor_ramp] setting pwm min %d", _min_pwm);
-				set_min_pwm(_min_pwm);
+				set_min_pwm(fd, max_channels, _min_pwm);
 				ramp_state = RAMP_MIN;
 				break;
 			}
@@ -285,7 +300,7 @@ int motor_ramp_thread_main(int argc, char *argv[])
 					PX4_WARN("[motor_ramp] ramp finished, waiting");
 				}
 
-				publish(actuator_outputs, output);
+				set_out(fd, max_channels, output);
 				break;
 			}
 
@@ -298,9 +313,15 @@ int motor_ramp_thread_main(int argc, char *argv[])
 				break;
 			}
 		}
+
+		// rate limit
+		usleep(2000);
 	}
 
-	publish(actuator_outputs, 0.0f);
+	if (fd >= 0) {
+		set_out(fd, max_channels, 0.0f);
+		px4_close(fd);
+	}
 
 	PX4_WARN("[motor_ramp] exiting");
 

From d8cdb2032cc40befaa94db930f365394fb3b002e Mon Sep 17 00:00:00 2001
From: Andreas Antener <antener_a@gmx.ch>
Date: Wed, 13 Apr 2016 17:46:20 +0200
Subject: [PATCH 0706/1230] constantly set outputs for ramp, otherwise the ESC
 doesn't keep it's setting (why?)

---
 src/systemcmds/motor_ramp/motor_ramp.cpp | 28 +++++++++++++-----------
 1 file changed, 15 insertions(+), 13 deletions(-)

diff --git a/src/systemcmds/motor_ramp/motor_ramp.cpp b/src/systemcmds/motor_ramp/motor_ramp.cpp
index 076e6e1e47..758dbc6b82 100644
--- a/src/systemcmds/motor_ramp/motor_ramp.cpp
+++ b/src/systemcmds/motor_ramp/motor_ramp.cpp
@@ -201,32 +201,30 @@ int set_out(int fd, unsigned long max_channels, float output)
 
 int prepare(int fd, unsigned long *max_channels)
 {
-	int ret;
-
 	/* get number of channels available on the device */
-	ret = ioctl(fd, PWM_SERVO_GET_COUNT, (unsigned long)max_channels);
-
-	if (ret != OK) {
+	if (px4_ioctl(fd, PWM_SERVO_GET_COUNT, (unsigned long)max_channels) != OK) {
 		PX4_ERR("[motor_ramp] PWM_SERVO_GET_COUNT");
 		return 1;
 	}
 
 	/* tell IO/FMU that its ok to disable its safety with the switch */
-	ret = ioctl(fd, PWM_SERVO_SET_ARM_OK, 0);
-
-	if (ret != OK) {
+	if (px4_ioctl(fd, PWM_SERVO_SET_ARM_OK, 0) != OK) {
 		PX4_ERR("[motor_ramp] PWM_SERVO_SET_ARM_OK");
 		return 1;
 	}
 
 	/* tell IO/FMU that the system is armed (it will output values if safety is off) */
-	ret = ioctl(fd, PWM_SERVO_ARM, 0);
-
-	if (ret != OK) {
+	if (px4_ioctl(fd, PWM_SERVO_ARM, 0) != OK) {
 		PX4_ERR("[motor_ramp] PWM_SERVO_ARM");
 		return 1;
 	}
 
+	/* tell IO to switch off safety without using the safety switch */
+	if (px4_ioctl(fd, PWM_SERVO_SET_FORCE_SAFETY_OFF, 0) != OK) {
+		PX4_ERR("[motor_ramp] PWM_SERVO_SET_FORCE_SAFETY_OFF");
+		return 1;
+	}
+
 	return 0;
 }
 
@@ -282,11 +280,12 @@ int motor_ramp_thread_main(int argc, char *argv[])
 
 		case RAMP_MIN: {
 				if (timer > 3.0f) {
-					PX4_WARN("[motor_ramp] min set, starting ramp");
+					PX4_WARN("[motor_ramp] starting ramp");
 					start = hrt_absolute_time();
 					ramp_state = RAMP_RAMP;
 				}
 
+				set_out(fd, max_channels, output);
 				break;
 			}
 
@@ -310,6 +309,7 @@ int motor_ramp_thread_main(int argc, char *argv[])
 					break;
 				}
 
+				set_out(fd, max_channels, output);
 				break;
 			}
 		}
@@ -319,7 +319,9 @@ int motor_ramp_thread_main(int argc, char *argv[])
 	}
 
 	if (fd >= 0) {
-		set_out(fd, max_channels, 0.0f);
+		/* disarm */
+		ioctl(fd, PWM_SERVO_DISARM, 0);
+
 		px4_close(fd);
 	}
 

From a7834693e875ac4122cbbd4a7dd9e4a7b6b9fc46 Mon Sep 17 00:00:00 2001
From: Andreas Antener <antener_a@gmx.ch>
Date: Wed, 13 Apr 2016 18:21:32 +0200
Subject: [PATCH 0707/1230] updated usage, increased prio and lowered max hold
 time

---
 src/systemcmds/motor_ramp/motor_ramp.cpp | 11 +++++++----
 1 file changed, 7 insertions(+), 4 deletions(-)

diff --git a/src/systemcmds/motor_ramp/motor_ramp.cpp b/src/systemcmds/motor_ramp/motor_ramp.cpp
index 758dbc6b82..d0cbbe81f3 100644
--- a/src/systemcmds/motor_ramp/motor_ramp.cpp
+++ b/src/systemcmds/motor_ramp/motor_ramp.cpp
@@ -101,8 +101,11 @@ usage(const char *reason)
 		PX4_WARN("[motor_ramp] %s", reason);
 	}
 
-	PX4_WARN("[motor_ramp] usage:   motor_ramp <min_pwm> <ramp_time>");
-	PX4_WARN("[motor_ramp] example: motor_ramp 1100 0.5\n");
+	PX4_WARN("[motor_ramp] usage: motor_ramp <min_pwm> <ramp_time>");
+	PX4_WARN("[motor_ramp] example:\n");
+	PX4_WARN("[motor_ramp] nsh> sdlog2 on");
+	PX4_WARN("[motor_ramp] nsh> mc_att_control stop");
+	PX4_WARN("[motor_ramp] nsh> motor_ramp 1100 0.5\n");
 }
 
 /**
@@ -143,7 +146,7 @@ int motor_ramp_main(int argc, char *argv[])
 	_thread_should_exit = false;
 	_motor_ramp_task = px4_task_spawn_cmd("motor_ramp",
 					      SCHED_DEFAULT,
-					      SCHED_PRIORITY_DEFAULT,
+					      SCHED_PRIORITY_DEFAULT + 40,
 					      2000,
 					      motor_ramp_thread_main,
 					      (argv) ? (char *const *)&argv[2] : (char *const *)NULL);
@@ -304,7 +307,7 @@ int motor_ramp_thread_main(int argc, char *argv[])
 			}
 
 		case RAMP_WAIT: {
-				if (timer > 3.0f) {
+				if (timer > 2.0f) {
 					_thread_should_exit = true;
 					break;
 				}

From 7c76c7c25ac5c950d0cca1c6afb5b16ff9183dae Mon Sep 17 00:00:00 2001
From: Andreas Antener <antener_a@gmx.ch>
Date: Thu, 14 Apr 2016 11:43:04 +0200
Subject: [PATCH 0708/1230] added warning, set hold time to 1 sec

---
 src/systemcmds/motor_ramp/motor_ramp.cpp | 13 +++++++------
 1 file changed, 7 insertions(+), 6 deletions(-)

diff --git a/src/systemcmds/motor_ramp/motor_ramp.cpp b/src/systemcmds/motor_ramp/motor_ramp.cpp
index d0cbbe81f3..e0056bd70c 100644
--- a/src/systemcmds/motor_ramp/motor_ramp.cpp
+++ b/src/systemcmds/motor_ramp/motor_ramp.cpp
@@ -98,11 +98,12 @@ static void
 usage(const char *reason)
 {
 	if (reason) {
-		PX4_WARN("[motor_ramp] %s", reason);
+		PX4_ERR("[motor_ramp] %s", reason);
 	}
 
+	PX4_WARN("[motor_ramp]\n\nWARNING: motors will ramp up to full speed!\n");
 	PX4_WARN("[motor_ramp] usage: motor_ramp <min_pwm> <ramp_time>");
-	PX4_WARN("[motor_ramp] example:\n");
+	PX4_WARN("[motor_ramp] example:");
 	PX4_WARN("[motor_ramp] nsh> sdlog2 on");
 	PX4_WARN("[motor_ramp] nsh> mc_att_control stop");
 	PX4_WARN("[motor_ramp] nsh> motor_ramp 1100 0.5\n");
@@ -250,7 +251,7 @@ int motor_ramp_thread_main(int argc, char *argv[])
 		_thread_should_exit = true;
 	}
 
-	PX4_WARN("[motor_ramp] max chan %d", max_channels);
+	PX4_WARN("[motor_ramp] max chan: %d", max_channels);
 
 	set_out(fd, max_channels, 0.0f);
 
@@ -275,7 +276,7 @@ int motor_ramp_thread_main(int argc, char *argv[])
 
 		switch (ramp_state) {
 		case RAMP_INIT: {
-				PX4_WARN("[motor_ramp] setting pwm min %d", _min_pwm);
+				PX4_WARN("[motor_ramp] setting pwm min: %d", _min_pwm);
 				set_min_pwm(fd, max_channels, _min_pwm);
 				ramp_state = RAMP_MIN;
 				break;
@@ -283,7 +284,7 @@ int motor_ramp_thread_main(int argc, char *argv[])
 
 		case RAMP_MIN: {
 				if (timer > 3.0f) {
-					PX4_WARN("[motor_ramp] starting ramp");
+					PX4_WARN("[motor_ramp] starting ramp: %.2f sec", (double)_ramp_time);
 					start = hrt_absolute_time();
 					ramp_state = RAMP_RAMP;
 				}
@@ -307,7 +308,7 @@ int motor_ramp_thread_main(int argc, char *argv[])
 			}
 
 		case RAMP_WAIT: {
-				if (timer > 2.0f) {
+				if (timer > 1.0f) {
 					_thread_should_exit = true;
 					break;
 				}

From b3c3d6f88f21feb518bab25e0e3020a2c84753c0 Mon Sep 17 00:00:00 2001
From: Andreas Antener <antener_a@gmx.ch>
Date: Wed, 20 Apr 2016 09:27:41 +0200
Subject: [PATCH 0709/1230] fixed format error

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

diff --git a/src/systemcmds/motor_ramp/motor_ramp.cpp b/src/systemcmds/motor_ramp/motor_ramp.cpp
index e0056bd70c..68858870d5 100644
--- a/src/systemcmds/motor_ramp/motor_ramp.cpp
+++ b/src/systemcmds/motor_ramp/motor_ramp.cpp
@@ -251,7 +251,7 @@ int motor_ramp_thread_main(int argc, char *argv[])
 		_thread_should_exit = true;
 	}
 
-	PX4_WARN("[motor_ramp] max chan: %d", max_channels);
+	PX4_WARN("[motor_ramp] max chan: %lu", max_channels);
 
 	set_out(fd, max_channels, 0.0f);
 

From 87dc996a4109515f4722c57575aae94dc9e2bfe1 Mon Sep 17 00:00:00 2001
From: Roman <bapstr@ethz.ch>
Date: Mon, 25 Apr 2016 22:39:19 +0200
Subject: [PATCH 0710/1230] added option to output sine wave

---
 src/systemcmds/motor_ramp/motor_ramp.cpp | 31 +++++++++++++++++++-----
 1 file changed, 25 insertions(+), 6 deletions(-)

diff --git a/src/systemcmds/motor_ramp/motor_ramp.cpp b/src/systemcmds/motor_ramp/motor_ramp.cpp
index 68858870d5..b937359376 100644
--- a/src/systemcmds/motor_ramp/motor_ramp.cpp
+++ b/src/systemcmds/motor_ramp/motor_ramp.cpp
@@ -36,6 +36,7 @@
  * Application to test motor ramp up.
  *
  * @author Andreas Antener <andreas@uaventure.com>
+ * @author Roman Bapst <bapstroman@gmail.com>
  */
 
 #include <px4_config.h>
@@ -63,6 +64,8 @@ static bool _thread_running = false;		/**< motor_ramp status flag */
 static int _motor_ramp_task;				/**< Handle of motor_ramp task / thread */
 static float _ramp_time;
 static int _min_pwm;
+static bool _sine_output = false;
+
 
 enum RampState {
 	RAMP_INIT,
@@ -102,7 +105,8 @@ usage(const char *reason)
 	}
 
 	PX4_WARN("[motor_ramp]\n\nWARNING: motors will ramp up to full speed!\n");
-	PX4_WARN("[motor_ramp] usage: motor_ramp <min_pwm> <ramp_time>");
+	PX4_WARN("[motor_ramp] usage: motor_ramp <min_pwm> <ramp_time> <-s>");
+	PX4_WARN("[motor_ramp] setting option <-s> will enable sinus output with period <ramp_time>");
 	PX4_WARN("[motor_ramp] example:");
 	PX4_WARN("[motor_ramp] nsh> sdlog2 on");
 	PX4_WARN("[motor_ramp] nsh> mc_att_control stop");
@@ -139,6 +143,13 @@ int motor_ramp_main(int argc, char *argv[])
 
 	_ramp_time = atof(argv[2]);
 
+	// check if we should apply sine ouput instead of ramp
+	if (argc > 3) {
+		if (strcmp(argv[3], "-s") == 0) {
+			_sine_output = true;
+		}
+	}
+
 	if (!(_ramp_time > 0)) {
 		usage("ramp time must be greater than 0");
 		return 1;
@@ -284,7 +295,7 @@ int motor_ramp_thread_main(int argc, char *argv[])
 
 		case RAMP_MIN: {
 				if (timer > 3.0f) {
-					PX4_WARN("[motor_ramp] starting ramp: %.2f sec", (double)_ramp_time);
+					PX4_WARN("[motor_ramp] starting %s: %.2f sec", _sine_output ? "sine" : "ramp", (double)_ramp_time);
 					start = hrt_absolute_time();
 					ramp_state = RAMP_RAMP;
 				}
@@ -294,13 +305,21 @@ int motor_ramp_thread_main(int argc, char *argv[])
 			}
 
 		case RAMP_RAMP: {
-				output += dt / _ramp_time;
+				if (!_sine_output) {
+					output += dt / _ramp_time;
 
-				if (output > 1.0f) {
-					output = 1.0f;
+				} else {
+					// sine outpout with period T = _ramp_time and magnitude between [0,1]
+					// phase shift makes sure that it starts at zero when timer is zero
+					output = 0.5f * (1.0f + sinf(M_TWOPI_F * timer / (_ramp_time * 1e6f) - M_PI_2_F));
+				}
+
+				if ((output > 1.0f && !_sine_output) || (timer > 3.0f * _ramp_time * 1e6f && _sine_output)) {
+					// for ramp mode we set output to 1, for sine mode we just leave it as is
+					output = _sine_output ? output : 1.0f;
 					start = hrt_absolute_time();
 					ramp_state = RAMP_WAIT;
-					PX4_WARN("[motor_ramp] ramp finished, waiting");
+					PX4_WARN("[motor_ramp] %s finished, waiting", _sine_output ? "sine" : "ramp");
 				}
 
 				set_out(fd, max_channels, output);

From 7f56961e2637e67267c9fc76728795e86cd81eb1 Mon Sep 17 00:00:00 2001
From: Andreas Antener <antener_a@gmx.ch>
Date: Mon, 20 Jun 2016 19:49:43 +0200
Subject: [PATCH 0711/1230] removed unnecessary output texts

---
 src/systemcmds/motor_ramp/motor_ramp.cpp | 40 ++++++++++++------------
 1 file changed, 20 insertions(+), 20 deletions(-)

diff --git a/src/systemcmds/motor_ramp/motor_ramp.cpp b/src/systemcmds/motor_ramp/motor_ramp.cpp
index b937359376..048533e9c5 100644
--- a/src/systemcmds/motor_ramp/motor_ramp.cpp
+++ b/src/systemcmds/motor_ramp/motor_ramp.cpp
@@ -101,16 +101,16 @@ static void
 usage(const char *reason)
 {
 	if (reason) {
-		PX4_ERR("[motor_ramp] %s", reason);
+		PX4_ERR("%s", reason);
 	}
 
-	PX4_WARN("[motor_ramp]\n\nWARNING: motors will ramp up to full speed!\n");
-	PX4_WARN("[motor_ramp] usage: motor_ramp <min_pwm> <ramp_time> <-s>");
-	PX4_WARN("[motor_ramp] setting option <-s> will enable sinus output with period <ramp_time>");
-	PX4_WARN("[motor_ramp] example:");
-	PX4_WARN("[motor_ramp] nsh> sdlog2 on");
-	PX4_WARN("[motor_ramp] nsh> mc_att_control stop");
-	PX4_WARN("[motor_ramp] nsh> motor_ramp 1100 0.5\n");
+	PX4_WARN("\n\nWARNING: motors will ramp up to full speed!\n");
+	PX4_WARN("Usage: motor_ramp <min_pwm> <ramp_time> <-s>");
+	PX4_WARN("Setting option <-s> will enable sinus output with period <ramp_time>");
+	PX4_WARN("Example:");
+	PX4_WARN("nsh> sdlog2 on");
+	PX4_WARN("nsh> mc_att_control stop");
+	PX4_WARN("nsh> motor_ramp 1100 0.5\n");
 }
 
 /**
@@ -189,7 +189,7 @@ int set_min_pwm(int fd, unsigned long max_channels, unsigned pwm_value)
 	ret = px4_ioctl(fd, PWM_SERVO_SET_MIN_PWM, (long unsigned int)&pwm_values);
 
 	if (ret != OK) {
-		PX4_ERR("[motor_ramp] failed setting min values");
+		PX4_ERR("failed setting min values");
 		return 1;
 	}
 
@@ -218,25 +218,25 @@ int prepare(int fd, unsigned long *max_channels)
 {
 	/* get number of channels available on the device */
 	if (px4_ioctl(fd, PWM_SERVO_GET_COUNT, (unsigned long)max_channels) != OK) {
-		PX4_ERR("[motor_ramp] PWM_SERVO_GET_COUNT");
+		PX4_ERR("PWM_SERVO_GET_COUNT");
 		return 1;
 	}
 
 	/* tell IO/FMU that its ok to disable its safety with the switch */
 	if (px4_ioctl(fd, PWM_SERVO_SET_ARM_OK, 0) != OK) {
-		PX4_ERR("[motor_ramp] PWM_SERVO_SET_ARM_OK");
+		PX4_ERR("PWM_SERVO_SET_ARM_OK");
 		return 1;
 	}
 
 	/* tell IO/FMU that the system is armed (it will output values if safety is off) */
 	if (px4_ioctl(fd, PWM_SERVO_ARM, 0) != OK) {
-		PX4_ERR("[motor_ramp] PWM_SERVO_ARM");
+		PX4_ERR("PWM_SERVO_ARM");
 		return 1;
 	}
 
 	/* tell IO to switch off safety without using the safety switch */
 	if (px4_ioctl(fd, PWM_SERVO_SET_FORCE_SAFETY_OFF, 0) != OK) {
-		PX4_ERR("[motor_ramp] PWM_SERVO_SET_FORCE_SAFETY_OFF");
+		PX4_ERR("PWM_SERVO_SET_FORCE_SAFETY_OFF");
 		return 1;
 	}
 
@@ -245,7 +245,7 @@ int prepare(int fd, unsigned long *max_channels)
 
 int motor_ramp_thread_main(int argc, char *argv[])
 {
-	PX4_WARN("[motor_ramp] starting");
+	PX4_WARN("starting");
 
 	_thread_running = true;
 
@@ -255,14 +255,14 @@ int motor_ramp_thread_main(int argc, char *argv[])
 	int fd = px4_open(dev, 0);
 
 	if (fd < 0) {
-		PX4_ERR("[motor_ramp] can't open %s", dev);
+		PX4_ERR("can't open %s", dev);
 	}
 
 	if (prepare(fd, &max_channels) != OK) {
 		_thread_should_exit = true;
 	}
 
-	PX4_WARN("[motor_ramp] max chan: %lu", max_channels);
+	PX4_WARN("max chan: %lu", max_channels);
 
 	set_out(fd, max_channels, 0.0f);
 
@@ -287,7 +287,7 @@ int motor_ramp_thread_main(int argc, char *argv[])
 
 		switch (ramp_state) {
 		case RAMP_INIT: {
-				PX4_WARN("[motor_ramp] setting pwm min: %d", _min_pwm);
+				PX4_WARN("setting pwm min: %d", _min_pwm);
 				set_min_pwm(fd, max_channels, _min_pwm);
 				ramp_state = RAMP_MIN;
 				break;
@@ -295,7 +295,7 @@ int motor_ramp_thread_main(int argc, char *argv[])
 
 		case RAMP_MIN: {
 				if (timer > 3.0f) {
-					PX4_WARN("[motor_ramp] starting %s: %.2f sec", _sine_output ? "sine" : "ramp", (double)_ramp_time);
+					PX4_WARN("starting %s: %.2f sec", _sine_output ? "sine" : "ramp", (double)_ramp_time);
 					start = hrt_absolute_time();
 					ramp_state = RAMP_RAMP;
 				}
@@ -319,7 +319,7 @@ int motor_ramp_thread_main(int argc, char *argv[])
 					output = _sine_output ? output : 1.0f;
 					start = hrt_absolute_time();
 					ramp_state = RAMP_WAIT;
-					PX4_WARN("[motor_ramp] %s finished, waiting", _sine_output ? "sine" : "ramp");
+					PX4_WARN("%s finished, waiting", _sine_output ? "sine" : "ramp");
 				}
 
 				set_out(fd, max_channels, output);
@@ -348,7 +348,7 @@ int motor_ramp_thread_main(int argc, char *argv[])
 		px4_close(fd);
 	}
 
-	PX4_WARN("[motor_ramp] exiting");
+	PX4_WARN("exiting");
 
 	_thread_running = false;
 

From 4e0980aeb979538097ba62c6e3732d8fc49dedd5 Mon Sep 17 00:00:00 2001
From: Andreas Antener <antener_a@gmx.ch>
Date: Mon, 20 Jun 2016 19:52:56 +0200
Subject: [PATCH 0712/1230] fixed sine output

---
 src/systemcmds/motor_ramp/motor_ramp.cpp | 7 +++++--
 1 file changed, 5 insertions(+), 2 deletions(-)

diff --git a/src/systemcmds/motor_ramp/motor_ramp.cpp b/src/systemcmds/motor_ramp/motor_ramp.cpp
index 048533e9c5..af39781bae 100644
--- a/src/systemcmds/motor_ramp/motor_ramp.cpp
+++ b/src/systemcmds/motor_ramp/motor_ramp.cpp
@@ -148,6 +148,9 @@ int motor_ramp_main(int argc, char *argv[])
 		if (strcmp(argv[3], "-s") == 0) {
 			_sine_output = true;
 		}
+
+	} else {
+		_sine_output = false;
 	}
 
 	if (!(_ramp_time > 0)) {
@@ -311,10 +314,10 @@ int motor_ramp_thread_main(int argc, char *argv[])
 				} else {
 					// sine outpout with period T = _ramp_time and magnitude between [0,1]
 					// phase shift makes sure that it starts at zero when timer is zero
-					output = 0.5f * (1.0f + sinf(M_TWOPI_F * timer / (_ramp_time * 1e6f) - M_PI_2_F));
+					output = 0.5f * (1.0f + sinf(M_TWOPI_F * timer / _ramp_time - M_PI_2_F));
 				}
 
-				if ((output > 1.0f && !_sine_output) || (timer > 3.0f * _ramp_time * 1e6f && _sine_output)) {
+				if ((output > 1.0f && !_sine_output) || (timer > 3.0f * _ramp_time && _sine_output)) {
 					// for ramp mode we set output to 1, for sine mode we just leave it as is
 					output = _sine_output ? output : 1.0f;
 					start = hrt_absolute_time();

From 2dc97fc68030160b986b6855017457bdb4f51036 Mon Sep 17 00:00:00 2001
From: Andreas Antener <antener_a@gmx.ch>
Date: Mon, 20 Jun 2016 20:18:51 +0200
Subject: [PATCH 0713/1230] added check for running attitude controllers,
 cleaned-up output

---
 src/systemcmds/motor_ramp/motor_ramp.cpp | 44 ++++++++++++++++--------
 1 file changed, 30 insertions(+), 14 deletions(-)

diff --git a/src/systemcmds/motor_ramp/motor_ramp.cpp b/src/systemcmds/motor_ramp/motor_ramp.cpp
index af39781bae..048d146feb 100644
--- a/src/systemcmds/motor_ramp/motor_ramp.cpp
+++ b/src/systemcmds/motor_ramp/motor_ramp.cpp
@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2012-2015 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2016 PX4 Development Team. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -104,13 +104,13 @@ usage(const char *reason)
 		PX4_ERR("%s", reason);
 	}
 
-	PX4_WARN("\n\nWARNING: motors will ramp up to full speed!\n");
-	PX4_WARN("Usage: motor_ramp <min_pwm> <ramp_time> <-s>");
-	PX4_WARN("Setting option <-s> will enable sinus output with period <ramp_time>");
-	PX4_WARN("Example:");
-	PX4_WARN("nsh> sdlog2 on");
-	PX4_WARN("nsh> mc_att_control stop");
-	PX4_WARN("nsh> motor_ramp 1100 0.5\n");
+	PX4_WARN("\n\nWARNING: motors will ramp up to full speed!\n\n"
+		"Usage: motor_ramp <min_pwm> <ramp_time> <-s>\n"
+		"Setting option <-s> will enable sinus output with period <ramp_time>\n\n"
+		"Example:\n"
+		"nsh> sdlog2 on\n"
+		"nsh> mc_att_control stop\n"
+		"nsh> motor_ramp 1100 0.5\n");
 }
 
 /**
@@ -219,6 +219,27 @@ int set_out(int fd, unsigned long max_channels, float output)
 
 int prepare(int fd, unsigned long *max_channels)
 {
+	/* make sure no other source is publishing control values now */
+	struct actuator_controls_s actuators;
+	int act_sub = orb_subscribe(ORB_ID_VEHICLE_ATTITUDE_CONTROLS);
+
+	/* clear changed flag */
+	orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, act_sub, &actuators);
+
+	/* wait 50 ms */
+	usleep(50000);
+
+	/* now expect nothing changed on that topic */
+	bool orb_updated;
+	orb_check(act_sub, &orb_updated);
+
+	if (orb_updated) {
+		PX4_ERR("ABORTING! Attitude control still active. Please ensure to shut down all controllers:\n"
+			"\tmc_att_control stop\n"
+			"\tfw_att_control stop\n");
+		return 1;
+	}
+
 	/* get number of channels available on the device */
 	if (px4_ioctl(fd, PWM_SERVO_GET_COUNT, (unsigned long)max_channels) != OK) {
 		PX4_ERR("PWM_SERVO_GET_COUNT");
@@ -248,8 +269,6 @@ int prepare(int fd, unsigned long *max_channels)
 
 int motor_ramp_thread_main(int argc, char *argv[])
 {
-	PX4_WARN("starting");
-
 	_thread_running = true;
 
 	char *dev = PWM_OUTPUT0_DEVICE_PATH;
@@ -265,8 +284,6 @@ int motor_ramp_thread_main(int argc, char *argv[])
 		_thread_should_exit = true;
 	}
 
-	PX4_WARN("max chan: %lu", max_channels);
-
 	set_out(fd, max_channels, 0.0f);
 
 	float dt = 0.01f; // prevent division with 0
@@ -278,6 +295,7 @@ int motor_ramp_thread_main(int argc, char *argv[])
 	float output = 0.0f;
 
 	while (!_thread_should_exit) {
+
 		if (last_run > 0) {
 			dt = hrt_elapsed_time(&last_run) * 1e-6;
 
@@ -351,8 +369,6 @@ int motor_ramp_thread_main(int argc, char *argv[])
 		px4_close(fd);
 	}
 
-	PX4_WARN("exiting");
-
 	_thread_running = false;
 
 	return 0;

From 0581f1af52606809fdcb12f3db83584d9a81d822 Mon Sep 17 00:00:00 2001
From: Andreas Antener <antener_a@gmx.ch>
Date: Mon, 20 Jun 2016 20:21:39 +0200
Subject: [PATCH 0714/1230] added motor_ramp to fmu v4

---
 cmake/configs/nuttx_px4fmu-v4_default.cmake | 1 +
 1 file changed, 1 insertion(+)

diff --git a/cmake/configs/nuttx_px4fmu-v4_default.cmake b/cmake/configs/nuttx_px4fmu-v4_default.cmake
index f40f25104a..2b2814d2b4 100644
--- a/cmake/configs/nuttx_px4fmu-v4_default.cmake
+++ b/cmake/configs/nuttx_px4fmu-v4_default.cmake
@@ -68,6 +68,7 @@ set(config_module_list
 	systemcmds/ver
 	systemcmds/sd_bench
 	systemcmds/tests
+	systemcmds/motor_ramp
 
 	#
 	# General system control

From 2c29652136f3509f81e6220bd4e81665f83c08f9 Mon Sep 17 00:00:00 2001
From: Andreas Antener <antener_a@gmx.ch>
Date: Mon, 20 Jun 2016 20:59:43 +0200
Subject: [PATCH 0715/1230] fixed code style in motor_ramp

---
 src/systemcmds/motor_ramp/motor_ramp.cpp | 12 ++++++------
 1 file changed, 6 insertions(+), 6 deletions(-)

diff --git a/src/systemcmds/motor_ramp/motor_ramp.cpp b/src/systemcmds/motor_ramp/motor_ramp.cpp
index 048d146feb..ea3e7ba272 100644
--- a/src/systemcmds/motor_ramp/motor_ramp.cpp
+++ b/src/systemcmds/motor_ramp/motor_ramp.cpp
@@ -105,12 +105,12 @@ usage(const char *reason)
 	}
 
 	PX4_WARN("\n\nWARNING: motors will ramp up to full speed!\n\n"
-		"Usage: motor_ramp <min_pwm> <ramp_time> <-s>\n"
-		"Setting option <-s> will enable sinus output with period <ramp_time>\n\n"
-		"Example:\n"
-		"nsh> sdlog2 on\n"
-		"nsh> mc_att_control stop\n"
-		"nsh> motor_ramp 1100 0.5\n");
+		 "Usage: motor_ramp <min_pwm> <ramp_time> <-s>\n"
+		 "Setting option <-s> will enable sinus output with period <ramp_time>\n\n"
+		 "Example:\n"
+		 "nsh> sdlog2 on\n"
+		 "nsh> mc_att_control stop\n"
+		 "nsh> motor_ramp 1100 0.5\n");
 }
 
 /**

From 85245471c05a7453ffa50e2e74dbf8f48c01982b Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Mon, 20 Jun 2016 22:36:50 +0200
Subject: [PATCH 0716/1230] Update 3DR quad config, remove rendundant entries

---
 ROMFS/px4fmu_common/init.d/10020_3dr_quad | 7 -------
 1 file changed, 7 deletions(-)

diff --git a/ROMFS/px4fmu_common/init.d/10020_3dr_quad b/ROMFS/px4fmu_common/init.d/10020_3dr_quad
index a1b2e90270..e91e994cbc 100644
--- a/ROMFS/px4fmu_common/init.d/10020_3dr_quad
+++ b/ROMFS/px4fmu_common/init.d/10020_3dr_quad
@@ -21,10 +21,3 @@ then
 	param set MC_PITCHRATE_D 0.004
 	param set MC_YAW_P 4
 fi
-
-set MIXER quad_x
-
-set PWM_OUT 1234
-
-set PWM_MIN 1100
-set PWM_MAX 1950

From 8aee4432a9aa517deb5b9851949a0d5f405e1ed4 Mon Sep 17 00:00:00 2001
From: Julian Oes <julian@oes.ch>
Date: Tue, 21 Jun 2016 09:15:38 +0200
Subject: [PATCH 0717/1230] px4io: set safety on before going into bootloader
 (#4860)

Sometimes when flashing new firmware, the IO update fails because safety
is off. In this case, we should set safety on first before putting the
IO board into bootloader mode.
---
 src/drivers/px4io/px4io.cpp | 6 ++++++
 1 file changed, 6 insertions(+)

diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp
index e3c1b1e6d3..66ea188244 100644
--- a/src/drivers/px4io/px4io.cpp
+++ b/src/drivers/px4io/px4io.cpp
@@ -699,6 +699,12 @@ PX4IO::init()
 		// be due to mismatched firmware versions and we want
 		// the startup script to be able to load a new IO
 		// firmware
+
+		// If IO has already safety off it won't accept going into bootloader mode,
+		// therefore we need to set safety on first.
+		io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_FORCE_SAFETY_ON, PX4IO_FORCE_SAFETY_MAGIC);
+
+		// Now the reboot into bootloader mode should succeed.
 		io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_REBOOT_BL, PX4IO_REBOOT_BL_MAGIC);
 		return -1;
 	}

From 3f45d008eb7b5c76664c3f64bdafc1bf2441ff18 Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Tue, 21 Jun 2016 09:43:00 +0200
Subject: [PATCH 0718/1230] Fix stack main usage in motor ramp app

---
 src/systemcmds/motor_ramp/CMakeLists.txt | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/src/systemcmds/motor_ramp/CMakeLists.txt b/src/systemcmds/motor_ramp/CMakeLists.txt
index 601d4a9199..21642c136a 100644
--- a/src/systemcmds/motor_ramp/CMakeLists.txt
+++ b/src/systemcmds/motor_ramp/CMakeLists.txt
@@ -33,7 +33,7 @@
 px4_add_module(
     MODULE systemcmds__motor_ramp
     MAIN motor_ramp
-    STACK 1200
+    STACK_MAIN 1200
     COMPILE_FLAGS
         -Wno-write-strings
     SRCS

From 4ef4be2d7012231a070a65d6783949a339d8e3f7 Mon Sep 17 00:00:00 2001
From: Chris Lovett <lovettchris@hotmail.com>
Date: Tue, 14 Jun 2016 18:07:24 -0700
Subject: [PATCH 0719/1230] MavlinkReceiver::handle_message_request_data_stream
 walks into deleted memory when you send the "stop" bit on a stream.  It also
 fails to restart the stream because it deletes the stream when you send the
 stop command, so restart needs to use stream_list to find the stream again.

---
 src/modules/mavlink/mavlink_receiver.cpp | 20 ++++++++++++++++++++
 1 file changed, 20 insertions(+)

diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp
index a769cb0315..9d33711b9e 100644
--- a/src/modules/mavlink/mavlink_receiver.cpp
+++ b/src/modules/mavlink/mavlink_receiver.cpp
@@ -1376,8 +1376,28 @@ MavlinkReceiver::handle_message_request_data_stream(mavlink_message_t *msg)
 		LL_FOREACH(_mavlink->get_streams(), stream) {
 			if (req.req_stream_id == stream->get_id()) {
 				_mavlink->configure_stream_threadsafe(stream->get_name(), rate);
+                // if the command here was to unsubscribe then the stream will be deleted
+                // in which case we cannot continue this LL_FOREACH loop.  We have to return.
+                // Besides there shouldn't be more than one stream with the same id, right?
+				return;
 			}
 		}
+
+        // If we get here then the stream may have been deleted, in which case we need to recreate it.
+        // todo: we could simplify this code by adding a get_id_static (get_name_static) so that
+		// we could do both subscribe and unsubscribe using the streams_list.
+        for (unsigned int i = 0; streams_list[i] != nullptr; i++) {
+            MavlinkStream *new_instance = streams_list[i]->new_instance(_mavlink);
+            bool found = false;
+            if (req.req_stream_id == new_instance->get_id()) {
+                _mavlink->configure_stream_threadsafe(new_instance->get_name(), rate);
+                found = true;
+            }
+            delete new_instance;
+            if (found) {
+                break;
+            }
+        }
 	}
 }
 

From 534e10c96a1b5fc7a98965d4b82feff4450719a7 Mon Sep 17 00:00:00 2001
From: Chris Lovett <lovettchris@hotmail.com>
Date: Wed, 15 Jun 2016 18:41:39 -0700
Subject: [PATCH 0720/1230] Implement code review feedback.  Add get_id_static
 to MavlinkStream items.  Add implementation of MAV_CMD_SET_MESSAGE_INTERVAL,
 MAV_CMD_GET_MESSAGE_INTERVAL.  Add deprecation message to
 REQUEST_DATA_STREAM.

---
 src/modules/mavlink/mavlink_main.cpp     |   2 +-
 src/modules/mavlink/mavlink_messages.cpp | 327 +++++++++++++++++------
 src/modules/mavlink/mavlink_messages.h   |   6 +-
 src/modules/mavlink/mavlink_receiver.cpp |  94 +++++--
 src/modules/mavlink/mavlink_receiver.h   |   7 +
 5 files changed, 332 insertions(+), 104 deletions(-)

diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp
index 125ed23a5d..3edf67676b 100644
--- a/src/modules/mavlink/mavlink_main.cpp
+++ b/src/modules/mavlink/mavlink_main.cpp
@@ -1346,7 +1346,7 @@ Mavlink::configure_stream(const char *stream_name, const float rate)
 		}
 	}
 
-	if (interval == 0) {
+	if (interval <= 0) {
 		/* stream was not active and is requested to be disabled, do nothing */
 		return OK;
 	}
diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp
index 8839c91b2a..9cd58d66d8 100644
--- a/src/modules/mavlink/mavlink_messages.cpp
+++ b/src/modules/mavlink/mavlink_messages.cpp
@@ -275,11 +275,16 @@ public:
 		return "HEARTBEAT";
 	}
 
-	uint8_t get_id()
+	static uint8_t get_id_static()
 	{
 		return MAVLINK_MSG_ID_HEARTBEAT;
 	}
 
+    uint8_t get_id()
+    {
+        return get_id_static();
+    }
+
 	static MavlinkStream *new_instance(Mavlink *mavlink)
 	{
 		return new MavlinkStreamHeartbeat(mavlink);
@@ -339,11 +344,16 @@ public:
 		return "STATUSTEXT";
 	}
 
-	uint8_t get_id()
+	static uint8_t get_id_static()
 	{
 		return MAVLINK_MSG_ID_STATUSTEXT;
 	}
 
+    uint8_t get_id()
+    {
+        return get_id_static();
+    }
+
 	static MavlinkStream *new_instance(Mavlink *mavlink)
 	{
 		return new MavlinkStreamStatustext(mavlink);
@@ -471,11 +481,16 @@ public:
 		return "COMMAND_LONG";
 	}
 
-	uint8_t get_id()
+	static uint8_t get_id_static()
 	{
 		return MAVLINK_MSG_ID_COMMAND_LONG;
 	}
 
+    uint8_t get_id()
+    {
+        return get_id_static();
+    }
+
 	static MavlinkStream *new_instance(Mavlink *mavlink)
 	{
 		return new MavlinkStreamCommandLong(mavlink);
@@ -539,11 +554,16 @@ public:
 		return "SYS_STATUS";
 	}
 
-	uint8_t get_id()
+    static uint8_t get_id_static()
 	{
 		return MAVLINK_MSG_ID_SYS_STATUS;
 	}
 
+    uint8_t get_id()
+    {
+        return get_id_static();
+    }
+
 	static MavlinkStream *new_instance(Mavlink *mavlink)
 	{
 		return new MavlinkStreamSysStatus(mavlink);
@@ -645,11 +665,16 @@ public:
 		return "HIGHRES_IMU";
 	}
 
-	uint8_t get_id()
+	static uint8_t get_id_static()
 	{
 		return MAVLINK_MSG_ID_HIGHRES_IMU;
 	}
 
+    uint8_t get_id()
+    {
+        return get_id_static();
+    }
+
 	static MavlinkStream *new_instance(Mavlink *mavlink)
 	{
 		return new MavlinkStreamHighresIMU(mavlink);
@@ -751,11 +776,16 @@ public:
 		return "ATTITUDE";
 	}
 
-	uint8_t get_id()
+	static uint8_t get_id_static()
 	{
 		return MAVLINK_MSG_ID_ATTITUDE;
 	}
 
+    uint8_t get_id()
+    {
+        return get_id_static();
+    }
+
 	static MavlinkStream *new_instance(Mavlink *mavlink)
 	{
 		return new MavlinkStreamAttitude(mavlink);
@@ -815,11 +845,16 @@ public:
 		return "ATTITUDE_QUATERNION";
 	}
 
-	uint8_t get_id()
+	static uint8_t get_id_static()
 	{
 		return MAVLINK_MSG_ID_ATTITUDE_QUATERNION;
 	}
 
+    uint8_t get_id()
+    {
+        return get_id_static();
+    }
+
 	static MavlinkStream *new_instance(Mavlink *mavlink)
 	{
 		return new MavlinkStreamAttitudeQuaternion(mavlink);
@@ -880,11 +915,16 @@ public:
 		return "VFR_HUD";
 	}
 
-	uint8_t get_id()
+	static uint8_t get_id_static()
 	{
 		return MAVLINK_MSG_ID_VFR_HUD;
 	}
 
+    uint8_t get_id()
+    {
+        return get_id_static();
+    }
+
 	static MavlinkStream *new_instance(Mavlink *mavlink)
 	{
 		return new MavlinkStreamVFRHUD(mavlink);
@@ -985,11 +1025,16 @@ public:
 		return "GPS_RAW_INT";
 	}
 
-	uint8_t get_id()
+	static uint8_t get_id_static()
 	{
 		return MAVLINK_MSG_ID_GPS_RAW_INT;
 	}
 
+    uint8_t get_id()
+    {
+        return get_id_static();
+    }
+
 	static MavlinkStream *new_instance(Mavlink *mavlink)
 	{
 		return new MavlinkStreamGPSRawInt(mavlink);
@@ -1048,10 +1093,15 @@ public:
 		return "SYSTEM_TIME";
 	}
 
-	uint8_t get_id() {
+	static uint8_t get_id_static() {
 		return MAVLINK_MSG_ID_SYSTEM_TIME;
 	}
 
+    uint8_t get_id()
+    {
+        return get_id_static();
+    }
+
 	static MavlinkStream *new_instance(Mavlink *mavlink) {
 		return new MavlinkStreamSystemTime(mavlink);
 	}
@@ -1093,10 +1143,15 @@ public:
 		return "TIMESYNC";
 	}
 
-	uint8_t get_id() {
+	static uint8_t get_id_static() {
 		return MAVLINK_MSG_ID_TIMESYNC;
 	}
 
+    uint8_t get_id()
+    {
+        return get_id_static();
+    }
+
 	static MavlinkStream *new_instance(Mavlink *mavlink) {
 		return new MavlinkStreamTimesync(mavlink);
 	}
@@ -1137,11 +1192,16 @@ public:
 		return "ADSB_VEHICLE";
 	}
 
-	uint8_t get_id()
+	static uint8_t get_id_static()
 	{
 		return MAVLINK_MSG_ID_ADSB_VEHICLE;
 	}
 
+    uint8_t get_id()
+    {
+        return get_id_static();
+    }
+
 	static MavlinkStream *new_instance(Mavlink *mavlink)
 	{
 		return new MavlinkStreamADSBVehicle(mavlink);
@@ -1205,11 +1265,16 @@ public:
 		return "CAMERA_TRIGGER";
 	}
 
-	uint8_t get_id()
+	static uint8_t get_id_static()
 	{
 		return MAVLINK_MSG_ID_CAMERA_TRIGGER;
 	}
 
+    uint8_t get_id()
+    {
+        return get_id_static();
+    }
+
 	static MavlinkStream *new_instance(Mavlink *mavlink)
 	{
 		return new MavlinkStreamCameraTrigger(mavlink);
@@ -1265,11 +1330,16 @@ public:
 		return "GLOBAL_POSITION_INT";
 	}
 
-	uint8_t get_id()
+	static uint8_t get_id_static()
 	{
 		return MAVLINK_MSG_ID_GLOBAL_POSITION_INT;
 	}
 
+    uint8_t get_id()
+    {
+        return get_id_static();
+    }
+
 	static MavlinkStream *new_instance(Mavlink *mavlink)
 	{
 		return new MavlinkStreamGlobalPositionInt(mavlink);
@@ -1338,11 +1408,16 @@ public:
         return "VISION_POSITION_NED";
     }
 
-    uint8_t get_id()
+    static uint8_t get_id_static()
     {
         return MAVLINK_MSG_ID_VISION_POSITION_ESTIMATE;
     }
 
+    uint8_t get_id()
+    {
+        return get_id_static();
+    }
+
     static MavlinkStream *new_instance(Mavlink *mavlink)
     {
         return new MavlinkStreamVisionPositionNED(mavlink);
@@ -1402,11 +1477,16 @@ public:
 		return "LOCAL_POSITION_NED";
 	}
 
-	uint8_t get_id()
+	static uint8_t get_id_static()
 	{
 		return MAVLINK_MSG_ID_LOCAL_POSITION_NED;
 	}
 
+    uint8_t get_id()
+    {
+        return get_id_static();
+    }
+
 	static MavlinkStream *new_instance(Mavlink *mavlink)
 	{
 		return new MavlinkStreamLocalPositionNED(mavlink);
@@ -1465,11 +1545,16 @@ public:
 		return "LOCAL_POSITION_NED_COV";
 	}
 
-	uint8_t get_id()
+	static uint8_t get_id_static()
 	{
 		return MAVLINK_MSG_ID_LOCAL_POSITION_NED_COV;
 	}
 
+    uint8_t get_id()
+    {
+        return get_id_static();
+    }
+
 	static MavlinkStream *new_instance(Mavlink *mavlink)
 	{
 		return new MavlinkStreamLocalPositionNEDCOV(mavlink);
@@ -1537,11 +1622,16 @@ public:
 		return "ESTIMATOR_STATUS";
 	}
 
-	uint8_t get_id()
+	static uint8_t get_id_static()
 	{
 		return MAVLINK_MSG_ID_VIBRATION;
 	}
 
+    uint8_t get_id()
+    {
+        return get_id_static();
+    }
+
 	static MavlinkStream *new_instance(Mavlink *mavlink)
 	{
 		return new MavlinkStreamEstimatorStatus(mavlink);
@@ -1600,11 +1690,16 @@ public:
 		return "ATT_POS_MOCAP";
 	}
 
-	uint8_t get_id()
+	static uint8_t get_id_static()
 	{
 		return MAVLINK_MSG_ID_ATT_POS_MOCAP;
 	}
 
+    uint8_t get_id()
+    {
+        return get_id_static();
+    }
+
 	static MavlinkStream *new_instance(Mavlink *mavlink)
 	{
 		return new MavlinkStreamAttPosMocap(mavlink);
@@ -1664,11 +1759,16 @@ public:
 		return "HOME_POSITION";
 	}
 
-	uint8_t get_id()
+	static uint8_t get_id_static()
 	{
 		return MAVLINK_MSG_ID_HOME_POSITION;
 	}
 
+    uint8_t get_id()
+    {
+        return get_id_static();
+    }
+
 	static MavlinkStream *new_instance(Mavlink *mavlink)
 	{
 		return new MavlinkStreamHomePosition(mavlink);
@@ -1734,11 +1834,16 @@ public:
 		return MavlinkStreamServoOutputRaw<N>::get_name_static();
 	}
 
-	uint8_t get_id()
+	static uint8_t get_id_static()
 	{
 		return MAVLINK_MSG_ID_SERVO_OUTPUT_RAW;
 	}
 
+    uint8_t get_id()
+    {
+        return get_id_static();
+    }
+
 	static const char *get_name_static()
 	{
 		switch (N) {
@@ -1831,11 +1936,16 @@ public:
 		}
 	}
 
-	uint8_t get_id()
+	static uint8_t get_id_static()
 	{
 		return MAVLINK_MSG_ID_ACTUATOR_CONTROL_TARGET;
 	}
 
+    uint8_t get_id()
+    {
+        return get_id_static();
+    }
+
 	static MavlinkStream *new_instance(Mavlink *mavlink)
 	{
 		return new MavlinkStreamActuatorControlTarget<N>(mavlink);
@@ -1912,11 +2022,16 @@ public:
 		return "HIL_CONTROLS";
 	}
 
-	uint8_t get_id()
+	static uint8_t get_id_static()
 	{
 		return MAVLINK_MSG_ID_HIL_CONTROLS;
 	}
 
+    uint8_t get_id()
+    {
+        return get_id_static();
+    }
+
 	static MavlinkStream *new_instance(Mavlink *mavlink)
 	{
 		return new MavlinkStreamHILControls(mavlink);
@@ -2071,11 +2186,16 @@ public:
 		return "POSITION_TARGET_GLOBAL_INT";
 	}
 
-	uint8_t get_id()
+	static uint8_t get_id_static()
 	{
 		return MAVLINK_MSG_ID_POSITION_TARGET_GLOBAL_INT;
 	}
 
+    uint8_t get_id()
+    {
+        return get_id_static();
+    }
+
 	static MavlinkStream *new_instance(Mavlink *mavlink)
 	{
 		return new MavlinkStreamPositionTargetGlobalInt(mavlink);
@@ -2130,11 +2250,16 @@ public:
 		return "POSITION_TARGET_LOCAL_NED";
 	}
 
-	uint8_t get_id()
+	static uint8_t get_id_static()
 	{
 		return MAVLINK_MSG_ID_POSITION_TARGET_LOCAL_NED;
 	}
 
+    uint8_t get_id()
+    {
+        return get_id_static();
+    }
+
 	static MavlinkStream *new_instance(Mavlink *mavlink)
 	{
 		return new MavlinkStreamLocalPositionSetpoint(mavlink);
@@ -2198,11 +2323,16 @@ public:
 		return "ATTITUDE_TARGET";
 	}
 
-	uint8_t get_id()
+	static uint8_t get_id_static()
 	{
 		return MAVLINK_MSG_ID_ATTITUDE_TARGET;
 	}
 
+    uint8_t get_id()
+    {
+        return get_id_static();
+    }
+
 	static MavlinkStream *new_instance(Mavlink *mavlink)
 	{
 		return new MavlinkStreamAttitudeTarget(mavlink);
@@ -2270,11 +2400,16 @@ public:
 		return "RC_CHANNELS";
 	}
 
-	uint8_t get_id()
+	static uint8_t get_id_static()
 	{
 		return MAVLINK_MSG_ID_RC_CHANNELS;
 	}
 
+    uint8_t get_id()
+    {
+        return get_id_static();
+    }
+
 	static MavlinkStream *new_instance(Mavlink *mavlink)
 	{
 		return new MavlinkStreamRCChannels(mavlink);
@@ -2366,11 +2501,16 @@ public:
 		return "MANUAL_CONTROL";
 	}
 
-	uint8_t get_id()
+	static uint8_t get_id_static()
 	{
 		return MAVLINK_MSG_ID_MANUAL_CONTROL;
 	}
 
+    uint8_t get_id()
+    {
+        return get_id_static();
+    }
+
 	static MavlinkStream *new_instance(Mavlink *mavlink)
 	{
 		return new MavlinkStreamManualControl(mavlink);
@@ -2434,11 +2574,16 @@ public:
 		return "OPTICAL_FLOW_RAD";
 	}
 
-	uint8_t get_id()
+	static uint8_t get_id_static()
 	{
 		return MAVLINK_MSG_ID_OPTICAL_FLOW_RAD;
 	}
 
+    uint8_t get_id()
+    {
+        return get_id_static();
+    }
+
 	static MavlinkStream *new_instance(Mavlink *mavlink)
 	{
 		return new MavlinkStreamOpticalFlowRad(mavlink);
@@ -2502,11 +2647,16 @@ public:
 		return "NAMED_VALUE_FLOAT";
 	}
 
-	uint8_t get_id()
+	static uint8_t get_id_static()
 	{
 		return MAVLINK_MSG_ID_NAMED_VALUE_FLOAT;
 	}
 
+    uint8_t get_id()
+    {
+        return get_id_static();
+    }
+
 	static MavlinkStream *new_instance(Mavlink *mavlink)
 	{
 		return new MavlinkStreamNamedValueFloat(mavlink);
@@ -2562,11 +2712,16 @@ public:
 		return "NAV_CONTROLLER_OUTPUT";
 	}
 
-	uint8_t get_id()
+	static uint8_t get_id_static()
 	{
 		return MAVLINK_MSG_ID_NAV_CONTROLLER_OUTPUT;
 	}
 
+    uint8_t get_id()
+    {
+        return get_id_static();
+    }
+
 	static MavlinkStream *new_instance(Mavlink *mavlink)
 	{
 		return new MavlinkStreamNavControllerOutput(mavlink);
@@ -2629,11 +2784,16 @@ public:
 		return "CAMERA_CAPTURE";
 	}
 
-	uint8_t get_id()
+	static uint8_t get_id_static()
 	{
 		return 0;
 	}
 
+    uint8_t get_id()
+    {
+        return get_id_static();
+    }
+
 	static MavlinkStream *new_instance(Mavlink *mavlink)
 	{
 		return new MavlinkStreamCameraCapture(mavlink);
@@ -2693,11 +2853,16 @@ public:
 		return "DISTANCE_SENSOR";
 	}
 
-	uint8_t get_id()
+	static uint8_t get_id_static()
 	{
 		return MAVLINK_MSG_ID_DISTANCE_SENSOR;
 	}
 
+    uint8_t get_id()
+    {
+        return get_id_static();
+    }
+
 	static MavlinkStream *new_instance(Mavlink *mavlink)
 	{
 		return new MavlinkStreamDistanceSensor(mavlink);
@@ -2777,11 +2942,16 @@ public:
 		return "EXTENDED_SYS_STATE";
 	}
 
-	uint8_t get_id()
+	static uint8_t get_id_static()
 	{
 		return MAVLINK_MSG_ID_EXTENDED_SYS_STATE;
 	}
 
+    uint8_t get_id()
+    {
+        return get_id_static();
+    }
+
 	static MavlinkStream *new_instance(Mavlink *mavlink)
 	{
 		return new MavlinkStreamExtendedSysState(mavlink);
@@ -2871,11 +3041,16 @@ public:
 		return "ALTITUDE";
 	}
 
-	uint8_t get_id()
+	static uint8_t get_id_static()
 	{
 		return MAVLINK_MSG_ID_ALTITUDE;
 	}
 
+    uint8_t get_id()
+    {
+        return get_id_static();
+    }
+
 	static MavlinkStream *new_instance(Mavlink *mavlink)
 	{
 		return new MavlinkStreamAltitude(mavlink);
@@ -2953,46 +3128,46 @@ protected:
 };
 
 const StreamListItem *streams_list[] = {
-	new StreamListItem(&MavlinkStreamHeartbeat::new_instance, &MavlinkStreamHeartbeat::get_name_static),
-	new StreamListItem(&MavlinkStreamStatustext::new_instance, &MavlinkStreamStatustext::get_name_static),
-	new StreamListItem(&MavlinkStreamCommandLong::new_instance, &MavlinkStreamCommandLong::get_name_static),
-	new StreamListItem(&MavlinkStreamSysStatus::new_instance, &MavlinkStreamSysStatus::get_name_static),
-	new StreamListItem(&MavlinkStreamHighresIMU::new_instance, &MavlinkStreamHighresIMU::get_name_static),
-	new StreamListItem(&MavlinkStreamAttitude::new_instance, &MavlinkStreamAttitude::get_name_static),
-	new StreamListItem(&MavlinkStreamAttitudeQuaternion::new_instance, &MavlinkStreamAttitudeQuaternion::get_name_static),
-	new StreamListItem(&MavlinkStreamVFRHUD::new_instance, &MavlinkStreamVFRHUD::get_name_static),
-	new StreamListItem(&MavlinkStreamGPSRawInt::new_instance, &MavlinkStreamGPSRawInt::get_name_static),
-	new StreamListItem(&MavlinkStreamSystemTime::new_instance, &MavlinkStreamSystemTime::get_name_static),
-	new StreamListItem(&MavlinkStreamTimesync::new_instance, &MavlinkStreamTimesync::get_name_static),
-	new StreamListItem(&MavlinkStreamGlobalPositionInt::new_instance, &MavlinkStreamGlobalPositionInt::get_name_static),
-	new StreamListItem(&MavlinkStreamLocalPositionNED::new_instance, &MavlinkStreamLocalPositionNED::get_name_static),
-	new StreamListItem(&MavlinkStreamVisionPositionNED::new_instance, &MavlinkStreamVisionPositionNED::get_name_static),
-	new StreamListItem(&MavlinkStreamLocalPositionNEDCOV::new_instance, &MavlinkStreamLocalPositionNEDCOV::get_name_static),
-	new StreamListItem(&MavlinkStreamEstimatorStatus::new_instance, &MavlinkStreamEstimatorStatus::get_name_static),
-	new StreamListItem(&MavlinkStreamAttPosMocap::new_instance, &MavlinkStreamAttPosMocap::get_name_static),
-	new StreamListItem(&MavlinkStreamHomePosition::new_instance, &MavlinkStreamHomePosition::get_name_static),
-	new StreamListItem(&MavlinkStreamServoOutputRaw<0>::new_instance, &MavlinkStreamServoOutputRaw<0>::get_name_static),
-	new StreamListItem(&MavlinkStreamServoOutputRaw<1>::new_instance, &MavlinkStreamServoOutputRaw<1>::get_name_static),
-	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(&MavlinkStreamPositionTargetGlobalInt::new_instance, &MavlinkStreamPositionTargetGlobalInt::get_name_static),
-	new StreamListItem(&MavlinkStreamLocalPositionSetpoint::new_instance, &MavlinkStreamLocalPositionSetpoint::get_name_static),
-	new StreamListItem(&MavlinkStreamAttitudeTarget::new_instance, &MavlinkStreamAttitudeTarget::get_name_static),
-	new StreamListItem(&MavlinkStreamRCChannels::new_instance, &MavlinkStreamRCChannels::get_name_static),
-	new StreamListItem(&MavlinkStreamManualControl::new_instance, &MavlinkStreamManualControl::get_name_static),
-	new StreamListItem(&MavlinkStreamOpticalFlowRad::new_instance, &MavlinkStreamOpticalFlowRad::get_name_static),
-	new StreamListItem(&MavlinkStreamActuatorControlTarget<0>::new_instance, &MavlinkStreamActuatorControlTarget<0>::get_name_static),
-	new StreamListItem(&MavlinkStreamActuatorControlTarget<1>::new_instance, &MavlinkStreamActuatorControlTarget<1>::get_name_static),
-	new StreamListItem(&MavlinkStreamActuatorControlTarget<2>::new_instance, &MavlinkStreamActuatorControlTarget<2>::get_name_static),
-	new StreamListItem(&MavlinkStreamActuatorControlTarget<3>::new_instance, &MavlinkStreamActuatorControlTarget<3>::get_name_static),
-	new StreamListItem(&MavlinkStreamNamedValueFloat::new_instance, &MavlinkStreamNamedValueFloat::get_name_static),
-	new StreamListItem(&MavlinkStreamNavControllerOutput::new_instance, &MavlinkStreamNavControllerOutput::get_name_static),
-	new StreamListItem(&MavlinkStreamCameraCapture::new_instance, &MavlinkStreamCameraCapture::get_name_static),
-	new StreamListItem(&MavlinkStreamCameraTrigger::new_instance, &MavlinkStreamCameraTrigger::get_name_static),
-	new StreamListItem(&MavlinkStreamDistanceSensor::new_instance, &MavlinkStreamDistanceSensor::get_name_static),
-	new StreamListItem(&MavlinkStreamExtendedSysState::new_instance, &MavlinkStreamExtendedSysState::get_name_static),
-	new StreamListItem(&MavlinkStreamAltitude::new_instance, &MavlinkStreamAltitude::get_name_static),
-	new StreamListItem(&MavlinkStreamADSBVehicle::new_instance, &MavlinkStreamADSBVehicle::get_name_static),
+	new StreamListItem(&MavlinkStreamHeartbeat::new_instance, &MavlinkStreamHeartbeat::get_name_static, &MavlinkStreamHeartbeat::get_id_static),
+	new StreamListItem(&MavlinkStreamStatustext::new_instance, &MavlinkStreamStatustext::get_name_static, &MavlinkStreamStatustext::get_id_static),
+	new StreamListItem(&MavlinkStreamCommandLong::new_instance, &MavlinkStreamCommandLong::get_name_static, &MavlinkStreamCommandLong::get_id_static),
+	new StreamListItem(&MavlinkStreamSysStatus::new_instance, &MavlinkStreamSysStatus::get_name_static, &MavlinkStreamSysStatus::get_id_static),
+	new StreamListItem(&MavlinkStreamHighresIMU::new_instance, &MavlinkStreamHighresIMU::get_name_static, &MavlinkStreamHighresIMU::get_id_static),
+	new StreamListItem(&MavlinkStreamAttitude::new_instance, &MavlinkStreamAttitude::get_name_static, &MavlinkStreamAttitude::get_id_static),
+	new StreamListItem(&MavlinkStreamAttitudeQuaternion::new_instance, &MavlinkStreamAttitudeQuaternion::get_name_static, &MavlinkStreamAttitudeQuaternion::get_id_static),
+	new StreamListItem(&MavlinkStreamVFRHUD::new_instance, &MavlinkStreamVFRHUD::get_name_static, &MavlinkStreamVFRHUD::get_id_static),
+	new StreamListItem(&MavlinkStreamGPSRawInt::new_instance, &MavlinkStreamGPSRawInt::get_name_static, &MavlinkStreamGPSRawInt::get_id_static),
+	new StreamListItem(&MavlinkStreamSystemTime::new_instance, &MavlinkStreamSystemTime::get_name_static, &MavlinkStreamSystemTime::get_id_static),
+	new StreamListItem(&MavlinkStreamTimesync::new_instance, &MavlinkStreamTimesync::get_name_static, &MavlinkStreamTimesync::get_id_static),
+	new StreamListItem(&MavlinkStreamGlobalPositionInt::new_instance, &MavlinkStreamGlobalPositionInt::get_name_static, &MavlinkStreamGlobalPositionInt::get_id_static),
+	new StreamListItem(&MavlinkStreamLocalPositionNED::new_instance, &MavlinkStreamLocalPositionNED::get_name_static, &MavlinkStreamLocalPositionNED::get_id_static),
+	new StreamListItem(&MavlinkStreamVisionPositionNED::new_instance, &MavlinkStreamVisionPositionNED::get_name_static, &MavlinkStreamVisionPositionNED::get_id_static),
+	new StreamListItem(&MavlinkStreamLocalPositionNEDCOV::new_instance, &MavlinkStreamLocalPositionNEDCOV::get_name_static, &MavlinkStreamLocalPositionNEDCOV::get_id_static),
+	new StreamListItem(&MavlinkStreamEstimatorStatus::new_instance, &MavlinkStreamEstimatorStatus::get_name_static, &MavlinkStreamEstimatorStatus::get_id_static),
+	new StreamListItem(&MavlinkStreamAttPosMocap::new_instance, &MavlinkStreamAttPosMocap::get_name_static, &MavlinkStreamAttPosMocap::get_id_static),
+	new StreamListItem(&MavlinkStreamHomePosition::new_instance, &MavlinkStreamHomePosition::get_name_static, &MavlinkStreamHomePosition::get_id_static),
+	new StreamListItem(&MavlinkStreamServoOutputRaw<0>::new_instance, &MavlinkStreamServoOutputRaw<0>::get_name_static, &MavlinkStreamServoOutputRaw<0>::get_id_static),
+	new StreamListItem(&MavlinkStreamServoOutputRaw<1>::new_instance, &MavlinkStreamServoOutputRaw<1>::get_name_static, &MavlinkStreamServoOutputRaw<1>::get_id_static),
+	new StreamListItem(&MavlinkStreamServoOutputRaw<2>::new_instance, &MavlinkStreamServoOutputRaw<2>::get_name_static, &MavlinkStreamServoOutputRaw<2>::get_id_static),
+	new StreamListItem(&MavlinkStreamServoOutputRaw<3>::new_instance, &MavlinkStreamServoOutputRaw<3>::get_name_static, &MavlinkStreamServoOutputRaw<3>::get_id_static),
+	new StreamListItem(&MavlinkStreamHILControls::new_instance, &MavlinkStreamHILControls::get_name_static, &MavlinkStreamHILControls::get_id_static),
+	new StreamListItem(&MavlinkStreamPositionTargetGlobalInt::new_instance, &MavlinkStreamPositionTargetGlobalInt::get_name_static, &MavlinkStreamPositionTargetGlobalInt::get_id_static),
+	new StreamListItem(&MavlinkStreamLocalPositionSetpoint::new_instance, &MavlinkStreamLocalPositionSetpoint::get_name_static, &MavlinkStreamLocalPositionSetpoint::get_id_static),
+	new StreamListItem(&MavlinkStreamAttitudeTarget::new_instance, &MavlinkStreamAttitudeTarget::get_name_static, &MavlinkStreamAttitudeTarget::get_id_static),
+	new StreamListItem(&MavlinkStreamRCChannels::new_instance, &MavlinkStreamRCChannels::get_name_static, &MavlinkStreamRCChannels::get_id_static),
+	new StreamListItem(&MavlinkStreamManualControl::new_instance, &MavlinkStreamManualControl::get_name_static, &MavlinkStreamManualControl::get_id_static),
+	new StreamListItem(&MavlinkStreamOpticalFlowRad::new_instance, &MavlinkStreamOpticalFlowRad::get_name_static, &MavlinkStreamOpticalFlowRad::get_id_static),
+	new StreamListItem(&MavlinkStreamActuatorControlTarget<0>::new_instance, &MavlinkStreamActuatorControlTarget<0>::get_name_static, &MavlinkStreamActuatorControlTarget<0>::get_id_static),
+	new StreamListItem(&MavlinkStreamActuatorControlTarget<1>::new_instance, &MavlinkStreamActuatorControlTarget<1>::get_name_static, &MavlinkStreamActuatorControlTarget<1>::get_id_static),
+	new StreamListItem(&MavlinkStreamActuatorControlTarget<2>::new_instance, &MavlinkStreamActuatorControlTarget<2>::get_name_static, &MavlinkStreamActuatorControlTarget<2>::get_id_static),
+	new StreamListItem(&MavlinkStreamActuatorControlTarget<3>::new_instance, &MavlinkStreamActuatorControlTarget<3>::get_name_static, &MavlinkStreamActuatorControlTarget<3>::get_id_static),
+	new StreamListItem(&MavlinkStreamNamedValueFloat::new_instance, &MavlinkStreamNamedValueFloat::get_name_static, &MavlinkStreamNamedValueFloat::get_id_static),
+	new StreamListItem(&MavlinkStreamNavControllerOutput::new_instance, &MavlinkStreamNavControllerOutput::get_name_static, &MavlinkStreamNavControllerOutput::get_id_static),
+	new StreamListItem(&MavlinkStreamCameraCapture::new_instance, &MavlinkStreamCameraCapture::get_name_static, &MavlinkStreamCameraCapture::get_id_static),
+	new StreamListItem(&MavlinkStreamCameraTrigger::new_instance, &MavlinkStreamCameraTrigger::get_name_static, &MavlinkStreamCameraTrigger::get_id_static),
+	new StreamListItem(&MavlinkStreamDistanceSensor::new_instance, &MavlinkStreamDistanceSensor::get_name_static, &MavlinkStreamDistanceSensor::get_id_static),
+	new StreamListItem(&MavlinkStreamExtendedSysState::new_instance, &MavlinkStreamExtendedSysState::get_name_static, &MavlinkStreamExtendedSysState::get_id_static),
+	new StreamListItem(&MavlinkStreamAltitude::new_instance, &MavlinkStreamAltitude::get_name_static, &MavlinkStreamAltitude::get_id_static),
+	new StreamListItem(&MavlinkStreamADSBVehicle::new_instance, &MavlinkStreamADSBVehicle::get_name_static, &MavlinkStreamADSBVehicle::get_id_static),
 	nullptr
 };
diff --git a/src/modules/mavlink/mavlink_messages.h b/src/modules/mavlink/mavlink_messages.h
index 5b6b9d6335..685eaff475 100644
--- a/src/modules/mavlink/mavlink_messages.h
+++ b/src/modules/mavlink/mavlink_messages.h
@@ -48,10 +48,12 @@ class StreamListItem {
 public:
 	MavlinkStream* (*new_instance)(Mavlink *mavlink);
 	const char* (*get_name)();
+	uint8_t (*get_id)();
 
-	StreamListItem(MavlinkStream* (*inst)(Mavlink *mavlink), const char* (*name)()) :
+	StreamListItem(MavlinkStream* (*inst)(Mavlink *mavlink), const char* (*name)(), uint8_t (*id)()) :
 		new_instance(inst),
-		get_name(name) {};
+		get_name(name),
+        get_id(id) {};
 
 	~StreamListItem() {};
 };
diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp
index 9d33711b9e..f04d98f887 100644
--- a/src/modules/mavlink/mavlink_receiver.cpp
+++ b/src/modules/mavlink/mavlink_receiver.cpp
@@ -355,6 +355,12 @@ MavlinkReceiver::handle_message_command_long(mavlink_message_t *msg)
 		} else if (cmd_mavlink.command == MAV_CMD_GET_HOME_POSITION) {
 			_mavlink->configure_stream_threadsafe("HOME_POSITION", 0.5f);
 
+        } else if (cmd_mavlink.command == MAV_CMD_SET_MESSAGE_INTERVAL) {
+            set_message_interval((int)cmd_mavlink.param1, cmd_mavlink.param2);
+
+        } else if (cmd_mavlink.command == MAV_CMD_GET_MESSAGE_INTERVAL) {
+            get_message_interval((int)cmd_mavlink.param1);
+
 		} else {
 
 			if (msg->sysid == mavlink_system.sysid && msg->compid == mavlink_system.compid) {
@@ -1368,37 +1374,75 @@ MavlinkReceiver::handle_message_request_data_stream(mavlink_message_t *msg)
 	mavlink_request_data_stream_t req;
 	mavlink_msg_request_data_stream_decode(msg, &req);
 
-	if (req.target_system == mavlink_system.sysid && req.target_component == mavlink_system.compid
-	    && req.req_message_rate != 0) {
-		float rate = req.start_stop ? (1000.0f / req.req_message_rate) : 0.0f;
+    PX4_WARN("REQUEST_DATA_STREAM is deprecated, please use SET_MESSAGE_INTERVAL instead");
 
-		MavlinkStream *stream;
-		LL_FOREACH(_mavlink->get_streams(), stream) {
-			if (req.req_stream_id == stream->get_id()) {
-				_mavlink->configure_stream_threadsafe(stream->get_name(), rate);
-                // if the command here was to unsubscribe then the stream will be deleted
-                // in which case we cannot continue this LL_FOREACH loop.  We have to return.
-                // Besides there shouldn't be more than one stream with the same id, right?
-				return;
-			}
-		}
+	if (req.target_system == mavlink_system.sysid && req.target_component == mavlink_system.compid &&
+	        req.req_stream_id != 0)
+	{
+	    // compute interval in microseconds.
+        float interval = 0;
+	    int rate = req.req_message_rate;
+	    if (req.start_stop == 0 || rate == 0) {
+	        rate = -1;
+	    }
+	    else if (rate > 0) {
+	        interval = 1000000.0f / rate;
+	    }
 
-        // If we get here then the stream may have been deleted, in which case we need to recreate it.
-        // todo: we could simplify this code by adding a get_id_static (get_name_static) so that
-		// we could do both subscribe and unsubscribe using the streams_list.
+	    // todo: note it is kind of bogus to interpret the req_stream_id as a message id
+	    // but this is how it was.  MAVLINK defines separate MAV_DATA_STREAM ids which bundle
+	    // messages into groups, but that is all deprecated now anyway, and this code was
+	    // previously interpreting the req_stream_id as a message id, so we'll leave it that way.
+	    set_message_interval(req.req_stream_id, interval);
+	}
+}
+
+void
+MavlinkReceiver::set_message_interval(int msgId, float interval)
+{
+    // configure_stream wants a rate (msgs/second), so convert here.
+    float rate = 0;
+    if (interval < 0) {
+        // stop the stream.
+        rate = 0;
+    }
+    else if (interval > 0) {
+        rate = 1000000.0f / interval;
+    }
+    else {
+        // note: mavlink spec says rate == 0 is requesting a default rate but our streams
+        // don't publish a default rate so for now let's pick a default rate of zero.
+    }
+
+    // The interval between two messages is in microseconds.
+    // Set to -1 to disable and 0 to request default rate
+    if (msgId != 0)
+    {
         for (unsigned int i = 0; streams_list[i] != nullptr; i++) {
-            MavlinkStream *new_instance = streams_list[i]->new_instance(_mavlink);
-            bool found = false;
-            if (req.req_stream_id == new_instance->get_id()) {
-                _mavlink->configure_stream_threadsafe(new_instance->get_name(), rate);
-                found = true;
-            }
-            delete new_instance;
-            if (found) {
+            const StreamListItem* item = streams_list[i];
+            if (msgId == item->get_id()) {
+                _mavlink->configure_stream_threadsafe(item->get_name(), rate);
                 break;
             }
         }
-	}
+    }
+}
+
+void
+MavlinkReceiver::get_message_interval(int msgId)
+{
+    unsigned interval = 0;
+
+    MavlinkStream *stream;
+    LL_FOREACH(_mavlink->get_streams(), stream) {
+        if (stream->get_id() == msgId) {
+            interval = stream->get_interval();
+            break;
+        }
+    }
+
+    // send back this value...
+    mavlink_msg_message_interval_send(_mavlink->get_channel(), msgId, interval);
 }
 
 void
diff --git a/src/modules/mavlink/mavlink_receiver.h b/src/modules/mavlink/mavlink_receiver.h
index 28ee0fa04e..7894c3b817 100644
--- a/src/modules/mavlink/mavlink_receiver.h
+++ b/src/modules/mavlink/mavlink_receiver.h
@@ -145,6 +145,13 @@ private:
 
 	void *receive_thread(void *arg);
 
+    /**
+     * Set the interval at which the given message stream is published.
+     * The rate is the number of messages per second.
+     */
+	void set_message_interval(int msgId, float rate);
+	void get_message_interval(int msgId);
+
 	/**
 	 * Convert remote timestamp to local hrt time (usec)
 	 * Use timesync if available, monotonic boot time otherwise

From 847d9ec4f49f318fddae20e677884eba1388c4a2 Mon Sep 17 00:00:00 2001
From: Chris Lovett <lovettchris@hotmail.com>
Date: Mon, 20 Jun 2016 19:14:03 -0700
Subject: [PATCH 0721/1230] Fix code style using Tools/fix_code_style.sh

---
 src/modules/mavlink/mavlink_messages.h   |  11 ++-
 src/modules/mavlink/mavlink_receiver.cpp | 119 ++++++++++++-----------
 src/modules/mavlink/mavlink_receiver.h   |   8 +-
 3 files changed, 72 insertions(+), 66 deletions(-)

diff --git a/src/modules/mavlink/mavlink_messages.h b/src/modules/mavlink/mavlink_messages.h
index 685eaff475..3c302891a2 100644
--- a/src/modules/mavlink/mavlink_messages.h
+++ b/src/modules/mavlink/mavlink_messages.h
@@ -43,17 +43,18 @@
 
 #include "mavlink_stream.h"
 
-class StreamListItem {
+class StreamListItem
+{
 
 public:
-	MavlinkStream* (*new_instance)(Mavlink *mavlink);
-	const char* (*get_name)();
+	MavlinkStream *(*new_instance)(Mavlink *mavlink);
+	const char *(*get_name)();
 	uint8_t (*get_id)();
 
-	StreamListItem(MavlinkStream* (*inst)(Mavlink *mavlink), const char* (*name)(), uint8_t (*id)()) :
+	StreamListItem(MavlinkStream * (*inst)(Mavlink *mavlink), const char *(*name)(), uint8_t (*id)()) :
 		new_instance(inst),
 		get_name(name),
-        get_id(id) {};
+		get_id(id) {};
 
 	~StreamListItem() {};
 };
diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp
index f04d98f887..1841861298 100644
--- a/src/modules/mavlink/mavlink_receiver.cpp
+++ b/src/modules/mavlink/mavlink_receiver.cpp
@@ -167,12 +167,14 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg)
 		if (_mavlink->accepting_commands()) {
 			handle_message_command_long(msg);
 		}
+
 		break;
 
 	case MAVLINK_MSG_ID_COMMAND_INT:
 		if (_mavlink->accepting_commands()) {
 			handle_message_command_int(msg);
 		}
+
 		break;
 
 	case MAVLINK_MSG_ID_OPTICAL_FLOW_RAD:
@@ -187,6 +189,7 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg)
 		if (_mavlink->accepting_commands()) {
 			handle_message_set_mode(msg);
 		}
+
 		break;
 
 	case MAVLINK_MSG_ID_ATT_POS_MOCAP:
@@ -229,6 +232,7 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg)
 		if (_mavlink->accepting_commands()) {
 			handle_message_request_data_stream(msg);
 		}
+
 		break;
 
 	case MAVLINK_MSG_ID_SYSTEM_TIME:
@@ -355,11 +359,11 @@ MavlinkReceiver::handle_message_command_long(mavlink_message_t *msg)
 		} else if (cmd_mavlink.command == MAV_CMD_GET_HOME_POSITION) {
 			_mavlink->configure_stream_threadsafe("HOME_POSITION", 0.5f);
 
-        } else if (cmd_mavlink.command == MAV_CMD_SET_MESSAGE_INTERVAL) {
-            set_message_interval((int)cmd_mavlink.param1, cmd_mavlink.param2);
+		} else if (cmd_mavlink.command == MAV_CMD_SET_MESSAGE_INTERVAL) {
+			set_message_interval((int)cmd_mavlink.param1, cmd_mavlink.param2);
 
-        } else if (cmd_mavlink.command == MAV_CMD_GET_MESSAGE_INTERVAL) {
-            get_message_interval((int)cmd_mavlink.param1);
+		} else if (cmd_mavlink.command == MAV_CMD_GET_MESSAGE_INTERVAL) {
+			get_message_interval((int)cmd_mavlink.param1);
 
 		} else {
 
@@ -539,7 +543,7 @@ MavlinkReceiver::handle_message_optical_flow_rad(mavlink_message_t *msg)
 
 		if (_flow_distance_sensor_pub == nullptr) {
 			_flow_distance_sensor_pub = orb_advertise_multi(ORB_ID(distance_sensor), &d,
-						   &_orb_class_instance, ORB_PRIO_HIGH);
+						    &_orb_class_instance, ORB_PRIO_HIGH);
 
 		} else {
 			orb_publish(ORB_ID(distance_sensor), _flow_distance_sensor_pub, &d);
@@ -1374,75 +1378,76 @@ MavlinkReceiver::handle_message_request_data_stream(mavlink_message_t *msg)
 	mavlink_request_data_stream_t req;
 	mavlink_msg_request_data_stream_decode(msg, &req);
 
-    PX4_WARN("REQUEST_DATA_STREAM is deprecated, please use SET_MESSAGE_INTERVAL instead");
+	PX4_WARN("REQUEST_DATA_STREAM is deprecated, please use SET_MESSAGE_INTERVAL instead");
 
 	if (req.target_system == mavlink_system.sysid && req.target_component == mavlink_system.compid &&
-	        req.req_stream_id != 0)
-	{
-	    // compute interval in microseconds.
-        float interval = 0;
-	    int rate = req.req_message_rate;
-	    if (req.start_stop == 0 || rate == 0) {
-	        rate = -1;
-	    }
-	    else if (rate > 0) {
-	        interval = 1000000.0f / rate;
-	    }
+	    req.req_stream_id != 0) {
+		// compute interval in microseconds.
+		float interval = 0;
+		int rate = req.req_message_rate;
 
-	    // todo: note it is kind of bogus to interpret the req_stream_id as a message id
-	    // but this is how it was.  MAVLINK defines separate MAV_DATA_STREAM ids which bundle
-	    // messages into groups, but that is all deprecated now anyway, and this code was
-	    // previously interpreting the req_stream_id as a message id, so we'll leave it that way.
-	    set_message_interval(req.req_stream_id, interval);
+		if (req.start_stop == 0 || rate == 0) {
+			rate = -1;
+
+		} else if (rate > 0) {
+			interval = 1000000.0f / rate;
+		}
+
+		// todo: note it is kind of bogus to interpret the req_stream_id as a message id
+		// but this is how it was.  MAVLINK defines separate MAV_DATA_STREAM ids which bundle
+		// messages into groups, but that is all deprecated now anyway, and this code was
+		// previously interpreting the req_stream_id as a message id, so we'll leave it that way.
+		set_message_interval(req.req_stream_id, interval);
 	}
 }
 
 void
 MavlinkReceiver::set_message_interval(int msgId, float interval)
 {
-    // configure_stream wants a rate (msgs/second), so convert here.
-    float rate = 0;
-    if (interval < 0) {
-        // stop the stream.
-        rate = 0;
-    }
-    else if (interval > 0) {
-        rate = 1000000.0f / interval;
-    }
-    else {
-        // note: mavlink spec says rate == 0 is requesting a default rate but our streams
-        // don't publish a default rate so for now let's pick a default rate of zero.
-    }
+	// configure_stream wants a rate (msgs/second), so convert here.
+	float rate = 0;
 
-    // The interval between two messages is in microseconds.
-    // Set to -1 to disable and 0 to request default rate
-    if (msgId != 0)
-    {
-        for (unsigned int i = 0; streams_list[i] != nullptr; i++) {
-            const StreamListItem* item = streams_list[i];
-            if (msgId == item->get_id()) {
-                _mavlink->configure_stream_threadsafe(item->get_name(), rate);
-                break;
-            }
-        }
-    }
+	if (interval < 0) {
+		// stop the stream.
+		rate = 0;
+
+	} else if (interval > 0) {
+		rate = 1000000.0f / interval;
+
+	} else {
+		// note: mavlink spec says rate == 0 is requesting a default rate but our streams
+		// don't publish a default rate so for now let's pick a default rate of zero.
+	}
+
+	// The interval between two messages is in microseconds.
+	// Set to -1 to disable and 0 to request default rate
+	if (msgId != 0) {
+		for (unsigned int i = 0; streams_list[i] != nullptr; i++) {
+			const StreamListItem *item = streams_list[i];
+
+			if (msgId == item->get_id()) {
+				_mavlink->configure_stream_threadsafe(item->get_name(), rate);
+				break;
+			}
+		}
+	}
 }
 
 void
 MavlinkReceiver::get_message_interval(int msgId)
 {
-    unsigned interval = 0;
+	unsigned interval = 0;
 
-    MavlinkStream *stream;
-    LL_FOREACH(_mavlink->get_streams(), stream) {
-        if (stream->get_id() == msgId) {
-            interval = stream->get_interval();
-            break;
-        }
-    }
+	MavlinkStream *stream;
+	LL_FOREACH(_mavlink->get_streams(), stream) {
+		if (stream->get_id() == msgId) {
+			interval = stream->get_interval();
+			break;
+		}
+	}
 
-    // send back this value...
-    mavlink_msg_message_interval_send(_mavlink->get_channel(), msgId, interval);
+	// send back this value...
+	mavlink_msg_message_interval_send(_mavlink->get_channel(), msgId, interval);
 }
 
 void
diff --git a/src/modules/mavlink/mavlink_receiver.h b/src/modules/mavlink/mavlink_receiver.h
index 7894c3b817..45e904d295 100644
--- a/src/modules/mavlink/mavlink_receiver.h
+++ b/src/modules/mavlink/mavlink_receiver.h
@@ -145,10 +145,10 @@ private:
 
 	void *receive_thread(void *arg);
 
-    /**
-     * Set the interval at which the given message stream is published.
-     * The rate is the number of messages per second.
-     */
+	/**
+	 * Set the interval at which the given message stream is published.
+	 * The rate is the number of messages per second.
+	 */
 	void set_message_interval(int msgId, float rate);
 	void get_message_interval(int msgId);
 

From 178f32ab41d8d300dcfeb6c4b382f0daba1e4641 Mon Sep 17 00:00:00 2001
From: Henry Zhang <zhanghui629@gmail.com>
Date: Tue, 21 Jun 2016 16:20:33 +0800
Subject: [PATCH 0722/1230] Commander:fill missing command ack. (#4814)

---
 src/modules/commander/commander.cpp | 17 ++++++++++++++---
 1 file changed, 14 insertions(+), 3 deletions(-)

diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp
index c9b50d162a..4d545fbfec 100644
--- a/src/modules/commander/commander.cpp
+++ b/src/modules/commander/commander.cpp
@@ -813,9 +813,8 @@ bool handle_command(struct vehicle_status_s *status_local, const struct safety_s
 				// Flick to inair restore first if this comes from an onboard system
 				if (cmd->source_system == status_local->system_id && cmd->source_component == status_local->component_id) {
 					status.arming_state = vehicle_status_s::ARMING_STATE_IN_AIR_RESTORE;
-				}
-				else {
 
+				} else {
 					// Refuse to arm if preflight checks have failed
 					if ((!status_local->hil_state) != vehicle_status_s::HIL_STATE_ON && !status_flags.condition_system_sensors_initialized) {
 						mavlink_log_critical(&mavlink_log_pub, "Arming DENIED. Preflight checks have failed.");
@@ -1014,6 +1013,13 @@ bool handle_command(struct vehicle_status_s *status_local, const struct safety_s
 				res = main_state_transition(status_local, main_state_pre_offboard, main_state_prev, &status_flags, &internal_state);
 				status_flags.offboard_control_set_by_command = false;
 			}
+
+			if (res == TRANSITION_DENIED) {
+				cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED;
+
+			} else {
+				cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED;
+			}
 		}
 		break;
 
@@ -1021,19 +1027,24 @@ bool handle_command(struct vehicle_status_s *status_local, const struct safety_s
 			/* ok, home set, use it to take off */
 			if (TRANSITION_CHANGED == main_state_transition(&status, commander_state_s::MAIN_STATE_AUTO_TAKEOFF, main_state_prev, &status_flags, &internal_state)) {
 				mavlink_and_console_log_info(&mavlink_log_pub, "Taking off");
+				cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED;
+
 			} else {
 				mavlink_and_console_log_critical(&mavlink_log_pub, "Takeoff denied, disarm and re-try");
+				cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED;
 			}
 
 		}
 		break;
 
 	case vehicle_command_s::VEHICLE_CMD_NAV_LAND: {
-			/* ok, home set, use it to take off */
 			if (TRANSITION_CHANGED == main_state_transition(&status, commander_state_s::MAIN_STATE_AUTO_LAND, main_state_prev, &status_flags, &internal_state)) {
 				mavlink_and_console_log_info(&mavlink_log_pub, "Landing at current position");
+				cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED;
+
 			} else {
 				mavlink_and_console_log_critical(&mavlink_log_pub, "Landing denied, land manually.");
+				cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED;
 			}
 
 		}

From 4d10759699b0e20a0223b1049377b2a35c08fe47 Mon Sep 17 00:00:00 2001
From: Julian Oes <julian@oes.ch>
Date: Tue, 17 May 2016 10:51:55 -0400
Subject: [PATCH 0723/1230] commander: use DESCEND mode and not LANDGPSFAIL

Always use the DESCEND mode and not LANDGPSFAIL because LANDGPSFAIL will
try to loiter for some time and then request termination instead of
descending gently and trying to land.
---
 src/modules/commander/state_machine_helper.cpp | 6 ++++--
 1 file changed, 4 insertions(+), 2 deletions(-)

diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp
index 8cfc38b109..8e266bf16a 100644
--- a/src/modules/commander/state_machine_helper.cpp
+++ b/src/modules/commander/state_machine_helper.cpp
@@ -665,7 +665,8 @@ bool set_nav_state(struct vehicle_status_s *status, struct commander_state_s *in
 		} else if (status_flags->data_link_lost_cmd) {
 			status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_RTGS;
 		} else if (status_flags->gps_failure_cmd) {
-			status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LANDGPSFAIL;
+			status->nav_state = vehicle_status_s::NAVIGATION_STATE_DESCEND;
+			status->failsafe = true;
 		} else if (status_flags->rc_signal_lost_cmd) {
 			status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_RCRECOVER;
 		} else if (status_flags->vtol_transition_failure_cmd) {
@@ -673,7 +674,8 @@ bool set_nav_state(struct vehicle_status_s *status, struct commander_state_s *in
 
 		/* finished handling commands which have priority, now handle failures */
 		} else if (status_flags->gps_failure) {
-			status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LANDGPSFAIL;
+			status->nav_state = vehicle_status_s::NAVIGATION_STATE_DESCEND;
+			status->failsafe = true;
 		} else if (status->engine_failure) {
 			status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LANDENGFAIL;
 		} else if (status_flags->vtol_transition_failure) {

From fe29d99d62b60a81a0b855f6e8825133e74b2ae8 Mon Sep 17 00:00:00 2001
From: Julian Oes <julian@oes.ch>
Date: Tue, 17 May 2016 10:53:07 -0400
Subject: [PATCH 0724/1230] commander: use the gps_failure flag

The gps_failure flag had been ignored in some navigation states.
---
 src/modules/commander/state_machine_helper.cpp | 14 ++++++++++++--
 1 file changed, 12 insertions(+), 2 deletions(-)

diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp
index 8e266bf16a..11e0c68b53 100644
--- a/src/modules/commander/state_machine_helper.cpp
+++ b/src/modules/commander/state_machine_helper.cpp
@@ -725,6 +725,11 @@ bool set_nav_state(struct vehicle_status_s *status, struct commander_state_s *in
 		/* go into failsafe on a engine failure */
 		if (status->engine_failure) {
 			status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LANDENGFAIL;
+
+		} else if (status_flags->gps_failure) {
+			status->nav_state = vehicle_status_s::NAVIGATION_STATE_DESCEND;
+			status->failsafe = true;
+
 		/* also go into failsafe if just datalink is lost */
 		} else if (status->data_link_lost && data_link_loss_enabled) {
 			status->failsafe = true;
@@ -769,6 +774,11 @@ bool set_nav_state(struct vehicle_status_s *status, struct commander_state_s *in
 
 		if (status->engine_failure) {
 			status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LANDENGFAIL;
+
+		} else if (status_flags->gps_failure) {
+			status->nav_state = vehicle_status_s::NAVIGATION_STATE_DESCEND;
+			status->failsafe = true;
+
 		} else if ((!status_flags->condition_global_position_valid ||
 					!status_flags->condition_home_position_valid)) {
 			status->failsafe = true;
@@ -810,7 +820,7 @@ bool set_nav_state(struct vehicle_status_s *status, struct commander_state_s *in
 
 		if (status->engine_failure) {
 			status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LANDENGFAIL;
-		} else if ((!status_flags->condition_global_position_valid ||
+		} else if (status_flags->gps_failure || (!status_flags->condition_global_position_valid ||
 					!status_flags->condition_home_position_valid)) {
 			status->failsafe = true;
 
@@ -831,7 +841,7 @@ bool set_nav_state(struct vehicle_status_s *status, struct commander_state_s *in
 
 		if (status->engine_failure) {
 			status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LANDENGFAIL;
-		} else if ((!status_flags->condition_global_position_valid ||
+		} else if (status_flags->gps_failure || (!status_flags->condition_global_position_valid ||
 					!status_flags->condition_home_position_valid)) {
 			status->failsafe = true;
 

From 739d1cfd3fe9ede871050b9c71f668ed92fcaede Mon Sep 17 00:00:00 2001
From: Julian Oes <julian@oes.ch>
Date: Tue, 17 May 2016 10:54:48 -0400
Subject: [PATCH 0725/1230] navigator: always run the loop, even without GPS

If the navigator stops when no more position updates arrive, it won't
switch to land mode when DESCEND is requested by the commander.
---
 src/modules/navigator/navigator_main.cpp | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp
index e74e75905e..c8b321fd62 100644
--- a/src/modules/navigator/navigator_main.cpp
+++ b/src/modules/navigator/navigator_main.cpp
@@ -338,10 +338,10 @@ Navigator::task_main()
 		if (pret == 0) {
 			/* timed out - periodic check for _task_should_exit, etc. */
 			if (global_pos_available_once) {
-				PX4_WARN("no GPS - navigator timed out");
 				global_pos_available_once = false;
+				PX4_WARN("navigator: global position timeout");
 			}
-			continue;
+			/* Let the loop run anyway, don't do `continue` here. */
 
 		} else if (pret < 0) {
 			/* this is undesirable but not much we can do - might want to flag unhappy status */

From ea10c8c8a36a070a4530423d11f7b3c953b1f32b Mon Sep 17 00:00:00 2001
From: Julian Oes <julian@oes.ch>
Date: Tue, 17 May 2016 10:55:47 -0400
Subject: [PATCH 0726/1230] circuit_breaker: change default for GPS failure

We should definitely take action when GPS fails, this circuit breaker
shouldn't be engaged anymore.
---
 src/modules/systemlib/circuit_breaker_params.c | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/src/modules/systemlib/circuit_breaker_params.c b/src/modules/systemlib/circuit_breaker_params.c
index f75b09120d..74817bb195 100644
--- a/src/modules/systemlib/circuit_breaker_params.c
+++ b/src/modules/systemlib/circuit_breaker_params.c
@@ -140,7 +140,7 @@ PARAM_DEFINE_INT32(CBRK_ENGINEFAIL, 284953);
  * @max 240024
  * @group Circuit Breaker
  */
-PARAM_DEFINE_INT32(CBRK_GPSFAIL, 240024);
+PARAM_DEFINE_INT32(CBRK_GPSFAIL, 0);
 
 /**
  * Circuit breaker for disabling buzzer

From 016d514d8063dc22d55981f32b963f6b139db736 Mon Sep 17 00:00:00 2001
From: Julian Oes <julian@oes.ch>
Date: Sun, 5 Jun 2016 14:27:57 +0200
Subject: [PATCH 0727/1230] commander/navigator: remove param NAV_RCL_ACT

The param NAV_RCL_ACT was not implemented as described. Also, it has the
completely the wrong name. It should be a COM param and not NAV.
Therefore, remove the param and delete the partly implemented
and probably never used functionality.
---
 src/modules/commander/commander.cpp            |  4 ----
 src/modules/commander/state_machine_helper.cpp |  4 ++--
 src/modules/commander/state_machine_helper.h   |  2 +-
 src/modules/navigator/navigator_params.c       | 18 ------------------
 4 files changed, 3 insertions(+), 25 deletions(-)

diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp
index 4d545fbfec..972ee38ea5 100644
--- a/src/modules/commander/commander.cpp
+++ b/src/modules/commander/commander.cpp
@@ -1171,7 +1171,6 @@ int commander_thread_main(int argc, char *argv[])
 	param_t _param_system_id = param_find("MAV_SYS_ID");
 	param_t _param_component_id = param_find("MAV_COMP_ID");
 	param_t _param_enable_datalink_loss = param_find("NAV_DLL_ACT");
-	param_t _param_enable_rc_loss = param_find("NAV_RCL_ACT");
 	param_t _param_offboard_loss_act = param_find("COM_OBL_ACT");
 	param_t _param_offboard_loss_rc_act = param_find("COM_OBL_RC_ACT");
 	param_t _param_datalink_loss_timeout = param_find("COM_DL_LOSS_T");
@@ -1526,7 +1525,6 @@ int commander_thread_main(int argc, char *argv[])
 	transition_result_t arming_ret;
 
 	int32_t datalink_loss_enabled = 0;
-	int32_t rc_loss_enabled = 0;
 	int32_t datalink_loss_timeout = 10;
 	float rc_loss_timeout = 0.5;
 	int32_t datalink_regain_timeout = 0;
@@ -1613,7 +1611,6 @@ int commander_thread_main(int argc, char *argv[])
 
 			/* Safety parameters */
 			param_get(_param_enable_datalink_loss, &datalink_loss_enabled);
-			param_get(_param_enable_rc_loss, &rc_loss_enabled);
 			param_get(_param_datalink_loss_timeout, &datalink_loss_timeout);
 			param_get(_param_rc_loss_timeout, &rc_loss_timeout);
 			param_get(_param_rc_in_off, &rc_in_off);
@@ -2708,7 +2705,6 @@ int commander_thread_main(int argc, char *argv[])
 						       mission_result.stay_in_failsafe,
 						       &status_flags,
 						       land_detector.landed,
-						       (rc_loss_enabled > 0),
 						       offboard_loss_act,
 						       offboard_loss_rc_act);
 
diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp
index 11e0c68b53..8e18c3f2aa 100644
--- a/src/modules/commander/state_machine_helper.cpp
+++ b/src/modules/commander/state_machine_helper.cpp
@@ -588,7 +588,7 @@ transition_result_t hil_state_transition(hil_state_t new_state, orb_advert_t sta
  */
 bool set_nav_state(struct vehicle_status_s *status, struct commander_state_s *internal_state,
 		   const bool data_link_loss_enabled, const bool mission_finished,
-		   const bool stay_in_failsafe, status_flags_s *status_flags, bool landed, const bool rc_loss_enabled,
+		   const bool stay_in_failsafe, status_flags_s *status_flags, bool landed,
 		   const int offb_loss_act, const int offb_loss_rc_act)
 {
 	navigation_state_t nav_state_old = status->nav_state;
@@ -605,7 +605,7 @@ bool set_nav_state(struct vehicle_status_s *status, struct commander_state_s *in
 	case commander_state_s::MAIN_STATE_ALTCTL:
 	case commander_state_s::MAIN_STATE_POSCTL:
 		/* require RC for all manual modes */
-		if (rc_loss_enabled && (status->rc_signal_lost || status_flags->rc_signal_lost_cmd) && armed && !landed) {
+		if ((status->rc_signal_lost || status_flags->rc_signal_lost_cmd) && armed && !landed) {
 			status->failsafe = true;
 
 			if (status_flags->condition_global_position_valid && status_flags->condition_home_position_valid) {
diff --git a/src/modules/commander/state_machine_helper.h b/src/modules/commander/state_machine_helper.h
index 4012c0e89f..07d616b5b9 100644
--- a/src/modules/commander/state_machine_helper.h
+++ b/src/modules/commander/state_machine_helper.h
@@ -114,7 +114,7 @@ transition_result_t hil_state_transition(hil_state_t new_state, orb_advert_t sta
 bool set_nav_state(struct vehicle_status_s *status, struct commander_state_s *internal_state,
 		   const bool data_link_loss_enabled, const bool mission_finished,
 		   const bool stay_in_failsafe, status_flags_s *status_flags, bool landed,
-		   const bool rc_loss_enabled, const int offb_loss_act, const int offb_loss_rc_act);
+		   const int offb_loss_act, const int offb_loss_rc_act);
 
 int preflight_check(struct vehicle_status_s *status, orb_advert_t *mavlink_log_pub, bool prearm, bool force_report, status_flags_s *status_flags, battery_status_s *battery);
 
diff --git a/src/modules/navigator/navigator_params.c b/src/modules/navigator/navigator_params.c
index 523f03b42e..415a51678a 100644
--- a/src/modules/navigator/navigator_params.c
+++ b/src/modules/navigator/navigator_params.c
@@ -86,24 +86,6 @@ PARAM_DEFINE_FLOAT(NAV_ACC_RAD, 10.0f);
  */
 PARAM_DEFINE_INT32(NAV_DLL_ACT, 0);
 
-/**
- * Set RC loss failsafe mode
- *
- * The RC loss failsafe will only be entered after a timeout,
- * set by COM_RC_LOSS_T in seconds. If RC input checks have been disabled
- * by setting the COM_RC_IN_MODE param it will not be triggered.
- * Setting this parameter to 4 will enable CASA Outback Challenge rules,
- * which are only recommended to participants of that competition.
- *
- * @value 0 Disabled
- * @value 1 Loiter
- * @value 2 Return to Land
- * @value 3 Land at current position
- *
- * @group Mission
- */
-PARAM_DEFINE_INT32(NAV_RCL_ACT, 0);
-
 /**
  * Airfield home Lat
  *

From 3e9d1388af60152efea84ff4c0f6e58488338511 Mon Sep 17 00:00:00 2001
From: Julian Oes <julian@oes.ch>
Date: Sun, 5 Jun 2016 14:29:32 +0200
Subject: [PATCH 0728/1230] commander: remove unused/wrong failsafe handling

The deleted code conflicts with the failsafe handling in set_nav_state.
Also, flight termination was usually disabled by circuit breaker which
means this code had no effect anyway.
---
 src/modules/commander/commander.cpp | 52 -----------------------------
 1 file changed, 52 deletions(-)

diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp
index 972ee38ea5..01dbfe6288 100644
--- a/src/modules/commander/commander.cpp
+++ b/src/modules/commander/commander.cpp
@@ -2622,58 +2622,6 @@ int commander_thread_main(int argc, char *argv[])
 			}
 		}
 
-		/* Check for failure combinations which lead to flight termination */
-		if (armed.armed) {
-			/* At this point the data link and the gps system have been checked
-			 * If we are not in a manual (RC stick controlled mode)
-			 * and both failed we want to terminate the flight */
-			if (internal_state.main_state != commander_state_s::MAIN_STATE_MANUAL &&
-			    internal_state.main_state != commander_state_s::MAIN_STATE_ACRO &&
-			    internal_state.main_state != commander_state_s::MAIN_STATE_RATTITUDE &&
-			    internal_state.main_state != commander_state_s::MAIN_STATE_STAB &&
-			    internal_state.main_state != commander_state_s::MAIN_STATE_ALTCTL &&
-			    internal_state.main_state != commander_state_s::MAIN_STATE_POSCTL &&
-			    ((status.data_link_lost && status_flags.gps_failure) ||
-			     (status_flags.data_link_lost_cmd && status_flags.gps_failure_cmd))) {
-				armed.force_failsafe = true;
-				status_changed = true;
-				static bool flight_termination_printed = false;
-
-				if (!flight_termination_printed) {
-					mavlink_and_console_log_critical(&mavlink_log_pub, "DL and GPS lost: flight termination");
-					flight_termination_printed = true;
-				}
-
-				if (counter % (1000000 / COMMANDER_MONITORING_INTERVAL) == 0) {
-					mavlink_log_critical(&mavlink_log_pub, "DL and GPS lost: flight termination");
-				}
-			}
-
-			/* At this point the rc signal and the gps system have been checked
-			 * If we are in manual (controlled with RC):
-			 * if both failed we want to terminate the flight */
-			if ((internal_state.main_state == commander_state_s::MAIN_STATE_ACRO ||
-			     internal_state.main_state == commander_state_s::MAIN_STATE_RATTITUDE ||
-			     internal_state.main_state == commander_state_s::MAIN_STATE_MANUAL ||
-			     internal_state.main_state == commander_state_s::MAIN_STATE_STAB ||
-			     internal_state.main_state == commander_state_s::MAIN_STATE_ALTCTL ||
-			     internal_state.main_state == commander_state_s::MAIN_STATE_POSCTL) &&
-			    ((status.rc_signal_lost && status_flags.gps_failure) ||
-			     (status_flags.rc_signal_lost_cmd && status_flags.gps_failure_cmd))) {
-				armed.force_failsafe = true;
-				status_changed = true;
-				static bool flight_termination_printed = false;
-
-				if (!flight_termination_printed) {
-					warnx("Flight termination because of RC signal loss and GPS failure");
-					flight_termination_printed = true;
-				}
-
-				if (counter % (1000000 / COMMANDER_MONITORING_INTERVAL) == 0) {
-					mavlink_log_critical(&mavlink_log_pub, "RC and GPS lost: flight termination");
-				}
-			}
-		}
 
 		/* Get current timestamp */
 		const hrt_abstime now = hrt_absolute_time();

From 049146ef9c83efb440b8b2edf1f70520bd0db786 Mon Sep 17 00:00:00 2001
From: Julian Oes <julian@oes.ch>
Date: Mon, 6 Jun 2016 11:18:26 +0200
Subject: [PATCH 0729/1230] commander: param to allow arming without GPS

---
 src/modules/commander/commander.cpp      | 10 +++++++---
 src/modules/commander/commander_params.c | 13 +++++++++++++
 2 files changed, 20 insertions(+), 3 deletions(-)

diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp
index 01dbfe6288..560686831f 100644
--- a/src/modules/commander/commander.cpp
+++ b/src/modules/commander/commander.cpp
@@ -1189,6 +1189,7 @@ int commander_thread_main(int argc, char *argv[])
 	param_t _param_disarm_land = param_find("COM_DISARM_LAND");
 	param_t _param_low_bat_act = param_find("COM_LOW_BAT_ACT");
 	param_t _param_offboard_loss_timeout = param_find("COM_OF_LOSS_T");
+	param_t _param_arm_without_gps = param_find("COM_ARM_WO_GPS");
 
 	param_t _param_fmode_1 = param_find("COM_FLTMODE1");
 	param_t _param_fmode_2 = param_find("COM_FLTMODE2");
@@ -1500,8 +1501,10 @@ int commander_thread_main(int argc, char *argv[])
 	// Run preflight check
 	int32_t rc_in_off = 0;
 	bool hotplug_timeout = hrt_elapsed_time(&commander_boot_timestamp) > HOTPLUG_SENS_TIMEOUT;
+	int32_t arm_without_gps = 0;
 	param_get(_param_autostart_id, &autostart_id);
 	param_get(_param_rc_in_off, &rc_in_off);
+	param_get(_param_arm_without_gps, &arm_without_gps);
 	status.rc_input_mode = rc_in_off;
 	if (is_hil_setup(autostart_id)) {
 		// HIL configuration selected: real sensors will be disabled
@@ -1511,7 +1514,7 @@ int commander_thread_main(int argc, char *argv[])
 			// sensor diagnostics done continuously, not just at boot so don't warn about any issues just yet
 			status_flags.condition_system_sensors_initialized = Commander::preflightCheck(&mavlink_log_pub, true, true, true, true,
 				checkAirspeed, (status.rc_input_mode == vehicle_status_s::RC_IN_MODE_DEFAULT),
-				!status_flags.circuit_breaker_engaged_gpsfailure_check, false);
+				(arm_without_gps == 0), false);
 			set_tune_override(TONE_STARTUP_TUNE); //normal boot tune
 	}
 
@@ -1627,6 +1630,7 @@ int commander_thread_main(int argc, char *argv[])
 			param_get(_param_offboard_loss_timeout, &offboard_loss_timeout);
 			param_get(_param_offboard_loss_act, &offboard_loss_act);
 			param_get(_param_offboard_loss_rc_act, &offboard_loss_rc_act);
+			param_get(_param_arm_without_gps, &arm_without_gps);
 
 			/* Autostart id */
 			param_get(_param_autostart_id, &autostart_id);
@@ -1739,7 +1743,7 @@ int commander_thread_main(int argc, char *argv[])
 					} else {
 						/* check sensors also */
 						(void)Commander::preflightCheck(&mavlink_log_pub, true, true, true, true, chAirspeed,
-								(status.rc_input_mode == vehicle_status_s::RC_IN_MODE_DEFAULT), !status_flags.circuit_breaker_engaged_gpsfailure_check, hotplug_timeout);
+								(status.rc_input_mode == vehicle_status_s::RC_IN_MODE_DEFAULT), (arm_without_gps == 0), hotplug_timeout);
 					}
 				}
 
@@ -3723,7 +3727,7 @@ void *commander_low_prio_loop(void *arg)
 						}
 
 						status_flags.condition_system_sensors_initialized = Commander::preflightCheck(&mavlink_log_pub, true, true, true, true, checkAirspeed,
-							!(status.rc_input_mode >= vehicle_status_s::RC_IN_MODE_OFF), !status_flags.circuit_breaker_engaged_gpsfailure_check, hotplug_timeout);
+							!(status.rc_input_mode >= vehicle_status_s::RC_IN_MODE_OFF), false, hotplug_timeout);
 
 						arming_state_transition(&status,
 									&battery,
diff --git a/src/modules/commander/commander_params.c b/src/modules/commander/commander_params.c
index fc9fcf5372..83d67a1264 100644
--- a/src/modules/commander/commander_params.c
+++ b/src/modules/commander/commander_params.c
@@ -262,6 +262,19 @@ PARAM_DEFINE_INT32(COM_RC_ARM_HYST, 1000);
  */
 PARAM_DEFINE_INT32(COM_DISARM_LAND, 0);
 
+/**
+ * Allow arming without GPS
+ *
+ * The default allows to arm the vehicle without GPS signal.
+ *
+ * @group Commander
+ * @min 0
+ * @max 1
+ * @value 0 Don't allow arming without GPS
+ * @value 1 Allow arming without GPS
+ */
+PARAM_DEFINE_INT32(COM_ARM_WO_GPS, 1);
+
 /**
  * Battery failsafe mode
  *

From d0355cef1f38909dd8fbc00b1e1fe8ed77fc5c69 Mon Sep 17 00:00:00 2001
From: Julian Oes <julian@oes.ch>
Date: Mon, 6 Jun 2016 11:19:10 +0200
Subject: [PATCH 0730/1230] Revert "commander/navigator: remove param
 NAV_RCL_ACT"

This reverts commit 77ea4cebf41cd106fe771b9eb469aa2326339467.
---
 src/modules/commander/commander.cpp            |  4 ++++
 src/modules/commander/state_machine_helper.cpp |  4 ++--
 src/modules/commander/state_machine_helper.h   |  2 +-
 src/modules/navigator/navigator_params.c       | 18 ++++++++++++++++++
 4 files changed, 25 insertions(+), 3 deletions(-)

diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp
index 560686831f..9d20a0ed0b 100644
--- a/src/modules/commander/commander.cpp
+++ b/src/modules/commander/commander.cpp
@@ -1173,6 +1173,7 @@ int commander_thread_main(int argc, char *argv[])
 	param_t _param_enable_datalink_loss = param_find("NAV_DLL_ACT");
 	param_t _param_offboard_loss_act = param_find("COM_OBL_ACT");
 	param_t _param_offboard_loss_rc_act = param_find("COM_OBL_RC_ACT");
+	param_t _param_enable_rc_loss = param_find("NAV_RCL_ACT");
 	param_t _param_datalink_loss_timeout = param_find("COM_DL_LOSS_T");
 	param_t _param_rc_loss_timeout = param_find("COM_RC_LOSS_T");
 	param_t _param_datalink_regain_timeout = param_find("COM_DL_REG_T");
@@ -1528,6 +1529,7 @@ int commander_thread_main(int argc, char *argv[])
 	transition_result_t arming_ret;
 
 	int32_t datalink_loss_enabled = 0;
+	int32_t rc_loss_enabled = 0;
 	int32_t datalink_loss_timeout = 10;
 	float rc_loss_timeout = 0.5;
 	int32_t datalink_regain_timeout = 0;
@@ -1614,6 +1616,7 @@ int commander_thread_main(int argc, char *argv[])
 
 			/* Safety parameters */
 			param_get(_param_enable_datalink_loss, &datalink_loss_enabled);
+			param_get(_param_enable_rc_loss, &rc_loss_enabled);
 			param_get(_param_datalink_loss_timeout, &datalink_loss_timeout);
 			param_get(_param_rc_loss_timeout, &rc_loss_timeout);
 			param_get(_param_rc_in_off, &rc_in_off);
@@ -2657,6 +2660,7 @@ int commander_thread_main(int argc, char *argv[])
 						       mission_result.stay_in_failsafe,
 						       &status_flags,
 						       land_detector.landed,
+						       (rc_loss_enabled > 0),
 						       offboard_loss_act,
 						       offboard_loss_rc_act);
 
diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp
index 8e18c3f2aa..b53b80acaf 100644
--- a/src/modules/commander/state_machine_helper.cpp
+++ b/src/modules/commander/state_machine_helper.cpp
@@ -589,7 +589,7 @@ transition_result_t hil_state_transition(hil_state_t new_state, orb_advert_t sta
 bool set_nav_state(struct vehicle_status_s *status, struct commander_state_s *internal_state,
 		   const bool data_link_loss_enabled, const bool mission_finished,
 		   const bool stay_in_failsafe, status_flags_s *status_flags, bool landed,
-		   const int offb_loss_act, const int offb_loss_rc_act)
+		   const bool rc_loss_enabled, const int offb_loss_act, const int offb_loss_rc_act)
 {
 	navigation_state_t nav_state_old = status->nav_state;
 
@@ -605,7 +605,7 @@ bool set_nav_state(struct vehicle_status_s *status, struct commander_state_s *in
 	case commander_state_s::MAIN_STATE_ALTCTL:
 	case commander_state_s::MAIN_STATE_POSCTL:
 		/* require RC for all manual modes */
-		if ((status->rc_signal_lost || status_flags->rc_signal_lost_cmd) && armed && !landed) {
+		if (rc_loss_enabled && (status->rc_signal_lost || status_flags->rc_signal_lost_cmd) && armed && !landed) {
 			status->failsafe = true;
 
 			if (status_flags->condition_global_position_valid && status_flags->condition_home_position_valid) {
diff --git a/src/modules/commander/state_machine_helper.h b/src/modules/commander/state_machine_helper.h
index 07d616b5b9..4012c0e89f 100644
--- a/src/modules/commander/state_machine_helper.h
+++ b/src/modules/commander/state_machine_helper.h
@@ -114,7 +114,7 @@ transition_result_t hil_state_transition(hil_state_t new_state, orb_advert_t sta
 bool set_nav_state(struct vehicle_status_s *status, struct commander_state_s *internal_state,
 		   const bool data_link_loss_enabled, const bool mission_finished,
 		   const bool stay_in_failsafe, status_flags_s *status_flags, bool landed,
-		   const int offb_loss_act, const int offb_loss_rc_act);
+		   const bool rc_loss_enabled, const int offb_loss_act, const int offb_loss_rc_act);
 
 int preflight_check(struct vehicle_status_s *status, orb_advert_t *mavlink_log_pub, bool prearm, bool force_report, status_flags_s *status_flags, battery_status_s *battery);
 
diff --git a/src/modules/navigator/navigator_params.c b/src/modules/navigator/navigator_params.c
index 415a51678a..523f03b42e 100644
--- a/src/modules/navigator/navigator_params.c
+++ b/src/modules/navigator/navigator_params.c
@@ -86,6 +86,24 @@ PARAM_DEFINE_FLOAT(NAV_ACC_RAD, 10.0f);
  */
 PARAM_DEFINE_INT32(NAV_DLL_ACT, 0);
 
+/**
+ * Set RC loss failsafe mode
+ *
+ * The RC loss failsafe will only be entered after a timeout,
+ * set by COM_RC_LOSS_T in seconds. If RC input checks have been disabled
+ * by setting the COM_RC_IN_MODE param it will not be triggered.
+ * Setting this parameter to 4 will enable CASA Outback Challenge rules,
+ * which are only recommended to participants of that competition.
+ *
+ * @value 0 Disabled
+ * @value 1 Loiter
+ * @value 2 Return to Land
+ * @value 3 Land at current position
+ *
+ * @group Mission
+ */
+PARAM_DEFINE_INT32(NAV_RCL_ACT, 0);
+
 /**
  * Airfield home Lat
  *

From fdff6ea325fc25abb25a3d1d1fb017640edd7fb2 Mon Sep 17 00:00:00 2001
From: Julian Oes <julian@oes.ch>
Date: Mon, 6 Jun 2016 11:19:26 +0200
Subject: [PATCH 0731/1230] Revert "commander: remove unused/wrong failsafe
 handling"

This reverts commit 9e704a5e121c7516e2133d02b328500d9d66fb67.
---
 src/modules/commander/commander.cpp | 52 +++++++++++++++++++++++++++++
 1 file changed, 52 insertions(+)

diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp
index 9d20a0ed0b..4ec4bcbc69 100644
--- a/src/modules/commander/commander.cpp
+++ b/src/modules/commander/commander.cpp
@@ -2629,6 +2629,58 @@ int commander_thread_main(int argc, char *argv[])
 			}
 		}
 
+		/* Check for failure combinations which lead to flight termination */
+		if (armed.armed) {
+			/* At this point the data link and the gps system have been checked
+			 * If we are not in a manual (RC stick controlled mode)
+			 * and both failed we want to terminate the flight */
+			if (internal_state.main_state != commander_state_s::MAIN_STATE_MANUAL &&
+			    internal_state.main_state != commander_state_s::MAIN_STATE_ACRO &&
+			    internal_state.main_state != commander_state_s::MAIN_STATE_RATTITUDE &&
+			    internal_state.main_state != commander_state_s::MAIN_STATE_STAB &&
+			    internal_state.main_state != commander_state_s::MAIN_STATE_ALTCTL &&
+			    internal_state.main_state != commander_state_s::MAIN_STATE_POSCTL &&
+			    ((status.data_link_lost && status_flags.gps_failure) ||
+			     (status_flags.data_link_lost_cmd && status_flags.gps_failure_cmd))) {
+				armed.force_failsafe = true;
+				status_changed = true;
+				static bool flight_termination_printed = false;
+
+				if (!flight_termination_printed) {
+					mavlink_and_console_log_critical(&mavlink_log_pub, "DL and GPS lost: flight termination");
+					flight_termination_printed = true;
+				}
+
+				if (counter % (1000000 / COMMANDER_MONITORING_INTERVAL) == 0) {
+					mavlink_log_critical(&mavlink_log_pub, "DL and GPS lost: flight termination");
+				}
+			}
+
+			/* At this point the rc signal and the gps system have been checked
+			 * If we are in manual (controlled with RC):
+			 * if both failed we want to terminate the flight */
+			if ((internal_state.main_state == commander_state_s::MAIN_STATE_ACRO ||
+			     internal_state.main_state == commander_state_s::MAIN_STATE_RATTITUDE ||
+			     internal_state.main_state == commander_state_s::MAIN_STATE_MANUAL ||
+			     internal_state.main_state == commander_state_s::MAIN_STATE_STAB ||
+			     internal_state.main_state == commander_state_s::MAIN_STATE_ALTCTL ||
+			     internal_state.main_state == commander_state_s::MAIN_STATE_POSCTL) &&
+			    ((status.rc_signal_lost && status_flags.gps_failure) ||
+			     (status_flags.rc_signal_lost_cmd && status_flags.gps_failure_cmd))) {
+				armed.force_failsafe = true;
+				status_changed = true;
+				static bool flight_termination_printed = false;
+
+				if (!flight_termination_printed) {
+					warnx("Flight termination because of RC signal loss and GPS failure");
+					flight_termination_printed = true;
+				}
+
+				if (counter % (1000000 / COMMANDER_MONITORING_INTERVAL) == 0) {
+					mavlink_log_critical(&mavlink_log_pub, "RC and GPS lost: flight termination");
+				}
+			}
+		}
 
 		/* Get current timestamp */
 		const hrt_abstime now = hrt_absolute_time();

From 872b08f677f497cd9a30da15409e5225b1e5071f Mon Sep 17 00:00:00 2001
From: Julian Oes <julian@oes.ch>
Date: Mon, 6 Jun 2016 13:54:20 +0200
Subject: [PATCH 0732/1230] commander: remove unneeded include

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

diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp
index 4ec4bcbc69..855a3d9a47 100644
--- a/src/modules/commander/commander.cpp
+++ b/src/modules/commander/commander.cpp
@@ -83,7 +83,6 @@
 #include <uORB/topics/differential_pressure.h>
 #include <uORB/topics/geofence_result.h>
 #include <uORB/topics/home_position.h>
-#include <uORB/topics/input_rc.h>
 #include <uORB/topics/manual_control_setpoint.h>
 #include <uORB/topics/mavlink_log.h>
 #include <uORB/topics/mission.h>
@@ -98,6 +97,8 @@
 #include <uORB/topics/telemetry_status.h>
 #include <uORB/topics/vehicle_attitude.h>
 #include <uORB/topics/vehicle_command.h>
+#include <uORB/topics/vtol_vehicle_status.h>
+#include <uORB/topics/vehicle_land_detected.h>
 #include <uORB/topics/vehicle_command_ack.h>
 #include <uORB/topics/vehicle_global_position.h>
 #include <uORB/topics/vehicle_gps_position.h>

From 67a4a5749126627df56fde5851ad333a859b82d3 Mon Sep 17 00:00:00 2001
From: Julian Oes <julian@oes.ch>
Date: Mon, 6 Jun 2016 13:55:18 +0200
Subject: [PATCH 0733/1230] commander: only warn if termination happens

The termination warnings were printed even if termination was not
actually enabled. This was confusing and is therefore fixed.
---
 src/modules/commander/commander.cpp          | 7 +++++--
 src/modules/commander/state_machine_helper.h | 1 +
 2 files changed, 6 insertions(+), 2 deletions(-)

diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp
index 855a3d9a47..f716bcb0f2 100644
--- a/src/modules/commander/commander.cpp
+++ b/src/modules/commander/commander.cpp
@@ -2300,7 +2300,8 @@ int commander_thread_main(int argc, char *argv[])
 
 
 		/* Check for mission flight termination */
-		if (armed.armed && mission_result.flight_termination) {
+		if (armed.armed && mission_result.flight_termination &&
+		    !status_flags.circuit_breaker_flight_termination_disabled) {
 			armed.force_failsafe = true;
 			status_changed = true;
 			static bool flight_termination_printed = false;
@@ -2631,7 +2632,8 @@ int commander_thread_main(int argc, char *argv[])
 		}
 
 		/* Check for failure combinations which lead to flight termination */
-		if (armed.armed) {
+		if (armed.armed &&
+		    !status_flags.circuit_breaker_flight_termination_disabled) {
 			/* At this point the data link and the gps system have been checked
 			 * If we are not in a manual (RC stick controlled mode)
 			 * and both failed we want to terminate the flight */
@@ -2879,6 +2881,7 @@ get_circuit_breaker_params()
 	status_flags.circuit_breaker_engaged_airspd_check = circuit_breaker_enabled("CBRK_AIRSPD_CHK", CBRK_AIRSPD_CHK_KEY);
 	status_flags.circuit_breaker_engaged_enginefailure_check = circuit_breaker_enabled("CBRK_ENGINEFAIL", CBRK_ENGINEFAIL_KEY);
 	status_flags.circuit_breaker_engaged_gpsfailure_check = circuit_breaker_enabled("CBRK_GPSFAIL", CBRK_GPSFAIL_KEY);
+	status_flags.circuit_breaker_flight_termination_disabled = circuit_breaker_enabled("CBRK_FLIGHTTERM", CBRK_FLIGHTTERM_KEY);
 }
 
 void
diff --git a/src/modules/commander/state_machine_helper.h b/src/modules/commander/state_machine_helper.h
index 4012c0e89f..a204381dff 100644
--- a/src/modules/commander/state_machine_helper.h
+++ b/src/modules/commander/state_machine_helper.h
@@ -76,6 +76,7 @@ struct status_flags_s {
 	bool circuit_breaker_engaged_airspd_check;
 	bool circuit_breaker_engaged_enginefailure_check;
 	bool circuit_breaker_engaged_gpsfailure_check;
+	bool circuit_breaker_flight_termination_disabled;
 	bool circuit_breaker_engaged_usb_check;
 	bool offboard_control_signal_found_once;
 	bool offboard_control_signal_lost;

From ef04085ac5e037adb7f95eb49284fcc6e01cd6c9 Mon Sep 17 00:00:00 2001
From: Julian Oes <julian@oes.ch>
Date: Mon, 6 Jun 2016 13:56:37 +0200
Subject: [PATCH 0734/1230] commander: go to ALTCTL if GPS is lost in POSCTL

Previously, we renained in POSCTL and would drift away if GPS was lost.
---
 .../commander/state_machine_helper.cpp        | 44 ++++++++++++++++---
 1 file changed, 39 insertions(+), 5 deletions(-)

diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp
index b53b80acaf..28222bd8a5 100644
--- a/src/modules/commander/state_machine_helper.cpp
+++ b/src/modules/commander/state_machine_helper.cpp
@@ -603,7 +603,6 @@ bool set_nav_state(struct vehicle_status_s *status, struct commander_state_s *in
 	case commander_state_s::MAIN_STATE_RATTITUDE:
 	case commander_state_s::MAIN_STATE_STAB:
 	case commander_state_s::MAIN_STATE_ALTCTL:
-	case commander_state_s::MAIN_STATE_POSCTL:
 		/* require RC for all manual modes */
 		if (rc_loss_enabled && (status->rc_signal_lost || status_flags->rc_signal_lost_cmd) && armed && !landed) {
 			status->failsafe = true;
@@ -640,10 +639,6 @@ bool set_nav_state(struct vehicle_status_s *status, struct commander_state_s *in
 				status->nav_state = vehicle_status_s::NAVIGATION_STATE_ALTCTL;
 				break;
 
-			case commander_state_s::MAIN_STATE_POSCTL:
-				status->nav_state = vehicle_status_s::NAVIGATION_STATE_POSCTL;
-				break;
-
 			default:
 				status->nav_state = vehicle_status_s::NAVIGATION_STATE_MANUAL;
 				break;
@@ -651,6 +646,45 @@ bool set_nav_state(struct vehicle_status_s *status, struct commander_state_s *in
 		}
 		break;
 
+	case commander_state_s::MAIN_STATE_POSCTL:
+		{
+			const bool rc_lost = rc_loss_enabled && (status->rc_signal_lost || status_flags->rc_signal_lost_cmd);
+			PX4_INFO("lost RC: %s", rc_lost ? "yes" : "no");
+
+			if (rc_lost && armed && !landed) {
+				status->failsafe = true;
+
+				if (status_flags->condition_global_position_valid &&
+				    status_flags->condition_home_position_valid &&
+				    !status_flags->gps_failure) {
+					status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_RCRECOVER;
+
+				} else if (status_flags->condition_local_position_valid &&
+					   !status_flags->gps_failure) {
+					status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LAND;
+
+				} else if (status_flags->condition_local_altitude_valid) {
+					status->nav_state = vehicle_status_s::NAVIGATION_STATE_DESCEND;
+
+				} else {
+					status->nav_state = vehicle_status_s::NAVIGATION_STATE_TERMINATION;
+				}
+
+			/* As long as there is RC, we can fallback to ALTCTL, or STAB. */
+			} else if (status_flags->gps_failure && armed && !landed) {
+				status->failsafe = true;
+
+				if (status_flags->condition_local_altitude_valid) {
+					status->nav_state = vehicle_status_s::NAVIGATION_STATE_ALTCTL;
+				} else {
+					status->nav_state = vehicle_status_s::NAVIGATION_STATE_STAB;
+				}
+			} else {
+				status->nav_state = vehicle_status_s::NAVIGATION_STATE_POSCTL;
+			}
+		}
+		break;
+
 	case commander_state_s::MAIN_STATE_AUTO_MISSION:
 
 		/* go into failsafe

From 66dd72555a1bf9be4453932c906c49ade1c5f587 Mon Sep 17 00:00:00 2001
From: Julian Oes <julian@oes.ch>
Date: Mon, 6 Jun 2016 13:57:23 +0200
Subject: [PATCH 0735/1230] navigator: change default of RC loss param to RTL

---
 src/modules/navigator/navigator_params.c | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/src/modules/navigator/navigator_params.c b/src/modules/navigator/navigator_params.c
index 523f03b42e..63701b4416 100644
--- a/src/modules/navigator/navigator_params.c
+++ b/src/modules/navigator/navigator_params.c
@@ -102,7 +102,7 @@ PARAM_DEFINE_INT32(NAV_DLL_ACT, 0);
  *
  * @group Mission
  */
-PARAM_DEFINE_INT32(NAV_RCL_ACT, 0);
+PARAM_DEFINE_INT32(NAV_RCL_ACT, 2);
 
 /**
  * Airfield home Lat

From f67e74935e976c75b6a82d635a444f4e94e04c99 Mon Sep 17 00:00:00 2001
From: Julian Oes <julian@oes.ch>
Date: Mon, 6 Jun 2016 14:02:25 +0200
Subject: [PATCH 0736/1230] commander: remove leftover printf

---
 src/modules/commander/state_machine_helper.cpp | 1 -
 1 file changed, 1 deletion(-)

diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp
index 28222bd8a5..1a973ff28b 100644
--- a/src/modules/commander/state_machine_helper.cpp
+++ b/src/modules/commander/state_machine_helper.cpp
@@ -649,7 +649,6 @@ bool set_nav_state(struct vehicle_status_s *status, struct commander_state_s *in
 	case commander_state_s::MAIN_STATE_POSCTL:
 		{
 			const bool rc_lost = rc_loss_enabled && (status->rc_signal_lost || status_flags->rc_signal_lost_cmd);
-			PX4_INFO("lost RC: %s", rc_lost ? "yes" : "no");
 
 			if (rc_lost && armed && !landed) {
 				status->failsafe = true;

From c7ec07be701f04851e5bffffecd48b7a0df4469b Mon Sep 17 00:00:00 2001
From: Julian Oes <julian@oes.ch>
Date: Tue, 7 Jun 2016 16:00:23 +0200
Subject: [PATCH 0737/1230] commander: properly use new param

The param COM_ARM_WO_GPS is set to 1 by default to allow arming without
GPS. This then sets a bool arm_without_gps which translates to
!GNSS_check in preflightCheck.
---
 src/modules/commander/commander.cpp           | 35 ++++++++++++-------
 .../state_machine_helper_test.cpp             |  4 ++-
 .../commander/state_machine_helper.cpp        | 11 +++---
 src/modules/commander/state_machine_helper.h  |  5 +--
 4 files changed, 35 insertions(+), 20 deletions(-)

diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp
index f716bcb0f2..299b312ba1 100644
--- a/src/modules/commander/commander.cpp
+++ b/src/modules/commander/commander.cpp
@@ -190,6 +190,8 @@ static uint64_t rc_signal_lost_timestamp;		// Time at which the RC reception was
 
 static float avionics_power_rail_voltage;		// voltage of the avionics power rail
 
+static bool can_arm_without_gps = false;
+
 
 /**
  * The daemon app only briefly exists to start
@@ -365,9 +367,9 @@ int commander_main(int argc, char *argv[])
 
 	if (!strcmp(argv[1], "check")) {
 		int checkres = 0;
-		checkres = preflight_check(&status, &mavlink_log_pub, false, true, &status_flags, &battery);
+		checkres = preflight_check(&status, &mavlink_log_pub, false, true, &status_flags, &battery, false);
 		warnx("Preflight check: %s", (checkres == 0) ? "OK" : "FAILED");
-		checkres = preflight_check(&status, &mavlink_log_pub, true, true, &status_flags, &battery);
+		checkres = preflight_check(&status, &mavlink_log_pub, true, true, &status_flags, &battery, true);
 		warnx("Prearm check: %s", (checkres == 0) ? "OK" : "FAILED");
 		return 0;
 	}
@@ -625,7 +627,8 @@ transition_result_t arm_disarm(bool arm, orb_advert_t *mavlink_log_pub_local, co
 					     true /* fRunPreArmChecks */,
 					     mavlink_log_pub_local,
 					     &status_flags,
-					     avionics_power_rail_voltage);
+					     avionics_power_rail_voltage,
+					     can_arm_without_gps);
 
 	if (arming_res == TRANSITION_CHANGED) {
 		mavlink_log_info(mavlink_log_pub_local, "[cmd] %s by %s", arm ? "ARMED" : "DISARMED", armedBy);
@@ -1507,6 +1510,7 @@ int commander_thread_main(int argc, char *argv[])
 	param_get(_param_autostart_id, &autostart_id);
 	param_get(_param_rc_in_off, &rc_in_off);
 	param_get(_param_arm_without_gps, &arm_without_gps);
+	can_arm_without_gps = (arm_without_gps == 1);
 	status.rc_input_mode = rc_in_off;
 	if (is_hil_setup(autostart_id)) {
 		// HIL configuration selected: real sensors will be disabled
@@ -1516,7 +1520,7 @@ int commander_thread_main(int argc, char *argv[])
 			// sensor diagnostics done continuously, not just at boot so don't warn about any issues just yet
 			status_flags.condition_system_sensors_initialized = Commander::preflightCheck(&mavlink_log_pub, true, true, true, true,
 				checkAirspeed, (status.rc_input_mode == vehicle_status_s::RC_IN_MODE_DEFAULT),
-				(arm_without_gps == 0), false);
+				!can_arm_without_gps, false);
 			set_tune_override(TONE_STARTUP_TUNE); //normal boot tune
 	}
 
@@ -1635,6 +1639,7 @@ int commander_thread_main(int argc, char *argv[])
 			param_get(_param_offboard_loss_act, &offboard_loss_act);
 			param_get(_param_offboard_loss_rc_act, &offboard_loss_rc_act);
 			param_get(_param_arm_without_gps, &arm_without_gps);
+			can_arm_without_gps = (arm_without_gps == 1);
 
 			/* Autostart id */
 			param_get(_param_autostart_id, &autostart_id);
@@ -1747,7 +1752,7 @@ int commander_thread_main(int argc, char *argv[])
 					} else {
 						/* check sensors also */
 						(void)Commander::preflightCheck(&mavlink_log_pub, true, true, true, true, chAirspeed,
-								(status.rc_input_mode == vehicle_status_s::RC_IN_MODE_DEFAULT), (arm_without_gps == 0), hotplug_timeout);
+								(status.rc_input_mode == vehicle_status_s::RC_IN_MODE_DEFAULT), !can_arm_without_gps, hotplug_timeout);
 					}
 				}
 
@@ -1852,7 +1857,8 @@ int commander_thread_main(int argc, char *argv[])
 										  true /* fRunPreArmChecks */,
 										  &mavlink_log_pub,
 										  &status_flags,
-										  avionics_power_rail_voltage)) {
+										  avionics_power_rail_voltage,
+										  can_arm_without_gps)) {
 					mavlink_log_info(&mavlink_log_pub, "DISARMED by safety switch");
 					arming_state_changed = true;
 				}
@@ -2128,7 +2134,8 @@ int commander_thread_main(int argc, char *argv[])
 							     true /* fRunPreArmChecks */,
 							     &mavlink_log_pub,
 							     &status_flags,
-							     avionics_power_rail_voltage);
+							     avionics_power_rail_voltage,
+							     can_arm_without_gps);
 
 			if (arming_ret == TRANSITION_CHANGED) {
 				arming_state_changed = true;
@@ -2382,7 +2389,8 @@ int commander_thread_main(int argc, char *argv[])
 									     true /* fRunPreArmChecks */,
 									     &mavlink_log_pub,
 									     &status_flags,
-									     avionics_power_rail_voltage);
+									     avionics_power_rail_voltage,
+									     can_arm_without_gps);
 
 					if (arming_ret == TRANSITION_CHANGED) {
 						arming_state_changed = true;
@@ -2429,7 +2437,8 @@ int commander_thread_main(int argc, char *argv[])
 										     true /* fRunPreArmChecks */,
 										     &mavlink_log_pub,
 										     &status_flags,
-										     avionics_power_rail_voltage);
+										     avionics_power_rail_voltage,
+										     can_arm_without_gps);
 
 						if (arming_ret == TRANSITION_CHANGED) {
 							arming_state_changed = true;
@@ -3703,7 +3712,8 @@ void *commander_low_prio_loop(void *arg)
 											 false /* fRunPreArmChecks */,
 											 &mavlink_log_pub,
 											 &status_flags,
-											 avionics_power_rail_voltage)) {
+											 avionics_power_rail_voltage,
+											 can_arm_without_gps)) {
 						answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_DENIED, command_ack_pub, command_ack);
 						break;
 					} else {
@@ -3787,7 +3797,7 @@ void *commander_low_prio_loop(void *arg)
 						}
 
 						status_flags.condition_system_sensors_initialized = Commander::preflightCheck(&mavlink_log_pub, true, true, true, true, checkAirspeed,
-							!(status.rc_input_mode >= vehicle_status_s::RC_IN_MODE_OFF), false, hotplug_timeout);
+							!(status.rc_input_mode >= vehicle_status_s::RC_IN_MODE_OFF), !can_arm_without_gps, hotplug_timeout);
 
 						arming_state_transition(&status,
 									&battery,
@@ -3797,7 +3807,8 @@ void *commander_low_prio_loop(void *arg)
 									false /* fRunPreArmChecks */,
 									&mavlink_log_pub,
 									&status_flags,
-								        avionics_power_rail_voltage);
+								        avionics_power_rail_voltage,
+									can_arm_without_gps);
 
 					} else {
 						tune_negative(true);
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 cc4d32bff3..78bc4bc22b 100644
--- a/src/modules/commander/commander_tests/state_machine_helper_test.cpp
+++ b/src/modules/commander/commander_tests/state_machine_helper_test.cpp
@@ -278,6 +278,8 @@ bool StateMachineHelperTest::armingStateTransitionTest(void)
     for (size_t i=0; i<cArmingTransitionTests; i++) {
         const ArmingTransitionTest_t* test = &rgArmingTransitionTests[i];
 
+	const bool check_gps = false;
+
         // Setup initial machine state
         status.arming_state = test->current_state.arming_state;
         status_flags.condition_system_sensors_initialized = test->condition_system_sensors_initialized;
@@ -294,7 +296,7 @@ bool StateMachineHelperTest::armingStateTransitionTest(void)
         		false /* no pre-arm checks */,
         		nullptr /* no mavlink_log_pub */,
         		&status_flags,
-        		5.0f);
+        		5.0f, check_gps);
 
         // Validate result of transition
         ut_compare(test->assertMsg, test->expected_transition_result, result);
diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp
index 1a973ff28b..64cd2dfa42 100644
--- a/src/modules/commander/state_machine_helper.cpp
+++ b/src/modules/commander/state_machine_helper.cpp
@@ -118,7 +118,8 @@ transition_result_t arming_state_transition(struct vehicle_status_s *status,
 					    bool fRunPreArmChecks,
 					    orb_advert_t *mavlink_log_pub,	///< uORB handle for mavlink log
 					    status_flags_s *status_flags,
-					    float avionics_power_rail_voltage)
+					    float avionics_power_rail_voltage,
+					    bool can_arm_without_gps)
 {
 	// Double check that our static arrays are still valid
 	ASSERT(vehicle_status_s::ARMING_STATE_INIT == 0);
@@ -144,7 +145,7 @@ transition_result_t arming_state_transition(struct vehicle_status_s *status,
 				&& status->hil_state == vehicle_status_s::HIL_STATE_OFF) {
 
 			prearm_ret = preflight_check(status, mavlink_log_pub, true /* pre-arm */, false /* force_report */,
-						     status_flags, battery);
+						     status_flags, battery, can_arm_without_gps);
 		}
 
 		/* re-run the pre-flight check as long as sensors are failing */
@@ -155,7 +156,7 @@ transition_result_t arming_state_transition(struct vehicle_status_s *status,
 
 			if (last_preflight_check == 0 || hrt_absolute_time() - last_preflight_check > 1000 * 1000) {
 				prearm_ret = preflight_check(status, mavlink_log_pub, false /* pre-flight */, false /* force_report */,
-							     status_flags, battery);
+							     status_flags, battery, can_arm_without_gps);
 				status_flags->condition_system_sensors_initialized = !prearm_ret;
 				last_preflight_check = hrt_absolute_time();
 				last_prearm_ret = prearm_ret;
@@ -972,7 +973,7 @@ bool set_nav_state(struct vehicle_status_s *status, struct commander_state_s *in
 	return status->nav_state != nav_state_old;
 }
 
-int preflight_check(struct vehicle_status_s *status, orb_advert_t *mavlink_log_pub, bool prearm, bool force_report, status_flags_s *status_flags, battery_status_s *battery)
+int preflight_check(struct vehicle_status_s *status, orb_advert_t *mavlink_log_pub, bool prearm, bool force_report, status_flags_s *status_flags, battery_status_s *battery, bool can_arm_without_gps)
 {
 	/*
 	 */
@@ -988,7 +989,7 @@ int preflight_check(struct vehicle_status_s *status, orb_advert_t *mavlink_log_p
 
 	bool preflight_ok = Commander::preflightCheck(mavlink_log_pub, true, true, true, true,
 				checkAirspeed, (status->rc_input_mode == vehicle_status_s::RC_IN_MODE_DEFAULT),
-				!status_flags->circuit_breaker_engaged_gpsfailure_check, true, reportFailures);
+				!can_arm_without_gps, true, reportFailures);
 
 	if (!status_flags->circuit_breaker_engaged_usb_check && status_flags->usb_connected && prearm) {
 		preflight_ok = false;
diff --git a/src/modules/commander/state_machine_helper.h b/src/modules/commander/state_machine_helper.h
index a204381dff..1506ff8acb 100644
--- a/src/modules/commander/state_machine_helper.h
+++ b/src/modules/commander/state_machine_helper.h
@@ -104,7 +104,8 @@ transition_result_t arming_state_transition(struct vehicle_status_s *status,
 					    bool fRunPreArmChecks,
 					    orb_advert_t *mavlink_log_pub,	///< uORB handle for mavlink log
 					    status_flags_s *status_flags,
-					    float avionics_power_rail_voltage);
+					    float avionics_power_rail_voltage,
+					    bool can_arm_without_gps);
 
 transition_result_t
 main_state_transition(struct vehicle_status_s *status, main_state_t new_main_state, uint8_t &main_state_prev,
@@ -117,6 +118,6 @@ bool set_nav_state(struct vehicle_status_s *status, struct commander_state_s *in
 		   const bool stay_in_failsafe, status_flags_s *status_flags, bool landed,
 		   const bool rc_loss_enabled, const int offb_loss_act, const int offb_loss_rc_act);
 
-int preflight_check(struct vehicle_status_s *status, orb_advert_t *mavlink_log_pub, bool prearm, bool force_report, status_flags_s *status_flags, battery_status_s *battery);
+int preflight_check(struct vehicle_status_s *status, orb_advert_t *mavlink_log_pub, bool prearm, bool force_report, status_flags_s *status_flags, battery_status_s *battery, bool can_arm_without_gps);
 
 #endif /* STATE_MACHINE_HELPER_H_ */

From 5ad671ed4c61b3481109dd33c3d56c5794a7e38f Mon Sep 17 00:00:00 2001
From: "dong.chen" <ender_chen@yeah.net>
Date: Mon, 23 May 2016 16:46:37 +0800
Subject: [PATCH 0738/1230] Solve the problem When lost rc signal, it will rtl
 repeated.

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

diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp
index 64cd2dfa42..0acc38a5bf 100644
--- a/src/modules/commander/state_machine_helper.cpp
+++ b/src/modules/commander/state_machine_helper.cpp
@@ -605,7 +605,7 @@ bool set_nav_state(struct vehicle_status_s *status, struct commander_state_s *in
 	case commander_state_s::MAIN_STATE_STAB:
 	case commander_state_s::MAIN_STATE_ALTCTL:
 		/* require RC for all manual modes */
-		if (rc_loss_enabled && (status->rc_signal_lost || status_flags->rc_signal_lost_cmd) && armed && !landed) {
+		if (rc_loss_enabled && (status->rc_signal_lost || status_flags->rc_signal_lost_cmd) && armed) {
 			status->failsafe = true;
 
 			if (status_flags->condition_global_position_valid && status_flags->condition_home_position_valid) {

From 91127d51c0e94341b469f4b8e1db0bbbadfe71b5 Mon Sep 17 00:00:00 2001
From: Julian Oes <julian@oes.ch>
Date: Wed, 8 Jun 2016 10:23:00 +0200
Subject: [PATCH 0739/1230] commander: stay in failsafe even when landed

If the failsafe state is ended when landed, we would switch back to
POSCTL and therefore take off again, however, all we want is stay on
ground and wait for the auto disarm.
---
 src/modules/commander/state_machine_helper.cpp | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp
index 0acc38a5bf..c334e2b381 100644
--- a/src/modules/commander/state_machine_helper.cpp
+++ b/src/modules/commander/state_machine_helper.cpp
@@ -651,7 +651,7 @@ bool set_nav_state(struct vehicle_status_s *status, struct commander_state_s *in
 		{
 			const bool rc_lost = rc_loss_enabled && (status->rc_signal_lost || status_flags->rc_signal_lost_cmd);
 
-			if (rc_lost && armed && !landed) {
+			if (rc_lost && armed) {
 				status->failsafe = true;
 
 				if (status_flags->condition_global_position_valid &&
@@ -671,7 +671,7 @@ bool set_nav_state(struct vehicle_status_s *status, struct commander_state_s *in
 				}
 
 			/* As long as there is RC, we can fallback to ALTCTL, or STAB. */
-			} else if (status_flags->gps_failure && armed && !landed) {
+			} else if (status_flags->gps_failure && armed) {
 				status->failsafe = true;
 
 				if (status_flags->condition_local_altitude_valid) {

From 1869ffd15f2db156054e645c4645c0779fdbf960 Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Tue, 21 Jun 2016 13:36:35 +0200
Subject: [PATCH 0740/1230] Fix unknown command message for Spektrum bind

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

diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp
index 299b312ba1..c7521acd88 100644
--- a/src/modules/commander/commander.cpp
+++ b/src/modules/commander/commander.cpp
@@ -1072,6 +1072,7 @@ bool handle_command(struct vehicle_status_s *status_local, const struct safety_s
 	case vehicle_command_s::VEHICLE_CMD_DO_DIGICAM_CONTROL:
 	case vehicle_command_s::VEHICLE_CMD_DO_SET_CAM_TRIGG_DIST:
 	case vehicle_command_s::VEHICLE_CMD_DO_CHANGE_SPEED:
+	case vehicle_command_s::VEHICLE_CMD_START_RX_PAIR:
 		/* ignore commands that handled in low prio loop */
 		break;
 

From 2f113a4ce3138d3ec9b595636aa0656b47a31ebf Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Tue, 21 Jun 2016 16:38:53 +0200
Subject: [PATCH 0741/1230] MAVLink: Allow data rate updates

---
 src/modules/mavlink/mavlink_main.h       | 3 +++
 src/modules/mavlink/mavlink_receiver.cpp | 8 ++++++--
 src/modules/mavlink/mavlink_receiver.h   | 6 +++++-
 3 files changed, 14 insertions(+), 3 deletions(-)

diff --git a/src/modules/mavlink/mavlink_main.h b/src/modules/mavlink/mavlink_main.h
index 6ae5636317..b3b15d3a2f 100644
--- a/src/modules/mavlink/mavlink_main.h
+++ b/src/modules/mavlink/mavlink_main.h
@@ -417,6 +417,9 @@ public:
 
 	void			set_logging_enabled(bool logging) { _logging_enabled = logging; }
 
+	int			get_data_rate() { return _datarate; }
+	void			set_data_rate(int rate) { if (rate > 0) _datarate = rate; }
+
 protected:
 	Mavlink			*next;
 
diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp
index 1841861298..f5c5516d5a 100644
--- a/src/modules/mavlink/mavlink_receiver.cpp
+++ b/src/modules/mavlink/mavlink_receiver.cpp
@@ -360,7 +360,7 @@ MavlinkReceiver::handle_message_command_long(mavlink_message_t *msg)
 			_mavlink->configure_stream_threadsafe("HOME_POSITION", 0.5f);
 
 		} else if (cmd_mavlink.command == MAV_CMD_SET_MESSAGE_INTERVAL) {
-			set_message_interval((int)cmd_mavlink.param1, cmd_mavlink.param2);
+			set_message_interval((int)(cmd_mavlink.param1 + 0.5f), cmd_mavlink.param2, cmd_mavlink.param3);
 
 		} else if (cmd_mavlink.command == MAV_CMD_GET_MESSAGE_INTERVAL) {
 			get_message_interval((int)cmd_mavlink.param1);
@@ -1402,8 +1402,12 @@ MavlinkReceiver::handle_message_request_data_stream(mavlink_message_t *msg)
 }
 
 void
-MavlinkReceiver::set_message_interval(int msgId, float interval)
+MavlinkReceiver::set_message_interval(int msgId, float interval, int data_rate)
 {
+	if (data_rate > 0) {
+		_mavlink->set_data_rate(data_rate);
+	}
+
 	// configure_stream wants a rate (msgs/second), so convert here.
 	float rate = 0;
 
diff --git a/src/modules/mavlink/mavlink_receiver.h b/src/modules/mavlink/mavlink_receiver.h
index 45e904d295..bf0a5121ed 100644
--- a/src/modules/mavlink/mavlink_receiver.h
+++ b/src/modules/mavlink/mavlink_receiver.h
@@ -148,8 +148,12 @@ private:
 	/**
 	 * Set the interval at which the given message stream is published.
 	 * The rate is the number of messages per second.
+	 *
+	 * @param msgId the message ID of to change the interval of
+	 * @param interval the interval in us to send the message at
+	 * @param data_rate the total link data rate in bytes per second
 	 */
-	void set_message_interval(int msgId, float rate);
+	void set_message_interval(int msgId, float interval, int data_rate=-1);
 	void get_message_interval(int msgId);
 
 	/**

From de675845afe69e5d1e622382cd5db6ed645bc90c Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Wed, 22 Jun 2016 00:04:31 +0200
Subject: [PATCH 0742/1230] Fix navigator timeout logic

---
 src/modules/navigator/navigator_main.cpp | 5 +++--
 1 file changed, 3 insertions(+), 2 deletions(-)

diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp
index c8b321fd62..fd7a54ff6e 100644
--- a/src/modules/navigator/navigator_main.cpp
+++ b/src/modules/navigator/navigator_main.cpp
@@ -347,10 +347,11 @@ Navigator::task_main()
 			/* this is undesirable but not much we can do - might want to flag unhappy status */
 			PX4_WARN("nav: poll error %d, %d", pret, errno);
 			continue;
+		} else {
+			/* success, global pos was available */
+			global_pos_available_once = true;
 		}
 
-		global_pos_available_once = true;
-
 		perf_begin(_loop_perf);
 
 		bool updated;

From b563fae11822c8f83e990c7d1dd497fd3d380126 Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Wed, 22 Jun 2016 00:09:13 +0200
Subject: [PATCH 0743/1230] Fix request data stream handling

---
 src/modules/mavlink/mavlink_receiver.cpp | 25 +-----------------------
 1 file changed, 1 insertion(+), 24 deletions(-)

diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp
index f5c5516d5a..56a6999147 100644
--- a/src/modules/mavlink/mavlink_receiver.cpp
+++ b/src/modules/mavlink/mavlink_receiver.cpp
@@ -1375,30 +1375,7 @@ MavlinkReceiver::handle_message_ping(mavlink_message_t *msg)
 void
 MavlinkReceiver::handle_message_request_data_stream(mavlink_message_t *msg)
 {
-	mavlink_request_data_stream_t req;
-	mavlink_msg_request_data_stream_decode(msg, &req);
-
-	PX4_WARN("REQUEST_DATA_STREAM is deprecated, please use SET_MESSAGE_INTERVAL instead");
-
-	if (req.target_system == mavlink_system.sysid && req.target_component == mavlink_system.compid &&
-	    req.req_stream_id != 0) {
-		// compute interval in microseconds.
-		float interval = 0;
-		int rate = req.req_message_rate;
-
-		if (req.start_stop == 0 || rate == 0) {
-			rate = -1;
-
-		} else if (rate > 0) {
-			interval = 1000000.0f / rate;
-		}
-
-		// todo: note it is kind of bogus to interpret the req_stream_id as a message id
-		// but this is how it was.  MAVLINK defines separate MAV_DATA_STREAM ids which bundle
-		// messages into groups, but that is all deprecated now anyway, and this code was
-		// previously interpreting the req_stream_id as a message id, so we'll leave it that way.
-		set_message_interval(req.req_stream_id, interval);
-	}
+	// REQUEST_DATA_STREAM is deprecated, please use SET_MESSAGE_INTERVAL instead
 }
 
 void

From c84870046e0dceafe79199cab06773e2b1c2d3e0 Mon Sep 17 00:00:00 2001
From: tumbili <roman@px4.io>
Date: Tue, 21 Jun 2016 13:55:41 +0200
Subject: [PATCH 0744/1230] updated ecl: added method to return ekf bias state

Signed-off-by: tumbili <roman@px4.io>
---
 src/lib/ecl | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/src/lib/ecl b/src/lib/ecl
index 74a77b45b2..1b394460a3 160000
--- a/src/lib/ecl
+++ b/src/lib/ecl
@@ -1 +1 @@
-Subproject commit 74a77b45b2b51cb9a2f2a05e0b356d87cac0d9a8
+Subproject commit 1b394460a39b6f69b913383e0b9e66527f617650

From 6739ae9dfc96e4e8bc3857d191722bb7b1ce7e8d Mon Sep 17 00:00:00 2001
From: tumbili <roman@px4.io>
Date: Tue, 21 Jun 2016 14:12:05 +0200
Subject: [PATCH 0745/1230] ekf2: substract gyro bias from control state rates

Signed-off-by: tumbili <roman@px4.io>
---
 src/modules/ekf2/ekf2_main.cpp | 8 +++++---
 1 file changed, 5 insertions(+), 3 deletions(-)

diff --git a/src/modules/ekf2/ekf2_main.cpp b/src/modules/ekf2/ekf2_main.cpp
index 8ab9faaac8..864505c7fe 100644
--- a/src/modules/ekf2/ekf2_main.cpp
+++ b/src/modules/ekf2/ekf2_main.cpp
@@ -609,10 +609,12 @@ void Ekf2::task_main()
 
 			// generate control state data
 			control_state_s ctrl_state = {};
+			float gyro_bias[3] = {};
+			_ekf.get_gyro_bias(gyro_bias);
 			ctrl_state.timestamp = hrt_absolute_time();
-			ctrl_state.roll_rate = _lp_roll_rate.apply(sensors.gyro_rad_s[0]);
-			ctrl_state.pitch_rate = _lp_pitch_rate.apply(sensors.gyro_rad_s[1]);
-			ctrl_state.yaw_rate = _lp_yaw_rate.apply(sensors.gyro_rad_s[2]);
+			ctrl_state.roll_rate = _lp_roll_rate.apply(sensors.gyro_rad_s[0]) - gyro_bias[0];
+			ctrl_state.pitch_rate = _lp_pitch_rate.apply(sensors.gyro_rad_s[1]) - gyro_bias[1];
+			ctrl_state.yaw_rate = _lp_yaw_rate.apply(sensors.gyro_rad_s[2]) - gyro_bias[2];
 
 			// Velocity in body frame
 			float velocity[3];

From a73ac821ab4ce909f409c99b6b6ab5e80737eb07 Mon Sep 17 00:00:00 2001
From: jwilson <jwilson@qti.qualcomm.com>
Date: Mon, 20 Jun 2016 16:01:32 -0700
Subject: [PATCH 0746/1230] Fixes shared memory locking bug and eliminates the
 need for an AppsProm driver to reserve a shared memory region.

---
 src/firmware/qurt/px4muorb.idl                |  22 ++
 src/modules/muorb/adsp/px4muorb.cpp           |  78 +++++++
 src/modules/muorb/adsp/px4muorb.hpp           |   6 +
 .../muorb/krait/px4muorb_KraitRpcWrapper.cpp  |  19 ++
 src/modules/systemlib/param/param_shmem.c     |  24 +-
 src/platforms/posix/px4_layer/CMakeLists.txt  |   5 +
 src/platforms/posix/px4_layer/shmem_posix.c   | 216 ++----------------
 src/platforms/qurt/px4_layer/shmem_qurt.c     |  64 ++++--
 src/platforms/shmem.h                         |   2 +
 9 files changed, 197 insertions(+), 239 deletions(-)

diff --git a/src/firmware/qurt/px4muorb.idl b/src/firmware/qurt/px4muorb.idl
index 49e1305ffd..e87492fb3e 100644
--- a/src/firmware/qurt/px4muorb.idl
+++ b/src/firmware/qurt/px4muorb.idl
@@ -49,6 +49,28 @@ interface px4muorb{
     * @param time_us: pointer to time in us
     */
    AEEResult get_absolute_time(rout unsigned long long time_us);
+   
+   /**
+    * Interface to update param for krait.
+    *
+    * @param param: param index.
+    * @param value: param value.
+    */
+   AEEResult param_update_to_shmem( in unsigned long param, in sequence<octet> value);
+   
+   /**
+    * Interface to update index for krait.
+    * @param data: param index array.
+    */
+   AEEResult param_update_index_from_shmem( rout sequence<octet> data);
+   
+   /**
+    * Interface to get param value for krait.
+    *
+    * @param param: param index.
+    * @param value: param value.
+    */
+   AEEResult param_update_value_from_shmem( in unsigned long param, rout sequence<octet> value);
 
    /**
     * Interface to add a subscriber to the identified topic
diff --git a/src/modules/muorb/adsp/px4muorb.cpp b/src/modules/muorb/adsp/px4muorb.cpp
index efe8fde2d0..8ce0a50d36 100644
--- a/src/modules/muorb/adsp/px4muorb.cpp
+++ b/src/modules/muorb/adsp/px4muorb.cpp
@@ -42,9 +42,15 @@
 #include "px4_log.h"
 #include "uORB/topics/sensor_combined.h"
 #include "uORB.h"
+#include "systemlib/param/param.h"
+#include <shmem.h>
 
 __BEGIN_DECLS
 extern int dspal_main(int argc, char *argv[]);
+extern struct shmem_info *shmem_info_p;
+extern int get_shmem_lock(const char *caller_file_name, int caller_line_number);
+extern void release_shmem_lock(const char *caller_file_name, int caller_line_number);
+extern void init_shared_memory(void);
 __END_DECLS
 
 int px4muorb_orb_initialize()
@@ -76,6 +82,78 @@ int px4muorb_get_absolute_time(uint64_t *time_us)
 	return 0;
 }
 
+/*update value and param's change bit in shared memory*/
+int px4muorb_param_update_to_shmem(uint32_t param, const uint8_t *value, int data_len_in_bytes)
+{
+	unsigned int byte_changed, bit_changed;
+	union param_value_u *param_value = (union param_value_u *)value;
+
+	if(!shmem_info_p) {
+		init_shared_memory();
+	}
+
+	if (get_shmem_lock(__FILE__, __LINE__) != 0) {
+		PX4_INFO("Could not get shmem lock\n");
+		return -1;
+	}
+
+	shmem_info_p->params_val[param] = *param_value;
+
+	byte_changed = param / 8;
+	bit_changed = 1 << param % 8;
+	shmem_info_p->krait_changed_index[byte_changed] |= bit_changed;
+
+	release_shmem_lock(__FILE__, __LINE__);
+
+	return 0;
+}
+
+int px4muorb_param_update_index_from_shmem(unsigned char *data, int data_len_in_bytes)
+{
+	unsigned int i;
+
+	if(!shmem_info_p)
+		return -1;
+
+	if (get_shmem_lock(__FILE__, __LINE__) != 0) {
+		PX4_INFO("Could not get shmem lock\n");
+		return -1;
+	}
+
+	for (i = 0; i < data_len_in_bytes; i++) {
+		data[i] = shmem_info_p->adsp_changed_index[i];
+	}
+
+	release_shmem_lock(__FILE__, __LINE__);
+
+	return 0;
+}
+
+int px4muorb_param_update_value_from_shmem(uint32_t param, const uint8_t *value, int data_len_in_bytes)
+{
+	unsigned int byte_changed, bit_changed;
+	union param_value_u *param_value = (union param_value_u *)value;
+
+	if(!shmem_info_p)
+		return -1;
+
+	if (get_shmem_lock(__FILE__, __LINE__) != 0) {
+		PX4_INFO("Could not get shmem lock\n");
+		return -1;
+	}
+
+	*param_value = shmem_info_p->params_val[param];
+
+	/*also clear the index since we are holding the lock*/
+	byte_changed = param / 8;
+	bit_changed = 1 << param % 8;
+	shmem_info_p->adsp_changed_index[byte_changed] &= ~bit_changed;
+
+	release_shmem_lock(__FILE__, __LINE__);
+
+	return 0;
+}
+
 int px4muorb_add_subscriber(const char *name)
 {
 	int rc = 0;
diff --git a/src/modules/muorb/adsp/px4muorb.hpp b/src/modules/muorb/adsp/px4muorb.hpp
index 56154becfa..36d5f98ca5 100644
--- a/src/modules/muorb/adsp/px4muorb.hpp
+++ b/src/modules/muorb/adsp/px4muorb.hpp
@@ -43,6 +43,12 @@ extern "C" {
 
 	int px4muorb_get_absolute_time(uint64_t *time_us) __EXPORT;
 
+	int px4muorb_param_update_to_shmem(uint32_t param, const uint8_t *value, int data_len_in_bytes) __EXPORT;
+
+	int px4muorb_param_update_index_from_shmem(unsigned char *data, int data_len_in_bytes) __EXPORT;
+
+	int px4muorb_param_update_value_from_shmem(uint32_t param, const uint8_t *value, int data_len_in_bytes) __EXPORT;
+
 	int px4muorb_add_subscriber(const char *name) __EXPORT;
 
 	int px4muorb_remove_subscriber(const char *name) __EXPORT;
diff --git a/src/modules/muorb/krait/px4muorb_KraitRpcWrapper.cpp b/src/modules/muorb/krait/px4muorb_KraitRpcWrapper.cpp
index b9fb8511e8..71a80e6969 100644
--- a/src/modules/muorb/krait/px4muorb_KraitRpcWrapper.cpp
+++ b/src/modules/muorb/krait/px4muorb_KraitRpcWrapper.cpp
@@ -38,6 +38,7 @@
 #include <rpcmem.h>
 #include "px4muorb.h"
 #include "px4_log.h"
+#include <shmem.h>
 
 using namespace px4muorb;
 
@@ -63,6 +64,8 @@ static const uint32_t _MAX_TOPICS = 64;
 static const uint32_t _MAX_BULK_TRANSFER_BUFFER_SIZE = _MAX_TOPIC_DATA_BUFFER_SIZE * _MAX_TOPICS;
 static uint8_t *_BulkTransferBuffer = 0;
 
+unsigned char *adsp_changed_index = 0;
+
 // The DSP timer can be read from this file.
 #define DSP_TIMER_FILE "/sys/kernel/boot_adsp/qdsp_qtimer"
 
@@ -195,6 +198,17 @@ bool px4muorb::KraitRpcWrapper::Initialize()
 		PX4_DEBUG("%s rpcmem_alloc passed for data_buffer", __FUNCTION__);
 	}
 
+	adsp_changed_index = (uint8_t *) rpcmem_alloc(MUORB_KRAIT_FASTRPC_HEAP_ID,
+					       MUORB_KRAIT_FASTRPC_MEM_FLAGS, PARAM_BUFFER_SIZE * sizeof(uint8_t));
+
+	rc = (adsp_changed_index != NULL) ? true : false;
+
+	if (!rc) {
+		PX4_ERR("%s rpcmem_alloc failed! for adsp_changed_index", __FUNCTION__);
+	} else {
+		memset(adsp_changed_index, 0, PARAM_BUFFER_SIZE * sizeof(uint8_t));
+	}
+
 	int32_t time_diff_us;
 
 	if (calc_timer_diff_to_dsp_us(&time_diff_us) != 0) {
@@ -242,6 +256,11 @@ bool px4muorb::KraitRpcWrapper::Terminate()
 		_DataBuffer = 0;
 	}
 
+	if(adsp_changed_index != NULL) {
+		rpcmem_free(adsp_changed_index);
+		adsp_changed_index = 0;
+	}
+
 	_Initialized = false;
 	return true;
 }
diff --git a/src/modules/systemlib/param/param_shmem.c b/src/modules/systemlib/param/param_shmem.c
index 5ff934b043..ea64b7e6ae 100644
--- a/src/modules/systemlib/param/param_shmem.c
+++ b/src/modules/systemlib/param/param_shmem.c
@@ -109,7 +109,7 @@ const int bits_per_allocation_unit  = (sizeof(*param_changed_storage) * 8);
 //#define ENABLE_SHMEM_DEBUG
 
 extern int get_shmem_lock(const char *caller_file_name, int caller_line_number);
-extern void release_shmem_lock(void);
+extern void release_shmem_lock(const char *caller_file_name, int caller_line_number);
 
 struct param_wbuf_s *param_find_changed(param_t param);
 
@@ -828,18 +828,9 @@ param_save_default(void)
 {
 	int res = OK;
 	int fd = -1;
-	bool is_locked = false;
 
 	const char *filename = param_get_default_file();
 
-	if (get_shmem_lock(__FILE__, __LINE__) != 0) {
-		PX4_ERR("Could not get shmem lock");
-		res = ERROR;
-		goto exit;
-	}
-
-	is_locked = true;
-
 	fd = PARAM_OPEN(filename, O_WRONLY | O_CREAT, PX4_O_MODE_666);
 
 	if (fd < 0) {
@@ -869,10 +860,6 @@ param_save_default(void)
 
 exit:
 
-	if (is_locked) {
-		release_shmem_lock();
-	}
-
 	if (fd >= 0) {
 		close(fd);
 	}
@@ -923,7 +910,9 @@ param_load_default_no_notify(void)
 	int fd_load = open(param_get_default_file(), O_RDONLY);
 
 	if (fd_load < 0) {
-		release_shmem_lock();
+#ifdef __PX4_QURT
+		release_shmem_lock(__FILE__, __LINE__);
+#endif
 
 		/* no parameter file is OK, otherwise this is an error */
 		if (errno != ENOENT) {
@@ -1241,8 +1230,10 @@ uint32_t param_hash_check(void)
 
 void init_params(void)
 {
+#ifdef __PX4_QURT
 	//copy params to shared memory
 	init_shared_memory();
+#endif
 
 	/*load params automatically*/
 #ifdef __PX4_POSIX
@@ -1250,6 +1241,7 @@ void init_params(void)
 #endif
 	param_import_done = 1;
 
+#ifdef __PX4_QURT
 	copy_params_to_shmem(param_info_base);
 
 
@@ -1260,6 +1252,6 @@ void init_params(void)
 		 (unsigned char *)&shmem_info_p->krait_changed_index - (unsigned char *)shmem_info_p,
 		 (unsigned char *)&shmem_info_p->adsp_changed_index - (unsigned char *)shmem_info_p);
 #endif
-
+#endif
 }
 
diff --git a/src/platforms/posix/px4_layer/CMakeLists.txt b/src/platforms/posix/px4_layer/CMakeLists.txt
index d80b6083ad..0e98f0bcb3 100644
--- a/src/platforms/posix/px4_layer/CMakeLists.txt
+++ b/src/platforms/posix/px4_layer/CMakeLists.txt
@@ -31,6 +31,11 @@
 #
 ############################################################################
 
+include(hexagon_sdk)
+
+include_directories(${CMAKE_BINARY_DIR}/src/firmware/posix)
+include_directories(${HEXAGON_SDK_INCLUDES})
+
 if("${CONFIG_SHMEM}" STREQUAL "1")
 	list(APPEND SHMEM_SRCS
 		shmem_posix.c
diff --git a/src/platforms/posix/px4_layer/shmem_posix.c b/src/platforms/posix/px4_layer/shmem_posix.c
index c9ec801ca9..8fbaece56f 100644
--- a/src/platforms/posix/px4_layer/shmem_posix.c
+++ b/src/platforms/posix/px4_layer/shmem_posix.c
@@ -51,236 +51,43 @@
 #include "systemlib/param/param.h"
 
 #include <shmem.h>
+#include "px4muorb.h"
 
 //#define SHMEM_DEBUG
-
-int mem_fd;
-unsigned char *map_base, *virt_addr;
-struct shmem_info *shmem_info_p;
-static void *map_memory(off_t target);
-
-int get_shmem_lock(const char *caller_file_name, int caller_line_number);
-void release_shmem_lock(void);
-void init_shared_memory(void);
-void copy_params_to_shmem(struct param_info_s *);
 void update_to_shmem(param_t param, union param_value_u value);
 int update_from_shmem(param_t param, union param_value_u *value);
 void update_index_from_shmem(void);
 uint64_t update_from_shmem_prev_time = 0, update_from_shmem_current_time = 0;
-static unsigned char adsp_changed_index[MAX_SHMEM_PARAMS / 8 + 1];
+extern unsigned char *adsp_changed_index;
 
 struct param_wbuf_s {
 	param_t param;
 	union param_value_u val;
 	bool unsaved;
 };
-extern struct param_wbuf_s *param_find_changed(param_t param);
 
-#define MEMDEVICE	"/dev/mem"
-
-static void *map_memory(off_t target)
-{
-
-	if ((mem_fd = open(MEMDEVICE, O_RDWR | O_SYNC)) == -1) {
-		PX4_ERR("Cannot open %s", MEMDEVICE);
-		exit(1);
-	}
-
-	/* Map one page */
-	map_base = (unsigned char *) mmap(0, MAP_SIZE, PROT_READ | PROT_WRITE,
-					  MAP_SHARED, mem_fd, target & ~MAP_MASK);
-
-	if (map_base == (void *) - 1) {
-		PX4_ERR("Cannot mmap /dev/atl_mem");
-		exit(1);
-	}
-
-	PX4_DEBUG("Initializing map memory: mem_fd: %d, 0x%X", mem_fd, map_base + (target & MAP_MASK) + LOCK_SIZE);
-
-	return (map_base + (target & MAP_MASK) + LOCK_SIZE);
-
-}
-
-int get_shmem_lock(const char *caller_file_name, int caller_line_number)
-{
-	// TODO FIXME: just say this went through
-	return 0;
-
-	int i = 0;
-
-	/* TODO: make this comment so somebody can understand it: ioctl calls cmpxchg */
-	while (ioctl(mem_fd, LOCK_MEM) != 0) {
-
-		usleep(10000); //sleep for 10 msec
-		i++;
-
-		if (i > 10) {
-			PX4_ERR("Could not get lock, file name: %s, line number: %d, errno: %d",
-				caller_file_name, caller_line_number, errno);
-			return -1;
-		}
-	}
-
-	return 0; //got the lock
-}
-
-void release_shmem_lock(void)
-{
-	// TODO FIXME: just say this went through
-	return;
-
-	int ret = ioctl(mem_fd, UNLOCK_MEM);
-
-	if (ret != 0) {
-		PX4_INFO("unlock failed, ret: %d, errno %d", ret, errno);
-	}
-}
-
-void init_shared_memory(void)
-{
-
-	virt_addr = map_memory(MAP_ADDRESS); //16K space
-	shmem_info_p = (struct shmem_info *) virt_addr;
-
-	PX4_DEBUG("linux memory mapped");
-}
-
-void copy_params_to_shmem(struct param_info_s *param_info_base)
-{
-	param_t param;
-	unsigned int i;
-
-	if (get_shmem_lock(__FILE__, __LINE__) != 0) {
-		PX4_ERR("Could not get shmem lock");
-		return;
-	}
-
-	PX4_DEBUG("%d krait params allocated", param_count());
-
-	for (param = 0; param < param_count(); param++) {
-		struct param_wbuf_s *s = param_find_changed(param);
-
-		if (s == NULL) {
-			shmem_info_p->params_val[param] = param_info_base[param].val;
-
-		} else {
-			shmem_info_p->params_val[param] = s->val;
-		}
-
-#ifdef SHMEM_DEBUG
-
-		if (param_type(param) == PARAM_TYPE_INT32) {
-			{
-				PX4_INFO("%d: written %d for param %s to shared mem",
-					 param, shmem_info_p->params_val[param].i, param_name(param));
-			}
-
-		} else if (param_type(param) == PARAM_TYPE_FLOAT) {
-			{
-				PX4_INFO("%d: written %f for param %s to shared mem",
-					 param, (double)shmem_info_p->params_val[param].f, param_name(param));
-			}
-		}
-
-#endif
-	}
-
-	PX4_DEBUG("written %u params to shmem", param_count());
-
-	for (i = 0; i < MAX_SHMEM_PARAMS / 8 + 1; i++) {
-		shmem_info_p->krait_changed_index[i] = 0;
-		adsp_changed_index[i] = 0;
-	}
-
-	release_shmem_lock();
-}
 
 /*update value and param's change bit in shared memory*/
 void update_to_shmem(param_t param, union param_value_u value)
 {
-	unsigned int byte_changed, bit_changed;
-
-	if (get_shmem_lock(__FILE__, __LINE__) != 0) {
-		PX4_ERR("Could not get shmem lock");
-		return;
-	}
-
-	shmem_info_p->params_val[param] = value;
-
-	byte_changed = param / 8;
-	bit_changed = 1 << param % 8;
-	shmem_info_p->krait_changed_index[byte_changed] |= bit_changed;
-
-	PX4_DEBUG("set %d bit on krait changed index[%d] to %d", bit_changed, byte_changed,
-		  shmem_info_p->krait_changed_index[byte_changed]);
-
-#ifdef SHMEM_DEBUG
-
-	if (param_type(param) == PARAM_TYPE_INT32) {
-		PX4_INFO("Set value %d for param %s to shmem, set krait index %d:%d",
-			 value.i, param_name(param), byte_changed, bit_changed);
-
-	} else if (param_type(param) == PARAM_TYPE_FLOAT) {
-		PX4_INFO("Set value %f for param %s to shmem, set krait index %d:%d",
-			 (double)value.f, param_name(param), byte_changed, bit_changed);
-	}
-
-#endif
-
-	release_shmem_lock();
-
+	if(px4muorb_param_update_to_shmem(param, (unsigned char*)&value, sizeof(value)))
+		PX4_ERR("krait update param %u failed", param);
 }
 
 void update_index_from_shmem(void)
 {
-	unsigned int i;
-
-	if (get_shmem_lock(__FILE__, __LINE__) != 0) {
-		PX4_ERR("Could not get shmem lock");
+	if(!adsp_changed_index) {
+		PX4_ERR("%s no param buffer", __FUNCTION__);
 		return;
 	}
 
-	PX4_DEBUG("Updating index from shmem");
-
-	for (i = 0; i < MAX_SHMEM_PARAMS / 8 + 1; i++) {
-		adsp_changed_index[i] = shmem_info_p->adsp_changed_index[i];
-	}
-
-	release_shmem_lock();
+	px4muorb_param_update_index_from_shmem(adsp_changed_index, PARAM_BUFFER_SIZE);
 }
 
 static void update_value_from_shmem(param_t param, union param_value_u *value)
 {
-	unsigned int byte_changed, bit_changed;
-
-	if (get_shmem_lock(__FILE__, __LINE__) != 0) {
-		PX4_ERR("Could not get shmem lock");
-		return;
-	}
-
-	*value = shmem_info_p->params_val[param];
-
-	/*also clear the index since we are holding the lock*/
-	byte_changed = param / 8;
-	bit_changed = 1 << param % 8;
-	shmem_info_p->adsp_changed_index[byte_changed] &= ~bit_changed;
-
-	release_shmem_lock();
-
-#ifdef SHMEM_DEBUG
-
-	if (param_type(param) == PARAM_TYPE_INT32) {
-		PX4_INFO(
-			"Got value %d for param %s from shmem, cleared adsp index %d:%d",
-			value->i, param_name(param), byte_changed, bit_changed);
-
-	} else if (param_type(param) == PARAM_TYPE_FLOAT) {
-		PX4_INFO(
-			"Got value %f for param %s from shmem, cleared adsp index %d:%d",
-			(double)value->f, param_name(param), byte_changed, bit_changed);
-	}
-
-#endif
+	if(px4muorb_param_update_value_from_shmem(param, (unsigned char*)value, sizeof(union param_value_u)))
+		PX4_ERR("%s get param failed", __FUNCTION__);
 }
 
 int update_from_shmem(param_t param, union param_value_u *value)
@@ -288,6 +95,11 @@ int update_from_shmem(param_t param, union param_value_u *value)
 	unsigned int byte_changed, bit_changed;
 	unsigned int retval = 0;
 
+	if(!adsp_changed_index) {
+		PX4_ERR("%s no param buffer", __FUNCTION__);
+		return 0;
+	}
+
 	update_from_shmem_current_time = hrt_absolute_time();
 
 	if ((update_from_shmem_current_time - update_from_shmem_prev_time)
diff --git a/src/platforms/qurt/px4_layer/shmem_qurt.c b/src/platforms/qurt/px4_layer/shmem_qurt.c
index ca29ba77db..5aeabf520b 100644
--- a/src/platforms/qurt/px4_layer/shmem_qurt.c
+++ b/src/platforms/qurt/px4_layer/shmem_qurt.c
@@ -48,13 +48,14 @@
 #include <drivers/drv_hrt.h>
 
 //#define SHMEM_DEBUG
+//#define PARAM_LOCK_DEBUG
 
 int mem_fd;
 unsigned char *map_base, *virt_addr;
 struct shmem_info *shmem_info_p;
 static void *map_memory(off_t target);
 int get_shmem_lock(const char *caller_file_name, int caller_line_number);
-void release_shmem_lock(void);
+void release_shmem_lock(const char *caller_file_name, int caller_line_number);
 void init_shared_memory(void);
 void copy_params_to_shmem(struct param_info_s *);
 void update_to_shmem(param_t param, union param_value_u value);
@@ -91,15 +92,14 @@ static void *map_memory(off_t target)
 
 int get_shmem_lock(const char *caller_file_name, int caller_line_number)
 {
-	// TODO: don't do this for now
-	return 0;
-
 	unsigned char *lock = (unsigned char *)(MAP_ADDRESS + LOCK_OFFSET);
 	unsigned int i = 0;
 
+#ifdef PARAM_LOCK_DEBUG
+	PX4_INFO("lock value %d before get from %s, line: %d\n", *(unsigned int*)0xfbfc000, strrchr(caller_file_name, '/'), caller_line_number);
+#endif
+
 	while (!atomic_compare_and_set(lock, 1, 0)) {
-		PX4_INFO("Could not get lock, file name: %s, line number: %d.\n",
-			 caller_file_name, caller_line_number);
 		i++;
 		usleep(1000);
 
@@ -107,6 +107,8 @@ int get_shmem_lock(const char *caller_file_name, int caller_line_number)
 	}
 
 	if (i > 100) {
+		PX4_INFO("Could not get lock, file name: %s, line number: %d.\n",
+			 strrchr(caller_file_name, '/'), caller_line_number);
 		return -1;
 
 	} else {
@@ -117,24 +119,39 @@ int get_shmem_lock(const char *caller_file_name, int caller_line_number)
 
 }
 
-void release_shmem_lock(void)
+void release_shmem_lock(const char *caller_file_name, int caller_line_number)
 {
-	// TODO: don't do this either
-	return;
-
 	unsigned char *lock = (unsigned char *)(MAP_ADDRESS + LOCK_OFFSET);
 
 	*lock = 1;
+
+#ifdef PARAM_LOCK_DEBUG
+	PX4_INFO("release lock, file name: %s, line number: %d.\n",
+		 strrchr(caller_file_name, '/'), caller_line_number);
+#endif
+
 	return;
 }
 
 void init_shared_memory(void)
 {
 	//PX4_INFO("Value at lock address is %d\n", *(unsigned int*)0xfbfc000);
+	int i;
+
+	if(shmem_info_p)
+		return;
 
 	virt_addr = map_memory(MAP_ADDRESS);
 	shmem_info_p = (struct shmem_info *)virt_addr;
 
+	//init lock as 1
+	unsigned char *lock = (unsigned char *)(MAP_ADDRESS + LOCK_OFFSET);
+	*lock = 1;
+
+	for (i = 0; i < MAX_SHMEM_PARAMS / 8 + 1; i++) {
+		shmem_info_p->krait_changed_index[i] = 0;
+	}
+
 	//PX4_INFO("adsp memory mapped\n");
 }
 
@@ -175,7 +192,7 @@ void copy_params_to_shmem(struct param_info_s *param_info_base)
 		krait_changed_index[i] = 0;
 	}
 
-	release_shmem_lock();
+	release_shmem_lock(__FILE__, __LINE__);
 	//PX4_INFO("Released lock\n");
 
 }
@@ -217,21 +234,20 @@ void update_to_shmem(param_t param, union param_value_u value)
 
 #endif
 
-	release_shmem_lock();
+	release_shmem_lock(__FILE__, __LINE__);
 
 }
 
 void update_index_from_shmem(void)
 {
 	unsigned int i;
+	param_t params[MAX_SHMEM_PARAMS / 8 + 1];
 
 	if (get_shmem_lock(__FILE__, __LINE__) != 0) {
 		PX4_ERR("Could not get shmem lock\n");
 		return;
 	}
 
-	PX4_DEBUG("Updating index from shmem\n");
-
 	for (i = 0; i < MAX_SHMEM_PARAMS / 8 + 1; i++) {
 		// Check if any param has been changed.
 		if (krait_changed_index[i] != shmem_info_p->krait_changed_index[i]) {
@@ -243,15 +259,21 @@ void update_index_from_shmem(void)
 
 			// Update our krait_changed_index as well.
 			krait_changed_index[i] = shmem_info_p->krait_changed_index[i];
-
-			// FIXME: this is a hack but it gets the param so that it gets added
-			// to the local list param_values in param_shmem.c.
-			int32_t dummy;
-			param_get(param_to_get, &dummy);
+			params[i] = param_to_get;
+		} else {
+			params[i] = 0xFFFF;
 		}
 	}
+	release_shmem_lock(__FILE__, __LINE__);
 
-	release_shmem_lock();
+	// FIXME: this is a hack but it gets the param so that it gets added
+	// to the local list param_values in param_shmem.c.
+	for (i = 0; i < MAX_SHMEM_PARAMS / 8 + 1; i++) {
+		if(params[i] != 0xFFFF){
+			int32_t dummy;
+			param_get(params[i], &dummy);
+		}
+	}
 }
 
 
@@ -271,7 +293,7 @@ static void update_value_from_shmem(param_t param, union param_value_u *value)
 	bit_changed = 1 << param % 8;
 	shmem_info_p->krait_changed_index[byte_changed] &= ~bit_changed;
 
-	release_shmem_lock();
+	release_shmem_lock(__FILE__, __LINE__);
 
 #ifdef SHMEM_DEBUG
 
diff --git a/src/platforms/shmem.h b/src/platforms/shmem.h
index 45f26684fa..bed181c267 100644
--- a/src/platforms/shmem.h
+++ b/src/platforms/shmem.h
@@ -33,6 +33,8 @@
 
 #define MAX_SHMEM_PARAMS 2000 //MAP_SIZE - (LOCK_SIZE - sizeof(struct shmem_info))
 
+#define PARAM_BUFFER_SIZE (MAX_SHMEM_PARAMS / 8 + 1)
+
 struct shmem_info {
 	union param_value_u params_val[MAX_SHMEM_PARAMS];
 	unsigned char krait_changed_index[MAX_SHMEM_PARAMS / 8 + 1]; /*bit map of all params changed by krait*/

From 701d6314d2239c3b66ebec775cbdb77da7e5d05e Mon Sep 17 00:00:00 2001
From: jwilson <jwilson@qti.qualcomm.com>
Date: Mon, 20 Jun 2016 17:36:01 -0700
Subject: [PATCH 0747/1230] Fixes build problem in the shmem code.

---
 src/platforms/posix/px4_layer/CMakeLists.txt | 1 +
 1 file changed, 1 insertion(+)

diff --git a/src/platforms/posix/px4_layer/CMakeLists.txt b/src/platforms/posix/px4_layer/CMakeLists.txt
index 0e98f0bcb3..e18636a879 100644
--- a/src/platforms/posix/px4_layer/CMakeLists.txt
+++ b/src/platforms/posix/px4_layer/CMakeLists.txt
@@ -31,6 +31,7 @@
 #
 ############################################################################
 
+set(CMAKE_MODULE_PATH ${CMAKE_MODULE_PATH} "${CMAKE_SOURCE_DIR}/cmake/cmake_hexagon")
 include(hexagon_sdk)
 
 include_directories(${CMAKE_BINARY_DIR}/src/firmware/posix)

From a2c16a3b5eea3510b1bb9abd0bee98c23b9b37d5 Mon Sep 17 00:00:00 2001
From: jwilson <jwilson@qti.qualcomm.com>
Date: Mon, 20 Jun 2016 17:44:55 -0700
Subject: [PATCH 0748/1230] Another attempt to fix the build problem in the
 shmem code.

---
 src/platforms/posix/px4_layer/CMakeLists.txt | 11 +++++------
 1 file changed, 5 insertions(+), 6 deletions(-)

diff --git a/src/platforms/posix/px4_layer/CMakeLists.txt b/src/platforms/posix/px4_layer/CMakeLists.txt
index e18636a879..344621c694 100644
--- a/src/platforms/posix/px4_layer/CMakeLists.txt
+++ b/src/platforms/posix/px4_layer/CMakeLists.txt
@@ -31,13 +31,12 @@
 #
 ############################################################################
 
-set(CMAKE_MODULE_PATH ${CMAKE_MODULE_PATH} "${CMAKE_SOURCE_DIR}/cmake/cmake_hexagon")
-include(hexagon_sdk)
-
-include_directories(${CMAKE_BINARY_DIR}/src/firmware/posix)
-include_directories(${HEXAGON_SDK_INCLUDES})
-
 if("${CONFIG_SHMEM}" STREQUAL "1")
+	set(CMAKE_MODULE_PATH ${CMAKE_MODULE_PATH} "${CMAKE_SOURCE_DIR}/cmake/cmake_hexagon")
+	include(hexagon_sdk)
+
+	include_directories(${CMAKE_BINARY_DIR}/src/firmware/posix)
+	include_directories(${HEXAGON_SDK_INCLUDES})
 	list(APPEND SHMEM_SRCS
 		shmem_posix.c
 	)

From 109131927452662dc173693e2160bc5c61068c01 Mon Sep 17 00:00:00 2001
From: jwilson <jwilson@qti.qualcomm.com>
Date: Mon, 20 Jun 2016 18:36:56 -0700
Subject: [PATCH 0749/1230] Removing shmem_posix.c from the unit testing since
 param_shmem_test is disabled.

---
 unittests/CMakeLists.txt | 1 -
 1 file changed, 1 deletion(-)

diff --git a/unittests/CMakeLists.txt b/unittests/CMakeLists.txt
index 2a628bb1f1..c2e11ea37a 100644
--- a/unittests/CMakeLists.txt
+++ b/unittests/CMakeLists.txt
@@ -98,7 +98,6 @@ add_library(px4_platform
 			${PX4_SRC}/platforms/posix/px4_layer/px4_posix_impl.cpp
 			${PX4_SRC}/platforms/posix/px4_layer/px4_posix_tasks.cpp
 			${PX4_SRC}/platforms/posix/px4_layer/px4_sem.cpp
-			${PX4_SRC}/platforms/posix/px4_layer/shmem_posix.c
 			${PX4_SRC}/platforms/posix/work_queue/dq_addlast.c
 			${PX4_SRC}/platforms/posix/work_queue/dq_rem.c
 			${PX4_SRC}/platforms/posix/work_queue/dq_remfirst.c

From acc1f04b67de6b893cf6846398bbd6bfe56b9761 Mon Sep 17 00:00:00 2001
From: jwilson <jwilson@qti.qualcomm.com>
Date: Mon, 20 Jun 2016 19:36:21 -0700
Subject: [PATCH 0750/1230] Unfortunately, lot's of whitespace changes,
 required to satisfy unspecified code style format errors.

---
 src/modules/muorb/adsp/px4muorb.cpp           | 60 ++++++++-------
 .../muorb/krait/px4muorb_KraitRpcWrapper.cpp  | 77 +++++++++++--------
 src/platforms/posix/px4_layer/shmem_posix.c   | 22 +++---
 src/platforms/qurt/px4_layer/shmem_qurt.c     | 66 +++++++++-------
 4 files changed, 131 insertions(+), 94 deletions(-)

diff --git a/src/modules/muorb/adsp/px4muorb.cpp b/src/modules/muorb/adsp/px4muorb.cpp
index 8ce0a50d36..dad971ac7a 100644
--- a/src/modules/muorb/adsp/px4muorb.cpp
+++ b/src/modules/muorb/adsp/px4muorb.cpp
@@ -49,10 +49,10 @@ __BEGIN_DECLS
 extern int dspal_main(int argc, char *argv[]);
 extern struct shmem_info *shmem_info_p;
 extern int get_shmem_lock(const char *caller_file_name, int caller_line_number);
-extern void release_shmem_lock(const char *caller_file_name, int caller_line_number);
+extern void release_shmem_lock(const char *caller_file_name,
+		int caller_line_number);
 extern void init_shared_memory(void);
 __END_DECLS
-
 int px4muorb_orb_initialize()
 {
 	HAP_power_request(100, 100, 1000);
@@ -60,13 +60,14 @@ int px4muorb_orb_initialize()
 	// The uORB Manager needs to be initialized first up, otherwise the instance is nullptr.
 	uORB::Manager::initialize();
 	// Register the fastrpc muorb with uORBManager.
-	uORB::Manager::get_instance()->set_uorb_communicator(uORB::FastRpcChannel::GetInstance());
+	uORB::Manager::get_instance()->set_uorb_communicator(
+			uORB::FastRpcChannel::GetInstance());
 
 	// Now continue with the usual dspal startup.
-	const char *argv[] = {"dspal", "start"};
+	const char *argv[] = { "dspal", "start" };
 	int argc = 2;
 	int rc;
-	rc = dspal_main(argc, (char **)argv);
+	rc = dspal_main(argc, (char **) argv);
 
 	return rc;
 }
@@ -83,12 +84,13 @@ int px4muorb_get_absolute_time(uint64_t *time_us)
 }
 
 /*update value and param's change bit in shared memory*/
-int px4muorb_param_update_to_shmem(uint32_t param, const uint8_t *value, int data_len_in_bytes)
+int px4muorb_param_update_to_shmem(uint32_t param, const uint8_t *value,
+		int data_len_in_bytes)
 {
 	unsigned int byte_changed, bit_changed;
-	union param_value_u *param_value = (union param_value_u *)value;
+	union param_value_u *param_value = (union param_value_u *) value;
 
-	if(!shmem_info_p) {
+	if (!shmem_info_p) {
 		init_shared_memory();
 	}
 
@@ -108,11 +110,12 @@ int px4muorb_param_update_to_shmem(uint32_t param, const uint8_t *value, int dat
 	return 0;
 }
 
-int px4muorb_param_update_index_from_shmem(unsigned char *data, int data_len_in_bytes)
+int px4muorb_param_update_index_from_shmem(unsigned char *data,
+		int data_len_in_bytes)
 {
 	unsigned int i;
 
-	if(!shmem_info_p)
+	if (!shmem_info_p)
 		return -1;
 
 	if (get_shmem_lock(__FILE__, __LINE__) != 0) {
@@ -129,12 +132,13 @@ int px4muorb_param_update_index_from_shmem(unsigned char *data, int data_len_in_
 	return 0;
 }
 
-int px4muorb_param_update_value_from_shmem(uint32_t param, const uint8_t *value, int data_len_in_bytes)
+int px4muorb_param_update_value_from_shmem(uint32_t param, const uint8_t *value,
+		int data_len_in_bytes)
 {
 	unsigned int byte_changed, bit_changed;
-	union param_value_u *param_value = (union param_value_u *)value;
+	union param_value_u *param_value = (union param_value_u *) value;
 
-	if(!shmem_info_p)
+	if (!shmem_info_p)
 		return -1;
 
 	if (get_shmem_lock(__FILE__, __LINE__) != 0) {
@@ -192,14 +196,16 @@ int px4muorb_remove_subscriber(const char *name)
 	return rc;
 }
 
-int px4muorb_send_topic_data(const char *name, const uint8_t *data, int data_len_in_bytes)
+int px4muorb_send_topic_data(const char *name, const uint8_t *data,
+		int data_len_in_bytes)
 {
 	int rc = 0;
 	uORB::FastRpcChannel *channel = uORB::FastRpcChannel::GetInstance();
 	uORBCommunicator::IChannelRxHandler *rxHandler = channel->GetRxHandler();
 
 	if (rxHandler != nullptr) {
-		rc = rxHandler->process_received_message(name, data_len_in_bytes, (uint8_t *)data);
+		rc = rxHandler->process_received_message(name, data_len_in_bytes,
+				(uint8_t *) data);
 
 	} else {
 		rc = -1;
@@ -216,37 +222,39 @@ int px4muorb_is_subscriber_present(const char *topic_name, int *status)
 	rc = channel->is_subscriber_present(topic_name, &local_status);
 
 	if (rc == 0) {
-		*status = (int)local_status;
+		*status = (int) local_status;
 	}
 
 	return rc;
 }
 
-int px4muorb_receive_msg(int *msg_type, char *topic_name, int topic_name_len, uint8_t *data, int data_len_in_bytes,
-			 int *bytes_returned)
+int px4muorb_receive_msg(int *msg_type, char *topic_name, int topic_name_len,
+		uint8_t *data, int data_len_in_bytes, int *bytes_returned)
 {
 	int rc = 0;
 	int32_t local_msg_type = 0;
 	int32_t local_bytes_returned = 0;
 	uORB::FastRpcChannel *channel = uORB::FastRpcChannel::GetInstance();
 	//PX4_DEBUG( "topic_namePtr: [0x%p] dataPtr: [0x%p]", topic_name, data );
-	rc = channel->get_data(&local_msg_type, topic_name, topic_name_len, data, data_len_in_bytes, &local_bytes_returned);
-	*msg_type = (int)local_msg_type;
-	*bytes_returned = (int)local_bytes_returned;
+	rc = channel->get_data(&local_msg_type, topic_name, topic_name_len, data,
+			data_len_in_bytes, &local_bytes_returned);
+	*msg_type = (int) local_msg_type;
+	*bytes_returned = (int) local_bytes_returned;
 	return rc;
 }
 
-int px4muorb_receive_bulk_data(uint8_t *bulk_transfer_buffer, int max_size_in_bytes,
-			       int *returned_length_in_bytes, int *topic_count)
+int px4muorb_receive_bulk_data(uint8_t *bulk_transfer_buffer,
+		int max_size_in_bytes, int *returned_length_in_bytes, int *topic_count)
 {
 	int rc = 0;
 	int32_t local_bytes_returned = 0;
 	int32_t local_topic_count = 0;
 	uORB::FastRpcChannel *channel = uORB::FastRpcChannel::GetInstance();
 	//PX4_DEBUG( "topic_namePtr: [0x%p] dataPtr: [0x%p]", topic_name, data );
-	rc = channel->get_bulk_data(bulk_transfer_buffer,  max_size_in_bytes, &local_bytes_returned, &local_topic_count);
-	*returned_length_in_bytes = (int)local_bytes_returned;
-	*topic_count = (int)local_topic_count;
+	rc = channel->get_bulk_data(bulk_transfer_buffer, max_size_in_bytes,
+			&local_bytes_returned, &local_topic_count);
+	*returned_length_in_bytes = (int) local_bytes_returned;
+	*topic_count = (int) local_topic_count;
 	return rc;
 }
 
diff --git a/src/modules/muorb/krait/px4muorb_KraitRpcWrapper.cpp b/src/modules/muorb/krait/px4muorb_KraitRpcWrapper.cpp
index 71a80e6969..8d7c2db5f2 100644
--- a/src/modules/muorb/krait/px4muorb_KraitRpcWrapper.cpp
+++ b/src/modules/muorb/krait/px4muorb_KraitRpcWrapper.cpp
@@ -52,7 +52,7 @@ using namespace px4muorb;
 static char *_TopicNameBuffer = 0;
 static const int32_t _MAX_TOPIC_NAME_BUFFER = 256;
 
-static uint8_t *_DataBuffer  = 0;
+static uint8_t *_DataBuffer = 0;
 static const uint32_t _MAX_DATA_BUFFER_SIZE = 2048;
 
 static bool _Initialized = false;
@@ -61,7 +61,8 @@ static bool _Initialized = false;
 // hence we are trying to allocation 64K of byte buffers.
 static const uint32_t _MAX_TOPIC_DATA_BUFFER_SIZE = 1024;
 static const uint32_t _MAX_TOPICS = 64;
-static const uint32_t _MAX_BULK_TRANSFER_BUFFER_SIZE = _MAX_TOPIC_DATA_BUFFER_SIZE * _MAX_TOPICS;
+static const uint32_t _MAX_BULK_TRANSFER_BUFFER_SIZE =
+		_MAX_TOPIC_DATA_BUFFER_SIZE * _MAX_TOPICS;
 static uint8_t *_BulkTransferBuffer = 0;
 
 unsigned char *adsp_changed_index = 0;
@@ -102,12 +103,13 @@ int calc_timer_diff_to_dsp_us(int32_t *time_diff_us)
 	// Do this call right after reading to avoid latency here.
 	timespec ts;
 	clock_gettime(CLOCK_MONOTONIC, &ts);
-	uint64_t time_appsproc = ((uint64_t)ts.tv_sec) * 1000000llu + (ts.tv_nsec / 1000);
+	uint64_t time_appsproc = ((uint64_t) ts.tv_sec) * 1000000llu
+			+ (ts.tv_nsec / 1000);
 
 	close(fd);
 
 	uint64_t time_dsp;
-	int ret  = sscanf(buffer, "%llx", &time_dsp);
+	int ret = sscanf(buffer, "%llx", &time_dsp);
 
 	if (ret < 0) {
 		PX4_ERR("Could not parse DSP timer.");
@@ -119,8 +121,9 @@ int calc_timer_diff_to_dsp_us(int32_t *time_diff_us)
 	time_dsp /= 19.2;
 
 	// Before casting to in32_t, check if it fits.
-	uint64_t abs_diff = (time_appsproc > time_dsp)
-			    ? (time_appsproc - time_dsp) : (time_dsp - time_appsproc);
+	uint64_t abs_diff =
+			(time_appsproc > time_dsp) ?
+					(time_appsproc - time_dsp) : (time_dsp - time_appsproc);
 
 	if (abs_diff > INT32_MAX) {
 		PX4_ERR("Timer difference too big");
@@ -129,13 +132,14 @@ int calc_timer_diff_to_dsp_us(int32_t *time_diff_us)
 
 	*time_diff_us = time_appsproc - time_dsp;
 
-	PX4_DEBUG("found time_dsp: %llu us, time_appsproc: %llu us", time_dsp, time_appsproc);
-	PX4_DEBUG("found time_diff: %li us, %.6f s", *time_diff_us, ((double)*time_diff_us) / 1e6);
+	PX4_DEBUG("found time_dsp: %llu us, time_appsproc: %llu us",
+			time_dsp, time_appsproc);
+	PX4_DEBUG("found time_diff: %li us, %.6f s",
+			*time_diff_us, ((double)*time_diff_us) / 1e6);
 
 	return 0;
 }
 
-
 px4muorb::KraitRpcWrapper::KraitRpcWrapper()
 {
 }
@@ -153,21 +157,25 @@ bool px4muorb::KraitRpcWrapper::Initialize()
 
 	PX4_DEBUG("%s Now calling rpcmem_alloc...", __FUNCTION__);
 
-	_BulkTransferBuffer = (uint8_t *) rpcmem_alloc(MUORB_KRAIT_FASTRPC_HEAP_ID, MUORB_KRAIT_FASTRPC_MEM_FLAGS,
-			      _MAX_BULK_TRANSFER_BUFFER_SIZE * sizeof(uint8_t));
+	_BulkTransferBuffer = (uint8_t *) rpcmem_alloc(MUORB_KRAIT_FASTRPC_HEAP_ID,
+			MUORB_KRAIT_FASTRPC_MEM_FLAGS,
+			_MAX_BULK_TRANSFER_BUFFER_SIZE * sizeof(uint8_t));
 	rc = (_BulkTransferBuffer != NULL) ? true : false;
 
 	if (!rc) {
-		PX4_ERR("%s rpcmem_alloc failed! for bulk transfer buffers", __FUNCTION__);
+		PX4_ERR("%s rpcmem_alloc failed! for bulk transfer buffers",
+				__FUNCTION__);
 		return rc;
 
 	} else {
-		PX4_DEBUG("%s rpcmem_alloc passed for Bulk transfer buffers buffer_size: %d addr: %p",
-			  __FUNCTION__, (_MAX_BULK_TRANSFER_BUFFER_SIZE * sizeof(uint8_t)), _BulkTransferBuffer);
+		PX4_DEBUG(
+				"%s rpcmem_alloc passed for Bulk transfer buffers buffer_size: %d addr: %p",
+				__FUNCTION__, (_MAX_BULK_TRANSFER_BUFFER_SIZE * sizeof(uint8_t)), _BulkTransferBuffer);
 	}
 
 	_TopicNameBuffer = (char *) rpcmem_alloc(MUORB_KRAIT_FASTRPC_HEAP_ID,
-			   MUORB_KRAIT_FASTRPC_MEM_FLAGS, _MAX_TOPIC_NAME_BUFFER * sizeof(char));
+			MUORB_KRAIT_FASTRPC_MEM_FLAGS,
+			_MAX_TOPIC_NAME_BUFFER * sizeof(char));
 
 	rc = (_TopicNameBuffer != NULL) ? true : false;
 
@@ -182,7 +190,8 @@ bool px4muorb::KraitRpcWrapper::Initialize()
 
 	// now allocate the data buffer.
 	_DataBuffer = (uint8_t *) rpcmem_alloc(MUORB_KRAIT_FASTRPC_HEAP_ID,
-					       MUORB_KRAIT_FASTRPC_MEM_FLAGS, _MAX_DATA_BUFFER_SIZE * sizeof(uint8_t));
+			MUORB_KRAIT_FASTRPC_MEM_FLAGS,
+			_MAX_DATA_BUFFER_SIZE * sizeof(uint8_t));
 
 	rc = (_DataBuffer != NULL) ? true : false;
 
@@ -199,7 +208,7 @@ bool px4muorb::KraitRpcWrapper::Initialize()
 	}
 
 	adsp_changed_index = (uint8_t *) rpcmem_alloc(MUORB_KRAIT_FASTRPC_HEAP_ID,
-					       MUORB_KRAIT_FASTRPC_MEM_FLAGS, PARAM_BUFFER_SIZE * sizeof(uint8_t));
+			MUORB_KRAIT_FASTRPC_MEM_FLAGS, PARAM_BUFFER_SIZE * sizeof(uint8_t));
 
 	rc = (adsp_changed_index != NULL) ? true : false;
 
@@ -218,7 +227,8 @@ bool px4muorb::KraitRpcWrapper::Initialize()
 
 	// call muorb initialize routine.
 	if (px4muorb_orb_initialize() != 0) {
-		PX4_ERR("%s Error calling the uorb fastrpc initalize method..", __FUNCTION__);
+		PX4_ERR("%s Error calling the uorb fastrpc initalize method..",
+				__FUNCTION__);
 		rc = false;
 		return rc;
 	}
@@ -233,7 +243,8 @@ bool px4muorb::KraitRpcWrapper::Initialize()
 
 	int diff = (time_dsp - time_appsproc);
 
-	PX4_DEBUG("time_dsp: %llu us, time appsproc: %llu us, diff: %d us", time_dsp, time_appsproc, diff);
+	PX4_DEBUG("time_dsp: %llu us, time appsproc: %llu us, diff: %d us",
+			time_dsp, time_appsproc, diff);
 
 	_Initialized = true;
 	return rc;
@@ -256,7 +267,7 @@ bool px4muorb::KraitRpcWrapper::Terminate()
 		_DataBuffer = 0;
 	}
 
-	if(adsp_changed_index != NULL) {
+	if (adsp_changed_index != NULL) {
 		rpcmem_free(adsp_changed_index);
 		adsp_changed_index = 0;
 	}
@@ -275,28 +286,32 @@ int32_t px4muorb::KraitRpcWrapper::RemoveSubscriber(const char *topic)
 	return (_Initialized ? px4muorb_remove_subscriber(topic) : -1);
 }
 
-int32_t px4muorb::KraitRpcWrapper::IsSubscriberPresent(const char *topic, int32_t *status)
+int32_t px4muorb::KraitRpcWrapper::IsSubscriberPresent(const char *topic,
+		int32_t *status)
 {
 	return (_Initialized ? px4muorb_is_subscriber_present(topic, status) : -1);
 }
 
-int32_t px4muorb::KraitRpcWrapper::SendData(const char *topic, int32_t length_in_bytes, const uint8_t *data)
+int32_t px4muorb::KraitRpcWrapper::SendData(const char *topic,
+		int32_t length_in_bytes, const uint8_t *data)
 {
-	return (_Initialized ? px4muorb_send_topic_data(topic, data, length_in_bytes) : -1);
+	return (_Initialized ?
+			px4muorb_send_topic_data(topic, data, length_in_bytes) : -1);
 }
 
-int32_t px4muorb::KraitRpcWrapper::ReceiveData(int32_t *msg_type, char **topic, int32_t *length_in_bytes,
-		uint8_t **data)
+int32_t px4muorb::KraitRpcWrapper::ReceiveData(int32_t *msg_type, char **topic,
+		int32_t *length_in_bytes, uint8_t **data)
 {
 	int32_t rc = -1;
 
 	if (_Initialized) {
-		rc = px4muorb_receive_msg(msg_type, _TopicNameBuffer, _MAX_TOPIC_NAME_BUFFER, _DataBuffer, _MAX_DATA_BUFFER_SIZE,
-					  length_in_bytes);
+		rc = px4muorb_receive_msg(msg_type, _TopicNameBuffer,
+				_MAX_TOPIC_NAME_BUFFER, _DataBuffer, _MAX_DATA_BUFFER_SIZE,
+				length_in_bytes);
 
 		if (rc == 0) {
 			*topic = _TopicNameBuffer;
-			*data  = _DataBuffer;
+			*data = _DataBuffer;
 
 		} else {
 			PX4_ERR("ERROR: Getting data from fastRPC link");
@@ -309,13 +324,15 @@ int32_t px4muorb::KraitRpcWrapper::ReceiveData(int32_t *msg_type, char **topic,
 	return rc;
 }
 
-int32_t px4muorb::KraitRpcWrapper::ReceiveBulkData(uint8_t **bulk_data, int32_t *length_in_bytes, int32_t *topic_count)
+int32_t px4muorb::KraitRpcWrapper::ReceiveBulkData(uint8_t **bulk_data,
+		int32_t *length_in_bytes, int32_t *topic_count)
 {
 	int32_t rc = -1;
 
 	if (_Initialized) {
 		//rc = px4muorb_receive_msg( msg_type, _TopicNameBuffer, _MAX_TOPIC_NAME_BUFFER, _DataBuffer, _MAX_DATA_BUFFER_SIZE, length_in_bytes  );
-		rc = px4muorb_receive_bulk_data(_BulkTransferBuffer, _MAX_BULK_TRANSFER_BUFFER_SIZE,  length_in_bytes, topic_count);
+		rc = px4muorb_receive_bulk_data(_BulkTransferBuffer,
+				_MAX_BULK_TRANSFER_BUFFER_SIZE, length_in_bytes, topic_count);
 
 		if (rc == 0) {
 			*bulk_data = _BulkTransferBuffer;
diff --git a/src/platforms/posix/px4_layer/shmem_posix.c b/src/platforms/posix/px4_layer/shmem_posix.c
index 8fbaece56f..aa0ef2b959 100644
--- a/src/platforms/posix/px4_layer/shmem_posix.c
+++ b/src/platforms/posix/px4_layer/shmem_posix.c
@@ -60,33 +60,36 @@ void update_index_from_shmem(void);
 uint64_t update_from_shmem_prev_time = 0, update_from_shmem_current_time = 0;
 extern unsigned char *adsp_changed_index;
 
-struct param_wbuf_s {
+struct param_wbuf_s
+{
 	param_t param;
 	union param_value_u val;
 	bool unsaved;
 };
 
-
 /*update value and param's change bit in shared memory*/
 void update_to_shmem(param_t param, union param_value_u value)
 {
-	if(px4muorb_param_update_to_shmem(param, (unsigned char*)&value, sizeof(value)))
+	if (px4muorb_param_update_to_shmem(param, (unsigned char*) &value,
+			sizeof(value)))
 		PX4_ERR("krait update param %u failed", param);
 }
 
 void update_index_from_shmem(void)
 {
-	if(!adsp_changed_index) {
+	if (!adsp_changed_index) {
 		PX4_ERR("%s no param buffer", __FUNCTION__);
 		return;
 	}
 
-	px4muorb_param_update_index_from_shmem(adsp_changed_index, PARAM_BUFFER_SIZE);
+	px4muorb_param_update_index_from_shmem(adsp_changed_index,
+			PARAM_BUFFER_SIZE);
 }
 
 static void update_value_from_shmem(param_t param, union param_value_u *value)
 {
-	if(px4muorb_param_update_value_from_shmem(param, (unsigned char*)value, sizeof(union param_value_u)))
+	if (px4muorb_param_update_value_from_shmem(param, (unsigned char*) value,
+			sizeof(union param_value_u)))
 		PX4_ERR("%s get param failed", __FUNCTION__);
 }
 
@@ -95,7 +98,7 @@ int update_from_shmem(param_t param, union param_value_u *value)
 	unsigned int byte_changed, bit_changed;
 	unsigned int retval = 0;
 
-	if(!adsp_changed_index) {
+	if (!adsp_changed_index) {
 		PX4_ERR("%s no param buffer", __FUNCTION__);
 		return 0;
 	}
@@ -103,7 +106,7 @@ int update_from_shmem(param_t param, union param_value_u *value)
 	update_from_shmem_current_time = hrt_absolute_time();
 
 	if ((update_from_shmem_current_time - update_from_shmem_prev_time)
-	    > 1000000) { //update every 1 second
+			> 1000000) { //update every 1 second
 		update_from_shmem_prev_time = update_from_shmem_current_time;
 		update_index_from_shmem();
 	}
@@ -119,7 +122,8 @@ int update_from_shmem(param_t param, union param_value_u *value)
 
 	//else {PX4_INFO("no change to param %s", param_name(param));}
 
-	PX4_DEBUG("%s %d bit on adsp index[%d]", (retval) ? "cleared" : "unchanged", bit_changed, byte_changed);
+	PX4_DEBUG("%s %d bit on adsp index[%d]",
+			(retval) ? "cleared" : "unchanged", bit_changed, byte_changed);
 
 	return retval;
 }
diff --git a/src/platforms/qurt/px4_layer/shmem_qurt.c b/src/platforms/qurt/px4_layer/shmem_qurt.c
index 5aeabf520b..b4b24eab39 100644
--- a/src/platforms/qurt/px4_layer/shmem_qurt.c
+++ b/src/platforms/qurt/px4_layer/shmem_qurt.c
@@ -1,4 +1,3 @@
-
 /****************************************************************************
  *
  * Copyright (c) 2015 Vijay Venkatraman. All rights reserved.
@@ -76,23 +75,24 @@ static unsigned log2_for_int(unsigned v)
 	return r;
 }
 
-struct param_wbuf_s {
-	param_t			param;
-	union param_value_u	val;
-	bool			unsaved;
+struct param_wbuf_s
+{
+	param_t param;
+	union param_value_u val;
+	bool unsaved;
 };
 extern struct param_wbuf_s *param_find_changed(param_t param);
 
 static void *map_memory(off_t target)
 {
 
-	return (void *)(target + LOCK_SIZE);
+	return (void *) (target + LOCK_SIZE);
 
 }
 
 int get_shmem_lock(const char *caller_file_name, int caller_line_number)
 {
-	unsigned char *lock = (unsigned char *)(MAP_ADDRESS + LOCK_OFFSET);
+	unsigned char *lock = (unsigned char *) (MAP_ADDRESS + LOCK_OFFSET);
 	unsigned int i = 0;
 
 #ifdef PARAM_LOCK_DEBUG
@@ -103,16 +103,19 @@ int get_shmem_lock(const char *caller_file_name, int caller_line_number)
 		i++;
 		usleep(1000);
 
-		if (i > 100) { break; }
+		if (i > 100) {
+			break;
+		}
 	}
 
 	if (i > 100) {
 		PX4_INFO("Could not get lock, file name: %s, line number: %d.\n",
-			 strrchr(caller_file_name, '/'), caller_line_number);
+				strrchr(caller_file_name, '/'), caller_line_number);
 		return -1;
 
 	} else {
-		PX4_DEBUG("Lock acquired, file name: %s, line number: %d\n", caller_file_name, caller_line_number);
+		PX4_DEBUG("Lock acquired, file name: %s, line number: %d\n",
+				caller_file_name, caller_line_number);
 	}
 
 	return 0; //got the lock
@@ -121,13 +124,13 @@ int get_shmem_lock(const char *caller_file_name, int caller_line_number)
 
 void release_shmem_lock(const char *caller_file_name, int caller_line_number)
 {
-	unsigned char *lock = (unsigned char *)(MAP_ADDRESS + LOCK_OFFSET);
+	unsigned char *lock = (unsigned char *) (MAP_ADDRESS + LOCK_OFFSET);
 
 	*lock = 1;
 
 #ifdef PARAM_LOCK_DEBUG
 	PX4_INFO("release lock, file name: %s, line number: %d.\n",
-		 strrchr(caller_file_name, '/'), caller_line_number);
+			strrchr(caller_file_name, '/'), caller_line_number);
 #endif
 
 	return;
@@ -138,14 +141,14 @@ void init_shared_memory(void)
 	//PX4_INFO("Value at lock address is %d\n", *(unsigned int*)0xfbfc000);
 	int i;
 
-	if(shmem_info_p)
+	if (shmem_info_p)
 		return;
 
 	virt_addr = map_memory(MAP_ADDRESS);
-	shmem_info_p = (struct shmem_info *)virt_addr;
+	shmem_info_p = (struct shmem_info *) virt_addr;
 
 	//init lock as 1
-	unsigned char *lock = (unsigned char *)(MAP_ADDRESS + LOCK_OFFSET);
+	unsigned char *lock = (unsigned char *) (MAP_ADDRESS + LOCK_OFFSET);
 	*lock = 1;
 
 	for (i = 0; i < MAX_SHMEM_PARAMS / 8 + 1; i++) {
@@ -157,7 +160,7 @@ void init_shared_memory(void)
 
 void copy_params_to_shmem(struct param_info_s *param_info_base)
 {
-	param_t	param;
+	param_t param;
 	unsigned int i;
 
 	if (get_shmem_lock(__FILE__, __LINE__) != 0) {
@@ -171,9 +174,13 @@ void copy_params_to_shmem(struct param_info_s *param_info_base)
 		//{PX4_INFO("writing to offset %d\n", (unsigned char*)(shmem_info_p->adsp_params[param].name)-(unsigned char*)shmem_info_p);}
 		struct param_wbuf_s *s = param_find_changed(param);
 
-		if (s == NULL) { shmem_info_p->params_val[param] = param_info_base[param].val; }
+		if (s == NULL) {
+			shmem_info_p->params_val[param] = param_info_base[param].val;
+		}
 
-		else { shmem_info_p->params_val[param] = s->val; }
+		else {
+			shmem_info_p->params_val[param] = s->val;
+		}
 
 #ifdef SHMEM_DEBUG
 
@@ -197,7 +204,6 @@ void copy_params_to_shmem(struct param_info_s *param_info_base)
 
 }
 
-
 /*update value and param's change bit in shared memory*/
 void update_to_shmem(param_t param, union param_value_u value)
 {
@@ -224,12 +230,12 @@ void update_to_shmem(param_t param, union param_value_u value)
 
 	if (param_type(param) == PARAM_TYPE_INT32) {
 		PX4_INFO("Set value %d for param %s to shmem, set adsp index %d:%d\n", value.i, param_name(param), byte_changed,
-			 bit_changed);
+				bit_changed);
 	}
 
 	else if (param_type(param) == PARAM_TYPE_FLOAT) {
 		PX4_INFO("Set value %f for param %s to shmem, set adsp index %d:%d\n", value.f, param_name(param), byte_changed,
-			 bit_changed);
+				bit_changed);
 	}
 
 #endif
@@ -254,7 +260,9 @@ void update_index_from_shmem(void)
 
 			// If a param has changed, we need to find out which one.
 			// From the byte and bit that is different, we can resolve the param number.
-			unsigned bit = log2_for_int(krait_changed_index[i] ^ shmem_info_p->krait_changed_index[i]);
+			unsigned bit = log2_for_int(
+					krait_changed_index[i]
+							^ shmem_info_p->krait_changed_index[i]);
 			param_t param_to_get = i * 8 + bit;
 
 			// Update our krait_changed_index as well.
@@ -269,14 +277,13 @@ void update_index_from_shmem(void)
 	// FIXME: this is a hack but it gets the param so that it gets added
 	// to the local list param_values in param_shmem.c.
 	for (i = 0; i < MAX_SHMEM_PARAMS / 8 + 1; i++) {
-		if(params[i] != 0xFFFF){
+		if (params[i] != 0xFFFF) {
 			int32_t dummy;
 			param_get(params[i], &dummy);
 		}
 	}
 }
 
-
 static void update_value_from_shmem(param_t param, union param_value_u *value)
 {
 	unsigned int byte_changed, bit_changed;
@@ -299,12 +306,12 @@ static void update_value_from_shmem(param_t param, union param_value_u *value)
 
 	if (param_type(param) == PARAM_TYPE_INT32) {
 		PX4_INFO("Got value %d for param %s from shmem, cleared krait index %d:%d\n", value->i, param_name(param), byte_changed,
-			 bit_changed);
+				bit_changed);
 	}
 
 	else if (param_type(param) == PARAM_TYPE_FLOAT) {
 		PX4_INFO("Got value %f for param %s from shmem, cleared krait index %d:%d\n", value->f, param_name(param), byte_changed,
-			 bit_changed);
+				bit_changed);
 	}
 
 #endif
@@ -321,7 +328,8 @@ int update_from_shmem(param_t param, union param_value_u *value)
 
 	update_from_shmem_current_time = hrt_absolute_time();
 
-	if ((update_from_shmem_current_time - update_from_shmem_prev_time) > 1000000) { //update every 1 second
+	if ((update_from_shmem_current_time - update_from_shmem_prev_time)
+			> 1000000) { //update every 1 second
 		update_from_shmem_prev_time = update_from_shmem_current_time;
 		update_index_from_shmem();
 	}
@@ -337,9 +345,9 @@ int update_from_shmem(param_t param, union param_value_u *value)
 
 	//else {PX4_INFO("no change to param %s\n", param_name(param));}
 
-	PX4_DEBUG("%s %d bit on krait changed index[%d]\n", (retval) ? "cleared" : "unchanged", bit_changed, byte_changed);
+	PX4_DEBUG("%s %d bit on krait changed index[%d]\n",
+			(retval) ? "cleared" : "unchanged", bit_changed, byte_changed);
 
 	return retval;
 }
 
-

From 9794bb2f2f8843890ec64cf4df330f0c533f69f6 Mon Sep 17 00:00:00 2001
From: jwilson <jwilson@qti.qualcomm.com>
Date: Tue, 21 Jun 2016 06:50:27 -0700
Subject: [PATCH 0751/1230] Running fix_code_style.sh on the requested source
 files.

---
 src/modules/muorb/adsp/px4muorb.cpp           | 22 ++++-----
 .../muorb/krait/px4muorb_KraitRpcWrapper.cpp  | 45 ++++++++++---------
 src/platforms/posix/px4_layer/shmem_posix.c   | 19 ++++----
 src/platforms/qurt/px4_layer/shmem_qurt.c     | 41 +++++++++--------
 4 files changed, 67 insertions(+), 60 deletions(-)

diff --git a/src/modules/muorb/adsp/px4muorb.cpp b/src/modules/muorb/adsp/px4muorb.cpp
index dad971ac7a..4e25989b17 100644
--- a/src/modules/muorb/adsp/px4muorb.cpp
+++ b/src/modules/muorb/adsp/px4muorb.cpp
@@ -50,7 +50,7 @@ extern int dspal_main(int argc, char *argv[]);
 extern struct shmem_info *shmem_info_p;
 extern int get_shmem_lock(const char *caller_file_name, int caller_line_number);
 extern void release_shmem_lock(const char *caller_file_name,
-		int caller_line_number);
+			       int caller_line_number);
 extern void init_shared_memory(void);
 __END_DECLS
 int px4muorb_orb_initialize()
@@ -61,7 +61,7 @@ int px4muorb_orb_initialize()
 	uORB::Manager::initialize();
 	// Register the fastrpc muorb with uORBManager.
 	uORB::Manager::get_instance()->set_uorb_communicator(
-			uORB::FastRpcChannel::GetInstance());
+		uORB::FastRpcChannel::GetInstance());
 
 	// Now continue with the usual dspal startup.
 	const char *argv[] = { "dspal", "start" };
@@ -85,7 +85,7 @@ int px4muorb_get_absolute_time(uint64_t *time_us)
 
 /*update value and param's change bit in shared memory*/
 int px4muorb_param_update_to_shmem(uint32_t param, const uint8_t *value,
-		int data_len_in_bytes)
+				   int data_len_in_bytes)
 {
 	unsigned int byte_changed, bit_changed;
 	union param_value_u *param_value = (union param_value_u *) value;
@@ -115,8 +115,9 @@ int px4muorb_param_update_index_from_shmem(unsigned char *data,
 {
 	unsigned int i;
 
-	if (!shmem_info_p)
+	if (!shmem_info_p) {
 		return -1;
+	}
 
 	if (get_shmem_lock(__FILE__, __LINE__) != 0) {
 		PX4_INFO("Could not get shmem lock\n");
@@ -138,8 +139,9 @@ int px4muorb_param_update_value_from_shmem(uint32_t param, const uint8_t *value,
 	unsigned int byte_changed, bit_changed;
 	union param_value_u *param_value = (union param_value_u *) value;
 
-	if (!shmem_info_p)
+	if (!shmem_info_p) {
 		return -1;
+	}
 
 	if (get_shmem_lock(__FILE__, __LINE__) != 0) {
 		PX4_INFO("Could not get shmem lock\n");
@@ -197,7 +199,7 @@ int px4muorb_remove_subscriber(const char *name)
 }
 
 int px4muorb_send_topic_data(const char *name, const uint8_t *data,
-		int data_len_in_bytes)
+			     int data_len_in_bytes)
 {
 	int rc = 0;
 	uORB::FastRpcChannel *channel = uORB::FastRpcChannel::GetInstance();
@@ -229,7 +231,7 @@ int px4muorb_is_subscriber_present(const char *topic_name, int *status)
 }
 
 int px4muorb_receive_msg(int *msg_type, char *topic_name, int topic_name_len,
-		uint8_t *data, int data_len_in_bytes, int *bytes_returned)
+			 uint8_t *data, int data_len_in_bytes, int *bytes_returned)
 {
 	int rc = 0;
 	int32_t local_msg_type = 0;
@@ -237,14 +239,14 @@ int px4muorb_receive_msg(int *msg_type, char *topic_name, int topic_name_len,
 	uORB::FastRpcChannel *channel = uORB::FastRpcChannel::GetInstance();
 	//PX4_DEBUG( "topic_namePtr: [0x%p] dataPtr: [0x%p]", topic_name, data );
 	rc = channel->get_data(&local_msg_type, topic_name, topic_name_len, data,
-			data_len_in_bytes, &local_bytes_returned);
+			       data_len_in_bytes, &local_bytes_returned);
 	*msg_type = (int) local_msg_type;
 	*bytes_returned = (int) local_bytes_returned;
 	return rc;
 }
 
 int px4muorb_receive_bulk_data(uint8_t *bulk_transfer_buffer,
-		int max_size_in_bytes, int *returned_length_in_bytes, int *topic_count)
+			       int max_size_in_bytes, int *returned_length_in_bytes, int *topic_count)
 {
 	int rc = 0;
 	int32_t local_bytes_returned = 0;
@@ -252,7 +254,7 @@ int px4muorb_receive_bulk_data(uint8_t *bulk_transfer_buffer,
 	uORB::FastRpcChannel *channel = uORB::FastRpcChannel::GetInstance();
 	//PX4_DEBUG( "topic_namePtr: [0x%p] dataPtr: [0x%p]", topic_name, data );
 	rc = channel->get_bulk_data(bulk_transfer_buffer, max_size_in_bytes,
-			&local_bytes_returned, &local_topic_count);
+				    &local_bytes_returned, &local_topic_count);
 	*returned_length_in_bytes = (int) local_bytes_returned;
 	*topic_count = (int) local_topic_count;
 	return rc;
diff --git a/src/modules/muorb/krait/px4muorb_KraitRpcWrapper.cpp b/src/modules/muorb/krait/px4muorb_KraitRpcWrapper.cpp
index 8d7c2db5f2..b7bea9cad9 100644
--- a/src/modules/muorb/krait/px4muorb_KraitRpcWrapper.cpp
+++ b/src/modules/muorb/krait/px4muorb_KraitRpcWrapper.cpp
@@ -62,7 +62,7 @@ static bool _Initialized = false;
 static const uint32_t _MAX_TOPIC_DATA_BUFFER_SIZE = 1024;
 static const uint32_t _MAX_TOPICS = 64;
 static const uint32_t _MAX_BULK_TRANSFER_BUFFER_SIZE =
-		_MAX_TOPIC_DATA_BUFFER_SIZE * _MAX_TOPICS;
+	_MAX_TOPIC_DATA_BUFFER_SIZE * _MAX_TOPICS;
 static uint8_t *_BulkTransferBuffer = 0;
 
 unsigned char *adsp_changed_index = 0;
@@ -104,7 +104,7 @@ int calc_timer_diff_to_dsp_us(int32_t *time_diff_us)
 	timespec ts;
 	clock_gettime(CLOCK_MONOTONIC, &ts);
 	uint64_t time_appsproc = ((uint64_t) ts.tv_sec) * 1000000llu
-			+ (ts.tv_nsec / 1000);
+				 + (ts.tv_nsec / 1000);
 
 	close(fd);
 
@@ -122,8 +122,8 @@ int calc_timer_diff_to_dsp_us(int32_t *time_diff_us)
 
 	// Before casting to in32_t, check if it fits.
 	uint64_t abs_diff =
-			(time_appsproc > time_dsp) ?
-					(time_appsproc - time_dsp) : (time_dsp - time_appsproc);
+		(time_appsproc > time_dsp) ?
+		(time_appsproc - time_dsp) : (time_dsp - time_appsproc);
 
 	if (abs_diff > INT32_MAX) {
 		PX4_ERR("Timer difference too big");
@@ -133,9 +133,9 @@ int calc_timer_diff_to_dsp_us(int32_t *time_diff_us)
 	*time_diff_us = time_appsproc - time_dsp;
 
 	PX4_DEBUG("found time_dsp: %llu us, time_appsproc: %llu us",
-			time_dsp, time_appsproc);
+		  time_dsp, time_appsproc);
 	PX4_DEBUG("found time_diff: %li us, %.6f s",
-			*time_diff_us, ((double)*time_diff_us) / 1e6);
+		  *time_diff_us, ((double)*time_diff_us) / 1e6);
 
 	return 0;
 }
@@ -158,24 +158,24 @@ bool px4muorb::KraitRpcWrapper::Initialize()
 	PX4_DEBUG("%s Now calling rpcmem_alloc...", __FUNCTION__);
 
 	_BulkTransferBuffer = (uint8_t *) rpcmem_alloc(MUORB_KRAIT_FASTRPC_HEAP_ID,
-			MUORB_KRAIT_FASTRPC_MEM_FLAGS,
-			_MAX_BULK_TRANSFER_BUFFER_SIZE * sizeof(uint8_t));
+			      MUORB_KRAIT_FASTRPC_MEM_FLAGS,
+			      _MAX_BULK_TRANSFER_BUFFER_SIZE * sizeof(uint8_t));
 	rc = (_BulkTransferBuffer != NULL) ? true : false;
 
 	if (!rc) {
 		PX4_ERR("%s rpcmem_alloc failed! for bulk transfer buffers",
-				__FUNCTION__);
+			__FUNCTION__);
 		return rc;
 
 	} else {
 		PX4_DEBUG(
-				"%s rpcmem_alloc passed for Bulk transfer buffers buffer_size: %d addr: %p",
-				__FUNCTION__, (_MAX_BULK_TRANSFER_BUFFER_SIZE * sizeof(uint8_t)), _BulkTransferBuffer);
+			"%s rpcmem_alloc passed for Bulk transfer buffers buffer_size: %d addr: %p",
+			__FUNCTION__, (_MAX_BULK_TRANSFER_BUFFER_SIZE * sizeof(uint8_t)), _BulkTransferBuffer);
 	}
 
 	_TopicNameBuffer = (char *) rpcmem_alloc(MUORB_KRAIT_FASTRPC_HEAP_ID,
-			MUORB_KRAIT_FASTRPC_MEM_FLAGS,
-			_MAX_TOPIC_NAME_BUFFER * sizeof(char));
+			   MUORB_KRAIT_FASTRPC_MEM_FLAGS,
+			   _MAX_TOPIC_NAME_BUFFER * sizeof(char));
 
 	rc = (_TopicNameBuffer != NULL) ? true : false;
 
@@ -190,8 +190,8 @@ bool px4muorb::KraitRpcWrapper::Initialize()
 
 	// now allocate the data buffer.
 	_DataBuffer = (uint8_t *) rpcmem_alloc(MUORB_KRAIT_FASTRPC_HEAP_ID,
-			MUORB_KRAIT_FASTRPC_MEM_FLAGS,
-			_MAX_DATA_BUFFER_SIZE * sizeof(uint8_t));
+					       MUORB_KRAIT_FASTRPC_MEM_FLAGS,
+					       _MAX_DATA_BUFFER_SIZE * sizeof(uint8_t));
 
 	rc = (_DataBuffer != NULL) ? true : false;
 
@@ -208,12 +208,13 @@ bool px4muorb::KraitRpcWrapper::Initialize()
 	}
 
 	adsp_changed_index = (uint8_t *) rpcmem_alloc(MUORB_KRAIT_FASTRPC_HEAP_ID,
-			MUORB_KRAIT_FASTRPC_MEM_FLAGS, PARAM_BUFFER_SIZE * sizeof(uint8_t));
+			     MUORB_KRAIT_FASTRPC_MEM_FLAGS, PARAM_BUFFER_SIZE * sizeof(uint8_t));
 
 	rc = (adsp_changed_index != NULL) ? true : false;
 
 	if (!rc) {
 		PX4_ERR("%s rpcmem_alloc failed! for adsp_changed_index", __FUNCTION__);
+
 	} else {
 		memset(adsp_changed_index, 0, PARAM_BUFFER_SIZE * sizeof(uint8_t));
 	}
@@ -228,7 +229,7 @@ bool px4muorb::KraitRpcWrapper::Initialize()
 	// call muorb initialize routine.
 	if (px4muorb_orb_initialize() != 0) {
 		PX4_ERR("%s Error calling the uorb fastrpc initalize method..",
-				__FUNCTION__);
+			__FUNCTION__);
 		rc = false;
 		return rc;
 	}
@@ -244,7 +245,7 @@ bool px4muorb::KraitRpcWrapper::Initialize()
 	int diff = (time_dsp - time_appsproc);
 
 	PX4_DEBUG("time_dsp: %llu us, time appsproc: %llu us, diff: %d us",
-			time_dsp, time_appsproc, diff);
+		  time_dsp, time_appsproc, diff);
 
 	_Initialized = true;
 	return rc;
@@ -296,7 +297,7 @@ int32_t px4muorb::KraitRpcWrapper::SendData(const char *topic,
 		int32_t length_in_bytes, const uint8_t *data)
 {
 	return (_Initialized ?
-			px4muorb_send_topic_data(topic, data, length_in_bytes) : -1);
+		px4muorb_send_topic_data(topic, data, length_in_bytes) : -1);
 }
 
 int32_t px4muorb::KraitRpcWrapper::ReceiveData(int32_t *msg_type, char **topic,
@@ -306,8 +307,8 @@ int32_t px4muorb::KraitRpcWrapper::ReceiveData(int32_t *msg_type, char **topic,
 
 	if (_Initialized) {
 		rc = px4muorb_receive_msg(msg_type, _TopicNameBuffer,
-				_MAX_TOPIC_NAME_BUFFER, _DataBuffer, _MAX_DATA_BUFFER_SIZE,
-				length_in_bytes);
+					  _MAX_TOPIC_NAME_BUFFER, _DataBuffer, _MAX_DATA_BUFFER_SIZE,
+					  length_in_bytes);
 
 		if (rc == 0) {
 			*topic = _TopicNameBuffer;
@@ -332,7 +333,7 @@ int32_t px4muorb::KraitRpcWrapper::ReceiveBulkData(uint8_t **bulk_data,
 	if (_Initialized) {
 		//rc = px4muorb_receive_msg( msg_type, _TopicNameBuffer, _MAX_TOPIC_NAME_BUFFER, _DataBuffer, _MAX_DATA_BUFFER_SIZE, length_in_bytes  );
 		rc = px4muorb_receive_bulk_data(_BulkTransferBuffer,
-				_MAX_BULK_TRANSFER_BUFFER_SIZE, length_in_bytes, topic_count);
+						_MAX_BULK_TRANSFER_BUFFER_SIZE, length_in_bytes, topic_count);
 
 		if (rc == 0) {
 			*bulk_data = _BulkTransferBuffer;
diff --git a/src/platforms/posix/px4_layer/shmem_posix.c b/src/platforms/posix/px4_layer/shmem_posix.c
index aa0ef2b959..66cf5ba9bf 100644
--- a/src/platforms/posix/px4_layer/shmem_posix.c
+++ b/src/platforms/posix/px4_layer/shmem_posix.c
@@ -60,8 +60,7 @@ void update_index_from_shmem(void);
 uint64_t update_from_shmem_prev_time = 0, update_from_shmem_current_time = 0;
 extern unsigned char *adsp_changed_index;
 
-struct param_wbuf_s
-{
+struct param_wbuf_s {
 	param_t param;
 	union param_value_u val;
 	bool unsaved;
@@ -70,9 +69,10 @@ struct param_wbuf_s
 /*update value and param's change bit in shared memory*/
 void update_to_shmem(param_t param, union param_value_u value)
 {
-	if (px4muorb_param_update_to_shmem(param, (unsigned char*) &value,
-			sizeof(value)))
+	if (px4muorb_param_update_to_shmem(param, (unsigned char *) &value,
+					   sizeof(value))) {
 		PX4_ERR("krait update param %u failed", param);
+	}
 }
 
 void update_index_from_shmem(void)
@@ -83,14 +83,15 @@ void update_index_from_shmem(void)
 	}
 
 	px4muorb_param_update_index_from_shmem(adsp_changed_index,
-			PARAM_BUFFER_SIZE);
+					       PARAM_BUFFER_SIZE);
 }
 
 static void update_value_from_shmem(param_t param, union param_value_u *value)
 {
-	if (px4muorb_param_update_value_from_shmem(param, (unsigned char*) value,
-			sizeof(union param_value_u)))
+	if (px4muorb_param_update_value_from_shmem(param, (unsigned char *) value,
+			sizeof(union param_value_u))) {
 		PX4_ERR("%s get param failed", __FUNCTION__);
+	}
 }
 
 int update_from_shmem(param_t param, union param_value_u *value)
@@ -106,7 +107,7 @@ int update_from_shmem(param_t param, union param_value_u *value)
 	update_from_shmem_current_time = hrt_absolute_time();
 
 	if ((update_from_shmem_current_time - update_from_shmem_prev_time)
-			> 1000000) { //update every 1 second
+	    > 1000000) { //update every 1 second
 		update_from_shmem_prev_time = update_from_shmem_current_time;
 		update_index_from_shmem();
 	}
@@ -123,7 +124,7 @@ int update_from_shmem(param_t param, union param_value_u *value)
 	//else {PX4_INFO("no change to param %s", param_name(param));}
 
 	PX4_DEBUG("%s %d bit on adsp index[%d]",
-			(retval) ? "cleared" : "unchanged", bit_changed, byte_changed);
+		  (retval) ? "cleared" : "unchanged", bit_changed, byte_changed);
 
 	return retval;
 }
diff --git a/src/platforms/qurt/px4_layer/shmem_qurt.c b/src/platforms/qurt/px4_layer/shmem_qurt.c
index b4b24eab39..780a4cc7cf 100644
--- a/src/platforms/qurt/px4_layer/shmem_qurt.c
+++ b/src/platforms/qurt/px4_layer/shmem_qurt.c
@@ -75,8 +75,7 @@ static unsigned log2_for_int(unsigned v)
 	return r;
 }
 
-struct param_wbuf_s
-{
+struct param_wbuf_s {
 	param_t param;
 	union param_value_u val;
 	bool unsaved;
@@ -86,17 +85,18 @@ extern struct param_wbuf_s *param_find_changed(param_t param);
 static void *map_memory(off_t target)
 {
 
-	return (void *) (target + LOCK_SIZE);
+	return (void *)(target + LOCK_SIZE);
 
 }
 
 int get_shmem_lock(const char *caller_file_name, int caller_line_number)
 {
-	unsigned char *lock = (unsigned char *) (MAP_ADDRESS + LOCK_OFFSET);
+	unsigned char *lock = (unsigned char *)(MAP_ADDRESS + LOCK_OFFSET);
 	unsigned int i = 0;
 
 #ifdef PARAM_LOCK_DEBUG
-	PX4_INFO("lock value %d before get from %s, line: %d\n", *(unsigned int*)0xfbfc000, strrchr(caller_file_name, '/'), caller_line_number);
+	PX4_INFO("lock value %d before get from %s, line: %d\n", *(unsigned int *)0xfbfc000, strrchr(caller_file_name, '/'),
+		 caller_line_number);
 #endif
 
 	while (!atomic_compare_and_set(lock, 1, 0)) {
@@ -110,12 +110,12 @@ int get_shmem_lock(const char *caller_file_name, int caller_line_number)
 
 	if (i > 100) {
 		PX4_INFO("Could not get lock, file name: %s, line number: %d.\n",
-				strrchr(caller_file_name, '/'), caller_line_number);
+			 strrchr(caller_file_name, '/'), caller_line_number);
 		return -1;
 
 	} else {
 		PX4_DEBUG("Lock acquired, file name: %s, line number: %d\n",
-				caller_file_name, caller_line_number);
+			  caller_file_name, caller_line_number);
 	}
 
 	return 0; //got the lock
@@ -124,13 +124,13 @@ int get_shmem_lock(const char *caller_file_name, int caller_line_number)
 
 void release_shmem_lock(const char *caller_file_name, int caller_line_number)
 {
-	unsigned char *lock = (unsigned char *) (MAP_ADDRESS + LOCK_OFFSET);
+	unsigned char *lock = (unsigned char *)(MAP_ADDRESS + LOCK_OFFSET);
 
 	*lock = 1;
 
 #ifdef PARAM_LOCK_DEBUG
 	PX4_INFO("release lock, file name: %s, line number: %d.\n",
-			strrchr(caller_file_name, '/'), caller_line_number);
+		 strrchr(caller_file_name, '/'), caller_line_number);
 #endif
 
 	return;
@@ -141,14 +141,15 @@ void init_shared_memory(void)
 	//PX4_INFO("Value at lock address is %d\n", *(unsigned int*)0xfbfc000);
 	int i;
 
-	if (shmem_info_p)
+	if (shmem_info_p) {
 		return;
+	}
 
 	virt_addr = map_memory(MAP_ADDRESS);
 	shmem_info_p = (struct shmem_info *) virt_addr;
 
 	//init lock as 1
-	unsigned char *lock = (unsigned char *) (MAP_ADDRESS + LOCK_OFFSET);
+	unsigned char *lock = (unsigned char *)(MAP_ADDRESS + LOCK_OFFSET);
 	*lock = 1;
 
 	for (i = 0; i < MAX_SHMEM_PARAMS / 8 + 1; i++) {
@@ -230,12 +231,12 @@ void update_to_shmem(param_t param, union param_value_u value)
 
 	if (param_type(param) == PARAM_TYPE_INT32) {
 		PX4_INFO("Set value %d for param %s to shmem, set adsp index %d:%d\n", value.i, param_name(param), byte_changed,
-				bit_changed);
+			 bit_changed);
 	}
 
 	else if (param_type(param) == PARAM_TYPE_FLOAT) {
 		PX4_INFO("Set value %f for param %s to shmem, set adsp index %d:%d\n", value.f, param_name(param), byte_changed,
-				bit_changed);
+			 bit_changed);
 	}
 
 #endif
@@ -261,17 +262,19 @@ void update_index_from_shmem(void)
 			// If a param has changed, we need to find out which one.
 			// From the byte and bit that is different, we can resolve the param number.
 			unsigned bit = log2_for_int(
-					krait_changed_index[i]
-							^ shmem_info_p->krait_changed_index[i]);
+					       krait_changed_index[i]
+					       ^ shmem_info_p->krait_changed_index[i]);
 			param_t param_to_get = i * 8 + bit;
 
 			// Update our krait_changed_index as well.
 			krait_changed_index[i] = shmem_info_p->krait_changed_index[i];
 			params[i] = param_to_get;
+
 		} else {
 			params[i] = 0xFFFF;
 		}
 	}
+
 	release_shmem_lock(__FILE__, __LINE__);
 
 	// FIXME: this is a hack but it gets the param so that it gets added
@@ -306,12 +309,12 @@ static void update_value_from_shmem(param_t param, union param_value_u *value)
 
 	if (param_type(param) == PARAM_TYPE_INT32) {
 		PX4_INFO("Got value %d for param %s from shmem, cleared krait index %d:%d\n", value->i, param_name(param), byte_changed,
-				bit_changed);
+			 bit_changed);
 	}
 
 	else if (param_type(param) == PARAM_TYPE_FLOAT) {
 		PX4_INFO("Got value %f for param %s from shmem, cleared krait index %d:%d\n", value->f, param_name(param), byte_changed,
-				bit_changed);
+			 bit_changed);
 	}
 
 #endif
@@ -329,7 +332,7 @@ int update_from_shmem(param_t param, union param_value_u *value)
 	update_from_shmem_current_time = hrt_absolute_time();
 
 	if ((update_from_shmem_current_time - update_from_shmem_prev_time)
-			> 1000000) { //update every 1 second
+	    > 1000000) { //update every 1 second
 		update_from_shmem_prev_time = update_from_shmem_current_time;
 		update_index_from_shmem();
 	}
@@ -346,7 +349,7 @@ int update_from_shmem(param_t param, union param_value_u *value)
 	//else {PX4_INFO("no change to param %s\n", param_name(param));}
 
 	PX4_DEBUG("%s %d bit on krait changed index[%d]\n",
-			(retval) ? "cleared" : "unchanged", bit_changed, byte_changed);
+		  (retval) ? "cleared" : "unchanged", bit_changed, byte_changed);
 
 	return retval;
 }

From 4d0bb9f1e2879a30b3fc01405386179c62e2bf1a Mon Sep 17 00:00:00 2001
From: Daniel Agar <daniel@agar.ca>
Date: Tue, 21 Jun 2016 10:15:42 -0400
Subject: [PATCH 0752/1230] update eclipse project file templates

---
 template_.cproject => eclipse.cproject | 39 +++++++++++++-------------
 template_.project => eclipse.project   |  0
 2 files changed, 20 insertions(+), 19 deletions(-)
 rename template_.cproject => eclipse.cproject (98%)
 rename template_.project => eclipse.project (100%)

diff --git a/template_.cproject b/eclipse.cproject
similarity index 98%
rename from template_.cproject
rename to eclipse.cproject
index d6fb4019f1..c7bec6ea52 100644
--- a/template_.cproject
+++ b/eclipse.cproject
@@ -5,12 +5,12 @@
 			<storageModule buildSystemId="org.eclipse.cdt.managedbuilder.core.configurationDataProvider" id="cdt.managedbuild.toolchain.gnu.cross.base.432866957" moduleId="org.eclipse.cdt.core.settings" name="Default">
 				<externalSettings/>
 				<extensions>
+					<extension id="org.eclipse.cdt.core.ELF" point="org.eclipse.cdt.core.BinaryParser"/>
 					<extension id="org.eclipse.cdt.core.GmakeErrorParser" point="org.eclipse.cdt.core.ErrorParser"/>
 					<extension id="org.eclipse.cdt.core.CWDLocator" point="org.eclipse.cdt.core.ErrorParser"/>
 					<extension id="org.eclipse.cdt.core.GCCErrorParser" point="org.eclipse.cdt.core.ErrorParser"/>
 					<extension id="org.eclipse.cdt.core.GASErrorParser" point="org.eclipse.cdt.core.ErrorParser"/>
 					<extension id="org.eclipse.cdt.core.GLDErrorParser" point="org.eclipse.cdt.core.ErrorParser"/>
-					<extension id="org.eclipse.cdt.core.ELF" point="org.eclipse.cdt.core.BinaryParser"/>
 				</extensions>
 			</storageModule>
 			<storageModule moduleId="cdtBuildSystem" version="4.0.0">
@@ -105,23 +105,32 @@
 	</storageModule>
 	<storageModule moduleId="org.eclipse.cdt.make.core.buildtargets">
 		<buildTargets>
+			<target name="px4fmu-v1_default" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments/>
+				<buildTarget>px4fmu-v1_default</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
 			<target name="px4fmu-v2_default" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>make</buildCommand>
+				<buildArguments/>
 				<buildTarget>px4fmu-v2_default</buildTarget>
 				<stopOnError>true</stopOnError>
 				<useDefaultCommand>true</useDefaultCommand>
 				<runAllBuilders>true</runAllBuilders>
 			</target>
-			<target name="px4fmu-v2_default upload" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+			<target name="px4fmu-v4_default" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>make</buildCommand>
-				<buildArguments/>
-				<buildTarget>px4fmu-v2_default upload</buildTarget>
+				<buildTarget>px4fmu-v4_default</buildTarget>
 				<stopOnError>true</stopOnError>
 				<useDefaultCommand>true</useDefaultCommand>
 				<runAllBuilders>true</runAllBuilders>
 			</target>
 			<target name="all" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>make</buildCommand>
+				<buildArguments/>
 				<buildTarget>all</buildTarget>
 				<stopOnError>true</stopOnError>
 				<useDefaultCommand>true</useDefaultCommand>
@@ -129,54 +138,46 @@
 			</target>
 			<target name="posix_sitl_default" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>make</buildCommand>
-				<buildArguments/>
 				<buildTarget>posix_sitl_default</buildTarget>
 				<stopOnError>true</stopOnError>
 				<useDefaultCommand>true</useDefaultCommand>
 				<runAllBuilders>true</runAllBuilders>
 			</target>
-			<target name="px4fmu-v1_default" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+			<target name="check" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>make</buildCommand>
-				<buildTarget>px4fmu-v1_default</buildTarget>
+				<buildTarget>check</buildTarget>
 				<stopOnError>true</stopOnError>
 				<useDefaultCommand>true</useDefaultCommand>
 				<runAllBuilders>true</runAllBuilders>
 			</target>
 			<target name="clean" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>make</buildCommand>
+				<buildArguments/>
 				<buildTarget>clean</buildTarget>
 				<stopOnError>true</stopOnError>
 				<useDefaultCommand>true</useDefaultCommand>
 				<runAllBuilders>true</runAllBuilders>
 			</target>
 			<target name="distclean" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildTarget>distclean</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="px4fmu-v4_default" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>make</buildCommand>
 				<buildArguments/>
-				<buildTarget>px4fmu-v4_default</buildTarget>
+				<buildTarget>distclean</buildTarget>
 				<stopOnError>true</stopOnError>
 				<useDefaultCommand>true</useDefaultCommand>
 				<runAllBuilders>true</runAllBuilders>
 			</target>
 			<target name="tests" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>make</buildCommand>
-				<buildArguments/>
 				<buildTarget>tests</buildTarget>
 				<stopOnError>true</stopOnError>
 				<useDefaultCommand>true</useDefaultCommand>
 				<runAllBuilders>true</runAllBuilders>
 			</target>
-			<target name="check" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+			<target name="submodulesclean" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>make</buildCommand>
 				<buildArguments/>
-				<buildTarget>check</buildTarget>
-				<stopOnError>true</stopOnError>
+				<buildTarget>submodulesclean</buildTarget>
+				<stopOnError>false</stopOnError>
 				<useDefaultCommand>true</useDefaultCommand>
 				<runAllBuilders>true</runAllBuilders>
 			</target>
diff --git a/template_.project b/eclipse.project
similarity index 100%
rename from template_.project
rename to eclipse.project

From b65291579fc71a03469252cb27a3e78809eb54a5 Mon Sep 17 00:00:00 2001
From: Daniel Agar <daniel@agar.ca>
Date: Tue, 21 Jun 2016 11:49:42 -0400
Subject: [PATCH 0753/1230] show bad formatting diff

---
 Tools/check_code_style.sh | 7 +++++++
 1 file changed, 7 insertions(+)

diff --git a/Tools/check_code_style.sh b/Tools/check_code_style.sh
index ab012b1c09..5ffc0ac5d2 100755
--- a/Tools/check_code_style.sh
+++ b/Tools/check_code_style.sh
@@ -9,6 +9,13 @@ then
 	${DIR}/fix_code_style.sh --dry-run $file | grep --quiet Formatted
 	if [[ $? -eq 0 ]]
 	then
+		${DIR}/fix_code_style.sh --quiet < $file > $file.pretty
+
+		echo
+		git diff --no-index --minimal --histogram --color=always  $file $file.pretty
+		echo
+
+		rm -f $file.pretty
 		echo $file 'bad formatting, please run "./Tools/fix_code_style.sh' $file'"'
 		exit 1
 	fi

From 924fb49bf5ee25a4a401218c79d57f46b4bc4750 Mon Sep 17 00:00:00 2001
From: Daniel Agar <daniel@agar.ca>
Date: Tue, 21 Jun 2016 11:50:51 -0400
Subject: [PATCH 0754/1230] fw_pos_control_l1 shorten task name

-limited to 16 chars
---
 src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

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 01f378b970..5350413756 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
@@ -2366,7 +2366,7 @@ FixedwingPositionControl::start()
 	ASSERT(_control_task == -1);
 
 	/* start the task */
-	_control_task = px4_task_spawn_cmd("fw_pos_control_l1",
+	_control_task = px4_task_spawn_cmd("fw_pos_ctrl_l1",
 					   SCHED_DEFAULT,
 					   SCHED_PRIORITY_MAX - 5,
 					   1400,

From 3f3a44fec5e2eb376e76dd6b6411ce3b707f0ab5 Mon Sep 17 00:00:00 2001
From: Daniel Agar <daniel@agar.ca>
Date: Tue, 21 Jun 2016 11:52:23 -0400
Subject: [PATCH 0755/1230] cmake status message if MEMORY_DEBUG enabled

---
 cmake/common/px4_base.cmake | 1 +
 1 file changed, 1 insertion(+)

diff --git a/cmake/common/px4_base.cmake b/cmake/common/px4_base.cmake
index 014fdda23a..70b2a7d854 100644
--- a/cmake/common/px4_base.cmake
+++ b/cmake/common/px4_base.cmake
@@ -659,6 +659,7 @@ function(px4_add_common_flags)
 	endif()
 
 	if ($ENV{MEMORY_DEBUG} MATCHES "1")
+		message(STATUS "address sanitizer enabled")
 		set(max_optimization -Os)
 
 		set(optimization_flags

From 36103d33d23355d4cd1f40758e0ae14776301a54 Mon Sep 17 00:00:00 2001
From: Daniel Agar <daniel@agar.ca>
Date: Tue, 21 Jun 2016 13:26:31 -0400
Subject: [PATCH 0756/1230] travis-ci only build qgc firmware

---
 .travis.yml | 7 ++++++-
 1 file changed, 6 insertions(+), 1 deletion(-)

diff --git a/.travis.yml b/.travis.yml
index 6e8742e8ab..527b0e3ab2 100644
--- a/.travis.yml
+++ b/.travis.yml
@@ -3,6 +3,11 @@
 
 language: cpp
 
+env:
+  global:
+    # build thiemar/vectorcontrol.git and include in px4fmu-v4
+    - VECTORCONTROL=1
+
 matrix:
   fast_finish: true
   include:
@@ -48,7 +53,7 @@ env:
 
 script:
   - if [ "${TRAVIS_OS_NAME}" = "linux" ]; then
-      docker run --rm -v `pwd`:`pwd`:rw -v $HOME/.ccache:$HOME/.ccache:rw -e CCACHE_DIR=$HOME/.ccache -w=`pwd` --user=$UID -it ${DOCKER_REPO} /bin/bash -c "make check VECTORCONTROL=1";
+      docker run --rm -v `pwd`:`pwd`:rw -v $HOME/.ccache:$HOME/.ccache:rw -e CCACHE_DIR=$HOME/.ccache -w=`pwd` --user=$UID -it ${DOCKER_REPO} /bin/bash -c "make qgc_firmware";
     elif [ "${TRAVIS_OS_NAME}" = "osx" ]; then
       make check_posix_sitl_default;
     fi

From 37108870e15a7b25f7aa06b98c6fbcb7f1ccfd0a Mon Sep 17 00:00:00 2001
From: Roman Bapst <bapstr@ethz.ch>
Date: Wed, 22 Jun 2016 15:19:11 +0200
Subject: [PATCH 0757/1230] fw_pos_control_l1: added roll setpoint for logging
 (#4869)

in altitude control mode for fixed wings the roll setpoint was not
logged because the position controller publishes the attitude setpoint
but the desired roll setpoint is calculated in the attitude control
module. Now the position controller calculates the roll setpoint as well
for the sake of logging.

Signed-off-by: tumbili <roman@px4.io>
---
 .../fw_pos_control_l1_main.cpp                | 22 +++++++++++++++++++
 1 file changed, 22 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 5350413756..ec42455951 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
@@ -291,6 +291,8 @@ private:
 		float throttle_idle;
 		float throttle_cruise;
 		float throttle_slew_max;
+		float man_roll_max_rad;
+		float rollsp_offset_rad;
 
 		float throttle_land_max;
 
@@ -342,6 +344,8 @@ private:
 		param_t throttle_idle;
 		param_t throttle_cruise;
 		param_t throttle_slew_max;
+		param_t man_roll_max_deg;
+		param_t rollsp_offset_deg;
 
 		param_t throttle_land_max;
 
@@ -611,6 +615,8 @@ FixedwingPositionControl::FixedwingPositionControl() :
 	_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");
+	_parameter_handles.man_roll_max_deg = param_find("FW_MAN_R_MAX");
+	_parameter_handles.rollsp_offset_deg = param_find("FW_RSP_OFF");
 
 	_parameter_handles.land_slope_angle = param_find("FW_LND_ANG");
 	_parameter_handles.land_H1_virt = param_find("FW_LND_HVIRT");
@@ -693,6 +699,12 @@ FixedwingPositionControl::parameters_update()
 
 	param_get(_parameter_handles.throttle_land_max, &(_parameters.throttle_land_max));
 
+	param_get(_parameter_handles.man_roll_max_deg, &_parameters.man_roll_max_rad);
+	_parameters.man_roll_max_rad = math::radians(_parameters.man_roll_max_rad);
+	param_get(_parameter_handles.rollsp_offset_deg, &_parameters.rollsp_offset_rad);
+	_parameters.rollsp_offset_rad = math::radians(_parameters.rollsp_offset_rad);
+
+
 	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));
@@ -1941,6 +1953,16 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
 					   ground_speed,
 					   tecs_status_s::TECS_MODE_NORMAL);
 
+		// calculate roll setpoint from user input
+		// this is already calculated in fw_att_control_main.cpp but we do it here for the sake of logging
+		if (fabsf(_manual.y) < 0.01f && fabsf(_roll) < 0.2f) {
+			_att_sp.roll_body = _parameters.rollsp_offset_rad;
+
+		} else {
+			_att_sp.roll_body = (_manual.y * _parameters.man_roll_max_rad)
+						+ _parameters.rollsp_offset_rad;
+		}
+
 	} else {
 		_control_mode_current = FW_POSCTRL_MODE_OTHER;
 

From dfa2ec8c6c2b01cfaeab04749e88b1836c46a275 Mon Sep 17 00:00:00 2001
From: Julian Oes <julian@oes.ch>
Date: Wed, 22 Jun 2016 15:19:29 +0200
Subject: [PATCH 0758/1230] DriverFramework: updated submodule (#4867)

This brings some makefile and script fixes, as well as an updated dspal.
---
 src/lib/DriverFramework | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/src/lib/DriverFramework b/src/lib/DriverFramework
index 03115219f2..7bf87592c5 160000
--- a/src/lib/DriverFramework
+++ b/src/lib/DriverFramework
@@ -1 +1 @@
-Subproject commit 03115219f20761682f50f201feba47eb642b6ae3
+Subproject commit 7bf87592c5f9bfe75cd33280b246c15f0ba476e5

From e7f31393bcf1bc7a96cda34cc2f31d38160b7987 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Beat=20K=C3=BCng?= <beat-kueng@gmx.net>
Date: Wed, 22 Jun 2016 15:28:23 +0200
Subject: [PATCH 0759/1230] orb: reduce size of SubscriberData struct (#4771)

- priority field uses only the lower 8 bits, so we can merge with the
  update_reported flag
- orb_set_interval is not used often, so make the necessary data an
  optional pointer and alloc only when needed.

Memory savings:
- pixracer (w. ekf2): 7.3kB
- pixhawk: 5.3kB
---
 src/modules/uORB/uORBDevices_nuttx.cpp | 66 ++++++++++++++++++------
 src/modules/uORB/uORBDevices_nuttx.hpp | 20 ++++++--
 src/modules/uORB/uORBDevices_posix.cpp | 69 +++++++++++++++++++-------
 src/modules/uORB/uORBDevices_posix.hpp | 20 ++++++--
 4 files changed, 133 insertions(+), 42 deletions(-)

diff --git a/src/modules/uORB/uORBDevices_nuttx.cpp b/src/modules/uORB/uORBDevices_nuttx.cpp
index 7ae8e3a1ce..93eeaa8d92 100644
--- a/src/modules/uORB/uORBDevices_nuttx.cpp
+++ b/src/modules/uORB/uORBDevices_nuttx.cpp
@@ -127,7 +127,7 @@ uORB::DeviceNode::open(struct file *filp)
 		sd->generation = _generation;
 
 		/* set priority */
-		sd->priority = _priority;
+		sd->set_priority(_priority);
 
 		filp->f_priv = (void *)sd;
 
@@ -157,7 +157,10 @@ uORB::DeviceNode::close(struct file *filp)
 		SubscriberData *sd = filp_to_sd(filp);
 
 		if (sd != nullptr) {
-			hrt_cancel(&sd->update_call);
+			if (sd->update_interval) {
+				hrt_cancel(&sd->update_interval->update_call);
+			}
+
 			remove_internal_subscriber();
 			delete sd;
 			sd = nullptr;
@@ -209,13 +212,13 @@ uORB::DeviceNode::read(struct file *filp, char *buffer, size_t buflen)
 	}
 
 	/* set priority */
-	sd->priority = _priority;
+	sd->set_priority(_priority);
 
 	/*
 	 * Clear the flag that indicates that an update has been reported, as
 	 * we have just collected it.
 	 */
-	sd->update_reported = false;
+	sd->set_update_reported(false);
 
 	px4_leave_critical_section(flags);
 
@@ -294,16 +297,43 @@ uORB::DeviceNode::ioctl(struct file *filp, int cmd, unsigned long arg)
 		*(bool *)arg = appears_updated(sd);
 		return OK;
 
-	case ORBIOCSETINTERVAL:
-		sd->update_interval = arg;
-		return OK;
+	case ORBIOCSETINTERVAL: {
+			int ret = PX4_OK;
+			lock();
+
+			if (arg == 0) {
+				if (sd->update_interval) {
+					delete(sd->update_interval);
+					sd->update_interval = nullptr;
+				}
+
+			} else {
+				if (sd->update_interval) {
+					sd->update_interval->interval = arg;
+
+				} else {
+					sd->update_interval = new UpdateIntervalData();
+
+					if (sd->update_interval) {
+						memset(&sd->update_interval->update_call, 0, sizeof(hrt_call));
+						sd->update_interval->interval = arg;
+
+					} else {
+						ret = -ENOMEM;
+					}
+				}
+			}
+
+			unlock();
+			return ret;
+		}
 
 	case ORBIOCGADVERTISER:
 		*(uintptr_t *)arg = (uintptr_t)this;
 		return OK;
 
 	case ORBIOCGPRIORITY:
-		*(int *)arg = sd->priority;
+		*(int *)arg = sd->priority();
 		return OK;
 
 	case ORBIOCSETQUEUESIZE:
@@ -312,7 +342,13 @@ uORB::DeviceNode::ioctl(struct file *filp, int cmd, unsigned long arg)
 		return update_queue_size(arg);
 
 	case ORBIOCGETINTERVAL:
-		*(unsigned *)arg = sd->update_interval;
+		if (sd->update_interval) {
+			*(unsigned *)arg = sd->update_interval->interval;
+
+		} else {
+			*(unsigned *)arg = 0;
+		}
+
 		return OK;
 
 	default:
@@ -443,7 +479,7 @@ uORB::DeviceNode::appears_updated(SubscriberData *sd)
 		/*
 		 * Handle non-rate-limited subscribers.
 		 */
-		if (sd->update_interval == 0) {
+		if (sd->update_interval == nullptr) {
 			ret = true;
 			break;
 		}
@@ -455,7 +491,7 @@ uORB::DeviceNode::appears_updated(SubscriberData *sd)
 		 * behaviour where checking / polling continues to report an update
 		 * until the topic is read.
 		 */
-		if (sd->update_reported) {
+		if (sd->update_reported()) {
 			ret = true;
 			break;
 		}
@@ -467,7 +503,7 @@ uORB::DeviceNode::appears_updated(SubscriberData *sd)
 		 * must have collected the update we reported, otherwise
 		 * update_reported would still be true.
 		 */
-		if (!hrt_called(&sd->update_call)) {
+		if (!hrt_called(&sd->update_interval->update_call)) {
 			break;
 		}
 
@@ -476,15 +512,15 @@ uORB::DeviceNode::appears_updated(SubscriberData *sd)
 		 * until the interval has passed once more by restarting the interval
 		 * timer and thereby re-scheduling a poll notification at that time.
 		 */
-		hrt_call_after(&sd->update_call,
-			       sd->update_interval,
+		hrt_call_after(&sd->update_interval->update_call,
+			       sd->update_interval->interval,
 			       &uORB::DeviceNode::update_deferred_trampoline,
 			       (void *)this);
 
 		/*
 		 * Remember that we have told the subscriber that there is data.
 		 */
-		sd->update_reported = true;
+		sd->set_update_reported(true);
 		ret = true;
 
 		break;
diff --git a/src/modules/uORB/uORBDevices_nuttx.hpp b/src/modules/uORB/uORBDevices_nuttx.hpp
index e4eedd6003..2ad16ee3c7 100644
--- a/src/modules/uORB/uORBDevices_nuttx.hpp
+++ b/src/modules/uORB/uORBDevices_nuttx.hpp
@@ -183,12 +183,22 @@ protected:
 	virtual void poll_notify_one(struct pollfd *fds, pollevent_t events);
 
 private:
-	struct SubscriberData {
-		unsigned  generation; /**< last generation the subscriber has seen */
-		unsigned  update_interval; /**< if nonzero minimum interval between updates */
+	struct UpdateIntervalData {
+		unsigned  interval; /**< if nonzero minimum interval between updates */
 		struct hrt_call update_call;  /**< deferred wakeup call if update_period is nonzero */
-		bool    update_reported; /**< true if we have reported the update via poll/check */
-		int   priority; /**< priority of publisher */
+	};
+	struct SubscriberData {
+		~SubscriberData() { if (update_interval) { delete(update_interval); } }
+
+		unsigned  generation; /**< last generation the subscriber has seen */
+		int   flags; /**< lowest 8 bits: priority of publisher, 9. bit: update_reported bit */
+		UpdateIntervalData *update_interval; /**< if null, no update interval */
+
+		int priority() const { return flags & 0xff; }
+		void set_priority(uint8_t prio) { flags = (flags & ~0xff) | prio; }
+
+		bool update_reported() const { return flags & (1 << 8); }
+		void set_update_reported(bool update_reported_flag) { flags = (flags & ~(1 << 8)) | (((int)update_reported_flag) << 8); }
 	};
 
 	const struct orb_metadata *_meta; /**< object metadata information */
diff --git a/src/modules/uORB/uORBDevices_posix.cpp b/src/modules/uORB/uORBDevices_posix.cpp
index ac936e45e9..a7c7d3cfe6 100644
--- a/src/modules/uORB/uORBDevices_posix.cpp
+++ b/src/modules/uORB/uORBDevices_posix.cpp
@@ -136,7 +136,7 @@ uORB::DeviceNode::open(device::file_t *filp)
 		sd->generation = _generation;
 
 		/* set priority */
-		sd->priority = _priority;
+		sd->set_priority(_priority);
 
 		filp->priv = (void *)sd;
 
@@ -169,7 +169,10 @@ uORB::DeviceNode::close(device::file_t *filp)
 		SubscriberData *sd = filp_to_sd(filp);
 
 		if (sd != nullptr) {
-			hrt_cancel(&sd->update_call);
+			if (sd->update_interval) {
+				hrt_cancel(&sd->update_interval->update_call);
+			}
+
 			remove_internal_subscriber();
 			delete sd;
 			sd = nullptr;
@@ -222,13 +225,13 @@ uORB::DeviceNode::read(device::file_t *filp, char *buffer, size_t buflen)
 	}
 
 	/* set priority */
-	sd->priority = _priority;
+	sd->set_priority(_priority);
 
 	/*
 	 * Clear the flag that indicates that an update has been reported, as
 	 * we have just collected it.
 	 */
-	sd->update_reported = false;
+	sd->set_update_reported(false);
 
 	unlock();
 
@@ -306,19 +309,45 @@ uORB::DeviceNode::ioctl(device::file_t *filp, int cmd, unsigned long arg)
 		unlock();
 		return PX4_OK;
 
-	case ORBIOCSETINTERVAL:
-		lock();
-		sd->update_interval = arg;
-		sd->last_update = hrt_absolute_time();
-		unlock();
-		return PX4_OK;
+	case ORBIOCSETINTERVAL: {
+			int ret = PX4_OK;
+			lock();
+
+			if (arg == 0) {
+				if (sd->update_interval) {
+					delete(sd->update_interval);
+					sd->update_interval = nullptr;
+				}
+
+			} else {
+				if (sd->update_interval) {
+					sd->update_interval->interval = arg;
+					sd->update_interval->last_update = hrt_absolute_time();
+
+				} else {
+					sd->update_interval = new UpdateIntervalData();
+
+					if (sd->update_interval) {
+						memset(&sd->update_interval->update_call, 0, sizeof(hrt_call));
+						sd->update_interval->interval = arg;
+						sd->update_interval->last_update = hrt_absolute_time();
+
+					} else {
+						ret = -ENOMEM;
+					}
+				}
+			}
+
+			unlock();
+			return ret;
+		}
 
 	case ORBIOCGADVERTISER:
 		*(uintptr_t *)arg = (uintptr_t)this;
 		return PX4_OK;
 
 	case ORBIOCGPRIORITY:
-		*(int *)arg = sd->priority;
+		*(int *)arg = sd->priority();
 		return PX4_OK;
 
 	case ORBIOCSETQUEUESIZE:
@@ -327,7 +356,13 @@ uORB::DeviceNode::ioctl(device::file_t *filp, int cmd, unsigned long arg)
 		return update_queue_size(arg);
 
 	case ORBIOCGETINTERVAL:
-		*(unsigned *)arg = sd->update_interval;
+		if (sd->update_interval) {
+			*(unsigned *)arg = sd->update_interval->interval;
+
+		} else {
+			*(unsigned *)arg = 0;
+		}
+
 		return OK;
 
 	default:
@@ -466,7 +501,7 @@ uORB::DeviceNode::appears_updated(SubscriberData *sd)
 		/*
 		 * Handle non-rate-limited subscribers.
 		 */
-		if (sd->update_interval == 0) {
+		if (sd->update_interval == nullptr) {
 			ret = true;
 			break;
 		}
@@ -478,22 +513,22 @@ uORB::DeviceNode::appears_updated(SubscriberData *sd)
 		 * behaviour where checking / polling continues to report an update
 		 * until the topic is read.
 		 */
-		if (sd->update_reported) {
+		if (sd->update_reported()) {
 			ret = true;
 			break;
 		}
 
 		// If we have not yet reached the deadline, then assume that we can ignore any
 		// newly received data.
-		if (sd->last_update + sd->update_interval > hrt_absolute_time()) {
+		if (sd->update_interval->last_update + sd->update_interval->interval > hrt_absolute_time()) {
 			break;
 		}
 
 		/*
 		 * Remember that we have told the subscriber that there is data.
 		 */
-		sd->update_reported = true;
-		sd->last_update = hrt_absolute_time();
+		sd->set_update_reported(true);
+		sd->update_interval->last_update = hrt_absolute_time();
 		ret = true;
 
 		break;
diff --git a/src/modules/uORB/uORBDevices_posix.hpp b/src/modules/uORB/uORBDevices_posix.hpp
index 25f241ff58..7fad8b5e7f 100644
--- a/src/modules/uORB/uORBDevices_posix.hpp
+++ b/src/modules/uORB/uORBDevices_posix.hpp
@@ -120,13 +120,23 @@ protected:
 	virtual void    poll_notify_one(px4_pollfd_struct_t *fds, pollevent_t events);
 
 private:
-	struct SubscriberData {
-		unsigned  generation; /**< last generation the subscriber has seen */
-		unsigned  update_interval; /**< if nonzero minimum interval between updates */
+	struct UpdateIntervalData {
+		unsigned  interval; /**< if nonzero minimum interval between updates */
 		uint64_t last_update; /**< time at which the last update was provided, used when update_interval is nonzero */
 		struct hrt_call update_call;  /**< deferred wakeup call if update_period is nonzero */
-		bool    update_reported; /**< true if we have reported the update via poll/check */
-		int   priority; /**< priority of publisher */
+	};
+	struct SubscriberData {
+		~SubscriberData() { if (update_interval) { delete(update_interval); } }
+
+		unsigned  generation; /**< last generation the subscriber has seen */
+		int   flags; /**< lowest 8 bits: priority of publisher, 9. bit: update_reported bit */
+		UpdateIntervalData *update_interval; /**< if null, no update interval */
+
+		int priority() const { return flags & 0xff; }
+		void set_priority(uint8_t prio) { flags = (flags & ~0xff) | prio; }
+
+		bool update_reported() const { return flags & (1 << 8); }
+		void set_update_reported(bool update_reported_flag) { flags = (flags & ~(1 << 8)) | (((int)update_reported_flag) << 8); }
 	};
 
 	const struct orb_metadata *_meta; /**< object metadata information */

From 56ddd29f1a822d3c4dba326671a49287e32de0e1 Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Wed, 22 Jun 2016 16:03:01 +0200
Subject: [PATCH 0760/1230] Commander: Update params on last step of mag cal

---
 src/modules/commander/mag_calibration.cpp | 6 ++++--
 1 file changed, 4 insertions(+), 2 deletions(-)

diff --git a/src/modules/commander/mag_calibration.cpp b/src/modules/commander/mag_calibration.cpp
index dfaf1d9766..8f589fb2c7 100644
--- a/src/modules/commander/mag_calibration.cpp
+++ b/src/modules/commander/mag_calibration.cpp
@@ -661,8 +661,6 @@ calibrate_return mag_calibrate_all(orb_advert_t *mavlink_log_pub)
 
 	if (result == calibrate_return_ok) {
 
-		(void)param_set_no_notification(param_find("CAL_MAG_PRIME"), &(device_id_primary));
-
 		for (unsigned cur_mag=0; cur_mag<max_mags; cur_mag++) {
 			if (device_ids[cur_mag] != 0) {
 				struct mag_calibration_s mscale;
@@ -746,6 +744,10 @@ calibrate_return mag_calibrate_all(orb_advert_t *mavlink_log_pub)
 				}
 			}
 		}
+
+		// Trigger a param set on the last step so the whole
+		// system updates
+		(void)param_set(param_find("CAL_MAG_PRIME"), &(device_id_primary));
 	}
 
 	return result;

From b247dac120a188d706deddc6950e3356c9fb42e8 Mon Sep 17 00:00:00 2001
From: Daniel Agar <daniel@agar.ca>
Date: Wed, 22 Jun 2016 17:42:49 -0400
Subject: [PATCH 0761/1230] travis-ci optimizations (#4870)

* move gcc 4.9 build to circleci

* travis-ci update to xcode 7.3

* travis-ci limit git fetching for OSX

* Makefile split firmware targets for CI

* OSX ccache
---
 .travis.yml | 34 ++++++++++++++++------------------
 Makefile    |  9 ++++++++-
 circle.yml  |  4 ++--
 3 files changed, 26 insertions(+), 21 deletions(-)

diff --git a/.travis.yml b/.travis.yml
index 527b0e3ab2..a1b414b113 100644
--- a/.travis.yml
+++ b/.travis.yml
@@ -3,11 +3,6 @@
 
 language: cpp
 
-env:
-  global:
-    # build thiemar/vectorcontrol.git and include in px4fmu-v4
-    - VECTORCONTROL=1
-
 matrix:
   fast_finish: true
   include:
@@ -16,30 +11,31 @@ matrix:
       env: GCC_VER=4.8 DOCKER_REPO="px4io/px4-dev-base"
       services:
         - docker
-    - os: linux
-      sudo: required
-      env: GCC_VER=4.9 DOCKER_REPO="px4io/px4-dev-nuttx-gcc4.9"
-      services:
-        - docker
     - os: osx
-      osx_image: xcode7
       sudo: true
+      osx_image: xcode7.3
+      env: CCACHE_CPP2=1
 
 cache:
+  ccache: true
+  pip: true
   directories:
-    - $HOME/.ccache
+    - $HOME/.pip/cache/
+    - $HOME/Library/Caches/pip
 
 before_install:
-  - cd ${TRAVIS_BUILD_DIR} && git fetch --unshallow && git fetch --all --tags && git submodule update --quiet --init --recursive
   - if [ "${TRAVIS_OS_NAME}" = "linux" ]; then
-      docker pull ${DOCKER_REPO};
+         git fetch --unshallow && git fetch --all --tags
+      && docker pull ${DOCKER_REPO}
+      ;
     elif [ "${TRAVIS_OS_NAME}" = "osx" ]; then
          brew tap PX4/homebrew-px4
-      && brew update; brew update
-      && brew install cmake ninja
-      && brew install genromfs
+      && brew update
+      && brew update
+      && brew install ccache cmake ninja genromfs
       && sudo easy_install pip
       && sudo pip install empy
+      && export PATH=/usr/local/opt/ccache/libexec:$PATH
       ;
     fi
 
@@ -52,11 +48,13 @@ env:
     - PX4_AWS_BUCKET=px4-travis
 
 script:
+  - ccache -M 1GB; ccache -z
   - if [ "${TRAVIS_OS_NAME}" = "linux" ]; then
-      docker run --rm -v `pwd`:`pwd`:rw -v $HOME/.ccache:$HOME/.ccache:rw -e CCACHE_DIR=$HOME/.ccache -w=`pwd` --user=$UID -it ${DOCKER_REPO} /bin/bash -c "make qgc_firmware";
+      docker run --rm -v `pwd`:`pwd`:rw -v $HOME/.ccache:$HOME/.ccache:rw -e CCACHE_DIR=$HOME/.ccache -w=`pwd` --user=$UID -it ${DOCKER_REPO} /bin/bash -c "make check_qgc_firmware VECTORCONTROL=1";
     elif [ "${TRAVIS_OS_NAME}" = "osx" ]; then
       make check_posix_sitl_default;
     fi
+  - ccache -s
 
 after_success:
   - make package_firmware && mkdir s3deploy-archive && cp Firmware.zip s3deploy-archive/
diff --git a/Makefile b/Makefile
index 05426971a7..f8c34223a8 100644
--- a/Makefile
+++ b/Makefile
@@ -289,7 +289,9 @@ endif
 unittest: posix_sitl_test
 	$(call cmake-build-other,unittest, ../unittests)
 	@(cd build_unittest && ctest -j2 --output-on-failure)
-	
+
+tests: posix_sitl_test unittest
+
 test_onboard_sitl:
 	@HEADLESS=1 make posix_sitl_test gazebo_iris
 
@@ -301,6 +303,11 @@ qgc_firmware: \
 	check_mindpx-v2_default \
 	check_px4fmu-v4_default_and_uavcan
 
+extra_firmware: \
+	check_px4-stm32f4discovery_default \
+	check_px4fmu-v2_test \
+	check_px4fmu-v2_ekf2
+
 package_firmware:
 	@zip --junk-paths Firmware.zip `find . -name \*.px4`
 
diff --git a/circle.yml b/circle.yml
index 7f87ee6381..4eae4127a7 100644
--- a/circle.yml
+++ b/circle.yml
@@ -10,10 +10,10 @@ checkout:
 ## Customize dependencies
 dependencies:
   pre:
-    - docker pull px4io/px4-dev-base
+    - docker pull px4io/px4-dev-nuttx-gcc4.9
     - docker info
 
 test:
   override:
     #- sudo docker run --rm -v `pwd`:`pwd`:rw -v $HOME/.ccache:$HOME/.ccache:rw -e CCACHE_DIR=$HOME/.ccache -w=`pwd` --user=$UID -it px4io/px4-dev-base /bin/bash -c "make"
-    - docker run -v `pwd`:`pwd`:rw -w=`pwd` --user=$UID -it px4io/px4-dev-base /bin/bash -c "make"
+    - docker run -v `pwd`:`pwd`:rw -w=`pwd` --user=$UID -it px4io/px4-dev-nuttx-gcc4.9 /bin/bash -c "make px4fmu-v4_default"

From 3194153b2199f3d2bf9c1e8ccce9a5116434a71e Mon Sep 17 00:00:00 2001
From: Daniel Agar <daniel@agar.ca>
Date: Wed, 22 Jun 2016 21:55:12 -0400
Subject: [PATCH 0762/1230] travis-ci homebrew cleanup (#4874)

---
 .travis.yml | 10 ++++------
 1 file changed, 4 insertions(+), 6 deletions(-)

diff --git a/.travis.yml b/.travis.yml
index a1b414b113..0b15e7538d 100644
--- a/.travis.yml
+++ b/.travis.yml
@@ -29,12 +29,10 @@ before_install:
       && docker pull ${DOCKER_REPO}
       ;
     elif [ "${TRAVIS_OS_NAME}" = "osx" ]; then
-         brew tap PX4/homebrew-px4
-      && brew update
-      && brew update
-      && brew install ccache cmake ninja genromfs
-      && sudo easy_install pip
-      && sudo pip install empy
+         brew update
+      && brew install ccache cmake ninja
+      && sudo -H easy_install pip
+      && sudo -H pip install empy
       && export PATH=/usr/local/opt/ccache/libexec:$PATH
       ;
     fi

From e0a214da20f862e1fbb712a8594f7431bcaf7084 Mon Sep 17 00:00:00 2001
From: Daniel Agar <daniel@agar.ca>
Date: Thu, 23 Jun 2016 01:22:47 -0400
Subject: [PATCH 0763/1230] travis-ci OSX don't use homebrew (#4875)

* the OSX builds were spending the majority of the time just updating homebrew and installing a couple packages
---
 .travis.yml | 24 ++++++++++++++++++------
 1 file changed, 18 insertions(+), 6 deletions(-)

diff --git a/.travis.yml b/.travis.yml
index 0b15e7538d..0493822186 100644
--- a/.travis.yml
+++ b/.travis.yml
@@ -25,15 +25,26 @@ cache:
 
 before_install:
   - if [ "${TRAVIS_OS_NAME}" = "linux" ]; then
-         git fetch --unshallow && git fetch --all --tags
+         git fetch --all --tags --unshallow
       && docker pull ${DOCKER_REPO}
       ;
     elif [ "${TRAVIS_OS_NAME}" = "osx" ]; then
-         brew update
-      && brew install ccache cmake ninja
-      && sudo -H easy_install pip
+         sudo -H easy_install pip
       && sudo -H pip install empy
-      && export PATH=/usr/local/opt/ccache/libexec:$PATH
+      && wget https://s3.amazonaws.com/px4-travis/toolchain/macos/ccache
+      && sudo mv ccache /usr/local/bin
+      && chmod +x /usr/local/bin/ccache
+      && mkdir -p ~/bin
+      && sudo ln -s /usr/local/bin/ccache ~/bin/c++
+      && sudo ln -s /usr/local/bin/ccache ~/bin/cc
+      && sudo ln -s /usr/local/bin/ccache ~/bin/clang
+      && sudo ln -s /usr/local/bin/ccache ~/bin/clang++
+      && sudo ln -s /usr/local/bin/ccache ~/bin/g++
+      && sudo ln -s /usr/local/bin/ccache ~/bin/gcc
+      && export PATH=~/bin:$PATH
+      && wget https://s3.amazonaws.com/px4-travis/toolchain/macos/ninja
+      && sudo mv ninja /usr/local/bin
+      && chmod +x /usr/local/bin/ninja
       ;
     fi
 
@@ -44,11 +55,12 @@ env:
 # AWS SECRET: $PX4_AWS_SECRET
     - secure: "h6oajlW68dWIr+wZhO58Dv6e68dZHrBLVA6lPXZmheFQBW6Xam1HuLGA0LOW6cL9TnrAsOZ8g4goB58eMQnMEijFZKi3mhRwZhd/Xjq/ZGJOWBUrLoQHZUw2dQk5ja5vmUlKEoQnFZjDuMjx8KfX5ZMNy8A3yssWZtJYHD8c+bk="
     - PX4_AWS_BUCKET=px4-travis
+    - GIT_SUBMODULES_ARE_EVIL=1
 
 script:
   - ccache -M 1GB; ccache -z
   - if [ "${TRAVIS_OS_NAME}" = "linux" ]; then
-      docker run --rm -v `pwd`:`pwd`:rw -v $HOME/.ccache:$HOME/.ccache:rw -e CCACHE_DIR=$HOME/.ccache -w=`pwd` --user=$UID -it ${DOCKER_REPO} /bin/bash -c "make check_qgc_firmware VECTORCONTROL=1";
+      docker run --rm -v `pwd`:`pwd`:rw -v $HOME/.ccache:$HOME/.ccache:rw -e CCACHE_DIR=$HOME/.ccache -e GIT_SUBMODULES_ARE_EVIL=1 -w=`pwd` --user=$UID -it ${DOCKER_REPO} /bin/bash -c "make check_qgc_firmware VECTORCONTROL=1";
     elif [ "${TRAVIS_OS_NAME}" = "osx" ]; then
       make check_posix_sitl_default;
     fi

From 099becb3536f1b480bd5ee4b6ee37f36694857e1 Mon Sep 17 00:00:00 2001
From: tumbili <roman@px4.io>
Date: Wed, 8 Jun 2016 11:26:54 +0200
Subject: [PATCH 0764/1230] added parameter for airspeed mode selection this
 will enable small planes flying without an airspeed sensor

---
 .../fw_att_control/fw_att_control_params.c       | 16 ++++++++++++++++
 1 file changed, 16 insertions(+)

diff --git a/src/modules/fw_att_control/fw_att_control_params.c b/src/modules/fw_att_control/fw_att_control_params.c
index 732c26a54d..80fe1c6228 100644
--- a/src/modules/fw_att_control/fw_att_control_params.c
+++ b/src/modules/fw_att_control/fw_att_control_params.c
@@ -502,3 +502,19 @@ PARAM_DEFINE_FLOAT(FW_FLAPS_SCL, 1.0f);
  * @group FW Attitude Control
  */
 PARAM_DEFINE_FLOAT(FW_FLAPERON_SCL, 0.0f);
+
+/**
+ * Airspeed mode
+ *
+ * The param value sets the method used to publish the control state airspeed.
+ * For small wings or VTOL without airspeed sensor this parameter can be used to
+ * enable flying without an airspeed reading
+ *
+ * @min 0
+ * @max 2
+ * @value 0 use measured airspeed
+ * @value 1 use vehicle ground velocity as airspeed
+ * @value 2 declare airspeed invalid
+ * @group FW Attitude Control
+ */
+PARAM_DEFINE_INT32(FW_ARSP_MODE, 0);

From c2da51ccf5a6a922ce1df7dcd0902c0756e6cce7 Mon Sep 17 00:00:00 2001
From: tumbili <roman@px4.io>
Date: Wed, 8 Jun 2016 11:29:29 +0200
Subject: [PATCH 0765/1230] use airspeed mode parameter to decide which method
 used to publish control state airspeed

---
 .../attitude_estimator_q_main.cpp             | 27 +++++++++++----
 src/modules/ekf2/ekf2_main.cpp                | 33 ++++++++++++++-----
 2 files changed, 45 insertions(+), 15 deletions(-)

diff --git a/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp b/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp
index af2ad11dfe..c8232eee43 100644
--- a/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp
+++ b/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp
@@ -141,6 +141,7 @@ private:
 		param_t	bias_max;
 		param_t vibe_thresh;
 		param_t	ext_hdg_mode;
+		param_t airspeed_mode;
 	}		_params_handles;		/**< handles for interesting parameters */
 
 	float		_w_accel = 0.0f;
@@ -154,6 +155,7 @@ private:
 	float		_vibration_warning_threshold = 2.0f;
 	hrt_abstime	_vibration_warning_timestamp = 0;
 	int		_ext_hdg_mode = 0;
+	int 	_airspeed_mode = 0;
 
 	Vector<3>	_gyro;
 	Vector<3>	_accel;
@@ -232,6 +234,7 @@ AttitudeEstimatorQ::AttitudeEstimatorQ() :
 	_params_handles.bias_max	= param_find("ATT_BIAS_MAX");
 	_params_handles.vibe_thresh	= param_find("ATT_VIBE_THRESH");
 	_params_handles.ext_hdg_mode	= param_find("ATT_EXT_HDG_M");
+	_params_handles.airspeed_mode = param_find("FW_ARSP_MODE");
 }
 
 /**
@@ -626,14 +629,25 @@ void AttitudeEstimatorQ::task_main()
 
 			ctrl_state.yaw_rate = _lp_yaw_rate.apply(_rates(2));
 
-			/* Airspeed - take airspeed measurement directly here as no wind is estimated */
-			if (PX4_ISFINITE(_airspeed.indicated_airspeed_m_s) && hrt_absolute_time() - _airspeed.timestamp < 1e6
-			    && _airspeed.timestamp > 0) {
-				ctrl_state.airspeed = _airspeed.indicated_airspeed_m_s;
-				ctrl_state.airspeed_valid = true;
+			ctrl_state.airspeed_valid = false;
 
+			// use estimated velocity for airspeed estimate
+			if (_airspeed_mode == 1) {
+				if (hrt_absolute_time() - _gpos.timestamp < 1e6) {
+					ctrl_state.airspeed = sqrtf(_gpos.vel_n * _gpos.vel_n + _gpos.vel_e * _gpos.vel_e +_gpos.vel_d * _gpos.vel_d);
+				}
+			// do nothing, airspeed has been declared as non-valid above, controllers will handle this assuming always trim airspeed	
+			} else if (_airspeed_mode == 2) {
+
+			// use the measured airspeed
 			} else {
-				ctrl_state.airspeed_valid = false;
+				/* Airspeed - take airspeed measurement directly here as no wind is estimated */
+				if (PX4_ISFINITE(_airspeed.indicated_airspeed_m_s) && hrt_absolute_time() - _airspeed.timestamp < 1e6
+				    && _airspeed.timestamp > 0) {
+					ctrl_state.airspeed = _airspeed.indicated_airspeed_m_s;
+					ctrl_state.airspeed_valid = true;
+
+				}
 			}
 
 			/* the instance count is not used here */
@@ -688,6 +702,7 @@ void AttitudeEstimatorQ::update_parameters(bool force)
 		param_get(_params_handles.bias_max, &_bias_max);
 		param_get(_params_handles.vibe_thresh, &_vibration_warning_threshold);
 		param_get(_params_handles.ext_hdg_mode, &_ext_hdg_mode);
+		param_get(_params_handles.airspeed_mode, &_airspeed_mode);
 	}
 }
 
diff --git a/src/modules/ekf2/ekf2_main.cpp b/src/modules/ekf2/ekf2_main.cpp
index 864505c7fe..ca94eaa7ce 100644
--- a/src/modules/ekf2/ekf2_main.cpp
+++ b/src/modules/ekf2/ekf2_main.cpp
@@ -271,6 +271,9 @@ private:
 	control::BlockParamFloat _acc_bias_init;	// 1-sigma accelerometer bias uncertainty at switch-on (m/s**2)
 	control::BlockParamFloat _ang_err_init;		// 1-sigma uncertainty in tilt angle after gravity vector alignment (rad)
 
+	// airspeed mode parameter
+	control::BlockParamInt _airspeed_mode;
+
 	int update_subscriptions();
 
 };
@@ -366,7 +369,8 @@ Ekf2::Ekf2():
 	_tau_pos(this, "EKF2_TAU_POS", false, &_params->pos_Tau),
 	_gyr_bias_init(this, "EKF2_GBIAS_INIT", false, &_params->switch_on_gyro_bias),
 	_acc_bias_init(this, "EKF2_ABIAS_INIT", false, &_params->switch_on_accel_bias),
-	_ang_err_init(this, "EKF2_ANGERR_INIT", false, &_params->initial_tilt_err)
+	_ang_err_init(this, "EKF2_ANGERR_INIT", false, &_params->initial_tilt_err),
+	_airspeed_mode(this, "FW_ARSP_MODE", false)
 {
 
 }
@@ -655,14 +659,27 @@ void Ekf2::task_main()
 						1) * acceleration(1));
 			ctrl_state.horz_acc_mag = _acc_hor_filt;
 
-			// Airspeed - take airspeed measurement directly here as no wind is estimated
-			if (PX4_ISFINITE(airspeed.indicated_airspeed_m_s) && hrt_absolute_time() - airspeed.timestamp < 1e6
-			    && airspeed.timestamp > 0) {
-				ctrl_state.airspeed = airspeed.indicated_airspeed_m_s;
-				ctrl_state.airspeed_valid = true;
+			float vel[3] = {};
+			_ekf.get_velocity(vel);
 
+			ctrl_state.airspeed_valid = false;
+
+			// use estimated velocity for airspeed estimate
+			if (_airspeed_mode.get() == 1) {
+				if (_ekf.local_position_is_valid()) {
+					ctrl_state.airspeed = sqrtf(vel[0] * vel[0] + vel[1] * vel[1] +vel[2] * vel[2]);
+				}
+			// do nothing, airspeed has been declared as non-valid above, controllers will handle this assuming always trim airspeed	
+			} else if (_airspeed_mode.get() == 2) {
+
+			// use the measured airspeed
 			} else {
-				ctrl_state.airspeed_valid = false;
+				/* Airspeed - take airspeed measurement directly here as no wind is estimated */
+				if (PX4_ISFINITE(airspeed.indicated_airspeed_m_s) && hrt_absolute_time() - airspeed.timestamp < 1e6
+				    && airspeed.timestamp > 0) {
+					ctrl_state.airspeed = airspeed.indicated_airspeed_m_s;
+					ctrl_state.airspeed_valid = true;
+				}
 			}
 
 			// publish control state data
@@ -702,7 +719,6 @@ void Ekf2::task_main()
 			// generate vehicle local position data
 			struct vehicle_local_position_s lpos = {};
 			float pos[3] = {};
-			float vel[3] = {};
 
 			lpos.timestamp = hrt_absolute_time();
 
@@ -713,7 +729,6 @@ void Ekf2::task_main()
 			lpos.z = pos[2];
 
 			// Velocity of body origin in local NED frame (m/s)
-			_ekf.get_velocity(vel);
 			lpos.vx = vel[0];
 			lpos.vy = vel[1];
 			lpos.vz = vel[2];

From 1bce38bd9b18ba23f81ac557f06b206dc631c864 Mon Sep 17 00:00:00 2001
From: tumbili <roman@px4.io>
Date: Wed, 8 Jun 2016 12:40:39 +0200
Subject: [PATCH 0766/1230] code style formatting

---
 .../attitude_estimator_q/attitude_estimator_q_main.cpp    | 8 +++++---
 src/modules/ekf2/ekf2_main.cpp                            | 8 +++++---
 2 files changed, 10 insertions(+), 6 deletions(-)

diff --git a/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp b/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp
index c8232eee43..3492eaef0c 100644
--- a/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp
+++ b/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp
@@ -634,12 +634,14 @@ void AttitudeEstimatorQ::task_main()
 			// use estimated velocity for airspeed estimate
 			if (_airspeed_mode == 1) {
 				if (hrt_absolute_time() - _gpos.timestamp < 1e6) {
-					ctrl_state.airspeed = sqrtf(_gpos.vel_n * _gpos.vel_n + _gpos.vel_e * _gpos.vel_e +_gpos.vel_d * _gpos.vel_d);
+					ctrl_state.airspeed = sqrtf(_gpos.vel_n * _gpos.vel_n + _gpos.vel_e * _gpos.vel_e + _gpos.vel_d * _gpos.vel_d);
 				}
-			// do nothing, airspeed has been declared as non-valid above, controllers will handle this assuming always trim airspeed	
+
+				// do nothing, airspeed has been declared as non-valid above, controllers will handle this assuming always trim airspeed
+
 			} else if (_airspeed_mode == 2) {
 
-			// use the measured airspeed
+				// use the measured airspeed
 			} else {
 				/* Airspeed - take airspeed measurement directly here as no wind is estimated */
 				if (PX4_ISFINITE(_airspeed.indicated_airspeed_m_s) && hrt_absolute_time() - _airspeed.timestamp < 1e6
diff --git a/src/modules/ekf2/ekf2_main.cpp b/src/modules/ekf2/ekf2_main.cpp
index ca94eaa7ce..51f15cfc46 100644
--- a/src/modules/ekf2/ekf2_main.cpp
+++ b/src/modules/ekf2/ekf2_main.cpp
@@ -667,12 +667,14 @@ void Ekf2::task_main()
 			// use estimated velocity for airspeed estimate
 			if (_airspeed_mode.get() == 1) {
 				if (_ekf.local_position_is_valid()) {
-					ctrl_state.airspeed = sqrtf(vel[0] * vel[0] + vel[1] * vel[1] +vel[2] * vel[2]);
+					ctrl_state.airspeed = sqrtf(vel[0] * vel[0] + vel[1] * vel[1] + vel[2] * vel[2]);
 				}
-			// do nothing, airspeed has been declared as non-valid above, controllers will handle this assuming always trim airspeed	
+
+				// do nothing, airspeed has been declared as non-valid above, controllers will handle this assuming always trim airspeed
+
 			} else if (_airspeed_mode.get() == 2) {
 
-			// use the measured airspeed
+				// use the measured airspeed
 			} else {
 				/* Airspeed - take airspeed measurement directly here as no wind is estimated */
 				if (PX4_ISFINITE(airspeed.indicated_airspeed_m_s) && hrt_absolute_time() - airspeed.timestamp < 1e6

From b54982965b15f1613f524c5b5c0cba30012c7da2 Mon Sep 17 00:00:00 2001
From: sander <sander@droneslab.com>
Date: Wed, 8 Jun 2016 23:06:51 +0200
Subject: [PATCH 0767/1230] Allow VTOL transition based on time

---
 src/modules/vtol_att_control/standard.cpp | 15 +++++++++++----
 src/modules/vtol_att_control/standard.h   |  6 ++++--
 2 files changed, 15 insertions(+), 6 deletions(-)

diff --git a/src/modules/vtol_att_control/standard.cpp b/src/modules/vtol_att_control/standard.cpp
index 9109a82f90..ce2d978436 100644
--- a/src/modules/vtol_att_control/standard.cpp
+++ b/src/modules/vtol_att_control/standard.cpp
@@ -67,7 +67,8 @@ Standard::Standard(VtolAttitudeControl *attc) :
 	_params_handles_standard.front_trans_timeout = param_find("VT_TRANS_TIMEOUT");
 	_params_handles_standard.front_trans_time_min = param_find("VT_TRANS_MIN_TM");
 	_params_handles_standard.down_pitch_max = param_find("VT_DWN_PITCH_MAX");
-	_params_handles_standard.forward_thurst_scale = param_find("VT_FWD_THRUST_SC");
+	_params_handles_standard.forward_thrust_scale = param_find("VT_FWD_THRUST_SC");
+	_params_handles_standard.airspeed_mode = param_find("FW_ARSP_MODE");
 }
 
 Standard::~Standard()
@@ -78,6 +79,7 @@ int
 Standard::parameters_update()
 {
 	float v;
+	int i;
 
 	/* duration of a forwards transition to fw mode */
 	param_get(_params_handles_standard.front_trans_dur, &v);
@@ -112,7 +114,12 @@ Standard::parameters_update()
 	_params_standard.down_pitch_max = math::radians(v);
 
 	/* scale for fixed wing thrust used for forward acceleration in multirotor mode */
-	param_get(_params_handles_standard.forward_thurst_scale, &_params_standard.forward_thurst_scale);
+	param_get(_params_handles_standard.forward_thrust_scale, &_params_standard.forward_thrust_scale);
+
+	/* airspeed mode */
+	param_get(_params_handles_standard.airspeed_mode, &i);
+	_params_standard.airspeed_mode = math::constrain(i, 0, 2);
+
 
 
 	return OK;
@@ -180,7 +187,7 @@ void Standard::update_vtol_state()
 
 		} else if (_vtol_schedule.flight_mode == TRANSITION_TO_FW) {
 			// continue the transition to fw mode while monitoring airspeed for a final switch to fw mode
-			if ((_airspeed->indicated_airspeed_m_s >= _params_standard.airspeed_trans &&
+			if (((_params_standard.airspeed_mode == 2 || _airspeed->indicated_airspeed_m_s >= _params_standard.airspeed_trans) &&
 			     (float)hrt_elapsed_time(&_vtol_schedule.transition_start)
 			     > (_params_standard.front_trans_time_min * 1000000.0f)) ||
 			    can_transition_on_ground()) {
@@ -294,7 +301,7 @@ void Standard::update_mc_state()
 	VtolType::update_mc_state();
 
 	// if the thrust scale param is zero then the pusher-for-pitch strategy is disabled and we can return
-	if (_params_standard.forward_thurst_scale < FLT_EPSILON) {
+	if (_params_standard.forward_thrust_scale < FLT_EPSILON) {
 		return;
 	}
 
diff --git a/src/modules/vtol_att_control/standard.h b/src/modules/vtol_att_control/standard.h
index b4ab84d59b..33fbd5d0f9 100644
--- a/src/modules/vtol_att_control/standard.h
+++ b/src/modules/vtol_att_control/standard.h
@@ -75,7 +75,8 @@ private:
 		float front_trans_timeout;
 		float front_trans_time_min;
 		float down_pitch_max;
-		float forward_thurst_scale;
+		float forward_thrust_scale;
+		int airspeed_mode;
 	} _params_standard;
 
 	struct {
@@ -87,7 +88,8 @@ private:
 		param_t front_trans_timeout;
 		param_t front_trans_time_min;
 		param_t down_pitch_max;
-		param_t forward_thurst_scale;
+		param_t forward_thrust_scale;
+		param_t airspeed_mode;
 	} _params_handles_standard;
 
 	enum vtol_mode {

From cea2350d2e66332a7048fbb253599269dc1d0f03 Mon Sep 17 00:00:00 2001
From: sander <sander@droneslab.com>
Date: Tue, 21 Jun 2016 21:57:30 +0200
Subject: [PATCH 0768/1230] Time based front transition blending

---
 src/modules/vtol_att_control/standard.cpp | 12 ++++++++++++
 1 file changed, 12 insertions(+)

diff --git a/src/modules/vtol_att_control/standard.cpp b/src/modules/vtol_att_control/standard.cpp
index ce2d978436..c49bc61d06 100644
--- a/src/modules/vtol_att_control/standard.cpp
+++ b/src/modules/vtol_att_control/standard.cpp
@@ -249,6 +249,18 @@ void Standard::update_transition_state()
 			_mc_yaw_weight = weight;
 			_mc_throttle_weight = weight;
 
+		// time based blending when no airspeed sensor is set
+		} else if (_params_standard.airspeed_mode == 2 &&
+				  (float)hrt_elapsed_time(&_vtol_schedule.transition_start) < (_params_standard.front_trans_time_min * 1000000.0f)
+				  ) {
+			float weight = 1.0f - (float)(hrt_elapsed_time(&_vtol_schedule.transition_start) / (_params_standard.front_trans_time_min * 1000000.0f));
+			printf("weight: %f \n", (double)weight);
+			_mc_roll_weight = weight;
+			_mc_pitch_weight = weight;
+			_mc_yaw_weight = weight;
+			_mc_throttle_weight = weight;
+
+
 		} else {
 			// at low speeds give full weight to mc
 			_mc_roll_weight = 1.0f;

From 22db94e352949110036d95d8d5c7603c7d67ffdb Mon Sep 17 00:00:00 2001
From: Roman <bapstr@ethz.ch>
Date: Tue, 21 Jun 2016 22:11:31 +0200
Subject: [PATCH 0769/1230] removed debug printf

Signed-off-by: Roman <bapstr@ethz.ch>
---
 src/modules/vtol_att_control/standard.cpp | 1 -
 1 file changed, 1 deletion(-)

diff --git a/src/modules/vtol_att_control/standard.cpp b/src/modules/vtol_att_control/standard.cpp
index c49bc61d06..b84605a65c 100644
--- a/src/modules/vtol_att_control/standard.cpp
+++ b/src/modules/vtol_att_control/standard.cpp
@@ -254,7 +254,6 @@ void Standard::update_transition_state()
 				  (float)hrt_elapsed_time(&_vtol_schedule.transition_start) < (_params_standard.front_trans_time_min * 1000000.0f)
 				  ) {
 			float weight = 1.0f - (float)(hrt_elapsed_time(&_vtol_schedule.transition_start) / (_params_standard.front_trans_time_min * 1000000.0f));
-			printf("weight: %f \n", (double)weight);
 			_mc_roll_weight = weight;
 			_mc_pitch_weight = weight;
 			_mc_yaw_weight = weight;

From 7f8c183d99372206e4b0258e2328a1be39d76551 Mon Sep 17 00:00:00 2001
From: Roman <bapstr@ethz.ch>
Date: Tue, 21 Jun 2016 22:46:10 +0200
Subject: [PATCH 0770/1230] added airspeed mode enum to control state topic

Signed-off-by: Roman <bapstr@ethz.ch>
---
 msg/control_state.msg                         |  4 +++
 .../attitude_estimator_q_main.cpp             | 28 +++++++++----------
 src/modules/ekf2/ekf2_main.cpp                | 24 ++++++++--------
 src/modules/vtol_att_control/standard.cpp     |  5 ++--
 .../vtol_att_control/vtol_att_control_main.h  |  1 +
 5 files changed, 34 insertions(+), 28 deletions(-)

diff --git a/msg/control_state.msg b/msg/control_state.msg
index af6de428fa..96efea0f81 100644
--- a/msg/control_state.msg
+++ b/msg/control_state.msg
@@ -1,4 +1,8 @@
 # This is similar to the mavlink message CONTROL_SYSTEM_STATE, but for onboard use */
+uint8 AIRSPD_MODE_MEAS = 0	# airspeed is measured airspeed from sensor
+uint8 AIRSPD_MODE_EST = 1	# airspeed is estimated by body velocity
+uint8 AIRSPD_MODE_DISABLED = 2	# airspeed is disabled
+
 float32 x_acc			# X acceleration in body frame
 float32 y_acc			# Y acceleration in body frame
 float32 z_acc			# Z acceleration in body frame
diff --git a/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp b/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp
index 3492eaef0c..9ffd4599fe 100644
--- a/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp
+++ b/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp
@@ -631,27 +631,27 @@ void AttitudeEstimatorQ::task_main()
 
 			ctrl_state.airspeed_valid = false;
 
-			// use estimated velocity for airspeed estimate
-			if (_airspeed_mode == 1) {
-				if (hrt_absolute_time() - _gpos.timestamp < 1e6) {
-					ctrl_state.airspeed = sqrtf(_gpos.vel_n * _gpos.vel_n + _gpos.vel_e * _gpos.vel_e + _gpos.vel_d * _gpos.vel_d);
-				}
-
-				// do nothing, airspeed has been declared as non-valid above, controllers will handle this assuming always trim airspeed
-
-			} else if (_airspeed_mode == 2) {
-
-				// use the measured airspeed
-			} else {
-				/* Airspeed - take airspeed measurement directly here as no wind is estimated */
+			if (_airspeed_mode == control_state_s::AIRSPD_MODE_MEAS) {
+				// use measured airspeed
 				if (PX4_ISFINITE(_airspeed.indicated_airspeed_m_s) && hrt_absolute_time() - _airspeed.timestamp < 1e6
 				    && _airspeed.timestamp > 0) {
 					ctrl_state.airspeed = _airspeed.indicated_airspeed_m_s;
 					ctrl_state.airspeed_valid = true;
-
 				}
 			}
 
+			else if (_airspeed_mode == control_state_s::AIRSPD_MODE_EST) {
+				// use estimated body velocity as airspeed estimate
+				if (hrt_absolute_time() - _gpos.timestamp < 1e6) {
+					ctrl_state.airspeed = sqrtf(_gpos.vel_n * _gpos.vel_n + _gpos.vel_e * _gpos.vel_e + _gpos.vel_d * _gpos.vel_d);
+					ctrl_state.airspeed_valid = true;
+				}
+
+			} else if (_airspeed_mode == control_state_s::AIRSPD_MODE_DISABLED) {
+				// do nothing, airspeed has been declared as non-valid above, controllers
+				// will handle this assuming always trim airspeed
+			}
+
 			/* the instance count is not used here */
 			int ctrl_inst;
 			/* publish to control state topic */
diff --git a/src/modules/ekf2/ekf2_main.cpp b/src/modules/ekf2/ekf2_main.cpp
index 51f15cfc46..96dae5ee69 100644
--- a/src/modules/ekf2/ekf2_main.cpp
+++ b/src/modules/ekf2/ekf2_main.cpp
@@ -665,24 +665,24 @@ void Ekf2::task_main()
 			ctrl_state.airspeed_valid = false;
 
 			// use estimated velocity for airspeed estimate
-			if (_airspeed_mode.get() == 1) {
-				if (_ekf.local_position_is_valid()) {
-					ctrl_state.airspeed = sqrtf(vel[0] * vel[0] + vel[1] * vel[1] + vel[2] * vel[2]);
-				}
-
-				// do nothing, airspeed has been declared as non-valid above, controllers will handle this assuming always trim airspeed
-
-			} else if (_airspeed_mode.get() == 2) {
-
-				// use the measured airspeed
-			} else {
-				/* Airspeed - take airspeed measurement directly here as no wind is estimated */
+			if (_airspeed_mode.get() == control_state_s::AIRSPD_MODE_MEAS) {
+				// use measured airspeed
 				if (PX4_ISFINITE(airspeed.indicated_airspeed_m_s) && hrt_absolute_time() - airspeed.timestamp < 1e6
 				    && airspeed.timestamp > 0) {
 					ctrl_state.airspeed = airspeed.indicated_airspeed_m_s;
 					ctrl_state.airspeed_valid = true;
 				}
 			}
+			if (_airspeed_mode.get() == control_state_s::AIRSPD_MODE_EST) {
+				if (_ekf.local_position_is_valid()) {
+					ctrl_state.airspeed = sqrtf(vel[0] * vel[0] + vel[1] * vel[1] + vel[2] * vel[2]);
+					ctrl_state.airspeed_valid = true;
+				}
+
+			} else if (_airspeed_mode.get() == control_state_s::AIRSPD_MODE_DISABLED) {
+				// do nothing, airspeed has been declared as non-valid above, controllers
+				// will handle this assuming always trim airspeed
+			}
 
 			// publish control state data
 			if (_control_state_pub == nullptr) {
diff --git a/src/modules/vtol_att_control/standard.cpp b/src/modules/vtol_att_control/standard.cpp
index b84605a65c..e1343a85a6 100644
--- a/src/modules/vtol_att_control/standard.cpp
+++ b/src/modules/vtol_att_control/standard.cpp
@@ -187,7 +187,8 @@ void Standard::update_vtol_state()
 
 		} else if (_vtol_schedule.flight_mode == TRANSITION_TO_FW) {
 			// continue the transition to fw mode while monitoring airspeed for a final switch to fw mode
-			if (((_params_standard.airspeed_mode == 2 || _airspeed->indicated_airspeed_m_s >= _params_standard.airspeed_trans) &&
+			if (((_params_standard.airspeed_mode == control_state_s::AIRSPD_MODE_DISABLED ||
+				_airspeed->indicated_airspeed_m_s >= _params_standard.airspeed_trans) &&
 			     (float)hrt_elapsed_time(&_vtol_schedule.transition_start)
 			     > (_params_standard.front_trans_time_min * 1000000.0f)) ||
 			    can_transition_on_ground()) {
@@ -250,7 +251,7 @@ void Standard::update_transition_state()
 			_mc_throttle_weight = weight;
 
 		// time based blending when no airspeed sensor is set
-		} else if (_params_standard.airspeed_mode == 2 &&
+		} else if (_params_standard.airspeed_mode == control_state_s::AIRSPD_MODE_DISABLED &&
 				  (float)hrt_elapsed_time(&_vtol_schedule.transition_start) < (_params_standard.front_trans_time_min * 1000000.0f)
 				  ) {
 			float weight = 1.0f - (float)(hrt_elapsed_time(&_vtol_schedule.transition_start) / (_params_standard.front_trans_time_min * 1000000.0f));
diff --git a/src/modules/vtol_att_control/vtol_att_control_main.h b/src/modules/vtol_att_control/vtol_att_control_main.h
index 90a36e652f..3412a53990 100644
--- a/src/modules/vtol_att_control/vtol_att_control_main.h
+++ b/src/modules/vtol_att_control/vtol_att_control_main.h
@@ -84,6 +84,7 @@
 #include <uORB/topics/vehicle_status.h>
 #include <uORB/topics/tecs_status.h>
 #include <uORB/topics/vehicle_land_detected.h>
+#include <uORB/topics/control_state.h>
 #include <systemlib/param/param.h>
 #include <systemlib/err.h>
 #include <systemlib/systemlib.h>

From 37531c018a7f789949c0d7514bbb2850ad5b48a2 Mon Sep 17 00:00:00 2001
From: sander <sander@droneslab.com>
Date: Thu, 9 Jun 2016 17:15:57 +0200
Subject: [PATCH 0771/1230] Implement MAV_CMD_DO_CHANGE_SPEED throttle

---
 msg/position_setpoint.msg                         |  1 +
 .../fw_pos_control_l1/fw_pos_control_l1_main.cpp  | 11 ++++++++++-
 src/modules/navigator/mission_block.cpp           |  8 ++++++++
 src/modules/navigator/navigator.h                 | 15 +++++++++++++++
 src/modules/navigator/navigator_main.cpp          | 12 ++++++++++++
 5 files changed, 46 insertions(+), 1 deletion(-)

diff --git a/msg/position_setpoint.msg b/msg/position_setpoint.msg
index 394885933b..eba8ed9160 100644
--- a/msg/position_setpoint.msg
+++ b/msg/position_setpoint.msg
@@ -37,3 +37,4 @@ bool acceleration_valid		# true if acceleration setpoint is valid/should be used
 bool acceleration_is_force	# interprete acceleration as force
 float32 acceptance_radius   # navigation acceptance_radius if we're doing waypoint navigation
 float32 cruising_speed		# the generally desired cruising speed (not a hard constraint)
+float32 cruising_throttle	# the generally desired cruising throttle (not a hard constraint)
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 ec42455951..eb2c5efe34 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
@@ -1323,6 +1323,15 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
 			mission_airspeed = _pos_sp_triplet.current.cruising_speed;
 		}
 
+		float mission_throttle = _parameters.throttle_cruise;
+
+		if (PX4_ISFINITE(_pos_sp_triplet.current.cruising_throttle) &&
+			_pos_sp_triplet.current.cruising_throttle > 0.1f) {
+			mission_throttle = _pos_sp_triplet.current.cruising_throttle;
+		}
+
+		printf("%f \n", (double)mission_throttle);
+
 		if (pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_IDLE) {
 			_att_sp.thrust = 0.0f;
 			_att_sp.roll_body = 0.0f;
@@ -1336,7 +1345,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
 
 			tecs_update_pitch_throttle(pos_sp_triplet.current.alt, calculate_target_airspeed(mission_airspeed), eas2tas,
 						   math::radians(_parameters.pitch_limit_min), math::radians(_parameters.pitch_limit_max),
-						   _parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise,
+						   _parameters.throttle_min, _parameters.throttle_max, mission_throttle,
 						   false, math::radians(_parameters.pitch_limit_min), _global_pos.alt, ground_speed);
 
 		} else if (pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_LOITER) {
diff --git a/src/modules/navigator/mission_block.cpp b/src/modules/navigator/mission_block.cpp
index 8c2c460c63..ae1820a4a6 100644
--- a/src/modules/navigator/mission_block.cpp
+++ b/src/modules/navigator/mission_block.cpp
@@ -127,7 +127,14 @@ MissionBlock::is_mission_item_reached()
 				_navigator->set_cruising_speed(_mission_item.params[1]);
 			} else {
 				_navigator->set_cruising_speed();
+				/* if no speed target was given try to set throttle */
+				if (_mission_item.params[2] > 0.0f) {
+					_navigator->set_cruising_throttle(_mission_item.params[2] / 100);
+				} else {
+					_navigator->set_cruising_throttle();
+				}
 			}
+
 			return true;
 
 		default:
@@ -367,6 +374,7 @@ MissionBlock::mission_item_to_position_setpoint(const struct mission_item_s *ite
 	sp->acceptance_radius = item->acceptance_radius;
 	sp->disable_mc_yaw_control = false;
 	sp->cruising_speed = _navigator->get_cruising_speed();
+	sp->cruising_throttle = _navigator->get_cruising_throttle();
 
 	switch (item->nav_cmd) {
 	case NAV_CMD_IDLE:
diff --git a/src/modules/navigator/navigator.h b/src/modules/navigator/navigator.h
index 2e6db02ab4..6092f6cc4a 100644
--- a/src/modules/navigator/navigator.h
+++ b/src/modules/navigator/navigator.h
@@ -178,6 +178,19 @@ public:
 	 */
 	void		set_cruising_speed(float speed=-1.0f) { _mission_cruising_speed = speed; }
 
+
+	/**
+	 * Get the target throttle
+	 *
+	 * @return the desired throttle for this mission
+	 */
+	float		get_cruising_throttle();
+
+	/**
+	 * Set the target throttle
+	 */
+	void		set_cruising_throttle(float percent=-1.0f) { _mission_throttle = percent; }
+
 	/**
 	 * Get the acceptance radius given the mission item preset radius
 	 *
@@ -277,8 +290,10 @@ private:
 	
 	control::BlockParamFloat _param_cruising_speed_hover;
 	control::BlockParamFloat _param_cruising_speed_plane;
+	control::BlockParamFloat _param_cruising_throttle_plane;
 
 	float _mission_cruising_speed;
+	float _mission_throttle;
 
 	/**
 	 * Retrieve global position
diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp
index fd7a54ff6e..7d70483561 100644
--- a/src/modules/navigator/navigator_main.cpp
+++ b/src/modules/navigator/navigator_main.cpp
@@ -159,6 +159,7 @@ Navigator::Navigator() :
 	_param_rcloss_act(this, "RCL_ACT"),
 	_param_cruising_speed_hover(this, "MPC_XY_CRUISE", false),
 	_param_cruising_speed_plane(this, "FW_AIRSPD_TRIM", false),
+	_param_cruising_throttle_plane(this, "FW_THR_CRUISE", false),
 	_mission_cruising_speed(-1.0f)
 {
 	/* Create a list of our possible navigation types */
@@ -725,6 +726,17 @@ Navigator::get_cruising_speed()
 	}
 }
 
+float
+Navigator::get_cruising_throttle()
+{
+	/* Return the mission-requested cruise speed, or -1 */
+	if (_mission_throttle > 0.0f) {
+		return _mission_throttle;
+	} else {
+		return _param_cruising_throttle_plane.get();
+	}
+}
+
 float
 Navigator::get_acceptance_radius(float mission_item_radius)
 {

From 9d59ba125d2185aefaa654f8285f96966cbb52e4 Mon Sep 17 00:00:00 2001
From: sander <sander@droneslab.com>
Date: Thu, 9 Jun 2016 17:18:09 +0200
Subject: [PATCH 0772/1230] remove debug info

---
 src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp | 2 --
 1 file changed, 2 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 eb2c5efe34..d21dfefd91 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
@@ -1330,8 +1330,6 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
 			mission_throttle = _pos_sp_triplet.current.cruising_throttle;
 		}
 
-		printf("%f \n", (double)mission_throttle);
-
 		if (pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_IDLE) {
 			_att_sp.thrust = 0.0f;
 			_att_sp.roll_body = 0.0f;

From f2e425b75baff61cca181ad0d8a98b2aa6aedd23 Mon Sep 17 00:00:00 2001
From: sander <sander@droneslab.com>
Date: Fri, 10 Jun 2016 01:50:23 +0200
Subject: [PATCH 0773/1230] commenting

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

diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp
index 7d70483561..01e168335e 100644
--- a/src/modules/navigator/navigator_main.cpp
+++ b/src/modules/navigator/navigator_main.cpp
@@ -729,7 +729,7 @@ Navigator::get_cruising_speed()
 float
 Navigator::get_cruising_throttle()
 {
-	/* Return the mission-requested cruise speed, or -1 */
+	/* Return the mission-requested cruise speed, or default FW_THR_CRUISE value */
 	if (_mission_throttle > 0.0f) {
 		return _mission_throttle;
 	} else {

From 3002852bfaaf96e1305e91c22c9728971f7117de Mon Sep 17 00:00:00 2001
From: sander <sander@droneslab.com>
Date: Fri, 10 Jun 2016 02:17:34 +0200
Subject: [PATCH 0774/1230] Allow throttle updates below 10%

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

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 d21dfefd91..a55a43c813 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
@@ -1326,7 +1326,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
 		float mission_throttle = _parameters.throttle_cruise;
 
 		if (PX4_ISFINITE(_pos_sp_triplet.current.cruising_throttle) &&
-			_pos_sp_triplet.current.cruising_throttle > 0.1f) {
+			_pos_sp_triplet.current.cruising_throttle > 0.01f) {
 			mission_throttle = _pos_sp_triplet.current.cruising_throttle;
 		}
 

From bbf852787e0c2adfb7f88e66857d907c28bd34b1 Mon Sep 17 00:00:00 2001
From: sander <sander@droneslab.com>
Date: Tue, 21 Jun 2016 21:22:36 +0200
Subject: [PATCH 0775/1230] Rename param to throttle

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

diff --git a/src/modules/navigator/navigator.h b/src/modules/navigator/navigator.h
index 6092f6cc4a..02b1548dbd 100644
--- a/src/modules/navigator/navigator.h
+++ b/src/modules/navigator/navigator.h
@@ -189,7 +189,7 @@ public:
 	/**
 	 * Set the target throttle
 	 */
-	void		set_cruising_throttle(float percent=-1.0f) { _mission_throttle = percent; }
+	void		set_cruising_throttle(float throttle=-1.0f) { _mission_throttle = throttle; }
 
 	/**
 	 * Get the acceptance radius given the mission item preset radius

From c1ba7ab62b5d2653e2898606bf82f5fa88371439 Mon Sep 17 00:00:00 2001
From: tumbili <roman@px4.io>
Date: Wed, 22 Jun 2016 11:14:35 +0200
Subject: [PATCH 0776/1230] vtol attitude control: fixed code style

Signed-off-by: tumbili <roman@px4.io>
---
 src/modules/vtol_att_control/standard.cpp | 12 +++++++-----
 1 file changed, 7 insertions(+), 5 deletions(-)

diff --git a/src/modules/vtol_att_control/standard.cpp b/src/modules/vtol_att_control/standard.cpp
index e1343a85a6..e54e0a5bb5 100644
--- a/src/modules/vtol_att_control/standard.cpp
+++ b/src/modules/vtol_att_control/standard.cpp
@@ -188,7 +188,7 @@ void Standard::update_vtol_state()
 		} else if (_vtol_schedule.flight_mode == TRANSITION_TO_FW) {
 			// continue the transition to fw mode while monitoring airspeed for a final switch to fw mode
 			if (((_params_standard.airspeed_mode == control_state_s::AIRSPD_MODE_DISABLED ||
-				_airspeed->indicated_airspeed_m_s >= _params_standard.airspeed_trans) &&
+			      _airspeed->indicated_airspeed_m_s >= _params_standard.airspeed_trans) &&
 			     (float)hrt_elapsed_time(&_vtol_schedule.transition_start)
 			     > (_params_standard.front_trans_time_min * 1000000.0f)) ||
 			    can_transition_on_ground()) {
@@ -250,11 +250,13 @@ void Standard::update_transition_state()
 			_mc_yaw_weight = weight;
 			_mc_throttle_weight = weight;
 
-		// time based blending when no airspeed sensor is set
+			// time based blending when no airspeed sensor is set
+
 		} else if (_params_standard.airspeed_mode == control_state_s::AIRSPD_MODE_DISABLED &&
-				  (float)hrt_elapsed_time(&_vtol_schedule.transition_start) < (_params_standard.front_trans_time_min * 1000000.0f)
-				  ) {
-			float weight = 1.0f - (float)(hrt_elapsed_time(&_vtol_schedule.transition_start) / (_params_standard.front_trans_time_min * 1000000.0f));
+			   (float)hrt_elapsed_time(&_vtol_schedule.transition_start) < (_params_standard.front_trans_time_min * 1000000.0f)
+			  ) {
+			float weight = 1.0f - (float)(hrt_elapsed_time(&_vtol_schedule.transition_start) /
+						      (_params_standard.front_trans_time_min * 1000000.0f));
 			_mc_roll_weight = weight;
 			_mc_pitch_weight = weight;
 			_mc_yaw_weight = weight;

From 6f6ae78cf2ef095a01f5a3b557df8699c146ca14 Mon Sep 17 00:00:00 2001
From: Roman <bapstr@ethz.ch>
Date: Thu, 23 Jun 2016 09:07:46 +0200
Subject: [PATCH 0777/1230] ekf_att_pos_estimator: added logic for airspeed
 modes

Signed-off-by: Roman <bapstr@ethz.ch>
---
 .../AttitudePositionEstimatorEKF.h            |  2 ++
 .../ekf_att_pos_estimator_main.cpp            | 30 ++++++++++++-------
 2 files changed, 22 insertions(+), 10 deletions(-)

diff --git a/src/modules/ekf_att_pos_estimator/AttitudePositionEstimatorEKF.h b/src/modules/ekf_att_pos_estimator/AttitudePositionEstimatorEKF.h
index 04a8b7cb07..f7ea01bbd9 100644
--- a/src/modules/ekf_att_pos_estimator/AttitudePositionEstimatorEKF.h
+++ b/src/modules/ekf_att_pos_estimator/AttitudePositionEstimatorEKF.h
@@ -255,6 +255,7 @@ private:
         float magb_pnoise;
         float eas_noise;
         float pos_stddev_threshold;
+        int32_t airspeed_mode;
     }       _parameters;            /**< local copies of interesting parameters */
 
     struct {
@@ -276,6 +277,7 @@ private:
         param_t magb_pnoise;
         param_t eas_noise;
         param_t pos_stddev_threshold;
+        param_t airspeed_mode;
     }       _parameter_handles;     /**< handles for interesting parameters */
 
     AttPosEKF                   *_ekf;
diff --git a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp
index ae452c01d3..7dc4afb77c 100644
--- a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp
+++ b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp
@@ -257,6 +257,7 @@ AttitudePositionEstimatorEKF::AttitudePositionEstimatorEKF() :
 	_parameter_handles.magb_pnoise = param_find("PE_MAGB_PNOISE");
 	_parameter_handles.eas_noise = param_find("PE_EAS_NOISE");
 	_parameter_handles.pos_stddev_threshold = param_find("PE_POSDEV_INIT");
+	_parameter_handles.airspeed_mode = param_find("FW_AIRSPD_MODE");
 
 	/* indicate consumers that the current position data is not valid */
 	_gps.eph = 10000.0f;
@@ -324,6 +325,7 @@ int AttitudePositionEstimatorEKF::parameters_update()
 	param_get(_parameter_handles.magb_pnoise, &(_parameters.magb_pnoise));
 	param_get(_parameter_handles.eas_noise, &(_parameters.eas_noise));
 	param_get(_parameter_handles.pos_stddev_threshold, &(_parameters.pos_stddev_threshold));
+	param_get(_parameter_handles.airspeed_mode, &_parameters.airspeed_mode);
 
 	if (_ekf) {
 		// _ekf->yawVarScale = 1.0f;
@@ -917,17 +919,25 @@ void AttitudePositionEstimatorEKF::publishControlState()
 	_ctrl_state.q[2] = _ekf->states[2];
 	_ctrl_state.q[3] = _ekf->states[3];
 
-	/* Airspeed (Groundspeed - Windspeed) */
-	//_ctrl_state.airspeed = sqrt(pow(_ekf->states[4] -  _ekf->states[14], 2) + pow(_ekf->states[5] - _ekf->states[15], 2) + pow(_ekf->states[6], 2));
-	// the line above was introduced by the control state PR. The airspeed it gives is totally wrong and leads to horrible flight performance in SITL
-	// and in outdoor tests
-	if (PX4_ISFINITE(_airspeed.indicated_airspeed_m_s) && hrt_absolute_time() - _airspeed.timestamp < 1e6
-	    && _airspeed.timestamp > 0) {
-		_ctrl_state.airspeed = _airspeed.indicated_airspeed_m_s;
-		_ctrl_state.airspeed_valid = true;
+	// use estimated velocity for airspeed estimate
+	if (_parameters.airspeed_mode == control_state_s::AIRSPD_MODE_MEAS) {
+		// use measured airspeed
+		if (PX4_ISFINITE(_airspeed.indicated_airspeed_m_s) && hrt_absolute_time() - _airspeed.timestamp < 1e6
+		    && _airspeed.timestamp > 0) {
+			_ctrl_state.airspeed = _airspeed.indicated_airspeed_m_s;
+			_ctrl_state.airspeed_valid = true;
+		}
 
-	} else {
-		_ctrl_state.airspeed_valid = false;
+	} else if (_parameters.airspeed_mode == control_state_s::AIRSPD_MODE_EST) {
+		if (_local_pos.v_xy_valid && _local_pos.v_z_valid) {
+			_ctrl_state.airspeed = sqrtf(_ekf->states[4] * _ekf->states[4]
+				+ _ekf->states[5] * _ekf->states[5] + _ekf->states[6] * _ekf->states[6]);
+			_ctrl_state.airspeed_valid = true;
+		}
+
+	} else if (_parameters.airspeed_mode == control_state_s::AIRSPD_MODE_DISABLED) {
+		// do nothing, airspeed has been declared as non-valid above, controllers
+		// will handle this assuming always trim airspeed
 	}
 
 	/* Attitude Rates */

From 27e61127a814830139c35d430df921dfb1c088b8 Mon Sep 17 00:00:00 2001
From: Roman <bapstr@ethz.ch>
Date: Thu, 23 Jun 2016 09:08:08 +0200
Subject: [PATCH 0778/1230] ekf2: fix if else logic

Signed-off-by: Roman <bapstr@ethz.ch>
---
 src/modules/ekf2/ekf2_main.cpp | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/src/modules/ekf2/ekf2_main.cpp b/src/modules/ekf2/ekf2_main.cpp
index 96dae5ee69..274d924032 100644
--- a/src/modules/ekf2/ekf2_main.cpp
+++ b/src/modules/ekf2/ekf2_main.cpp
@@ -672,8 +672,8 @@ void Ekf2::task_main()
 					ctrl_state.airspeed = airspeed.indicated_airspeed_m_s;
 					ctrl_state.airspeed_valid = true;
 				}
-			}
-			if (_airspeed_mode.get() == control_state_s::AIRSPD_MODE_EST) {
+
+			} else if (_airspeed_mode.get() == control_state_s::AIRSPD_MODE_EST) {
 				if (_ekf.local_position_is_valid()) {
 					ctrl_state.airspeed = sqrtf(vel[0] * vel[0] + vel[1] * vel[1] + vel[2] * vel[2]);
 					ctrl_state.airspeed_valid = true;

From 0551e001e2dc9799cf9a61b2bb87192e7685d1f9 Mon Sep 17 00:00:00 2001
From: Nate Weibley <nweibley@gmail.com>
Date: Thu, 23 Jun 2016 04:17:17 -0400
Subject: [PATCH 0779/1230] Properly reflect flow control state if IOCTL fails
 (#4873)

The flow control state is improperly reflected as enabled if the arch/HAL rejects an IOCTL to turn it on. Mavlink::enable_flow_control updates _flow_control_enabled only if the IOCTL call does not fail.
---
 src/modules/mavlink/mavlink_main.cpp | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp
index 3edf67676b..146ce40109 100644
--- a/src/modules/mavlink/mavlink_main.cpp
+++ b/src/modules/mavlink/mavlink_main.cpp
@@ -205,7 +205,7 @@ Mavlink::Mavlink() :
 	_subscribe_to_stream(nullptr),
 	_subscribe_to_stream_rate(0.0f),
 	_udp_initialised(false),
-	_flow_control_enabled(true),
+	_flow_control_enabled(false),
 	_last_write_success_time(0),
 	_last_write_try_time(0),
 	_mavlink_start_time(0),

From 7dea8d4a24b6cb00b05b9e29fb21e4db6d99a3f4 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Beat=20K=C3=BCng?= <beat-kueng@gmx.net>
Date: Thu, 23 Jun 2016 14:54:20 +0200
Subject: [PATCH 0780/1230] fix 10020_3dr_quad script: load proper mixer file
 (#4880)

fixes regression from 85245471c05a7453ffa50e2e74dbf8f48c01982b
---
 ROMFS/px4fmu_common/init.d/10020_3dr_quad | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/ROMFS/px4fmu_common/init.d/10020_3dr_quad b/ROMFS/px4fmu_common/init.d/10020_3dr_quad
index e91e994cbc..1917df18dd 100644
--- a/ROMFS/px4fmu_common/init.d/10020_3dr_quad
+++ b/ROMFS/px4fmu_common/init.d/10020_3dr_quad
@@ -7,7 +7,7 @@
 # @maintainer Lorenz Meier <lorenz@px4.io>
 #
 
-sh /etc/init.d/rc.mc_defaults
+sh /etc/init.d/4001_quad_x
 
 if [ $AUTOCNF == yes ]
 then

From 8026273cb08053e7a26a985e85b7acafa5665d81 Mon Sep 17 00:00:00 2001
From: tumbili <roman@px4.io>
Date: Thu, 23 Jun 2016 15:54:24 +0200
Subject: [PATCH 0781/1230] land_detector: do not publish if landing or
 freefall state has not changed

Signed-off-by: tumbili <roman@px4.io> and bkueng <beat-kueng@gmx.net>
---
 src/modules/land_detector/LandDetector.cpp | 18 ++++++++----------
 1 file changed, 8 insertions(+), 10 deletions(-)

diff --git a/src/modules/land_detector/LandDetector.cpp b/src/modules/land_detector/LandDetector.cpp
index 484ebd6d19..a80b1dad48 100644
--- a/src/modules/land_detector/LandDetector.cpp
+++ b/src/modules/land_detector/LandDetector.cpp
@@ -103,17 +103,15 @@ void LandDetector::cycle()
 	}
 
 	LandDetectionResult current_state = update();
+	bool landDetected = (current_state == LANDDETECTION_RES_LANDED);
+	bool freefallDetected = (current_state == LANDDETECTION_RES_FREEFALL);
 
-	_landDetected.timestamp = hrt_absolute_time();
-	_landDetected.landed = (current_state == LANDDETECTION_RES_LANDED);
-	_landDetected.freefall = (current_state == LANDDETECTION_RES_FREEFALL);
-
-	// publish the land detected broadcast
-	if (_landDetectedPub == nullptr) {
-		_landDetectedPub = orb_advertise(ORB_ID(vehicle_land_detected), &_landDetected);
-
-	} else {
-		orb_publish(ORB_ID(vehicle_land_detected), (orb_advert_t)_landDetectedPub, &_landDetected);
+	if (_landDetectedPub == nullptr || _landDetected.landed != landDetected || _landDetected.freefall != freefallDetected) {
+		_landDetected.timestamp = hrt_absolute_time();
+		_landDetected.landed = (current_state == LANDDETECTION_RES_LANDED);
+		_landDetected.freefall = (current_state == LANDDETECTION_RES_FREEFALL);
+		int instance;
+		orb_publish_auto(ORB_ID(vehicle_land_detected), &_landDetectedPub, &_landDetected, &instance, ORB_PRIO_DEFAULT);
 	}
 
 	if (!_taskShouldExit) {

From c2825f701a10d295399c727dc916ec53ab516714 Mon Sep 17 00:00:00 2001
From: tumbili <roman@px4.io>
Date: Thu, 23 Jun 2016 15:56:30 +0200
Subject: [PATCH 0782/1230] ekf_att_pos_estimator: fixed saving params when
 landed

fixed logic such that parameters are saved when vehicle just landed.
only save parameters once when state changed from in_air to landed.

Signed-off-by: tumbili <roman@px4.io> and bkueng <beat-kueng@gmx.net>
---
 .../ekf_att_pos_estimator/AttitudePositionEstimatorEKF.h   | 1 +
 .../ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp   | 7 +++++--
 2 files changed, 6 insertions(+), 2 deletions(-)

diff --git a/src/modules/ekf_att_pos_estimator/AttitudePositionEstimatorEKF.h b/src/modules/ekf_att_pos_estimator/AttitudePositionEstimatorEKF.h
index f7ea01bbd9..b66025c30b 100644
--- a/src/modules/ekf_att_pos_estimator/AttitudePositionEstimatorEKF.h
+++ b/src/modules/ekf_att_pos_estimator/AttitudePositionEstimatorEKF.h
@@ -224,6 +224,7 @@ private:
     bool            _vibration_warning; ///< high vibration levels detected
     bool            _ekf_logging;       ///< log EKF state
     unsigned        _debug;             ///< debug level - default 0
+    bool            _was_landed;        ///< indicates if was landed in previous iteration
 
     bool            _newHgtData;
     bool            _newAdsData;
diff --git a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp
index 7dc4afb77c..5b75ab2b12 100644
--- a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp
+++ b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp
@@ -216,6 +216,7 @@ AttitudePositionEstimatorEKF::AttitudePositionEstimatorEKF() :
 	_vibration_warning(false),
 	_ekf_logging(true),
 	_debug(0),
+	_was_landed(true),
 
 	_newHgtData(false),
 	_newAdsData(false),
@@ -381,8 +382,8 @@ void AttitudePositionEstimatorEKF::vehicle_land_detected_poll()
 
 		orb_copy(ORB_ID(vehicle_land_detected), _vehicle_land_detected_sub, &_vehicle_land_detected);
 
-		// Save params on landed
-		if (!_vehicle_land_detected.landed) {
+		// Save params on landed and previously not landed
+		if (_vehicle_land_detected.landed && !_was_landed) {
 			_mag_offset_x.set(_ekf->magBias.x);
 			_mag_offset_x.commit();
 			_mag_offset_y.set(_ekf->magBias.y);
@@ -390,6 +391,8 @@ void AttitudePositionEstimatorEKF::vehicle_land_detected_poll()
 			_mag_offset_z.set(_ekf->magBias.z);
 			_mag_offset_z.commit();
 		}
+
+		_was_landed = _vehicle_land_detected.landed;
 	}
 }
 

From b09872e79509fccb9fbf62cb5ae0e63302dfa334 Mon Sep 17 00:00:00 2001
From: Daniel Agar <daniel@agar.ca>
Date: Thu, 23 Jun 2016 11:04:36 -0400
Subject: [PATCH 0783/1230] travis-ci fix git shallow clone for git ver (#4885)

---
 .travis.yml | 3 ++-
 1 file changed, 2 insertions(+), 1 deletion(-)

diff --git a/.travis.yml b/.travis.yml
index 0493822186..50e56d71a8 100644
--- a/.travis.yml
+++ b/.travis.yml
@@ -25,7 +25,8 @@ cache:
 
 before_install:
   - if [ "${TRAVIS_OS_NAME}" = "linux" ]; then
-         git fetch --all --tags --unshallow
+         cd ${TRAVIS_BUILD_DIR}
+      && git fetch --unshallow && git fetch --all --tags
       && docker pull ${DOCKER_REPO}
       ;
     elif [ "${TRAVIS_OS_NAME}" = "osx" ]; then

From 9f95f457a987b7bb3b49f8166d2cfe1c74534c65 Mon Sep 17 00:00:00 2001
From: sirPerna <sir.perna@gmail.com>
Date: Thu, 23 Jun 2016 18:09:07 +0200
Subject: [PATCH 0784/1230] Update default Caipirinha parameters (#4883)

* Update default parameters for TBS Caipirinha

* reverse channel 0 doesn't exist
---
 .../px4fmu_common/init.d/3100_tbs_caipirinha  | 41 ++++++++++++-------
 1 file changed, 27 insertions(+), 14 deletions(-)

diff --git a/ROMFS/px4fmu_common/init.d/3100_tbs_caipirinha b/ROMFS/px4fmu_common/init.d/3100_tbs_caipirinha
index 780efd9807..f9c63b18ed 100644
--- a/ROMFS/px4fmu_common/init.d/3100_tbs_caipirinha
+++ b/ROMFS/px4fmu_common/init.d/3100_tbs_caipirinha
@@ -11,31 +11,44 @@ sh /etc/init.d/rc.fw_defaults
 
 if [ $AUTOCNF == yes ]
 then
-	param set FW_AIRSPD_MAX 20
-	param set FW_AIRSPD_MIN 12
-	param set FW_AIRSPD_TRIM 16
-	param set FW_R_TC 0.3
-	param set FW_P_TC 0.3
-	param set FW_L1_DAMPING 0.74
-	param set FW_L1_PERIOD 16
+	param set FW_AIRSPD_MAX 25
+	param set FW_AIRSPD_MIN 12.5
+	param set FW_AIRSPD_TRIM 16.5
+	param set LNDFW_AIRSPD_MAX 6
+	param set LNDFW_VELI_MAX 4
+	param set LNDFW_VEL_XY_MAX 3
+	param set LNDFW_VEL_Z_MAX 5
+	param set FW_R_TC 0.4
+	param set FW_P_TC 0.4
+	param set FW_THR_CRUISE 0.55
+	param set FW_L1_DAMPING 0.75
+	param set FW_L1_PERIOD 15
 	param set FW_LND_ANG 15
-	param set FW_LND_FLALT 5
+	param set FW_LND_FLALT 8
 	param set FW_LND_HHDIST 15
 	param set FW_LND_HVIRT 13
-	param set FW_LND_TLALT 5
+	param set FW_LND_TLALT 10
 	param set FW_THR_LND_MAX 0
-	param set FW_PR_FF 0.35
+	param set FW_P_LIM_MAX 20
+	param set FW_P_LIM_MIN -30
+	param set FW_R_LIM 45
+	param set FW_PR_FF 0.45
 	param set FW_PR_IMAX 0.4
-	param set FW_PR_P 0.08
-	param set FW_RR_FF 0.6
+	param set FW_PR_P 0.005
+	param set FW_RR_FF 0.45
 	param set FW_RR_IMAX 0.2
-	param set FW_RR_P 0.04
+	param set FW_RR_P 0.013
+	param set FW_P_RMAX_NEG 70
+	param set FW_P_RMAX_POS 70
+	param set FW_R_RMAX 70
 	param set SYS_COMPANION 157600
-	param set PWM_MAIN_REV0 1
 	param set PWM_MAIN_REV1 1
+	param set PWM_MAIN_REV2 1
 	param set PWM_DISARMED 0
 	param set PWM_MIN 900
 	param set PWM_MAX 2100
+	param set MIS_TAKEOFF_ALT 50
+	param set NAV_LOITER_RAD 30
 fi
 
 set PWM_DISARMED p:PWM_DISARMED

From 571798c318a6736f748495378907cf76fae3c884 Mon Sep 17 00:00:00 2001
From: Roman Bapst <bapstr@ethz.ch>
Date: Thu, 23 Jun 2016 18:41:38 +0200
Subject: [PATCH 0785/1230] Pr external battery monitoring (#4881)

* mavlink receiver: added handling of battery status

handle incoming battery status messages in order to support external
battery monitoring

Signed-off-by: Roman <bapstr@ethz.ch>

* sensor params: added parameter for battery monitoring source

Signed-off-by: Roman <bapstr@ethz.ch>

* sensors: only publish battery status if we don't have external battery
monitoring activated

Signed-off-by: Roman <bapstr@ethz.ch>
---
 src/modules/mavlink/mavlink_receiver.cpp | 41 ++++++++++++++++++++++++
 src/modules/mavlink/mavlink_receiver.h   |  1 +
 src/modules/sensors/sensor_params.c      | 16 +++++++++
 src/modules/sensors/sensors.cpp          |  7 +++-
 4 files changed, 64 insertions(+), 1 deletion(-)

diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp
index 56a6999147..c54c225d40 100644
--- a/src/modules/mavlink/mavlink_receiver.cpp
+++ b/src/modules/mavlink/mavlink_receiver.cpp
@@ -258,6 +258,9 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg)
 	case MAVLINK_MSG_ID_GPS_RTCM_DATA:
 		handle_message_gps_rtcm_data(msg);
 		break;
+	case MAVLINK_MSG_ID_BATTERY_STATUS:
+		handle_message_battery_status(msg);
+		break;
 
 	default:
 		break;
@@ -1154,6 +1157,44 @@ MavlinkReceiver::handle_message_radio_status(mavlink_message_t *msg)
 	}
 }
 
+void
+MavlinkReceiver::handle_message_battery_status(mavlink_message_t *msg)
+{
+	// external battery measurements
+	mavlink_battery_status_t battery_mavlink;
+	mavlink_msg_battery_status_decode(msg, &battery_mavlink);
+
+	battery_status_s battery_status = {};
+	battery_status.timestamp = hrt_absolute_time();
+
+	float voltage_sum = 0.0f;
+	uint8_t cell_count = 0;
+	for (unsigned i = 0; i < 10;i++) {
+		if (battery_mavlink.voltages[i] < UINT16_MAX) {
+			voltage_sum += (float)(battery_mavlink.voltages[i]) / 1000.0f;
+		} else {
+			cell_count = i;
+			break;
+		}
+	}
+
+	battery_status.voltage_v = voltage_sum;
+	battery_status.voltage_filtered_v  = voltage_sum;
+	battery_status.current_a = battery_status.current_filtered_a = (float)(battery_mavlink.current_battery) / 10000.0f;
+	battery_status.current_filtered_a = battery_status.current_a;
+	battery_status.remaining = (float)battery_mavlink.battery_remaining / 100.0f;
+	battery_status.discharged_mah = (float)battery_mavlink.current_consumed;
+	battery_status.cell_count = cell_count;
+	battery_status.connected = true;
+
+	if (_battery_pub == nullptr) {
+		_battery_pub = orb_advertise(ORB_ID(battery_status), &battery_status);
+
+	} else {
+		orb_publish(ORB_ID(battery_status), _battery_pub, &battery_status);
+	}
+}
+
 switch_pos_t
 MavlinkReceiver::decode_switch_pos(uint16_t buttons, unsigned sw)
 {
diff --git a/src/modules/mavlink/mavlink_receiver.h b/src/modules/mavlink/mavlink_receiver.h
index bf0a5121ed..8d3b3d0396 100644
--- a/src/modules/mavlink/mavlink_receiver.h
+++ b/src/modules/mavlink/mavlink_receiver.h
@@ -142,6 +142,7 @@ private:
 	void handle_message_follow_target(mavlink_message_t *msg);
 	void handle_message_adsb_vehicle(mavlink_message_t *msg);
 	void handle_message_gps_rtcm_data(mavlink_message_t *msg);
+	void handle_message_battery_status(mavlink_message_t *msg);
 
 	void *receive_thread(void *arg);
 
diff --git a/src/modules/sensors/sensor_params.c b/src/modules/sensors/sensor_params.c
index 2de4b4216f..57313d2afb 100644
--- a/src/modules/sensors/sensor_params.c
+++ b/src/modules/sensors/sensor_params.c
@@ -2023,6 +2023,22 @@ PARAM_DEFINE_FLOAT(BAT_V_DIV, -1.0);
  */
 PARAM_DEFINE_FLOAT(BAT_A_PER_V, -1.0);
 
+/**
+ * Battery monitoring source.
+ *
+ * This parameter controls the source of battery data. The value 'Power Module'
+ * means that measurements are expected to come from a power module. If the value is set to
+ * 'External' then the system expects to receive mavlink battery status messages.
+ *
+ * @min 0
+ * @max 1
+ * @value 0 Power Module
+ * @value 1 External
+ * @group Battery Calibration
+ */
+
+PARAM_DEFINE_INT32(BAT_SOURCE, 0);
+
 
 /**
  * RC channel count
diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp
index 7041d5a5a0..cea1686143 100644
--- a/src/modules/sensors/sensors.cpp
+++ b/src/modules/sensors/sensors.cpp
@@ -319,6 +319,7 @@ private:
 		float battery_current_offset;
 		float battery_v_div;
 		float battery_a_per_v;
+		int32_t battery_source;
 
 		float baro_qnh;
 
@@ -381,6 +382,7 @@ private:
 		param_t battery_current_offset;
 		param_t battery_v_div;
 		param_t battery_a_per_v;
+		param_t battery_source;
 
 		param_t board_rotation;
 
@@ -653,6 +655,7 @@ Sensors::Sensors() :
 	_parameter_handles.battery_current_offset = param_find("BAT_V_OFFS_CURR");
 	_parameter_handles.battery_v_div = param_find("BAT_V_DIV");
 	_parameter_handles.battery_a_per_v = param_find("BAT_A_PER_V");
+	_parameter_handles.battery_source = param_find("BAT_SOURCE");
 
 	/* rotations */
 	_parameter_handles.board_rotation = param_find("SENS_BOARD_ROT");
@@ -948,6 +951,8 @@ Sensors::parameters_update()
 #endif
 	}
 
+	param_get(_parameter_handles.battery_source, &(_parameters.battery_source));
+
 	param_get(_parameter_handles.board_rotation, &(_parameters.board_rotation));
 	get_rot_matrix((enum Rotation)_parameters.board_rotation, &_board_rotation);
 
@@ -1718,7 +1723,7 @@ Sensors::adc_poll(struct sensor_combined_s &raw)
 				}
 			}
 
-			if (updated_battery) {
+			if (_parameters.battery_source == 0 && updated_battery) {
 				actuator_controls_s ctrl;
 				orb_copy(ORB_ID(actuator_controls_0), _actuator_ctrl_0_sub, &ctrl);
 				_battery.updateBatteryStatus(t, bat_voltage_v, bat_current_a, ctrl.control[actuator_controls_s::INDEX_THROTTLE],

From 024a86c30979ded47dade49cd0b5dc2ecb1f9067 Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Thu, 23 Jun 2016 22:32:14 +0200
Subject: [PATCH 0786/1230] Simulator: fix battery sim

---
 src/modules/simulator/simulator_mavlink.cpp | 7 ++++---
 1 file changed, 4 insertions(+), 3 deletions(-)

diff --git a/src/modules/simulator/simulator_mavlink.cpp b/src/modules/simulator/simulator_mavlink.cpp
index d3b6e53761..6b01a22909 100644
--- a/src/modules/simulator/simulator_mavlink.cpp
+++ b/src/modules/simulator/simulator_mavlink.cpp
@@ -69,7 +69,6 @@ static int _fd;
 static unsigned char _buf[1024];
 sockaddr_in _srcaddr;
 static socklen_t _addrlen = sizeof(_srcaddr);
-static bool actuators_on = false;
 static hrt_abstime batt_sim_start = 0;
 
 const unsigned mode_flag_armed = 128; // following MAVLink spec
@@ -245,7 +244,9 @@ void Simulator::handle_message(mavlink_message_t *msg, bool publish)
 
 			const float discharge_interval_us = 60 * 1000 * 1000;
 
-			if (!actuators_on || batt_sim_start == 0 || batt_sim_start > now) {
+			bool armed = (_vehicle_status.arming_state == vehicle_status_s::ARMING_STATE_ARMED);
+
+			if (!armed || batt_sim_start == 0 || batt_sim_start > now) {
 				batt_sim_start = now;
 			}
 
@@ -268,7 +269,7 @@ void Simulator::handle_message(mavlink_message_t *msg, bool publish)
 
 			// TODO: don't hard-code throttle.
 			const float throttle = 0.5f;
-			_battery.updateBatteryStatus(now, vbatt, ibatt, throttle, actuators_on, &battery_status);
+			_battery.updateBatteryStatus(now, vbatt, ibatt, throttle, armed, &battery_status);
 
 			// publish the battery voltage
 			int batt_multi;

From 40a7bd009f289899649f01827089c55e1ff7b340 Mon Sep 17 00:00:00 2001
From: Mark Whitehorn <kd0aij@gmail.com>
Date: Thu, 23 Jun 2016 15:27:51 -0600
Subject: [PATCH 0787/1230] implement Spektrum bind function for Pixracer R14
 (#4887)

---
 src/drivers/boards/px4fmu-v4/board_config.h | 2 +-
 src/drivers/px4fmu/fmu.cpp                  | 2 +-
 2 files changed, 2 insertions(+), 2 deletions(-)

diff --git a/src/drivers/boards/px4fmu-v4/board_config.h b/src/drivers/boards/px4fmu-v4/board_config.h
index 9c6584c4e5..97499d0136 100644
--- a/src/drivers/boards/px4fmu-v4/board_config.h
+++ b/src/drivers/boards/px4fmu-v4/board_config.h
@@ -268,7 +268,7 @@ __BEGIN_DECLS
 #define SPEKTRUM_RX_AS_UART()		px4_arch_configgpio(GPIO_USART1_RX)
 
 // FMUv4 has a separate GPIO for serial RC output
-#define GPIO_RC_OUT			(GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTC|GPIO_PIN6)
+#define GPIO_RC_OUT			(GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN0)
 #define SPEKTRUM_RX_AS_GPIO()		px4_arch_configgpio(GPIO_RC_OUT)
 #define SPEKTRUM_RX_HIGH(_s)		px4_arch_gpiowrite(GPIO_RC_OUT, (_s))
 
diff --git a/src/drivers/px4fmu/fmu.cpp b/src/drivers/px4fmu/fmu.cpp
index 48fc9950d1..65c23423a8 100644
--- a/src/drivers/px4fmu/fmu.cpp
+++ b/src/drivers/px4fmu/fmu.cpp
@@ -1989,9 +1989,9 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg)
 		    arg == DSMX8_BIND_PULSES) {
 
 			dsm_bind(DSM_CMD_BIND_POWER_DOWN, 0);
-			usleep(500000);
 
 			dsm_bind(DSM_CMD_BIND_SET_RX_OUT, 0);
+			usleep(500000);
 
 			dsm_bind(DSM_CMD_BIND_POWER_UP, 0);
 			usleep(72000);

From fab201a2d6e1d646caa342c247635b60277c83a5 Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Thu, 23 Jun 2016 13:30:35 +0200
Subject: [PATCH 0788/1230] Initial import of TAP controllers

---
 src/drivers/tap_esc/CMakeLists.txt |   43 ++
 src/drivers/tap_esc/drv_tap_esc.h  |  184 +++++
 src/drivers/tap_esc/tap_esc.cpp    | 1013 ++++++++++++++++++++++++++++
 3 files changed, 1240 insertions(+)
 create mode 100644 src/drivers/tap_esc/CMakeLists.txt
 create mode 100755 src/drivers/tap_esc/drv_tap_esc.h
 create mode 100644 src/drivers/tap_esc/tap_esc.cpp

diff --git a/src/drivers/tap_esc/CMakeLists.txt b/src/drivers/tap_esc/CMakeLists.txt
new file mode 100644
index 0000000000..1869d3c43f
--- /dev/null
+++ b/src/drivers/tap_esc/CMakeLists.txt
@@ -0,0 +1,43 @@
+############################################################################
+#
+#   Copyright (c) 2016 PX4 Development Team. All rights reserved.
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions
+# are met:
+#
+# 1. Redistributions of source code must retain the above copyright
+#    notice, this list of conditions and the following disclaimer.
+# 2. Redistributions in binary form must reproduce the above copyright
+#    notice, this list of conditions and the following disclaimer in
+#    the documentation and/or other materials provided with the
+#    distribution.
+# 3. Neither the name PX4 nor the names of its contributors may be
+#    used to endorse or promote products derived from this software
+#    without specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.
+#
+############################################################################
+px4_add_module(
+	MODULE drivers__tap_esc
+	MAIN tap_esc
+	COMPILE_FLAGS
+		-Os
+	SRCS
+		tap_esc.cpp
+	DEPENDS
+		platforms__common
+	)
+# vim: set noet ft=cmake fenc=utf-8 ff=unix : 
diff --git a/src/drivers/tap_esc/drv_tap_esc.h b/src/drivers/tap_esc/drv_tap_esc.h
new file mode 100755
index 0000000000..1647aa17e9
--- /dev/null
+++ b/src/drivers/tap_esc/drv_tap_esc.h
@@ -0,0 +1,184 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2016 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+#define TAP_ESC_CRC {\
+		0x00, 0xE7, 0x29, 0xCE, 0x52, 0xB5, 0x7B, 0x9C, 0xA4, 0x43, 0x8D, 0x6A,\
+		0xF6, 0x11, 0xDF, 0x38, 0xAF, 0x48, 0x86, 0x61, 0xFD, 0x1A, 0xD4, 0x33,\
+		0x0B, 0xEC, 0x22, 0xC5, 0x59, 0xBE, 0x70, 0x97, 0xB9, 0x5E, 0x90, 0x77,\
+		0xEB, 0x0C, 0xC2, 0x25, 0x1D, 0xFA, 0x34, 0xD3, 0x4F, 0xA8, 0x66, 0x81,\
+		0x16, 0xF1, 0x3F, 0xD8, 0x44, 0xA3, 0x6D, 0x8A, 0xB2, 0x55, 0x9B, 0x7C,\
+		0xE0, 0x07, 0xC9, 0x2E, 0x95, 0x72, 0xBC, 0x5B, 0xC7, 0x20, 0xEE, 0x09,\
+		0x31, 0xD6, 0x18, 0xFF, 0x63, 0x84, 0x4A, 0xAD, 0x3A, 0xDD, 0x13, 0xF4,\
+		0x68, 0x8F, 0x41, 0xA6, 0x9E, 0x79, 0xB7, 0x50, 0xCC, 0x2B, 0xE5, 0x02,\
+		0x2C, 0xCB, 0x05, 0xE2, 0x7E, 0x99, 0x57, 0xB0, 0x88, 0x6F, 0xA1, 0x46,\
+		0xDA, 0x3D, 0xF3, 0x14, 0x83, 0x64, 0xAA, 0x4D, 0xD1, 0x36, 0xF8, 0x1F,\
+		0x27, 0xC0, 0x0E, 0xE9, 0x75, 0x92, 0x5C, 0xBB, 0xCD, 0x2A, 0xE4, 0x03,\
+		0x9F, 0x78, 0xB6, 0x51, 0x69, 0x8E, 0x40, 0xA7, 0x3B, 0xDC, 0x12, 0xF5,\
+		0x62, 0x85, 0x4B, 0xAC, 0x30, 0xD7, 0x19, 0xFE, 0xC6, 0x21, 0xEF, 0x08,\
+		0x94, 0x73, 0xBD, 0x5A, 0x74, 0x93, 0x5D, 0xBA, 0x26, 0xC1, 0x0F, 0xE8,\
+		0xD0, 0x37, 0xF9, 0x1E, 0x82, 0x65, 0xAB, 0x4C, 0xDB, 0x3C, 0xF2, 0x15,\
+		0x89, 0x6E, 0xA0, 0x47, 0x7F, 0x98, 0x56, 0xB1, 0x2D, 0xCA, 0x04, 0xE3,\
+		0x58, 0xBF, 0x71, 0x96, 0x0A, 0xED, 0x23, 0xC4, 0xFC, 0x1B, 0xD5, 0x32,\
+		0xAE, 0x49, 0x87, 0x60, 0xF7, 0x10, 0xDE, 0x39, 0xA5, 0x42, 0x8C, 0x6B,\
+		0x53, 0xB4, 0x7A, 0x9D, 0x01, 0xE6, 0x28, 0xCF, 0xE1, 0x06, 0xC8, 0x2F,\
+		0xB3, 0x54, 0x9A, 0x7D, 0x45, 0xA2, 0x6C, 0x8B, 0x17, 0xF0, 0x3E, 0xD9,\
+		0x4E, 0xA9, 0x67, 0x80, 0x1C, 0xFB, 0x35, 0xD2, 0xEA, 0x0D, 0xC3, 0x24,\
+		0xB8, 0x5F, 0x91, 0x76\
+	}
+
+#define TAP_ESC_MAX_PACKET_LEN 20
+#define TAP_ESC_MAX_MOTOR_NUM 8
+
+#define RPMMAX 1900
+#define RPMMIN 1100
+
+#define RUN_CHANNEL_VALUE_MASK	 (uint16_t)0x07ff
+#define RUN_RED_LED_ON_MASK	 (uint16_t)0x0800
+#define RUN_GREEN_LED_ON_MASK	 (uint16_t)0x1000
+#define RUN_BLUE_LED_ON_MASK	 (uint16_t)0x2000
+#define RUN_LED_ON_MASK	 (uint16_t)0x3800
+#define RUN_FEEDBACK_ENABLE_MASK	(uint16_t)0x4000
+#define RUN_REVERSE_MASK	 (uint16_t)0x8000
+
+
+#pragma pack(push,1)
+typedef  struct {
+	uint8_t ESC_ID;
+	uint8_t ESC_STATUS;
+	int16_t speed;
+#ifdef VOLTAGE_SENSOR_HAVE
+	uint16_t voltage;
+#endif
+#ifdef CURRENT_SENSOR_HAVE
+	uint16_t current;
+#endif
+#ifdef TEMPERATURE_SENSOR_HAVE
+	uint8_t temperature;
+#endif
+
+} ESC_FEEDBACK_DATA;
+
+typedef  struct {
+	uint8_t head;
+	uint8_t len;
+	uint8_t msg_id;
+	uint8_t data[100];
+	uint8_t crc_data;
+
+} ESC_FEEDBACK_PACKET;
+
+typedef  struct {
+	uint8_t head;
+	uint8_t tail;
+	uint8_t dat_cnt;
+	uint8_t esc_feedbacck_buf[128];
+} ESC_UART_BUF;
+
+#pragma pack(pop)
+/******************************************************************************************
+ * ESCBUS_MSG_ID_RUN_INFO packet
+ *
+ * Monitor message of ESCs while motor is running
+ *
+ * channelID: assigned channel number
+ *
+ * ESCStatus: status of ESC
+ * 	Num		Health status
+ * 	0		HEALTHY
+ * 	1		WARNING_LOW_VOLTAGE
+ * 	2		WARNING_OVER_CURRENT
+ * 	3		WARNING_OVER_HEAT
+ *	4		ERROR_MOTOR_LOW_SPEED_LOSE_STEP
+ *  5		ERROR_MOTOR_STALL
+ *  6		ERROR_HARDWARE
+ *  7		ERROR_LOSE_PROPELLER
+ *  8		ERROR_OVER_CURRENT
+ *
+ * speed: -32767 - 32767 rpm
+ *
+ * temperature: 0 - 256 celsius degree (if available)
+ * voltage: 0.00 - 100.00 V (if available)
+ * current: 0.0 - 200.0 A (if available)
+ */
+
+typedef enum {
+	ESC_STATUS_HEALTHY,
+	ESC_STATUS_WARNING_LOW_VOLTAGE,
+	ESC_STATUS_WARNING_OVER_HEAT,
+	ESC_STATUS_ERROR_MOTOR_LOW_SPEED_LOSE_STEP,
+	ESC_STATUS_ERROR_MOTOR_STALL,
+	ESC_STATUS_ERROR_HARDWARE,
+	ESC_STATUS_ERROR_LOSE_PROPELLER,
+	ESC_STATUS_ERROR_OVER_CURRENT,
+} ESCBUS_ENUM_ESC_STATUS;
+
+
+typedef enum {
+// messages or command to ESC
+	ESCBUS_MSG_ID_CONFIG_BASIC = 0,
+	ESCBUS_MSG_ID_CONFIG_FULL,
+	ESCBUS_MSG_ID_RUN,
+	ESCBUS_MSG_ID_TUNE,
+	ESCBUS_MSG_ID_DO_CMD,
+// messages from ESC
+	ESCBUS_MSG_ID_REQUEST_INFO,
+	ESCBUS_MSG_ID_CONFIG_INFO_BASIC,	// simple configuration info for request from flight controller
+	ESCBUS_MSG_ID_CONFIG_INFO_FULL,// full configuration info for request from host such as computer
+	ESCBUS_MSG_ID_RUN_INFO,// feedback message in RUN mode
+	ESCBUS_MSG_ID_STUDY_INFO,	// studied parameters in STUDY mode
+	ESCBUS_MSG_ID_COMM_INFO,	// communication method info
+	ESCBUS_MSG_ID_DEVICE_INFO,// ESC device info
+	ESCBUS_MSG_ID_ASSIGNED_ID,	// never touch ESCBUS_MSG_ID_MAX_NUM
+	//boot loader used
+	PROTO_OK = 0x10, // INSYNC/OK - 'ok' response
+	PROTO_FAILED = 0x11, // INSYNC/FAILED - 'fail' response
+
+	ESCBUS_MSG_ID_BOOT_SYNC = 0x21, // boot loader used
+	PROTO_GET_DEVICE = 0x22, // get device ID bytes
+	PROTO_CHIP_ERASE = 0x23, // erase program area and reset program address
+	PROTO_PROG_MULTI = 0x27, // write bytes at program address and increment
+	PROTO_GET_CRC = 0x29, // compute & return a CRC
+	PROTO_BOOT = 0x30, // boot the application
+	PROTO_GET_SOFTWARE_VERSION = 0x40,
+	ESCBUS_MSG_ID_MAX_NUM,
+}
+ESCBUS_ENUM_MESSAGE_ID;
+
+typedef enum {
+	HEAD,
+	LEN,
+	ID,
+	DATA,
+	CRC,
+
+} PARSR_ESC_STATE;
diff --git a/src/drivers/tap_esc/tap_esc.cpp b/src/drivers/tap_esc/tap_esc.cpp
new file mode 100644
index 0000000000..7abf297ef3
--- /dev/null
+++ b/src/drivers/tap_esc/tap_esc.cpp
@@ -0,0 +1,1013 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2016 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+#include <stdint.h>
+
+#include <px4_tasks.h>
+#include <px4_getopt.h>
+#include <px4_posix.h>
+#include <errno.h>
+#include <termios.h>
+
+#include <drivers/device/device.h>
+#include <uORB/uORB.h>
+#include <uORB/topics/actuator_controls.h>
+#include <uORB/topics/actuator_outputs.h>
+#include <uORB/topics/actuator_armed.h>
+#include <uORB/topics/input_rc.h>
+#include <uORB/topics/tap_esc_report.h>
+
+#include <drivers/drv_hrt.h>
+#include <drivers/drv_mixer.h>
+#include <systemlib/mixer/mixer.h>
+#include <systemlib/param/param.h>
+#include <systemlib/pwm_limit/pwm_limit.h>
+
+#define NAN_VALUE	(0.0f/0.0f)
+
+#define VOLTAGE_SENSOR_HAVE
+
+#include "drv_tap_esc.h"
+
+/*
+ * This driver connects to TAP ESCs via serial.
+ */
+static const uint8_t crcTable[256] = TAP_ESC_CRC;
+static int _uart_fd = -1;
+class TAP_ESC : public device::CDev
+{
+public:
+	enum Mode {
+		MODE_NONE,
+		MODE_2PWM,
+		MODE_2PWM2CAP,
+		MODE_3PWM,
+		MODE_3PWM1CAP,
+		MODE_4PWM,
+		MODE_6PWM,
+		MODE_8PWM,
+		MODE_4CAP,
+		MODE_5CAP,
+		MODE_6CAP,
+	};
+	TAP_ESC();
+	virtual ~TAP_ESC();
+	virtual int	init();
+	virtual int	ioctl(file *filp, int cmd, unsigned long arg);
+	void cycle();
+private:
+
+	bool IS_armed;
+
+	unsigned	_poll_fds_num;
+	Mode		_mode;
+	// subscriptions
+	unsigned	_num_outputs;
+	int	_armed_sub;
+	orb_advert_t        	_outputs_pub = nullptr;
+	actuator_outputs_s      _outputs;
+	static actuator_armed_s	_armed;
+
+	int	_control_subs[actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS];
+
+	px4_pollfd_struct_t	_poll_fds[actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS];
+
+	actuator_controls_s _controls[actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS];
+
+	orb_id_t	_control_topics[actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS];
+
+	orb_advert_t        _esc_feedback_pub = nullptr;
+	tap_esc_report_s      _esc_feedback;
+
+	MixerGroup	*_mixers;
+	uint32_t	_groups_required;
+	uint32_t	_groups_subscribed;
+	volatile bool	_initialized;
+	unsigned	_pwm_default_rate;
+	unsigned	_current_update_rate;
+	ESC_UART_BUF uartbuf;
+	ESC_FEEDBACK_DATA feed_back_data;
+	ESC_FEEDBACK_PACKET  feed_back_packet;
+	void		subscribe();
+
+	void		work_start();
+	void		work_stop();
+	void send_esc_outputs(const float *pwm, const unsigned num_pwm);
+	uint8_t crc8_esc(uint8_t *p, uint8_t len);
+	void read_data_from_uart();
+	bool parse_tap_esc_feedback(ESC_UART_BUF *serial_buf, ESC_FEEDBACK_PACKET *packetdata);
+	static int control_callback(uintptr_t handle,
+				    uint8_t control_group,
+				    uint8_t control_index,
+				    float &input);
+};
+
+actuator_armed_s TAP_ESC::_armed = {};
+
+namespace
+{
+TAP_ESC	*tap_esc;
+}
+
+# define TAP_ESC_DEVICE_PATH	"/dev/tap_esc"
+
+TAP_ESC::TAP_ESC():
+	CDev("tap_esc", TAP_ESC_DEVICE_PATH),
+	IS_armed(false),
+	_poll_fds_num(0),
+	_mode(MODE_4PWM),
+	_num_outputs(0),
+	_armed_sub(-1),
+	_outputs_pub(nullptr),
+	_control_subs{ -1},
+	_esc_feedback_pub(nullptr),
+	_mixers(nullptr),
+	_groups_required(0),
+	_groups_subscribed(0),
+	_initialized(false),
+	_pwm_default_rate(400),
+	_current_update_rate(0)
+
+{
+	_control_topics[0] = ORB_ID(actuator_controls_0);
+	_control_topics[1] = ORB_ID(actuator_controls_1);
+	_control_topics[2] = ORB_ID(actuator_controls_2);
+	_control_topics[3] = ORB_ID(actuator_controls_3);
+	memset(_controls, 0, sizeof(_controls));
+	memset(_poll_fds, 0, sizeof(_poll_fds));
+	uartbuf.head = 0;
+	uartbuf.tail = 0;
+	uartbuf.dat_cnt = 0;
+	memset(uartbuf.esc_feedbacck_buf, 0, sizeof(uartbuf.esc_feedbacck_buf));
+}
+
+TAP_ESC::~TAP_ESC()
+{
+	if (_initialized) {
+		/* tell the task we want it to go away */
+		work_stop();
+
+		int i = 10;
+
+		do {
+			/* wait 50ms - it should wake every 100ms or so worst-case */
+			usleep(50000);
+			i--;
+
+		} while (_initialized && i > 0);
+	}
+
+	// clean up the alternate device node
+	//unregister_class_devname(PWM_OUTPUT_BASE_DEVICE_PATH, _class_instance);
+
+	tap_esc = nullptr;
+}
+
+int
+TAP_ESC::init()
+{
+	int ret;
+
+	ASSERT(!_initialized);
+
+	/* do regular cdev init */
+	ret = CDev::init();
+
+	if (ret != OK) {
+		return ret;
+	}
+
+	return OK;
+}
+
+void
+TAP_ESC::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;
+
+	for (unsigned i = 0; i < actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS; i++) {
+		if (sub_groups & (1 << i)) {
+			DEVICE_DEBUG("subscribe to actuator_controls_%d", i);
+			_control_subs[i] = orb_subscribe(_control_topics[i]);
+		}
+
+		if (unsub_groups & (1 << i)) {
+			DEVICE_DEBUG("unsubscribe from actuator_controls_%d", i);
+			::close(_control_subs[i]);
+			_control_subs[i] = -1;
+		}
+
+		if (_control_subs[i] > 0) {
+			_poll_fds[_poll_fds_num].fd = _control_subs[i];
+			_poll_fds[_poll_fds_num].events = POLLIN;
+			_poll_fds_num++;
+		}
+	}
+}
+
+uint8_t TAP_ESC::crc8_esc(uint8_t *p, uint8_t len)
+{
+	uint8_t crc = 0;
+
+	for (uint8_t i = 0; i < len; i++) {
+		crc = crcTable[crc^*p++];
+	}
+
+	return crc;
+}
+
+void TAP_ESC:: send_esc_outputs(const float *pwm, const unsigned num_pwm)
+{
+
+	uint16_t rpm[TAP_ESC_MAX_MOTOR_NUM];
+	uint8_t motor_cnt = num_pwm;
+	unsigned char ESCData[TAP_ESC_MAX_PACKET_LEN] = {0};
+	static uint8_t which_to_respone = 0;
+
+	for (uint8_t i = 0; i < motor_cnt; i++) {
+		rpm[i] = pwm[i];
+
+		if (rpm[i] > RPMMAX) {
+			rpm[i] = RPMMAX;
+
+		} else if (rpm[i] < RPMMIN) {
+			rpm[i] = RPMMIN;
+		}
+	}
+
+	switch (which_to_respone) {
+	case 0: rpm[0] |= RUN_FEEDBACK_ENABLE_MASK;
+		break;
+
+	case 1: rpm[1] |= RUN_FEEDBACK_ENABLE_MASK;
+		break;
+
+	case 2: rpm[2] |= RUN_FEEDBACK_ENABLE_MASK;
+		break;
+
+	case 3: rpm[3] |= RUN_FEEDBACK_ENABLE_MASK;
+		break;
+
+	case 4: rpm[4] |= RUN_FEEDBACK_ENABLE_MASK;
+		break;
+
+	case 5: rpm[5] |= RUN_FEEDBACK_ENABLE_MASK;
+		break;
+
+	case 6: rpm[6] |= RUN_FEEDBACK_ENABLE_MASK;
+		break;
+
+	case 7: rpm[7] |= RUN_FEEDBACK_ENABLE_MASK;
+
+	default: break;
+
+
+	}
+
+	if (++which_to_respone == motor_cnt) {
+		which_to_respone = 0;
+	}
+
+	//rpm[2]|=RUN_FEEDBACK_ENABLE_MASK;
+
+	ESCData[0] = 0xFE;
+	ESCData[1] = motor_cnt * 2;
+	ESCData[2] = ESCBUS_MSG_ID_RUN;
+
+	for (uint8_t i = 0; i < motor_cnt; i++) {
+		ESCData[3 + 2 * i] = rpm[i] & 0x00ff;
+		ESCData[3 + 2 * i + 1] = rpm[i] >> 8;
+	}
+
+	ESCData[3 + 2 * motor_cnt] = crc8_esc(ESCData + 1, motor_cnt * 2 + 2);
+
+	int ret = ::write(_uart_fd, &ESCData[0], motor_cnt * 2 + 4);
+
+
+	if (ret < 1) {
+		PX4_WARN("TX ERROR: ret: %d, errno: %d", ret, errno);
+	}
+}
+
+void TAP_ESC::read_data_from_uart()
+{
+	uint8_t tmp_serial_buf[128] = {0};
+
+	int len =::read(_uart_fd, &tmp_serial_buf[0], 128);
+
+	if (len > 0 && (uartbuf.dat_cnt + len < 128)) {
+		for (int i = 0; i < len; i++) {
+			uartbuf.esc_feedbacck_buf[uartbuf.tail++] = tmp_serial_buf[i];
+			uartbuf.dat_cnt++;
+
+			if (uartbuf.tail >= 128) {
+				uartbuf.tail = 0;
+			}
+		}
+	}
+}
+
+bool TAP_ESC:: parse_tap_esc_feedback(ESC_UART_BUF *serial_buf, ESC_FEEDBACK_PACKET *packetdata)
+{
+	static PARSR_ESC_STATE state = HEAD;
+	static uint8_t data_index = 0;
+	static uint8_t crc_data_cal;
+
+	if (serial_buf->dat_cnt > 0) {
+		for (int i = 0; i < serial_buf->dat_cnt; i++) {
+			switch (state) {
+			case HEAD: if (serial_buf->esc_feedbacck_buf[serial_buf->head] == 0xFE) {
+					packetdata->head = 0xFE; //just_keep the format
+					state = LEN;
+				}
+
+				break;
+
+			case LEN:  if (serial_buf->esc_feedbacck_buf[serial_buf->head] < 100) {
+					packetdata->len = serial_buf->esc_feedbacck_buf[serial_buf->head];
+					state = ID;
+
+				} else {
+					state = HEAD;
+				}
+
+				break;
+
+			case ID:   if (serial_buf->esc_feedbacck_buf[serial_buf->head] < ESCBUS_MSG_ID_MAX_NUM) {
+					packetdata->msg_id = serial_buf->esc_feedbacck_buf[serial_buf->head];
+					data_index = 0;
+					state = DATA;
+
+				} else {
+					state = HEAD;
+				}
+
+				break;
+
+			case DATA: packetdata->data[data_index++] = serial_buf->esc_feedbacck_buf[serial_buf->head];
+
+				if (data_index >= packetdata->len) {
+
+					crc_data_cal = crc8_esc((uint8_t *)(&packetdata->len), packetdata->len + 2);
+					state = CRC;
+				}
+
+				break;
+
+			case CRC: if (crc_data_cal == serial_buf->esc_feedbacck_buf[serial_buf->head]) {
+					packetdata->crc_data = serial_buf->esc_feedbacck_buf[serial_buf->head];
+
+					if (++serial_buf->head >= 128) {
+						serial_buf->head = 0;
+					}
+
+					serial_buf->dat_cnt--;
+					state = HEAD;
+					return true;
+				}
+
+				state = HEAD;
+				break;
+
+			default : state = HEAD;
+				break;
+
+			}
+
+			if (++serial_buf->head >= 128) {
+				serial_buf->head = 0;
+			}
+
+			serial_buf->dat_cnt--;
+
+		}
+
+
+	}
+
+	return false;
+}
+
+void
+TAP_ESC::cycle()
+{
+
+	if (!_initialized) {
+
+		_current_update_rate = 0;
+		/* advertise the mixed control outputs, insist on the first group output */
+		_outputs_pub = orb_advertise(ORB_ID(actuator_outputs), &_outputs);
+		_esc_feedback_pub = orb_advertise(ORB_ID(tap_esc_report), &_esc_feedback);
+		_armed_sub = orb_subscribe(ORB_ID(actuator_armed));
+		_initialized = true;
+	}
+
+	if (_groups_subscribed != _groups_required) {
+		subscribe();
+		_groups_subscribed = _groups_required;
+		_current_update_rate = 0;
+	}
+
+	unsigned max_rate = _pwm_default_rate ;
+
+	if (_current_update_rate != max_rate) {
+		_current_update_rate = max_rate;
+		int update_rate_in_ms = int(1000 / _current_update_rate);
+
+		/* reject faster than 500 Hz updates */
+		if (update_rate_in_ms < 2) {
+			update_rate_in_ms = 2;
+		}
+
+		/* reject slower than 10 Hz updates */
+		if (update_rate_in_ms > 100) {
+			update_rate_in_ms = 100;
+		}
+
+		DEVICE_DEBUG("adjusted actuator update interval to %ums", update_rate_in_ms);
+
+		for (unsigned i = 0; i < actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS; i++) {
+			if (_control_subs[i] > 0) {
+				orb_set_interval(_control_subs[i], update_rate_in_ms);
+			}
+		}
+
+		// set to current max rate, even if we are actually checking slower/faster
+		_current_update_rate = max_rate;
+	}
+
+
+
+	/* check if anything updated */
+	int ret = ::poll(_poll_fds, _poll_fds_num, 0);
+
+
+	/* this would be bad... */
+	if (ret < 0) {
+		DEVICE_LOG("poll error %d", errno);
+
+	} else if (ret == 0) {
+		/* timeout: no control data, switch to failsafe values */
+//		warnx("no PWM: failsafe");
+		//printf("timeout\n");
+
+	} else {
+
+		/* get controls for required topics */
+		unsigned poll_id = 0;
+
+		for (unsigned i = 0; i < actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS; i++) {
+			if (_control_subs[i] > 0) {
+				if (_poll_fds[poll_id].revents & POLLIN) {
+					orb_copy(_control_topics[i], _control_subs[i], &_controls[i]);
+
+				}
+
+				poll_id++;
+			}
+		}
+
+		size_t num_outputs = 0;
+
+		switch (_mode) {
+		case MODE_2PWM:
+			num_outputs = 2;
+			break;
+
+		case MODE_4PWM:
+			num_outputs = 4;
+			break;
+
+		case MODE_6PWM:
+			num_outputs = 6;
+			break;
+
+		case MODE_8PWM:
+			num_outputs = 8;
+			break;
+
+		default:
+			num_outputs = 0;
+			break;
+		}
+
+		/* can we mix? */
+		if (IS_armed && _mixers != nullptr) {
+
+
+			/* do mixing */
+			num_outputs = _mixers->mix(&_outputs.output[0], num_outputs, NULL);
+			_outputs.noutputs = num_outputs;
+			_outputs.timestamp = hrt_absolute_time();
+
+			/* disable unused ports by setting their output to NaN */
+			for (size_t i = 0; i < sizeof(_outputs.output) / sizeof(_outputs.output[0]); i++) {
+				if (i >= num_outputs) {
+					_outputs.output[i] = NAN;
+				}
+			}
+
+			/* iterate actuators */
+			for (unsigned i = 0; i < num_outputs; i++) {
+				/* last resort: catch NaN, INF and out-of-band errors */
+				if (i < _outputs.noutputs &&
+				    PX4_ISFINITE(_outputs.output[i]) &&
+				    _outputs.output[i] >= -1.0f &&
+				    _outputs.output[i] <= 1.0f) {
+					/* scale for PWM output 1000 - 2000us */
+					_outputs.output[i] = 1600 + (350 * _outputs.output[i]);
+
+				} else {
+					/*
+					 * Value is NaN, INF or out of band - set to the minimum value.
+					 * This will be clearly visible on the servo status and will limit the risk of accidentally
+					 * spinning motors. It would be deadly in flight.
+					 */
+					_outputs.output[i] = 900;
+				}
+			}
+
+		} else {
+
+			_outputs.noutputs = num_outputs;
+			_outputs.timestamp = hrt_absolute_time();
+
+			for (unsigned i = 0; i < num_outputs; i++) {
+
+				_outputs.output[i] = 900;
+
+			}
+
+			for (size_t i = 0; i < sizeof(_outputs.output) / sizeof(_outputs.output[0]); i++) {
+				if (i >= num_outputs) {
+					_outputs.output[i] = NAN;
+				}
+			}
+
+		}
+
+		float motor_out[TAP_ESC_MAX_MOTOR_NUM];
+		motor_out[0] = _outputs.output[0];
+		motor_out[1] = _outputs.output[1];
+		motor_out[2] = _outputs.output[1];
+		motor_out[3] = 900;
+		send_esc_outputs(motor_out, 4);
+		read_data_from_uart();
+
+		if (parse_tap_esc_feedback(&uartbuf, &feed_back_packet) == true) {
+			if (feed_back_packet.msg_id == ESCBUS_MSG_ID_RUN_INFO) {
+				feed_back_data = *(ESC_FEEDBACK_DATA *)feed_back_packet.data;
+				_esc_feedback.esc_rpm[feed_back_data.ESC_ID] = feed_back_data.speed;
+				_esc_feedback.esc_voltage[feed_back_data.ESC_ID] = feed_back_data.voltage;
+				_esc_feedback.esc_state[feed_back_data.ESC_ID] = feed_back_data.ESC_STATUS;
+				// printf("vol is %d\n",feed_back_data.voltage );
+				// printf("speed is %d\n",feed_back_data.speed );
+				orb_publish(ORB_ID(tap_esc_report), _esc_feedback_pub, &_esc_feedback);
+			}
+
+
+		}
+
+		/* and publish for anyone that cares to see */
+		orb_publish(ORB_ID(actuator_outputs), _outputs_pub, &_outputs);
+
+
+		/* overwrite outputs in case of lockdown with disarmed PWM values */
+
+	}
+
+	bool updated;
+
+	orb_check(_armed_sub, &updated);
+
+	if (updated) {
+		orb_copy(ORB_ID(actuator_armed), _armed_sub, &_armed);
+		/* do not obey the lockdown value, as lockdown is for PWMSim */
+		IS_armed = _armed.armed;
+	}
+
+
+}
+
+void TAP_ESC::work_stop()
+{
+	for (unsigned i = 0; i < actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS; i++) {
+		if (_control_subs[i] > 0) {
+			::close(_control_subs[i]);
+			_control_subs[i] = -1;
+		}
+	}
+
+	::close(_armed_sub);
+
+	DEVICE_LOG("stopping");
+	_initialized = false;
+}
+
+int
+TAP_ESC::control_callback(uintptr_t handle,
+			  uint8_t control_group,
+			  uint8_t control_index,
+			  float &input)
+{
+	const actuator_controls_s *controls = (actuator_controls_s *)handle;
+
+	input = controls[control_group].control[control_index];
+
+	/* limit control input */
+	if (input > 1.0f) {
+		input = 1.0f;
+
+	} else if (input < -1.0f) {
+		input = -1.0f;
+	}
+
+	/* motor spinup phase - lock throttle to zero */
+	// if (_pwm_limit.state == PWM_LIMIT_STATE_RAMP) {
+	// 	if ((control_group == actuator_controls_s::GROUP_INDEX_ATTITUDE ||
+	// 	     control_group == actuator_controls_s::GROUP_INDEX_ATTITUDE_ALTERNATE) &&
+	// 	    control_index == actuator_controls_s::INDEX_THROTTLE) {
+	// 		/* limit the throttle output to zero during motor spinup,
+	// 		 * as the motors cannot follow any demand yet
+	// 		 */
+	// 		input = 0.0f;
+	// 	}
+	// }
+
+	/* throttle not arming - mark throttle input as invalid */
+	if (_armed.prearmed && !_armed.armed) {
+		if ((control_group == actuator_controls_s::GROUP_INDEX_ATTITUDE ||
+		     control_group == actuator_controls_s::GROUP_INDEX_ATTITUDE_ALTERNATE) &&
+		    control_index == actuator_controls_s::INDEX_THROTTLE) {
+			/* set the throttle to an invalid value */
+			input = NAN_VALUE;
+		}
+	}
+
+	return 0;
+}
+
+int
+TAP_ESC::ioctl(file *filp, int cmd, unsigned long arg)
+{
+	int ret = OK;
+
+	switch (cmd) {
+
+	case MIXERIOCRESET:
+		if (_mixers != nullptr) {
+			delete _mixers;
+			_mixers = nullptr;
+			_groups_required = 0;
+		}
+
+		break;
+
+	case MIXERIOCLOADBUF: {
+			const char *buf = (const char *)arg;
+			unsigned buflen = strnlen(buf, 1024);
+
+			if (_mixers == nullptr) {
+				_mixers = new MixerGroup(control_callback, (uintptr_t)_controls);
+			}
+
+			if (_mixers == nullptr) {
+				_groups_required = 0;
+				ret = -ENOMEM;
+
+			} else {
+
+				ret = _mixers->load_from_buf(buf, buflen);
+
+				if (ret != 0) {
+					DEVICE_DEBUG("mixer load failed with %d", ret);
+					delete _mixers;
+					_mixers = nullptr;
+					_groups_required = 0;
+					ret = -EINVAL;
+
+				} else {
+
+					_mixers->groups_required(_groups_required);
+				}
+			}
+
+			break;
+		}
+
+	default:
+		ret = -ENOTTY;
+		break;
+	}
+
+
+
+	return ret;
+}
+
+namespace tap_esc
+{
+
+
+volatile bool _task_should_exit = false; // flag indicating if tap_esc task should exit
+static char _device[32] = {};
+static bool _is_running = false;         // flag indicating if tap_esc app is running
+static px4_task_t _task_handle = -1;     // handle to the task main thread
+
+static bool _flow_control_enabled = false;
+
+void usage();
+
+void start();
+void stop();
+int tap_esc_start(void);
+int tap_esc_stop(void);
+
+void task_main_trampoline(int argc, char *argv[]);
+
+void task_main(int argc, char *argv[]);
+
+int initialise_uart();
+
+int deinitialize_uart();
+
+int enable_flow_control(bool enabled);
+
+int tap_esc_start(void)
+{
+	int ret = OK;
+
+	if (tap_esc == nullptr) {
+
+		tap_esc = new TAP_ESC;
+
+		if (tap_esc == nullptr) {
+			ret = -ENOMEM;
+
+		} else {
+			ret = tap_esc->init();
+
+			if (ret != OK) {
+				delete tap_esc;
+				tap_esc = nullptr;
+			}
+		}
+	}
+
+	return ret;
+}
+
+int tap_esc_stop(void)
+{
+	int ret = OK;
+
+	if (tap_esc != nullptr) {
+
+		delete tap_esc;
+		tap_esc = nullptr;
+	}
+
+	return ret;
+}
+
+int initialise_uart()
+{
+	// open uart
+	_uart_fd = px4_open(_device, O_RDWR | O_NOCTTY | O_NONBLOCK);
+	int termios_state = -1;
+
+	if (_uart_fd < 0) {
+		PX4_ERR("failed to open uart device!");
+		return -1;
+	}
+
+	// set baud rate
+	int speed = B250000;
+	struct termios uart_config;
+	tcgetattr(_uart_fd, &uart_config);
+
+	// clear ONLCR flag (which appends a CR for every LF)
+	uart_config.c_oflag &= ~ONLCR;
+
+	// set baud rate
+	if (cfsetispeed(&uart_config, speed) < 0 || cfsetospeed(&uart_config, speed) < 0) {
+		warnx("ERR SET BAUD %s: %d\n", _device, termios_state);
+		::close(_uart_fd);
+		return -1;
+	}
+
+	if ((termios_state = tcsetattr(_uart_fd, TCSANOW, &uart_config)) < 0) {
+		PX4_WARN("ERR SET CONF %s\n", _device);
+		px4_close(_uart_fd);
+		return -1;
+	}
+
+	// setup output flow control
+	if (enable_flow_control(false)) {
+		PX4_WARN("hardware flow disable failed");
+	}
+
+	return _uart_fd;
+}
+
+int enable_flow_control(bool enabled)
+{
+	struct termios uart_config;
+
+	int ret = tcgetattr(_uart_fd, &uart_config);
+
+	if (enabled) {
+		uart_config.c_cflag |= CRTSCTS;
+
+	} else {
+		uart_config.c_cflag &= ~CRTSCTS;
+	}
+
+	ret = tcsetattr(_uart_fd, TCSANOW, &uart_config);
+
+	if (!ret) {
+		_flow_control_enabled = enabled;
+	}
+
+	return ret;
+}
+
+int deinitialize_uart()
+{
+	return close(_uart_fd);
+}
+
+void task_main(int argc, char *argv[])
+{
+
+	_is_running = true;
+
+	if (initialise_uart() < 0) {
+		PX4_ERR("Failed to initialize UART.");
+
+		while (_task_should_exit == false) {
+			usleep(100000);
+		}
+
+		_is_running = false;
+		return;
+	}
+
+	if (tap_esc_start() != OK) {
+		PX4_ERR("Failed to start TAP_ESC.");
+	}
+
+
+	// Main loop
+	while (!_task_should_exit) {
+
+		tap_esc->cycle();
+
+	}
+
+
+	_is_running = false;
+}
+
+void task_main_trampoline(int argc, char *argv[])
+{
+	task_main(argc, argv);
+}
+
+void start()
+{
+	ASSERT(_task_handle == -1);
+
+	_task_should_exit = false;
+
+	/* start the task */
+	_task_handle = px4_task_spawn_cmd("tap_esc_main",
+					  SCHED_DEFAULT,
+					  SCHED_PRIORITY_MAX,
+					  1500,
+					  (px4_main_t)&task_main_trampoline,
+					  nullptr);
+
+	if (_task_handle < 0) {
+		warn("task start failed");
+		return;
+	}
+}
+
+void stop()
+{
+	_task_should_exit = true;
+
+	while (_is_running) {
+		usleep(200000);
+		PX4_INFO("tap_esc_stop");
+	}
+
+	tap_esc_stop();
+	deinitialize_uart();
+	_task_handle = -1;
+}
+
+void usage()
+{
+	PX4_INFO("usage: tap_esc start -d /dev/ttyS2");
+	PX4_INFO("       tap_esc stop");
+	PX4_INFO("       tap_esc status");
+}
+
+} // namespace tap_esc
+
+// driver 'main' command
+extern "C" __EXPORT int tap_esc_main(int argc, char *argv[]);
+
+int tap_esc_main(int argc, char *argv[])
+{
+	const char *device = nullptr;
+	int ch;
+	int myoptind = 1;
+	const char *myoptarg = nullptr;
+
+	char *verb = nullptr;
+
+	if (argc >= 2) {
+		verb = argv[1];
+	}
+
+	while ((ch = px4_getopt(argc, argv, "d:", &myoptind, &myoptarg)) != EOF) {
+		switch (ch) {
+		case 'd':
+			device = myoptarg;
+			strncpy(tap_esc::_device, device, strlen(device));
+			break;
+		}
+	}
+
+	// Start/load the driver.
+	if (!strcmp(verb, "start")) {
+		if (tap_esc::_is_running) {
+			PX4_WARN("tap_esc already running");
+			return 1;
+		}
+
+		// Check on required arguments
+		if (device == nullptr || strlen(device) == 0) {
+			tap_esc::usage();
+			return 1;
+		}
+
+		tap_esc::start();
+	}
+
+	else if (!strcmp(verb, "stop")) {
+		if (!tap_esc::_is_running) {
+			PX4_WARN("tap_esc is not running");
+			return 1;
+		}
+
+		tap_esc::stop();
+	}
+
+	else if (!strcmp(verb, "status")) {
+		PX4_WARN("tap_esc is %s", tap_esc::_is_running ? "running" : "not running");
+		return 0;
+
+	} else {
+		tap_esc::usage();
+		return 1;
+	}
+
+	return 0;
+}

From 1f8b75c9f3d5d241479a6581a9e6bbae85b3d2bc Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Thu, 23 Jun 2016 14:19:12 +0200
Subject: [PATCH 0789/1230] Enable it in build

---
 cmake/configs/nuttx_px4fmu-v2_default.cmake | 1 +
 1 file changed, 1 insertion(+)

diff --git a/cmake/configs/nuttx_px4fmu-v2_default.cmake b/cmake/configs/nuttx_px4fmu-v2_default.cmake
index 1699e6df9d..7cba1d8516 100644
--- a/cmake/configs/nuttx_px4fmu-v2_default.cmake
+++ b/cmake/configs/nuttx_px4fmu-v2_default.cmake
@@ -48,6 +48,7 @@ set(config_module_list
 	drivers/bst
 	drivers/snapdragon_rc_pwm
 	drivers/lis3mdl
+	drivers/tap_esc
 
 	#
 	# System commands

From de1c865881a9addac696ee9fc53340aee171e990 Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Thu, 23 Jun 2016 18:25:51 +0200
Subject: [PATCH 0790/1230] Move TAP ESC to FMUv4 to save flash on FMUv2

---
 cmake/configs/nuttx_px4fmu-v2_default.cmake | 1 -
 cmake/configs/nuttx_px4fmu-v4_default.cmake | 1 +
 2 files changed, 1 insertion(+), 1 deletion(-)

diff --git a/cmake/configs/nuttx_px4fmu-v2_default.cmake b/cmake/configs/nuttx_px4fmu-v2_default.cmake
index 7cba1d8516..1699e6df9d 100644
--- a/cmake/configs/nuttx_px4fmu-v2_default.cmake
+++ b/cmake/configs/nuttx_px4fmu-v2_default.cmake
@@ -48,7 +48,6 @@ set(config_module_list
 	drivers/bst
 	drivers/snapdragon_rc_pwm
 	drivers/lis3mdl
-	drivers/tap_esc
 
 	#
 	# System commands
diff --git a/cmake/configs/nuttx_px4fmu-v4_default.cmake b/cmake/configs/nuttx_px4fmu-v4_default.cmake
index 2b2814d2b4..d9c6e4fb52 100644
--- a/cmake/configs/nuttx_px4fmu-v4_default.cmake
+++ b/cmake/configs/nuttx_px4fmu-v4_default.cmake
@@ -48,6 +48,7 @@ set(config_module_list
 	drivers/bmp280
 	drivers/bma180
 	drivers/bmi160
+	drivers/tap_esc
 
 	#
 	# System commands

From b04e2526a130484e09c97e57c90bb5098479bba5 Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Thu, 23 Jun 2016 18:26:08 +0200
Subject: [PATCH 0791/1230] Fix compile errors for tap ESC

---
 src/drivers/tap_esc/drv_tap_esc.h |  2 +-
 src/drivers/tap_esc/tap_esc.cpp   | 67 ++++++++++++++++++-------------
 2 files changed, 39 insertions(+), 30 deletions(-)

diff --git a/src/drivers/tap_esc/drv_tap_esc.h b/src/drivers/tap_esc/drv_tap_esc.h
index 1647aa17e9..01cc7e078e 100755
--- a/src/drivers/tap_esc/drv_tap_esc.h
+++ b/src/drivers/tap_esc/drv_tap_esc.h
@@ -101,7 +101,7 @@ typedef  struct {
 	uint8_t head;
 	uint8_t tail;
 	uint8_t dat_cnt;
-	uint8_t esc_feedbacck_buf[128];
+	uint8_t esc_feedback_buf[128];
 } ESC_UART_BUF;
 
 #pragma pack(pop)
diff --git a/src/drivers/tap_esc/tap_esc.cpp b/src/drivers/tap_esc/tap_esc.cpp
index 7abf297ef3..e4b724bdc3 100644
--- a/src/drivers/tap_esc/tap_esc.cpp
+++ b/src/drivers/tap_esc/tap_esc.cpp
@@ -45,7 +45,7 @@
 #include <uORB/topics/actuator_outputs.h>
 #include <uORB/topics/actuator_armed.h>
 #include <uORB/topics/input_rc.h>
-#include <uORB/topics/tap_esc_report.h>
+#include <uORB/topics/esc_status.h>
 
 #include <drivers/drv_hrt.h>
 #include <drivers/drv_mixer.h>
@@ -55,6 +55,10 @@
 
 #define NAN_VALUE	(0.0f/0.0f)
 
+#ifndef B250000
+#define B250000 250000
+#endif
+
 #define VOLTAGE_SENSOR_HAVE
 
 #include "drv_tap_esc.h"
@@ -107,7 +111,7 @@ private:
 	orb_id_t	_control_topics[actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS];
 
 	orb_advert_t        _esc_feedback_pub = nullptr;
-	tap_esc_report_s      _esc_feedback;
+	esc_status_s      _esc_feedback;
 
 	MixerGroup	*_mixers;
 	uint32_t	_groups_required;
@@ -168,7 +172,7 @@ TAP_ESC::TAP_ESC():
 	uartbuf.head = 0;
 	uartbuf.tail = 0;
 	uartbuf.dat_cnt = 0;
-	memset(uartbuf.esc_feedbacck_buf, 0, sizeof(uartbuf.esc_feedbacck_buf));
+	memset(uartbuf.esc_feedback_buf, 0, sizeof(uartbuf.esc_feedback_buf));
 }
 
 TAP_ESC::~TAP_ESC()
@@ -330,7 +334,7 @@ void TAP_ESC::read_data_from_uart()
 
 	if (len > 0 && (uartbuf.dat_cnt + len < 128)) {
 		for (int i = 0; i < len; i++) {
-			uartbuf.esc_feedbacck_buf[uartbuf.tail++] = tmp_serial_buf[i];
+			uartbuf.esc_feedback_buf[uartbuf.tail++] = tmp_serial_buf[i];
 			uartbuf.dat_cnt++;
 
 			if (uartbuf.tail >= 128) {
@@ -349,15 +353,15 @@ bool TAP_ESC:: parse_tap_esc_feedback(ESC_UART_BUF *serial_buf, ESC_FEEDBACK_PAC
 	if (serial_buf->dat_cnt > 0) {
 		for (int i = 0; i < serial_buf->dat_cnt; i++) {
 			switch (state) {
-			case HEAD: if (serial_buf->esc_feedbacck_buf[serial_buf->head] == 0xFE) {
+			case HEAD: if (serial_buf->esc_feedback_buf[serial_buf->head] == 0xFE) {
 					packetdata->head = 0xFE; //just_keep the format
 					state = LEN;
 				}
 
 				break;
 
-			case LEN:  if (serial_buf->esc_feedbacck_buf[serial_buf->head] < 100) {
-					packetdata->len = serial_buf->esc_feedbacck_buf[serial_buf->head];
+			case LEN:  if (serial_buf->esc_feedback_buf[serial_buf->head] < 100) {
+					packetdata->len = serial_buf->esc_feedback_buf[serial_buf->head];
 					state = ID;
 
 				} else {
@@ -366,8 +370,8 @@ bool TAP_ESC:: parse_tap_esc_feedback(ESC_UART_BUF *serial_buf, ESC_FEEDBACK_PAC
 
 				break;
 
-			case ID:   if (serial_buf->esc_feedbacck_buf[serial_buf->head] < ESCBUS_MSG_ID_MAX_NUM) {
-					packetdata->msg_id = serial_buf->esc_feedbacck_buf[serial_buf->head];
+			case ID:   if (serial_buf->esc_feedback_buf[serial_buf->head] < ESCBUS_MSG_ID_MAX_NUM) {
+					packetdata->msg_id = serial_buf->esc_feedback_buf[serial_buf->head];
 					data_index = 0;
 					state = DATA;
 
@@ -377,7 +381,7 @@ bool TAP_ESC:: parse_tap_esc_feedback(ESC_UART_BUF *serial_buf, ESC_FEEDBACK_PAC
 
 				break;
 
-			case DATA: packetdata->data[data_index++] = serial_buf->esc_feedbacck_buf[serial_buf->head];
+			case DATA: packetdata->data[data_index++] = serial_buf->esc_feedback_buf[serial_buf->head];
 
 				if (data_index >= packetdata->len) {
 
@@ -387,8 +391,8 @@ bool TAP_ESC:: parse_tap_esc_feedback(ESC_UART_BUF *serial_buf, ESC_FEEDBACK_PAC
 
 				break;
 
-			case CRC: if (crc_data_cal == serial_buf->esc_feedbacck_buf[serial_buf->head]) {
-					packetdata->crc_data = serial_buf->esc_feedbacck_buf[serial_buf->head];
+			case CRC: if (crc_data_cal == serial_buf->esc_feedback_buf[serial_buf->head]) {
+					packetdata->crc_data = serial_buf->esc_feedback_buf[serial_buf->head];
 
 					if (++serial_buf->head >= 128) {
 						serial_buf->head = 0;
@@ -430,7 +434,7 @@ TAP_ESC::cycle()
 		_current_update_rate = 0;
 		/* advertise the mixed control outputs, insist on the first group output */
 		_outputs_pub = orb_advertise(ORB_ID(actuator_outputs), &_outputs);
-		_esc_feedback_pub = orb_advertise(ORB_ID(tap_esc_report), &_esc_feedback);
+		_esc_feedback_pub = orb_advertise(ORB_ID(esc_report), &_esc_feedback);
 		_armed_sub = orb_subscribe(ORB_ID(actuator_armed));
 		_initialized = true;
 	}
@@ -579,26 +583,31 @@ TAP_ESC::cycle()
 
 		}
 
+		const unsigned esc_count = 4;
+
 		float motor_out[TAP_ESC_MAX_MOTOR_NUM];
 		motor_out[0] = _outputs.output[0];
 		motor_out[1] = _outputs.output[1];
 		motor_out[2] = _outputs.output[1];
 		motor_out[3] = 900;
-		send_esc_outputs(motor_out, 4);
+		send_esc_outputs(motor_out, esc_count);
 		read_data_from_uart();
 
 		if (parse_tap_esc_feedback(&uartbuf, &feed_back_packet) == true) {
 			if (feed_back_packet.msg_id == ESCBUS_MSG_ID_RUN_INFO) {
 				feed_back_data = *(ESC_FEEDBACK_DATA *)feed_back_packet.data;
-				_esc_feedback.esc_rpm[feed_back_data.ESC_ID] = feed_back_data.speed;
-				_esc_feedback.esc_voltage[feed_back_data.ESC_ID] = feed_back_data.voltage;
-				_esc_feedback.esc_state[feed_back_data.ESC_ID] = feed_back_data.ESC_STATUS;
+				_esc_feedback.esc[feed_back_data.ESC_ID].esc_rpm = feed_back_data.speed;
+				_esc_feedback.esc[feed_back_data.ESC_ID].esc_voltage = feed_back_data.voltage;
+				_esc_feedback.esc[feed_back_data.ESC_ID].esc_state = feed_back_data.ESC_STATUS;
 				// printf("vol is %d\n",feed_back_data.voltage );
 				// printf("speed is %d\n",feed_back_data.speed );
-				orb_publish(ORB_ID(tap_esc_report), _esc_feedback_pub, &_esc_feedback);
+
+				_esc_feedback.esc_connectiontype = esc_status_s::ESC_CONNECTION_TYPE_SERIAL;
+				_esc_feedback.counter++;
+				_esc_feedback.esc_count = esc_count;
+
+				orb_publish(ORB_ID(esc_status), _esc_feedback_pub, &_esc_feedback);
 			}
-
-
 		}
 
 		/* and publish for anyone that cares to see */
@@ -738,7 +747,7 @@ TAP_ESC::ioctl(file *filp, int cmd, unsigned long arg)
 	return ret;
 }
 
-namespace tap_esc
+namespace tap_esc_drv
 {
 
 
@@ -970,42 +979,42 @@ int tap_esc_main(int argc, char *argv[])
 		switch (ch) {
 		case 'd':
 			device = myoptarg;
-			strncpy(tap_esc::_device, device, strlen(device));
+			strncpy(tap_esc_drv::_device, device, strlen(device));
 			break;
 		}
 	}
 
 	// Start/load the driver.
 	if (!strcmp(verb, "start")) {
-		if (tap_esc::_is_running) {
+		if (tap_esc_drv::_is_running) {
 			PX4_WARN("tap_esc already running");
 			return 1;
 		}
 
 		// Check on required arguments
 		if (device == nullptr || strlen(device) == 0) {
-			tap_esc::usage();
+			tap_esc_drv::usage();
 			return 1;
 		}
 
-		tap_esc::start();
+		tap_esc_drv::start();
 	}
 
 	else if (!strcmp(verb, "stop")) {
-		if (!tap_esc::_is_running) {
+		if (!tap_esc_drv::_is_running) {
 			PX4_WARN("tap_esc is not running");
 			return 1;
 		}
 
-		tap_esc::stop();
+		tap_esc_drv::stop();
 	}
 
 	else if (!strcmp(verb, "status")) {
-		PX4_WARN("tap_esc is %s", tap_esc::_is_running ? "running" : "not running");
+		PX4_WARN("tap_esc is %s", tap_esc_drv::_is_running ? "running" : "not running");
 		return 0;
 
 	} else {
-		tap_esc::usage();
+		tap_esc_drv::usage();
 		return 1;
 	}
 

From 34c0d3e99a2410abe191402a42e208d7594147c1 Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Thu, 23 Jun 2016 18:38:53 +0200
Subject: [PATCH 0792/1230] Add TAP to vendor list

---
 msg/esc_status.msg | 1 +
 1 file changed, 1 insertion(+)

diff --git a/msg/esc_status.msg b/msg/esc_status.msg
index 13c0a925f8..5148642459 100644
--- a/msg/esc_status.msg
+++ b/msg/esc_status.msg
@@ -3,6 +3,7 @@ uint8 CONNECTED_ESC_MAX = 8				# The number of ESCs supported. Current (Q2/2013)
 uint8 ESC_VENDOR_GENERIC = 0				# generic ESC
 uint8 ESC_VENDOR_MIKROKOPTER = 1			# Mikrokopter
 uint8 ESC_VENDOR_GRAUPNER_HOTT = 2			# Graupner HoTT ESC
+uint8 ESC_VENDOR_TAP = 3				# TAP ESC
 
 uint8 ESC_CONNECTION_TYPE_PPM = 0			# Traditional PPM ESC
 uint8 ESC_CONNECTION_TYPE_SERIAL = 1			# Serial Bus connected ESC

From bf0b3c15857f6d9624d9aef5ac569e124d95796d Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Thu, 23 Jun 2016 18:39:16 +0200
Subject: [PATCH 0793/1230] More complete ESC feedback, ensure to include  a
 timestamp

---
 src/drivers/tap_esc/tap_esc.cpp | 4 ++++
 1 file changed, 4 insertions(+)

diff --git a/src/drivers/tap_esc/tap_esc.cpp b/src/drivers/tap_esc/tap_esc.cpp
index e4b724bdc3..397dcdd1e5 100644
--- a/src/drivers/tap_esc/tap_esc.cpp
+++ b/src/drivers/tap_esc/tap_esc.cpp
@@ -155,6 +155,7 @@ TAP_ESC::TAP_ESC():
 	_outputs_pub(nullptr),
 	_control_subs{ -1},
 	_esc_feedback_pub(nullptr),
+	_esc_feedback{},
 	_mixers(nullptr),
 	_groups_required(0),
 	_groups_subscribed(0),
@@ -599,6 +600,7 @@ TAP_ESC::cycle()
 				_esc_feedback.esc[feed_back_data.ESC_ID].esc_rpm = feed_back_data.speed;
 				_esc_feedback.esc[feed_back_data.ESC_ID].esc_voltage = feed_back_data.voltage;
 				_esc_feedback.esc[feed_back_data.ESC_ID].esc_state = feed_back_data.ESC_STATUS;
+				_esc_feedback.esc[feed_back_data.ESC_ID].esc_vendor = esc_status_s::ESC_VENDOR_TAP;
 				// printf("vol is %d\n",feed_back_data.voltage );
 				// printf("speed is %d\n",feed_back_data.speed );
 
@@ -606,6 +608,8 @@ TAP_ESC::cycle()
 				_esc_feedback.counter++;
 				_esc_feedback.esc_count = esc_count;
 
+				_esc_feedback.timestamp = hrt_absolute_time();
+
 				orb_publish(ORB_ID(esc_status), _esc_feedback_pub, &_esc_feedback);
 			}
 		}

From 89f5bd27e8ecedd57b2cc45060be3d94b9897725 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Beat=20K=C3=BCng?= <beat-kueng@gmx.net>
Date: Wed, 15 Jun 2016 12:59:39 +0200
Subject: [PATCH 0794/1230] vehicle_gps_position: use timestamp field instead
 of timestamp_position

timestamp was unused. This allows to remove timestamp_position.
---
 msg/vehicle_gps_position.msg                   |  1 -
 src/drivers/gps/devices                        |  2 +-
 src/drivers/gps/gps.cpp                        |  8 ++++----
 .../terrain_estimation/terrain_estimator.cpp   |  4 ++--
 .../attitude_estimator_ekf_main.cpp            |  4 ++--
 src/modules/commander/PreflightCheck.cpp       |  2 +-
 src/modules/commander/commander.cpp            |  4 ++--
 src/modules/ekf2/ekf2_main.cpp                 |  6 +++---
 src/modules/ekf2_replay/ekf2_replay_main.cpp   |  2 +-
 .../ekf_att_pos_estimator_main.cpp             | 10 +++++-----
 src/modules/mavlink/mavlink_messages.cpp       |  2 +-
 src/modules/mavlink/mavlink_receiver.cpp       |  2 +-
 .../position_estimator_inav_main.cpp           |  2 +-
 src/modules/uavcan/sensors/gnss.cpp            |  8 ++++----
 src/platforms/posix/drivers/gpssim/gpssim.cpp  | 18 +++++++++---------
 15 files changed, 37 insertions(+), 38 deletions(-)

diff --git a/msg/vehicle_gps_position.msg b/msg/vehicle_gps_position.msg
index ab8a92873f..4be23df42e 100644
--- a/msg/vehicle_gps_position.msg
+++ b/msg/vehicle_gps_position.msg
@@ -1,5 +1,4 @@
 # GPS position in WGS84 coordinates.
-uint64 timestamp_position	# Time of the position estimates since system start, (microseconds)
 int32 lat			# Latitude in 1E-7 degrees
 int32 lon			# Longitude in 1E-7 degrees 
 int32 alt			# Altitude in 1E-3 meters above MSL, (millimetres)
diff --git a/src/drivers/gps/devices b/src/drivers/gps/devices
index 5c1ae95655..9e7ed8a5ea 160000
--- a/src/drivers/gps/devices
+++ b/src/drivers/gps/devices
@@ -1 +1 @@
-Subproject commit 5c1ae956552c3887c5fddd303a8f242efa715333
+Subproject commit 9e7ed8a5ea85c76c5d563413df051f138b9a0622
diff --git a/src/drivers/gps/gps.cpp b/src/drivers/gps/gps.cpp
index 3e29be8425..21950bbf4c 100644
--- a/src/drivers/gps/gps.cpp
+++ b/src/drivers/gps/gps.cpp
@@ -687,7 +687,7 @@ GPS::task_main()
 	while (!_task_should_exit) {
 
 		if (_fake_gps) {
-			_report_gps_pos.timestamp_position = hrt_absolute_time();
+			_report_gps_pos.timestamp = hrt_absolute_time();
 			_report_gps_pos.lat = (int32_t)47.378301e7f;
 			_report_gps_pos.lon = (int32_t)8.538777e7f;
 			_report_gps_pos.alt = (int32_t)1200e3f;
@@ -757,7 +757,7 @@ GPS::task_main()
 					_report_gps_pos.timestamp_time = hrt_absolute_time();
 
 					/* reset the timestamps for data, because we have no data yet */
-					_report_gps_pos.timestamp_position = 0;
+					_report_gps_pos.timestamp = 0;
 					_report_gps_pos.timestamp_variance = 0;
 					_report_gps_pos.timestamp_velocity = 0;
 
@@ -960,9 +960,9 @@ GPS::print_info()
 		 _report_gps_pos.noise_per_ms,
 		 _report_gps_pos.jamming_indicator == 255 ? "YES" : "NO");
 
-	if (_report_gps_pos.timestamp_position != 0) {
+	if (_report_gps_pos.timestamp != 0) {
 		PX4_WARN("position lock: %d, satellites: %d, last update: %8.4fms ago", (int)_report_gps_pos.fix_type,
-			 _report_gps_pos.satellites_used, (double)(hrt_absolute_time() - _report_gps_pos.timestamp_position) / 1000.0);
+			 _report_gps_pos.satellites_used, (double)(hrt_absolute_time() - _report_gps_pos.timestamp) / 1000.0);
 		PX4_WARN("lat: %d, lon: %d, alt: %d", _report_gps_pos.lat, _report_gps_pos.lon, _report_gps_pos.alt);
 		PX4_WARN("vel: %.2fm/s, %.2fm/s, %.2fm/s", (double)_report_gps_pos.vel_n_m_s,
 			 (double)_report_gps_pos.vel_e_m_s, (double)_report_gps_pos.vel_d_m_s);
diff --git a/src/lib/terrain_estimation/terrain_estimator.cpp b/src/lib/terrain_estimation/terrain_estimator.cpp
index 64a747f532..20bdabf6e5 100644
--- a/src/lib/terrain_estimation/terrain_estimator.cpp
+++ b/src/lib/terrain_estimation/terrain_estimator.cpp
@@ -153,7 +153,7 @@ void TerrainEstimator::measurement_update(uint64_t time_ref, const struct vehicl
 		_distance_last = distance->current_distance;
 	}
 
-	if (gps->timestamp_position > _time_last_gps && gps->fix_type >= 3) {
+	if (gps->timestamp > _time_last_gps && gps->fix_type >= 3) {
 		matrix::Matrix<float, 1, n_x> C;
 		C(0, 1) = 1;
 
@@ -172,7 +172,7 @@ void TerrainEstimator::measurement_update(uint64_t time_ref, const struct vehicl
 		_x += K * r;
 		_P -= K * C * _P;
 
-		_time_last_gps = gps->timestamp_position;
+		_time_last_gps = gps->timestamp;
 	}
 
 	// reinitialise filter if we find bad data
diff --git a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp
index 273d159f2e..4aa6abca72 100755
--- a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp
+++ b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp
@@ -381,7 +381,7 @@ int attitude_estimator_ekf_thread_main(int argc, char *argv[])
 				if (gps_updated) {
 					orb_copy(ORB_ID(vehicle_gps_position), sub_gps, &gps);
 
-					if (gps.eph < 20.0f && hrt_elapsed_time(&gps.timestamp_position) < 1000000) {
+					if (gps.eph < 20.0f && hrt_elapsed_time(&gps.timestamp) < 1000000) {
 						mag_decl = math::radians(get_mag_declination(gps.lat / 1e7f, gps.lon / 1e7f));
 
 						/* update mag declination rotation matrix */
@@ -537,7 +537,7 @@ int attitude_estimator_ekf_thread_main(int argc, char *argv[])
 						parameters_update(&ekf_param_handles, &ekf_params);
 
 						/* update mag declination rotation matrix */
-						if (gps.eph < 20.0f && hrt_elapsed_time(&gps.timestamp_position) < 1000000) {
+						if (gps.eph < 20.0f && hrt_elapsed_time(&gps.timestamp) < 1000000) {
 							mag_decl = math::radians(get_mag_declination(gps.lat / 1e7f, gps.lon / 1e7f));
 
 						}
diff --git a/src/modules/commander/PreflightCheck.cpp b/src/modules/commander/PreflightCheck.cpp
index 3141d6d0b3..1f31586e19 100644
--- a/src/modules/commander/PreflightCheck.cpp
+++ b/src/modules/commander/PreflightCheck.cpp
@@ -366,7 +366,7 @@ static bool gnssCheck(orb_advert_t *mavlink_log_pub, bool report_fail)
 	else {
 		struct vehicle_gps_position_s gps;
 		if ( (OK != orb_copy(ORB_ID(vehicle_gps_position), gpsSub, &gps)) ||
-		    (hrt_elapsed_time(&gps.timestamp_position) > 1000000)) {
+		    (hrt_elapsed_time(&gps.timestamp) > 1000000)) {
 			success = false;
 		}
 	}
diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp
index c7521acd88..3752f56bf0 100644
--- a/src/modules/commander/commander.cpp
+++ b/src/modules/commander/commander.cpp
@@ -2167,7 +2167,7 @@ int commander_thread_main(int argc, char *argv[])
 		if (!map_projection_global_initialized()
 		    && (gps_position.eph < eph_threshold)
 		    && (gps_position.epv < epv_threshold)
-		    && hrt_elapsed_time((hrt_abstime *)&gps_position.timestamp_position) < 1e6) {
+		    && hrt_elapsed_time((hrt_abstime *)&gps_position.timestamp) < 1e6) {
 			/* set reference for global coordinates <--> local coordiantes conversion and map_projection */
 			globallocalconverter_init((double)gps_position.lat * 1.0e-7, (double)gps_position.lon * 1.0e-7,
 						  (float)gps_position.alt * 1.0e-3f, hrt_absolute_time());
@@ -2189,7 +2189,7 @@ int commander_thread_main(int argc, char *argv[])
 				}
 			}
 
-			if (gps_position.fix_type >= 3 && hrt_elapsed_time(&gps_position.timestamp_position) < FAILSAFE_DEFAULT_TIMEOUT) {
+			if (gps_position.fix_type >= 3 && hrt_elapsed_time(&gps_position.timestamp) < FAILSAFE_DEFAULT_TIMEOUT) {
 				/* handle the case where gps was regained */
 				if (status_flags.gps_failure && !gpsIsNoisy) {
 					status_flags.gps_failure = false;
diff --git a/src/modules/ekf2/ekf2_main.cpp b/src/modules/ekf2/ekf2_main.cpp
index 274d924032..d482c4d8ac 100644
--- a/src/modules/ekf2/ekf2_main.cpp
+++ b/src/modules/ekf2/ekf2_main.cpp
@@ -517,7 +517,7 @@ void Ekf2::task_main()
 		// read gps data if available
 		if (gps_updated) {
 			struct gps_message gps_msg = {};
-			gps_msg.time_usec = gps.timestamp_position;
+			gps_msg.time_usec = gps.timestamp;
 			gps_msg.lat = gps.lat;
 			gps_msg.lon = gps.lon;
 			gps_msg.alt = gps.alt;
@@ -535,7 +535,7 @@ void Ekf2::task_main()
 			//TODO add gdop to gps topic
 			gps_msg.gdop = 0.0f;
 
-			_ekf.setGpsData(gps.timestamp_position, &gps_msg);
+			_ekf.setGpsData(gps.timestamp, &gps_msg);
 		}
 
 		// only set airspeed data if condition for airspeed fusion are met
@@ -909,7 +909,7 @@ void Ekf2::task_main()
 
 			// only write gps data if we had a gps update.
 			if (gps_updated) {
-				replay.time_usec = gps.timestamp_position;
+				replay.time_usec = gps.timestamp;
 				replay.time_usec_vel = gps.timestamp_velocity;
 				replay.lat = gps.lat;
 				replay.lon = gps.lon;
diff --git a/src/modules/ekf2_replay/ekf2_replay_main.cpp b/src/modules/ekf2_replay/ekf2_replay_main.cpp
index b8de2c70b1..823c60744b 100644
--- a/src/modules/ekf2_replay/ekf2_replay_main.cpp
+++ b/src/modules/ekf2_replay/ekf2_replay_main.cpp
@@ -396,7 +396,7 @@ void Ekf2Replay::setEstimatorInput(uint8_t *data, uint8_t type)
 	} else if (type == LOG_RPL2_MSG) {
 		uint8_t *dest_ptr = (uint8_t *)&replay_part2.time_pos_usec;
 		parseMessage(data, dest_ptr, type);
-		_gps.timestamp_position = replay_part2.time_pos_usec;
+		_gps.timestamp = replay_part2.time_pos_usec;
 		_gps.timestamp_velocity = replay_part2.time_vel_usec;
 		_gps.lat = replay_part2.lat;
 		_gps.lon = replay_part2.lon;
diff --git a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp
index 5b75ab2b12..4fde0bea22 100644
--- a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp
+++ b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp
@@ -448,7 +448,7 @@ int AttitudePositionEstimatorEKF::check_filter_state()
 		// Set up height correctly
 		orb_copy(ORB_ID(sensor_baro), _baro_sub, &_baro);
 
-		initReferencePosition(_gps.timestamp_position, _gpsIsGood, lat, lon, gps_alt, _baro.altitude);
+		initReferencePosition(_gps.timestamp, _gpsIsGood, lat, lon, gps_alt, _baro.altitude);
 
 	} else if (_ekf_logging) {
 		_ekf->GetFilterState(&ekf_report);
@@ -821,7 +821,7 @@ void AttitudePositionEstimatorEKF::initializeGPS()
 
 	_ekf->InitialiseFilter(initVelNED, math::radians(lat), math::radians(lon) - M_PI, gps_alt, declination);
 
-	initReferencePosition(_gps.timestamp_position, _gpsIsGood, lat, lon, gps_alt, _baro.altitude);
+	initReferencePosition(_gps.timestamp, _gpsIsGood, lat, lon, gps_alt, _baro.altitude);
 
 #if 0
 	PX4_INFO("HOME/REF: LA %8.4f,LO %8.4f,ALT %8.2f V: %8.4f %8.4f %8.4f", lat, lon, (double)gps_alt,
@@ -1068,7 +1068,7 @@ void AttitudePositionEstimatorEKF::publishGlobalPosition()
 
 	if (!_local_pos.xy_global ||
 	    !_local_pos.v_xy_valid ||
-	    _gps.timestamp_position == 0 ||
+	    _gps.timestamp == 0 ||
 	    (dtLastGoodGPS >= POS_RESET_THRESHOLD)) {
 
 		_global_pos.eph = EPH_LARGE_VALUE;
@@ -1584,7 +1584,7 @@ void AttitudePositionEstimatorEKF::pollData()
 		if (_gpsIsGood) {
 
 			//Calculate time since last good GPS fix
-			const float dtLastGoodGPS = static_cast<float>(_gps.timestamp_position - _previousGPSTimestamp) / 1e6f;
+			const float dtLastGoodGPS = static_cast<float>(_gps.timestamp - _previousGPSTimestamp) / 1e6f;
 
 			//Stop dead-reckoning mode
 			if (_global_pos.dead_reckoning) {
@@ -1643,7 +1643,7 @@ void AttitudePositionEstimatorEKF::pollData()
 
 			// PX4_INFO("vel: %8.4f pos: %8.4f", _gps.s_variance_m_s, _gps.p_variance_m);
 
-			_previousGPSTimestamp = _gps.timestamp_position;
+			_previousGPSTimestamp = _gps.timestamp;
 
 		}
 	}
diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp
index 9cd58d66d8..125530fb98 100644
--- a/src/modules/mavlink/mavlink_messages.cpp
+++ b/src/modules/mavlink/mavlink_messages.cpp
@@ -1066,7 +1066,7 @@ protected:
 		if (_gps_sub->update(&_gps_time, &gps)) {
 			mavlink_gps_raw_int_t msg = {};
 
-			msg.time_usec = gps.timestamp_position;
+			msg.time_usec = gps.timestamp;
 			msg.fix_type = gps.fix_type;
 			msg.lat = gps.lat;
 			msg.lon = gps.lon;
diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp
index c54c225d40..37530bb13a 100644
--- a/src/modules/mavlink/mavlink_receiver.cpp
+++ b/src/modules/mavlink/mavlink_receiver.cpp
@@ -1784,7 +1784,7 @@ MavlinkReceiver::handle_message_hil_gps(mavlink_message_t *msg)
 	hil_gps.timestamp_time = timestamp;
 	hil_gps.time_utc_usec = gps.time_usec;
 
-	hil_gps.timestamp_position = timestamp;
+	hil_gps.timestamp = timestamp;
 	hil_gps.lat = gps.lat;
 	hil_gps.lon = gps.lon;
 	hil_gps.alt = gps.alt;
diff --git a/src/modules/position_estimator_inav/position_estimator_inav_main.cpp b/src/modules/position_estimator_inav/position_estimator_inav_main.cpp
index 1677a91d3c..79e070c614 100644
--- a/src/modules/position_estimator_inav/position_estimator_inav_main.cpp
+++ b/src/modules/position_estimator_inav/position_estimator_inav_main.cpp
@@ -944,7 +944,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
 		}
 
 		/* check for timeout on GPS topic */
-		if (gps_valid && (t > (gps.timestamp_position + gps_topic_timeout))) {
+		if (gps_valid && (t > (gps.timestamp + gps_topic_timeout))) {
 			gps_valid = false;
 			warnx("GPS timeout");
 			mavlink_log_info(&mavlink_log_pub, "[inav] GPS timeout");
diff --git a/src/modules/uavcan/sensors/gnss.cpp b/src/modules/uavcan/sensors/gnss.cpp
index d80b4b425d..bf56539b7a 100644
--- a/src/modules/uavcan/sensors/gnss.cpp
+++ b/src/modules/uavcan/sensors/gnss.cpp
@@ -104,12 +104,12 @@ void UavcanGnssBridge::gnss_fix_sub_cb(const uavcan::ReceivedDataStructure<uavca
 	 * to use an independent time source (based on hardware TIM5) instead of HRT.
 	 * The proper solution is to be developed.
 	 */
-	report.timestamp_position = hrt_absolute_time();
+	report.timestamp = hrt_absolute_time();
 	report.lat = msg.latitude_deg_1e8 / 10;
 	report.lon = msg.longitude_deg_1e8 / 10;
 	report.alt = msg.height_msl_mm;
 
-	report.timestamp_variance = report.timestamp_position;
+	report.timestamp_variance = report.timestamp;
 
 
 	// Check if the msg contains valid covariance information
@@ -163,7 +163,7 @@ void UavcanGnssBridge::gnss_fix_sub_cb(const uavcan::ReceivedDataStructure<uavca
 
 	report.fix_type = msg.status;
 
-	report.timestamp_velocity = report.timestamp_position;
+	report.timestamp_velocity = report.timestamp;
 	report.vel_n_m_s = msg.ned_velocity[0];
 	report.vel_e_m_s = msg.ned_velocity[1];
 	report.vel_d_m_s = msg.ned_velocity[2];
@@ -172,7 +172,7 @@ void UavcanGnssBridge::gnss_fix_sub_cb(const uavcan::ReceivedDataStructure<uavca
 	report.cog_rad = atan2f(report.vel_e_m_s, report.vel_n_m_s);
 	report.vel_ned_valid = true;
 
-	report.timestamp_time = report.timestamp_position;
+	report.timestamp_time = report.timestamp;
 	report.time_utc_usec = uavcan::UtcTime(msg.gnss_timestamp).toUSec();	// Convert to microseconds
 
 	report.satellites_used = msg.sats_used;
diff --git a/src/platforms/posix/drivers/gpssim/gpssim.cpp b/src/platforms/posix/drivers/gpssim/gpssim.cpp
index e7f6387308..258e1b054a 100644
--- a/src/platforms/posix/drivers/gpssim/gpssim.cpp
+++ b/src/platforms/posix/drivers/gpssim/gpssim.cpp
@@ -278,14 +278,14 @@ GPSSIM::receive(int timeout)
 	simulator::RawGPSData gps;
 	sim->getGPSSample((uint8_t *)&gps, sizeof(gps));
 
-	_report_gps_pos.timestamp_position = hrt_absolute_time();
+	_report_gps_pos.timestamp = hrt_absolute_time();
 	_report_gps_pos.lat = gps.lat;
 	_report_gps_pos.lon = gps.lon;
 	_report_gps_pos.alt = gps.alt;
-	_report_gps_pos.timestamp_variance = _report_gps_pos.timestamp_position;
+	_report_gps_pos.timestamp_variance = _report_gps_pos.timestamp;
 	_report_gps_pos.eph = (float)gps.eph * 1e-2f;
 	_report_gps_pos.epv = (float)gps.epv * 1e-2f;
-	_report_gps_pos.timestamp_velocity = _report_gps_pos.timestamp_position;
+	_report_gps_pos.timestamp_velocity = _report_gps_pos.timestamp;
 	_report_gps_pos.vel_m_s = (float)(gps.vel) / 100.0f;
 	_report_gps_pos.vel_n_m_s = (float)(gps.vn) / 100.0f;
 	_report_gps_pos.vel_e_m_s = (float)(gps.ve) / 100.0f;
@@ -306,17 +306,17 @@ GPSSIM::task_main()
 	while (!_task_should_exit) {
 
 		if (_fake_gps) {
-			_report_gps_pos.timestamp_position = hrt_absolute_time();
+			_report_gps_pos.timestamp = hrt_absolute_time();
 			_report_gps_pos.lat = (int32_t)47.378301e7f;
 			_report_gps_pos.lon = (int32_t)8.538777e7f;
 			_report_gps_pos.alt = (int32_t)1200e3f;
-			_report_gps_pos.timestamp_variance = _report_gps_pos.timestamp_position;
+			_report_gps_pos.timestamp_variance = _report_gps_pos.timestamp;
 			_report_gps_pos.s_variance_m_s = 10.0f;
 			_report_gps_pos.c_variance_rad = 0.1f;
 			_report_gps_pos.fix_type = 3;
 			_report_gps_pos.eph = 0.9f;
 			_report_gps_pos.epv = 1.8f;
-			_report_gps_pos.timestamp_velocity = _report_gps_pos.timestamp_position;
+			_report_gps_pos.timestamp_velocity = _report_gps_pos.timestamp;
 			_report_gps_pos.vel_n_m_s = 0.0f;
 			_report_gps_pos.vel_e_m_s = 0.0f;
 			_report_gps_pos.vel_d_m_s = 0.0f;
@@ -342,7 +342,7 @@ GPSSIM::task_main()
 			//Publish initial report that we have access to a GPS
 			//Make sure to clear any stale data in case driver is reset
 			memset(&_report_gps_pos, 0, sizeof(_report_gps_pos));
-			_report_gps_pos.timestamp_position = hrt_absolute_time();
+			_report_gps_pos.timestamp = hrt_absolute_time();
 			_report_gps_pos.timestamp_variance = hrt_absolute_time();
 			_report_gps_pos.timestamp_velocity = hrt_absolute_time();
 			_report_gps_pos.timestamp_time = hrt_absolute_time();
@@ -413,9 +413,9 @@ GPSSIM::print_info()
 		 _report_gps_pos.noise_per_ms,
 		 _report_gps_pos.jamming_indicator == 255 ? "YES" : "NO");
 
-	if (_report_gps_pos.timestamp_position != 0) {
+	if (_report_gps_pos.timestamp != 0) {
 		PX4_INFO("position lock: %dD, satellites: %d, last update: %8.4fms ago", (int)_report_gps_pos.fix_type,
-			 _report_gps_pos.satellites_used, (double)(hrt_absolute_time() - _report_gps_pos.timestamp_position) / 1000.0);
+			 _report_gps_pos.satellites_used, (double)(hrt_absolute_time() - _report_gps_pos.timestamp) / 1000.0);
 		PX4_INFO("lat: %d, lon: %d, alt: %d", _report_gps_pos.lat, _report_gps_pos.lon, _report_gps_pos.alt);
 		PX4_INFO("vel: %.2fm/s, %.2fm/s, %.2fm/s", (double)_report_gps_pos.vel_n_m_s,
 			 (double)_report_gps_pos.vel_e_m_s, (double)_report_gps_pos.vel_d_m_s);

From e2a71453791c5714f1c12312627d5fe821522696 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Beat=20K=C3=BCng?= <beat-kueng@gmx.net>
Date: Wed, 15 Jun 2016 13:11:40 +0200
Subject: [PATCH 0795/1230] vehicle_gps_position: remove timestamp_variance &
 timestamp_velocity (they're not used)

---
 msg/vehicle_gps_position.msg                                | 3 +--
 src/drivers/gps/devices                                     | 2 +-
 src/drivers/gps/gps.cpp                                     | 6 +-----
 src/modules/ekf2/ekf2_main.cpp                              | 4 ++--
 src/modules/mavlink/mavlink_receiver.cpp                    | 2 --
 .../position_estimator_inav_main.cpp                        | 2 +-
 src/modules/uavcan/sensors/gnss.cpp                         | 3 ---
 src/platforms/posix/drivers/gpssim/gpssim.cpp               | 6 ------
 8 files changed, 6 insertions(+), 22 deletions(-)

diff --git a/msg/vehicle_gps_position.msg b/msg/vehicle_gps_position.msg
index 4be23df42e..77ef0f3e15 100644
--- a/msg/vehicle_gps_position.msg
+++ b/msg/vehicle_gps_position.msg
@@ -1,10 +1,10 @@
 # GPS position in WGS84 coordinates.
+# the auto-generated field 'timestamp' is for the position & velocity (microseconds)
 int32 lat			# Latitude in 1E-7 degrees
 int32 lon			# Longitude in 1E-7 degrees 
 int32 alt			# Altitude in 1E-3 meters above MSL, (millimetres)
 int32 alt_ellipsoid 		# Altitude in 1E-3 meters bove Ellipsoid, (millimetres)
 
-uint64 timestamp_variance	# Time of the accuracy estimates since system start, (microseconds)
 float32 s_variance_m_s		# GPS speed accuracy estimate, (metres/sec)
 float32 c_variance_rad		# GPS course accuracy estimate, (radians) 
 uint8 fix_type # 0-1: no fix, 2: 2D fix, 3: 3D fix, 4: RTCM code differential, 5: Real-Time Kinematic, float, 6: Real-Time Kinematic, fixed, 8: Extrapolated. Some applications will not use the value of this field unless it is at least two, so always correctly fill in the fix.   
@@ -18,7 +18,6 @@ float32 vdop			# Vertical dilution of precision
 int32 noise_per_ms		# GPS noise per millisecond
 int32 jamming_indicator		# indicates jamming is occurring
 
-uint64 timestamp_velocity	# Time of the velocity estimates since system start, (microseconds)
 float32 vel_m_s			# GPS ground speed, (metres/sec) 
 float32 vel_n_m_s		# GPS North velocity, (metres/sec) 
 float32 vel_e_m_s		# GPS East velocity, (metres/sec)
diff --git a/src/drivers/gps/devices b/src/drivers/gps/devices
index 9e7ed8a5ea..682fca425e 160000
--- a/src/drivers/gps/devices
+++ b/src/drivers/gps/devices
@@ -1 +1 @@
-Subproject commit 9e7ed8a5ea85c76c5d563413df051f138b9a0622
+Subproject commit 682fca425e6f3d67ac8062dcc9b36d8bca210175
diff --git a/src/drivers/gps/gps.cpp b/src/drivers/gps/gps.cpp
index 21950bbf4c..54f5987deb 100644
--- a/src/drivers/gps/gps.cpp
+++ b/src/drivers/gps/gps.cpp
@@ -691,13 +691,11 @@ GPS::task_main()
 			_report_gps_pos.lat = (int32_t)47.378301e7f;
 			_report_gps_pos.lon = (int32_t)8.538777e7f;
 			_report_gps_pos.alt = (int32_t)1200e3f;
-			_report_gps_pos.timestamp_variance = hrt_absolute_time();
 			_report_gps_pos.s_variance_m_s = 10.0f;
 			_report_gps_pos.c_variance_rad = 0.1f;
 			_report_gps_pos.fix_type = 3;
 			_report_gps_pos.eph = 0.9f;
 			_report_gps_pos.epv = 1.8f;
-			_report_gps_pos.timestamp_velocity = hrt_absolute_time();
 			_report_gps_pos.vel_n_m_s = 0.0f;
 			_report_gps_pos.vel_e_m_s = 0.0f;
 			_report_gps_pos.vel_d_m_s = 0.0f;
@@ -756,10 +754,8 @@ GPS::task_main()
 
 					_report_gps_pos.timestamp_time = hrt_absolute_time();
 
-					/* reset the timestamps for data, because we have no data yet */
+					/* reset the timestamp for data, because we have no data yet */
 					_report_gps_pos.timestamp = 0;
-					_report_gps_pos.timestamp_variance = 0;
-					_report_gps_pos.timestamp_velocity = 0;
 
 					/* set a massive variance */
 					_report_gps_pos.eph = 10000.0f;
diff --git a/src/modules/ekf2/ekf2_main.cpp b/src/modules/ekf2/ekf2_main.cpp
index d482c4d8ac..6311790127 100644
--- a/src/modules/ekf2/ekf2_main.cpp
+++ b/src/modules/ekf2/ekf2_main.cpp
@@ -525,7 +525,7 @@ void Ekf2::task_main()
 			gps_msg.eph = gps.eph;
 			gps_msg.epv = gps.epv;
 			gps_msg.sacc = gps.s_variance_m_s;
-			gps_msg.time_usec_vel = gps.timestamp_velocity;
+			gps_msg.time_usec_vel = gps.timestamp;
 			gps_msg.vel_m_s = gps.vel_m_s;
 			gps_msg.vel_ned[0] = gps.vel_n_m_s;
 			gps_msg.vel_ned[1] = gps.vel_e_m_s;
@@ -910,7 +910,7 @@ void Ekf2::task_main()
 			// only write gps data if we had a gps update.
 			if (gps_updated) {
 				replay.time_usec = gps.timestamp;
-				replay.time_usec_vel = gps.timestamp_velocity;
+				replay.time_usec_vel = gps.timestamp;
 				replay.lat = gps.lat;
 				replay.lon = gps.lon;
 				replay.alt = gps.alt;
diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp
index 37530bb13a..1c5730c3cf 100644
--- a/src/modules/mavlink/mavlink_receiver.cpp
+++ b/src/modules/mavlink/mavlink_receiver.cpp
@@ -1791,10 +1791,8 @@ MavlinkReceiver::handle_message_hil_gps(mavlink_message_t *msg)
 	hil_gps.eph = (float)gps.eph * 1e-2f; // from cm to m
 	hil_gps.epv = (float)gps.epv * 1e-2f; // from cm to m
 
-	hil_gps.timestamp_variance = timestamp;
 	hil_gps.s_variance_m_s = 5.0f;
 
-	hil_gps.timestamp_velocity = timestamp;
 	hil_gps.vel_m_s = (float)gps.vel * 1e-2f; // from cm/s to m/s
 	hil_gps.vel_n_m_s = gps.vn * 1e-2f; // from cm to m
 	hil_gps.vel_e_m_s = gps.ve * 1e-2f; // from cm to m
diff --git a/src/modules/position_estimator_inav/position_estimator_inav_main.cpp b/src/modules/position_estimator_inav/position_estimator_inav_main.cpp
index 79e070c614..8d4b882b0b 100644
--- a/src/modules/position_estimator_inav/position_estimator_inav_main.cpp
+++ b/src/modules/position_estimator_inav/position_estimator_inav_main.cpp
@@ -1212,7 +1212,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
 				inertial_filter_correct(corr_gps[0][0], dt, x_est, 0, w_xy_gps_p);
 				inertial_filter_correct(corr_gps[1][0], dt, y_est, 0, w_xy_gps_p);
 
-				if (gps.vel_ned_valid && t < gps.timestamp_velocity + gps_topic_timeout) {
+				if (gps.vel_ned_valid && t < gps.timestamp + gps_topic_timeout) {
 					inertial_filter_correct(corr_gps[0][1], dt, x_est, 1, w_xy_gps_v);
 					inertial_filter_correct(corr_gps[1][1], dt, y_est, 1, w_xy_gps_v);
 				}
diff --git a/src/modules/uavcan/sensors/gnss.cpp b/src/modules/uavcan/sensors/gnss.cpp
index bf56539b7a..f31ca0174a 100644
--- a/src/modules/uavcan/sensors/gnss.cpp
+++ b/src/modules/uavcan/sensors/gnss.cpp
@@ -109,8 +109,6 @@ void UavcanGnssBridge::gnss_fix_sub_cb(const uavcan::ReceivedDataStructure<uavca
 	report.lon = msg.longitude_deg_1e8 / 10;
 	report.alt = msg.height_msl_mm;
 
-	report.timestamp_variance = report.timestamp;
-
 
 	// Check if the msg contains valid covariance information
 	const bool valid_position_covariance = !msg.position_covariance.empty();
@@ -163,7 +161,6 @@ void UavcanGnssBridge::gnss_fix_sub_cb(const uavcan::ReceivedDataStructure<uavca
 
 	report.fix_type = msg.status;
 
-	report.timestamp_velocity = report.timestamp;
 	report.vel_n_m_s = msg.ned_velocity[0];
 	report.vel_e_m_s = msg.ned_velocity[1];
 	report.vel_d_m_s = msg.ned_velocity[2];
diff --git a/src/platforms/posix/drivers/gpssim/gpssim.cpp b/src/platforms/posix/drivers/gpssim/gpssim.cpp
index 258e1b054a..fa49bd0b2a 100644
--- a/src/platforms/posix/drivers/gpssim/gpssim.cpp
+++ b/src/platforms/posix/drivers/gpssim/gpssim.cpp
@@ -282,10 +282,8 @@ GPSSIM::receive(int timeout)
 	_report_gps_pos.lat = gps.lat;
 	_report_gps_pos.lon = gps.lon;
 	_report_gps_pos.alt = gps.alt;
-	_report_gps_pos.timestamp_variance = _report_gps_pos.timestamp;
 	_report_gps_pos.eph = (float)gps.eph * 1e-2f;
 	_report_gps_pos.epv = (float)gps.epv * 1e-2f;
-	_report_gps_pos.timestamp_velocity = _report_gps_pos.timestamp;
 	_report_gps_pos.vel_m_s = (float)(gps.vel) / 100.0f;
 	_report_gps_pos.vel_n_m_s = (float)(gps.vn) / 100.0f;
 	_report_gps_pos.vel_e_m_s = (float)(gps.ve) / 100.0f;
@@ -310,13 +308,11 @@ GPSSIM::task_main()
 			_report_gps_pos.lat = (int32_t)47.378301e7f;
 			_report_gps_pos.lon = (int32_t)8.538777e7f;
 			_report_gps_pos.alt = (int32_t)1200e3f;
-			_report_gps_pos.timestamp_variance = _report_gps_pos.timestamp;
 			_report_gps_pos.s_variance_m_s = 10.0f;
 			_report_gps_pos.c_variance_rad = 0.1f;
 			_report_gps_pos.fix_type = 3;
 			_report_gps_pos.eph = 0.9f;
 			_report_gps_pos.epv = 1.8f;
-			_report_gps_pos.timestamp_velocity = _report_gps_pos.timestamp;
 			_report_gps_pos.vel_n_m_s = 0.0f;
 			_report_gps_pos.vel_e_m_s = 0.0f;
 			_report_gps_pos.vel_d_m_s = 0.0f;
@@ -343,8 +339,6 @@ GPSSIM::task_main()
 			//Make sure to clear any stale data in case driver is reset
 			memset(&_report_gps_pos, 0, sizeof(_report_gps_pos));
 			_report_gps_pos.timestamp = hrt_absolute_time();
-			_report_gps_pos.timestamp_variance = hrt_absolute_time();
-			_report_gps_pos.timestamp_velocity = hrt_absolute_time();
 			_report_gps_pos.timestamp_time = hrt_absolute_time();
 
 			if (!(m_pub_blocked)) {

From f8e9a19889164750bcfd937a3c8084a8d399316b Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Beat=20K=C3=BCng?= <beat-kueng@gmx.net>
Date: Mon, 20 Jun 2016 22:14:16 +0200
Subject: [PATCH 0796/1230] gps_position: convert uint64 timestamp_time ->
 int32 timestamp_time_relative

We need to make this timestamp relative to the main timestamp. Necessary
for replay, and saves some space.
---
 msg/vehicle_gps_position.msg                  | 2 +-
 src/drivers/gps/devices                       | 2 +-
 src/drivers/gps/gps.cpp                       | 3 +--
 src/modules/mavlink/mavlink_receiver.cpp      | 2 +-
 src/platforms/posix/drivers/gpssim/gpssim.cpp | 2 +-
 5 files changed, 5 insertions(+), 6 deletions(-)

diff --git a/msg/vehicle_gps_position.msg b/msg/vehicle_gps_position.msg
index 77ef0f3e15..c57a078033 100644
--- a/msg/vehicle_gps_position.msg
+++ b/msg/vehicle_gps_position.msg
@@ -25,7 +25,7 @@ float32 vel_d_m_s		# GPS Down velocity, (metres/sec)
 float32 cog_rad			# Course over ground (NOT heading, but direction of movement), -PI..PI, (radians) 
 bool vel_ned_valid		# True if NED velocity is valid 
 
-uint64 timestamp_time		# Time of the UTC timestamp since system start, (microseconds) 
+int32 timestamp_time_relative	# timestamp + timestamp_time_relative = Time of the UTC timestamp since system start, (microseconds)
 uint64 time_utc_usec		# Timestamp (microseconds, UTC), this is the timestamp which comes from the gps module. It might be unavailable right after cold start, indicated by a value of 0 
 
 uint8 satellites_used		# Number of satellites used 
diff --git a/src/drivers/gps/devices b/src/drivers/gps/devices
index 682fca425e..a5e3e263aa 160000
--- a/src/drivers/gps/devices
+++ b/src/drivers/gps/devices
@@ -1 +1 @@
-Subproject commit 682fca425e6f3d67ac8062dcc9b36d8bca210175
+Subproject commit a5e3e263aaaa613e007717d3ebf5ca7c597bcf51
diff --git a/src/drivers/gps/gps.cpp b/src/drivers/gps/gps.cpp
index 54f5987deb..6d9d0f07c8 100644
--- a/src/drivers/gps/gps.cpp
+++ b/src/drivers/gps/gps.cpp
@@ -752,10 +752,9 @@ GPS::task_main()
 					 * no valid position lock
 					 */
 
-					_report_gps_pos.timestamp_time = hrt_absolute_time();
-
 					/* reset the timestamp for data, because we have no data yet */
 					_report_gps_pos.timestamp = 0;
+					_report_gps_pos.timestamp_time_relative = 0;
 
 					/* set a massive variance */
 					_report_gps_pos.eph = 10000.0f;
diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp
index 1c5730c3cf..8cbcdf682d 100644
--- a/src/modules/mavlink/mavlink_receiver.cpp
+++ b/src/modules/mavlink/mavlink_receiver.cpp
@@ -1781,7 +1781,7 @@ MavlinkReceiver::handle_message_hil_gps(mavlink_message_t *msg)
 	struct vehicle_gps_position_s hil_gps;
 	memset(&hil_gps, 0, sizeof(hil_gps));
 
-	hil_gps.timestamp_time = timestamp;
+	hil_gps.timestamp_time_relative = 0;
 	hil_gps.time_utc_usec = gps.time_usec;
 
 	hil_gps.timestamp = timestamp;
diff --git a/src/platforms/posix/drivers/gpssim/gpssim.cpp b/src/platforms/posix/drivers/gpssim/gpssim.cpp
index fa49bd0b2a..0b3f4ba2b2 100644
--- a/src/platforms/posix/drivers/gpssim/gpssim.cpp
+++ b/src/platforms/posix/drivers/gpssim/gpssim.cpp
@@ -339,7 +339,7 @@ GPSSIM::task_main()
 			//Make sure to clear any stale data in case driver is reset
 			memset(&_report_gps_pos, 0, sizeof(_report_gps_pos));
 			_report_gps_pos.timestamp = hrt_absolute_time();
-			_report_gps_pos.timestamp_time = hrt_absolute_time();
+			_report_gps_pos.timestamp_time_relative = 0;
 
 			if (!(m_pub_blocked)) {
 				if (_report_gps_pos_pub != nullptr) {

From 940ac5471d5fe8397415463dc99bd0faa4b70d9f Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Beat=20K=C3=BCng?= <beat-kueng@gmx.net>
Date: Tue, 21 Jun 2016 11:04:53 +0200
Subject: [PATCH 0797/1230] ekf2: remove unused gps_msg.time_usec_vel

---
 src/modules/ekf2/ekf2_main.cpp | 1 -
 1 file changed, 1 deletion(-)

diff --git a/src/modules/ekf2/ekf2_main.cpp b/src/modules/ekf2/ekf2_main.cpp
index 6311790127..a02e1ceeb3 100644
--- a/src/modules/ekf2/ekf2_main.cpp
+++ b/src/modules/ekf2/ekf2_main.cpp
@@ -525,7 +525,6 @@ void Ekf2::task_main()
 			gps_msg.eph = gps.eph;
 			gps_msg.epv = gps.epv;
 			gps_msg.sacc = gps.s_variance_m_s;
-			gps_msg.time_usec_vel = gps.timestamp;
 			gps_msg.vel_m_s = gps.vel_m_s;
 			gps_msg.vel_ned[0] = gps.vel_n_m_s;
 			gps_msg.vel_ned[1] = gps.vel_e_m_s;

From cf5d959f1b3569d67310cd18ecf8432a91e28cbf Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Beat=20K=C3=BCng?= <beat-kueng@gmx.net>
Date: Tue, 21 Jun 2016 11:09:08 +0200
Subject: [PATCH 0798/1230] gnss.cpp: switch to relative gps timestamp

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

diff --git a/src/modules/uavcan/sensors/gnss.cpp b/src/modules/uavcan/sensors/gnss.cpp
index f31ca0174a..b1cb428c5b 100644
--- a/src/modules/uavcan/sensors/gnss.cpp
+++ b/src/modules/uavcan/sensors/gnss.cpp
@@ -169,7 +169,7 @@ void UavcanGnssBridge::gnss_fix_sub_cb(const uavcan::ReceivedDataStructure<uavca
 	report.cog_rad = atan2f(report.vel_e_m_s, report.vel_n_m_s);
 	report.vel_ned_valid = true;
 
-	report.timestamp_time = report.timestamp;
+	report.timestamp_time_relative = 0;
 	report.time_utc_usec = uavcan::UtcTime(msg.gnss_timestamp).toUSec();	// Convert to microseconds
 
 	report.satellites_used = msg.sats_used;

From e0c11f5545e7b4e95ce4242391d3c79b7f1d800e Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Beat=20K=C3=BCng?= <beat-kueng@gmx.net>
Date: Thu, 23 Jun 2016 21:50:26 +0200
Subject: [PATCH 0799/1230] fix ekf2_replay_main.cpp: remove timestamp_velocity
 from gps topic

---
 src/modules/ekf2_replay/ekf2_replay_main.cpp | 1 -
 1 file changed, 1 deletion(-)

diff --git a/src/modules/ekf2_replay/ekf2_replay_main.cpp b/src/modules/ekf2_replay/ekf2_replay_main.cpp
index 823c60744b..7faae2f89f 100644
--- a/src/modules/ekf2_replay/ekf2_replay_main.cpp
+++ b/src/modules/ekf2_replay/ekf2_replay_main.cpp
@@ -397,7 +397,6 @@ void Ekf2Replay::setEstimatorInput(uint8_t *data, uint8_t type)
 		uint8_t *dest_ptr = (uint8_t *)&replay_part2.time_pos_usec;
 		parseMessage(data, dest_ptr, type);
 		_gps.timestamp = replay_part2.time_pos_usec;
-		_gps.timestamp_velocity = replay_part2.time_vel_usec;
 		_gps.lat = replay_part2.lat;
 		_gps.lon = replay_part2.lon;
 		_gps.fix_type = replay_part2.fix_type;

From 925c3409150ee2469829575e29da8541d61061e8 Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Fri, 24 Jun 2016 00:24:09 +0200
Subject: [PATCH 0800/1230] Remove unused code from simulated driver

---
 .../drivers/airspeedsim/meas_airspeed_sim.cpp | 55 -------------------
 1 file changed, 55 deletions(-)

diff --git a/src/platforms/posix/drivers/airspeedsim/meas_airspeed_sim.cpp b/src/platforms/posix/drivers/airspeedsim/meas_airspeed_sim.cpp
index 123a22b69e..94d8d5ce7b 100644
--- a/src/platforms/posix/drivers/airspeedsim/meas_airspeed_sim.cpp
+++ b/src/platforms/posix/drivers/airspeedsim/meas_airspeed_sim.cpp
@@ -291,61 +291,6 @@ MEASAirspeedSim::cycle()
 void
 MEASAirspeedSim::voltage_correction(float &diff_press_pa, float &temperature)
 {
-#if defined(CONFIG_ARCH_BOARD_PX4FMU_V2) || defined(CONFIG_ARCH_BOARD_PX4FMU_V4)
-
-	if (_t_system_power == -1) {
-		_t_system_power = orb_subscribe(ORB_ID(system_power));
-	}
-
-	if (_t_system_power == -1) {
-		// not available
-		return;
-	}
-
-	bool updated = false;
-	orb_check(_t_system_power, &updated);
-
-	if (updated) {
-		orb_copy(ORB_ID(system_power), _t_system_power, &system_power);
-	}
-
-	if (system_power.voltage5V_v < 3.0f || system_power.voltage5V_v > 6.0f) {
-		// not valid, skip correction
-		return;
-	}
-
-	const float slope = 65.0f;
-	/*
-	  apply a piecewise linear correction, flattening at 0.5V from 5V
-	 */
-	float voltage_diff = system_power.voltage5V_v - 5.0f;
-
-	if (voltage_diff > 0.5f) {
-		voltage_diff = 0.5f;
-	}
-
-	if (voltage_diff < -0.5f) {
-		voltage_diff = -0.5f;
-	}
-
-	diff_press_pa -= voltage_diff * slope;
-
-	/*
-	  the temperature masurement varies as well
-	 */
-	const float temp_slope = 0.887f;
-	voltage_diff = system_power.voltage5V_v - 5.0f;
-
-	if (voltage_diff > 0.5f) {
-		voltage_diff = 0.5f;
-	}
-
-	if (voltage_diff < -1.0f) {
-		voltage_diff = -1.0f;
-	}
-
-	temperature -= voltage_diff * temp_slope;
-#endif // CONFIG_ARCH_BOARD_PX4FMU_V2
 }
 
 /**

From f754d23a0fe5c77e1d69570c467f1f535f947c7d Mon Sep 17 00:00:00 2001
From: Roman <bapstr@ethz.ch>
Date: Fri, 24 Jun 2016 08:08:53 +0200
Subject: [PATCH 0801/1230] mavlink receiver: fixed computation of cell count

Signed-off-by: Roman <bapstr@ethz.ch>
---
 src/modules/mavlink/mavlink_receiver.cpp | 12 +++++-------
 1 file changed, 5 insertions(+), 7 deletions(-)

diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp
index 8cbcdf682d..b973cf3629 100644
--- a/src/modules/mavlink/mavlink_receiver.cpp
+++ b/src/modules/mavlink/mavlink_receiver.cpp
@@ -258,6 +258,7 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg)
 	case MAVLINK_MSG_ID_GPS_RTCM_DATA:
 		handle_message_gps_rtcm_data(msg);
 		break;
+
 	case MAVLINK_MSG_ID_BATTERY_STATUS:
 		handle_message_battery_status(msg);
 		break;
@@ -1169,13 +1170,10 @@ MavlinkReceiver::handle_message_battery_status(mavlink_message_t *msg)
 
 	float voltage_sum = 0.0f;
 	uint8_t cell_count = 0;
-	for (unsigned i = 0; i < 10;i++) {
-		if (battery_mavlink.voltages[i] < UINT16_MAX) {
-			voltage_sum += (float)(battery_mavlink.voltages[i]) / 1000.0f;
-		} else {
-			cell_count = i;
-			break;
-		}
+
+	while (battery_mavlink.voltages[cell_count] < UINT16_MAX && cell_count < 10) {
+		voltage_sum += (float)(battery_mavlink.voltages[cell_count]) / 1000.0f;
+		cell_count++;
 	}
 
 	battery_status.voltage_v = voltage_sum;

From eabc4647c27e9aa795cbc7eb196a9333ca382df0 Mon Sep 17 00:00:00 2001
From: Roman <bapstr@ethz.ch>
Date: Fri, 24 Jun 2016 08:35:27 +0200
Subject: [PATCH 0802/1230] mavlink receiver: fixed unit conversions for
 current

Signed-off-by: Roman <bapstr@ethz.ch>
---
 src/modules/mavlink/mavlink_receiver.cpp | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp
index b973cf3629..0af8bb3139 100644
--- a/src/modules/mavlink/mavlink_receiver.cpp
+++ b/src/modules/mavlink/mavlink_receiver.cpp
@@ -1178,7 +1178,7 @@ MavlinkReceiver::handle_message_battery_status(mavlink_message_t *msg)
 
 	battery_status.voltage_v = voltage_sum;
 	battery_status.voltage_filtered_v  = voltage_sum;
-	battery_status.current_a = battery_status.current_filtered_a = (float)(battery_mavlink.current_battery) / 10000.0f;
+	battery_status.current_a = battery_status.current_filtered_a = (float)(battery_mavlink.current_battery) / 100.0f;
 	battery_status.current_filtered_a = battery_status.current_a;
 	battery_status.remaining = (float)battery_mavlink.battery_remaining / 100.0f;
 	battery_status.discharged_mah = (float)battery_mavlink.current_consumed;

From 993831aba8a587643d31390e6f0b5ad23005c07f Mon Sep 17 00:00:00 2001
From: ecmnet <ecm@gmx.de>
Date: Fri, 24 Jun 2016 08:21:42 +0200
Subject: [PATCH 0803/1230] SF10A driver fix

---
 src/drivers/sf10a/sf10a.cpp | 13 +++++++------
 1 file changed, 7 insertions(+), 6 deletions(-)

diff --git a/src/drivers/sf10a/sf10a.cpp b/src/drivers/sf10a/sf10a.cpp
index 32919a4033..026b54b181 100644
--- a/src/drivers/sf10a/sf10a.cpp
+++ b/src/drivers/sf10a/sf10a.cpp
@@ -572,12 +572,13 @@ SF10A::start()
 	work_queue(HPWORK, &_work, (worker_t)&SF10A::cycle_trampoline, this, 5);
 
 	/* notify about state change */
-	struct subsystem_info_s info = {
-		true,
-		true,
-		true,
-		subsystem_info_s::SUBSYSTEM_TYPE_RANGEFINDER
-	};
+		struct subsystem_info_s info = {};
+		info.present = true;
+		info.enabled = true;
+		info.ok = true;
+		info.subsystem_type = subsystem_info_s::SUBSYSTEM_TYPE_RANGEFINDER;
+
+
 	static orb_advert_t pub = nullptr;
 
 	if (pub != nullptr) {

From 7b2367cdff04a3b761996c4b383b94684378b2c7 Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Fri, 24 Jun 2016 10:31:17 +0200
Subject: [PATCH 0804/1230] Remove unused topic from SF10A

---
 src/drivers/sf10a/sf10a.cpp | 30 +-----------------------------
 1 file changed, 1 insertion(+), 29 deletions(-)

diff --git a/src/drivers/sf10a/sf10a.cpp b/src/drivers/sf10a/sf10a.cpp
index 026b54b181..21b0a8bcb7 100644
--- a/src/drivers/sf10a/sf10a.cpp
+++ b/src/drivers/sf10a/sf10a.cpp
@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2013-2015 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2013-2016 PX4 Development Team. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -70,7 +70,6 @@
 #include <drivers/device/ringbuffer.h>
 
 #include <uORB/uORB.h>
-#include <uORB/topics/subsystem_info.h>
 #include <uORB/topics/distance_sensor.h>
 
 #include <board_config.h>
@@ -87,7 +86,6 @@
 
 /* conversion rates */
 
-//#define SF10A_CONVERSION_INTERVAL 		25000			// Overclocking SF10a to 40 Hz
 #define SF10A_CONVERSION_INTERVAL 		31250		// Maximum rate according to datasheet is 32Hz
 
 
@@ -426,7 +424,6 @@ SF10A::ioctl(struct file *filp, int cmd, unsigned long arg)
 ssize_t
 SF10A::read(struct file *filp, char *buffer, size_t buflen)
 {
-
 	unsigned count = buflen / sizeof(struct distance_sensor_s);
 	struct distance_sensor_s *rbuf = reinterpret_cast<struct distance_sensor_s *>(buffer);
 	int ret = 0;
@@ -487,7 +484,6 @@ SF10A::read(struct file *filp, char *buffer, size_t buflen)
 int
 SF10A::measure()
 {
-
 	int ret;
 
 	/*
@@ -564,31 +560,11 @@ SF10A::collect()
 void
 SF10A::start()
 {
-
 	/* reset the report ring and state machine */
 	_reports->flush();
 
 	/* schedule a cycle to start things */
 	work_queue(HPWORK, &_work, (worker_t)&SF10A::cycle_trampoline, this, 5);
-
-	/* notify about state change */
-		struct subsystem_info_s info = {};
-		info.present = true;
-		info.enabled = true;
-		info.ok = true;
-		info.subsystem_type = subsystem_info_s::SUBSYSTEM_TYPE_RANGEFINDER;
-
-
-	static orb_advert_t pub = nullptr;
-
-	if (pub != nullptr) {
-		orb_publish(ORB_ID(subsystem_info), pub, &info);
-
-
-	} else {
-		pub = orb_advertise(ORB_ID(subsystem_info), &info);
-
-	}
 }
 
 void
@@ -600,17 +576,14 @@ SF10A::stop()
 void
 SF10A::cycle_trampoline(void *arg)
 {
-
 	SF10A *dev = (SF10A *)arg;
 
 	dev->cycle();
-
 }
 
 void
 SF10A::cycle()
 {
-
 	set_address(SF10A_BASEADDR);
 
 	/* Collect results */
@@ -635,7 +608,6 @@ SF10A::cycle()
 		   (worker_t)&SF10A::cycle_trampoline,
 		   this,
 		   USEC2TICK(SF10A_CONVERSION_INTERVAL));
-
 }
 
 void

From d49598b8b3106f503a5482bcaee879387d96efe4 Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Fri, 24 Jun 2016 10:31:28 +0200
Subject: [PATCH 0805/1230] Build SF10A on FMUv4

---
 cmake/configs/nuttx_px4fmu-v4_default.cmake | 1 +
 1 file changed, 1 insertion(+)

diff --git a/cmake/configs/nuttx_px4fmu-v4_default.cmake b/cmake/configs/nuttx_px4fmu-v4_default.cmake
index d9c6e4fb52..545dfb76da 100644
--- a/cmake/configs/nuttx_px4fmu-v4_default.cmake
+++ b/cmake/configs/nuttx_px4fmu-v4_default.cmake
@@ -23,6 +23,7 @@ set(config_module_list
 	drivers/mb12xx
 	drivers/srf02
 	drivers/sf0x
+	drivers/sf10a
 	drivers/ll40ls
 	drivers/trone
 	drivers/gps

From 286efb6b3443dc70bff837f5c6b80907ecc5d476 Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lorenz@px4.io>
Date: Fri, 24 Jun 2016 10:39:55 +0200
Subject: [PATCH 0806/1230] Remove POOSIX porting noise from ms5611 driver
 (#4896)

---
 src/drivers/ms5611/CMakeLists.txt             |   15 +-
 .../ms5611/{ms5611_nuttx.cpp => ms5611.cpp}   |    0
 src/drivers/ms5611/ms5611_posix.cpp           | 1335 -----------------
 src/drivers/ms5611/ms5611_sim.cpp             |  206 ---
 4 files changed, 2 insertions(+), 1554 deletions(-)
 rename src/drivers/ms5611/{ms5611_nuttx.cpp => ms5611.cpp} (100%)
 delete mode 100644 src/drivers/ms5611/ms5611_posix.cpp
 delete mode 100644 src/drivers/ms5611/ms5611_sim.cpp

diff --git a/src/drivers/ms5611/CMakeLists.txt b/src/drivers/ms5611/CMakeLists.txt
index 1cfa739a2e..1c5ebbb673 100644
--- a/src/drivers/ms5611/CMakeLists.txt
+++ b/src/drivers/ms5611/CMakeLists.txt
@@ -33,19 +33,8 @@
 set(srcs
 	ms5611_spi.cpp
 	ms5611_i2c.cpp
-	)
-
-if(${OS} STREQUAL "nuttx")
-	list(APPEND srcs
-		ms5611_nuttx.cpp
-		)
-else()
-	list(APPEND srcs
-		ms5611_posix.cpp
-		ms5611_sim.cpp
-		)
-
-endif()
+	ms5611.cpp
+)
 
 px4_add_module(
 	MODULE drivers__ms5611
diff --git a/src/drivers/ms5611/ms5611_nuttx.cpp b/src/drivers/ms5611/ms5611.cpp
similarity index 100%
rename from src/drivers/ms5611/ms5611_nuttx.cpp
rename to src/drivers/ms5611/ms5611.cpp
diff --git a/src/drivers/ms5611/ms5611_posix.cpp b/src/drivers/ms5611/ms5611_posix.cpp
deleted file mode 100644
index b4b22f6c35..0000000000
--- a/src/drivers/ms5611/ms5611_posix.cpp
+++ /dev/null
@@ -1,1335 +0,0 @@
-/****************************************************************************
- *
- *   Copyright (c) 2012-2015 PX4 Development Team. All rights reserved.
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions
- * are met:
- *
- * 1. Redistributions of source code must retain the above copyright
- *    notice, this list of conditions and the following disclaimer.
- * 2. Redistributions in binary form must reproduce the above copyright
- *    notice, this list of conditions and the following disclaimer in
- *    the documentation and/or other materials provided with the
- *    distribution.
- * 3. Neither the name PX4 nor the names of its contributors may be
- *    used to endorse or promote products derived from this software
- *    without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
- * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
- * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- *
- ****************************************************************************/
-
-/**
- * @file ms5611.cpp
- * Driver for the MS5611 barometric pressure sensor connected via I2C or SPI.
- */
-
-#include <px4_config.h>
-#include <px4_defines.h>
-#include <px4_getopt.h>
-
-#include <sys/types.h>
-#include <stdint.h>
-#include <stdbool.h>
-#include <stdlib.h>
-#include <semaphore.h>
-#include <string.h>
-#include <fcntl.h>
-#include <poll.h>
-#include <errno.h>
-#include <stdio.h>
-#include <math.h>
-#include <unistd.h>
-
-#include <px4_workqueue.h>
-#include <arch/board/board.h>
-#include <board_config.h>
-
-#include <drivers/device/device.h>
-#include <drivers/drv_baro.h>
-#include <drivers/drv_hrt.h>
-#include <drivers/device/ringbuffer.h>
-
-#include <systemlib/perf_counter.h>
-#include <systemlib/err.h>
-
-#include "ms5611.h"
-
-enum MS5611_BUS {
-	MS5611_BUS_ALL = 0,
-	MS5611_BUS_I2C_INTERNAL,
-	MS5611_BUS_I2C_EXTERNAL,
-	MS5611_BUS_SPI_INTERNAL,
-	MS5611_BUS_SPI_EXTERNAL,
-	MS5611_BUS_SIM_EXTERNAL
-};
-
-/* oddly, ERROR is not defined for c++ */
-#ifdef ERROR
-# undef ERROR
-#endif
-static const int ERROR = -1;
-
-//#ifndef CONFIG_SCHED_WORKQUEUE
-//# error This requires CONFIG_SCHED_WORKQUEUE.
-//#endif
-
-/* helper macro for handling report buffer indices */
-#define INCREMENT(_x, _lim)	do { __typeof__(_x) _tmp = _x+1; if (_tmp >= _lim) _tmp = 0; _x = _tmp; } while(0)
-
-/* helper macro for arithmetic - returns the square of the argument */
-#define POW2(_x)		((_x) * (_x))
-
-/*
- * MS5611 internal constants and data structures.
- */
-
-/* internal conversion time: 9.17 ms, so should not be read at rates higher than 100 Hz */
-#define MS5611_CONVERSION_INTERVAL	10000	/* microseconds */
-#define MS5611_MEASUREMENT_RATIO	3	/* pressure measurements per temperature measurement */
-#define MS5611_BARO_DEVICE_PATH_EXT	"/dev/ms5611_ext"
-#define MS5611_BARO_DEVICE_PATH_INT	"/dev/ms5611_int"
-
-class MS5611 : public device::VDev
-{
-public:
-	MS5611(device::Device *interface, ms5611::prom_u &prom_buf, const char *path);
-	~MS5611();
-
-	virtual int		init();
-
-	virtual ssize_t		read(device::file_t *handlep, char *buffer, size_t buflen);
-	virtual int		ioctl(device::file_t *handlep, int cmd, unsigned long arg);
-
-	/**
-	 * Diagnostics - print some basic information about the driver.
-	 */
-	void			print_info();
-
-protected:
-	Device			*_interface;
-
-	ms5611::prom_s		_prom;
-
-	struct work_s		_work;
-	unsigned		_measure_ticks;
-
-	ringbuffer::RingBuffer	*_reports;
-
-	bool			_collect_phase;
-	unsigned		_measure_phase;
-
-	/* intermediate temperature values per MS5611 datasheet */
-	int32_t			_TEMP;
-	int64_t			_OFF;
-	int64_t			_SENS;
-	float			_P;
-	float			_T;
-
-	/* altitude conversion calibration */
-	unsigned		_msl_pressure;	/* in Pa */
-
-	orb_advert_t		_baro_topic;
-	int			_orb_class_instance;
-	int			_class_instance;
-
-	perf_counter_t		_sample_perf;
-	perf_counter_t		_measure_perf;
-	perf_counter_t		_comms_errors;
-	perf_counter_t		_buffer_overflows;
-
-	/**
-	 * Initialize the automatic measurement state machine and start it.
-	 *
-	 * @note This function is called at open and error time.  It might make sense
-	 *       to make it more aggressive about resetting the bus in case of errors.
-	 */
-	void			start_cycle();
-
-	/**
-	 * Stop the automatic measurement state machine.
-	 */
-	void			stop_cycle();
-
-	/**
-	 * Perform a poll cycle; collect from the previous measurement
-	 * and start a new one.
-	 *
-	 * This is the heart of the measurement state machine.  This function
-	 * alternately starts a measurement, or collects the data from the
-	 * previous measurement.
-	 *
-	 * When the interval between measurements is greater than the minimum
-	 * measurement interval, a gap is inserted between collection
-	 * and measurement to provide the most recent measurement possible
-	 * at the next interval.
-	 */
-	void			cycle();
-
-	/**
-	 * Get the internal / external state
-	 *
-	 * @return true if the sensor is not on the main MCU board
-	 */
-	bool			is_external() { return (_orb_class_instance == 0); /* XXX put this into the interface class */ }
-
-	/**
-	 * Static trampoline from the workq context; because we don't have a
-	 * generic workq wrapper yet.
-	 *
-	 * @param arg		Instance pointer for the driver that is polling.
-	 */
-	static void		cycle_trampoline(void *arg);
-
-	/**
-	 * Issue a measurement command for the current state.
-	 *
-	 * @return		OK if the measurement command was successful.
-	 */
-	virtual int		measure();
-
-	/**
-	 * Collect the result of the most recent measurement.
-	 */
-	virtual int		collect();
-};
-
-/*
- * Driver 'main' command.
- */
-extern "C" __EXPORT int ms5611_main(int argc, char *argv[]);
-
-MS5611::MS5611(device::Device *interface, ms5611::prom_u &prom_buf, const char *path) :
-	VDev("MS5611", path),
-	_interface(interface),
-	_prom(prom_buf.s),
-	_measure_ticks(0),
-	_reports(nullptr),
-	_collect_phase(false),
-	_measure_phase(0),
-	_TEMP(0),
-	_OFF(0),
-	_SENS(0),
-	_msl_pressure(101325),
-	_baro_topic(nullptr),
-	_orb_class_instance(-1),
-	_class_instance(-1),
-	_sample_perf(perf_alloc(PC_ELAPSED, "ms5611_read")),
-	_measure_perf(perf_alloc(PC_ELAPSED, "ms5611_measure")),
-	_comms_errors(perf_alloc(PC_COUNT, "ms5611_comms_errors")),
-	_buffer_overflows(perf_alloc(PC_COUNT, "ms5611_buffer_overflows"))
-{
-	// work_cancel in stop_cycle called from the dtor will explode if we don't do this...
-	memset(&_work, 0, sizeof(_work));
-}
-
-MS5611::~MS5611()
-{
-	/* make sure we are truly inactive */
-	stop_cycle();
-
-	if (_class_instance != -1) {
-		unregister_class_devname(get_devname(), _class_instance);
-	}
-
-	/* free any existing reports */
-	if (_reports != nullptr) {
-		delete _reports;
-	}
-
-	// free perf counters
-	perf_free(_sample_perf);
-	perf_free(_measure_perf);
-	perf_free(_comms_errors);
-	perf_free(_buffer_overflows);
-
-	delete _interface;
-}
-
-int
-MS5611::init()
-{
-	int ret;
-	warnx("MS5611::init");
-
-	ret = VDev::init();
-
-	if (ret != OK) {
-		DEVICE_DEBUG("VDev init failed");
-		goto out;
-	}
-
-	/* allocate basic report buffers */
-	_reports = new ringbuffer::RingBuffer(2, sizeof(baro_report));
-
-	if (_reports == nullptr) {
-		DEVICE_DEBUG("can't get memory for reports");
-		ret = -ENOMEM;
-		goto out;
-	}
-
-	/* register alternate interfaces if we have to */
-	_class_instance = register_class_devname(BARO_BASE_DEVICE_PATH);
-
-	struct baro_report brp;
-	/* do a first measurement cycle to populate reports with valid data */
-	_measure_phase = 0;
-	_reports->flush();
-
-	/* this do..while is goto without goto */
-	do {
-		/* do temperature first */
-		if (OK != measure()) {
-			ret = -EIO;
-			warnx("temp measure failed");
-			break;
-		}
-
-		usleep(MS5611_CONVERSION_INTERVAL);
-
-		if (OK != collect()) {
-			ret = -EIO;
-			warnx("temp collect failed");
-			break;
-		}
-
-		/* now do a pressure measurement */
-		if (OK != measure()) {
-			ret = -EIO;
-			warnx("pressure collect failed");
-			break;
-		}
-
-		usleep(MS5611_CONVERSION_INTERVAL);
-
-		if (OK != collect()) {
-			ret = -EIO;
-			warnx("pressure collect failed");
-			break;
-		}
-
-		/* state machine will have generated a report, copy it out */
-		_reports->get(&brp);
-
-		ret = OK;
-
-		_baro_topic = orb_advertise_multi(ORB_ID(sensor_baro), &brp,
-						  &_orb_class_instance, (is_external()) ? ORB_PRIO_HIGH : ORB_PRIO_DEFAULT);
-
-		if (_baro_topic == nullptr) {
-			warnx("failed to create sensor_baro publication");
-		}
-
-		//warnx("sensor_baro publication %ld", _baro_topic);
-
-	} while (0);
-
-out:
-	return ret;
-}
-
-ssize_t
-MS5611::read(device::file_t *handlep, char *buffer, size_t buflen)
-{
-	unsigned count = buflen / sizeof(struct baro_report);
-	struct baro_report *brp = reinterpret_cast<struct baro_report *>(buffer);
-	int ret = 0;
-
-	/* buffer must be large enough */
-	if (count < 1) {
-		return -ENOSPC;
-	}
-
-	/* if automatic measurement is enabled */
-	if (_measure_ticks > 0) {
-
-		/*
-		 * While there is space in the caller's buffer, and reports, copy them.
-		 * Note that we may be pre-empted by the workq thread while we are doing this;
-		 * we are careful to avoid racing with them.
-		 */
-		while (count--) {
-			if (_reports->get(brp)) {
-				ret += sizeof(*brp);
-				brp++;
-			}
-		}
-
-		/* if there was no data, warn the caller */
-		return ret ? ret : -EAGAIN;
-	}
-
-	/* manual measurement - run one conversion */
-	do {
-		_measure_phase = 0;
-		_reports->flush();
-
-		/* do temperature first */
-		if (OK != measure()) {
-			ret = -EIO;
-			break;
-		}
-
-		usleep(MS5611_CONVERSION_INTERVAL);
-
-		if (OK != collect()) {
-			ret = -EIO;
-			break;
-		}
-
-		/* now do a pressure measurement */
-		if (OK != measure()) {
-			ret = -EIO;
-			break;
-		}
-
-		usleep(MS5611_CONVERSION_INTERVAL);
-
-		if (OK != collect()) {
-			ret = -EIO;
-			break;
-		}
-
-		/* state machine will have generated a report, copy it out */
-		if (_reports->get(brp)) {
-			ret = sizeof(*brp);
-		}
-
-	} while (0);
-
-	return ret;
-}
-
-int
-MS5611::ioctl(device::file_t *handlep, int cmd, unsigned long arg)
-{
-	switch (cmd) {
-
-	case SENSORIOCSPOLLRATE: {
-			switch (arg) {
-
-			/* switching to manual polling */
-			case SENSOR_POLLRATE_MANUAL:
-				stop_cycle();
-				_measure_ticks = 0;
-				return OK;
-
-			/* external signalling not supported */
-			case SENSOR_POLLRATE_EXTERNAL:
-
-			/* zero would be bad */
-			case 0:
-				return -EINVAL;
-
-			/* set default/max polling rate */
-			case SENSOR_POLLRATE_MAX:
-			case SENSOR_POLLRATE_DEFAULT: {
-					/* do we need to start internal polling? */
-					bool want_start = (_measure_ticks == 0);
-
-					/* set interval for next measurement to minimum legal value */
-					_measure_ticks = USEC2TICK(MS5611_CONVERSION_INTERVAL);
-
-					/* if we need to start the poll state machine, do it */
-					if (want_start) {
-						start_cycle();
-					}
-
-					return OK;
-				}
-
-			/* adjust to a legal polling interval in Hz */
-			default: {
-					/* do we need to start internal polling? */
-					bool want_start = (_measure_ticks == 0);
-
-					/* convert hz to tick interval via microseconds */
-					unsigned ticks = USEC2TICK(1000000 / arg);
-
-					/* check against maximum rate */
-					if ((unsigned long)ticks < USEC2TICK(MS5611_CONVERSION_INTERVAL)) {
-						return -EINVAL;
-					}
-
-					/* update interval for next measurement */
-					_measure_ticks = ticks;
-
-					/* if we need to start the poll state machine, do it */
-					if (want_start) {
-						start_cycle();
-					}
-
-					return OK;
-				}
-			}
-		}
-
-	case SENSORIOCGPOLLRATE:
-		if (_measure_ticks == 0) {
-			return SENSOR_POLLRATE_MANUAL;
-		}
-
-		return (1000 / _measure_ticks);
-
-	case SENSORIOCSQUEUEDEPTH: {
-			/* lower bound is mandatory, upper bound is a sanity check */
-			if ((arg < 1) || (arg > 100)) {
-				return -EINVAL;
-			}
-
-			if (!_reports->resize(arg)) {
-				return -ENOMEM;
-			}
-
-			return OK;
-		}
-
-	case SENSORIOCGQUEUEDEPTH:
-		return _reports->size();
-
-	case SENSORIOCRESET:
-		/*
-		 * Since we are initialized, we do not need to do anything, since the
-		 * PROM is correctly read and the part does not need to be configured.
-		 */
-		return OK;
-
-	case BAROIOCSMSLPRESSURE:
-
-		/* range-check for sanity */
-		if ((arg < 80000) || (arg > 120000)) {
-			return -EINVAL;
-		}
-
-		_msl_pressure = arg;
-		return OK;
-
-	case BAROIOCGMSLPRESSURE:
-		return _msl_pressure;
-
-	default:
-		break;
-	}
-
-	/* give it to the bus-specific superclass */
-	// return bus_ioctl(filp, cmd, arg);
-	return VDev::ioctl(handlep, cmd, arg);
-}
-
-void
-MS5611::start_cycle()
-{
-
-	/* reset the report ring and state machine */
-	_collect_phase = false;
-	_measure_phase = 0;
-	_reports->flush();
-
-	/* schedule a cycle to start things */
-	work_queue(HPWORK, &_work, (worker_t)&MS5611::cycle_trampoline, this, 1);
-}
-
-void
-MS5611::stop_cycle()
-{
-	work_cancel(HPWORK, &_work);
-}
-
-void
-MS5611::cycle_trampoline(void *arg)
-{
-	MS5611 *dev = reinterpret_cast<MS5611 *>(arg);
-
-	dev->cycle();
-}
-
-void
-MS5611::cycle()
-{
-	int ret;
-	unsigned dummy;
-
-	/* collection phase? */
-	if (_collect_phase) {
-
-		/* perform collection */
-		ret = collect();
-
-		if (ret != OK) {
-			if (ret == -6) {
-				/*
-				 * The ms5611 seems to regularly fail to respond to
-				 * its address; this happens often enough that we'd rather not
-				 * spam the console with a message for this.
-				 */
-			} else {
-				//DEVICE_LOG("collection error %d", ret);
-			}
-
-			/* issue a reset command to the sensor */
-			_interface->dev_ioctl(IOCTL_RESET, dummy);
-			/* reset the collection state machine and try again */
-			start_cycle();
-			return;
-		}
-
-		/* next phase is measurement */
-		_collect_phase = false;
-
-		/*
-		 * Is there a collect->measure gap?
-		 * Don't inject one after temperature measurements, so we can keep
-		 * doing pressure measurements at something close to the desired rate.
-		 */
-		if ((_measure_phase != 0) &&
-		    ((unsigned long)_measure_ticks > USEC2TICK(MS5611_CONVERSION_INTERVAL))) {
-
-			/* schedule a fresh cycle call when we are ready to measure again */
-			work_queue(HPWORK,
-				   &_work,
-				   (worker_t)&MS5611::cycle_trampoline,
-				   this,
-				   _measure_ticks - USEC2TICK(MS5611_CONVERSION_INTERVAL));
-
-			return;
-		}
-	}
-
-	/* measurement phase */
-	ret = measure();
-
-	if (ret != OK) {
-		//DEVICE_LOG("measure error %d", ret);
-		/* issue a reset command to the sensor */
-		_interface->dev_ioctl(IOCTL_RESET, dummy);
-		/* reset the collection state machine and try again */
-		start_cycle();
-		return;
-	}
-
-	/* next phase is collection */
-	_collect_phase = true;
-
-	/* schedule a fresh cycle call when the measurement is done */
-	work_queue(HPWORK,
-		   &_work,
-		   (worker_t)&MS5611::cycle_trampoline,
-		   this,
-		   USEC2TICK(MS5611_CONVERSION_INTERVAL));
-}
-
-int
-MS5611::measure()
-{
-	int ret;
-
-	perf_begin(_measure_perf);
-
-	/*
-	 * In phase zero, request temperature; in other phases, request pressure.
-	 */
-	unsigned addr = (_measure_phase == 0) ? ADDR_CMD_CONVERT_D2 : ADDR_CMD_CONVERT_D1;
-
-	/*
-	 * Send the command to begin measuring.
-	 */
-	ret = _interface->dev_ioctl(IOCTL_MEASURE, addr);
-
-	if (OK != ret) {
-		perf_count(_comms_errors);
-	}
-
-	perf_end(_measure_perf);
-
-	return ret;
-}
-
-int
-MS5611::collect()
-{
-	int ret;
-	uint32_t raw;
-
-	perf_begin(_sample_perf);
-
-	struct baro_report report;
-	/* this should be fairly close to the end of the conversion, so the best approximation of the time */
-	report.timestamp = hrt_absolute_time();
-	report.error_count = perf_event_count(_comms_errors);
-
-	/* read the most recent measurement - read offset/size are hardcoded in the interface */
-	ret = _interface->dev_read(0, (void *)&raw, 0);
-
-	if (ret < 0) {
-		perf_count(_comms_errors);
-		perf_end(_sample_perf);
-		return ret;
-	}
-
-	/* handle a measurement */
-	if (_measure_phase == 0) {
-
-		/* temperature offset (in ADC units) */
-		int32_t dT = (int32_t)raw - ((int32_t)_prom.c5_reference_temp << 8);
-
-		/* absolute temperature in centidegrees - note intermediate value is outside 32-bit range */
-		_TEMP = 2000 + (int32_t)(((int64_t)dT * _prom.c6_temp_coeff_temp) >> 23);
-
-		/* base sensor scale/offset values */
-		_SENS = ((int64_t)_prom.c1_pressure_sens << 15) + (((int64_t)_prom.c3_temp_coeff_pres_sens * dT) >> 8);
-		_OFF  = ((int64_t)_prom.c2_pressure_offset << 16) + (((int64_t)_prom.c4_temp_coeff_pres_offset * dT) >> 7);
-
-		/* temperature compensation */
-		if (_TEMP < 2000) {
-
-			int32_t T2 = POW2(dT) >> 31;
-
-			int64_t f = POW2((int64_t)_TEMP - 2000);
-			int64_t OFF2 = 5 * f >> 1;
-			int64_t SENS2 = 5 * f >> 2;
-
-			if (_TEMP < -1500) {
-				int64_t f2 = POW2(_TEMP + 1500);
-				OFF2 += 7 * f2;
-				SENS2 += 11 * f2 >> 1;
-			}
-
-			_TEMP -= T2;
-			_OFF  -= OFF2;
-			_SENS -= SENS2;
-		}
-
-	} else {
-
-		/* pressure calculation, result in Pa */
-		int32_t P = (((raw * _SENS) >> 21) - _OFF) >> 15;
-		_P = P * 0.01f;
-		_T = _TEMP * 0.01f;
-
-		/* generate a new report */
-		report.temperature = _TEMP / 100.0f;
-		report.pressure = P / 100.0f;		/* convert to millibar */
-
-		/* altitude calculations based on http://www.kansasflyer.org/index.asp?nav=Avi&sec=Alti&tab=Theory&pg=1 */
-
-		/*
-		 * PERFORMANCE HINT:
-		 *
-		 * The single precision calculation is 50 microseconds faster than the double
-		 * precision variant. It is however not obvious if double precision is required.
-		 * Pending more inspection and tests, we'll leave the double precision variant active.
-		 *
-		 * Measurements:
-		 * 	double precision: ms5611_read: 992 events, 258641us elapsed, min 202us max 305us
-		 *	single precision: ms5611_read: 963 events, 208066us elapsed, min 202us max 241us
-		 */
-
-		/* tropospheric properties (0-11km) for standard atmosphere */
-		const double T1 = 15.0 + 273.15;	/* temperature at base height in Kelvin */
-		const double a  = -6.5 / 1000;	/* temperature gradient in degrees per metre */
-		const double g  = 9.80665;	/* gravity constant in m/s/s */
-		const double R  = 287.05;	/* ideal gas constant in J/kg/K */
-
-		/* current pressure at MSL in kPa */
-		double p1 = _msl_pressure / 1000.0;
-
-		/* measured pressure in kPa */
-		double p = P / 1000.0;
-
-		/*
-		 * Solve:
-		 *
-		 *     /        -(aR / g)     \
-		 *    | (p / p1)          . T1 | - T1
-		 *     \                      /
-		 * h = -------------------------------  + h1
-		 *                   a
-		 */
-		report.altitude = (((pow((p / p1), (-(a * R) / g))) * T1) - T1) / a;
-
-		/* publish it */
-		if (!(_pub_blocked)) {
-			if (_baro_topic != (orb_advert_t)(-1)) {
-				/* publish it */
-				orb_publish(ORB_ID(sensor_baro), _baro_topic, &report);
-
-			} else {
-				printf("MS5611::collect _baro_topic not initialized\n");
-			}
-		}
-
-		if (_reports->force(&report)) {
-			perf_count(_buffer_overflows);
-		}
-
-		/* notify anyone waiting for data */
-		poll_notify(POLLIN);
-	}
-
-	/* update the measurement state machine */
-	INCREMENT(_measure_phase, MS5611_MEASUREMENT_RATIO + 1);
-
-	perf_end(_sample_perf);
-
-	return OK;
-}
-
-void
-MS5611::print_info()
-{
-	perf_print_counter(_sample_perf);
-	perf_print_counter(_comms_errors);
-	perf_print_counter(_buffer_overflows);
-	printf("poll interval:  %u ticks\n", _measure_ticks);
-	_reports->print_info("report queue");
-	printf("TEMP:           %d\n", _TEMP);
-	printf("SENS:           %lld\n", (long long)_SENS);
-	printf("OFF:            %lld\n", (long long)_OFF);
-	printf("P:              %.3f\n", (double)_P);
-	printf("T:              %.3f\n", (double)_T);
-	printf("MSL pressure:   %10.4f\n", (double)(_msl_pressure / 100.f));
-
-	printf("factory_setup             %u\n", _prom.factory_setup);
-	printf("c1_pressure_sens          %u\n", _prom.c1_pressure_sens);
-	printf("c2_pressure_offset        %u\n", _prom.c2_pressure_offset);
-	printf("c3_temp_coeff_pres_sens   %u\n", _prom.c3_temp_coeff_pres_sens);
-	printf("c4_temp_coeff_pres_offset %u\n", _prom.c4_temp_coeff_pres_offset);
-	printf("c5_reference_temp         %u\n", _prom.c5_reference_temp);
-	printf("c6_temp_coeff_temp        %u\n", _prom.c6_temp_coeff_temp);
-	printf("serial_and_crc            %u\n", _prom.serial_and_crc);
-}
-
-/**
- * Local functions in support of the shell command.
- */
-namespace ms5611
-{
-
-/*
-  list of supported bus configurations
- */
-struct ms5611_bus_option {
-	enum MS5611_BUS busid;
-	const char *devpath;
-	MS5611_constructor interface_constructor;
-	uint8_t busnum;
-	MS5611 *dev;
-} bus_options[] = {
-#if 0
-#if defined(PX4_SPIDEV_EXT_BARO) && defined(PX4_SPI_BUS_EXT)
-	{ MS5611_BUS_SPI_EXTERNAL, "/dev/ms5611_spi_ext", &MS5611_spi_interface, PX4_SPI_BUS_EXT, NULL },
-#endif
-#ifdef PX4_SPIDEV_BARO
-	{ MS5611_BUS_SPI_INTERNAL, "/dev/ms5611_spi_int", &MS5611_spi_interface, PX4_SPI_BUS_SENSORS, NULL },
-#endif
-#ifdef PX4_I2C_BUS_ONBOARD
-	{ MS5611_BUS_I2C_INTERNAL, "/dev/ms5611_int", &MS5611_i2c_interface, PX4_I2C_BUS_ONBOARD, NULL },
-#endif
-#ifdef PX4_I2C_BUS_EXPANSION
-	{ MS5611_BUS_I2C_EXTERNAL, "/dev/ms5611_ext", &MS5611_i2c_interface, PX4_I2C_BUS_EXPANSION, NULL },
-#endif
-#endif
-#ifdef PX4_SIM_BUS_TEST
-	{ MS5611_BUS_SIM_EXTERNAL, "/dev/ms5611_sim", &MS5611_sim_interface, PX4_SIM_BUS_TEST, NULL },
-#endif
-};
-#define NUM_BUS_OPTIONS (sizeof(bus_options)/sizeof(bus_options[0]))
-
-bool	start_bus(struct ms5611_bus_option &bus);
-bool	find_bus(enum MS5611_BUS busid, ms5611_bus_option &bus);
-int	start(enum MS5611_BUS busid);
-int	test(enum MS5611_BUS busid);
-int	reset(enum MS5611_BUS busid);
-int	info();
-int	calibrate(unsigned altitude, enum MS5611_BUS busid);
-void	usage();
-
-/**
- * MS5611 crc4 cribbed from the datasheet
- */
-bool
-crc4(uint16_t *n_prom)
-{
-	int16_t cnt;
-	uint16_t n_rem;
-	uint16_t crc_read;
-	uint8_t n_bit;
-
-	n_rem = 0x00;
-
-	/* save the read crc */
-	crc_read = n_prom[7];
-
-	/* remove CRC byte */
-	n_prom[7] = (0xFF00 & (n_prom[7]));
-
-	for (cnt = 0; cnt < 16; cnt++) {
-		/* uneven bytes */
-		if (cnt & 1) {
-			n_rem ^= (uint8_t)((n_prom[cnt >> 1]) & 0x00FF);
-
-		} else {
-			n_rem ^= (uint8_t)(n_prom[cnt >> 1] >> 8);
-		}
-
-		for (n_bit = 8; n_bit > 0; n_bit--) {
-			if (n_rem & 0x8000) {
-				n_rem = (n_rem << 1) ^ 0x3000;
-
-			} else {
-				n_rem = (n_rem << 1);
-			}
-		}
-	}
-
-	/* final 4 bit remainder is CRC value */
-	n_rem = (0x000F & (n_rem >> 12));
-	n_prom[7] = crc_read;
-
-	/* return true if CRCs match */
-	return (0x000F & crc_read) == (n_rem ^ 0x00);
-}
-
-
-/**
- * Start the driver.
- */
-bool
-start_bus(struct ms5611_bus_option &bus)
-{
-	if (bus.dev != nullptr) {
-		warnx("bus option already started");
-		return false;
-	}
-
-	prom_u prom_buf;
-	device::Device *interface = bus.interface_constructor(prom_buf, bus.busnum);
-
-	if (interface->init() != OK) {
-		delete interface;
-		warnx("no device on bus %u", (unsigned)bus.busid);
-		return false;
-	}
-
-	bus.dev = new MS5611(interface, prom_buf, bus.devpath);
-
-	if (bus.dev != nullptr && OK != bus.dev->init()) {
-		delete bus.dev;
-		bus.dev = NULL;
-		warnx("bus init failed %p", bus.dev);
-		return false;
-	}
-
-	int fd = px4_open(bus.devpath, O_RDONLY);
-
-	/* set the poll rate to default, starts automatic data collection */
-	if (fd == -1) {
-		warnx("can't open baro device");
-		return false;
-	}
-
-	if (px4_ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) {
-		px4_close(fd);
-		warnx("failed setting default poll rate");
-		return false;
-	}
-
-	px4_close(fd);
-	return true;
-}
-
-
-/**
- * Start the driver.
- *
- * This function call only returns once the driver
- * is either successfully up and running or failed to start.
- */
-int
-start(enum MS5611_BUS busid)
-{
-	uint8_t i;
-	bool started = false;
-
-	for (i = 0; i < NUM_BUS_OPTIONS; i++) {
-		if (busid == MS5611_BUS_ALL && bus_options[i].dev != NULL) {
-			// this device is already started
-			continue;
-		}
-
-		if (busid != MS5611_BUS_ALL && bus_options[i].busid != busid) {
-			// not the one that is asked for
-			continue;
-		}
-
-		started |= start_bus(bus_options[i]);
-	}
-
-	if (!started) {
-		warnx("driver start failed");
-		return 1;
-	}
-
-	// one or more drivers started OK
-	return 0;
-}
-
-
-/**
- * find a bus structure for a busid
- */
-bool find_bus(enum MS5611_BUS busid, ms5611_bus_option &bus)
-{
-	for (uint8_t i = 0; i < NUM_BUS_OPTIONS; i++) {
-		if ((busid == MS5611_BUS_ALL ||
-		     busid == bus_options[i].busid) && bus_options[i].dev != NULL) {
-			bus = bus_options[i];
-			return true;
-		}
-	}
-
-	PX4_WARN("bus %u not started", (unsigned)busid);
-	return false;
-}
-
-/**
- * Perform some basic functional tests on the driver;
- * make sure we can collect data from the sensor in polled
- * and automatic modes.
- */
-int
-test(enum MS5611_BUS busid)
-{
-	struct ms5611_bus_option bus;
-
-	if (!find_bus(busid, bus)) {
-		return 1;
-	}
-
-	struct baro_report report;
-
-	ssize_t sz;
-
-	int ret;
-
-	int fd;
-
-	fd = px4_open(bus.devpath, O_RDONLY);
-
-	if (fd < 0) {
-		warn("open failed (try 'ms5611 start' if the driver is not running)");
-		return 1;
-	}
-
-	/* do a simple demand read */
-	sz = px4_read(fd, &report, sizeof(report));
-
-	if (sz != sizeof(report)) {
-		warn("immediate read failed");
-		return 1;
-	}
-
-	warnx("single read");
-	warnx("pressure:    %10.4f", (double)report.pressure);
-	warnx("altitude:    %11.4f", (double)report.altitude);
-	warnx("temperature: %8.4f", (double)report.temperature);
-	warnx("time:        %lld", (long long)report.timestamp);
-
-	/* set the queue depth to 10 */
-	if (OK != px4_ioctl(fd, SENSORIOCSQUEUEDEPTH, 10)) {
-		warnx("failed to set queue depth");
-		return 1;
-	}
-
-	/* start the sensor polling at 2Hz */
-	if (OK != px4_ioctl(fd, SENSORIOCSPOLLRATE, 2)) {
-		warnx("failed to set 2Hz poll rate");
-		return 1;
-	}
-
-	/* read the sensor 5x and report each value */
-	for (unsigned i = 0; i < 5; i++) {
-		struct pollfd fds;
-
-		/* wait for data to be ready */
-		fds.fd = fd;
-		fds.events = POLLIN;
-		ret = poll(&fds, 1, 2000);
-
-		if (ret != 1) {
-			warnx("timed out waiting for sensor data");
-		}
-
-		/* now go get it */
-		sz = px4_read(fd, &report, sizeof(report));
-
-		if (sz != sizeof(report)) {
-			warn("periodic read failed");
-			return 1;
-		}
-
-		warnx("periodic read %u", i);
-		warnx("pressure:    %10.4f", (double)report.pressure);
-		warnx("altitude:    %11.4f", (double)report.altitude);
-		warnx("temperature: %8.4f", (double)report.temperature);
-		warnx("time:        %lld", (long long)report.timestamp);
-	}
-
-	close(fd);
-	warnx("PASS");
-	return 0;
-}
-
-/**
- * Reset the driver.
- */
-int
-reset(enum MS5611_BUS busid)
-{
-	struct ms5611_bus_option bus;
-
-	if (!find_bus(busid, bus)) {
-		return 1;
-	}
-
-	int fd;
-
-	fd = open(bus.devpath, O_RDONLY);
-
-	if (fd < 0) {
-		warn("failed ");
-		return 1;
-	}
-
-	if (px4_ioctl(fd, SENSORIOCRESET, 0) < 0) {
-		warn("driver reset failed");
-		return 1;
-	}
-
-	if (px4_ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) {
-		warn("driver poll restart failed");
-		return 1;
-	}
-
-	return 0;
-}
-
-/**
- * Print a little info about the driver.
- */
-int
-info()
-{
-	for (uint8_t i = 0; i < NUM_BUS_OPTIONS; i++) {
-		struct ms5611_bus_option &bus = bus_options[i];
-
-		if (bus.dev != nullptr) {
-			warnx("%s", bus.devpath);
-			bus.dev->print_info();
-		}
-	}
-
-	return 0;
-}
-
-/**
- * Calculate actual MSL pressure given current altitude
- */
-int
-calibrate(unsigned altitude, enum MS5611_BUS busid)
-{
-	struct ms5611_bus_option bus;
-
-	if (!find_bus(busid, bus)) {
-		return 1;
-	}
-
-	struct baro_report report;
-
-	float	pressure;
-
-	float	p1;
-
-	int fd;
-
-	fd = px4_open(bus.devpath, O_RDONLY);
-
-	if (fd < 0) {
-		warn("open failed (try 'ms5611 start' if the driver is not running)");
-		return 1;
-	}
-
-	/* start the sensor polling at max */
-	if (OK != px4_ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_MAX)) {
-		warnx("failed to set poll rate");
-		return 1;
-	}
-
-	/* average a few measurements */
-	pressure = 0.0f;
-
-	for (unsigned i = 0; i < 20; i++) {
-		struct pollfd fds;
-		int ret;
-		ssize_t sz;
-
-		/* wait for data to be ready */
-		fds.fd = fd;
-		fds.events = POLLIN;
-		ret = poll(&fds, 1, 1000);
-
-		if (ret != 1) {
-			warnx("timed out waiting for sensor data");
-			return 1;
-		}
-
-		/* now go get it */
-		sz = px4_read(fd, &report, sizeof(report));
-
-		if (sz != sizeof(report)) {
-			warn("sensor read failed");
-			return 1;
-		}
-
-		pressure += report.pressure;
-	}
-
-	pressure /= 20;		/* average */
-	pressure /= 10;		/* scale from millibar to kPa */
-
-	/* tropospheric properties (0-11km) for standard atmosphere */
-	const float T1 = 15.0 + 273.15;	/* temperature at base height in Kelvin */
-	const float a  = -6.5 / 1000;	/* temperature gradient in degrees per metre */
-	const float g  = 9.80665f;	/* gravity constant in m/s/s */
-	const float R  = 287.05f;	/* ideal gas constant in J/kg/K */
-
-	warnx("averaged pressure %10.4fkPa at %um", (double)pressure, altitude);
-
-	p1 = pressure * (powf(((T1 + (a * (float)altitude)) / T1), (g / (a * R))));
-
-	warnx("calculated MSL pressure %10.4fkPa", (double)p1);
-
-	/* save as integer Pa */
-	p1 *= 1000.0f;
-
-	if (px4_ioctl(fd, BAROIOCSMSLPRESSURE, (unsigned long)p1) != OK) {
-		warn("BAROIOCSMSLPRESSURE");
-		return 1;
-	}
-
-	close(fd);
-	return 0;
-}
-
-void
-usage()
-{
-	warnx("missing command: try 'start', 'info', 'test', 'test2', 'reset', 'calibrate'");
-	warnx("options:");
-	warnx("    -X    (external I2C bus)");
-	warnx("    -I    (intternal I2C bus)");
-	warnx("    -S    (Simulation bus)");
-}
-
-} // namespace
-
-int
-ms5611_main(int argc, char *argv[])
-{
-	enum MS5611_BUS busid = MS5611_BUS_ALL;
-	int ch;
-	int ret;
-
-	if (argc < 2) {
-		ms5611::usage();
-		return 1;
-	}
-
-	/* jump over start/off/etc and look at options first */
-	int myoptind = 1;
-	const char *myoptarg = NULL;
-
-	while ((ch = px4_getopt(argc, argv, "XIS", &myoptind, &myoptarg)) != EOF) {
-		printf("ch = %d\n", ch);
-
-		switch (ch) {
-		case 'X':
-			busid = MS5611_BUS_I2C_EXTERNAL;
-			break;
-
-		case 'I':
-			busid = MS5611_BUS_I2C_INTERNAL;
-			break;
-
-		case 'S':
-			busid = MS5611_BUS_SIM_EXTERNAL;
-			break;
-
-		default:
-			ms5611::usage();
-			return 1;
-		}
-	}
-
-	const char *verb = argv[myoptind];
-
-	/*
-	 * Start/load the driver.
-	 */
-	if (!strcmp(verb, "start")) {
-		ret = ms5611::start(busid);
-	}
-
-	/*
-	 * Test the driver/device.
-	 */
-	else if (!strcmp(verb, "test")) {
-		ret = ms5611::test(busid);
-	}
-
-	/*
-	 * Reset the driver.
-	 */
-	else if (!strcmp(verb, "reset")) {
-		ret = ms5611::reset(busid);
-	}
-
-	/*
-	 * Print driver information.
-	 */
-	else if (!strcmp(verb, "info")) {
-		ret = ms5611::info();
-	}
-
-	/*
-	 * Perform MSL pressure calibration given an altitude in metres
-	 */
-	else if (!strcmp(verb, "calibrate")) {
-		if (argc < 2) {
-			PX4_WARN("missing altitude");
-		}
-
-		long altitude = strtol(argv[optind + 1], nullptr, 10);
-
-		ret = ms5611::calibrate(altitude, busid);
-
-	} else {
-		ms5611::usage();
-		warnx("unrecognised command, try 'start', 'test', 'reset' or 'info'");
-		return 1;
-	}
-
-	return ret;
-}
diff --git a/src/drivers/ms5611/ms5611_sim.cpp b/src/drivers/ms5611/ms5611_sim.cpp
deleted file mode 100644
index 72c02f6b38..0000000000
--- a/src/drivers/ms5611/ms5611_sim.cpp
+++ /dev/null
@@ -1,206 +0,0 @@
-/****************************************************************************
- *
- *   Copyright (c) 2013 PX4 Development Team. All rights reserved.
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions
- * are met:
- *
- * 1. Redistributions of source code must retain the above copyright
- *    notice, this list of conditions and the following disclaimer.
- * 2. Redistributions in binary form must reproduce the above copyright
- *    notice, this list of conditions and the following disclaimer in
- *    the documentation and/or other materials provided with the
- *    distribution.
- * 3. Neither the name PX4 nor the names of its contributors may be
- *    used to endorse or promote products derived from this software
- *    without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
- * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
- * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- *
- ****************************************************************************/
-
-/**
- * @file ms5611_i2c.cpp
- *
- * SIM interface for MS5611
- */
-
-/* XXX trim includes */
-#include <px4_config.h>
-#include <px4_defines.h>
-
-#include <sys/types.h>
-#include <stdint.h>
-#include <stdbool.h>
-#include <assert.h>
-//#include <debug.h>
-#include <errno.h>
-#include <unistd.h>
-
-#include <drivers/device/sim.h>
-#include "ms5611.h"
-#include "board_config.h"
-
-device::Device *MS5611_sim_interface(ms5611::prom_u &prom_buf);
-
-class MS5611_SIM : public device::SIM
-{
-public:
-	MS5611_SIM(uint8_t bus, ms5611::prom_u &prom_buf);
-	virtual ~MS5611_SIM();
-
-	virtual int	init();
-	virtual int	dev_read(unsigned offset, void *data, unsigned count);
-	virtual int	dev_ioctl(unsigned operation, unsigned &arg);
-
-	virtual int	transfer(const uint8_t *send, unsigned send_len,
-				 uint8_t *recv, unsigned recv_len);
-private:
-	ms5611::prom_u	&_prom;
-
-	/**
-	 * Send a reset command to the MS5611.
-	 *
-	 * This is required after any bus reset.
-	 */
-	int		_reset();
-
-	/**
-	 * Send a measure command to the MS5611.
-	 *
-	 * @param addr		Which address to use for the measure operation.
-	 */
-	int		_measure(unsigned addr);
-
-	/**
-	 * Read the MS5611 PROM
-	 *
-	 * @return		PX4_OK if the PROM reads successfully.
-	 */
-	int		_read_prom();
-
-};
-
-device::Device *
-MS5611_sim_interface(ms5611::prom_u &prom_buf, uint8_t busnum)
-{
-	return new MS5611_SIM(busnum, prom_buf);
-}
-
-MS5611_SIM::MS5611_SIM(uint8_t bus, ms5611::prom_u &prom) :
-	SIM("MS5611_SIM", "/dev/MS5611_SIM", bus, 0),
-	_prom(prom)
-{
-}
-
-MS5611_SIM::~MS5611_SIM()
-{
-}
-
-int
-MS5611_SIM::init()
-{
-	return SIM::init();
-}
-
-int
-MS5611_SIM::dev_read(unsigned offset, void *data, unsigned count)
-{
-	union _cvt {
-		uint8_t	b[4];
-		uint32_t w;
-	} *cvt = (_cvt *)data;
-	uint8_t buf[3];
-
-	/* read the most recent measurement */
-	uint8_t cmd = 0;
-	int ret = transfer(&cmd, 1, &buf[0], 3);
-
-	if (ret == PX4_OK) {
-		/* fetch the raw value */
-		cvt->b[0] = buf[2];
-		cvt->b[1] = buf[1];
-		cvt->b[2] = buf[0];
-		cvt->b[3] = 0;
-	}
-
-	return ret;
-}
-
-int
-MS5611_SIM::dev_ioctl(unsigned operation, unsigned &arg)
-{
-	int ret;
-
-	switch (operation) {
-	case IOCTL_RESET:
-		ret = _reset();
-		break;
-
-	case IOCTL_MEASURE:
-		ret = _measure(arg);
-		break;
-
-	default:
-		ret = EINVAL;
-	}
-
-	return ret;
-}
-
-int
-MS5611_SIM::_reset()
-{
-	unsigned	old_retrycount = _retries;
-	uint8_t		cmd = ADDR_RESET_CMD;
-	int		result;
-
-	/* bump the retry count */
-	_retries = 10;
-	result = transfer(&cmd, 1, nullptr, 0);
-	_retries = old_retrycount;
-
-	return result;
-}
-
-int
-MS5611_SIM::_measure(unsigned addr)
-{
-	/*
-	 * Disable retries on this command; we can't know whether failure
-	 * means the device did or did not see the command.
-	 */
-	_retries = 0;
-
-	uint8_t cmd = addr;
-	return transfer(&cmd, 1, nullptr, 0);
-}
-
-int
-MS5611_SIM::_read_prom()
-{
-	int ret = 1;
-	// TODO input simlation data
-	return ret;
-}
-
-int
-MS5611_SIM::transfer(const uint8_t *send, unsigned send_len,
-		     uint8_t *recv, unsigned recv_len)
-{
-	// TODO add Simulation data connection so calls retrieve
-	// data from the simulator
-	return 0;
-}

From bf9f3b60613473bf4696d37a3716dc92dd723dd7 Mon Sep 17 00:00:00 2001
From: James Goppert <jgoppert@users.noreply.github.com>
Date: Fri, 24 Jun 2016 05:44:39 -0400
Subject: [PATCH 0807/1230] Added QAV-R (raceblade) 5". (#4897)

* Added QAV-R (raceblade) 5".

* Added I gain for qav-r.
---
 ROMFS/px4fmu_common/init.d/4002_qavr5 | 34 +++++++++++++++++++++++++++
 1 file changed, 34 insertions(+)
 create mode 100644 ROMFS/px4fmu_common/init.d/4002_qavr5

diff --git a/ROMFS/px4fmu_common/init.d/4002_qavr5 b/ROMFS/px4fmu_common/init.d/4002_qavr5
new file mode 100644
index 0000000000..142982bf4b
--- /dev/null
+++ b/ROMFS/px4fmu_common/init.d/4002_qavr5
@@ -0,0 +1,34 @@
+#!nsh
+#
+# @name Lumenier QAV-R (raceblade) 5" arms
+#
+# @type Quadrotor x
+#
+# @output AUX1 feed-through of RC AUX1 channel
+# @output AUX2 feed-through of RC AUX2 channel
+# @output AUX3 feed-through of RC AUX3 channel
+#
+# @maintainer James Goppert <james.goppert@gmail.com>
+#
+
+sh /etc/init.d/4001_quad_x
+
+if [ $AUTOCNF == yes ]
+then
+	param set MC_ROLL_P 8.0
+	param set MC_ROLLRATE_P 0.08
+	param set MC_ROLLRATE_I 0.05
+	param set MC_ROLLRATE_D 0.003
+	param set MC_PITCH_P 8.0
+	param set MC_PITCHRATE_P 0.1
+	param set MC_PITCHRATE_I 0.05
+	param set MC_PITCHRATE_D 0.003
+	param set MC_YAW_P 2.8
+	param set MC_YAWRATE_P 0.15
+	param set MC_YAWRATE_I 0.1
+	param set MC_YAWRATE_D 0.0
+	param set MC_YAW_FF 0.5
+	param set PWM_MIN 1075
+	param set MPC_THR_MIN 0.06
+	param set MPC_MANTHR_MIN 0.06
+fi

From e432a406b6980b1d15f6f14212e10a3a4c7fbac9 Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Fri, 24 Jun 2016 11:42:53 +0200
Subject: [PATCH 0808/1230] Update MAVLink submodules

---
 .gitmodules | 12 ++++++------
 1 file changed, 6 insertions(+), 6 deletions(-)

diff --git a/.gitmodules b/.gitmodules
index fe09b6399c..513501ab5c 100644
--- a/.gitmodules
+++ b/.gitmodules
@@ -1,12 +1,15 @@
 [submodule "mavlink/include/mavlink/v1.0"]
 	path = mavlink/include/mavlink/v1.0
-	url = git://github.com/mavlink/c_library.git
+	url = https://github.com/mavlink/c_library_v1.git
+[submodule "mavlink/include/mavlink/v2.0"]
+	path = mavlink/include/mavlink/v2.0
+	url = https://github.com/mavlink/c_library_v2.git
 [submodule "NuttX"]
 	path = NuttX
-	url = git://github.com/PX4/NuttX.git
+	url = https://github.com/PX4/NuttX.git
 [submodule "src/modules/uavcan/libuavcan"]
 	path = src/modules/uavcan/libuavcan
-	url = git://github.com/UAVCAN/libuavcan.git
+	url = https://github.com/UAVCAN/libuavcan.git
 [submodule "Tools/genmsg"]
 	path = Tools/genmsg
 	url = https://github.com/ros/genmsg.git
@@ -38,6 +41,3 @@
 [submodule "src/drivers/gps/devices"]
 	path = src/drivers/gps/devices
 	url = https://github.com/PX4/GpsDrivers.git
-[submodule "mavlink/include/mavlink/v2.0"]
-	path = mavlink/include/mavlink/v2.0
-	url = git://github.com/mavlink/c_library.git

From 645204eb4276aeacc0945d4db795389d582d9454 Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Fri, 24 Jun 2016 12:47:59 +0200
Subject: [PATCH 0809/1230] Update MAVLink library versions

---
 mavlink/include/mavlink/v1.0 | 2 +-
 mavlink/include/mavlink/v2.0 | 2 +-
 2 files changed, 2 insertions(+), 2 deletions(-)

diff --git a/mavlink/include/mavlink/v1.0 b/mavlink/include/mavlink/v1.0
index d8ec54a69f..6adda4dbc1 160000
--- a/mavlink/include/mavlink/v1.0
+++ b/mavlink/include/mavlink/v1.0
@@ -1 +1 @@
-Subproject commit d8ec54a69f2d35f0e46c0115628d82d113cc5974
+Subproject commit 6adda4dbc1b68a29b8729c6e7ab34c9a1851c4df
diff --git a/mavlink/include/mavlink/v2.0 b/mavlink/include/mavlink/v2.0
index 699a2b0df1..1e2920c68d 160000
--- a/mavlink/include/mavlink/v2.0
+++ b/mavlink/include/mavlink/v2.0
@@ -1 +1 @@
-Subproject commit 699a2b0df1b259a962a01af788e8306ee8d016d8
+Subproject commit 1e2920c68df46c25b7a14ca6cc47a478c6081d81

From 52c790b1847eba0659a6c63f4b144112c8b78304 Mon Sep 17 00:00:00 2001
From: Daniel Agar <daniel@agar.ca>
Date: Fri, 24 Jun 2016 20:21:46 -0400
Subject: [PATCH 0810/1230] circleci sync submodules recursive

---
 circle.yml | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/circle.yml b/circle.yml
index 4eae4127a7..9c6d84e4af 100644
--- a/circle.yml
+++ b/circle.yml
@@ -4,7 +4,7 @@ machine:
 
 checkout:
   post:
-    - git submodule sync
+    - git submodule sync --recursive
     - git submodule update --init --recursive
 
 ## Customize dependencies

From 8ba5afcd5a87aa5c965ca2e141e3e68148cb7811 Mon Sep 17 00:00:00 2001
From: Daniel Agar <daniel@agar.ca>
Date: Sat, 25 Jun 2016 00:25:13 -0400
Subject: [PATCH 0811/1230] circleci ccache (#4906)

---
 circle.yml | 7 ++++---
 1 file changed, 4 insertions(+), 3 deletions(-)

diff --git a/circle.yml b/circle.yml
index 9c6d84e4af..492719d00b 100644
--- a/circle.yml
+++ b/circle.yml
@@ -9,11 +9,12 @@ checkout:
 
 ## Customize dependencies
 dependencies:
+  cache_directories:
+    - "~/.ccache"
   pre:
     - docker pull px4io/px4-dev-nuttx-gcc4.9
-    - docker info
 
 test:
   override:
-    #- sudo docker run --rm -v `pwd`:`pwd`:rw -v $HOME/.ccache:$HOME/.ccache:rw -e CCACHE_DIR=$HOME/.ccache -w=`pwd` --user=$UID -it px4io/px4-dev-base /bin/bash -c "make"
-    - docker run -v `pwd`:`pwd`:rw -w=`pwd` --user=$UID -it px4io/px4-dev-nuttx-gcc4.9 /bin/bash -c "make px4fmu-v4_default"
+    - docker run --rm -v `pwd`:`pwd`:rw -w=`pwd` -v $HOME/.ccache:$HOME/.ccache:rw -e CCACHE_DIR=$HOME/.ccache --user=$UID -it px4io/px4-dev-nuttx-gcc4.9 /bin/bash -c "ccache -z; make px4fmu-v4_default; ccache -s"
+

From 705a08bbbda4a3defa082332e82864b45e5bc19a Mon Sep 17 00:00:00 2001
From: Andreas Daniel Antener <antener_a@gmx.ch>
Date: Sat, 25 Jun 2016 10:59:50 +0200
Subject: [PATCH 0812/1230] do not modify attitude setpoint in velocity
 controlled mode (#4905)

---
 .../mc_pos_control/mc_pos_control_main.cpp    | 68 ++++++++++---------
 1 file changed, 35 insertions(+), 33 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 7438a4ac91..9bb7b56d49 100644
--- a/src/modules/mc_pos_control/mc_pos_control_main.cpp
+++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp
@@ -2002,48 +2002,50 @@ MulticopterPositionControl::task_main()
 
 			math::Matrix<3, 3> R_sp;
 
-			// construct attitude setpoint rotation matrix. modify the setpoints for roll
-			// and pitch such that they reflect the user's intention even if a yaw error
-			// (yaw_sp - yaw) is present. In the presence of a yaw error constructing a rotation matrix
-			// from the pure euler angle setpoints will lead to unexpected attitude behaviour from
-			// the user's view as the euler angle sequence uses the  yaw setpoint and not the current
-			// heading of the vehicle.
+			if (!_control_mode.flag_control_velocity_enabled) {
+				// construct attitude setpoint rotation matrix. modify the setpoints for roll
+				// and pitch such that they reflect the user's intention even if a yaw error
+				// (yaw_sp - yaw) is present. In the presence of a yaw error constructing a rotation matrix
+				// from the pure euler angle setpoints will lead to unexpected attitude behaviour from
+				// the user's view as the euler angle sequence uses the  yaw setpoint and not the current
+				// heading of the vehicle.
 
-			// calculate our current yaw error
-			float yaw_error = _wrap_pi(_att_sp.yaw_body - _yaw);
+				// calculate our current yaw error
+				float yaw_error = _wrap_pi(_att_sp.yaw_body - _yaw);
 
-			// compute the vector obtained by rotating a z unit vector by the rotation
-			// given by the roll and pitch commands of the user
-			math::Vector<3> zB = {0, 0, 1};
-			math::Matrix<3,3> R_sp_roll_pitch;
-			R_sp_roll_pitch.from_euler(_att_sp.roll_body, _att_sp.pitch_body, 0);
-			math::Vector<3> z_roll_pitch_sp = R_sp_roll_pitch * zB;
+				// compute the vector obtained by rotating a z unit vector by the rotation
+				// given by the roll and pitch commands of the user
+				math::Vector<3> zB = {0, 0, 1};
+				math::Matrix<3,3> R_sp_roll_pitch;
+				R_sp_roll_pitch.from_euler(_att_sp.roll_body, _att_sp.pitch_body, 0);
+				math::Vector<3> z_roll_pitch_sp = R_sp_roll_pitch * zB;
 
 
-			// transform the vector into a new frame which is rotated around the z axis
-			// by the current yaw error. this vector defines the desired tilt when we look
-			// into the direction of the desired heading
-			math::Matrix<3,3> R_yaw_correction;
-			R_yaw_correction.from_euler(0.0f, 0.0f, -yaw_error);
-			z_roll_pitch_sp = R_yaw_correction * z_roll_pitch_sp;
+				// transform the vector into a new frame which is rotated around the z axis
+				// by the current yaw error. this vector defines the desired tilt when we look
+				// into the direction of the desired heading
+				math::Matrix<3,3> R_yaw_correction;
+				R_yaw_correction.from_euler(0.0f, 0.0f, -yaw_error);
+				z_roll_pitch_sp = R_yaw_correction * z_roll_pitch_sp;
 
-			// use the formula z_roll_pitch_sp = R_tilt * [0;0;1]
-			// to calculate the new desired roll and pitch angles
-			// R_tilt can be written as a function of the new desired roll and pitch
-			// angles. we get three equations and have to solve for 2 unknowns
-			float pitch_new = asinf(z_roll_pitch_sp(0));
-			float roll_new = -atan2f(z_roll_pitch_sp(1), z_roll_pitch_sp(2));
+				// use the formula z_roll_pitch_sp = R_tilt * [0;0;1]
+				// to calculate the new desired roll and pitch angles
+				// R_tilt can be written as a function of the new desired roll and pitch
+				// angles. we get three equations and have to solve for 2 unknowns
+				float pitch_new = asinf(z_roll_pitch_sp(0));
+				float roll_new = -atan2f(z_roll_pitch_sp(1), z_roll_pitch_sp(2));
 
-			R_sp.from_euler(roll_new, pitch_new, _att_sp.yaw_body);
+				R_sp.from_euler(roll_new, pitch_new, _att_sp.yaw_body);
 
-			memcpy(&_att_sp.R_body[0], R_sp.data, sizeof(_att_sp.R_body));
+				memcpy(&_att_sp.R_body[0], R_sp.data, sizeof(_att_sp.R_body));
 
-			/* reset the acceleration set point for all non-attitude flight modes */
-			if (!(_control_mode.flag_control_offboard_enabled &&
-					!(_control_mode.flag_control_position_enabled ||
-					  _control_mode.flag_control_velocity_enabled))) {
+				/* reset the acceleration set point for all non-attitude flight modes */
+				if (!(_control_mode.flag_control_offboard_enabled &&
+						!(_control_mode.flag_control_position_enabled ||
+						  _control_mode.flag_control_velocity_enabled))) {
 
-				_thrust_sp_prev = R_sp * math::Vector<3>(0, 0, -_att_sp.thrust);
+					_thrust_sp_prev = R_sp * math::Vector<3>(0, 0, -_att_sp.thrust);
+				}
 			}
 
 			/* copy quaternion setpoint to attitude setpoint topic */

From ae6600e48fe2dd72bec0a4eadba66c11491762e8 Mon Sep 17 00:00:00 2001
From: Henry Zhang <zhanghui629@gmail.com>
Date: Sat, 25 Jun 2016 17:02:02 +0800
Subject: [PATCH 0813/1230] MAVLink app: fix mavlink forwarding issue. (#4907)

---
 src/modules/mavlink/mavlink_main.cpp | 52 ----------------------------
 src/modules/mavlink/mavlink_main.h   |  2 +-
 2 files changed, 1 insertion(+), 53 deletions(-)

diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp
index 146ce40109..1d4d3a58b3 100644
--- a/src/modules/mavlink/mavlink_main.cpp
+++ b/src/modules/mavlink/mavlink_main.cpp
@@ -993,58 +993,6 @@ Mavlink::send_bytes(const uint8_t *buf, unsigned packet_len)
 	pthread_mutex_unlock(&_send_mutex);
 }
 
-void
-Mavlink::resend_message(mavlink_message_t *msg)
-{
-	/* If the wait until transmit flag is on, only transmit after we've received messages.
-	   Otherwise, transmit all the time. */
-	if (!should_transmit()) {
-		return;
-	}
-
-	pthread_mutex_lock(&_send_mutex);
-
-	unsigned buf_free = get_free_tx_buf();
-
-	_last_write_try_time = hrt_absolute_time();
-
-	unsigned packet_len = msg->len + MAVLINK_NUM_NON_PAYLOAD_BYTES;
-
-	/* check if there is space in the buffer, let it overflow else */
-	if (buf_free < packet_len) {
-		/* no enough space in buffer to send */
-		count_txerr();
-		count_txerrbytes(packet_len);
-		pthread_mutex_unlock(&_send_mutex);
-		return;
-	}
-
-	uint8_t buf[MAVLINK_MAX_PACKET_LEN];
-
-	/* header and payload */
-	memcpy(&buf[0], &msg->magic, MAVLINK_NUM_HEADER_BYTES + msg->len);
-
-	/* checksum */
-	buf[MAVLINK_NUM_HEADER_BYTES + msg->len] = (uint8_t)(msg->checksum & 0xFF);
-	buf[MAVLINK_NUM_HEADER_BYTES + msg->len + 1] = (uint8_t)(msg->checksum >> 8);
-
-	if (_uart_fd >= 0) {
-		/* send message to UART */
-		ssize_t ret = ::write(_uart_fd, buf, packet_len);
-
-		if (ret != (int) packet_len) {
-			count_txerr();
-			count_txerrbytes(packet_len);
-
-		} else {
-			_last_write_success_time = _last_write_try_time;
-			count_txbytes(packet_len);
-		}
-	}
-
-	pthread_mutex_unlock(&_send_mutex);
-}
-
 void
 Mavlink::find_broadcast_address()
 {
diff --git a/src/modules/mavlink/mavlink_main.h b/src/modules/mavlink/mavlink_main.h
index b3b15d3a2f..a07385c77d 100644
--- a/src/modules/mavlink/mavlink_main.h
+++ b/src/modules/mavlink/mavlink_main.h
@@ -280,7 +280,7 @@ public:
 	/**
 	 * Resend message as is, don't change sequence number and CRC.
 	 */
-	void			resend_message(mavlink_message_t *msg);
+	void			resend_message(mavlink_message_t *msg) { _mavlink_resend_uart(_channel, msg); }
 
 	void			handle_message(const mavlink_message_t *msg);
 

From 8b8513fe8c5d4baa83bbc03d8d9ebbfaf9d4d965 Mon Sep 17 00:00:00 2001
From: Miguel Arroyo <miguel@cs.columbia.edu>
Date: Thu, 23 Jun 2016 14:08:03 -0400
Subject: [PATCH 0814/1230] Adds LSM9DS1 Wrapper

---
 .../drivers/df_lsm9ds1_wrapper/CMakeLists.txt |  46 +
 .../df_lsm9ds1_wrapper/df_lsm9ds1_wrapper.cpp | 887 ++++++++++++++++++
 2 files changed, 933 insertions(+)
 create mode 100644 src/platforms/posix/drivers/df_lsm9ds1_wrapper/CMakeLists.txt
 create mode 100644 src/platforms/posix/drivers/df_lsm9ds1_wrapper/df_lsm9ds1_wrapper.cpp

diff --git a/src/platforms/posix/drivers/df_lsm9ds1_wrapper/CMakeLists.txt b/src/platforms/posix/drivers/df_lsm9ds1_wrapper/CMakeLists.txt
new file mode 100644
index 0000000000..8a514ad8e3
--- /dev/null
+++ b/src/platforms/posix/drivers/df_lsm9ds1_wrapper/CMakeLists.txt
@@ -0,0 +1,46 @@
+############################################################################
+#
+#   Copyright (c) 2016 PX4 Development Team. All rights reserved.
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions
+# are met:
+#
+# 1. Redistributions of source code must retain the above copyright
+#    notice, this list of conditions and the following disclaimer.
+# 2. Redistributions in binary form must reproduce the above copyright
+#    notice, this list of conditions and the following disclaimer in
+#    the documentation and/or other materials provided with the
+#    distribution.
+# 3. Neither the name PX4 nor the names of its contributors may be
+#    used to endorse or promote products derived from this software
+#    without specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.
+#
+############################################################################
+
+include_directories(../../../../lib/DriverFramework/drivers)
+
+px4_add_module(
+	MODULE platforms__posix__drivers__df_lsm9ds1_wrapper
+	MAIN df_lsm9ds1_wrapper
+	SRCS
+		df_lsm9ds1_wrapper.cpp
+	DEPENDS
+		platforms__common
+		df_driver_framework
+		df_lsm9ds1
+	)
+# vim: set noet ft=cmake fenc=utf-8 ff=unix :
diff --git a/src/platforms/posix/drivers/df_lsm9ds1_wrapper/df_lsm9ds1_wrapper.cpp b/src/platforms/posix/drivers/df_lsm9ds1_wrapper/df_lsm9ds1_wrapper.cpp
new file mode 100644
index 0000000000..5b3ac0243b
--- /dev/null
+++ b/src/platforms/posix/drivers/df_lsm9ds1_wrapper/df_lsm9ds1_wrapper.cpp
@@ -0,0 +1,887 @@
+/****************************************************************************
+ *
+ * Copyright (c) 2016 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file df_lsm9ds1_wrapper.cpp
+ * Lightweight driver to access the MPU9250 of the DriverFramework.
+ *
+ * @author Miguel Arroyo <miguel@arroyo.me>
+ */
+
+#include <px4_config.h>
+
+#include <sys/types.h>
+#include <sys/stat.h>
+#include <stdint.h>
+#include <stddef.h>
+#include <stdlib.h>
+#include <string.h>
+#include <math.h>
+#include <unistd.h>
+#include <px4_getopt.h>
+#include <errno.h>
+
+#include <systemlib/err.h>
+
+#include <systemlib/perf_counter.h>
+#include <systemlib/mavlink_log.h>
+
+#include <drivers/drv_hrt.h>
+#include <drivers/drv_accel.h>
+#include <drivers/drv_gyro.h>
+#include <drivers/drv_mag.h>
+#include <drivers/device/integrator.h>
+
+#include <lib/conversion/rotation.h>
+
+#include <uORB/topics/parameter_update.h>
+
+#include <lsm9ds1/LSM9DS1.hpp>
+#include <DevMgr.hpp>
+
+// We don't want to auto publish, therefore set this to 0.
+#define LSM9DS1_NEVER_AUTOPUBLISH_US 0
+
+
+extern "C" { __EXPORT int df_lsm9ds1_wrapper_main(int argc, char *argv[]); }
+
+using namespace DriverFramework;
+
+
+class DfLsm9ds1Wrapper : public LSM9DS1
+{
+public:
+	DfLsm9ds1Wrapper(bool mag_enabled, enum Rotation rotation);
+	~DfLsm9ds1Wrapper();
+
+
+	/**
+	 * Start automatic measurement.
+	 *
+	 * @return 0 on success
+	 */
+	int		start();
+
+	/**
+	 * Stop automatic measurement.
+	 *
+	 * @return 0 on success
+	 */
+	int		stop();
+
+	/**
+	 * Print some debug info.
+	 */
+	void		info();
+
+private:
+	int _publish(struct imu_sensor_data &data);
+
+	void _update_accel_calibration();
+	void _update_gyro_calibration();
+  	void _update_mag_calibration();
+
+	orb_advert_t		    _accel_topic;
+	orb_advert_t		    _gyro_topic;
+  	orb_advert_t        	    _mag_topic;
+	orb_advert_t		    _mavlink_log_pub;
+
+	int			    _param_update_sub;
+
+	struct accel_calibration_s {
+		float x_offset;
+		float x_scale;
+		float y_offset;
+		float y_scale;
+		float z_offset;
+		float z_scale;
+	} _accel_calibration;
+
+	struct gyro_calibration_s {
+		float x_offset;
+		float x_scale;
+		float y_offset;
+		float y_scale;
+		float z_offset;
+		float z_scale;
+	} _gyro_calibration;
+
+	struct mag_calibration_s {
+		float x_offset;
+		float x_scale;
+		float y_offset;
+		float y_scale;
+		float z_offset;
+		float z_scale;
+	} _mag_calibration;
+  
+	math::Matrix<3, 3>	    _rotation_matrix;
+
+	int			    _accel_orb_class_instance;
+	int			    _gyro_orb_class_instance;
+	int         _mag_orb_class_instance;
+
+	Integrator		    _accel_int;
+	Integrator		    _gyro_int;
+
+	unsigned		    _publish_count;
+
+	perf_counter_t		    _read_counter;
+	perf_counter_t		    _error_counter;
+	perf_counter_t		    _fifo_overflow_counter;
+	perf_counter_t		    _fifo_corruption_counter;
+	perf_counter_t		    _gyro_range_hit_counter;
+	perf_counter_t		    _accel_range_hit_counter;
+	perf_counter_t		    _publish_perf;
+
+	hrt_abstime		    _last_accel_range_hit_time;
+	uint64_t		    _last_accel_range_hit_count;
+
+	bool _mag_enabled;
+};
+
+DfLsm9ds1Wrapper::DfLsm9ds1Wrapper(bool mag_enabled, enum Rotation rotation) :
+	LSM9DS1(IMU_DEVICE_ACC_GYRO, IMU_DEVICE_MAG),
+	_accel_topic(nullptr),
+	_gyro_topic(nullptr),
+  	_mag_topic(nullptr),
+	_mavlink_log_pub(nullptr),
+	_param_update_sub(-1),
+	_accel_calibration{},
+	_gyro_calibration{},
+  	_mag_calibration{},
+	_accel_orb_class_instance(-1),
+	_gyro_orb_class_instance(-1),
+  	_mag_orb_class_instance(-1),
+	_accel_int(LSM9DS1_NEVER_AUTOPUBLISH_US, false),
+	_gyro_int(LSM9DS1_NEVER_AUTOPUBLISH_US, true),
+	_publish_count(0),
+	_read_counter(perf_alloc(PC_COUNT, "lsm9ds1_reads")),
+	_error_counter(perf_alloc(PC_COUNT, "lsm9ds1_errors")),
+	_fifo_overflow_counter(perf_alloc(PC_COUNT, "lsm9ds1_fifo_overflows")),
+	_fifo_corruption_counter(perf_alloc(PC_COUNT, "lsm9ds1_fifo_corruptions")),
+	_gyro_range_hit_counter(perf_alloc(PC_COUNT, "lsm9ds1_gyro_range_hits")),
+	_accel_range_hit_counter(perf_alloc(PC_COUNT, "lsm9ds1_accel_range_hits")),
+	_publish_perf(perf_alloc(PC_ELAPSED, "lsm9ds1_publish")),
+	_last_accel_range_hit_time(0),
+	_last_accel_range_hit_count(0),
+  	_mag_enabled(mag_enabled)
+{
+	// Set sane default calibration values
+	_accel_calibration.x_scale = 1.0f;
+	_accel_calibration.y_scale = 1.0f;
+	_accel_calibration.z_scale = 1.0f;
+	_accel_calibration.x_offset = 0.0f;
+	_accel_calibration.y_offset = 0.0f;
+	_accel_calibration.z_offset = 0.0f;
+
+	_gyro_calibration.x_scale = 1.0f;
+	_gyro_calibration.y_scale = 1.0f;
+	_gyro_calibration.z_scale = 1.0f;
+	_gyro_calibration.x_offset = 0.0f;
+	_gyro_calibration.y_offset = 0.0f;
+	_gyro_calibration.z_offset = 0.0f;
+
+	if (_mag_enabled) {
+		_mag_calibration.x_scale = 1.0f;
+		_mag_calibration.y_scale = 1.0f;
+		_mag_calibration.z_scale = 1.0f;
+		_mag_calibration.x_offset = 0.0f;
+		_mag_calibration.y_offset = 0.0f;
+		_mag_calibration.z_offset = 0.0f;
+	}
+
+	// Get sensor rotation matrix
+	get_rot_matrix(rotation, &_rotation_matrix);
+}
+
+DfLsm9ds1Wrapper::~DfLsm9ds1Wrapper()
+{
+	perf_free(_read_counter);
+	perf_free(_error_counter);
+	perf_free(_fifo_overflow_counter);
+	perf_free(_fifo_corruption_counter);
+	perf_free(_gyro_range_hit_counter);
+	perf_free(_accel_range_hit_counter);
+
+	perf_free(_publish_perf);
+}
+
+int DfLsm9ds1Wrapper::start()
+{
+	// TODO: don't publish garbage here
+	accel_report accel_report = {};
+	_accel_topic = orb_advertise_multi(ORB_ID(sensor_accel), &accel_report,
+					   &_accel_orb_class_instance, ORB_PRIO_DEFAULT);
+
+	if (_accel_topic == nullptr) {
+		PX4_ERR("sensor_accel advert fail");
+		return -1;
+	}
+
+	// TODO: don't publish garbage here
+	gyro_report gyro_report = {};
+	_gyro_topic = orb_advertise_multi(ORB_ID(sensor_gyro), &gyro_report,
+					  &_gyro_orb_class_instance, ORB_PRIO_DEFAULT);
+
+	if (_gyro_topic == nullptr) {
+		PX4_ERR("sensor_gyro advert fail");
+		return -1;
+	}
+
+	  if (_mag_enabled) {
+	    mag_report mag_report = {};
+	    _mag_topic = orb_advertise_multi(ORB_ID(sensor_mag), &mag_report,
+					     &_mag_orb_class_instance, ORB_PRIO_DEFAULT);
+
+	    if (_mag_topic == nullptr) {
+	      PX4_ERR("sensor_mag advert fail");
+	      return -1;
+	    }
+	  }
+
+  /* Subscribe to param update topic. */
+	if (_param_update_sub < 0) {
+		_param_update_sub = orb_subscribe(ORB_ID(parameter_update));
+	}
+
+	/* Init device and start sensor. */
+	int ret = init();
+
+	if (ret != 0) {
+		PX4_ERR("LSM9DS1 init fail: %d", ret);
+		return ret;
+	}
+
+	ret = LSM9DS1::start();
+
+	if (ret != 0) {
+		PX4_ERR("LSM9DS1 start fail: %d", ret);
+		return ret;
+	}
+
+	PX4_DEBUG("LSM9DS1 device id is: %d", m_id.dev_id);
+
+	/* Force getting the calibration values. */
+	_update_accel_calibration();
+	_update_gyro_calibration();
+  	_update_mag_calibration();
+
+	return 0;
+}
+
+int DfLsm9ds1Wrapper::stop()
+{
+	/* Stop sensor. */
+	int ret = LSM9DS1::stop();
+
+	if (ret != 0) {
+		PX4_ERR("LSM9DS1 stop fail: %d", ret);
+		return ret;
+	}
+
+	return 0;
+}
+
+void DfLsm9ds1Wrapper::info()
+{
+	perf_print_counter(_read_counter);
+	perf_print_counter(_error_counter);
+	perf_print_counter(_fifo_overflow_counter);
+	perf_print_counter(_fifo_corruption_counter);
+	perf_print_counter(_gyro_range_hit_counter);
+	perf_print_counter(_accel_range_hit_counter);
+
+	perf_print_counter(_publish_perf);
+}
+
+void DfLsm9ds1Wrapper::_update_gyro_calibration()
+{
+	// TODO: replace magic number
+	for (unsigned i = 0; i < 3; ++i) {
+
+		// TODO: remove printfs and add error counter
+
+		char str[30];
+		(void)sprintf(str, "CAL_GYRO%u_ID", i);
+		int32_t device_id;
+		int res = param_get(param_find(str), &device_id);
+
+		if (res != OK) {
+			PX4_ERR("Could not access param %s", str);
+			continue;
+		}
+
+		if ((uint32_t)device_id != m_id.dev_id) {
+			continue;
+		}
+
+		(void)sprintf(str, "CAL_GYRO%u_XSCALE", i);
+		res = param_get(param_find(str), &_gyro_calibration.x_scale);
+
+		if (res != OK) {
+			PX4_ERR("Could not access param %s", str);
+		}
+
+		(void)sprintf(str, "CAL_GYRO%u_YSCALE", i);
+		res = param_get(param_find(str), &_gyro_calibration.y_scale);
+
+		if (res != OK) {
+			PX4_ERR("Could not access param %s", str);
+		}
+
+		(void)sprintf(str, "CAL_GYRO%u_ZSCALE", i);
+		res = param_get(param_find(str), &_gyro_calibration.z_scale);
+
+		if (res != OK) {
+			PX4_ERR("Could not access param %s", str);
+		}
+
+		(void)sprintf(str, "CAL_GYRO%u_XOFF", i);
+		res = param_get(param_find(str), &_gyro_calibration.x_offset);
+
+		if (res != OK) {
+			PX4_ERR("Could not access param %s", str);
+		}
+
+		(void)sprintf(str, "CAL_GYRO%u_YOFF", i);
+		res = param_get(param_find(str), &_gyro_calibration.y_offset);
+
+		if (res != OK) {
+			PX4_ERR("Could not access param %s", str);
+		}
+
+		(void)sprintf(str, "CAL_GYRO%u_ZOFF", i);
+		res = param_get(param_find(str), &_gyro_calibration.z_offset);
+
+		if (res != OK) {
+			PX4_ERR("Could not access param %s", str);
+		}
+
+		// We got calibration values, let's exit.
+		return;
+	}
+
+	_gyro_calibration.x_scale = 1.0f;
+	_gyro_calibration.y_scale = 1.0f;
+	_gyro_calibration.z_scale = 1.0f;
+	_gyro_calibration.x_offset = 0.0f;
+	_gyro_calibration.y_offset = 0.0f;
+	_gyro_calibration.z_offset = 0.0f;
+}
+
+void DfLsm9ds1Wrapper::_update_accel_calibration()
+{
+	// TODO: replace magic number
+	for (unsigned i = 0; i < 3; ++i) {
+
+		// TODO: remove printfs and add error counter
+
+		char str[30];
+		(void)sprintf(str, "CAL_ACC%u_ID", i);
+		int32_t device_id;
+		int res = param_get(param_find(str), &device_id);
+
+		if (res != OK) {
+			PX4_ERR("Could not access param %s", str);
+			continue;
+		}
+
+		if ((uint32_t)device_id != m_id.dev_id) {
+			continue;
+		}
+
+		(void)sprintf(str, "CAL_ACC%u_XSCALE", i);
+		res = param_get(param_find(str), &_accel_calibration.x_scale);
+
+		if (res != OK) {
+			PX4_ERR("Could not access param %s", str);
+		}
+
+		(void)sprintf(str, "CAL_ACC%u_YSCALE", i);
+		res = param_get(param_find(str), &_accel_calibration.y_scale);
+
+		if (res != OK) {
+			PX4_ERR("Could not access param %s", str);
+		}
+
+		(void)sprintf(str, "CAL_ACC%u_ZSCALE", i);
+		res = param_get(param_find(str), &_accel_calibration.z_scale);
+
+		if (res != OK) {
+			PX4_ERR("Could not access param %s", str);
+		}
+
+		(void)sprintf(str, "CAL_ACC%u_XOFF", i);
+		res = param_get(param_find(str), &_accel_calibration.x_offset);
+
+		if (res != OK) {
+			PX4_ERR("Could not access param %s", str);
+		}
+
+		(void)sprintf(str, "CAL_ACC%u_YOFF", i);
+		res = param_get(param_find(str), &_accel_calibration.y_offset);
+
+		if (res != OK) {
+			PX4_ERR("Could not access param %s", str);
+		}
+
+		(void)sprintf(str, "CAL_ACC%u_ZOFF", i);
+		res = param_get(param_find(str), &_accel_calibration.z_offset);
+
+		if (res != OK) {
+			PX4_ERR("Could not access param %s", str);
+		}
+
+		// We got calibration values, let's exit.
+		return;
+	}
+
+	// Set sane default calibration values
+	_accel_calibration.x_scale = 1.0f;
+	_accel_calibration.y_scale = 1.0f;
+	_accel_calibration.z_scale = 1.0f;
+	_accel_calibration.x_offset = 0.0f;
+	_accel_calibration.y_offset = 0.0f;
+	_accel_calibration.z_offset = 0.0f;
+}
+
+void DfLsm9ds1Wrapper::_update_mag_calibration()
+{
+	if (!_mag_enabled) {
+		return;
+	}
+
+	// TODO: replace magic number
+	for (unsigned i = 0; i < 3; ++i) {
+
+		// TODO: remove printfs and add error counter
+
+		char str[30];
+		(void)sprintf(str, "CAL_MAG%u_ID", i);
+		int32_t device_id;
+		int res = param_get(param_find(str), &device_id);
+
+		if (res != OK) {
+			PX4_ERR("Could not access param %s", str);
+			continue;
+		}
+
+		if ((uint32_t)device_id != m_id.dev_id) {
+			continue;
+		}
+
+		(void)sprintf(str, "CAL_MAG%u_XSCALE", i);
+		res = param_get(param_find(str), &_mag_calibration.x_scale);
+
+		if (res != OK) {
+			PX4_ERR("Could not access param %s", str);
+		}
+
+		(void)sprintf(str, "CAL_MAG%u_YSCALE", i);
+		res = param_get(param_find(str), &_mag_calibration.y_scale);
+
+		if (res != OK) {
+			PX4_ERR("Could not access param %s", str);
+		}
+
+		(void)sprintf(str, "CAL_MAG%u_ZSCALE", i);
+		res = param_get(param_find(str), &_mag_calibration.z_scale);
+
+		if (res != OK) {
+			PX4_ERR("Could not access param %s", str);
+		}
+
+		(void)sprintf(str, "CAL_MAG%u_XOFF", i);
+		res = param_get(param_find(str), &_mag_calibration.x_offset);
+
+		if (res != OK) {
+			PX4_ERR("Could not access param %s", str);
+		}
+
+		(void)sprintf(str, "CAL_MAG%u_YOFF", i);
+		res = param_get(param_find(str), &_mag_calibration.y_offset);
+
+		if (res != OK) {
+			PX4_ERR("Could not access param %s", str);
+		}
+
+		(void)sprintf(str, "CAL_MAG%u_ZOFF", i);
+		res = param_get(param_find(str), &_mag_calibration.z_offset);
+
+		if (res != OK) {
+			PX4_ERR("Could not access param %s", str);
+		}
+
+		// We got calibration values, let's exit.
+		return;
+	}
+
+	// Set sane default calibration values
+	_mag_calibration.x_scale = 1.0f;
+	_mag_calibration.y_scale = 1.0f;
+	_mag_calibration.z_scale = 1.0f;
+	_mag_calibration.x_offset = 0.0f;
+	_mag_calibration.y_offset = 0.0f;
+	_mag_calibration.z_offset = 0.0f;
+}
+
+int DfLsm9ds1Wrapper::_publish(struct imu_sensor_data &data)
+{
+	/* Check if calibration values are still up-to-date. */
+	bool updated;
+	orb_check(_param_update_sub, &updated);
+
+	if (updated) {
+		parameter_update_s parameter_update;
+		orb_copy(ORB_ID(parameter_update), _param_update_sub, &parameter_update);
+
+		_update_accel_calibration();
+		_update_gyro_calibration();
+	}
+
+	math::Vector<3> vec_integrated_unused;
+	uint64_t integral_dt_unused;
+
+	math::Vector<3> accel_val((data.accel_m_s2_x - _accel_calibration.x_offset) * _accel_calibration.x_scale,
+				  (data.accel_m_s2_y - _accel_calibration.y_offset) * _accel_calibration.y_scale,
+				  (data.accel_m_s2_z - _accel_calibration.z_offset) * _accel_calibration.z_scale);
+
+	// apply sensor rotation on the accel measurement
+	accel_val = _rotation_matrix * accel_val;
+
+	_accel_int.put_with_interval(data.fifo_sample_interval_us,
+				     accel_val,
+				     vec_integrated_unused,
+				     integral_dt_unused);
+
+	math::Vector<3> gyro_val((data.gyro_rad_s_x - _gyro_calibration.x_offset) * _gyro_calibration.x_scale,
+				 (data.gyro_rad_s_y - _gyro_calibration.y_offset) * _gyro_calibration.y_scale,
+				 (data.gyro_rad_s_z - _gyro_calibration.z_offset) * _gyro_calibration.z_scale);
+
+	// apply sensor rotation on the gyro measurement
+	gyro_val = _rotation_matrix * gyro_val;
+
+	_gyro_int.put_with_interval(data.fifo_sample_interval_us,
+				    gyro_val,
+				    vec_integrated_unused,
+				    integral_dt_unused);
+
+	// If we are not receiving the last sample from the FIFO buffer yet, let's stop here
+	// and wait for more packets.
+	if (!data.is_last_fifo_sample) {
+		return 0;
+	}
+
+	// The driver empties the FIFO buffer at 1kHz, however we only need to publish at 250Hz.
+	// Therefore, only publish every forth time.
+	++_publish_count;
+
+	if (_publish_count < 4) {
+		return 0;
+	}
+
+	_publish_count = 0;
+
+	// Update all the counters.
+	perf_set_count(_read_counter, data.read_counter);
+	perf_set_count(_error_counter, data.error_counter);
+	perf_set_count(_fifo_overflow_counter, data.fifo_overflow_counter);
+	perf_set_count(_fifo_corruption_counter, data.fifo_overflow_counter);
+	perf_set_count(_gyro_range_hit_counter, data.gyro_range_hit_counter);
+	perf_set_count(_accel_range_hit_counter, data.accel_range_hit_counter);
+
+	perf_begin(_publish_perf);
+
+	accel_report accel_report = {};
+	gyro_report gyro_report = {};
+  	mag_report mag_report = {};
+
+	accel_report.timestamp = gyro_report.timestamp = hrt_absolute_time();
+  	mag_report.timestamp = accel_report.timestamp;
+	
+	// TODO: get these right
+	gyro_report.scaling = -1.0f;
+	gyro_report.range_rad_s = -1.0f;
+	gyro_report.device_id = m_id.dev_id;
+
+	accel_report.scaling = -1.0f;
+	accel_report.range_m_s2 = -1.0f;
+	accel_report.device_id = m_id.dev_id;
+
+  	if (_mag_enabled) {
+  	    mag_report.scaling = -1.0f;
+    	    mag_report.range_ga = -1.0f;
+    	    mag_report.device_id = m_id.dev_id;
+  	}
+ 	// TODO: remove these (or get the values)
+	gyro_report.x_raw = NAN;
+	gyro_report.y_raw = NAN;
+	gyro_report.z_raw = NAN;
+
+	accel_report.x_raw = NAN;
+	accel_report.y_raw = NAN;
+	accel_report.z_raw = NAN;
+
+  	if (_mag_enabled) {
+    	    mag_report.x_raw = NAN;
+    	    mag_report.y_raw = NAN;
+    	    mag_report.z_raw = NAN;
+  	}
+
+	math::Vector<3> gyro_val_filt;
+	math::Vector<3> accel_val_filt;
+
+	// Read and reset.
+	math::Vector<3> gyro_val_integ = _gyro_int.get_and_filtered(true, gyro_report.integral_dt, gyro_val_filt);
+	math::Vector<3> accel_val_integ = _accel_int.get_and_filtered(true, accel_report.integral_dt, accel_val_filt);
+
+	// Use the filtered (by integration) values to get smoother / less noisy data.
+	gyro_report.x = gyro_val_filt(0);
+	gyro_report.y = gyro_val_filt(1);
+	gyro_report.z = gyro_val_filt(2);
+
+	accel_report.x = accel_val_filt(0);
+	accel_report.y = accel_val_filt(1);
+	accel_report.z = accel_val_filt(2);
+
+  	if (_mag_enabled) {
+
+		math::Vector<3> mag_val((data.mag_ga_x - _mag_calibration.x_offset) * _mag_calibration.x_scale,
+                            (data.mag_ga_y - _mag_calibration.y_offset) * _mag_calibration.y_scale,
+                            (data.mag_ga_z - _mag_calibration.z_offset) * _mag_calibration.z_scale);
+
+		mag_val = _rotation_matrix * mag_val;
+
+		mag_report.x = mag_val(0);
+		mag_report.y = mag_val(1);
+		mag_report.z = mag_val(2);
+	}
+  
+	gyro_report.x_integral = gyro_val_integ(0);
+	gyro_report.y_integral = gyro_val_integ(1);
+	gyro_report.z_integral = gyro_val_integ(2);
+
+	accel_report.x_integral = accel_val_integ(0);
+	accel_report.y_integral = accel_val_integ(1);
+	accel_report.z_integral = accel_val_integ(2);
+
+	// TODO: when is this ever blocked?
+	if (!(m_pub_blocked)) {
+
+		if (_gyro_topic != nullptr) {
+			orb_publish(ORB_ID(sensor_gyro), _gyro_topic, &gyro_report);
+		}
+
+		if (_accel_topic != nullptr) {
+			orb_publish(ORB_ID(sensor_accel), _accel_topic, &accel_report);
+		}
+
+    		if (_mag_topic != nullptr) {
+      	    		orb_publish(ORB_ID(sensor_mag), _mag_topic, &mag_report);
+    		}
+		/* Notify anyone waiting for data. */
+		DevMgr::updateNotify(*this);
+
+		// Report if there are high vibrations, every 10 times it happens.
+		const bool threshold_reached = (data.accel_range_hit_counter - _last_accel_range_hit_count > 10);
+
+		// Report every 5s.
+		const bool due_to_report = (hrt_elapsed_time(&_last_accel_range_hit_time) > 5000000);
+
+		if (threshold_reached && due_to_report) {
+			mavlink_log_critical(&_mavlink_log_pub,
+					     "High accelerations, range exceeded %llu times",
+					     data.accel_range_hit_counter);
+
+			_last_accel_range_hit_time = hrt_absolute_time();
+			_last_accel_range_hit_count = data.accel_range_hit_counter;
+		}
+	}
+
+	perf_end(_publish_perf);
+
+	// TODO: check the return codes of this function
+	return 0;
+};
+
+
+namespace df_lsm9ds1_wrapper
+{
+
+DfLsm9ds1Wrapper *g_dev = nullptr;
+
+int start(bool mag_enabled, enum Rotation rotation);
+int stop();
+int info();
+void usage();
+
+int start(bool mag_enabled, enum Rotation rotation)
+{
+	g_dev = new DfLsm9ds1Wrapper(mag_enabled, rotation);
+
+	if (g_dev == nullptr) {
+		PX4_ERR("failed instantiating DfLsm9ds1Wrapper object");
+		return -1;
+	}
+
+	int ret = g_dev->start();
+
+	if (ret != 0) {
+		PX4_ERR("DfLsm9ds1Wrapper start failed");
+		return ret;
+	}
+
+	// Open the IMU sensor
+	DevHandle h;
+	DevMgr::getHandle(IMU_DEVICE_ACC_GYRO, h);
+
+	if (!h.isValid()) {
+		DF_LOG_INFO("Error: unable to obtain a valid handle for the receiver at: %s (%d)",
+			    IMU_DEVICE_ACC_GYRO, h.getError());
+		return -1;
+	}
+
+	DevMgr::releaseHandle(h);
+
+	DevMgr::getHandle(IMU_DEVICE_MAG, h);
+
+	if (!h.isValid()) {
+		DF_LOG_INFO("Error: unable to obtain a valid handle for the receiver at: %s (%d)",
+                IMU_DEVICE_MAG, h.getError());
+		return -1;
+	}
+
+	DevMgr::releaseHandle(h);
+	return 0;
+}
+
+int stop()
+{
+	if (g_dev == nullptr) {
+		PX4_ERR("driver not running");
+		return 1;
+	}
+
+	int ret = g_dev->stop();
+
+	if (ret != 0) {
+		PX4_ERR("driver could not be stopped");
+		return ret;
+	}
+
+	delete g_dev;
+	g_dev = nullptr;
+	return 0;
+}
+
+/**
+ * Print a little info about the driver.
+ */
+int
+info()
+{
+	if (g_dev == nullptr) {
+		PX4_ERR("driver not running");
+		return 1;
+	}
+
+	PX4_INFO("state @ %p", g_dev);
+	g_dev->info();
+
+	return 0;
+}
+
+void
+usage()
+{
+	PX4_INFO("Usage: df_lsm9ds1_wrapper 'start', 'info', 'stop', 'start_without_mag'");
+	PX4_INFO("options:");
+	PX4_INFO("    -R rotation");
+}
+
+} // namespace df_lsm9ds1_wrapper
+
+
+int
+df_lsm9ds1_wrapper_main(int argc, char *argv[])
+{
+	int ch;
+	enum Rotation rotation = ROTATION_NONE;
+	int ret = 0;
+	int myoptind = 1;
+	const char *myoptarg = NULL;
+
+	/* jump over start/off/etc and look at options first */
+	while ((ch = px4_getopt(argc, argv, "R:", &myoptind, &myoptarg)) != EOF) {
+		switch (ch) {
+		case 'R':
+			rotation = (enum Rotation)atoi(myoptarg);
+			break;
+
+		default:
+			df_lsm9ds1_wrapper::usage();
+			return 0;
+		}
+	}
+
+	if (argc <= 1) {
+		df_lsm9ds1_wrapper::usage();
+		return 1;
+	}
+
+	const char *verb = argv[myoptind];
+
+  	if (!strcmp(verb, "start_without_mag")) {
+    		ret = df_lsm9ds1_wrapper::start(false, rotation);
+  	}
+
+	else if (!strcmp(verb, "start")) {
+		ret = df_lsm9ds1_wrapper::start(true, rotation);
+	}
+
+	else if (!strcmp(verb, "stop")) {
+		ret = df_lsm9ds1_wrapper::stop();
+	}
+
+	else if (!strcmp(verb, "info")) {
+		ret = df_lsm9ds1_wrapper::info();
+	}
+
+	else {
+		df_lsm9ds1_wrapper::usage();
+		return 1;
+	}
+
+	return ret;
+}

From f0dbae2e19149801087a1e0bb190a483d111cb3a Mon Sep 17 00:00:00 2001
From: Miguel Arroyo <miguel@cs.columbia.edu>
Date: Thu, 23 Jun 2016 14:09:20 -0400
Subject: [PATCH 0815/1230] Adds LSM9DS1 DriverFramework

---
 src/lib/DriverFramework | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/src/lib/DriverFramework b/src/lib/DriverFramework
index 7bf87592c5..0010ee4798 160000
--- a/src/lib/DriverFramework
+++ b/src/lib/DriverFramework
@@ -1 +1 @@
-Subproject commit 7bf87592c5f9bfe75cd33280b246c15f0ba476e5
+Subproject commit 0010ee479899d9971fdb55d832fcdbcdf36e39bb

From ef729ab2d8b5154252447055946309662a23bf59 Mon Sep 17 00:00:00 2001
From: Hidenori <hidenori.kobayashi@gmail.com>
Date: Thu, 23 Jun 2016 14:42:12 -0400
Subject: [PATCH 0816/1230] MS5611 driver wrapper for RPi

---
 cmake/configs/posix_rpi2_release.cmake        |  19 +-
 .../drivers/df_ms5611_wrapper/CMakeLists.txt  |  46 +++
 .../df_ms5611_wrapper/df_ms5611_wrapper.cpp   | 322 ++++++++++++++++++
 3 files changed, 380 insertions(+), 7 deletions(-)
 create mode 100644 src/platforms/posix/drivers/df_ms5611_wrapper/CMakeLists.txt
 create mode 100644 src/platforms/posix/drivers/df_ms5611_wrapper/df_ms5611_wrapper.cpp

diff --git a/cmake/configs/posix_rpi2_release.cmake b/cmake/configs/posix_rpi2_release.cmake
index 64d20d2fbb..4065b3240c 100644
--- a/cmake/configs/posix_rpi2_release.cmake
+++ b/cmake/configs/posix_rpi2_release.cmake
@@ -11,6 +11,7 @@ set(CMAKE_PROGRAM_PATH
 # or if it is for the RPi.
 add_definitions(
 	-D__PX4_POSIX_RPI2
+	-D__LINUX
 	)
 
 set(config_module_list
@@ -20,10 +21,12 @@ set(config_module_list
 	drivers/device
 	modules/sensors
 	platforms/posix/drivers/df_mpu9250_wrapper
-	platforms/posix/drivers/df_bmp280_wrapper
-	platforms/posix/drivers/df_hmc5883_wrapper
-	platforms/posix/drivers/df_trone_wrapper
-	platforms/posix/drivers/df_isl29501_wrapper
+	#platforms/posix/drivers/df_bmp280_wrapper
+	#platforms/posix/drivers/df_hmc5883_wrapper
+	#platforms/posix/drivers/df_trone_wrapper
+	#platforms/posix/drivers/df_isl29501_wrapper
+	platforms/posix/drivers/df_lsm9ds1_wrapper
+	platforms/posix/drivers/df_ms5611_wrapper
 
 	#
 	# System commands
@@ -102,8 +105,10 @@ set(config_module_list
 
 set(config_df_driver_list
 	mpu9250
-	bmp280
-	hmc5883
+	#bmp280
+	#hmc5883
 	trone
-	isl29501
+	#isl29501
+	lsm9ds1
+	ms5611
 	)
diff --git a/src/platforms/posix/drivers/df_ms5611_wrapper/CMakeLists.txt b/src/platforms/posix/drivers/df_ms5611_wrapper/CMakeLists.txt
new file mode 100644
index 0000000000..a84bd5114b
--- /dev/null
+++ b/src/platforms/posix/drivers/df_ms5611_wrapper/CMakeLists.txt
@@ -0,0 +1,46 @@
+############################################################################
+#
+#   Copyright (c) 2016 PX4 Development Team. All rights reserved.
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions
+# are met:
+#
+# 1. Redistributions of source code must retain the above copyright
+#    notice, this list of conditions and the following disclaimer.
+# 2. Redistributions in binary form must reproduce the above copyright
+#    notice, this list of conditions and the following disclaimer in
+#    the documentation and/or other materials provided with the
+#    distribution.
+# 3. Neither the name PX4 nor the names of its contributors may be
+#    used to endorse or promote products derived from this software
+#    without specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.
+#
+############################################################################
+
+include_directories(../../../../lib/DriverFramework/drivers)
+
+px4_add_module(
+	MODULE platforms__posix__drivers__df_ms5611_wrapper
+	MAIN df_ms5611_wrapper
+	SRCS
+		df_ms5611_wrapper.cpp
+	DEPENDS
+		platforms__common
+		df_driver_framework
+		df_ms5611
+	)
+# vim: set noet ft=cmake fenc=utf-8 ff=unix :
diff --git a/src/platforms/posix/drivers/df_ms5611_wrapper/df_ms5611_wrapper.cpp b/src/platforms/posix/drivers/df_ms5611_wrapper/df_ms5611_wrapper.cpp
new file mode 100644
index 0000000000..2938f17495
--- /dev/null
+++ b/src/platforms/posix/drivers/df_ms5611_wrapper/df_ms5611_wrapper.cpp
@@ -0,0 +1,322 @@
+/****************************************************************************
+ *
+ * Copyright (c) 2016 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file df_ms5611_wrapper.cpp
+ * Lightweight driver to access the MS5611 of the DriverFramework.
+ */
+
+#include <px4_config.h>
+
+#include <sys/types.h>
+#include <sys/stat.h>
+#include <stdint.h>
+#include <stddef.h>
+#include <stdlib.h>
+#include <string.h>
+#include <math.h>
+#include <unistd.h>
+#include <px4_getopt.h>
+#include <errno.h>
+
+#include <systemlib/perf_counter.h>
+#include <systemlib/err.h>
+
+#include <drivers/drv_baro.h>
+
+#include <board_config.h>
+
+#include <ms5611/MS5611.hpp>
+#include <DevMgr.hpp>
+
+
+extern "C" { __EXPORT int df_ms5611_wrapper_main(int argc, char *argv[]); }
+
+using namespace DriverFramework;
+
+
+class DfMS5611Wrapper : public MS5611
+{
+public:
+	DfMS5611Wrapper();
+	~DfMS5611Wrapper();
+
+
+	/**
+	 * Start automatic measurement.
+	 *
+	 * @return 0 on success
+	 */
+	int		start();
+
+	/**
+	 * Stop automatic measurement.
+	 *
+	 * @return 0 on success
+	 */
+	int		stop();
+
+private:
+	int _publish(struct baro_sensor_data &data);
+
+	orb_advert_t		_baro_topic;
+
+	int			_baro_orb_class_instance;
+
+	perf_counter_t		_baro_sample_perf;
+
+};
+
+DfMS5611Wrapper::DfMS5611Wrapper() :
+	MS5611(BARO_DEVICE_PATH),
+	_baro_topic(nullptr),
+	_baro_orb_class_instance(-1),
+	_baro_sample_perf(perf_alloc(PC_ELAPSED, "df_baro_read"))
+{
+}
+
+DfMS5611Wrapper::~DfMS5611Wrapper()
+{
+	perf_free(_baro_sample_perf);
+}
+
+int DfMS5611Wrapper::start()
+{
+	/* Init device and start sensor. */
+	int ret = init();
+
+	if (ret != 0) {
+		PX4_ERR("MS5611 init fail: %d", ret);
+		return ret;
+	}
+
+	ret = MS5611::start();
+
+	if (ret != 0) {
+		PX4_ERR("MS5611 start fail: %d", ret);
+		return ret;
+	}
+
+	return 0;
+}
+
+int DfMS5611Wrapper::stop()
+{
+	/* Stop sensor. */
+	int ret = MS5611::stop();
+
+	if (ret != 0) {
+		PX4_ERR("MS5611 stop fail: %d", ret);
+		return ret;
+	}
+
+	return 0;
+}
+
+int DfMS5611Wrapper::_publish(struct baro_sensor_data &data)
+{
+	perf_begin(_baro_sample_perf);
+
+	baro_report baro_report = {};
+	baro_report.timestamp = hrt_absolute_time();
+
+	baro_report.pressure = data.pressure_pa;
+	baro_report.temperature = data.temperature_c;
+
+	// TODO: verify this, it's just copied from the MS5611 driver.
+
+	// Constant for now
+	const double MSL_PRESSURE_KPA = 101325.0 / 1000.0;
+
+	/* tropospheric properties (0-11km) for standard atmosphere */
+	const double T1 = 15.0 + 273.15;	/* temperature at base height in Kelvin */
+	const double a  = -6.5 / 1000;	/* temperature gradient in degrees per metre */
+	const double g  = 9.80665;	/* gravity constant in m/s/s */
+	const double R  = 287.05;	/* ideal gas constant in J/kg/K */
+
+	/* current pressure at MSL in kPa */
+	double p1 = MSL_PRESSURE_KPA;
+
+	/* measured pressure in kPa */
+	double p = static_cast<double>(data.pressure_pa) / 1000.0;
+
+	/*
+	 * Solve:
+	 *
+	 *     /        -(aR / g)     \
+	 *    | (p / p1)          . T1 | - T1
+	 *     \                      /
+	 * h = -------------------------------  + h1
+	 *                   a
+	 */
+	baro_report.altitude = (((pow((p / p1), (-(a * R) / g))) * T1) - T1) / a;
+
+	// TODO: when is this ever blocked?
+	if (!(m_pub_blocked)) {
+
+		if (_baro_topic == nullptr) {
+			_baro_topic = orb_advertise_multi(ORB_ID(sensor_baro), &baro_report,
+							  &_baro_orb_class_instance, ORB_PRIO_DEFAULT);
+
+		} else {
+			orb_publish(ORB_ID(sensor_baro), _baro_topic, &baro_report);
+		}
+	}
+
+	/* Notify anyone waiting for data. */
+	DevMgr::updateNotify(*this);
+
+	perf_end(_baro_sample_perf);
+
+	return 0;
+};
+
+
+namespace df_ms5611_wrapper
+{
+
+DfMS5611Wrapper *g_dev = nullptr;
+
+int start(/* enum Rotation rotation */);
+int stop();
+int info();
+void usage();
+
+int start(/*enum Rotation rotation*/)
+{
+	g_dev = new DfMS5611Wrapper(/*rotation*/);
+
+	if (g_dev == nullptr) {
+		PX4_ERR("failed instantiating DfMS5611Wrapper object");
+		return -1;
+	}
+
+	int ret = g_dev->start();
+
+	if (ret != 0) {
+		PX4_ERR("DfMS5611Wrapper start failed");
+		return ret;
+	}
+
+	// Open the IMU sensor
+	DevHandle h;
+	DevMgr::getHandle(BARO_DEVICE_PATH, h);
+
+	if (!h.isValid()) {
+		DF_LOG_INFO("Error: unable to obtain a valid handle for the receiver at: %s (%d)",
+			    BARO_DEVICE_PATH, h.getError());
+		return -1;
+	}
+
+	DevMgr::releaseHandle(h);
+
+	return 0;
+}
+
+int stop()
+{
+	if (g_dev == nullptr) {
+		PX4_ERR("driver not running");
+		return 1;
+	}
+
+	int ret = g_dev->stop();
+
+	if (ret != 0) {
+		PX4_ERR("driver could not be stopped");
+		return ret;
+	}
+
+	delete g_dev;
+	g_dev = nullptr;
+	return 0;
+}
+
+/**
+ * Print a little info about the driver.
+ */
+int
+info()
+{
+	if (g_dev == nullptr) {
+		PX4_ERR("driver not running");
+		return 1;
+	}
+
+	PX4_DEBUG("state @ %p", g_dev);
+
+	return 0;
+}
+
+void
+usage()
+{
+	PX4_WARN("Usage: df_ms5611_wrapper 'start', 'info', 'stop'");
+}
+
+} // namespace df_ms5611_wrapper
+
+
+int
+df_ms5611_wrapper_main(int argc, char *argv[])
+{
+	int ret = 0;
+	int myoptind = 1;
+
+	if (argc <= 1) {
+		df_ms5611_wrapper::usage();
+		return 1;
+	}
+
+	const char *verb = argv[myoptind];
+
+
+	if (!strcmp(verb, "start")) {
+		ret = df_ms5611_wrapper::start();
+	}
+
+	else if (!strcmp(verb, "stop")) {
+		ret = df_ms5611_wrapper::stop();
+	}
+
+	else if (!strcmp(verb, "info")) {
+		ret = df_ms5611_wrapper::info();
+	}
+
+	else {
+		df_ms5611_wrapper::usage();
+		return 1;
+	}
+
+	return ret;
+}

From 4c0ed8bdd54cb173ef7cee73c7f87878ee2ff865 Mon Sep 17 00:00:00 2001
From: Julian Oes <julian@oes.ch>
Date: Fri, 24 Jun 2016 17:05:27 +0200
Subject: [PATCH 0817/1230] df_lsm9ds1_wrapper: astyle

---
 .../df_lsm9ds1_wrapper/df_lsm9ds1_wrapper.cpp | 88 ++++++++++---------
 1 file changed, 45 insertions(+), 43 deletions(-)

diff --git a/src/platforms/posix/drivers/df_lsm9ds1_wrapper/df_lsm9ds1_wrapper.cpp b/src/platforms/posix/drivers/df_lsm9ds1_wrapper/df_lsm9ds1_wrapper.cpp
index 5b3ac0243b..cef0cdd7b8 100644
--- a/src/platforms/posix/drivers/df_lsm9ds1_wrapper/df_lsm9ds1_wrapper.cpp
+++ b/src/platforms/posix/drivers/df_lsm9ds1_wrapper/df_lsm9ds1_wrapper.cpp
@@ -109,11 +109,11 @@ private:
 
 	void _update_accel_calibration();
 	void _update_gyro_calibration();
-  	void _update_mag_calibration();
+	void _update_mag_calibration();
 
 	orb_advert_t		    _accel_topic;
 	orb_advert_t		    _gyro_topic;
-  	orb_advert_t        	    _mag_topic;
+	orb_advert_t        	    _mag_topic;
 	orb_advert_t		    _mavlink_log_pub;
 
 	int			    _param_update_sub;
@@ -144,7 +144,7 @@ private:
 		float z_offset;
 		float z_scale;
 	} _mag_calibration;
-  
+
 	math::Matrix<3, 3>	    _rotation_matrix;
 
 	int			    _accel_orb_class_instance;
@@ -174,15 +174,15 @@ DfLsm9ds1Wrapper::DfLsm9ds1Wrapper(bool mag_enabled, enum Rotation rotation) :
 	LSM9DS1(IMU_DEVICE_ACC_GYRO, IMU_DEVICE_MAG),
 	_accel_topic(nullptr),
 	_gyro_topic(nullptr),
-  	_mag_topic(nullptr),
+	_mag_topic(nullptr),
 	_mavlink_log_pub(nullptr),
 	_param_update_sub(-1),
 	_accel_calibration{},
 	_gyro_calibration{},
-  	_mag_calibration{},
+	_mag_calibration{},
 	_accel_orb_class_instance(-1),
 	_gyro_orb_class_instance(-1),
-  	_mag_orb_class_instance(-1),
+	_mag_orb_class_instance(-1),
 	_accel_int(LSM9DS1_NEVER_AUTOPUBLISH_US, false),
 	_gyro_int(LSM9DS1_NEVER_AUTOPUBLISH_US, true),
 	_publish_count(0),
@@ -195,7 +195,7 @@ DfLsm9ds1Wrapper::DfLsm9ds1Wrapper(bool mag_enabled, enum Rotation rotation) :
 	_publish_perf(perf_alloc(PC_ELAPSED, "lsm9ds1_publish")),
 	_last_accel_range_hit_time(0),
 	_last_accel_range_hit_count(0),
-  	_mag_enabled(mag_enabled)
+	_mag_enabled(mag_enabled)
 {
 	// Set sane default calibration values
 	_accel_calibration.x_scale = 1.0f;
@@ -259,18 +259,18 @@ int DfLsm9ds1Wrapper::start()
 		return -1;
 	}
 
-	  if (_mag_enabled) {
-	    mag_report mag_report = {};
-	    _mag_topic = orb_advertise_multi(ORB_ID(sensor_mag), &mag_report,
-					     &_mag_orb_class_instance, ORB_PRIO_DEFAULT);
+	if (_mag_enabled) {
+		mag_report mag_report = {};
+		_mag_topic = orb_advertise_multi(ORB_ID(sensor_mag), &mag_report,
+						 &_mag_orb_class_instance, ORB_PRIO_DEFAULT);
 
-	    if (_mag_topic == nullptr) {
-	      PX4_ERR("sensor_mag advert fail");
-	      return -1;
-	    }
-	  }
+		if (_mag_topic == nullptr) {
+			PX4_ERR("sensor_mag advert fail");
+			return -1;
+		}
+	}
 
-  /* Subscribe to param update topic. */
+	/* Subscribe to param update topic. */
 	if (_param_update_sub < 0) {
 		_param_update_sub = orb_subscribe(ORB_ID(parameter_update));
 	}
@@ -295,7 +295,7 @@ int DfLsm9ds1Wrapper::start()
 	/* Force getting the calibration values. */
 	_update_accel_calibration();
 	_update_gyro_calibration();
-  	_update_mag_calibration();
+	_update_mag_calibration();
 
 	return 0;
 }
@@ -625,11 +625,11 @@ int DfLsm9ds1Wrapper::_publish(struct imu_sensor_data &data)
 
 	accel_report accel_report = {};
 	gyro_report gyro_report = {};
-  	mag_report mag_report = {};
+	mag_report mag_report = {};
 
 	accel_report.timestamp = gyro_report.timestamp = hrt_absolute_time();
-  	mag_report.timestamp = accel_report.timestamp;
-	
+	mag_report.timestamp = accel_report.timestamp;
+
 	// TODO: get these right
 	gyro_report.scaling = -1.0f;
 	gyro_report.range_rad_s = -1.0f;
@@ -639,12 +639,13 @@ int DfLsm9ds1Wrapper::_publish(struct imu_sensor_data &data)
 	accel_report.range_m_s2 = -1.0f;
 	accel_report.device_id = m_id.dev_id;
 
-  	if (_mag_enabled) {
-  	    mag_report.scaling = -1.0f;
-    	    mag_report.range_ga = -1.0f;
-    	    mag_report.device_id = m_id.dev_id;
-  	}
- 	// TODO: remove these (or get the values)
+	if (_mag_enabled) {
+		mag_report.scaling = -1.0f;
+		mag_report.range_ga = -1.0f;
+		mag_report.device_id = m_id.dev_id;
+	}
+
+	// TODO: remove these (or get the values)
 	gyro_report.x_raw = NAN;
 	gyro_report.y_raw = NAN;
 	gyro_report.z_raw = NAN;
@@ -653,11 +654,11 @@ int DfLsm9ds1Wrapper::_publish(struct imu_sensor_data &data)
 	accel_report.y_raw = NAN;
 	accel_report.z_raw = NAN;
 
-  	if (_mag_enabled) {
-    	    mag_report.x_raw = NAN;
-    	    mag_report.y_raw = NAN;
-    	    mag_report.z_raw = NAN;
-  	}
+	if (_mag_enabled) {
+		mag_report.x_raw = NAN;
+		mag_report.y_raw = NAN;
+		mag_report.z_raw = NAN;
+	}
 
 	math::Vector<3> gyro_val_filt;
 	math::Vector<3> accel_val_filt;
@@ -675,11 +676,11 @@ int DfLsm9ds1Wrapper::_publish(struct imu_sensor_data &data)
 	accel_report.y = accel_val_filt(1);
 	accel_report.z = accel_val_filt(2);
 
-  	if (_mag_enabled) {
+	if (_mag_enabled) {
 
 		math::Vector<3> mag_val((data.mag_ga_x - _mag_calibration.x_offset) * _mag_calibration.x_scale,
-                            (data.mag_ga_y - _mag_calibration.y_offset) * _mag_calibration.y_scale,
-                            (data.mag_ga_z - _mag_calibration.z_offset) * _mag_calibration.z_scale);
+					(data.mag_ga_y - _mag_calibration.y_offset) * _mag_calibration.y_scale,
+					(data.mag_ga_z - _mag_calibration.z_offset) * _mag_calibration.z_scale);
 
 		mag_val = _rotation_matrix * mag_val;
 
@@ -687,7 +688,7 @@ int DfLsm9ds1Wrapper::_publish(struct imu_sensor_data &data)
 		mag_report.y = mag_val(1);
 		mag_report.z = mag_val(2);
 	}
-  
+
 	gyro_report.x_integral = gyro_val_integ(0);
 	gyro_report.y_integral = gyro_val_integ(1);
 	gyro_report.z_integral = gyro_val_integ(2);
@@ -707,9 +708,10 @@ int DfLsm9ds1Wrapper::_publish(struct imu_sensor_data &data)
 			orb_publish(ORB_ID(sensor_accel), _accel_topic, &accel_report);
 		}
 
-    		if (_mag_topic != nullptr) {
-      	    		orb_publish(ORB_ID(sensor_mag), _mag_topic, &mag_report);
-    		}
+		if (_mag_topic != nullptr) {
+			orb_publish(ORB_ID(sensor_mag), _mag_topic, &mag_report);
+		}
+
 		/* Notify anyone waiting for data. */
 		DevMgr::updateNotify(*this);
 
@@ -778,7 +780,7 @@ int start(bool mag_enabled, enum Rotation rotation)
 
 	if (!h.isValid()) {
 		DF_LOG_INFO("Error: unable to obtain a valid handle for the receiver at: %s (%d)",
-                IMU_DEVICE_MAG, h.getError());
+			    IMU_DEVICE_MAG, h.getError());
 		return -1;
 	}
 
@@ -862,9 +864,9 @@ df_lsm9ds1_wrapper_main(int argc, char *argv[])
 
 	const char *verb = argv[myoptind];
 
-  	if (!strcmp(verb, "start_without_mag")) {
-    		ret = df_lsm9ds1_wrapper::start(false, rotation);
-  	}
+	if (!strcmp(verb, "start_without_mag")) {
+		ret = df_lsm9ds1_wrapper::start(false, rotation);
+	}
 
 	else if (!strcmp(verb, "start")) {
 		ret = df_lsm9ds1_wrapper::start(true, rotation);

From ea7cebbf102ee73bc0cda72b2e3fc457cb5d6243 Mon Sep 17 00:00:00 2001
From: Julian Oes <julian@oes.ch>
Date: Fri, 24 Jun 2016 19:41:43 +0200
Subject: [PATCH 0818/1230] scp_upload.sh: use ENV variable to set IP of RPi2

---
 Tools/scp_upload.sh | 10 +++++++++-
 1 file changed, 9 insertions(+), 1 deletion(-)

diff --git a/Tools/scp_upload.sh b/Tools/scp_upload.sh
index 37b2ce9898..2659af37cd 100755
--- a/Tools/scp_upload.sh
+++ b/Tools/scp_upload.sh
@@ -5,6 +5,14 @@ if [[ "$#" < 2 ]]; then
 	exit
 fi
 
+if [ -z ${AUTOPILOT_HOST+x} ]; then
+	host=px4autopilot
+	echo "\$AUTOPILOT_HOST is not set (use default: $host)"
+else
+	host=$AUTOPILOT_HOST
+	echo "\$AUTOPILOT_HOST is set to $host"
+fi
+
 echo "Uploading..."
 
 # Get last argument
@@ -19,6 +27,6 @@ do
 	fi
 	# echo "Pushing $arg to $last"
 	#adb push $arg $last
-	scp $arg pi@px4autopilot:$last
+	scp $arg pi@$host:$last
 	((i+=1))
 done

From 8b8766e840c5fc35505617b5a35204d47b338f46 Mon Sep 17 00:00:00 2001
From: Julian Oes <julian@oes.ch>
Date: Fri, 24 Jun 2016 19:42:04 +0200
Subject: [PATCH 0819/1230] RPi2: put drivers back in

---
 cmake/configs/posix_rpi2_release.cmake | 23 ++++++++++-------------
 posix-configs/rpi2/mainapp.config      |  5 +++--
 2 files changed, 13 insertions(+), 15 deletions(-)

diff --git a/cmake/configs/posix_rpi2_release.cmake b/cmake/configs/posix_rpi2_release.cmake
index 4065b3240c..0778efa1ff 100644
--- a/cmake/configs/posix_rpi2_release.cmake
+++ b/cmake/configs/posix_rpi2_release.cmake
@@ -5,14 +5,14 @@ set(CMAKE_TOOLCHAIN_FILE ${CMAKE_SOURCE_DIR}/cmake/toolchains/Toolchain-arm-linu
 set(CMAKE_PROGRAM_PATH
 	"${RPI_TOOLCHAIN_DIR}/gcc-linaro-arm-linux-gnueabihf-raspbian/bin"
 	${CMAKE_PROGRAM_PATH}
-	)
+)
 
 # This definition allows to differentiate if this just the usual POSIX build
 # or if it is for the RPi.
 add_definitions(
 	-D__PX4_POSIX_RPI2
 	-D__LINUX
-	)
+)
 
 set(config_module_list
 	#
@@ -21,12 +21,11 @@ set(config_module_list
 	drivers/device
 	modules/sensors
 	platforms/posix/drivers/df_mpu9250_wrapper
-	#platforms/posix/drivers/df_bmp280_wrapper
-	#platforms/posix/drivers/df_hmc5883_wrapper
-	#platforms/posix/drivers/df_trone_wrapper
-	#platforms/posix/drivers/df_isl29501_wrapper
 	platforms/posix/drivers/df_lsm9ds1_wrapper
 	platforms/posix/drivers/df_ms5611_wrapper
+	platforms/posix/drivers/df_hmc5883_wrapper
+	platforms/posix/drivers/df_trone_wrapper
+	platforms/posix/drivers/df_isl29501_wrapper
 
 	#
 	# System commands
@@ -100,15 +99,13 @@ set(config_module_list
 	platforms/common
 	platforms/posix/px4_layer
 	platforms/posix/work_queue
-
-	)
+)
 
 set(config_df_driver_list
 	mpu9250
-	#bmp280
-	#hmc5883
-	trone
-	#isl29501
 	lsm9ds1
 	ms5611
-	)
+	hmc5883
+	trone
+	isl29501
+)
diff --git a/posix-configs/rpi2/mainapp.config b/posix-configs/rpi2/mainapp.config
index 3f68fe09e5..fabb7e5baf 100644
--- a/posix-configs/rpi2/mainapp.config
+++ b/posix-configs/rpi2/mainapp.config
@@ -3,8 +3,9 @@ param set SYS_AUTOSTART 4001
 param set MAV_BROADCAST 1
 sleep 1
 param set MAV_TYPE 2
-df_mpu9250_wrapper start
-df_hmc5883_wrapper start
+df_lsm9ds1_wrapper start -R 4
+#df_mpu9250_wrapper start -R 10
+#df_hmc5883_wrapper start
 sensors start
 commander start
 ekf2 start

From e56be33e50f0a9731ad44c58ff5ad16dbf198d92 Mon Sep 17 00:00:00 2001
From: Julian Oes <julian@oes.ch>
Date: Fri, 24 Jun 2016 19:42:38 +0200
Subject: [PATCH 0820/1230] RPi2: switch from ekf2 to q/inav

---
 posix-configs/rpi2/mainapp.config | 3 ++-
 1 file changed, 2 insertions(+), 1 deletion(-)

diff --git a/posix-configs/rpi2/mainapp.config b/posix-configs/rpi2/mainapp.config
index fabb7e5baf..bee08bfd70 100644
--- a/posix-configs/rpi2/mainapp.config
+++ b/posix-configs/rpi2/mainapp.config
@@ -8,7 +8,8 @@ df_lsm9ds1_wrapper start -R 4
 #df_hmc5883_wrapper start
 sensors start
 commander start
-ekf2 start
+attitude_estimator_q start
+position_estimator_inav start
 land_detector start multicopter
 mc_pos_control start
 mc_att_control start

From fedde86bf4b9f62e59e40cafab7a8821bdad83f3 Mon Sep 17 00:00:00 2001
From: Julian Oes <julian@oes.ch>
Date: Fri, 24 Jun 2016 19:43:17 +0200
Subject: [PATCH 0821/1230] df_lsm9ds1_wrapper: new DF submodule, fixes

---
 src/lib/DriverFramework                                    | 2 +-
 .../drivers/df_lsm9ds1_wrapper/df_lsm9ds1_wrapper.cpp      | 7 +------
 2 files changed, 2 insertions(+), 7 deletions(-)

diff --git a/src/lib/DriverFramework b/src/lib/DriverFramework
index 0010ee4798..d1ab15894a 160000
--- a/src/lib/DriverFramework
+++ b/src/lib/DriverFramework
@@ -1 +1 @@
-Subproject commit 0010ee479899d9971fdb55d832fcdbcdf36e39bb
+Subproject commit d1ab15894a147ecd9fce021f17dd591b352e126c
diff --git a/src/platforms/posix/drivers/df_lsm9ds1_wrapper/df_lsm9ds1_wrapper.cpp b/src/platforms/posix/drivers/df_lsm9ds1_wrapper/df_lsm9ds1_wrapper.cpp
index cef0cdd7b8..490423f976 100644
--- a/src/platforms/posix/drivers/df_lsm9ds1_wrapper/df_lsm9ds1_wrapper.cpp
+++ b/src/platforms/posix/drivers/df_lsm9ds1_wrapper/df_lsm9ds1_wrapper.cpp
@@ -597,12 +597,6 @@ int DfLsm9ds1Wrapper::_publish(struct imu_sensor_data &data)
 				    vec_integrated_unused,
 				    integral_dt_unused);
 
-	// If we are not receiving the last sample from the FIFO buffer yet, let's stop here
-	// and wait for more packets.
-	if (!data.is_last_fifo_sample) {
-		return 0;
-	}
-
 	// The driver empties the FIFO buffer at 1kHz, however we only need to publish at 250Hz.
 	// Therefore, only publish every forth time.
 	++_publish_count;
@@ -700,6 +694,7 @@ int DfLsm9ds1Wrapper::_publish(struct imu_sensor_data &data)
 	// TODO: when is this ever blocked?
 	if (!(m_pub_blocked)) {
 
+
 		if (_gyro_topic != nullptr) {
 			orb_publish(ORB_ID(sensor_gyro), _gyro_topic, &gyro_report);
 		}

From 6618cac10a8eae3280244aeec97c028c85279725 Mon Sep 17 00:00:00 2001
From: Julian Oes <julian@oes.ch>
Date: Fri, 24 Jun 2016 20:19:34 +0200
Subject: [PATCH 0822/1230] RPi2: don't forget to start the baro

---
 posix-configs/rpi2/mainapp.config | 1 +
 1 file changed, 1 insertion(+)

diff --git a/posix-configs/rpi2/mainapp.config b/posix-configs/rpi2/mainapp.config
index bee08bfd70..752ea8f66b 100644
--- a/posix-configs/rpi2/mainapp.config
+++ b/posix-configs/rpi2/mainapp.config
@@ -6,6 +6,7 @@ param set MAV_TYPE 2
 df_lsm9ds1_wrapper start -R 4
 #df_mpu9250_wrapper start -R 10
 #df_hmc5883_wrapper start
+df_ms5611_wrapper start
 sensors start
 commander start
 attitude_estimator_q start

From e6bb21db657d636eae5b0a4f2cc97e3ec33cd7b5 Mon Sep 17 00:00:00 2001
From: tommises <tommises@gmail.com>
Date: Fri, 24 Jun 2016 14:30:18 -0600
Subject: [PATCH 0823/1230] Leave some pins available for camera trigger GPIO.

---
 ROMFS/px4fmu_common/init.d/rcS | 11 +++++++++++
 1 file changed, 11 insertions(+)

diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS
index e6d95e59f7..ef0375ce82 100644
--- a/ROMFS/px4fmu_common/init.d/rcS
+++ b/ROMFS/px4fmu_common/init.d/rcS
@@ -298,6 +298,17 @@ then
 		set FMU_MODE gpio_serial
 	fi
 
+	if param greater TRIG_MODE 0
+	then
+		if [ $PWM_OUT == 1234 ]
+		then
+			if ver hwcmp PX4FMU_V4
+			then
+				set FMU_MODE pwm4
+			fi
+		fi
+	fi
+
 	if [ $HIL == yes ]
 	then
 		set OUTPUT_MODE hil

From 27e20acbaeded33b8980526496dace4d750d6c39 Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Sat, 25 Jun 2016 12:15:32 +0200
Subject: [PATCH 0824/1230] Leave pin 5 and 6 of the AUX port available for
 camera triggering when the trigger is enabled

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

diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS
index ef0375ce82..6ef983e40c 100644
--- a/ROMFS/px4fmu_common/init.d/rcS
+++ b/ROMFS/px4fmu_common/init.d/rcS
@@ -306,6 +306,11 @@ then
 			then
 				set FMU_MODE pwm4
 			fi
+
+			if ver hwcmp PX4FMU_V2
+			then
+				set FMU_MODE pwm4
+			fi
 		fi
 	fi
 

From cb320f6e8a7baf5d0ec44d3077d9c73b27083820 Mon Sep 17 00:00:00 2001
From: Daniel Agar <daniel@agar.ca>
Date: Sun, 26 Jun 2016 15:27:11 -0400
Subject: [PATCH 0825/1230] param set default battery parameters (#4912)

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

diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp
index cea1686143..1a46c8ac2f 100644
--- a/src/modules/sensors/sensors.cpp
+++ b/src/modules/sensors/sensors.cpp
@@ -897,6 +897,7 @@ Sensors::parameters_update()
 	} else if (_parameters.battery_voltage_scaling < 0.0f) {
 		/* apply scaling according to defaults if set to default */
 		_parameters.battery_voltage_scaling = (3.3f / 4096);
+		param_set(_parameter_handles.battery_voltage_scaling, &_parameters.battery_voltage_scaling);
 	}
 
 	/* scaling of ADC ticks to battery current */
@@ -906,6 +907,7 @@ Sensors::parameters_update()
 	} else if (_parameters.battery_current_scaling < 0.0f) {
 		/* apply scaling according to defaults if set to default */
 		_parameters.battery_current_scaling = (3.3f / 4096);
+		param_set(_parameter_handles.battery_current_scaling, &_parameters.battery_current_scaling);
 	}
 
 	if (param_get(_parameter_handles.battery_current_offset, &(_parameters.battery_current_offset)) != OK) {
@@ -931,6 +933,7 @@ Sensors::parameters_update()
 		/* ensure a missing default trips a low voltage lockdown */
 		_parameters.battery_v_div = 0.0f;
 #endif
+		param_set(_parameter_handles.battery_v_div, &_parameters.battery_v_div);
 	}
 
 	if (param_get(_parameter_handles.battery_a_per_v, &(_parameters.battery_a_per_v)) != OK) {
@@ -949,6 +952,7 @@ Sensors::parameters_update()
 		/* ensure a missing default leads to an unrealistic current value */
 		_parameters.battery_a_per_v = 0.0f;
 #endif
+		param_set(_parameter_handles.battery_a_per_v, &_parameters.battery_a_per_v);
 	}
 
 	param_get(_parameter_handles.battery_source, &(_parameters.battery_source));

From e9fb929f5063ec08f3e22e99ec273c99081fafb5 Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Sun, 26 Jun 2016 21:38:14 +0200
Subject: [PATCH 0826/1230] Annotate build type classes for ver command

---
 src/systemcmds/ver/ver.c | 5 +++++
 1 file changed, 5 insertions(+)

diff --git a/src/systemcmds/ver/ver.c b/src/systemcmds/ver/ver.c
index f4d34719b0..0b7584c888 100644
--- a/src/systemcmds/ver/ver.c
+++ b/src/systemcmds/ver/ver.c
@@ -104,6 +104,11 @@ uint32_t version_tag_to_number(const char *tag)
 	}
 
 	// XXX not reporting patch version yet
+	// dev > 0
+	// alpha > 64
+	// beta > 128
+	// release candidate > 192
+	// release > 255
 	ver = (ver << 8);
 
 	return ver;

From 47a4b952176067e00948eb28ea69c399d8144b1c Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Sun, 26 Jun 2016 22:25:26 +0200
Subject: [PATCH 0827/1230] Updated simulation models

---
 Tools/sitl_gazebo | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/Tools/sitl_gazebo b/Tools/sitl_gazebo
index 8b5465b2af..82b3548750 160000
--- a/Tools/sitl_gazebo
+++ b/Tools/sitl_gazebo
@@ -1 +1 @@
-Subproject commit 8b5465b2af52eeee121426f287daf4070fe63bcb
+Subproject commit 82b354875040d97a3f598f0dc260ddc9ca4a95b3

From 33e259e8278ef60777998716379632dc012569d6 Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Sun, 26 Jun 2016 22:25:55 +0200
Subject: [PATCH 0828/1230] Update controller gains to match better vehicle
 models

---
 posix-configs/SITL/init/rcS_gazebo_iris          | 8 ++++----
 posix-configs/SITL/init/rcS_gazebo_solo          | 8 ++++----
 posix-configs/SITL/init/rcS_gazebo_standard_vtol | 4 ++--
 posix-configs/SITL/init/rcS_gazebo_tailsitter    | 4 ++--
 posix-configs/SITL/init/rcS_gazebo_typhoon_h480  | 9 ++++-----
 5 files changed, 16 insertions(+), 17 deletions(-)

diff --git a/posix-configs/SITL/init/rcS_gazebo_iris b/posix-configs/SITL/init/rcS_gazebo_iris
index 47f862fb04..b416d1f684 100644
--- a/posix-configs/SITL/init/rcS_gazebo_iris
+++ b/posix-configs/SITL/init/rcS_gazebo_iris
@@ -30,10 +30,10 @@ param set RTL_RETURN_ALT 30.0
 param set RTL_DESCEND_ALT 5.0
 param set RTL_LAND_DELAY 5
 param set MIS_TAKEOFF_ALT 2.5
-param set MC_ROLLRATE_P 0.3
-param set MC_PITCHRATE_P 0.3
-param set MC_PITCH_P 7
-param set MC_ROLL_P 7
+param set MC_ROLLRATE_P 0.25
+param set MC_PITCHRATE_P 0.25
+param set MC_PITCH_P 5.5
+param set MC_ROLL_P 5.5
 param set MC_ROLLRATE_I 0.1
 param set MC_PITCHRATE_I 0.1
 param set MPC_HOLD_MAX_Z 2.0
diff --git a/posix-configs/SITL/init/rcS_gazebo_solo b/posix-configs/SITL/init/rcS_gazebo_solo
index ded76b8505..eeb85b54a6 100644
--- a/posix-configs/SITL/init/rcS_gazebo_solo
+++ b/posix-configs/SITL/init/rcS_gazebo_solo
@@ -27,10 +27,10 @@ param set RTL_RETURN_ALT 30.0
 param set RTL_DESCEND_ALT 10.0
 param set RTL_LAND_DELAY 0
 param set MIS_TAKEOFF_ALT 2.5
-param set MC_ROLLRATE_P 0.3
-param set MC_PITCHRATE_P 0.3
-param set MC_PITCH_P 7
-param set MC_ROLL_P 7
+param set MC_ROLLRATE_P 0.25
+param set MC_PITCHRATE_P 0.25
+param set MC_PITCH_P 5.5
+param set MC_ROLL_P 5.5
 param set MC_ROLLRATE_I 0.1
 param set MC_PITCHRATE_I 0.1
 param set MPC_HOLD_MAX_Z 2.0
diff --git a/posix-configs/SITL/init/rcS_gazebo_standard_vtol b/posix-configs/SITL/init/rcS_gazebo_standard_vtol
index 6e7861b896..f43d6330b4 100644
--- a/posix-configs/SITL/init/rcS_gazebo_standard_vtol
+++ b/posix-configs/SITL/init/rcS_gazebo_standard_vtol
@@ -18,8 +18,8 @@ param set CAL_ACC0_YSCALE 1.01
 param set CAL_ACC0_ZSCALE 1.01
 param set CAL_ACC1_XOFF 0.01
 param set CAL_MAG0_XOFF 0.01
-param set MC_ROLLRATE_P 0.3
-param set MC_PITCHRATE_P 0.3
+param set MC_ROLLRATE_P 0.25
+param set MC_PITCHRATE_P 0.25
 param set MC_PITCH_P 5.5
 param set MC_ROLL_P 5.5
 param set MC_ROLLRATE_I 0.1
diff --git a/posix-configs/SITL/init/rcS_gazebo_tailsitter b/posix-configs/SITL/init/rcS_gazebo_tailsitter
index 5d3d44e633..d5edadeea5 100644
--- a/posix-configs/SITL/init/rcS_gazebo_tailsitter
+++ b/posix-configs/SITL/init/rcS_gazebo_tailsitter
@@ -18,8 +18,8 @@ param set CAL_ACC0_YSCALE 1.01
 param set CAL_ACC0_ZSCALE 1.01
 param set CAL_ACC1_XOFF 0.01
 param set CAL_MAG0_XOFF 0.01
-param set MC_ROLLRATE_P 0.3
-param set MC_PITCHRATE_P 0.3
+param set MC_ROLLRATE_P 0.25
+param set MC_PITCHRATE_P 0.25
 param set MC_PITCH_P 5.5
 param set MC_ROLL_P 5.5
 param set MC_ROLLRATE_I 0.1
diff --git a/posix-configs/SITL/init/rcS_gazebo_typhoon_h480 b/posix-configs/SITL/init/rcS_gazebo_typhoon_h480
index 33ca3f67b7..3688dae818 100644
--- a/posix-configs/SITL/init/rcS_gazebo_typhoon_h480
+++ b/posix-configs/SITL/init/rcS_gazebo_typhoon_h480
@@ -27,13 +27,12 @@ param set RTL_RETURN_ALT 30.0
 param set RTL_DESCEND_ALT 10.0
 param set RTL_LAND_DELAY 0
 param set MIS_TAKEOFF_ALT 2.5
-param set MC_ROLLRATE_P 0.3
-param set MC_PITCHRATE_P 0.3
-param set MC_PITCH_P 7
-param set MC_ROLL_P 7
+param set MC_ROLLRATE_P 0.25
+param set MC_PITCHRATE_P 0.25
+param set MC_PITCH_P 5.5
+param set MC_ROLL_P 5.5
 param set MC_ROLLRATE_I 0.1
 param set MC_PITCHRATE_I 0.1
-param set MC_YAWRATE_P 0.08
 param set MPC_HOLD_MAX_Z 2.0
 param set MPC_HOLD_XY_DZ 0.1
 param set MPC_Z_VEL_P 0.8

From acc8acd059df7bd448f1faad050e98ff001d7538 Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Sun, 26 Jun 2016 22:27:45 +0200
Subject: [PATCH 0829/1230] FMU driver: Fix typo

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

diff --git a/src/drivers/px4fmu/fmu.cpp b/src/drivers/px4fmu/fmu.cpp
index 65c23423a8..81712d1860 100644
--- a/src/drivers/px4fmu/fmu.cpp
+++ b/src/drivers/px4fmu/fmu.cpp
@@ -2499,7 +2499,7 @@ fmu_new_mode(PortMode new_mode)
 		break;
 
 	case PORT_PWM3:
-		/* select 4-pin PWM mode */
+		/* select 3-pin PWM mode */
 		servo_mode = PX4FMU::MODE_3PWM;
 		break;
 

From 3c2bd4f6dd88d61c8e1fb5b07c1ce732fb0e6f9b Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Sun, 26 Jun 2016 22:29:14 +0200
Subject: [PATCH 0830/1230] Param interface: Only mark as changed if value
 changed

---
 src/modules/systemlib/param/param.c | 4 +++-
 1 file changed, 3 insertions(+), 1 deletion(-)

diff --git a/src/modules/systemlib/param/param.c b/src/modules/systemlib/param/param.c
index ffd2d6dc0f..c872d0d58a 100644
--- a/src/modules/systemlib/param/param.c
+++ b/src/modules/systemlib/param/param.c
@@ -538,10 +538,12 @@ param_set_internal(param_t param, const void *val, bool mark_saved, bool notify_
 		switch (param_type(param)) {
 
 		case PARAM_TYPE_INT32:
+			params_changed = s->val.i != *(int32_t *)val;
 			s->val.i = *(int32_t *)val;
 			break;
 
 		case PARAM_TYPE_FLOAT:
+			params_changed = fabsf(s->val.f - *(float *)val) > FLT_EPSILON;
 			s->val.f = *(float *)val;
 			break;
 
@@ -556,6 +558,7 @@ param_set_internal(param_t param, const void *val, bool mark_saved, bool notify_
 			}
 
 			memcpy(s->val.p, val, param_size(param));
+			params_changed = true;
 			break;
 
 		default:
@@ -563,7 +566,6 @@ param_set_internal(param_t param, const void *val, bool mark_saved, bool notify_
 		}
 
 		s->unsaved = !mark_saved;
-		params_changed = true;
 		result = 0;
 	}
 

From b8b855f2aac2ad546c8e4188cde9f5027910d019 Mon Sep 17 00:00:00 2001
From: Daniel Agar <daniel@agar.ca>
Date: Sun, 26 Jun 2016 17:36:45 -0400
Subject: [PATCH 0831/1230] param.c fix style

---
 src/modules/systemlib/param/param.c | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/src/modules/systemlib/param/param.c b/src/modules/systemlib/param/param.c
index c872d0d58a..e758099124 100644
--- a/src/modules/systemlib/param/param.c
+++ b/src/modules/systemlib/param/param.c
@@ -543,7 +543,7 @@ param_set_internal(param_t param, const void *val, bool mark_saved, bool notify_
 			break;
 
 		case PARAM_TYPE_FLOAT:
-			params_changed = fabsf(s->val.f - *(float *)val) > FLT_EPSILON;
+			params_changed = fabsf(s->val.f - * (float *)val) > FLT_EPSILON;
 			s->val.f = *(float *)val;
 			break;
 

From 422acc0b699a1fd2c5c0732082645c135cda1bd3 Mon Sep 17 00:00:00 2001
From: Daniel Agar <daniel@agar.ca>
Date: Sun, 26 Jun 2016 17:42:00 -0400
Subject: [PATCH 0832/1230] travis-ci add check_format to qgc_firmware

---
 Makefile | 3 ++-
 1 file changed, 2 insertions(+), 1 deletion(-)

diff --git a/Makefile b/Makefile
index f8c34223a8..5357635845 100644
--- a/Makefile
+++ b/Makefile
@@ -301,7 +301,8 @@ qgc_firmware: \
 	check_px4fmu-v1_default \
 	check_px4fmu-v2_default \
 	check_mindpx-v2_default \
-	check_px4fmu-v4_default_and_uavcan
+	check_px4fmu-v4_default_and_uavcan \
+	check_format
 
 extra_firmware: \
 	check_px4-stm32f4discovery_default \

From 04e8b40a5ce8d33588d6e0cd89d4b70607f452b2 Mon Sep 17 00:00:00 2001
From: Eike <ecm@gmx.de>
Date: Mon, 27 Jun 2016 08:43:39 +0200
Subject: [PATCH 0833/1230] Posix LPE target (#4911)

---
 Makefile | 3 +++
 1 file changed, 3 insertions(+)

diff --git a/Makefile b/Makefile
index 5357635845..61ba857dbd 100644
--- a/Makefile
+++ b/Makefile
@@ -160,6 +160,9 @@ mindpx-v2_default:
 
 posix_sitl_default:
 	$(call cmake-build,$@)
+	
+posix_sitl_lpe:
+	$(call cmake-build,$@)
 
 posix_sitl_test:
 	$(call cmake-build,$@)

From 5935b18581619990534dd68df3d53d11d077ad77 Mon Sep 17 00:00:00 2001
From: James Goppert <jgoppert@users.noreply.github.com>
Date: Mon, 27 Jun 2016 02:46:00 -0400
Subject: [PATCH 0834/1230] Added EPH/EPV min to LPE. (#4915)

---
 src/modules/local_position_estimator/sensors/gps.cpp | 8 +++++---
 1 file changed, 5 insertions(+), 3 deletions(-)

diff --git a/src/modules/local_position_estimator/sensors/gps.cpp b/src/modules/local_position_estimator/sensors/gps.cpp
index c71512deb9..617849b50f 100644
--- a/src/modules/local_position_estimator/sensors/gps.cpp
+++ b/src/modules/local_position_estimator/sensors/gps.cpp
@@ -8,6 +8,8 @@ extern orb_advert_t mavlink_log_pub;
 // to initialize
 static const uint32_t 		REQ_GPS_INIT_COUNT = 10;
 static const uint32_t 		GPS_TIMEOUT =      1000000; // 1.0 s
+static const float 			GPS_EPH_MIN =      2.0; // m, min allowed by gps self reporting
+static const float 			GPS_EPV_MIN =      2.0; // m, min allowed by gps self reporting
 
 void BlockLocalPositionEstimator::gpsInit()
 {
@@ -122,12 +124,12 @@ void BlockLocalPositionEstimator::gpsCorrect()
 	float var_vxy = _gps_vxy_stddev.get() * _gps_vxy_stddev.get();
 	float var_vz = _gps_vz_stddev.get() * _gps_vz_stddev.get();
 
-	// if field is not zero, set it to the value provided
-	if (_sub_gps.get().eph > 1e-3f) {
+	// if field is not below minimum, set it to the value provided
+	if (_sub_gps.get().eph > GPS_EPH_MIN) {
 		var_xy = _sub_gps.get().eph * _sub_gps.get().eph;
 	}
 
-	if (_sub_gps.get().epv > 1e-3f) {
+	if (_sub_gps.get().epv > GPS_EPV_MIN) {
 		var_z = _sub_gps.get().epv * _sub_gps.get().epv;
 	}
 

From 050eedc4f8b65a2273683e658eddebcf92009e7d Mon Sep 17 00:00:00 2001
From: Daniel Agar <daniel@agar.ca>
Date: Mon, 27 Jun 2016 10:16:24 -0400
Subject: [PATCH 0835/1230] mavlink publish WIND_COV (#4913)

* mavlink publish WIND_COV

-closes #4678

* px4fmu-v2_default disable logger and sync configs
---
 cmake/configs/nuttx_px4fmu-v2_default.cmake |   2 +-
 cmake/configs/nuttx_px4fmu-v2_ekf2.cmake    |  13 ++-
 cmake/configs/nuttx_px4fmu-v2_test.cmake    |   7 +-
 src/modules/mavlink/mavlink_main.cpp        |   4 +
 src/modules/mavlink/mavlink_messages.cpp    | 109 ++++++++++++++++++--
 5 files changed, 115 insertions(+), 20 deletions(-)

diff --git a/cmake/configs/nuttx_px4fmu-v2_default.cmake b/cmake/configs/nuttx_px4fmu-v2_default.cmake
index 1699e6df9d..72876e8800 100644
--- a/cmake/configs/nuttx_px4fmu-v2_default.cmake
+++ b/cmake/configs/nuttx_px4fmu-v2_default.cmake
@@ -101,7 +101,7 @@ set(config_module_list
 	#
 	# Logging
 	#
-	modules/logger
+	#modules/logger
 	modules/sdlog2
 
 	#
diff --git a/cmake/configs/nuttx_px4fmu-v2_ekf2.cmake b/cmake/configs/nuttx_px4fmu-v2_ekf2.cmake
index 1eeab2fa96..f43b782045 100644
--- a/cmake/configs/nuttx_px4fmu-v2_ekf2.cmake
+++ b/cmake/configs/nuttx_px4fmu-v2_ekf2.cmake
@@ -41,11 +41,13 @@ set(config_module_list
 	modules/sensors
 	#drivers/mkblctrl
 	drivers/px4flow
-	drivers/oreoled
+	#drivers/oreoled
 	drivers/gimbal
 	drivers/pwm_input
 	drivers/camera_trigger
 	drivers/bst
+	#drivers/snapdragon_rc_pwm
+	#drivers/lis3mdl
 
 	#
 	# System commands
@@ -64,6 +66,9 @@ set(config_module_list
 	systemcmds/mtd
 	systemcmds/dumpfile
 	systemcmds/ver
+	#systemcmds/sd_bench
+	#systemcmds/tests
+	systemcmds/motor_ramp
 
 	#
 	# General system control
@@ -72,7 +77,7 @@ set(config_module_list
 	modules/load_mon
 	modules/navigator
 	modules/mavlink
-	modules/gpio_led
+	#modules/gpio_led
 	modules/uavcan
 	modules/land_detector
 
@@ -94,8 +99,8 @@ set(config_module_list
 	#
 	# Logging
 	#
-	modules/sdlog2
 	modules/logger
+	modules/sdlog2
 
 	#
 	# Library modules
@@ -136,7 +141,7 @@ set(config_module_list
 	#
 	# Rover apps
 	#
-	examples/rover_steering_control
+	#examples/rover_steering_control
 
 	#
 	# Demo apps
diff --git a/cmake/configs/nuttx_px4fmu-v2_test.cmake b/cmake/configs/nuttx_px4fmu-v2_test.cmake
index 27bdf6ea8a..80f8649e3d 100644
--- a/cmake/configs/nuttx_px4fmu-v2_test.cmake
+++ b/cmake/configs/nuttx_px4fmu-v2_test.cmake
@@ -48,7 +48,6 @@ set(config_module_list
 	#drivers/bst
 	#drivers/snapdragon_rc_pwm
 	#drivers/lis3mdl
-	#drivers/bmi160
 
 	#
 	# System commands
@@ -67,8 +66,9 @@ set(config_module_list
 	systemcmds/mtd
 	systemcmds/dumpfile
 	systemcmds/ver
-	systemcmds/sd_bench
+	#systemcmds/sd_bench
 	systemcmds/tests
+	systemcmds/motor_ramp
 
 	#
 	# Testing
@@ -87,7 +87,7 @@ set(config_module_list
 	modules/load_mon
 	modules/navigator
 	modules/mavlink
-	modules/gpio_led
+	#modules/gpio_led
 	modules/uavcan
 	modules/land_detector
 
@@ -102,7 +102,6 @@ set(config_module_list
 	#
 	# Vehicle Control
 	#
-	# modules/segway # XXX Needs GCC 4.7 fix
 	modules/fw_pos_control_l1
 	modules/fw_att_control
 	modules/mc_att_control
diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp
index 1d4d3a58b3..30ad39d3d3 100644
--- a/src/modules/mavlink/mavlink_main.cpp
+++ b/src/modules/mavlink/mavlink_main.cpp
@@ -1868,6 +1868,7 @@ Mavlink::task_main(int argc, char *argv[])
 		configure_stream("ESTIMATOR_STATUS", 0.5f);
 		configure_stream("ADSB_VEHICLE", 2.0f);
 		configure_stream("NAV_CONTROLLER_OUTPUT", 2.0f);
+		configure_stream("WIND", 2.0f);
 		break;
 
 	case MAVLINK_MODE_ONBOARD:
@@ -1899,6 +1900,7 @@ Mavlink::task_main(int argc, char *argv[])
 		configure_stream("ESTIMATOR_STATUS", 1.0f);
 		configure_stream("ADSB_VEHICLE", 10.0f);
 		configure_stream("NAV_CONTROLLER_OUTPUT", 10.0f);
+		configure_stream("WIND", 10.0f);
 		break;
 
 	case MAVLINK_MODE_OSD:
@@ -1915,6 +1917,7 @@ Mavlink::task_main(int argc, char *argv[])
 		configure_stream("EXTENDED_SYS_STATE", 1.0f);
 		configure_stream("ALTITUDE", 1.0f);
 		configure_stream("ESTIMATOR_STATUS", 1.0f);
+		configure_stream("WIND", 2.0f);
 		break;
 
 	case MAVLINK_MODE_MAGIC:
@@ -1949,6 +1952,7 @@ Mavlink::task_main(int argc, char *argv[])
 		configure_stream("ESTIMATOR_STATUS", 5.0f);
 		configure_stream("ADSB_VEHICLE", 20.0f);
 		configure_stream("NAV_CONTROLLER_OUTPUT", 10.0f);
+		configure_stream("WIND", 10.0f);
 	default:
 		break;
 	}
diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp
index 125530fb98..b62f150dbc 100644
--- a/src/modules/mavlink/mavlink_messages.cpp
+++ b/src/modules/mavlink/mavlink_messages.cpp
@@ -39,13 +39,21 @@
  * @author Anton Babushkin <anton.babushkin@me.com>
  */
 
-#include <px4_time.h>
 #include <stdio.h>
 #include <errno.h>
 
+#include "mavlink_main.h"
+#include "mavlink_messages.h"
+
 #include <commander/px4_custom_mode.h>
+#include <drivers/drv_pwm_output.h>
+#include <drivers/drv_rc_input.h>
 #include <lib/geo/geo.h>
-#include <uORB/uORB.h>
+#include <mathlib/mathlib.h>
+#include <px4_time.h>
+#include <systemlib/err.h>
+#include <systemlib/mavlink_log.h>
+
 #include <uORB/topics/actuator_armed.h>
 #include <uORB/topics/actuator_controls.h>
 #include <uORB/topics/actuator_outputs.h>
@@ -57,10 +65,10 @@
 #include <uORB/topics/debug_key_value.h>
 #include <uORB/topics/distance_sensor.h>
 #include <uORB/topics/estimator_status.h>
+#include <uORB/topics/fw_pos_ctrl_status.h>
 #include <uORB/topics/home_position.h>
 #include <uORB/topics/manual_control_setpoint.h>
 #include <uORB/topics/mavlink_log.h>
-#include <uORB/topics/fw_pos_ctrl_status.h>
 #include <uORB/topics/optical_flow.h>
 #include <uORB/topics/position_setpoint_triplet.h>
 #include <uORB/topics/sensor_combined.h>
@@ -79,15 +87,9 @@
 #include <uORB/topics/vehicle_status.h>
 #include <uORB/topics/vision_position_estimate.h>
 #include <uORB/topics/vtol_vehicle_status.h>
-#include <drivers/drv_rc_input.h>
-#include <drivers/drv_pwm_output.h>
-#include <systemlib/err.h>
-#include <systemlib/mavlink_log.h>
+#include <uORB/topics/wind_estimate.h>
+#include <uORB/uORB.h>
 
-#include <mathlib/mathlib.h>
-
-#include "mavlink_messages.h"
-#include "mavlink_main.h"
 
 static uint16_t cm_uint16_from_m_float(float m);
 static void get_mavlink_mode_state(struct vehicle_status_s *status, uint8_t *mavlink_state,
@@ -3127,6 +3129,90 @@ protected:
 	}
 };
 
+class MavlinkStreamWind : public MavlinkStream
+{
+public:
+	const char *get_name() const
+	{
+		return MavlinkStreamWind::get_name_static();
+	}
+
+	static const char *get_name_static()
+	{
+		return "WIND";
+	}
+
+	static uint8_t get_id_static()
+	{
+		return MAVLINK_MSG_ID_WIND_COV;
+	}
+
+    uint8_t get_id()
+    {
+        return get_id_static();
+    }
+
+	static MavlinkStream *new_instance(Mavlink *mavlink)
+	{
+		return new MavlinkStreamWind(mavlink);
+	}
+
+	unsigned get_size()
+	{
+		return MAVLINK_MSG_ID_WIND_COV_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES;
+	}
+
+private:
+	MavlinkOrbSubscription *_wind_estimate_sub;
+	uint64_t _wind_estimate_time;
+
+	MavlinkOrbSubscription *_global_pos_sub;
+	uint64_t _global_pos_time;
+
+	/* do not allow top copying this class */
+	MavlinkStreamWind(MavlinkStreamWind &);
+	MavlinkStreamWind& operator = (const MavlinkStreamWind &);
+
+protected:
+	explicit MavlinkStreamWind(Mavlink *mavlink) : MavlinkStream(mavlink),
+		_wind_estimate_sub(_mavlink->add_orb_subscription(ORB_ID(wind_estimate))),
+		_wind_estimate_time(0),
+		_global_pos_sub(_mavlink->add_orb_subscription(ORB_ID(vehicle_global_position))),
+		_global_pos_time(0)
+	{}
+
+	void send(const hrt_abstime t)
+	{
+		struct wind_estimate_s wind_estimate;
+
+		bool updated = _wind_estimate_sub->update(&_wind_estimate_time, &wind_estimate);
+
+		if (updated) {
+
+			mavlink_wind_cov_t msg;
+
+			msg.time_usec = wind_estimate.timestamp;
+
+			msg.wind_x = wind_estimate.windspeed_north;
+			msg.wind_y = wind_estimate.windspeed_east;
+			msg.wind_z = 0.0f;
+
+			msg.var_horiz = wind_estimate.covariance_north + wind_estimate.covariance_east;
+			msg.var_vert = 0.0f;
+
+			struct vehicle_global_position_s global_pos;
+			_global_pos_sub->update(&_global_pos_time, &global_pos);
+			msg.wind_alt = (_global_pos_time > 0) ? global_pos.alt : NAN;
+
+
+			msg.horiz_accuracy = 0.0f;
+			msg.vert_accuracy = 0.0f;
+
+			mavlink_msg_wind_cov_send_struct(_mavlink->get_channel(), &msg);
+		}
+	}
+};
+
 const StreamListItem *streams_list[] = {
 	new StreamListItem(&MavlinkStreamHeartbeat::new_instance, &MavlinkStreamHeartbeat::get_name_static, &MavlinkStreamHeartbeat::get_id_static),
 	new StreamListItem(&MavlinkStreamStatustext::new_instance, &MavlinkStreamStatustext::get_name_static, &MavlinkStreamStatustext::get_id_static),
@@ -3169,5 +3255,6 @@ const StreamListItem *streams_list[] = {
 	new StreamListItem(&MavlinkStreamExtendedSysState::new_instance, &MavlinkStreamExtendedSysState::get_name_static, &MavlinkStreamExtendedSysState::get_id_static),
 	new StreamListItem(&MavlinkStreamAltitude::new_instance, &MavlinkStreamAltitude::get_name_static, &MavlinkStreamAltitude::get_id_static),
 	new StreamListItem(&MavlinkStreamADSBVehicle::new_instance, &MavlinkStreamADSBVehicle::get_name_static, &MavlinkStreamADSBVehicle::get_id_static),
+	new StreamListItem(&MavlinkStreamWind::new_instance, &MavlinkStreamWind::get_name_static, &MavlinkStreamWind::get_id_static),
 	nullptr
 };

From ec35e77175cdc71efe7bd8ab5ec52aa1a13750a5 Mon Sep 17 00:00:00 2001
From: Roman Bapst <bapstr@ethz.ch>
Date: Mon, 27 Jun 2016 17:31:43 +0200
Subject: [PATCH 0836/1230] px4io driver: fix reporting of mixer limits (#4922)

mixer limit topic was not filled correctly

Signed-off-by: tumbili <roman@px4.io>
---
 src/drivers/px4io/px4io.cpp | 4 +++-
 1 file changed, 3 insertions(+), 1 deletion(-)

diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp
index 66ea188244..7937bcfd40 100644
--- a/src/drivers/px4io/px4io.cpp
+++ b/src/drivers/px4io/px4io.cpp
@@ -1950,7 +1950,9 @@ PX4IO::io_publish_pwm_outputs()
 	/* get mixer status flags from IO */
 	uint16_t mixer_status;
 	ret = io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_MIXER, &mixer_status, sizeof(mixer_status) / sizeof(uint16_t));
-	memcpy(&motor_limits, &mixer_status, sizeof(motor_limits));
+	motor_limits.lower_limit = mixer_status & PX4IO_P_STATUS_MIXER_LOWER_LIMIT;
+	motor_limits.upper_limit = mixer_status & PX4IO_P_STATUS_MIXER_UPPER_LIMIT;
+	motor_limits.yaw = mixer_status & PX4IO_P_STATUS_MIXER_YAW_LIMIT;
 
 	if (ret != OK) {
 		return ret;

From b28bfce186fa26f7fe5d93669681027c4611daee Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Beat=20K=C3=BCng?= <beat-kueng@gmx.net>
Date: Tue, 28 Jun 2016 09:25:36 +0200
Subject: [PATCH 0837/1230] position_estimator_inav: fix compiler issue for GCC
 6.1.1 (#4923)
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

GCC output:
implicit conversion from ‘float’ to ‘double’ to match other operand of
binary expression [-Werror=double-promotion]

It seems gcc 6.1.1 uses the float variant of fabs, whereas older gcc's
use the double version. This makes it compile for both.
---
 .../position_estimator_inav_main.cpp                      | 8 ++++----
 1 file changed, 4 insertions(+), 4 deletions(-)

diff --git a/src/modules/position_estimator_inav/position_estimator_inav_main.cpp b/src/modules/position_estimator_inav/position_estimator_inav_main.cpp
index 8d4b882b0b..191b5be099 100644
--- a/src/modules/position_estimator_inav/position_estimator_inav_main.cpp
+++ b/src/modules/position_estimator_inav/position_estimator_inav_main.cpp
@@ -657,9 +657,9 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
 					if (updated)
 						orb_copy(ORB_ID(vehicle_rates_setpoint), vehicle_rate_sp_sub, &rates_setpoint);
 
-					double rate_threshold = 0.15f;
+					float rate_threshold = 0.15f;
 
-					if (fabs(rates_setpoint.pitch) < rate_threshold) {
+					if (fabsf(rates_setpoint.pitch) < rate_threshold) {
 						//warnx("[inav] test ohne comp");
 						flow_ang[0] = (flow.pixel_flow_x_integral / (float)flow.integration_timespan * 1000000.0f) * params.flow_k;//for now the flow has to be scaled (to small)
 					}
@@ -670,7 +670,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
 							       + gyro_offset_filtered[0]) * params.flow_k;//for now the flow has to be scaled (to small)
 					}
 
-					if (fabs(rates_setpoint.roll) < rate_threshold) {
+					if (fabsf(rates_setpoint.roll) < rate_threshold) {
 						flow_ang[1] = (flow.pixel_flow_y_integral / (float)flow.integration_timespan * 1000000.0f) * params.flow_k;//for now the flow has to be scaled (to small)
 					}
 					else {
@@ -681,7 +681,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
 
 					/* flow measurements vector */
 					float flow_m[3];
-					if (fabs(rates_setpoint.yaw) < rate_threshold) {
+					if (fabsf(rates_setpoint.yaw) < rate_threshold) {
 						flow_m[0] = -flow_ang[0] * flow_dist;
 						flow_m[1] = -flow_ang[1] * flow_dist;
 					} else {

From 8a12dee125c37a3e48104877efd0f109d90b039b Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Beat=20K=C3=BCng?= <beat-kueng@gmx.net>
Date: Tue, 28 Jun 2016 09:26:36 +0200
Subject: [PATCH 0838/1230] cmake: remove all module.mk files & cmake
 conversion script (#4918)

It seems these files are leftovers.
---
 cmake/scripts/convert_modules_to_cmake.py     | 91 -------------------
 cmake/scripts/test_compare.py                 | 50 ----------
 src/drivers/hott/hott_sensors/module.mk       | 42 ---------
 src/drivers/hott/hott_telemetry/module.mk     | 42 ---------
 src/drivers/qshell/posix/module.mk            | 46 ----------
 src/drivers/qshell/qurt/module.mk             | 46 ----------
 src/drivers/stm32/adc/module.mk               | 44 ---------
 src/drivers/stm32/tone_alarm/module.mk        | 44 ---------
 src/examples/px4_simple_app/module.mk         | 40 --------
 src/lib/mathlib/math/filter/module.mk         | 43 ---------
 .../commander/commander_tests/module.mk       | 42 ---------
 src/modules/muorb/adsp/module.mk              | 52 -----------
 src/modules/muorb/krait/module.mk             | 48 ----------
 src/modules/systemlib/mixer/module.mk         | 47 ----------
 src/platforms/common/module.mk                |  6 --
 src/platforms/nuttx/module.mk                 | 40 --------
 src/platforms/nuttx/px4_layer/module.mk       | 41 ---------
 src/platforms/posix/px4_layer/module.mk       | 48 ----------
 src/platforms/posix/tests/hello/module.mk     | 43 ---------
 src/platforms/posix/tests/hrt_test/module.mk  | 43 ---------
 src/platforms/posix/tests/muorb/module.mk     | 47 ----------
 .../posix/tests/vcdev_test/module.mk          | 43 ---------
 src/platforms/posix/tests/wqueue/module.mk    | 43 ---------
 src/platforms/posix/work_queue/module.mk      | 54 -----------
 src/platforms/px4_app.h                       |  5 -
 src/platforms/qurt/px4_layer/module.mk        | 51 -----------
 src/platforms/qurt/tests/hello/module.mk      | 43 ---------
 src/platforms/qurt/tests/muorb/module.mk      | 48 ----------
 src/systemcmds/bl_update/module.mk            | 43 ---------
 src/systemcmds/config/module.mk               | 44 ---------
 src/systemcmds/dumpfile/module.mk             | 41 ---------
 src/systemcmds/esc_calib/module.mk            | 43 ---------
 src/systemcmds/i2c/module.mk                  | 41 ---------
 src/systemcmds/mixer/module.mk                | 48 ----------
 src/systemcmds/motor_test/module.mk           | 43 ---------
 src/systemcmds/mtd/module.mk                  | 11 ---
 src/systemcmds/nshterm/module.mk              | 45 ---------
 src/systemcmds/perf/module.mk                 | 43 ---------
 src/systemcmds/pwm/module.mk                  | 43 ---------
 src/systemcmds/reboot/module.mk               | 43 ---------
 src/systemcmds/reflect/module.mk              | 41 ---------
 src/systemcmds/top/module.mk                  | 44 ---------
 src/systemcmds/topic_listener/module.mk       | 46 ----------
 src/systemcmds/usb_connected/module.mk        | 37 --------
 src/systemcmds/ver/module.mk                  | 44 ---------
 45 files changed, 1932 deletions(-)
 delete mode 100755 cmake/scripts/convert_modules_to_cmake.py
 delete mode 100755 cmake/scripts/test_compare.py
 delete mode 100644 src/drivers/hott/hott_sensors/module.mk
 delete mode 100644 src/drivers/hott/hott_telemetry/module.mk
 delete mode 100644 src/drivers/qshell/posix/module.mk
 delete mode 100644 src/drivers/qshell/qurt/module.mk
 delete mode 100644 src/drivers/stm32/adc/module.mk
 delete mode 100644 src/drivers/stm32/tone_alarm/module.mk
 delete mode 100644 src/examples/px4_simple_app/module.mk
 delete mode 100644 src/lib/mathlib/math/filter/module.mk
 delete mode 100644 src/modules/commander/commander_tests/module.mk
 delete mode 100644 src/modules/muorb/adsp/module.mk
 delete mode 100644 src/modules/muorb/krait/module.mk
 delete mode 100644 src/modules/systemlib/mixer/module.mk
 delete mode 100644 src/platforms/common/module.mk
 delete mode 100644 src/platforms/nuttx/module.mk
 delete mode 100644 src/platforms/nuttx/px4_layer/module.mk
 delete mode 100644 src/platforms/posix/px4_layer/module.mk
 delete mode 100644 src/platforms/posix/tests/hello/module.mk
 delete mode 100644 src/platforms/posix/tests/hrt_test/module.mk
 delete mode 100644 src/platforms/posix/tests/muorb/module.mk
 delete mode 100644 src/platforms/posix/tests/vcdev_test/module.mk
 delete mode 100644 src/platforms/posix/tests/wqueue/module.mk
 delete mode 100644 src/platforms/posix/work_queue/module.mk
 delete mode 100644 src/platforms/qurt/px4_layer/module.mk
 delete mode 100644 src/platforms/qurt/tests/hello/module.mk
 delete mode 100644 src/platforms/qurt/tests/muorb/module.mk
 delete mode 100644 src/systemcmds/bl_update/module.mk
 delete mode 100644 src/systemcmds/config/module.mk
 delete mode 100644 src/systemcmds/dumpfile/module.mk
 delete mode 100644 src/systemcmds/esc_calib/module.mk
 delete mode 100644 src/systemcmds/i2c/module.mk
 delete mode 100644 src/systemcmds/mixer/module.mk
 delete mode 100644 src/systemcmds/motor_test/module.mk
 delete mode 100644 src/systemcmds/mtd/module.mk
 delete mode 100644 src/systemcmds/nshterm/module.mk
 delete mode 100644 src/systemcmds/perf/module.mk
 delete mode 100644 src/systemcmds/pwm/module.mk
 delete mode 100644 src/systemcmds/reboot/module.mk
 delete mode 100644 src/systemcmds/reflect/module.mk
 delete mode 100644 src/systemcmds/top/module.mk
 delete mode 100644 src/systemcmds/topic_listener/module.mk
 delete mode 100644 src/systemcmds/usb_connected/module.mk
 delete mode 100644 src/systemcmds/ver/module.mk

diff --git a/cmake/scripts/convert_modules_to_cmake.py b/cmake/scripts/convert_modules_to_cmake.py
deleted file mode 100755
index 7cd8768c65..0000000000
--- a/cmake/scripts/convert_modules_to_cmake.py
+++ /dev/null
@@ -1,91 +0,0 @@
-#!/usr/bin/env python
-
-from __future__ import print_function
-
-import argparse
-import os
-import sys
-import fnmatch
-import re
-import shutil
-import jinja2
-
-src_path = os.path.join(os.path.curdir, 'src')
-
-parser = argparse.ArgumentParser('converts module.mk to CMakeList.txt, run in root of repo')
-parser.add_argument('path', help='directory of modules to convert')
-parser.add_argument('--overwrite', help='overwrite existing files', dest='overwrite', action='store_true')
-parser.add_argument('--backup', help='create backup of existing files if overwriting', dest='backup', action='store_true')
-parser.set_defaults(overwrite=False, backup=False)
-args = parser.parse_args()
-
-cmake_template = jinja2.Template(open('cmake/scripts/cmake_lists.jinja', 'r').read())
-
-module_files = []
-for root, dirnames, filenames in os.walk(args.path):
-    for filename in fnmatch.filter(filenames, 'module.mk'):
-        module_files.append(os.path.join(root, filename))
-
-
-search_data = [
-    # name             # re string
-    ('command', r'.*MODULE_COMMAND\s*[\+]?=\s*([^\n]+)'),
-    ('stacksize', r'.*MODULE_STACKSIZE\s*[\+]?=([^\n]+)'),
-    ('extracxxflags', r'.*EXTRACXXFLAGS\s*[\+]?=([^\n]+)'),
-    ('extracflags', r'.*EXTRACFLAGS\s*[\+]?=\s*([^\s]+)\s*'),
-    ('priority', r'.*MODULE_PRIORITY\s*[\+]?=\s*([^\s]+)\s*'),
-    ('maxoptimization', r'.*MAXOPTIMIZATION\s*[\+]?=\s*([^\s]+)\s*'),
-    ('srcs', '.*SRCS\s*[\+]?=([^\n\\\]*([\\\]\s*\n[^\n\\\]*)*)'),
-    ('include_dirs', '.*INCLUDE_DIRS\s*[\+]?=([^\n\\\]*([\\\]\s*\n[^\n\\\]*)*)'),
-    ]
-
-progs = {}
-for name, re_str in search_data:
-    progs[name] = re.compile(re_str)
-
-for module_file in module_files:
-
-    data = {}
-    with open(module_file, 'r') as f:
-        module_text = f.read()
-        data['text'] = module_text
-        module_dir = os.path.dirname(module_file)
-        data['module'] = os.path.relpath(module_dir, src_path).replace(
-                os.sep, '__').split('.')[0]
-        #print(module_text)
-        for name, re_str in search_data:
-            result = progs[name].search(module_text)
-            if result is not None:
-                d = result.group(1).strip()
-                if name in ['srcs', 'extracxxflags', 'extracflags']:
-                    d_store = d.replace('\\', '').split()
-                elif name == 'include_dirs':
-                    d_store = d.replace('(', '{').replace(')', '}').split()
-                else:
-                    d_store = d
-                data[name] = d_store
-            else:
-                data[name] = ''
-
-    cmake_file = os.path.join(os.path.dirname(module_file), 'CMakeLists.txt')
-    cmake_file_backup = cmake_file + '.backup'
-
-    if os.path.exists(cmake_file):
-        if args.backup:
-            if os.path.exists(cmake_file_backup):
-                print('error: file already exists:', cmake_file_backup)
-                continue
-            else:
-                shutil.copyfile(cmake_file, cmake_file_backup)
-        if args.overwrite:
-            print('overwriting', cmake_file)
-        else:
-            print('error: file already exists:', cmake_file)
-            continue
-
-    with open(cmake_file, 'w') as f:
-        data_rendered = cmake_template.render(data=data)
-        f.write(data_rendered)
-
-
-# vim: set et fenc= ff=unix sts=4 sw=4 ts=4 : 
diff --git a/cmake/scripts/test_compare.py b/cmake/scripts/test_compare.py
deleted file mode 100755
index fb2b4745a4..0000000000
--- a/cmake/scripts/test_compare.py
+++ /dev/null
@@ -1,50 +0,0 @@
-#!/usr/bin/env python
-"""
-This runs a command and compares output to a known file over
-a given line range.
-"""
-from __future__ import print_function
-import subprocess
-import argparse
-import os
-
-
-#pylint: disable=invalid-name
-parser = argparse.ArgumentParser(description='Process some integers.')
-parser.add_argument('--command', required=True)
-parser.add_argument('--stdin', required=True)
-parser.add_argument('--stdout', required=True)
-parser.add_argument('--check', required=True)
-parser.add_argument('--start', default=0)
-parser.add_argument('--stop', default=-1)
-args = parser.parse_args()
-
-d = os.path.dirname(args.stdout)
-if not os.path.exists(d):
-    os.makedirs(d)
-
-with open(args.stdout, 'w') as outfile:
-    with open(args.stdin, 'r') as infile:
-        proc = subprocess.Popen(
-            args.command, stdout=outfile, stdin=infile)
-proc.communicate()
-
-i_start = int(args.start)
-i_stop = int(args.stop)
-
-with open(args.stdout, 'r') as outfile:
-    out_contents = file.readlines(outfile)
-out_contents = "".join(out_contents[i_start:i_stop])
-
-with open(args.check, 'r') as checkfile:
-    check_contents = file.readlines(checkfile)
-check_contents = "".join(check_contents[i_start:i_stop])
-
-if (out_contents != check_contents):
-    print("output:\n", out_contents)
-    print("check:\n", check_contents)
-    exit(1)
-
-exit(0)
-
-# vim: set et ft=python fenc= ff=unix sts=4 sw=4 ts=4 :
diff --git a/src/drivers/hott/hott_sensors/module.mk b/src/drivers/hott/hott_sensors/module.mk
deleted file mode 100644
index 25a6f0ac04..0000000000
--- a/src/drivers/hott/hott_sensors/module.mk
+++ /dev/null
@@ -1,42 +0,0 @@
-############################################################################
-#
-#   Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved.
-#
-# Redistribution and use in source and binary forms, with or without
-# modification, are permitted provided that the following conditions
-# are met:
-#
-# 1. Redistributions of source code must retain the above copyright
-#    notice, this list of conditions and the following disclaimer.
-# 2. Redistributions in binary form must reproduce the above copyright
-#    notice, this list of conditions and the following disclaimer in
-#    the documentation and/or other materials provided with the
-#    distribution.
-# 3. Neither the name PX4 nor the names of its contributors may be
-#    used to endorse or promote products derived from this software
-#    without specific prior written permission.
-#
-# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
-# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
-# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
-# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
-# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
-# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
-# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
-# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
-# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
-# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
-# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
-# POSSIBILITY OF SUCH DAMAGE.
-#
-############################################################################
-
-#
-# Graupner HoTT Sensors application.
-#
-
-MODULE_COMMAND	= hott_sensors
-
-SRCS		= hott_sensors.cpp
-
-MAXOPTIMIZATION	 = -Os
diff --git a/src/drivers/hott/hott_telemetry/module.mk b/src/drivers/hott/hott_telemetry/module.mk
deleted file mode 100644
index 9de47b356c..0000000000
--- a/src/drivers/hott/hott_telemetry/module.mk
+++ /dev/null
@@ -1,42 +0,0 @@
-############################################################################
-#
-#   Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved.
-#
-# Redistribution and use in source and binary forms, with or without
-# modification, are permitted provided that the following conditions
-# are met:
-#
-# 1. Redistributions of source code must retain the above copyright
-#    notice, this list of conditions and the following disclaimer.
-# 2. Redistributions in binary form must reproduce the above copyright
-#    notice, this list of conditions and the following disclaimer in
-#    the documentation and/or other materials provided with the
-#    distribution.
-# 3. Neither the name PX4 nor the names of its contributors may be
-#    used to endorse or promote products derived from this software
-#    without specific prior written permission.
-#
-# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
-# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
-# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
-# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
-# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
-# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
-# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
-# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
-# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
-# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
-# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
-# POSSIBILITY OF SUCH DAMAGE.
-#
-############################################################################
-
-#
-# Graupner HoTT Telemetry applications.
-#
-
-MODULE_COMMAND	= hott_telemetry
-
-SRCS		= hott_telemetry.cpp
-
-MAXOPTIMIZATION	 = -Os
diff --git a/src/drivers/qshell/posix/module.mk b/src/drivers/qshell/posix/module.mk
deleted file mode 100644
index 33a313576d..0000000000
--- a/src/drivers/qshell/posix/module.mk
+++ /dev/null
@@ -1,46 +0,0 @@
-############################################################################
-#
-#   Copyright (c) 2016 PX4 Development Team. All rights reserved.
-#
-# Redistribution and use in source and binary forms, with or without
-# modification, are permitted provided that the following conditions
-# are met:
-#
-# 1. Redistributions of source code must retain the above copyright
-#    notice, this list of conditions and the following disclaimer.
-# 2. Redistributions in binary form must reproduce the above copyright
-#    notice, this list of conditions and the following disclaimer in
-#    the documentation and/or other materials provided with the
-#    distribution.
-# 3. Neither the name PX4 nor the names of its contributors may be
-#    used to endorse or promote products derived from this software
-#    without specific prior written permission.
-#
-# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
-# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
-# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
-# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
-# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
-# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
-# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
-# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
-# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
-# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
-# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
-# POSSIBILITY OF SUCH DAMAGE.
-#
-############################################################################
-
-#
-# Send shell commands to qurt
-#
-
-MODULE_COMMAND	= qshell
-
-SRCS		= \
-		  qshell_start_qurt.cpp \
-		  qshell.cpp
-
-INCLUDE_DIRS    += $(PX4_BASE)/src/modules/uORB  \
-                  $(PX4_BASE)/src/platforms \
-                  $(PX4_BASE)/src/modules
diff --git a/src/drivers/qshell/qurt/module.mk b/src/drivers/qshell/qurt/module.mk
deleted file mode 100644
index 97473d9554..0000000000
--- a/src/drivers/qshell/qurt/module.mk
+++ /dev/null
@@ -1,46 +0,0 @@
-############################################################################
-#
-#   Copyright (c) 2016 PX4 Development Team. All rights reserved.
-#
-# Redistribution and use in source and binary forms, with or without
-# modification, are permitted provided that the following conditions
-# are met:
-#
-# 1. Redistributions of source code must retain the above copyright
-#    notice, this list of conditions and the following disclaimer.
-# 2. Redistributions in binary form must reproduce the above copyright
-#    notice, this list of conditions and the following disclaimer in
-#    the documentation and/or other materials provided with the
-#    distribution.
-# 3. Neither the name PX4 nor the names of its contributors may be
-#    used to endorse or promote products derived from this software
-#    without specific prior written permission.
-#
-# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
-# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
-# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
-# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
-# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
-# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
-# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
-# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
-# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
-# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
-# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
-# POSSIBILITY OF SUCH DAMAGE.
-#
-############################################################################
-
-#
-# Listener for shell commands from posix
-#
-
-MODULE_COMMAND	= qshell
-
-SRCS		= \
-		  qshell_start_qurt.cpp \
-		  qshell.cpp
-
-INCLUDE_DIRS    += $(PX4_BASE)/src/modules/uORB  \
-                  $(PX4_BASE)/src/platforms \
-                  $(PX4_BASE)/src/modules
diff --git a/src/drivers/stm32/adc/module.mk b/src/drivers/stm32/adc/module.mk
deleted file mode 100644
index 38ea490a0e..0000000000
--- a/src/drivers/stm32/adc/module.mk
+++ /dev/null
@@ -1,44 +0,0 @@
-############################################################################
-#
-#   Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved.
-#
-# Redistribution and use in source and binary forms, with or without
-# modification, are permitted provided that the following conditions
-# are met:
-#
-# 1. Redistributions of source code must retain the above copyright
-#    notice, this list of conditions and the following disclaimer.
-# 2. Redistributions in binary form must reproduce the above copyright
-#    notice, this list of conditions and the following disclaimer in
-#    the documentation and/or other materials provided with the
-#    distribution.
-# 3. Neither the name PX4 nor the names of its contributors may be
-#    used to endorse or promote products derived from this software
-#    without specific prior written permission.
-#
-# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
-# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
-# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
-# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
-# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
-# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
-# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
-# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
-# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
-# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
-# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
-# POSSIBILITY OF SUCH DAMAGE.
-#
-############################################################################
-
-#
-# STM32 ADC driver
-#
-
-MODULE_COMMAND	= adc
-
-SRCS		= adc.cpp
-
-INCLUDE_DIRS	+= $(NUTTX_SRC)/arch/arm/src/stm32 $(NUTTX_SRC)/arch/arm/src/common
-
-MAXOPTIMIZATION	 = -Os
diff --git a/src/drivers/stm32/tone_alarm/module.mk b/src/drivers/stm32/tone_alarm/module.mk
deleted file mode 100644
index 25a194ef61..0000000000
--- a/src/drivers/stm32/tone_alarm/module.mk
+++ /dev/null
@@ -1,44 +0,0 @@
-############################################################################
-#
-#   Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved.
-#
-# Redistribution and use in source and binary forms, with or without
-# modification, are permitted provided that the following conditions
-# are met:
-#
-# 1. Redistributions of source code must retain the above copyright
-#    notice, this list of conditions and the following disclaimer.
-# 2. Redistributions in binary form must reproduce the above copyright
-#    notice, this list of conditions and the following disclaimer in
-#    the documentation and/or other materials provided with the
-#    distribution.
-# 3. Neither the name PX4 nor the names of its contributors may be
-#    used to endorse or promote products derived from this software
-#    without specific prior written permission.
-#
-# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
-# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
-# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
-# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
-# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
-# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
-# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
-# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
-# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
-# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
-# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
-# POSSIBILITY OF SUCH DAMAGE.
-#
-############################################################################
-
-#
-# Tone alarm driver
-#
-
-MODULE_COMMAND	= tone_alarm
-
-SRCS		= tone_alarm.cpp
-
-INCLUDE_DIRS	+= $(NUTTX_SRC)/arch/arm/src/stm32 $(NUTTX_SRC)/arch/arm/src/common
-
-MAXOPTIMIZATION	 = -Os
diff --git a/src/examples/px4_simple_app/module.mk b/src/examples/px4_simple_app/module.mk
deleted file mode 100644
index 32b06c3a58..0000000000
--- a/src/examples/px4_simple_app/module.mk
+++ /dev/null
@@ -1,40 +0,0 @@
-############################################################################
-#
-#   Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved.
-#
-# Redistribution and use in source and binary forms, with or without
-# modification, are permitted provided that the following conditions
-# are met:
-#
-# 1. Redistributions of source code must retain the above copyright
-#    notice, this list of conditions and the following disclaimer.
-# 2. Redistributions in binary form must reproduce the above copyright
-#    notice, this list of conditions and the following disclaimer in
-#    the documentation and/or other materials provided with the
-#    distribution.
-# 3. Neither the name PX4 nor the names of its contributors may be
-#    used to endorse or promote products derived from this software
-#    without specific prior written permission.
-#
-# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
-# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
-# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
-# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
-# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
-# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
-# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
-# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
-# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
-# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
-# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
-# POSSIBILITY OF SUCH DAMAGE.
-#
-############################################################################
-
-#
-# Basic example application
-#
-
-MODULE_COMMAND		= px4_simple_app
-
-SRCS			= px4_simple_app.c
diff --git a/src/lib/mathlib/math/filter/module.mk b/src/lib/mathlib/math/filter/module.mk
deleted file mode 100644
index f5c0647c88..0000000000
--- a/src/lib/mathlib/math/filter/module.mk
+++ /dev/null
@@ -1,43 +0,0 @@
-############################################################################
-#
-#   Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved.
-#
-# Redistribution and use in source and binary forms, with or without
-# modification, are permitted provided that the following conditions
-# are met:
-#
-# 1. Redistributions of source code must retain the above copyright
-#    notice, this list of conditions and the following disclaimer.
-# 2. Redistributions in binary form must reproduce the above copyright
-#    notice, this list of conditions and the following disclaimer in
-#    the documentation and/or other materials provided with the
-#    distribution.
-# 3. Neither the name PX4 nor the names of its contributors may be
-#    used to endorse or promote products derived from this software
-#    without specific prior written permission.
-#
-# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
-# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
-# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
-# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
-# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
-# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
-# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
-# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
-# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
-# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
-# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
-# POSSIBILITY OF SUCH DAMAGE.
-#
-############################################################################
-
-#
-# filter library
-#
-SRCS		 = LowPassFilter2p.cpp
-
-#
-# In order to include .config we first have to save off the
-# current makefile name, since app.mk needs it.
-#
-APP_MAKEFILE	:= $(lastword $(MAKEFILE_LIST))
diff --git a/src/modules/commander/commander_tests/module.mk b/src/modules/commander/commander_tests/module.mk
deleted file mode 100644
index 64afa86c44..0000000000
--- a/src/modules/commander/commander_tests/module.mk
+++ /dev/null
@@ -1,42 +0,0 @@
-############################################################################
-#
-#   Copyright (c) 2013 PX4 Development Team. All rights reserved.
-#
-# Redistribution and use in source and binary forms, with or without
-# modification, are permitted provided that the following conditions
-# are met:
-#
-# 1. Redistributions of source code must retain the above copyright
-#    notice, this list of conditions and the following disclaimer.
-# 2. Redistributions in binary form must reproduce the above copyright
-#    notice, this list of conditions and the following disclaimer in
-#    the documentation and/or other materials provided with the
-#    distribution.
-# 3. Neither the name PX4 nor the names of its contributors may be
-#    used to endorse or promote products derived from this software
-#    without specific prior written permission.
-#
-# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
-# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
-# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
-# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
-# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
-# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
-# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
-# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
-# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
-# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
-# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
-# POSSIBILITY OF SUCH DAMAGE.
-#
-############################################################################
-
-#
-# System state machine tests.
-#
-
-MODULE_COMMAND		= commander_tests
-SRCS			= commander_tests.cpp \
-			state_machine_helper_test.cpp \
-			../state_machine_helper.cpp \
-			../PreflightCheck.cpp
diff --git a/src/modules/muorb/adsp/module.mk b/src/modules/muorb/adsp/module.mk
deleted file mode 100644
index d6ff5a74df..0000000000
--- a/src/modules/muorb/adsp/module.mk
+++ /dev/null
@@ -1,52 +0,0 @@
-############################################################################
-#
-#   Copyright (c) 2012-2015 PX4 Development Team. All rights reserved.
-#
-# Redistribution and use in source and binary forms, with or without
-# modification, are permitted provided that the following conditions
-# are met:
-#
-# 1. Redistributions of source code must retain the above copyright
-#    notice, this list of conditions and the following disclaimer.
-# 2. Redistributions in binary form must reproduce the above copyright
-#    notice, this list of conditions and the following disclaimer in
-#    the documentation and/or other materials provided with the
-#    distribution.
-# 3. Neither the name PX4 nor the names of its contributors may be
-#    used to endorse or promote products derived from this software
-#    without specific prior written permission.
-#
-# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
-# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
-# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
-# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
-# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
-# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
-# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
-# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
-# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
-# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
-# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
-# POSSIBILITY OF SUCH DAMAGE.
-#
-############################################################################
-
-#
-# Makefile to build muorb
-#
-
-
-ifeq ($(PX4_TARGET_OS),qurt)
-
-SRCS		= \
-		  px4muorb.cpp \
-		  uORBFastRpcChannel.cpp
-endif
-
-
-INCLUDE_DIRS	+= \
-		  ${PX4_BASE}/src/modules/uORB
-
-endif
-
-MAXOPTIMIZATION	 = -Os
diff --git a/src/modules/muorb/krait/module.mk b/src/modules/muorb/krait/module.mk
deleted file mode 100644
index 4f30ee46e1..0000000000
--- a/src/modules/muorb/krait/module.mk
+++ /dev/null
@@ -1,48 +0,0 @@
-############################################################################
-#
-#   Copyright (c) 2012-2015 PX4 Development Team. All rights reserved.
-#
-# Redistribution and use in source and binary forms, with or without
-# modification, are permitted provided that the following conditions
-# are met:
-#
-# 1. Redistributions of source code must retain the above copyright
-#    notice, this list of conditions and the following disclaimer.
-# 2. Redistributions in binary form must reproduce the above copyright
-#    notice, this list of conditions and the following disclaimer in
-#    the documentation and/or other materials provided with the
-#    distribution.
-# 3. Neither the name PX4 nor the names of its contributors may be
-#    used to endorse or promote products derived from this software
-#    without specific prior written permission.
-#
-# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
-# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
-# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
-# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
-# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
-# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
-# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
-# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
-# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
-# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
-# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
-# POSSIBILITY OF SUCH DAMAGE.
-#
-############################################################################
-
-#
-# Makefile to build uORB
-#
-
-MODULE_COMMAND = muorb
-
-ifeq ($(PX4_TARGET_OS),posix-arm)
-SRCS			= uORBKraitFastRpcChannel.cpp \
-                          muorb_main.cpp
-INCLUDE_DIRS           += $(EXT_MUORB_LIB_ROOT)/krait/include \
-                          $(PX4_BASE)/src/modules/uORB \
-                          $(PX4_BASE)/src/modules
-
-EXTRA_LIBS             += $(EXT_MUORB_LIB_ROOT)/krait/libs/libpx4muorb.so
-endif
diff --git a/src/modules/systemlib/mixer/module.mk b/src/modules/systemlib/mixer/module.mk
deleted file mode 100644
index 3fd07f5ba1..0000000000
--- a/src/modules/systemlib/mixer/module.mk
+++ /dev/null
@@ -1,47 +0,0 @@
-############################################################################
-#
-#   Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved.
-#
-# Redistribution and use in source and binary forms, with or without
-# modification, are permitted provided that the following conditions
-# are met:
-#
-# 1. Redistributions of source code must retain the above copyright
-#    notice, this list of conditions and the following disclaimer.
-# 2. Redistributions in binary form must reproduce the above copyright
-#    notice, this list of conditions and the following disclaimer in
-#    the documentation and/or other materials provided with the
-#    distribution.
-# 3. Neither the name PX4 nor the names of its contributors may be
-#    used to endorse or promote products derived from this software
-#    without specific prior written permission.
-#
-# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
-# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
-# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
-# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
-# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
-# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
-# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
-# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
-# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
-# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
-# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
-# POSSIBILITY OF SUCH DAMAGE.
-#
-############################################################################
-
-
-#
-# mixer library
-#
-LIBNAME	= mixerlib
-	
-SRCS		= mixer.cpp \
-		  mixer_group.cpp \
-		  mixer_multirotor.cpp \
-		  mixer_simple.cpp \
-		  mixer_load.c
-
-SELF_DIR := $(dir $(lastword $(MAKEFILE_LIST)))
-include $(SELF_DIR)multi_tables.mk
diff --git a/src/platforms/common/module.mk b/src/platforms/common/module.mk
deleted file mode 100644
index 0e5f463e69..0000000000
--- a/src/platforms/common/module.mk
+++ /dev/null
@@ -1,6 +0,0 @@
-#
-# Common OS porting APIs
-#
-
-SRCS		 = px4_getopt.c
-
diff --git a/src/platforms/nuttx/module.mk b/src/platforms/nuttx/module.mk
deleted file mode 100644
index 4a2aff8249..0000000000
--- a/src/platforms/nuttx/module.mk
+++ /dev/null
@@ -1,40 +0,0 @@
-############################################################################
-#
-#   Copyright (c) 2014 PX4 Development Team. All rights reserved.
-#
-# Redistribution and use in source and binary forms, with or without
-# modification, are permitted provided that the following conditions
-# are met:
-#
-# 1. Redistributions of source code must retain the above copyright
-#    notice, this list of conditions and the following disclaimer.
-# 2. Redistributions in binary form must reproduce the above copyright
-#    notice, this list of conditions and the following disclaimer in
-#    the documentation and/or other materials provided with the
-#    distribution.
-# 3. Neither the name PX4 nor the names of its contributors may be
-#    used to endorse or promote products derived from this software
-#    without specific prior written permission.
-#
-# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
-# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
-# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
-# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
-# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
-# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
-# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
-# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
-# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
-# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
-# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
-# POSSIBILITY OF SUCH DAMAGE.
-#
-############################################################################
-
-#
-# NuttX / uORB adapter library
-#
-
-SRCS		 = 	px4_nuttx_impl.cpp
-
-MAXOPTIMIZATION	 = -Os
diff --git a/src/platforms/nuttx/px4_layer/module.mk b/src/platforms/nuttx/px4_layer/module.mk
deleted file mode 100644
index 93b55e67ab..0000000000
--- a/src/platforms/nuttx/px4_layer/module.mk
+++ /dev/null
@@ -1,41 +0,0 @@
-############################################################################
-#
-#   Copyright (c) 2015 Mark Charlebois. All rights reserved.
-#
-# Redistribution and use in source and binary forms, with or without
-# modification, are permitted provided that the following conditions
-# are met:
-#
-# 1. Redistributions of source code must retain the above copyright
-#    notice, this list of conditions and the following disclaimer.
-# 2. Redistributions in binary form must reproduce the above copyright
-#    notice, this list of conditions and the following disclaimer in
-#    the documentation and/or other materials provided with the
-#    distribution.
-# 3. Neither the name PX4 nor the names of its contributors may be
-#    used to endorse or promote products derived from this software
-#    without specific prior written permission.
-#
-# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
-# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
-# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
-# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
-# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
-# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
-# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
-# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
-# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
-# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
-# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
-# POSSIBILITY OF SUCH DAMAGE.
-#
-############################################################################
-
-#
-# NuttX porting layer
-#
-
-SRCS		 = 	px4_nuttx_tasks.c \
-			../../posix/px4_layer/px4_log.c
-
-MAXOPTIMIZATION	 = -Os
diff --git a/src/platforms/posix/px4_layer/module.mk b/src/platforms/posix/px4_layer/module.mk
deleted file mode 100644
index efa942c626..0000000000
--- a/src/platforms/posix/px4_layer/module.mk
+++ /dev/null
@@ -1,48 +0,0 @@
-############################################################################
-#
-#   Copyright (c) 2014 PX4 Development Team. All rights reserved.
-#
-# Redistribution and use in source and binary forms, with or without
-# modification, are permitted provided that the following conditions
-# are met:
-#
-# 1. Redistributions of source code must retain the above copyright
-#    notice, this list of conditions and the following disclaimer.
-# 2. Redistributions in binary form must reproduce the above copyright
-#    notice, this list of conditions and the following disclaimer in
-#    the documentation and/or other materials provided with the
-#    distribution.
-# 3. Neither the name PX4 nor the names of its contributors may be
-#    used to endorse or promote products derived from this software
-#    without specific prior written permission.
-#
-# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
-# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
-# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
-# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
-# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
-# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
-# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
-# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
-# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
-# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
-# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
-# POSSIBILITY OF SUCH DAMAGE.
-#
-############################################################################
-
-#
-# POSIX platform dependent files
-#
-
-SRCS		 = 	\
-			px4_posix_impl.cpp \
-			px4_posix_tasks.cpp  \
-			lib_crc32.c \
-			drv_hrt.c \
-			px4_log.c
-ifeq ($(CONFIG_SHMEM), 1)
-SRCS		+=	shmem_posix.c
-endif
-
-MAXOPTIMIZATION	 = -Os
diff --git a/src/platforms/posix/tests/hello/module.mk b/src/platforms/posix/tests/hello/module.mk
deleted file mode 100644
index 294c0ad7fc..0000000000
--- a/src/platforms/posix/tests/hello/module.mk
+++ /dev/null
@@ -1,43 +0,0 @@
-############################################################################
-#
-#   Copyright (c) 2014 PX4 Development Team. All rights reserved.
-#
-# Redistribution and use in source and binary forms, with or without
-# modification, are permitted provided that the following conditions
-# are met:
-#
-# 1. Redistributions of source code must retain the above copyright
-#    notice, this list of conditions and the following disclaimer.
-# 2. Redistributions in binary form must reproduce the above copyright
-#    notice, this list of conditions and the following disclaimer in
-#    the documentation and/or other materials provided with the
-#    distribution.
-# 3. Neither the name PX4 nor the names of its contributors may be
-#    used to endorse or promote products derived from this software
-#    without specific prior written permission.
-#
-# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
-# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
-# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
-# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
-# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
-# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
-# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
-# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
-# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
-# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
-# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
-# POSSIBILITY OF SUCH DAMAGE.
-#
-############################################################################
-
-#
-# Publisher Example Application
-#
-
-MODULE_COMMAND	= hello
-
-SRCS		= hello_main.cpp \
-		  hello_start_posix.cpp \
-		  hello_example.cpp
-
diff --git a/src/platforms/posix/tests/hrt_test/module.mk b/src/platforms/posix/tests/hrt_test/module.mk
deleted file mode 100644
index e1bd89ef7f..0000000000
--- a/src/platforms/posix/tests/hrt_test/module.mk
+++ /dev/null
@@ -1,43 +0,0 @@
-############################################################################
-#
-#   Copyright (c) 2014 PX4 Development Team. All rights reserved.
-#
-# Redistribution and use in source and binary forms, with or without
-# modification, are permitted provided that the following conditions
-# are met:
-#
-# 1. Redistributions of source code must retain the above copyright
-#    notice, this list of conditions and the following disclaimer.
-# 2. Redistributions in binary form must reproduce the above copyright
-#    notice, this list of conditions and the following disclaimer in
-#    the documentation and/or other materials provided with the
-#    distribution.
-# 3. Neither the name PX4 nor the names of its contributors may be
-#    used to endorse or promote products derived from this software
-#    without specific prior written permission.
-#
-# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
-# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
-# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
-# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
-# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
-# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
-# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
-# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
-# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
-# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
-# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
-# POSSIBILITY OF SUCH DAMAGE.
-#
-############################################################################
-
-#
-# Publisher Example Application
-#
-
-MODULE_COMMAND	= hrttest
-
-SRCS		= hrt_test_main.cpp \
-		  hrt_test_start_posix.cpp \
-		  hrt_test.cpp
-
diff --git a/src/platforms/posix/tests/muorb/module.mk b/src/platforms/posix/tests/muorb/module.mk
deleted file mode 100644
index 880fe82007..0000000000
--- a/src/platforms/posix/tests/muorb/module.mk
+++ /dev/null
@@ -1,47 +0,0 @@
-############################################################################
-#
-#   Copyright (c) 2014 PX4 Development Team. All rights reserved.
-#
-# Redistribution and use in source and binary forms, with or without
-# modification, are permitted provided that the following conditions
-# are met:
-#
-# 1. Redistributions of source code must retain the above copyright
-#    notice, this list of conditions and the following disclaimer.
-# 2. Redistributions in binary form must reproduce the above copyright
-#    notice, this list of conditions and the following disclaimer in
-#    the documentation and/or other materials provided with the
-#    distribution.
-# 3. Neither the name PX4 nor the names of its contributors may be
-#    used to endorse or promote products derived from this software
-#    without specific prior written permission.
-#
-# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
-# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
-# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
-# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
-# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
-# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
-# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
-# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
-# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
-# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
-# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
-# POSSIBILITY OF SUCH DAMAGE.
-#
-############################################################################
-
-#
-# Publisher Example Application
-#
-
-MODULE_COMMAND	= muorb_test
-
-INCLUDE_DIRS           += \
-                          $(EXT_MUORB_LIB_ROOT)/krait/include \
-                          $(PX4_BASE)src/modules/muorb/krait 
-
-SRCS		= muorb_test_main.cpp \
-		  muorb_test_start_posix.cpp \
-		  muorb_test_example.cpp
-
diff --git a/src/platforms/posix/tests/vcdev_test/module.mk b/src/platforms/posix/tests/vcdev_test/module.mk
deleted file mode 100644
index 81920c860c..0000000000
--- a/src/platforms/posix/tests/vcdev_test/module.mk
+++ /dev/null
@@ -1,43 +0,0 @@
-############################################################################
-#
-#   Copyright (c) 2014 PX4 Development Team. All rights reserved.
-#
-# Redistribution and use in source and binary forms, with or without
-# modification, are permitted provided that the following conditions
-# are met:
-#
-# 1. Redistributions of source code must retain the above copyright
-#    notice, this list of conditions and the following disclaimer.
-# 2. Redistributions in binary form must reproduce the above copyright
-#    notice, this list of conditions and the following disclaimer in
-#    the documentation and/or other materials provided with the
-#    distribution.
-# 3. Neither the name PX4 nor the names of its contributors may be
-#    used to endorse or promote products derived from this software
-#    without specific prior written permission.
-#
-# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
-# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
-# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
-# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
-# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
-# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
-# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
-# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
-# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
-# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
-# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
-# POSSIBILITY OF SUCH DAMAGE.
-#
-############################################################################
-
-#
-# Publisher Example Application
-#
-
-MODULE_COMMAND	= vcdevtest
-
-SRCS		= vcdevtest_main.cpp \
-		  vcdevtest_start_posix.cpp \
-		  vcdevtest_example.cpp
-
diff --git a/src/platforms/posix/tests/wqueue/module.mk b/src/platforms/posix/tests/wqueue/module.mk
deleted file mode 100644
index 4c3b6550c6..0000000000
--- a/src/platforms/posix/tests/wqueue/module.mk
+++ /dev/null
@@ -1,43 +0,0 @@
-############################################################################
-#
-#   Copyright (c) 2014 PX4 Development Team. All rights reserved.
-#
-# Redistribution and use in source and binary forms, with or without
-# modification, are permitted provided that the following conditions
-# are met:
-#
-# 1. Redistributions of source code must retain the above copyright
-#    notice, this list of conditions and the following disclaimer.
-# 2. Redistributions in binary form must reproduce the above copyright
-#    notice, this list of conditions and the following disclaimer in
-#    the documentation and/or other materials provided with the
-#    distribution.
-# 3. Neither the name PX4 nor the names of its contributors may be
-#    used to endorse or promote products derived from this software
-#    without specific prior written permission.
-#
-# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
-# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
-# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
-# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
-# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
-# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
-# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
-# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
-# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
-# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
-# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
-# POSSIBILITY OF SUCH DAMAGE.
-#
-############################################################################
-
-#
-# Publisher Example Application
-#
-
-MODULE_COMMAND	= wqueue_test
-
-SRCS		= wqueue_main.cpp \
-		  wqueue_start_posix.cpp \
-		  wqueue_test.cpp
-
diff --git a/src/platforms/posix/work_queue/module.mk b/src/platforms/posix/work_queue/module.mk
deleted file mode 100644
index 2a493ab7b6..0000000000
--- a/src/platforms/posix/work_queue/module.mk
+++ /dev/null
@@ -1,54 +0,0 @@
-############################################################################
-#
-#   Copyright (c) 2014 PX4 Development Team. All rights reserved.
-#
-# Redistribution and use in source and binary forms, with or without
-# modification, are permitted provided that the following conditions
-# are met:
-#
-# 1. Redistributions of source code must retain the above copyright
-#    notice, this list of conditions and the following disclaimer.
-# 2. Redistributions in binary form must reproduce the above copyright
-#    notice, this list of conditions and the following disclaimer in
-#    the documentation and/or other materials provided with the
-#    distribution.
-# 3. Neither the name PX4 nor the names of its contributors may be
-#    used to endorse or promote products derived from this software
-#    without specific prior written permission.
-#
-# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
-# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
-# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
-# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
-# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
-# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
-# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
-# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
-# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
-# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
-# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
-# POSSIBILITY OF SUCH DAMAGE.
-#
-############################################################################
-
-#
-# POSIX compatible queue and work_queue implementation
-#
-
-SRCS		 = 	\
-			hrt_thread.c \
-			hrt_queue.c \
-			hrt_work_cancel.c \
-			work_thread.c \
-			work_lock.c \
-			work_queue.c \
-			work_cancel.c \
-			queue.c \
-			dq_addlast.c \
-			dq_remfirst.c \
-			sq_addlast.c \
-			sq_remfirst.c \
-			sq_addafter.c \
-			dq_rem.c 
-
-MAXOPTIMIZATION	 = -Os
diff --git a/src/platforms/px4_app.h b/src/platforms/px4_app.h
index df3a8d8294..c2310899ac 100644
--- a/src/platforms/px4_app.h
+++ b/src/platforms/px4_app.h
@@ -72,11 +72,6 @@ private:
 };
 }
 
-// PX4_MAIN is defined if module.mk sets MODULE_COMMAND
-// For ROS and NuttX it is "main" and for Linux it is
-// $(MODULE_COMMAND)_app_main since some apps already
-// define $(MODULE_COMMAND)_main
-
 // Task/process based build
 #if defined(__PX4_ROS) || defined(__PX4_NUTTX)
 
diff --git a/src/platforms/qurt/px4_layer/module.mk b/src/platforms/qurt/px4_layer/module.mk
deleted file mode 100644
index 23d10df6b2..0000000000
--- a/src/platforms/qurt/px4_layer/module.mk
+++ /dev/null
@@ -1,51 +0,0 @@
-############################################################################
-#
-#   Copyright (c) 2014 PX4 Development Team. All rights reserved.
-#
-# Redistribution and use in source and binary forms, with or without
-# modification, are permitted provided that the following conditions
-# are met:
-#
-# 1. Redistributions of source code must retain the above copyright
-#    notice, this list of conditions and the following disclaimer.
-# 2. Redistributions in binary form must reproduce the above copyright
-#    notice, this list of conditions and the following disclaimer in
-#    the documentation and/or other materials provided with the
-#    distribution.
-# 3. Neither the name PX4 nor the names of its contributors may be
-#    used to endorse or promote products derived from this software
-#    without specific prior written permission.
-#
-# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
-# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
-# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
-# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
-# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
-# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
-# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
-# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
-# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
-# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
-# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
-# POSSIBILITY OF SUCH DAMAGE.
-#
-############################################################################
-
-#
-# NuttX / uORB adapter library
-#
-
-MODULE_NAME = dspal
-
-SRCDIR=$(dir $(MODULE_MK))
-
-SRCS		 = 	\
-			px4_qurt_impl.cpp \
-			px4_qurt_tasks.cpp  \
-			lib_crc32.c \
-			drv_hrt.c \
-            		qurt_stubs.c \
-            		main.cpp \
-			shmem_qurt.c
-
-MAXOPTIMIZATION	 = -Os
diff --git a/src/platforms/qurt/tests/hello/module.mk b/src/platforms/qurt/tests/hello/module.mk
deleted file mode 100644
index 6bed2aea67..0000000000
--- a/src/platforms/qurt/tests/hello/module.mk
+++ /dev/null
@@ -1,43 +0,0 @@
-############################################################################
-#
-#   Copyright (c) 2014 PX4 Development Team. All rights reserved.
-#
-# Redistribution and use in source and binary forms, with or without
-# modification, are permitted provided that the following conditions
-# are met:
-#
-# 1. Redistributions of source code must retain the above copyright
-#    notice, this list of conditions and the following disclaimer.
-# 2. Redistributions in binary form must reproduce the above copyright
-#    notice, this list of conditions and the following disclaimer in
-#    the documentation and/or other materials provided with the
-#    distribution.
-# 3. Neither the name PX4 nor the names of its contributors may be
-#    used to endorse or promote products derived from this software
-#    without specific prior written permission.
-#
-# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
-# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
-# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
-# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
-# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
-# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
-# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
-# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
-# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
-# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
-# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
-# POSSIBILITY OF SUCH DAMAGE.
-#
-############################################################################
-
-#
-# Publisher Example Application
-#
-
-MODULE_COMMAND	= hello
-
-SRCS		= hello_main.cpp \
-		  hello_start_qurt.cpp \
-		  hello_example.cpp
-
diff --git a/src/platforms/qurt/tests/muorb/module.mk b/src/platforms/qurt/tests/muorb/module.mk
deleted file mode 100644
index e21ee3c60b..0000000000
--- a/src/platforms/qurt/tests/muorb/module.mk
+++ /dev/null
@@ -1,48 +0,0 @@
-############################################################################
-#
-#   Copyright (c) 2014 PX4 Development Team. All rights reserved.
-#
-# Redistribution and use in source and binary forms, with or without
-# modification, are permitted provided that the following conditions
-# are met:
-#
-# 1. Redistributions of source code must retain the above copyright
-#    notice, this list of conditions and the following disclaimer.
-# 2. Redistributions in binary form must reproduce the above copyright
-#    notice, this list of conditions and the following disclaimer in
-#    the documentation and/or other materials provided with the
-#    distribution.
-# 3. Neither the name PX4 nor the names of its contributors may be
-#    used to endorse or promote products derived from this software
-#    without specific prior written permission.
-#
-# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
-# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
-# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
-# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
-# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
-# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
-# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
-# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
-# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
-# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
-# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
-# POSSIBILITY OF SUCH DAMAGE.
-#
-############################################################################
-
-#
-# Publisher Example Application
-#
-
-MODULE_COMMAND	= muorb_test
-
-SRCS		= \
-		  muorb_test_start_qurt.cpp \
-		  muorb_test_example.cpp
-
-INCLUDE_DIRS    += $(PX4_BASE)/src/modules/uORB  \
-                  $(PX4_BASE)/src/platforms \
-                  $(PX4_BASE)/src/modules
-
-
diff --git a/src/systemcmds/bl_update/module.mk b/src/systemcmds/bl_update/module.mk
deleted file mode 100644
index e8eea045e5..0000000000
--- a/src/systemcmds/bl_update/module.mk
+++ /dev/null
@@ -1,43 +0,0 @@
-############################################################################
-#
-#   Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved.
-#
-# Redistribution and use in source and binary forms, with or without
-# modification, are permitted provided that the following conditions
-# are met:
-#
-# 1. Redistributions of source code must retain the above copyright
-#    notice, this list of conditions and the following disclaimer.
-# 2. Redistributions in binary form must reproduce the above copyright
-#    notice, this list of conditions and the following disclaimer in
-#    the documentation and/or other materials provided with the
-#    distribution.
-# 3. Neither the name PX4 nor the names of its contributors may be
-#    used to endorse or promote products derived from this software
-#    without specific prior written permission.
-#
-# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
-# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
-# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
-# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
-# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
-# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
-# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
-# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
-# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
-# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
-# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
-# POSSIBILITY OF SUCH DAMAGE.
-#
-############################################################################
-
-#
-# Bootloader updater (flashes the FMU USB bootloader software)
-#
-
-MODULE_COMMAND	 = bl_update
-SRCS		 = bl_update.c
-
-MODULE_STACKSIZE = 4096
-
-MAXOPTIMIZATION	 = -Os
diff --git a/src/systemcmds/config/module.mk b/src/systemcmds/config/module.mk
deleted file mode 100644
index 0a75810b0c..0000000000
--- a/src/systemcmds/config/module.mk
+++ /dev/null
@@ -1,44 +0,0 @@
-############################################################################
-#
-#   Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved.
-#
-# Redistribution and use in source and binary forms, with or without
-# modification, are permitted provided that the following conditions
-# are met:
-#
-# 1. Redistributions of source code must retain the above copyright
-#    notice, this list of conditions and the following disclaimer.
-# 2. Redistributions in binary form must reproduce the above copyright
-#    notice, this list of conditions and the following disclaimer in
-#    the documentation and/or other materials provided with the
-#    distribution.
-# 3. Neither the name PX4 nor the names of its contributors may be
-#    used to endorse or promote products derived from this software
-#    without specific prior written permission.
-#
-# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
-# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
-# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
-# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
-# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
-# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
-# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
-# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
-# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
-# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
-# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
-# POSSIBILITY OF SUCH DAMAGE.
-#
-############################################################################
-
-#
-# Build the config tool.
-#
-
-MODULE_COMMAND	 = config
-SRCS		 = config.c
-
-MODULE_STACKSIZE = 4096
-
-MAXOPTIMIZATION	 = -Os
-
diff --git a/src/systemcmds/dumpfile/module.mk b/src/systemcmds/dumpfile/module.mk
deleted file mode 100644
index 36461f4772..0000000000
--- a/src/systemcmds/dumpfile/module.mk
+++ /dev/null
@@ -1,41 +0,0 @@
-############################################################################
-#
-#   Copyright (c) 2014 PX4 Development Team. All rights reserved.
-#
-# Redistribution and use in source and binary forms, with or without
-# modification, are permitted provided that the following conditions
-# are met:
-#
-# 1. Redistributions of source code must retain the above copyright
-#    notice, this list of conditions and the following disclaimer.
-# 2. Redistributions in binary form must reproduce the above copyright
-#    notice, this list of conditions and the following disclaimer in
-#    the documentation and/or other materials provided with the
-#    distribution.
-# 3. Neither the name PX4 nor the names of its contributors may be
-#    used to endorse or promote products derived from this software
-#    without specific prior written permission.
-#
-# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
-# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
-# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
-# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
-# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
-# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
-# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
-# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
-# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
-# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
-# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
-# POSSIBILITY OF SUCH DAMAGE.
-#
-############################################################################
-
-#
-# Dump file utility
-#
-
-MODULE_COMMAND	 = dumpfile
-SRCS		 = dumpfile.c
-
-MAXOPTIMIZATION	 = -Os
diff --git a/src/systemcmds/esc_calib/module.mk b/src/systemcmds/esc_calib/module.mk
deleted file mode 100644
index ce87eb3e2c..0000000000
--- a/src/systemcmds/esc_calib/module.mk
+++ /dev/null
@@ -1,43 +0,0 @@
-############################################################################
-#
-#   Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved.
-#
-# Redistribution and use in source and binary forms, with or without
-# modification, are permitted provided that the following conditions
-# are met:
-#
-# 1. Redistributions of source code must retain the above copyright
-#    notice, this list of conditions and the following disclaimer.
-# 2. Redistributions in binary form must reproduce the above copyright
-#    notice, this list of conditions and the following disclaimer in
-#    the documentation and/or other materials provided with the
-#    distribution.
-# 3. Neither the name PX4 nor the names of its contributors may be
-#    used to endorse or promote products derived from this software
-#    without specific prior written permission.
-#
-# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
-# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
-# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
-# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
-# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
-# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
-# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
-# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
-# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
-# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
-# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
-# POSSIBILITY OF SUCH DAMAGE.
-#
-############################################################################
-
-#
-# Build the esc_calib tool.
-#
-
-MODULE_COMMAND	 = esc_calib
-SRCS		 = esc_calib.c
-
-MODULE_STACKSIZE = 4096
-
-MAXOPTIMIZATION	 = -Os
diff --git a/src/systemcmds/i2c/module.mk b/src/systemcmds/i2c/module.mk
deleted file mode 100644
index ab2357c7d0..0000000000
--- a/src/systemcmds/i2c/module.mk
+++ /dev/null
@@ -1,41 +0,0 @@
-############################################################################
-#
-#   Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved.
-#
-# Redistribution and use in source and binary forms, with or without
-# modification, are permitted provided that the following conditions
-# are met:
-#
-# 1. Redistributions of source code must retain the above copyright
-#    notice, this list of conditions and the following disclaimer.
-# 2. Redistributions in binary form must reproduce the above copyright
-#    notice, this list of conditions and the following disclaimer in
-#    the documentation and/or other materials provided with the
-#    distribution.
-# 3. Neither the name PX4 nor the names of its contributors may be
-#    used to endorse or promote products derived from this software
-#    without specific prior written permission.
-#
-# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
-# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
-# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
-# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
-# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
-# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
-# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
-# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
-# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
-# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
-# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
-# POSSIBILITY OF SUCH DAMAGE.
-#
-############################################################################
-
-#
-# Build the i2c test tool.
-#
-
-MODULE_COMMAND	 = i2c
-SRCS		 = i2c.c
-
-MAXOPTIMIZATION	 = -Os
diff --git a/src/systemcmds/mixer/module.mk b/src/systemcmds/mixer/module.mk
deleted file mode 100644
index 5d5cf84857..0000000000
--- a/src/systemcmds/mixer/module.mk
+++ /dev/null
@@ -1,48 +0,0 @@
-############################################################################
-#
-#   Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved.
-#
-# Redistribution and use in source and binary forms, with or without
-# modification, are permitted provided that the following conditions
-# are met:
-#
-# 1. Redistributions of source code must retain the above copyright
-#    notice, this list of conditions and the following disclaimer.
-# 2. Redistributions in binary form must reproduce the above copyright
-#    notice, this list of conditions and the following disclaimer in
-#    the documentation and/or other materials provided with the
-#    distribution.
-# 3. Neither the name PX4 nor the names of its contributors may be
-#    used to endorse or promote products derived from this software
-#    without specific prior written permission.
-#
-# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
-# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
-# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
-# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
-# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
-# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
-# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
-# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
-# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
-# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
-# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
-# POSSIBILITY OF SUCH DAMAGE.
-#
-############################################################################
-
-#
-# Build the mixer tool.
-#
-
-MODULE_COMMAND	 = mixer
-SRCS		 = mixer.cpp
-
-MODULE_STACKSIZE = 4096
-
-MAXOPTIMIZATION	 = -Os
-
-ifdef ($(PX4_TARGET_OS),nuttx)
-EXTRACXXFLAGS = -Wframe-larger-than=2048
-endif
-
diff --git a/src/systemcmds/motor_test/module.mk b/src/systemcmds/motor_test/module.mk
deleted file mode 100644
index 261428ef94..0000000000
--- a/src/systemcmds/motor_test/module.mk
+++ /dev/null
@@ -1,43 +0,0 @@
-############################################################################
-#
-#   Copyright (c) 2014 PX4 Development Team. All rights reserved.
-#
-# Redistribution and use in source and binary forms, with or without
-# modification, are permitted provided that the following conditions
-# are met:
-#
-# 1. Redistributions of source code must retain the above copyright
-#    notice, this list of conditions and the following disclaimer.
-# 2. Redistributions in binary form must reproduce the above copyright
-#    notice, this list of conditions and the following disclaimer in
-#    the documentation and/or other materials provided with the
-#    distribution.
-# 3. Neither the name PX4 nor the names of its contributors may be
-#    used to endorse or promote products derived from this software
-#    without specific prior written permission.
-#
-# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
-# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
-# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
-# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
-# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
-# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
-# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
-# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
-# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
-# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
-# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
-# POSSIBILITY OF SUCH DAMAGE.
-#
-############################################################################
-
-#
-# Build the motor_test tool.
-#
-
-MODULE_COMMAND	 = motor_test
-SRCS		 = motor_test.c
-
-MODULE_STACKSIZE = 4096
-
-MAXOPTIMIZATION	 = -Os
diff --git a/src/systemcmds/mtd/module.mk b/src/systemcmds/mtd/module.mk
deleted file mode 100644
index bca1cdcc18..0000000000
--- a/src/systemcmds/mtd/module.mk
+++ /dev/null
@@ -1,11 +0,0 @@
-#
-# RAMTRON file system driver
-#
-
-MODULE_COMMAND	 = mtd
-SRCS		 = mtd.c 24xxxx_mtd.c
-
-MAXOPTIMIZATION	 = -Os
-
-EXTRACFLAGS	= -Wno-error
-
diff --git a/src/systemcmds/nshterm/module.mk b/src/systemcmds/nshterm/module.mk
deleted file mode 100644
index 4e27105723..0000000000
--- a/src/systemcmds/nshterm/module.mk
+++ /dev/null
@@ -1,45 +0,0 @@
-############################################################################
-#
-#   Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved.
-#
-# Redistribution and use in source and binary forms, with or without
-# modification, are permitted provided that the following conditions
-# are met:
-#
-# 1. Redistributions of source code must retain the above copyright
-#    notice, this list of conditions and the following disclaimer.
-# 2. Redistributions in binary form must reproduce the above copyright
-#    notice, this list of conditions and the following disclaimer in
-#    the documentation and/or other materials provided with the
-#    distribution.
-# 3. Neither the name PX4 nor the names of its contributors may be
-#    used to endorse or promote products derived from this software
-#    without specific prior written permission.
-#
-# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
-# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
-# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
-# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
-# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
-# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
-# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
-# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
-# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
-# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
-# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
-# POSSIBILITY OF SUCH DAMAGE.
-#
-############################################################################
-
-#
-# Build nshterm utility
-#
-
-MODULE_COMMAND	 = nshterm
-SRCS		 = nshterm.c
-
-MODULE_STACKSIZE = 1500
-
-MAXOPTIMIZATION	 = -Os
-
-MODULE_PRIORITY	= "SCHED_PRIORITY_DEFAULT-30"
diff --git a/src/systemcmds/perf/module.mk b/src/systemcmds/perf/module.mk
deleted file mode 100644
index ec39a7a85e..0000000000
--- a/src/systemcmds/perf/module.mk
+++ /dev/null
@@ -1,43 +0,0 @@
-############################################################################
-#
-#   Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved.
-#
-# Redistribution and use in source and binary forms, with or without
-# modification, are permitted provided that the following conditions
-# are met:
-#
-# 1. Redistributions of source code must retain the above copyright
-#    notice, this list of conditions and the following disclaimer.
-# 2. Redistributions in binary form must reproduce the above copyright
-#    notice, this list of conditions and the following disclaimer in
-#    the documentation and/or other materials provided with the
-#    distribution.
-# 3. Neither the name PX4 nor the names of its contributors may be
-#    used to endorse or promote products derived from this software
-#    without specific prior written permission.
-#
-# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
-# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
-# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
-# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
-# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
-# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
-# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
-# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
-# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
-# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
-# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
-# POSSIBILITY OF SUCH DAMAGE.
-#
-############################################################################
-
-#
-# perf_counter reporting tool
-#
-
-MODULE_COMMAND	 = perf
-SRCS		 = perf.c
-
-MAXOPTIMIZATION	 = -Os
-
-MODULE_STACKSIZE = 1800
diff --git a/src/systemcmds/pwm/module.mk b/src/systemcmds/pwm/module.mk
deleted file mode 100644
index a51ac8e0cf..0000000000
--- a/src/systemcmds/pwm/module.mk
+++ /dev/null
@@ -1,43 +0,0 @@
-############################################################################
-#
-#   Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved.
-#
-# Redistribution and use in source and binary forms, with or without
-# modification, are permitted provided that the following conditions
-# are met:
-#
-# 1. Redistributions of source code must retain the above copyright
-#    notice, this list of conditions and the following disclaimer.
-# 2. Redistributions in binary form must reproduce the above copyright
-#    notice, this list of conditions and the following disclaimer in
-#    the documentation and/or other materials provided with the
-#    distribution.
-# 3. Neither the name PX4 nor the names of its contributors may be
-#    used to endorse or promote products derived from this software
-#    without specific prior written permission.
-#
-# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
-# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
-# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
-# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
-# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
-# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
-# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
-# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
-# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
-# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
-# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
-# POSSIBILITY OF SUCH DAMAGE.
-#
-############################################################################
-
-#
-# Build the pwm tool.
-#
-
-MODULE_COMMAND	 = pwm
-SRCS		 = pwm.c
-
-MODULE_STACKSIZE = 1800
-
-MAXOPTIMIZATION	 = -Os
diff --git a/src/systemcmds/reboot/module.mk b/src/systemcmds/reboot/module.mk
deleted file mode 100644
index edf9d8b376..0000000000
--- a/src/systemcmds/reboot/module.mk
+++ /dev/null
@@ -1,43 +0,0 @@
-############################################################################
-#
-#   Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved.
-#
-# Redistribution and use in source and binary forms, with or without
-# modification, are permitted provided that the following conditions
-# are met:
-#
-# 1. Redistributions of source code must retain the above copyright
-#    notice, this list of conditions and the following disclaimer.
-# 2. Redistributions in binary form must reproduce the above copyright
-#    notice, this list of conditions and the following disclaimer in
-#    the documentation and/or other materials provided with the
-#    distribution.
-# 3. Neither the name PX4 nor the names of its contributors may be
-#    used to endorse or promote products derived from this software
-#    without specific prior written permission.
-#
-# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
-# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
-# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
-# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
-# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
-# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
-# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
-# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
-# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
-# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
-# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
-# POSSIBILITY OF SUCH DAMAGE.
-#
-############################################################################
-
-#
-# reboot command.
-#
-
-MODULE_COMMAND	 = reboot
-SRCS		 = reboot.c
-
-MAXOPTIMIZATION	 = -Os
-
-MODULE_STACKSIZE = 800
diff --git a/src/systemcmds/reflect/module.mk b/src/systemcmds/reflect/module.mk
deleted file mode 100644
index 973eb775df..0000000000
--- a/src/systemcmds/reflect/module.mk
+++ /dev/null
@@ -1,41 +0,0 @@
-############################################################################
-#
-#   Copyright (c) 2014 PX4 Development Team. All rights reserved.
-#
-# Redistribution and use in source and binary forms, with or without
-# modification, are permitted provided that the following conditions
-# are met:
-#
-# 1. Redistributions of source code must retain the above copyright
-#    notice, this list of conditions and the following disclaimer.
-# 2. Redistributions in binary form must reproduce the above copyright
-#    notice, this list of conditions and the following disclaimer in
-#    the documentation and/or other materials provided with the
-#    distribution.
-# 3. Neither the name PX4 nor the names of its contributors may be
-#    used to endorse or promote products derived from this software
-#    without specific prior written permission.
-#
-# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
-# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
-# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
-# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
-# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
-# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
-# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
-# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
-# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
-# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
-# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
-# POSSIBILITY OF SUCH DAMAGE.
-#
-############################################################################
-
-#
-# Dump file utility
-#
-
-MODULE_COMMAND	 = reflect
-SRCS		 = reflect.c
-
-MAXOPTIMIZATION	 = -Os
diff --git a/src/systemcmds/top/module.mk b/src/systemcmds/top/module.mk
deleted file mode 100644
index 548a09f856..0000000000
--- a/src/systemcmds/top/module.mk
+++ /dev/null
@@ -1,44 +0,0 @@
-############################################################################
-#
-#   Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved.
-#
-# Redistribution and use in source and binary forms, with or without
-# modification, are permitted provided that the following conditions
-# are met:
-#
-# 1. Redistributions of source code must retain the above copyright
-#    notice, this list of conditions and the following disclaimer.
-# 2. Redistributions in binary form must reproduce the above copyright
-#    notice, this list of conditions and the following disclaimer in
-#    the documentation and/or other materials provided with the
-#    distribution.
-# 3. Neither the name PX4 nor the names of its contributors may be
-#    used to endorse or promote products derived from this software
-#    without specific prior written permission.
-#
-# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
-# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
-# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
-# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
-# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
-# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
-# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
-# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
-# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
-# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
-# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
-# POSSIBILITY OF SUCH DAMAGE.
-#
-############################################################################
-
-#
-# Build top memory / cpu status tool.
-#
-
-MODULE_COMMAND	 = top
-SRCS		 = top.c
-
-MODULE_STACKSIZE = 1700
-
-MAXOPTIMIZATION	 = -Os
-
diff --git a/src/systemcmds/topic_listener/module.mk b/src/systemcmds/topic_listener/module.mk
deleted file mode 100644
index 4048c6b318..0000000000
--- a/src/systemcmds/topic_listener/module.mk
+++ /dev/null
@@ -1,46 +0,0 @@
-############################################################################
-#
-#   Copyright (c) 2015 PX4 Development Team. All rights reserved.
-#
-# Redistribution and use in source and binary forms, with or without
-# modification, are permitted provided that the following conditions
-# are met:
-#
-# 1. Redistributions of source code must retain the above copyright
-#    notice, this list of conditions and the following disclaimer.
-# 2. Redistributions in binary form must reproduce the above copyright
-#    notice, this list of conditions and the following disclaimer in
-#    the documentation and/or other materials provided with the
-#    distribution.
-# 3. Neither the name PX4 nor the names of its contributors may be
-#    used to endorse or promote products derived from this software
-#    without specific prior written permission.
-#
-# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
-# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
-# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
-# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
-# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
-# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
-# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
-# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
-# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
-# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
-# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
-# POSSIBILITY OF SUCH DAMAGE.
-#
-############################################################################
-
-#
-# Build the topic listener tool.
-#
-
-./topic_listener.cpp : $(PX4_BASE)/Tools/generate_listener.py
-	$(PX4_BASE)/Tools/generate_listener.py $(PX4_BASE) > $@
-
-MODULE_COMMAND	 = listener
-SRCS		 = topic_listener.cpp
-
-MODULE_STACKSIZE = 1800
-
-MAXOPTIMIZATION	 = -Os
diff --git a/src/systemcmds/usb_connected/module.mk b/src/systemcmds/usb_connected/module.mk
deleted file mode 100644
index 22ee1300d2..0000000000
--- a/src/systemcmds/usb_connected/module.mk
+++ /dev/null
@@ -1,37 +0,0 @@
-############################################################################
-#
-#   Copyright (c) 2014 PX4 Development Team. All rights reserved.
-#
-# Redistribution and use in source and binary forms, with or without
-# modification, are permitted provided that the following conditions
-# are met:
-#
-# 1. Redistributions of source code must retain the above copyright
-#    notice, this list of conditions and the following disclaimer.
-# 2. Redistributions in binary form must reproduce the above copyright
-#    notice, this list of conditions and the following disclaimer in
-#    the documentation and/or other materials provided with the
-#    distribution.
-# 3. Neither the name PX4 nor the names of its contributors may be
-#    used to endorse or promote products derived from this software
-#    without specific prior written permission.
-#
-# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
-# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
-# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
-# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
-# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
-# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
-# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
-# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
-# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
-# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
-# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
-# POSSIBILITY OF SUCH DAMAGE.
-#
-############################################################################
-
-MODULE_COMMAND	 = usb_connected
-SRCS		 = usb_connected.c
-
-MAXOPTIMIZATION	 = -Os
diff --git a/src/systemcmds/ver/module.mk b/src/systemcmds/ver/module.mk
deleted file mode 100644
index 2eeb80b616..0000000000
--- a/src/systemcmds/ver/module.mk
+++ /dev/null
@@ -1,44 +0,0 @@
-############################################################################
-#
-#   Copyright (c) 2014 PX4 Development Team. All rights reserved.
-#
-# Redistribution and use in source and binary forms, with or without
-# modification, are permitted provided that the following conditions
-# are met:
-#
-# 1. Redistributions of source code must retain the above copyright
-#    notice, this list of conditions and the following disclaimer.
-# 2. Redistributions in binary form must reproduce the above copyright
-#    notice, this list of conditions and the following disclaimer in
-#    the documentation and/or other materials provided with the
-#    distribution.
-# 3. Neither the name PX4 nor the names of its contributors may be
-#    used to endorse or promote products derived from this software
-#    without specific prior written permission.
-#
-# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
-# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
-# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
-# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
-# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
-# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
-# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
-# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
-# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
-# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
-# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
-# POSSIBILITY OF SUCH DAMAGE.
-#
-############################################################################
-
-#
-# "version" nsh-command displays version infromation for hw,sw, gcc,build etc
-# can be also included and used in own code via "ver.h"
-#
-
-MODULE_COMMAND	 	= ver
-SRCS		 		= ver.c
-
-MODULE_STACKSIZE 	= 1024
-
-MAXOPTIMIZATION	 	= -Os

From 5c88353d0589f80443aad274c0ae3e220b985a87 Mon Sep 17 00:00:00 2001
From: Andreas Antener <antener_a@gmx.ch>
Date: Mon, 25 Apr 2016 17:04:33 +0200
Subject: [PATCH 0839/1230] removed GCS link from mavros

---
 launch/mavros_posix_sitl.launch | 1 +
 1 file changed, 1 insertion(+)

diff --git a/launch/mavros_posix_sitl.launch b/launch/mavros_posix_sitl.launch
index 32a21fcf3b..558f3bbadb 100644
--- a/launch/mavros_posix_sitl.launch
+++ b/launch/mavros_posix_sitl.launch
@@ -18,6 +18,7 @@
 
     <include file="$(find px4)/launch/mavros.launch">
         <arg name="ns" value="$(arg ns)"/>
+        <arg name="gcs_url" value=""/> <!-- GCS link is provided by SITL -->
         <arg name="fcu_url" value="$(arg fcu_url)"/>
     </include>
 </launch>

From 150eb779aeda4d1d4626250fbbc06d264a89a30b Mon Sep 17 00:00:00 2001
From: Andreas Antener <antener_a@gmx.ch>
Date: Mon, 25 Apr 2016 17:04:52 +0200
Subject: [PATCH 0840/1230] added draft script to run missions against SITL

---
 .../python_src/px4_it/mavros/mission_test.py  | 195 ++++++++++++++++++
 1 file changed, 195 insertions(+)
 create mode 100755 integrationtests/python_src/px4_it/mavros/mission_test.py

diff --git a/integrationtests/python_src/px4_it/mavros/mission_test.py b/integrationtests/python_src/px4_it/mavros/mission_test.py
new file mode 100755
index 0000000000..17124a65ca
--- /dev/null
+++ b/integrationtests/python_src/px4_it/mavros/mission_test.py
@@ -0,0 +1,195 @@
+#!/usr/bin/env python
+#***************************************************************************
+#
+#   Copyright (c) 2015 PX4 Development Team. All rights reserved.
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions
+# are met:
+#
+# 1. Redistributions of source code must retain the above copyright
+#    notice, this list of conditions and the following disclaimer.
+# 2. Redistributions in binary form must reproduce the above copyright
+#    notice, this list of conditions and the following disclaimer in
+#    the documentation and/or other materials provided with the
+#    distribution.
+# 3. Neither the name PX4 nor the names of its contributors may be
+#    used to endorse or promote products derived from this software
+#    without specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.
+#
+#***************************************************************************/
+
+#
+# @author Andreas Antener <andreas@uaventure.com>
+#
+PKG = 'px4'
+
+import unittest
+import rospy
+import math
+import rosbag
+import sys
+
+import mavros
+from pymavlink import mavutil
+from mavros import mavlink
+
+from geometry_msgs.msg import PoseStamped
+from mavros_msgs.srv import CommandLong, WaypointPush
+from mavros_msgs.msg import Mavlink, Waypoint
+from sensor_msgs.msg import NavSatFix
+from mavros.mission import QGroundControlWP
+#from px4_test_helper import PX4TestHelper
+
+class MavrosMissionTest(unittest.TestCase):
+    """
+    Run a mission
+    """
+
+    def setUp(self):
+        rospy.init_node('test_node', anonymous=True)
+        #self.helper = PX4TestHelper("mavros_offboard_posctl_test")
+        #self.helper.setUp()
+
+        rospy.Subscriber("mavros/local_position/pose", PoseStamped, self.position_callback)
+        rospy.Subscriber("mavros/global_position/global", NavSatFix, self.global_position_callback)
+        self.pub_mavlink = rospy.Publisher('mavlink/to', Mavlink, queue_size=1)
+        rospy.wait_for_service('mavros/cmd/command', 30)
+        self._srv_cmd_long = rospy.ServiceProxy('mavros/cmd/command', CommandLong, persistent=True)
+        self._srv_wp_push = rospy.ServiceProxy('mavros/mission/push', WaypointPush, persistent=True)
+        self.rate = rospy.Rate(10) # 10hz
+        self.has_global_pos = False
+        self.local_position = PoseStamped()
+        self.global_position = NavSatFix()
+        self.home_alt = 0
+
+        # need to simulate heartbeat for datalink loss detection
+        rospy.Timer(rospy.Duration(0.5), self.send_heartbeat)
+
+    def tearDown(self):
+        #self.helper.tearDown()
+        pass
+
+    #
+    # General callback functions used in tests
+    #
+    def position_callback(self, data):
+        self.local_position = data
+
+    def global_position_callback(self, data):
+        self.global_position = data
+
+        if not self.has_global_pos:
+            self.home_alt = data.altitude
+            self.has_global_pos = True
+
+    #
+    # Helper methods
+    #
+    def is_at_position(self, lat, lon, alt, offset):
+        R = 6371000 # metres
+        rlat1 = math.radians(lat)
+        rlat2 = math.radians(self.global_position.latitude)
+
+        rlat_d = math.radians(self.global_position.latitude - lat)
+        rlon_d = math.radians(self.global_position.longitude - lon)
+
+        a = (math.sin(rlat_d / 2) * math.sin(rlat_d / 2) +
+             math.cos(rlat1) * math.cos(rlat2) *
+             math.sin(rlon_d / 2) * math.sin(rlon_d / 2))
+        c = 2 * math.atan2(math.sqrt(a), math.sqrt(1-a))
+
+        d = R * c
+        alt_d = abs(alt - self.global_position.altitude)
+
+        rospy.loginfo("d: %f, alt_d: %f", d, alt_d)
+        return d < offset and alt_d < offset
+
+    def reach_position(self, lat, lon, alt, radius, timeout):
+        # does it reach the position in X seconds?
+        count = 0
+        while count < timeout:
+            if self.is_at_position(lat, lon, alt, radius):
+                break
+            count = count + 1
+            self.rate.sleep()
+
+        self.assertTrue(count < timeout, ("took too long to get to position %f, %f, %f, %f, %d" %
+            (lat, lon, alt, radius, timeout)))
+
+    def run_mission(self):
+        """switch mode: armed | auto"""
+        self._srv_cmd_long(False, 176, False,
+                           # arm | custom, auto, mission
+                           128 | 1, 4, 4, 0, 0, 0, 0)
+
+    def wait_until_ready(self):
+        """FIXME: hack to wait for simulation to be ready"""
+        while not self.has_global_pos:
+            self.rate.sleep()
+
+    def send_heartbeat(self, event=None):
+        # mav type gcs
+        mavmsg = mavutil.mavlink.MAVLink_heartbeat_message(6, 0, 0, 0, 0, 0)
+        # XXX: hack: using header object to set mav properties
+        mavmsg.pack(mavutil.mavlink.MAVLink_header(0, 0, 0, 2, 1))
+        rosmsg = mavlink.convert_to_rosmsg(mavmsg)
+        self.pub_mavlink.publish(rosmsg)
+
+    def test_mission(self):
+        """Test mission"""
+
+        if len(sys.argv) < 2:
+            self.fail("no mission argument")
+            return
+
+        rospy.loginfo("reading mission")
+        mission = QGroundControlWP()
+        wps = []
+        for wp in mission.read(open(sys.argv[1], 'r')):
+            wps.append(wp)
+            rospy.logdebug(wp)
+
+        rospy.loginfo("wait until ready")
+        self.wait_until_ready()
+
+        rospy.loginfo("send mission")
+        res = self._srv_wp_push(wps)
+        rospy.loginfo(res)
+        self.assertTrue(res.success, "mission could not be transfered")
+
+        rospy.loginfo("run mission")
+        self.run_mission()
+
+        positions = (
+            (0, 0, 0),
+            (2, 2, 2),
+            (2, -2, 2),
+            (-2, -2, 2),
+            (2, 2, 2))
+
+        for wp in wps:
+            # only check position waypoints
+            if wp.frame == Waypoint.FRAME_GLOBAL_REL_ALT or wp.frame == Waypoint.FRAME_GLOBAL:
+                alt = wp.z_alt
+                if wp.frame == Waypoint.FRAME_GLOBAL_REL_ALT:
+                    alt += self.home_alt
+                self.reach_position(wp.x_lat, wp.y_long, alt, 10, 300)
+
+
+if __name__ == '__main__':
+    import rostest
+    rostest.rosrun(PKG, 'mavros_mission_test', MavrosMissionTest)

From 00d56b9ef8138889feeb7f6ff51ff38d52bb0a88 Mon Sep 17 00:00:00 2001
From: Andreas Antener <antener_a@gmx.ch>
Date: Mon, 25 Apr 2016 21:04:22 +0200
Subject: [PATCH 0841/1230] added VTOL mission test, updated mission test to
 check mission depending on vehicle state

---
 .../python_src/px4_it/mavros/mission_test.py  | 29 ++++++++++++++-----
 .../python_src/px4_it/mavros/vtol_mis_1.txt   |  9 ++++++
 .../mavros_posix_tests_standard_vtol.launch   | 18 ++++++++++++
 .../SITL/init/rcS_gazebo_standard_vtol        | 10 ++++++-
 4 files changed, 58 insertions(+), 8 deletions(-)
 create mode 100644 integrationtests/python_src/px4_it/mavros/vtol_mis_1.txt
 create mode 100644 launch/mavros_posix_tests_standard_vtol.launch

diff --git a/integrationtests/python_src/px4_it/mavros/mission_test.py b/integrationtests/python_src/px4_it/mavros/mission_test.py
index 17124a65ca..28b47163cd 100755
--- a/integrationtests/python_src/px4_it/mavros/mission_test.py
+++ b/integrationtests/python_src/px4_it/mavros/mission_test.py
@@ -42,6 +42,7 @@ import rospy
 import math
 import rosbag
 import sys
+import os
 
 import mavros
 from pymavlink import mavutil
@@ -49,7 +50,7 @@ from mavros import mavlink
 
 from geometry_msgs.msg import PoseStamped
 from mavros_msgs.srv import CommandLong, WaypointPush
-from mavros_msgs.msg import Mavlink, Waypoint
+from mavros_msgs.msg import Mavlink, Waypoint, ExtendedState
 from sensor_msgs.msg import NavSatFix
 from mavros.mission import QGroundControlWP
 #from px4_test_helper import PX4TestHelper
@@ -66,6 +67,7 @@ class MavrosMissionTest(unittest.TestCase):
 
         rospy.Subscriber("mavros/local_position/pose", PoseStamped, self.position_callback)
         rospy.Subscriber("mavros/global_position/global", NavSatFix, self.global_position_callback)
+        rospy.Subscriber("mavros/extended_state", ExtendedState, self.extended_state_callback)
         self.pub_mavlink = rospy.Publisher('mavlink/to', Mavlink, queue_size=1)
         rospy.wait_for_service('mavros/cmd/command', 30)
         self._srv_cmd_long = rospy.ServiceProxy('mavros/cmd/command', CommandLong, persistent=True)
@@ -74,7 +76,10 @@ class MavrosMissionTest(unittest.TestCase):
         self.has_global_pos = False
         self.local_position = PoseStamped()
         self.global_position = NavSatFix()
+        self.extended_state = ExtendedState()
         self.home_alt = 0
+        self.mc_rad = 5
+        self.fw_rad = 40
 
         # need to simulate heartbeat for datalink loss detection
         rospy.Timer(rospy.Duration(0.5), self.send_heartbeat)
@@ -96,6 +101,9 @@ class MavrosMissionTest(unittest.TestCase):
             self.home_alt = data.altitude
             self.has_global_pos = True
 
+    def extended_state_callback(self, data):
+        self.extended_state = data
+
     #
     # Helper methods
     #
@@ -115,13 +123,18 @@ class MavrosMissionTest(unittest.TestCase):
         d = R * c
         alt_d = abs(alt - self.global_position.altitude)
 
-        rospy.loginfo("d: %f, alt_d: %f", d, alt_d)
+        #rospy.loginfo("d: %f, alt_d: %f", d, alt_d)
         return d < offset and alt_d < offset
 
-    def reach_position(self, lat, lon, alt, radius, timeout):
+    def reach_position(self, lat, lon, alt, timeout):
         # does it reach the position in X seconds?
         count = 0
         while count < timeout:
+            # use different radius matching vehicle state
+            radius = self.mc_rad
+            if self.extended_state.vtol_state == ExtendedState.VTOL_STATE_FW:
+                radius = self.fw_rad
+
             if self.is_at_position(lat, lon, alt, radius):
                 break
             count = count + 1
@@ -153,13 +166,15 @@ class MavrosMissionTest(unittest.TestCase):
         """Test mission"""
 
         if len(sys.argv) < 2:
-            self.fail("no mission argument")
+            self.fail("usage: mission_test.py mission_file")
             return
 
-        rospy.loginfo("reading mission")
+        file = os.path.dirname(os.path.realpath(__file__)) + "/" + sys.argv[1]
+
+        rospy.loginfo("reading mission %s", file)
         mission = QGroundControlWP()
         wps = []
-        for wp in mission.read(open(sys.argv[1], 'r')):
+        for wp in mission.read(open(file, 'r')):
             wps.append(wp)
             rospy.logdebug(wp)
 
@@ -187,7 +202,7 @@ class MavrosMissionTest(unittest.TestCase):
                 alt = wp.z_alt
                 if wp.frame == Waypoint.FRAME_GLOBAL_REL_ALT:
                     alt += self.home_alt
-                self.reach_position(wp.x_lat, wp.y_long, alt, 10, 300)
+                self.reach_position(wp.x_lat, wp.y_long, alt, 600)
 
 
 if __name__ == '__main__':
diff --git a/integrationtests/python_src/px4_it/mavros/vtol_mis_1.txt b/integrationtests/python_src/px4_it/mavros/vtol_mis_1.txt
new file mode 100644
index 0000000000..6d61dbfc96
--- /dev/null
+++ b/integrationtests/python_src/px4_it/mavros/vtol_mis_1.txt
@@ -0,0 +1,9 @@
+QGC WPL 110
+0	1	0	16	0	0	0	0	47.3979949951	8.54559612274	12.0	1
+1	0	2	3000	4.0	0.0	0.0	0.0	47.3980331421	8.54578971863	12.0	1
+2	0	3	16	0.0	0.0	-0.0	0.0	47.399269104	8.54557228088	12.0	1
+3	0	3	16	0.0	0.0	-0.0	0.0	47.3992652893	8.54230213165	12.0	1
+4	0	3	16	0.0	0.0	-0.0	0.0	47.3974761963	8.54239082336	12.0	1
+5	0	3	16	0.0	0.0	-0.0	0.0	47.3976669312	8.54509449005	12.0	1
+6	0	2	3000	3.0	0.0	0.0	0.0	47.3977851868	8.54526233673	12.0	1
+7	0	3	21	25.0	0.0	-0.0	0.0	47.3979797363	8.54460906982	12.0	1
diff --git a/launch/mavros_posix_tests_standard_vtol.launch b/launch/mavros_posix_tests_standard_vtol.launch
new file mode 100644
index 0000000000..58b928995d
--- /dev/null
+++ b/launch/mavros_posix_tests_standard_vtol.launch
@@ -0,0 +1,18 @@
+<launch>
+    <!-- Posix SITL MAVROS integration tests -->
+
+    <arg name="ns" default="/"/>
+    <arg name="headless" default="true"/>
+    <arg name="gui" default="false"/>
+
+    <include file="$(find px4)/launch/mavros_posix_sitl.launch">
+        <arg name="ns" value="$(arg ns)"/>
+        <arg name="headless" value="$(arg headless)"/>
+        <arg name="gui" value="$(arg gui)"/>
+        <arg name="world" value="standard_vtol"/>
+    </include>
+
+    <group ns="$(arg ns)">
+        <test test-name="mission_test" pkg="px4" type="mission_test.py" time-limit="300.0" args="vtol_mis_1.txt" />
+    </group>
+</launch>
diff --git a/posix-configs/SITL/init/rcS_gazebo_standard_vtol b/posix-configs/SITL/init/rcS_gazebo_standard_vtol
index f43d6330b4..44294fa203 100644
--- a/posix-configs/SITL/init/rcS_gazebo_standard_vtol
+++ b/posix-configs/SITL/init/rcS_gazebo_standard_vtol
@@ -39,7 +39,15 @@ param set NAV_DLL_ACT 2
 param set NAV_ACC_RAD 3.0
 param set MPC_TKO_SPEED 1.0
 param set MIS_YAW_TMT 10
-simulator start -s
+param set RTL_RETURN_ALT 30.0
+param set RTL_DESCEND_ALT 10.0
+param set RTL_LAND_DELAY 0
+param set COM_DISARM_LAND 5
+param set COM_DL_LOSS_EN 1
+param set MPC_ACC_HOR_MAX 2
+param set GF_ACTION 3
+param set GF_MAX_HOR_DIST 500
+param set GF_MAX_VER_DIST 500
 rgbledsim start
 tone_alarm start
 gyrosim start

From 361abd7f044f5cd1e34e402ba0b97e62a8ea26b8 Mon Sep 17 00:00:00 2001
From: Andreas Antener <antener_a@gmx.ch>
Date: Tue, 10 May 2016 00:28:18 +0200
Subject: [PATCH 0842/1230] added VTOL test missions

---
 integrationtests/python_src/px4_it/mavros/vtol_mis_1.txt | 9 ---------
 integrationtests/python_src/px4_it/mavros/vtol_new_1.txt | 4 ++++
 integrationtests/python_src/px4_it/mavros/vtol_new_2.txt | 5 +++++
 integrationtests/python_src/px4_it/mavros/vtol_old_1.txt | 8 ++++++++
 integrationtests/python_src/px4_it/mavros/vtol_old_2.txt | 8 ++++++++
 integrationtests/python_src/px4_it/mavros/vtol_old_3.txt | 5 +++++
 6 files changed, 30 insertions(+), 9 deletions(-)
 delete mode 100644 integrationtests/python_src/px4_it/mavros/vtol_mis_1.txt
 create mode 100644 integrationtests/python_src/px4_it/mavros/vtol_new_1.txt
 create mode 100644 integrationtests/python_src/px4_it/mavros/vtol_new_2.txt
 create mode 100644 integrationtests/python_src/px4_it/mavros/vtol_old_1.txt
 create mode 100644 integrationtests/python_src/px4_it/mavros/vtol_old_2.txt
 create mode 100644 integrationtests/python_src/px4_it/mavros/vtol_old_3.txt

diff --git a/integrationtests/python_src/px4_it/mavros/vtol_mis_1.txt b/integrationtests/python_src/px4_it/mavros/vtol_mis_1.txt
deleted file mode 100644
index 6d61dbfc96..0000000000
--- a/integrationtests/python_src/px4_it/mavros/vtol_mis_1.txt
+++ /dev/null
@@ -1,9 +0,0 @@
-QGC WPL 110
-0	1	0	16	0	0	0	0	47.3979949951	8.54559612274	12.0	1
-1	0	2	3000	4.0	0.0	0.0	0.0	47.3980331421	8.54578971863	12.0	1
-2	0	3	16	0.0	0.0	-0.0	0.0	47.399269104	8.54557228088	12.0	1
-3	0	3	16	0.0	0.0	-0.0	0.0	47.3992652893	8.54230213165	12.0	1
-4	0	3	16	0.0	0.0	-0.0	0.0	47.3974761963	8.54239082336	12.0	1
-5	0	3	16	0.0	0.0	-0.0	0.0	47.3976669312	8.54509449005	12.0	1
-6	0	2	3000	3.0	0.0	0.0	0.0	47.3977851868	8.54526233673	12.0	1
-7	0	3	21	25.0	0.0	-0.0	0.0	47.3979797363	8.54460906982	12.0	1
diff --git a/integrationtests/python_src/px4_it/mavros/vtol_new_1.txt b/integrationtests/python_src/px4_it/mavros/vtol_new_1.txt
new file mode 100644
index 0000000000..527bd5f5ad
--- /dev/null
+++ b/integrationtests/python_src/px4_it/mavros/vtol_new_1.txt
@@ -0,0 +1,4 @@
+# New style transitions, takeoff not at home
+QGC WPL 110
+0	1	3	84	15.0	0	0	0	47.397731862593744	8.543269295853861	12.0	1
+1	0	3	85	0.0	0.0	-0.0	0.0	47.3990753922183	8.5432371088360526	0.0	1
diff --git a/integrationtests/python_src/px4_it/mavros/vtol_new_2.txt b/integrationtests/python_src/px4_it/mavros/vtol_new_2.txt
new file mode 100644
index 0000000000..7d6908a986
--- /dev/null
+++ b/integrationtests/python_src/px4_it/mavros/vtol_new_2.txt
@@ -0,0 +1,5 @@
+# New style transitions, takeoff at home
+QGC WPL 110
+0	1	3	84	15.0	0	0	0	47.397746387399621	8.5455920888607579	12.0	1
+1	0	3	16	0.0	0.0	0.0	0.0	47.399093548895856	8.5455438069836305	12.0	1
+2	0	3	85	0.0	0.0	-0.0	0.0	47.39903545	8.5432263800000001	0.0	1
diff --git a/integrationtests/python_src/px4_it/mavros/vtol_old_1.txt b/integrationtests/python_src/px4_it/mavros/vtol_old_1.txt
new file mode 100644
index 0000000000..123423751d
--- /dev/null
+++ b/integrationtests/python_src/px4_it/mavros/vtol_old_1.txt
@@ -0,0 +1,8 @@
+# Old style transitions, takeoff and landing WPs away from home and last WP
+QGC WPL 110
+0	1	3	22	15.0	0	0	0	47.398046406687619	8.5458366721115908	12.0	1
+1	0	2	3000	4	0.0	0.0	0.0	47.398033142089844	8.5457897186279297	12.0	1
+2	0	3	16	0	0.0	0.0	0.0	47.399269104003906	8.5455722808837891	12.0	1
+3	0	3	16	0	0.0	0.0	0.0	47.399281145681528	8.547910568913295	12.0	1
+4	0	2	3000	3	0.0	0.0	0.0	47.397785186767578	8.545262336730957	12.0	1
+5	0	3	21	25.0	0.0	0.0	0.0	47.398884865088675	8.547918116539563	12.0	1
diff --git a/integrationtests/python_src/px4_it/mavros/vtol_old_2.txt b/integrationtests/python_src/px4_it/mavros/vtol_old_2.txt
new file mode 100644
index 0000000000..d021eb9ddd
--- /dev/null
+++ b/integrationtests/python_src/px4_it/mavros/vtol_old_2.txt
@@ -0,0 +1,8 @@
+# Old style transitions, takeoff WP at home location, landing WP at last WP location
+QGC WPL 110
+0	1	3	22	15.0	0	0	0	47.397740722711738	8.5455944102696719	12.0	1
+1	0	2	3000	4	0.0	0.0	0.0	47.397732380000001	8.5458224099999995	12.0	1
+2	0	3	16	0	0.0	0.0	0.0	47.397749067235559	8.5429755110031351	12.0	1
+3	0	3	16	0	0.0	0.0	0.0	47.399213119631852	8.5430124879018479	12.0	1
+4	0	2	3000	3	0.0	0.0	0.0	47.399158900000003	8.5429077299999996	12.0	1
+5	0	3	21	25.0	0.0	0.0	0.0	47.399211883544922	8.5430123448444419	12.0	1
diff --git a/integrationtests/python_src/px4_it/mavros/vtol_old_3.txt b/integrationtests/python_src/px4_it/mavros/vtol_old_3.txt
new file mode 100644
index 0000000000..ab7814c9c9
--- /dev/null
+++ b/integrationtests/python_src/px4_it/mavros/vtol_old_3.txt
@@ -0,0 +1,5 @@
+# Old style transitions, takeoff with normal WP
+QGC WPL 110
+0	1	3	16	15.0	0	0	0	47.398157669127094	8.5460336317115377	12.0	1
+1	0	2	3000	4	0.0	0.0	0.0	47.398033142089844	8.5457897186279297	12.0	1
+2	0	3	16	0	0.0	0.0	0.0	47.399269104003906	8.5455722808837891	12.0	1

From 05dc643f176e74b685a5a2d1c364eef1acae6344 Mon Sep 17 00:00:00 2001
From: Andreas Antener <antener_a@gmx.ch>
Date: Sun, 15 May 2016 16:02:28 +0200
Subject: [PATCH 0843/1230] increased fixed wing radius for mission tests and
 added more informative output for position matching

---
 .../python_src/px4_it/mavros/mission_test.py  | 45 ++++++++++++++-----
 .../mavros_posix_tests_standard_vtol.launch   |  6 ++-
 2 files changed, 38 insertions(+), 13 deletions(-)

diff --git a/integrationtests/python_src/px4_it/mavros/mission_test.py b/integrationtests/python_src/px4_it/mavros/mission_test.py
index 28b47163cd..6ace70a4e6 100755
--- a/integrationtests/python_src/px4_it/mavros/mission_test.py
+++ b/integrationtests/python_src/px4_it/mavros/mission_test.py
@@ -65,13 +65,6 @@ class MavrosMissionTest(unittest.TestCase):
         #self.helper = PX4TestHelper("mavros_offboard_posctl_test")
         #self.helper.setUp()
 
-        rospy.Subscriber("mavros/local_position/pose", PoseStamped, self.position_callback)
-        rospy.Subscriber("mavros/global_position/global", NavSatFix, self.global_position_callback)
-        rospy.Subscriber("mavros/extended_state", ExtendedState, self.extended_state_callback)
-        self.pub_mavlink = rospy.Publisher('mavlink/to', Mavlink, queue_size=1)
-        rospy.wait_for_service('mavros/cmd/command', 30)
-        self._srv_cmd_long = rospy.ServiceProxy('mavros/cmd/command', CommandLong, persistent=True)
-        self._srv_wp_push = rospy.ServiceProxy('mavros/mission/push', WaypointPush, persistent=True)
         self.rate = rospy.Rate(10) # 10hz
         self.has_global_pos = False
         self.local_position = PoseStamped()
@@ -79,11 +72,22 @@ class MavrosMissionTest(unittest.TestCase):
         self.extended_state = ExtendedState()
         self.home_alt = 0
         self.mc_rad = 5
-        self.fw_rad = 40
+        self.fw_rad = 50
+        self.last_alt_d = 9999
+        self.last_pos_d = 9999
 
         # need to simulate heartbeat for datalink loss detection
         rospy.Timer(rospy.Duration(0.5), self.send_heartbeat)
 
+        rospy.wait_for_service('mavros/cmd/command', 30)
+        self.pub_mavlink = rospy.Publisher('mavlink/to', Mavlink, queue_size=1)
+        self._srv_cmd_long = rospy.ServiceProxy('mavros/cmd/command', CommandLong, persistent=True)
+        self._srv_wp_push = rospy.ServiceProxy('mavros/mission/push', WaypointPush, persistent=True)
+
+        rospy.Subscriber("mavros/local_position/pose", PoseStamped, self.position_callback)
+        rospy.Subscriber("mavros/global_position/global", NavSatFix, self.global_position_callback)
+        rospy.Subscriber("mavros/extended_state", ExtendedState, self.extended_state_callback)
+
     def tearDown(self):
         #self.helper.tearDown()
         pass
@@ -124,9 +128,20 @@ class MavrosMissionTest(unittest.TestCase):
         alt_d = abs(alt - self.global_position.altitude)
 
         #rospy.loginfo("d: %f, alt_d: %f", d, alt_d)
+
+        # remember best distances
+        if self.last_pos_d > d:
+            self.last_pos_d = d
+        if self.last_alt_d > alt_d:
+            self.last_alt_d = alt_d
+
         return d < offset and alt_d < offset
 
-    def reach_position(self, lat, lon, alt, timeout):
+    def reach_position(self, lat, lon, alt, timeout, index):
+        # reset best distances
+        self.last_alt_d = 9999
+        self.last_pos_d = 9999
+
         # does it reach the position in X seconds?
         count = 0
         while count < timeout:
@@ -136,12 +151,15 @@ class MavrosMissionTest(unittest.TestCase):
                 radius = self.fw_rad
 
             if self.is_at_position(lat, lon, alt, radius):
+                rospy.loginfo("position reached, index: %d, count: %d, pos_d: %f, alt_d: %f" %
+                    (index, count, self.last_pos_d, self.last_alt_d))
                 break
             count = count + 1
             self.rate.sleep()
 
-        self.assertTrue(count < timeout, ("took too long to get to position %f, %f, %f, %f, %d" %
-            (lat, lon, alt, radius, timeout)))
+        self.assertTrue(count < timeout, ("took too long to get to position " +
+            "lat: %f, lon: %f, alt: %f, radius: %f, timeout: %d, index: %d, pos_d: %f, alt_d: %f" %
+            (lat, lon, alt, radius, timeout, index, self.last_pos_d, self.last_alt_d)))
 
     def run_mission(self):
         """switch mode: armed | auto"""
@@ -196,13 +214,16 @@ class MavrosMissionTest(unittest.TestCase):
             (-2, -2, 2),
             (2, 2, 2))
 
+        index = 0
         for wp in wps:
             # only check position waypoints
             if wp.frame == Waypoint.FRAME_GLOBAL_REL_ALT or wp.frame == Waypoint.FRAME_GLOBAL:
                 alt = wp.z_alt
                 if wp.frame == Waypoint.FRAME_GLOBAL_REL_ALT:
                     alt += self.home_alt
-                self.reach_position(wp.x_lat, wp.y_long, alt, 600)
+                self.reach_position(wp.x_lat, wp.y_long, alt, 600, index)
+
+            index += 1
 
 
 if __name__ == '__main__':
diff --git a/launch/mavros_posix_tests_standard_vtol.launch b/launch/mavros_posix_tests_standard_vtol.launch
index 58b928995d..d5aa9e285e 100644
--- a/launch/mavros_posix_tests_standard_vtol.launch
+++ b/launch/mavros_posix_tests_standard_vtol.launch
@@ -13,6 +13,10 @@
     </include>
 
     <group ns="$(arg ns)">
-        <test test-name="mission_test" pkg="px4" type="mission_test.py" time-limit="300.0" args="vtol_mis_1.txt" />
+        <test test-name="mission_test_new_1" pkg="px4" type="mission_test.py" time-limit="300.0" args="vtol_new_1.txt" />
+        <test test-name="mission_test_new_2" pkg="px4" type="mission_test.py" time-limit="300.0" args="vtol_new_2.txt" />
+        <test test-name="mission_test_old_1" pkg="px4" type="mission_test.py" time-limit="300.0" args="vtol_old_1.txt" />
+        <test test-name="mission_test_old_2" pkg="px4" type="mission_test.py" time-limit="300.0" args="vtol_old_2.txt" />
+        <test test-name="mission_test_old_3" pkg="px4" type="mission_test.py" time-limit="300.0" args="vtol_old_3.txt" />
     </group>
 </launch>

From 57fa9d2070b92ebaffee51fcff59707dbe46c681 Mon Sep 17 00:00:00 2001
From: Andreas Antener <antener_a@gmx.ch>
Date: Sun, 15 May 2016 16:33:44 +0200
Subject: [PATCH 0844/1230] use separate altitude offset check in FW

---
 .../python_src/px4_it/mavros/mission_test.py    | 17 ++++++++++-------
 1 file changed, 10 insertions(+), 7 deletions(-)

diff --git a/integrationtests/python_src/px4_it/mavros/mission_test.py b/integrationtests/python_src/px4_it/mavros/mission_test.py
index 6ace70a4e6..7e2a094709 100755
--- a/integrationtests/python_src/px4_it/mavros/mission_test.py
+++ b/integrationtests/python_src/px4_it/mavros/mission_test.py
@@ -73,6 +73,7 @@ class MavrosMissionTest(unittest.TestCase):
         self.home_alt = 0
         self.mc_rad = 5
         self.fw_rad = 50
+        self.fw_alt_rad = 10
         self.last_alt_d = 9999
         self.last_pos_d = 9999
 
@@ -111,7 +112,7 @@ class MavrosMissionTest(unittest.TestCase):
     #
     # Helper methods
     #
-    def is_at_position(self, lat, lon, alt, offset):
+    def is_at_position(self, lat, lon, alt, xy_offset, z_offset):
         R = 6371000 # metres
         rlat1 = math.radians(lat)
         rlat2 = math.radians(self.global_position.latitude)
@@ -135,7 +136,7 @@ class MavrosMissionTest(unittest.TestCase):
         if self.last_alt_d > alt_d:
             self.last_alt_d = alt_d
 
-        return d < offset and alt_d < offset
+        return d < xy_offset and alt_d < z_offset
 
     def reach_position(self, lat, lon, alt, timeout, index):
         # reset best distances
@@ -146,11 +147,13 @@ class MavrosMissionTest(unittest.TestCase):
         count = 0
         while count < timeout:
             # use different radius matching vehicle state
-            radius = self.mc_rad
+            xy_radius = self.mc_rad
+            z_radius = self.mc_rad
             if self.extended_state.vtol_state == ExtendedState.VTOL_STATE_FW:
-                radius = self.fw_rad
+                xy_radius = self.fw_rad
+                z_radius = self.fw_alt_rad
 
-            if self.is_at_position(lat, lon, alt, radius):
+            if self.is_at_position(lat, lon, alt, xy_radius, z_radius):
                 rospy.loginfo("position reached, index: %d, count: %d, pos_d: %f, alt_d: %f" %
                     (index, count, self.last_pos_d, self.last_alt_d))
                 break
@@ -158,8 +161,8 @@ class MavrosMissionTest(unittest.TestCase):
             self.rate.sleep()
 
         self.assertTrue(count < timeout, ("took too long to get to position " +
-            "lat: %f, lon: %f, alt: %f, radius: %f, timeout: %d, index: %d, pos_d: %f, alt_d: %f" %
-            (lat, lon, alt, radius, timeout, index, self.last_pos_d, self.last_alt_d)))
+            "lat: %f, lon: %f, alt: %f, xy off: %f, z off: %f, timeout: %d, index: %d, pos_d: %f, alt_d: %f" %
+            (lat, lon, alt, xy_radius, z_radius, timeout, index, self.last_pos_d, self.last_alt_d)))
 
     def run_mission(self):
         """switch mode: armed | auto"""

From f252ac3eff260acc2b457e0f0f1be728138488ac Mon Sep 17 00:00:00 2001
From: Andreas Antener <antener_a@gmx.ch>
Date: Mon, 27 Jun 2016 22:47:18 +0200
Subject: [PATCH 0845/1230] added mission checks for landing and VTOL
 transition

---
 .../python_src/px4_it/mavros/mission_test.py  | 83 +++++++++++++++----
 1 file changed, 65 insertions(+), 18 deletions(-)

diff --git a/integrationtests/python_src/px4_it/mavros/mission_test.py b/integrationtests/python_src/px4_it/mavros/mission_test.py
index 7e2a094709..7f15428849 100755
--- a/integrationtests/python_src/px4_it/mavros/mission_test.py
+++ b/integrationtests/python_src/px4_it/mavros/mission_test.py
@@ -62,8 +62,6 @@ class MavrosMissionTest(unittest.TestCase):
 
     def setUp(self):
         rospy.init_node('test_node', anonymous=True)
-        #self.helper = PX4TestHelper("mavros_offboard_posctl_test")
-        #self.helper.setUp()
 
         self.rate = rospy.Rate(10) # 10hz
         self.has_global_pos = False
@@ -143,6 +141,10 @@ class MavrosMissionTest(unittest.TestCase):
         self.last_alt_d = 9999
         self.last_pos_d = 9999
 
+        rospy.loginfo("trying to reach waypoint " +
+            "lat: %f, lon: %f, alt: %f, timeout: %d, index: %d" %
+            (lat, lon, alt, timeout, index))
+
         # does it reach the position in X seconds?
         count = 0
         while count < timeout:
@@ -157,6 +159,7 @@ class MavrosMissionTest(unittest.TestCase):
                 rospy.loginfo("position reached, index: %d, count: %d, pos_d: %f, alt_d: %f" %
                     (index, count, self.last_pos_d, self.last_alt_d))
                 break
+
             count = count + 1
             self.rate.sleep()
 
@@ -175,6 +178,49 @@ class MavrosMissionTest(unittest.TestCase):
         while not self.has_global_pos:
             self.rate.sleep()
 
+    def wait_on_landing(self, timeout, index):
+        """Wait for landed state"""
+
+        rospy.loginfo("waiting for landing " +
+            "timeout: %d, index: %d" %
+            (timeout, index))
+
+        count = 0
+        while count < timeout:
+            if self.extended_state.landed_state == ExtendedState.LANDED_STATE_ON_GROUND:
+                break
+
+            count = count + 1
+            self.rate.sleep()
+
+        self.assertTrue(count < timeout, ("landing not detected after landing WP " +
+            "timeout: %d, index: %d") %
+            (timeout, index))
+
+    def wait_on_transition(self, transition, timeout, index):
+        """Wait for VTOL transition"""
+
+        rospy.loginfo("waiting for VTOL transition " +
+            "transition: %d, timeout: %d, index: %d" %
+            (transition, timeout, index))
+
+        count = 0
+        while count < timeout:
+            # transition to MC
+            if transition == 3 and self.extended_state.vtol_state == ExtendedState.VTOL_STATE_MC:
+                break
+
+            # transition to FW
+            if transition == 4 and self.extended_state.vtol_state == ExtendedState.VTOL_STATE_FW:
+                break
+
+            count = count + 1
+            self.rate.sleep()
+
+        self.assertTrue(count < timeout, ("landing not detected after landing WP " +
+            "timeout: %d, index: %d") %
+            (timeout, index))
+
     def send_heartbeat(self, event=None):
         # mav type gcs
         mavmsg = mavutil.mavlink.MAVLink_heartbeat_message(6, 0, 0, 0, 0, 0)
@@ -195,9 +241,9 @@ class MavrosMissionTest(unittest.TestCase):
         rospy.loginfo("reading mission %s", file)
         mission = QGroundControlWP()
         wps = []
-        for wp in mission.read(open(file, 'r')):
-            wps.append(wp)
-            rospy.logdebug(wp)
+        for waypoint in mission.read(open(file, 'r')):
+            wps.append(waypoint)
+            rospy.logdebug(waypoint)
 
         rospy.loginfo("wait until ready")
         self.wait_until_ready()
@@ -210,21 +256,22 @@ class MavrosMissionTest(unittest.TestCase):
         rospy.loginfo("run mission")
         self.run_mission()
 
-        positions = (
-            (0, 0, 0),
-            (2, 2, 2),
-            (2, -2, 2),
-            (-2, -2, 2),
-            (2, 2, 2))
-
         index = 0
-        for wp in wps:
-            # only check position waypoints
-            if wp.frame == Waypoint.FRAME_GLOBAL_REL_ALT or wp.frame == Waypoint.FRAME_GLOBAL:
-                alt = wp.z_alt
-                if wp.frame == Waypoint.FRAME_GLOBAL_REL_ALT:
+        for waypoint in wps:
+            # only check position for waypoints where this makes sense
+            if waypoint.frame == Waypoint.FRAME_GLOBAL_REL_ALT or waypoint.frame == Waypoint.FRAME_GLOBAL:
+                alt = waypoint.z_alt
+                if waypoint.frame == Waypoint.FRAME_GLOBAL_REL_ALT:
                     alt += self.home_alt
-                self.reach_position(wp.x_lat, wp.y_long, alt, 600, index)
+                self.reach_position(waypoint.x_lat, waypoint.y_long, alt, 600, index)
+
+            # after reaching position, wait for landing detection if applicable
+            if waypoint.command == 85 or waypoint.command == 21:
+                self.wait_on_landing(600, index)
+
+            # check if VTOL transition happens if applicable
+            if waypoint.command == 84 or waypoint.command == 3000:
+                self.wait_on_transition(waypoint.param1, 600, index)
 
             index += 1
 

From d995f758c2574fbb0ea25db90506b1eb47906081 Mon Sep 17 00:00:00 2001
From: Andreas Antener <antener_a@gmx.ch>
Date: Mon, 27 Jun 2016 22:47:54 +0200
Subject: [PATCH 0846/1230] added mission test to CI run

---
 integrationtests/run_tests.bash | 1 +
 1 file changed, 1 insertion(+)

diff --git a/integrationtests/run_tests.bash b/integrationtests/run_tests.bash
index 6894252a56..b17b7511dc 100755
--- a/integrationtests/run_tests.bash
+++ b/integrationtests/run_tests.bash
@@ -50,6 +50,7 @@ echo "<====="
 set +e
 echo "=====> run tests"
 rostest px4 mavros_posix_tests_iris.launch
+rostest px4 mavros_posix_tests_standard_vtol.launch
 TEST_RESULT=$?
 echo "<====="
 

From 37884dc5ddd152ff7b430e24679d06ed1f6f16d4 Mon Sep 17 00:00:00 2001
From: Andreas Antener <antener_a@gmx.ch>
Date: Mon, 27 Jun 2016 23:32:29 +0200
Subject: [PATCH 0847/1230] fixed landing and transition detection test

---
 .../python_src/px4_it/mavros/mission_test.py  | 24 +++++++++++++------
 1 file changed, 17 insertions(+), 7 deletions(-)

diff --git a/integrationtests/python_src/px4_it/mavros/mission_test.py b/integrationtests/python_src/px4_it/mavros/mission_test.py
index 7f15428849..c16fe9d6b6 100755
--- a/integrationtests/python_src/px4_it/mavros/mission_test.py
+++ b/integrationtests/python_src/px4_it/mavros/mission_test.py
@@ -207,17 +207,19 @@ class MavrosMissionTest(unittest.TestCase):
         count = 0
         while count < timeout:
             # transition to MC
-            if transition == 3 and self.extended_state.vtol_state == ExtendedState.VTOL_STATE_MC:
+            if (transition == ExtendedState.VTOL_STATE_MC and
+                    self.extended_state.vtol_state == ExtendedState.VTOL_STATE_MC):
                 break
 
             # transition to FW
-            if transition == 4 and self.extended_state.vtol_state == ExtendedState.VTOL_STATE_FW:
+            if (transition == ExtendedState.VTOL_STATE_FW and
+                    self.extended_state.vtol_state == ExtendedState.VTOL_STATE_FW):
                 break
 
             count = count + 1
             self.rate.sleep()
 
-        self.assertTrue(count < timeout, ("landing not detected after landing WP " +
+        self.assertTrue(count < timeout, ("transition not detected " +
             "timeout: %d, index: %d") %
             (timeout, index))
 
@@ -265,14 +267,22 @@ class MavrosMissionTest(unittest.TestCase):
                     alt += self.home_alt
                 self.reach_position(waypoint.x_lat, waypoint.y_long, alt, 600, index)
 
+            # check if VTOL transition happens if applicable
+            if waypoint.command == 84 or waypoint.command == 85 or waypoint.command == 3000:
+                transition = waypoint.param1
+
+                if waypoint.command == 84: # VTOL takeoff implies transition to FW
+                    transition = ExtendedState.VTOL_STATE_FW
+
+                if waypoint.command == 85: # VTOL takeoff implies transition to MC
+                    transition = ExtendedState.VTOL_STATE_MC
+
+                self.wait_on_transition(transition, 600, index)
+
             # after reaching position, wait for landing detection if applicable
             if waypoint.command == 85 or waypoint.command == 21:
                 self.wait_on_landing(600, index)
 
-            # check if VTOL transition happens if applicable
-            if waypoint.command == 84 or waypoint.command == 3000:
-                self.wait_on_transition(waypoint.param1, 600, index)
-
             index += 1
 
 

From 0e5a83f3c1175006fdd80b9481d943127779cf33 Mon Sep 17 00:00:00 2001
From: Andreas Antener <antener_a@gmx.ch>
Date: Mon, 27 Jun 2016 23:32:53 +0200
Subject: [PATCH 0848/1230] temporarily disabled running mission test on CI

---
 integrationtests/run_tests.bash | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/integrationtests/run_tests.bash b/integrationtests/run_tests.bash
index b17b7511dc..83e3121d87 100755
--- a/integrationtests/run_tests.bash
+++ b/integrationtests/run_tests.bash
@@ -50,7 +50,7 @@ echo "<====="
 set +e
 echo "=====> run tests"
 rostest px4 mavros_posix_tests_iris.launch
-rostest px4 mavros_posix_tests_standard_vtol.launch
+#rostest px4 mavros_posix_tests_standard_vtol.launch
 TEST_RESULT=$?
 echo "<====="
 

From c9f278e46f962b660d19f43a29d6c6b4acaaee43 Mon Sep 17 00:00:00 2001
From: Andreas Antener <antener_a@gmx.ch>
Date: Tue, 28 Jun 2016 00:01:34 +0200
Subject: [PATCH 0849/1230] fix rcS for standard vtol

---
 posix-configs/SITL/init/rcS_gazebo_standard_vtol | 1 +
 1 file changed, 1 insertion(+)

diff --git a/posix-configs/SITL/init/rcS_gazebo_standard_vtol b/posix-configs/SITL/init/rcS_gazebo_standard_vtol
index 44294fa203..2a663400ca 100644
--- a/posix-configs/SITL/init/rcS_gazebo_standard_vtol
+++ b/posix-configs/SITL/init/rcS_gazebo_standard_vtol
@@ -48,6 +48,7 @@ param set MPC_ACC_HOR_MAX 2
 param set GF_ACTION 3
 param set GF_MAX_HOR_DIST 500
 param set GF_MAX_VER_DIST 500
+simulator start -s 
 rgbledsim start
 tone_alarm start
 gyrosim start

From 2f581a296ef574ed6420454dd9bbad3944cac82f Mon Sep 17 00:00:00 2001
From: Andreas Antener <antener_a@gmx.ch>
Date: Tue, 28 Jun 2016 00:01:53 +0200
Subject: [PATCH 0850/1230] enable VTOL tests on CI again

---
 integrationtests/run_tests.bash | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/integrationtests/run_tests.bash b/integrationtests/run_tests.bash
index 83e3121d87..b17b7511dc 100755
--- a/integrationtests/run_tests.bash
+++ b/integrationtests/run_tests.bash
@@ -50,7 +50,7 @@ echo "<====="
 set +e
 echo "=====> run tests"
 rostest px4 mavros_posix_tests_iris.launch
-#rostest px4 mavros_posix_tests_standard_vtol.launch
+rostest px4 mavros_posix_tests_standard_vtol.launch
 TEST_RESULT=$?
 echo "<====="
 

From 26de353d4f8e885727438c3c4db61717fac3409d Mon Sep 17 00:00:00 2001
From: Andreas Antener <antener_a@gmx.ch>
Date: Tue, 28 Jun 2016 11:11:46 +0200
Subject: [PATCH 0851/1230] added mission file to test name

---
 integrationtests/python_src/px4_it/mavros/mission_test.py | 5 ++++-
 1 file changed, 4 insertions(+), 1 deletion(-)

diff --git a/integrationtests/python_src/px4_it/mavros/mission_test.py b/integrationtests/python_src/px4_it/mavros/mission_test.py
index c16fe9d6b6..e0459b91c3 100755
--- a/integrationtests/python_src/px4_it/mavros/mission_test.py
+++ b/integrationtests/python_src/px4_it/mavros/mission_test.py
@@ -288,4 +288,7 @@ class MavrosMissionTest(unittest.TestCase):
 
 if __name__ == '__main__':
     import rostest
-    rostest.rosrun(PKG, 'mavros_mission_test', MavrosMissionTest)
+    name = "mavros_mission_test"
+    if len(sys.argv) > 1:
+        name += "-%s" % sys.argv[1]
+    rostest.rosrun(PKG, name, MavrosMissionTest)

From 85b5b399b9ae5c097a5f2b9a2fd43b0a6064823d Mon Sep 17 00:00:00 2001
From: Andreas Antener <antener_a@gmx.ch>
Date: Tue, 28 Jun 2016 11:30:16 +0200
Subject: [PATCH 0852/1230] updated FW horizontal acceptance radius to work
 with deltaquad

---
 integrationtests/python_src/px4_it/mavros/mission_test.py | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/integrationtests/python_src/px4_it/mavros/mission_test.py b/integrationtests/python_src/px4_it/mavros/mission_test.py
index e0459b91c3..7215b855f8 100755
--- a/integrationtests/python_src/px4_it/mavros/mission_test.py
+++ b/integrationtests/python_src/px4_it/mavros/mission_test.py
@@ -70,7 +70,7 @@ class MavrosMissionTest(unittest.TestCase):
         self.extended_state = ExtendedState()
         self.home_alt = 0
         self.mc_rad = 5
-        self.fw_rad = 50
+        self.fw_rad = 80
         self.fw_alt_rad = 10
         self.last_alt_d = 9999
         self.last_pos_d = 9999

From 53b5758eb418be0ea09cab949a532ab5e063e7a1 Mon Sep 17 00:00:00 2001
From: Andreas Antener <antener_a@gmx.ch>
Date: Tue, 28 Jun 2016 11:34:29 +0200
Subject: [PATCH 0853/1230] added mission name to assertion outputs

---
 .../python_src/px4_it/mavros/mission_test.py  | 24 ++++++++++---------
 1 file changed, 13 insertions(+), 11 deletions(-)

diff --git a/integrationtests/python_src/px4_it/mavros/mission_test.py b/integrationtests/python_src/px4_it/mavros/mission_test.py
index 7215b855f8..76ec1a45a2 100755
--- a/integrationtests/python_src/px4_it/mavros/mission_test.py
+++ b/integrationtests/python_src/px4_it/mavros/mission_test.py
@@ -74,6 +74,7 @@ class MavrosMissionTest(unittest.TestCase):
         self.fw_alt_rad = 10
         self.last_alt_d = 9999
         self.last_pos_d = 9999
+        self.mission_name = ""
 
         # need to simulate heartbeat for datalink loss detection
         rospy.Timer(rospy.Duration(0.5), self.send_heartbeat)
@@ -163,9 +164,9 @@ class MavrosMissionTest(unittest.TestCase):
             count = count + 1
             self.rate.sleep()
 
-        self.assertTrue(count < timeout, ("took too long to get to position " +
-            "lat: %f, lon: %f, alt: %f, xy off: %f, z off: %f, timeout: %d, index: %d, pos_d: %f, alt_d: %f" %
-            (lat, lon, alt, xy_radius, z_radius, timeout, index, self.last_pos_d, self.last_alt_d)))
+        self.assertTrue(count < timeout, (("(%s) took too long to get to position " +
+            "lat: %f, lon: %f, alt: %f, xy off: %f, z off: %f, timeout: %d, index: %d, pos_d: %f, alt_d: %f") %
+            (self.mission_name, lat, lon, alt, xy_radius, z_radius, timeout, index, self.last_pos_d, self.last_alt_d)))
 
     def run_mission(self):
         """switch mode: armed | auto"""
@@ -193,9 +194,9 @@ class MavrosMissionTest(unittest.TestCase):
             count = count + 1
             self.rate.sleep()
 
-        self.assertTrue(count < timeout, ("landing not detected after landing WP " +
+        self.assertTrue(count < timeout, ("(%s) landing not detected after landing WP " +
             "timeout: %d, index: %d") %
-            (timeout, index))
+            (self.mission_name, timeout, index))
 
     def wait_on_transition(self, transition, timeout, index):
         """Wait for VTOL transition"""
@@ -219,9 +220,9 @@ class MavrosMissionTest(unittest.TestCase):
             count = count + 1
             self.rate.sleep()
 
-        self.assertTrue(count < timeout, ("transition not detected " +
+        self.assertTrue(count < timeout, ("(%s) transition not detected " +
             "timeout: %d, index: %d") %
-            (timeout, index))
+            (self.mission_name, timeout, index))
 
     def send_heartbeat(self, event=None):
         # mav type gcs
@@ -238,12 +239,13 @@ class MavrosMissionTest(unittest.TestCase):
             self.fail("usage: mission_test.py mission_file")
             return
 
-        file = os.path.dirname(os.path.realpath(__file__)) + "/" + sys.argv[1]
+        self.mission_name = sys.argv[1]
+        mission_file = os.path.dirname(os.path.realpath(__file__)) + "/" + sys.argv[1]
 
-        rospy.loginfo("reading mission %s", file)
+        rospy.loginfo("reading mission %s", mission_file)
         mission = QGroundControlWP()
         wps = []
-        for waypoint in mission.read(open(file, 'r')):
+        for waypoint in mission.read(open(mission_file, 'r')):
             wps.append(waypoint)
             rospy.logdebug(waypoint)
 
@@ -253,7 +255,7 @@ class MavrosMissionTest(unittest.TestCase):
         rospy.loginfo("send mission")
         res = self._srv_wp_push(wps)
         rospy.loginfo(res)
-        self.assertTrue(res.success, "mission could not be transfered")
+        self.assertTrue(res.success, "(%s) mission could not be transfered" % self.mission_name)
 
         rospy.loginfo("run mission")
         self.run_mission()

From 5ed4e4e3a5ae161d20507758e0dce131c9ca3f6f Mon Sep 17 00:00:00 2001
From: Andreas Antener <antener_a@gmx.ch>
Date: Tue, 28 Jun 2016 15:45:51 +0200
Subject: [PATCH 0854/1230] use proper matching for VTOL fixed-wing state
 regarding position acceptance

---
 .../python_src/px4_it/mavros/mission_test.py          | 11 ++++++++---
 1 file changed, 8 insertions(+), 3 deletions(-)

diff --git a/integrationtests/python_src/px4_it/mavros/mission_test.py b/integrationtests/python_src/px4_it/mavros/mission_test.py
index 76ec1a45a2..5b3fbd2e53 100755
--- a/integrationtests/python_src/px4_it/mavros/mission_test.py
+++ b/integrationtests/python_src/px4_it/mavros/mission_test.py
@@ -70,7 +70,7 @@ class MavrosMissionTest(unittest.TestCase):
         self.extended_state = ExtendedState()
         self.home_alt = 0
         self.mc_rad = 5
-        self.fw_rad = 80
+        self.fw_rad = 60
         self.fw_alt_rad = 10
         self.last_alt_d = 9999
         self.last_pos_d = 9999
@@ -149,10 +149,15 @@ class MavrosMissionTest(unittest.TestCase):
         # does it reach the position in X seconds?
         count = 0
         while count < timeout:
-            # use different radius matching vehicle state
+            # use MC radius by default
+            # FIXME: also check MAV_TYPE from system status, otherwise pure fixed-wing won't work
             xy_radius = self.mc_rad
             z_radius = self.mc_rad
-            if self.extended_state.vtol_state == ExtendedState.VTOL_STATE_FW:
+
+            # use FW radius if in FW or in transition
+            if (self.extended_state.vtol_state == ExtendedState.VTOL_STATE_FW or
+                    self.extended_state.vtol_state == ExtendedState.VTOL_STATE_TRANSITION_TO_MC or
+                    self.extended_state.vtol_state == ExtendedState.VTOL_STATE_TRANSITION_TO_FW):
                 xy_radius = self.fw_rad
                 z_radius = self.fw_alt_rad
 

From fcee34a9d111814db6f2cbde97f6db135bde4681 Mon Sep 17 00:00:00 2001
From: Samay Siga <samay_s@icloud.com>
Date: Wed, 29 Jun 2016 09:14:18 +0200
Subject: [PATCH 0855/1230] Quad tilt vtol config (#4473)

* Added new VTOL Config

I added new Vehicle ID for our VTOL aircraft - Quad - Tilt Rotor - FW

Added
13010_claire
claire.main.mix
claire.aux.mix

* Added files via upload

* Delete 13010_claire

* Create 13010_claire

* Update 13010_claire

* Update claire.main.mix

* Update claire.aux.mix
---
 ROMFS/px4fmu_common/init.d/13010_claire    | 36 ++++++++++++++++++++
 ROMFS/px4fmu_common/mixers/claire.aux.mix  | 38 ++++++++++++++++++++++
 ROMFS/px4fmu_common/mixers/claire.main.mix |  7 ++++
 3 files changed, 81 insertions(+)
 create mode 100644 ROMFS/px4fmu_common/init.d/13010_claire
 create mode 100644 ROMFS/px4fmu_common/mixers/claire.aux.mix
 create mode 100644 ROMFS/px4fmu_common/mixers/claire.main.mix

diff --git a/ROMFS/px4fmu_common/init.d/13010_claire b/ROMFS/px4fmu_common/init.d/13010_claire
new file mode 100644
index 0000000000..21d7b9ff09
--- /dev/null
+++ b/ROMFS/px4fmu_common/init.d/13010_claire
@@ -0,0 +1,36 @@
+#!nsh
+#
+# @name CruiseAder Claire
+#
+# @type VTOL Tiltrotor
+#
+# @maintainer Samay Siga <samay_s@icloud.com>
+#
+
+sh /etc/init.d/rc.vtol_defaults
+
+if [ $AUTOCNF == yes ]
+then
+param set VT_TYPE 1
+param set VT_TILT_MC 0.08
+param set VT_TILT_TRANS 0.5
+param set VT_TILT_FW 0.9
+
+param set VT_MOT_COUNT 4
+param set VT_IDLE_PWM_MC 1080
+param set VT_TYPE 1
+fi
+
+set MIXER claire
+set PWM_OUT 1234
+set PWM_RATE 400
+set PWM_MAX 2000
+
+set MIXER_AUX claire
+set PWM_AUX_RATE 50
+set PWM_AUX_RATE 123
+set PWM_AUX_MIN 1000
+set PWM_AUX_MAX 2000
+set PWM_AUX_DISARMED 1000
+
+set MAV_TYPE 21
diff --git a/ROMFS/px4fmu_common/mixers/claire.aux.mix b/ROMFS/px4fmu_common/mixers/claire.aux.mix
new file mode 100644
index 0000000000..54b2708294
--- /dev/null
+++ b/ROMFS/px4fmu_common/mixers/claire.aux.mix
@@ -0,0 +1,38 @@
+# mixer for the CruiseAder Claire tilt mechansim servo, aileron and elevator
+
+=======================================================================
+
+
+Tilt mechanism servo mixer
+
+---------------------------
+
+M: 1
+
+O:      10000  10000      0 -10000  10000
+
+S: 1 4  10000  10000      0 -10000  10000
+
+
+
+Aileron mixers
+
+-------------
+
+M: 1
+
+O:      10000  10000      0 -10000  10000
+
+S: 1 0  10000  10000      0 -10000  10000
+
+
+
+Elevator mixers
+
+-------------
+
+M: 1
+
+O:      10000  10000      0 -10000  10000
+
+S: 1 1 -10000 -10000      0 -10000  10000
diff --git a/ROMFS/px4fmu_common/mixers/claire.main.mix b/ROMFS/px4fmu_common/mixers/claire.main.mix
new file mode 100644
index 0000000000..baad77d3f8
--- /dev/null
+++ b/ROMFS/px4fmu_common/mixers/claire.main.mix
@@ -0,0 +1,7 @@
+# CruiseAder Claire Main Multirotor mixer for PX4FMU
+
+#
+
+#===========================
+
+R: 4x 10000 10000 10000 0

From 57a665ad99240d7cb363e8cfb1b4fb7dac8cdf43 Mon Sep 17 00:00:00 2001
From: tumbili <roman@px4.io>
Date: Fri, 17 Jun 2016 07:59:11 +0200
Subject: [PATCH 0856/1230] vtol delta quad plane: adjusted mixer scaling

Signed-off-by: tumbili <roman@px4.io>
---
 ROMFS/px4fmu_common/mixers/vtol_delta.aux.mix | 8 ++++----
 1 file changed, 4 insertions(+), 4 deletions(-)

diff --git a/ROMFS/px4fmu_common/mixers/vtol_delta.aux.mix b/ROMFS/px4fmu_common/mixers/vtol_delta.aux.mix
index ae3bbf9385..6b4990220b 100644
--- a/ROMFS/px4fmu_common/mixers/vtol_delta.aux.mix
+++ b/ROMFS/px4fmu_common/mixers/vtol_delta.aux.mix
@@ -19,13 +19,13 @@ input is inverted between the two servos.
 
 M: 2
 O:      10000  10000      0 -10000  10000
-S: 1 0  -6000  -6000      0 -10000  10000
-S: 1 1   6500   6500      0 -10000  10000
+S: 1 0  -8000  -8000      0 -10000  10000
+S: 1 1   8000   8000      0 -10000  10000
 
 M: 2
 O:      10000  10000      0 -10000  10000
-S: 1 0  -6000  -6000      0 -10000  10000
-S: 1 1  -6500  -6500      0 -10000  10000
+S: 1 0  -8000  -8000      0 -10000  10000
+S: 1 1  -8000  -8000      0 -10000  10000
 
 Motor speed mixer
 -----------------

From 036f42999a8f13c2a9c80046e2d942ecd4f60e52 Mon Sep 17 00:00:00 2001
From: tumbili <roman@px4.io>
Date: Wed, 29 Jun 2016 13:28:27 +0200
Subject: [PATCH 0857/1230] vtol delta quad plane: adjusted default controller
 gains

Signed-off-by: tumbili <roman@px4.io>
---
 .../init.d/13006_vtol_standard_delta          | 22 ++++++++++++-------
 1 file changed, 14 insertions(+), 8 deletions(-)

diff --git a/ROMFS/px4fmu_common/init.d/13006_vtol_standard_delta b/ROMFS/px4fmu_common/init.d/13006_vtol_standard_delta
index deb9aa3049..c3395eed0d 100644
--- a/ROMFS/px4fmu_common/init.d/13006_vtol_standard_delta
+++ b/ROMFS/px4fmu_common/init.d/13006_vtol_standard_delta
@@ -15,22 +15,28 @@ then
 	param set VT_MOT_COUNT 4
 	param set VT_TRANS_THR 0.75
 
-	param set MC_ROLL_P 7.0
+	param set MC_ROLL_P 6.5
 	param set MC_ROLLRATE_P 0.15
-	param set MC_ROLLRATE_I 0.002
+	param set MC_ROLLRATE_I 0.01
 	param set MC_ROLLRATE_D 0.003
 	param set MC_ROLLRATE_FF 0.0
-	param set MC_PITCH_P 7.0
-	param set MC_PITCHRATE_P 0.12
-	param set MC_PITCHRATE_I 0.002
+	param set MC_PITCH_P 6.5
+	param set MC_PITCHRATE_P 0.15
+	param set MC_PITCHRATE_I 0.01
 	param set MC_PITCHRATE_D 0.003
 	param set MC_PITCHRATE_FF 0.0
-	param set MC_YAW_P 2.8
+	param set MC_YAW_P 3.5
 	param set MC_YAW_FF 0.5
-	param set MC_YAWRATE_P 0.22
-	param set MC_YAWRATE_I 0.02
+	param set MC_YAWRATE_P 0.2
+	param set MC_YAWRATE_I 0.1
 	param set MC_YAWRATE_D 0.0
 	param set MC_YAWRATE_FF 0.0
+	param set MC_YAWRATE_MAX 20
+	param set MC_YAWRAUTO_MAX 20
+
+	param set MPC_XY_P 0.8
+	param set MPC_XY_VEL_P 0.1
+	param set MPC_ACC_HOR_MAX 2.0
 
 	param set VT_MOT_COUNT 4
 	param set VT_IDLE_PWM_MC 1080

From 9649050c2ecc8f344aec9e81b018b78cb5201857 Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Thu, 30 Jun 2016 13:29:47 +0200
Subject: [PATCH 0858/1230] Update ECL to master

---
 src/lib/ecl | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/src/lib/ecl b/src/lib/ecl
index 1b394460a3..5e0a229ee8 160000
--- a/src/lib/ecl
+++ b/src/lib/ecl
@@ -1 +1 @@
-Subproject commit 1b394460a39b6f69b913383e0b9e66527f617650
+Subproject commit 5e0a229ee8bf09430051090f58f71c95fa4755f0

From 07fa814597a675eebf3eb262143b4fe731698a79 Mon Sep 17 00:00:00 2001
From: Daniel Agar <daniel@agar.ca>
Date: Thu, 30 Jun 2016 07:55:47 -0400
Subject: [PATCH 0859/1230] FW vtol landing always forced (#4939)

---
 src/modules/navigator/mission.cpp       | 5 +++--
 src/modules/navigator/mission_block.cpp | 8 ++++----
 2 files changed, 7 insertions(+), 6 deletions(-)

diff --git a/src/modules/navigator/mission.cpp b/src/modules/navigator/mission.cpp
index 401fe0899d..8640051177 100644
--- a/src/modules/navigator/mission.cpp
+++ b/src/modules/navigator/mission.cpp
@@ -442,8 +442,9 @@ Mission::set_mission_items()
 	if (item_contains_position(&_mission_item)) {
 
 		/* force vtol land */
-		if(_mission_item.nav_cmd == NAV_CMD_LAND && _param_force_vtol.get()
-				&& !_navigator->get_vstatus()->is_rotary_wing){
+		if (_mission_item.nav_cmd == NAV_CMD_LAND && _navigator->get_vstatus()->is_vtol
+			&& _param_force_vtol.get() && !_navigator->get_vstatus()->is_rotary_wing) {
+
 			_mission_item.nav_cmd = NAV_CMD_VTOL_LAND;
 		}
 
diff --git a/src/modules/navigator/mission_block.cpp b/src/modules/navigator/mission_block.cpp
index ae1820a4a6..f00e05d1fe 100644
--- a/src/modules/navigator/mission_block.cpp
+++ b/src/modules/navigator/mission_block.cpp
@@ -344,7 +344,7 @@ MissionBlock::mission_item_to_position_setpoint(const struct mission_item_s *ite
 {
 	/* set the correct setpoint for vtol transition */
 
-	if(item->nav_cmd == NAV_CMD_DO_VTOL_TRANSITION && PX4_ISFINITE(item->yaw)
+	if (item->nav_cmd == NAV_CMD_DO_VTOL_TRANSITION && PX4_ISFINITE(item->yaw)
 			&& item->params[0] >= vtol_vehicle_status_s::VEHICLE_VTOL_STATE_FW - 0.5f) {
 		sp->type = position_setpoint_s::SETPOINT_TYPE_POSITION;
 		waypoint_from_heading_and_distance(_navigator->get_global_position()->lat,
@@ -389,7 +389,7 @@ MissionBlock::mission_item_to_position_setpoint(const struct mission_item_s *ite
 	case NAV_CMD_LAND:
 	case NAV_CMD_VTOL_LAND:
 		sp->type = position_setpoint_s::SETPOINT_TYPE_LAND;
-		if(_navigator->get_vstatus()->is_vtol && _param_vtol_wv_land.get()){
+		if (_navigator->get_vstatus()->is_vtol && _param_vtol_wv_land.get()) {
 			sp->disable_mc_yaw_control = true;
 		}
 		break;
@@ -398,7 +398,7 @@ MissionBlock::mission_item_to_position_setpoint(const struct mission_item_s *ite
 	case NAV_CMD_LOITER_TURN_COUNT:
 	case NAV_CMD_LOITER_UNLIMITED:
 		sp->type = position_setpoint_s::SETPOINT_TYPE_LOITER;
-		if(_navigator->get_vstatus()->is_vtol && _param_vtol_wv_loiter.get()){
+		if (_navigator->get_vstatus()->is_vtol && _param_vtol_wv_loiter.get()) {
 			sp->disable_mc_yaw_control = true;
 		}
 		break;
@@ -529,7 +529,7 @@ MissionBlock::set_land_item(struct mission_item_s *item, bool at_current_locatio
 {
 
 	/* VTOL transition to RW before landing */
-	if(_navigator->get_vstatus()->is_vtol && !_navigator->get_vstatus()->is_rotary_wing){
+	if (_navigator->get_vstatus()->is_vtol && !_navigator->get_vstatus()->is_rotary_wing) {
 		struct vehicle_command_s cmd = {};
 		cmd.command = NAV_CMD_DO_VTOL_TRANSITION;
 		cmd.param1 = vtol_vehicle_status_s::VEHICLE_VTOL_STATE_MC;

From 377726a9a7e72505b3ee3425a19610d9918955a4 Mon Sep 17 00:00:00 2001
From: Daniel Agar <daniel@agar.ca>
Date: Thu, 30 Jun 2016 07:56:06 -0400
Subject: [PATCH 0860/1230] sitl gazebo plane fix land detector startup order
 (#4932)

---
 posix-configs/SITL/init/rcS_gazebo_plane                 | 2 +-
 src/modules/commander/commander.cpp                      | 2 --
 src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp | 9 ++++++---
 3 files changed, 7 insertions(+), 6 deletions(-)

diff --git a/posix-configs/SITL/init/rcS_gazebo_plane b/posix-configs/SITL/init/rcS_gazebo_plane
index bcd62ca582..a431c1ed28 100644
--- a/posix-configs/SITL/init/rcS_gazebo_plane
+++ b/posix-configs/SITL/init/rcS_gazebo_plane
@@ -37,11 +37,11 @@ pwm_out_sim mode_pwm
 sleep 1
 sensors start
 commander start
-land_detector start fixedwing
 navigator start
 ekf2 start
 fw_pos_control_l1 start
 fw_att_control start
+land_detector start fixedwing
 mixer load /dev/pwm_output0 ../../../../ROMFS/px4fmu_common/mixers/plane_sitl.main.mix
 mavlink start -u 14556 -r 2000000
 mavlink start -u 14557 -r 2000000 -m onboard -o 14540
diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp
index 3752f56bf0..e45cb9d15c 100644
--- a/src/modules/commander/commander.cpp
+++ b/src/modules/commander/commander.cpp
@@ -97,8 +97,6 @@
 #include <uORB/topics/telemetry_status.h>
 #include <uORB/topics/vehicle_attitude.h>
 #include <uORB/topics/vehicle_command.h>
-#include <uORB/topics/vtol_vehicle_status.h>
-#include <uORB/topics/vehicle_land_detected.h>
 #include <uORB/topics/vehicle_command_ack.h>
 #include <uORB/topics/vehicle_global_position.h>
 #include <uORB/topics/vehicle_gps_position.h>
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 a55a43c813..0788056104 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
@@ -1184,9 +1184,8 @@ bool FixedwingPositionControl::update_desired_altitude(float dt)
 bool FixedwingPositionControl::in_takeoff_situation()
 {
 	const hrt_abstime delta_takeoff = 10000000;
-	const float throttle_threshold = 0.1f;
 
-	if (hrt_elapsed_time(&_time_went_in_air) < delta_takeoff && _manual.z > throttle_threshold
+	if (hrt_elapsed_time(&_time_went_in_air) < delta_takeoff
 	    && _global_pos.alt <= _takeoff_ground_alt + _parameters.climbout_diff) {
 		return true;
 	}
@@ -1362,10 +1361,14 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
 				alt_sp = pos_sp_triplet.current.alt + 2.0f * _parameters.climbout_diff;
 
 			} else {
-				// use altitude given by wapoint
+				// use altitude given by waypoint
 				alt_sp = pos_sp_triplet.current.alt;
 			}
 
+			if (in_takeoff_situation()) {
+				alt_sp = math::max(alt_sp, _takeoff_ground_alt + _parameters.climbout_diff);
+			}
+
 			if (in_takeoff_situation() ||
 			    ((_global_pos.alt < pos_sp_triplet.current.alt + _parameters.climbout_diff)
 			     && _fw_pos_ctrl_status.abort_landing == true)) {

From 5dcc62d8f9ebf9927814e827ef9796bc2913c017 Mon Sep 17 00:00:00 2001
From: Andreas Antener <antener_a@gmx.ch>
Date: Fri, 24 Jun 2016 23:01:17 +0200
Subject: [PATCH 0861/1230] allow yaw setpoint offset to be reduced once maxed
 out

---
 src/modules/mc_pos_control/mc_pos_control_main.cpp | 8 ++++----
 1 file changed, 4 insertions(+), 4 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 9bb7b56d49..22fb7daf0a 100644
--- a/src/modules/mc_pos_control/mc_pos_control_main.cpp
+++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp
@@ -1975,10 +1975,10 @@ MulticopterPositionControl::task_main()
 				float yaw_offs = _wrap_pi(yaw_target - _yaw);
 
 				// If the yaw offset became too big for the system to track stop
-				// shifting it
-
-				// XXX this needs inspection - probably requires a clamp, not an if
-				if (fabsf(yaw_offs) < yaw_offset_max) {
+				// shifting it, only allow if it would make the offset smaller again.
+				if (fabsf(yaw_offs) < yaw_offset_max ||
+						(_att_sp.yaw_sp_move_rate > 0 && yaw_offs < 0) ||
+						(_att_sp.yaw_sp_move_rate < 0 && yaw_offs > 0)) {
 					_att_sp.yaw_body = yaw_target;
 				}
 			}

From 0a40034159e38eded3eac6541ca5c417cf6013d6 Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lorenz@px4.io>
Date: Thu, 30 Jun 2016 16:26:05 +0200
Subject: [PATCH 0862/1230] Takeoff: Fix coordinate scaling (#4947)

---
 src/modules/navigator/navigator_main.cpp | 17 +++++++++++++----
 1 file changed, 13 insertions(+), 4 deletions(-)

diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp
index 01e168335e..163238da75 100644
--- a/src/modules/navigator/navigator_main.cpp
+++ b/src/modules/navigator/navigator_main.cpp
@@ -436,8 +436,9 @@ Navigator::task_main()
 				}
 
 				if (PX4_ISFINITE(cmd.param5) && PX4_ISFINITE(cmd.param6)) {
-					rep->current.lat = cmd.param5 / (double)1e7;
-					rep->current.lon = cmd.param6 / (double)1e7;
+					rep->current.lat = (cmd.param5 < 1000) ? cmd.param5 : cmd.param5 / (double)1e7;
+					rep->current.lon = (cmd.param6 < 1000) ? cmd.param6 : cmd.param6 / (double)1e7;
+
 				} else {
 					rep->current.lat = get_global_position()->lat;
 					rep->current.lon = get_global_position()->lon;
@@ -465,8 +466,16 @@ Navigator::task_main()
 				rep->current.loiter_direction = 1;
 				rep->current.type = position_setpoint_s::SETPOINT_TYPE_TAKEOFF;
 				rep->current.yaw = cmd.param4;
-				rep->current.lat = cmd.param5 / (double)1e7;
-				rep->current.lon = cmd.param6 / (double)1e7;
+
+				if (PX4_ISFINITE(cmd.param5) && PX4_ISFINITE(cmd.param6)) {
+					rep->current.lat = (cmd.param5 < 1000) ? cmd.param5 : cmd.param5 / (double)1e7;
+					rep->current.lon = (cmd.param6 < 1000) ? cmd.param6 : cmd.param6 / (double)1e7;
+				} else {
+					// If one of them is non-finite, reset both
+					rep->current.lat = NAN;
+					rep->current.lon = NAN;
+				}
+
 				rep->current.alt = cmd.param7;
 
 				rep->previous.valid = true;

From 998579befc22d15c22c9c5b78ff612dd4de06f56 Mon Sep 17 00:00:00 2001
From: tumbili <roman@px4.io>
Date: Mon, 27 Jun 2016 15:03:07 +0200
Subject: [PATCH 0863/1230] mc pos control: zero yaw setpoint move rate in
 attitude setpoint topic

- fixed bug where a non-zero yaw setpoint move rate could make the drone
yaw around in non-manual modes

Signed-off-by: tumbili <roman@px4.io>
---
 src/modules/mc_pos_control/mc_pos_control_main.cpp | 1 +
 1 file changed, 1 insertion(+)

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 22fb7daf0a..bc5d7a3bfc 100644
--- a/src/modules/mc_pos_control/mc_pos_control_main.cpp
+++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp
@@ -2056,6 +2056,7 @@ MulticopterPositionControl::task_main()
 
 		} else {
 			reset_yaw_sp = true;
+			_att_sp.yaw_sp_move_rate = 0.0f;
 		}
 
 		/* update previous velocity for velocity controller D part */

From 2214e7c202367268b73a2704a584654bc8ac5544 Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Thu, 30 Jun 2016 16:59:17 +0200
Subject: [PATCH 0864/1230] Commander: Remove annoying GPS fix regained warning

---
 src/modules/commander/commander.cpp | 4 +++-
 1 file changed, 3 insertions(+), 1 deletion(-)

diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp
index e45cb9d15c..d543afc33c 100644
--- a/src/modules/commander/commander.cpp
+++ b/src/modules/commander/commander.cpp
@@ -2192,7 +2192,9 @@ int commander_thread_main(int argc, char *argv[])
 				if (status_flags.gps_failure && !gpsIsNoisy) {
 					status_flags.gps_failure = false;
 					status_changed = true;
-					mavlink_log_critical(&mavlink_log_pub, "GPS fix regained");
+					if (status_flags.condition_home_position_valid) {
+						mavlink_log_critical(&mavlink_log_pub, "GPS fix regained");
+					}
 				}
 
 			} else if (!status_flags.gps_failure) {

From a2b1269b275d53af3241669ff676da3e7b0db17f Mon Sep 17 00:00:00 2001
From: Daniel Agar <daniel@agar.ca>
Date: Thu, 30 Jun 2016 21:00:11 -0400
Subject: [PATCH 0865/1230] check_code_style don't paginate output (#4952)

- fixes #4943
---
 Tools/check_code_style.sh | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/Tools/check_code_style.sh b/Tools/check_code_style.sh
index 5ffc0ac5d2..898f9d1c38 100755
--- a/Tools/check_code_style.sh
+++ b/Tools/check_code_style.sh
@@ -12,7 +12,7 @@ then
 		${DIR}/fix_code_style.sh --quiet < $file > $file.pretty
 
 		echo
-		git diff --no-index --minimal --histogram --color=always  $file $file.pretty
+		git --no-pager diff --no-index --minimal --histogram --color=always  $file $file.pretty
 		echo
 
 		rm -f $file.pretty

From 25e749de7764bd466db0108ee849fe524ccbb6ab Mon Sep 17 00:00:00 2001
From: Andreas Antener <antener_a@gmx.ch>
Date: Thu, 30 Jun 2016 16:36:47 +0200
Subject: [PATCH 0866/1230] use MC auto rates always in AUTO removed duplicate
 weathervaning limit

---
 .../mc_att_control/mc_att_control_main.cpp     | 18 ++++++------------
 1 file changed, 6 insertions(+), 12 deletions(-)

diff --git a/src/modules/mc_att_control/mc_att_control_main.cpp b/src/modules/mc_att_control/mc_att_control_main.cpp
index c06259b74c..1d6e9609fc 100644
--- a/src/modules/mc_att_control/mc_att_control_main.cpp
+++ b/src/modules/mc_att_control/mc_att_control_main.cpp
@@ -724,32 +724,26 @@ MulticopterAttitudeControl::control_attitude(float dt)
 
 	/* limit rates */
 	for (int i = 0; i < 3; i++) {
-		if (_v_control_mode.flag_control_velocity_enabled && !_v_control_mode.flag_control_manual_enabled) {
+		if ((_v_control_mode.flag_control_velocity_enabled || _v_control_mode.flag_control_auto_enabled) &&
+		    !_v_control_mode.flag_control_manual_enabled) {
 			_rates_sp(i) = math::constrain(_rates_sp(i), -_params.auto_rate_max(i), _params.auto_rate_max(i));
+
 		} else {
 			_rates_sp(i) = math::constrain(_rates_sp(i), -_params.mc_rate_max(i), _params.mc_rate_max(i));
 		}
 	}
 
-	/* weather-vane mode, dampen yaw rate */
-	if (_v_att_sp.disable_mc_yaw_control == true && _v_control_mode.flag_control_velocity_enabled && !_v_control_mode.flag_control_manual_enabled) {
-		float wv_yaw_rate_max = _params.auto_rate_max(2) * _params.vtol_wv_yaw_rate_scale;
-		_rates_sp(2) = math::constrain(_rates_sp(2), -wv_yaw_rate_max, wv_yaw_rate_max);
-		// prevent integrator winding up in weathervane mode
-		_rates_int(2) = 0.0f;
-	}
-
 	/* feed forward yaw setpoint rate */
 	_rates_sp(2) += _v_att_sp.yaw_sp_move_rate * yaw_w * _params.yaw_ff;
 
-	/* weather-vane mode, scale down yaw rate */
-	if (_v_att_sp.disable_mc_yaw_control == true && _v_control_mode.flag_control_velocity_enabled && !_v_control_mode.flag_control_manual_enabled) {
+	/* weather-vane mode, dampen yaw rate */
+	if ((_v_control_mode.flag_control_velocity_enabled || _v_control_mode.flag_control_auto_enabled) &&
+	    _v_att_sp.disable_mc_yaw_control == true && !_v_control_mode.flag_control_manual_enabled) {
 		float wv_yaw_rate_max = _params.auto_rate_max(2) * _params.vtol_wv_yaw_rate_scale;
 		_rates_sp(2) = math::constrain(_rates_sp(2), -wv_yaw_rate_max, wv_yaw_rate_max);
 		// prevent integrator winding up in weathervane mode
 		_rates_int(2) = 0.0f;
 	}
-
 }
 
 /*

From 1548a9a2a1abbc6df48a4d48c29ba9f4bb037473 Mon Sep 17 00:00:00 2001
From: sander <sander@droneslab.com>
Date: Tue, 28 Jun 2016 15:41:19 +0200
Subject: [PATCH 0867/1230] Change mission to new format and reset defaults

---
 Tools/CI/MissionCheck.py      | 10 +++++-----
 Tools/CI/VTOL_TAKEOFF.mission |  5 +++++
 Tools/CI/VTOLmission.txt      |  9 ---------
 3 files changed, 10 insertions(+), 14 deletions(-)
 create mode 100644 Tools/CI/VTOL_TAKEOFF.mission
 delete mode 100644 Tools/CI/VTOLmission.txt

diff --git a/Tools/CI/MissionCheck.py b/Tools/CI/MissionCheck.py
index a7421250da..444a0b1483 100755
--- a/Tools/CI/MissionCheck.py
+++ b/Tools/CI/MissionCheck.py
@@ -17,9 +17,9 @@
 
 connection_string       = '127.0.0.1:14540'
 
-import_mission_filename = 'VTOLmission.txt'
-max_execution_time      = 250
-alt_acceptance_radius   = 10
+import_mission_filename = 'VTOL_TAKEOFF.mission'
+max_execution_time      = 200
+alt_acceptance_radius   = 5
 
 ################################################################################################
 # Init
@@ -150,14 +150,14 @@ def save_mission(aFileName):
     Save a mission in the Waypoint file format
     (http://qgroundcontrol.org/mavlink/waypoint_protocol#waypoint_file_format).
     """
-    print "\nSave mission from Vehicle to file: %s" % export_mission_filename
+    print "\nSave mission from Vehicle to file: %s" % aFileName
     #Download mission from vehicle
     missionlist = download_mission()
     #Add file-format information
     output='QGC WPL 110\n'
     #Add home location as 0th waypoint
     home = vehicle.home_location
-    output+="%s\t%s\t%s\t%s\t%s\t%s\t%s\t%s\t%s\t%s\t%s\t%s\n" % (0,1,0,16,0,0,0,0,home.lat,home.lon,home.alt,1)
+    output+="%s\t%s\t%s\t%s\t%s\t%s\t%s\t%s\t%s\t%s\t%s\t%s\n\n\n" % (0,1,0,16,0,0,0,0,home.lat,home.lon,home.alt,1)
     #Add commands
     for cmd in missionlist:
         commandline="%s\t%s\t%s\t%s\t%s\t%s\t%s\t%s\t%s\t%s\t%s\t%s\n" % (cmd.seq,cmd.current,cmd.frame,cmd.command,cmd.param1,cmd.param2,cmd.param3,cmd.param4,cmd.x,cmd.y,cmd.z,cmd.autocontinue)
diff --git a/Tools/CI/VTOL_TAKEOFF.mission b/Tools/CI/VTOL_TAKEOFF.mission
new file mode 100644
index 0000000000..0c0d6592ba
--- /dev/null
+++ b/Tools/CI/VTOL_TAKEOFF.mission
@@ -0,0 +1,5 @@
+QGC WPL 110
+1	0	3	84	0.0	0.0	-0.0	0.0	47.3975105286	8.55026626587	10.0	1
+1	0	3	16	0.0	0.0	-0.0	0.0	47.395450592	8.55018424988	10.0	1
+2	0	3	16	0.0	0.0	-0.0	0.0	47.3954582214	8.54566764832	10.0	1
+3	0	3	85	0.0	0.0	-0.0	0.0	47.3977355957	8.54561138153	10.0	1
diff --git a/Tools/CI/VTOLmission.txt b/Tools/CI/VTOLmission.txt
deleted file mode 100644
index 65bb78aea2..0000000000
--- a/Tools/CI/VTOLmission.txt
+++ /dev/null
@@ -1,9 +0,0 @@
-QGC WPL 110
-0	1	0	16	0	0	0	0	47.3979949951	8.54559612274	25.0	1
-1	0	2	3000	4.0	0.0	0.0	0.0	47.3980331421	8.54578971863	25.0	1
-2	0	3	16	0.0	0.0	-0.0	0.0	47.399269104	8.54557228088	25.0	1
-3	0	3	16	0.0	0.0	-0.0	0.0	47.3992652893	8.54230213165	25.0	1
-4	0	3	16	0.0	0.0	-0.0	0.0	47.3974761963	8.54239082336	25.0	1
-5	0	3	16	0.0	0.0	-0.0	0.0	47.3976669312	8.54509449005	25.0	1
-6	0	2	3000	3.0	0.0	0.0	0.0	47.3977851868	8.54526233673	25.0	1
-7	0	3	21	25.0	0.0	-0.0	0.0	47.3979797363	8.54460906982	25.0	1

From 2a395c3fecd9281821e4a44faa968429d4be9792 Mon Sep 17 00:00:00 2001
From: sander <sander@droneslab.com>
Date: Tue, 28 Jun 2016 15:57:48 +0200
Subject: [PATCH 0868/1230] Moved to integrationtests

---
 .../python_src/px4_it/dronekit}/MissionCheck.py                   | 0
 .../python_src/px4_it/dronekit}/VTOL_TAKEOFF.mission              | 0
 2 files changed, 0 insertions(+), 0 deletions(-)
 rename {Tools/CI => integrationtests/python_src/px4_it/dronekit}/MissionCheck.py (100%)
 rename {Tools/CI => integrationtests/python_src/px4_it/dronekit}/VTOL_TAKEOFF.mission (100%)

diff --git a/Tools/CI/MissionCheck.py b/integrationtests/python_src/px4_it/dronekit/MissionCheck.py
similarity index 100%
rename from Tools/CI/MissionCheck.py
rename to integrationtests/python_src/px4_it/dronekit/MissionCheck.py
diff --git a/Tools/CI/VTOL_TAKEOFF.mission b/integrationtests/python_src/px4_it/dronekit/VTOL_TAKEOFF.mission
similarity index 100%
rename from Tools/CI/VTOL_TAKEOFF.mission
rename to integrationtests/python_src/px4_it/dronekit/VTOL_TAKEOFF.mission

From 00dfc99e08de7c685c0345f30e17f2763b519166 Mon Sep 17 00:00:00 2001
From: James Goppert <jgoppert@users.noreply.github.com>
Date: Fri, 1 Jul 2016 11:43:09 -0400
Subject: [PATCH 0869/1230] LPE Variance Dependent Publication (#4914)

* Use variance to control publishing for LPE.

* Don't stop publishing if we have gps/ baro.

* LPE tuning and cleanup.

* Added bias saturation to LPE.

* Added vector enabled low pass filter block.

* Added rk4 integration and pub lowpass to LPE.

* Fix std::abs issue on mac/ reset lowpass on state reset.

* Don't estimate gyro bias when rotating at high speed  att_est_q.

* Lowered low pass on position to 5 Hz for LPE.

* Streamline state space update for LPE.

* Added health flags to est2 log.

* Revert to old tuning, more conservative, less faults.

* Formatting.

* Fix for fault message on LPE.

* Added subscription throttling to LPE.

* Formatting.
---
 src/lib/controllib/blocks.cpp                 |   1 +
 src/lib/controllib/blocks.hpp                 |  39 ++
 .../attitude_estimator_q_main.cpp             |   5 +-
 .../BlockLocalPositionEstimator.cpp           | 424 +++++++++++-------
 .../BlockLocalPositionEstimator.hpp           |  29 +-
 .../local_position_estimator/CMakeLists.txt   |   2 +-
 .../local_position_estimator_main.cpp         |   2 +-
 src/modules/local_position_estimator/params.c |  56 ++-
 .../local_position_estimator/sensors/baro.cpp |  13 +-
 .../local_position_estimator/sensors/flow.cpp |   6 +-
 .../local_position_estimator/sensors/gps.cpp  |  51 +--
 .../sensors/lidar.cpp                         |   9 +-
 .../sensors/mocap.cpp                         |   4 +-
 .../sensors/sonar.cpp                         |   9 +-
 .../sensors/vision.cpp                        |   4 +-
 src/modules/sdlog2/sdlog2.c                   |   1 +
 src/modules/sdlog2/sdlog2_messages.h          |   9 +-
 17 files changed, 417 insertions(+), 247 deletions(-)

diff --git a/src/lib/controllib/blocks.cpp b/src/lib/controllib/blocks.cpp
index aeef0f0977..0e0c3a6ccc 100644
--- a/src/lib/controllib/blocks.cpp
+++ b/src/lib/controllib/blocks.cpp
@@ -84,6 +84,7 @@ float BlockLowPass::update(float input)
 	return getState();
 }
 
+
 float BlockHighPass::update(float input)
 {
 	float b = 2 * float(M_PI) * getFCut() * getDt();
diff --git a/src/lib/controllib/blocks.hpp b/src/lib/controllib/blocks.hpp
index 28b73aa62d..a56a32b216 100644
--- a/src/lib/controllib/blocks.hpp
+++ b/src/lib/controllib/blocks.hpp
@@ -127,6 +127,45 @@ protected:
 	control::BlockParamFloat _fCut;
 };
 
+template<class Type, size_t M>
+class __EXPORT BlockLowPassVector: public Block
+{
+public:
+// methods
+	BlockLowPassVector(SuperBlock *parent,
+			   const char *name) :
+		Block(parent, name),
+		_state(),
+		_fCut(this, "") // only one parameter, no need to name
+	{
+		for (int i = 0; i < M; i++) {
+			_state(i) = 0.0f / 0.0f;
+		}
+	};
+	virtual ~BlockLowPassVector() {};
+	matrix::Vector<Type, M> update(const matrix::Matrix<Type, M, 1> &input)
+	{
+		for (int i = 0; i < M; i++) {
+			if (!PX4_ISFINITE(getState()(i))) {
+				setState(input);
+			}
+		}
+
+		float b = 2 * float(M_PI) * getFCut() * getDt();
+		float a = b / (1 + b);
+		setState(input * a + getState() * (1 - a));
+		return getState();
+	}
+// accessors
+	matrix::Vector<Type, M> getState() { return _state; }
+	float getFCut() { return _fCut.get(); }
+	void setState(const matrix::Vector<Type, M> &state) { _state = state; }
+private:
+// attributes
+	matrix::Vector<Type, M> _state;
+	control::BlockParamFloat _fCut;
+};
+
 /**
  * A high pass filter as described here:
  * http://en.wikipedia.org/wiki/High-pass_filter.
diff --git a/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp b/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp
index 9ffd4599fe..b2b6c6bdda 100644
--- a/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp
+++ b/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp
@@ -797,6 +797,7 @@ bool AttitudeEstimatorQ::update(float dt)
 
 	_q.normalize();
 
+
 	// Accelerometer correction
 	// Project 'k' unit vector of earth frame to body frame
 	// Vector<3> k = _q.conjugate_inversed(Vector<3>(0.0f, 0.0f, 1.0f));
@@ -810,7 +811,9 @@ bool AttitudeEstimatorQ::update(float dt)
 	corr += (k % (_accel - _pos_acc).normalized()) * _w_accel;
 
 	// Gyro bias estimation
-	_gyro_bias += corr * (_w_gyro_bias * dt);
+	if (_gyro.length() < 1.0f) {
+		_gyro_bias += corr * (_w_gyro_bias * dt);
+	}
 
 	for (int i = 0; i < 3; i++) {
 		_gyro_bias(i) = math::constrain(_gyro_bias(i), -_bias_max, _bias_max);
diff --git a/src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp b/src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp
index 12360a18a2..b9b4c21fa5 100644
--- a/src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp
+++ b/src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp
@@ -3,40 +3,45 @@
 #include <fcntl.h>
 #include <systemlib/err.h>
 #include <matrix/math.hpp>
+#include <cstdlib>
 
 orb_advert_t mavlink_log_pub = nullptr;
 
 // timeouts for sensors in microseconds
 static const uint32_t 		EST_SRC_TIMEOUT = 10000; // 0.01 s
 
+// required standard deviation of estimate for estimator to publish data
+static const uint32_t 		EST_STDDEV_XY_VALID = 2.0; // 2.0 m
+static const uint32_t 		EST_STDDEV_Z_VALID = 2.0; // 2.0 m
+static const uint32_t 		EST_STDDEV_TZ_VALID = 2.0; // 2.0 m
+
 // minimum flow altitude
 static const float flow_min_agl = 0.3;
 
 BlockLocalPositionEstimator::BlockLocalPositionEstimator() :
 	// this block has no parent, and has name LPE
 	SuperBlock(NULL, "LPE"),
-
 	// subscriptions, set rate, add to list
-	// TODO topic speed limiting?
-	_sub_status(ORB_ID(vehicle_status), 0, 0, &getSubscriptions()),
-	_sub_armed(ORB_ID(actuator_armed), 0, 0, &getSubscriptions()),
-	_sub_control_mode(ORB_ID(vehicle_control_mode),
-			  0, 0, &getSubscriptions()),
-	_sub_att(ORB_ID(vehicle_attitude), 0, 0, &getSubscriptions()),
-	_sub_att_sp(ORB_ID(vehicle_attitude_setpoint),
-		    0, 0, &getSubscriptions()),
-	_sub_flow(ORB_ID(optical_flow), 0, 0, &getSubscriptions()),
-	_sub_sensor(ORB_ID(sensor_combined), 0, 0, &getSubscriptions()),
-	_sub_param_update(ORB_ID(parameter_update), 0, 0, &getSubscriptions()),
-	_sub_manual(ORB_ID(manual_control_setpoint), 0, 0, &getSubscriptions()),
-	_sub_home(ORB_ID(home_position), 0, 0, &getSubscriptions()),
-	_sub_gps(ORB_ID(vehicle_gps_position), 0, 0, &getSubscriptions()),
-	_sub_vision_pos(ORB_ID(vision_position_estimate), 0, 0, &getSubscriptions()),
-	_sub_mocap(ORB_ID(att_pos_mocap), 0, 0, &getSubscriptions()),
-	_sub_dist0(ORB_ID(distance_sensor), 0, 0, &getSubscriptions()),
-	_sub_dist1(ORB_ID(distance_sensor), 0, 1, &getSubscriptions()),
-	_sub_dist2(ORB_ID(distance_sensor), 0, 2, &getSubscriptions()),
-	_sub_dist3(ORB_ID(distance_sensor), 0, 3, &getSubscriptions()),
+	_sub_armed(ORB_ID(actuator_armed), 1000 / 2, 0, &getSubscriptions()),
+	_sub_att(ORB_ID(vehicle_attitude), 1000 / 100, 0, &getSubscriptions()),
+	// flow 10 hz
+	_sub_flow(ORB_ID(optical_flow), 1000 / 10, 0, &getSubscriptions()),
+	// main prediction loop, 100 hz
+	_sub_sensor(ORB_ID(sensor_combined), 1000 / 100, 0, &getSubscriptions()),
+	// status updates 2 hz
+	_sub_param_update(ORB_ID(parameter_update), 1000 / 2, 0, &getSubscriptions()),
+	_sub_manual(ORB_ID(manual_control_setpoint), 1000 / 2, 0, &getSubscriptions()),
+	_sub_home(ORB_ID(home_position), 1000 / 2, 0, &getSubscriptions()),
+	// gps 10 hz
+	_sub_gps(ORB_ID(vehicle_gps_position), 1000 / 10, 0, &getSubscriptions()),
+	// vision 5 hz
+	_sub_vision_pos(ORB_ID(vision_position_estimate), 1000 / 5, 0, &getSubscriptions()),
+	// all distance sensors, 10 hz
+	_sub_mocap(ORB_ID(att_pos_mocap), 1000 / 10, 0, &getSubscriptions()),
+	_sub_dist0(ORB_ID(distance_sensor), 1000 / 10, 0, &getSubscriptions()),
+	_sub_dist1(ORB_ID(distance_sensor), 1000 / 10, 1, &getSubscriptions()),
+	_sub_dist2(ORB_ID(distance_sensor), 1000 / 10, 2, &getSubscriptions()),
+	_sub_dist3(ORB_ID(distance_sensor), 1000 / 10, 3, &getSubscriptions()),
 	_dist_subs(),
 	_sub_lidar(NULL),
 	_sub_sonar(NULL),
@@ -97,7 +102,10 @@ BlockLocalPositionEstimator::BlockLocalPositionEstimator() :
 	_mocapStats(this, ""),
 	_gpsStats(this, ""),
 
-	// stats
+	// low pass
+	_xLowPass(this, "X_LP"),
+
+	// delay
 	_xDelay(this, ""),
 	_tDelay(this, ""),
 
@@ -141,9 +149,9 @@ BlockLocalPositionEstimator::BlockLocalPositionEstimator() :
 	_flowMeanQual(0),
 
 	// status
-	_canEstimateXY(false),
-	_canEstimateZ(false),
-	_canEstimateT(false),
+	_validXY(false),
+	_validZ(false),
+	_validTZ(false),
 	_xyTimeout(true),
 	_zTimeout(true),
 	_tzTimeout(true),
@@ -182,12 +190,12 @@ BlockLocalPositionEstimator::BlockLocalPositionEstimator() :
 	_polls[POLL_SENSORS].fd = _sub_sensor.getHandle();
 	_polls[POLL_SENSORS].events = POLLIN;
 
-	// initialize P, x, u
-	initP();
+	// initialize A, B,  P, x, u
 	_x.setZero();
 	_u.setZero();
 	_flowX = 0;
 	_flowY = 0;
+	initSS();
 
 	// perf counters
 	_loop_perf = perf_alloc(PC_ELAPSED,
@@ -207,6 +215,14 @@ BlockLocalPositionEstimator::~BlockLocalPositionEstimator()
 {
 }
 
+Vector<float, BlockLocalPositionEstimator::n_x> BlockLocalPositionEstimator::dynamics(
+	float t,
+	const Vector<float, BlockLocalPositionEstimator::n_x> &x,
+	const Vector<float, BlockLocalPositionEstimator::n_u> &u)
+{
+	return _A * x + _B * u;
+}
+
 void BlockLocalPositionEstimator::update()
 {
 
@@ -254,6 +270,9 @@ void BlockLocalPositionEstimator::update()
 
 		// assume we are on the ground, so terrain alt is local alt
 		_x(X_tz) = _x(X_z);
+
+		// reset lowpass filter as well
+		_xLowPass.setState(_x);
 	}
 
 	_lastArmedState = armedState;
@@ -275,6 +294,7 @@ void BlockLocalPositionEstimator::update()
 	// update parameters
 	if (paramsUpdated) {
 		updateParams();
+		updateSSParams();
 	}
 
 	// update home position projection
@@ -282,27 +302,60 @@ void BlockLocalPositionEstimator::update()
 		updateHome();
 	}
 
-	// determine if we should start estimating
-	_canEstimateZ =
-		(_baroInitialized && _baroFault < fault_lvl_disable);
-	_canEstimateXY =
-		(_gpsInitialized && _gpsFault < fault_lvl_disable) ||
-		(_flowInitialized && _flowFault < fault_lvl_disable) ||
-		(_visionInitialized && _visionFault < fault_lvl_disable) ||
-		(_mocapInitialized && _mocapFault < fault_lvl_disable);
-	_canEstimateT =
-		(_lidarInitialized && _lidarFault < fault_lvl_disable) ||
-		(_sonarInitialized && _sonarFault < fault_lvl_disable);
+	// is xy valid?
+	bool xy_stddev_ok = sqrt(math::max(_P(X_x, X_x), _P(X_y, X_y))) < EST_STDDEV_XY_VALID;
 
-	if (_canEstimateXY) {
+	if (_validXY) {
+		// if valid and gps has timed out, set to not valid
+		if (!xy_stddev_ok && !_gpsInitialized) {
+			_validXY = false;
+		}
+
+	} else {
+		if (xy_stddev_ok) {
+			_validXY = true;
+		}
+	}
+
+	// is z valid?
+	bool z_stddev_ok = sqrt(_P(X_z, X_z)) < EST_STDDEV_Z_VALID;
+
+	if (_validZ) {
+		// if valid and baro has timed out, set to not valid
+		if (!z_stddev_ok && !_baroInitialized) {
+			_validZ = false;
+		}
+
+	} else {
+		if (z_stddev_ok) {
+			_validZ = true;
+		}
+	}
+
+	// is terrain valid?
+	bool tz_stddev_ok = sqrt(_P(X_tz, X_tz)) < EST_STDDEV_TZ_VALID;
+
+	if (_validTZ) {
+		if (!tz_stddev_ok) {
+			_validTZ = false;
+		}
+
+	} else {
+		if (tz_stddev_ok) {
+			_validTZ = true;
+		}
+	}
+
+	// timeouts
+	if (_validXY) {
 		_time_last_xy = _timeStamp;
 	}
 
-	if (_canEstimateZ) {
+	if (_validZ) {
 		_time_last_z = _timeStamp;
 	}
 
-	if (_canEstimateT) {
+	if (_validTZ) {
 		_time_last_tz = _timeStamp;
 	}
 
@@ -310,7 +363,7 @@ void BlockLocalPositionEstimator::update()
 	checkTimeouts();
 
 	// if we have no lat, lon initialize projection at 0,0
-	if (_canEstimateXY && !_map_ref.init_done) {
+	if (_validXY && !_map_ref.init_done) {
 		map_projection_init(&_map_ref,
 				    _init_home_lat.get(),
 				    _init_home_lon.get());
@@ -431,7 +484,7 @@ void BlockLocalPositionEstimator::update()
 		publishLocalPos();
 		publishEstimatorStatus();
 
-		if (_canEstimateXY) {
+		if (_validXY) {
 			publishGlobalPos();
 		}
 	}
@@ -495,7 +548,53 @@ void BlockLocalPositionEstimator::checkTimeouts()
 
 float BlockLocalPositionEstimator::agl()
 {
-	return _x(X_tz) - _x(X_z);
+	const Vector<float, n_x> &xLP = _xLowPass.getState();
+	return xLP(X_tz) - xLP(X_z);
+}
+
+void BlockLocalPositionEstimator::correctionLogic(Vector<float, n_x> &dx)
+{
+	// don't correct bias when rotating rapidly
+	float ang_speed = sqrt(
+				  _sub_att.get().rollspeed * _sub_att.get().rollspeed +
+				  _sub_att.get().pitchspeed * _sub_att.get().pitchspeed +
+				  _sub_att.get().yawspeed * _sub_att.get().yawspeed);
+
+	if (ang_speed > 1) {
+		dx(X_bx) = 0;
+		dx(X_by) = 0;
+		dx(X_bz) = 0;
+	}
+
+	if (!_validXY) {
+		dx(X_x) = 0;
+		dx(X_y) = 0;
+		dx(X_vx) = 0;
+		dx(X_vy) = 0;
+		dx(X_bx) = 0;
+		dx(X_by) = 0;
+	}
+
+	if (!_validZ) {
+		dx(X_z) = 0;
+		dx(X_vz) = 0;
+		dx(X_bz) = 0;
+	}
+
+	if (!_validTZ) {
+		dx(X_tz) = 0;
+	}
+
+	// saturate bias
+	float bx = dx(X_bx) + _x(X_bx);
+	float by = dx(X_by) + _x(X_by);
+	float bz = dx(X_bz) + _x(X_bz);
+
+	if (std::abs(bx) > BIAS_MAX) { bx = BIAS_MAX * bx / std::abs(bx); }
+
+	if (std::abs(by) > BIAS_MAX) { by = BIAS_MAX * by / std::abs(by); }
+
+	if (std::abs(bz) > BIAS_MAX) { bz = BIAS_MAX * bz / std::abs(bz); }
 }
 
 void BlockLocalPositionEstimator::detectDistanceSensors()
@@ -551,21 +650,23 @@ void BlockLocalPositionEstimator::updateHome()
 
 void BlockLocalPositionEstimator::publishLocalPos()
 {
+	const Vector<float, n_x> &xLP = _xLowPass.getState();
+
 	// publish local position
 	if (PX4_ISFINITE(_x(X_x)) && PX4_ISFINITE(_x(X_y)) && PX4_ISFINITE(_x(X_z)) &&
 	    PX4_ISFINITE(_x(X_vx)) && PX4_ISFINITE(_x(X_vy))
 	    && PX4_ISFINITE(_x(X_vz))) {
 		_pub_lpos.get().timestamp = _timeStamp;
-		_pub_lpos.get().xy_valid = _canEstimateXY;
-		_pub_lpos.get().z_valid = _canEstimateZ;
-		_pub_lpos.get().v_xy_valid = _canEstimateXY;
-		_pub_lpos.get().v_z_valid = _canEstimateZ;
-		_pub_lpos.get().x = _x(X_x); 	// north
-		_pub_lpos.get().y = _x(X_y);  	// east
-		_pub_lpos.get().z = _x(X_z); 	// down
-		_pub_lpos.get().vx = _x(X_vx);  // north
-		_pub_lpos.get().vy = _x(X_vy);  // east
-		_pub_lpos.get().vz = _x(X_vz); 	// down
+		_pub_lpos.get().xy_valid = _validXY;
+		_pub_lpos.get().z_valid = _validZ;
+		_pub_lpos.get().v_xy_valid = _validXY;
+		_pub_lpos.get().v_z_valid = _validZ;
+		_pub_lpos.get().x = xLP(X_x); 	// north
+		_pub_lpos.get().y = xLP(X_y);  	// east
+		_pub_lpos.get().z = xLP(X_z); 	// down
+		_pub_lpos.get().vx = xLP(X_vx); // north
+		_pub_lpos.get().vy = xLP(X_vy); // east
+		_pub_lpos.get().vz = xLP(X_vz); // down
 		_pub_lpos.get().yaw = _sub_att.get().yaw;
 		_pub_lpos.get().xy_global = _sub_home.get().timestamp != 0; // need home for reference
 		_pub_lpos.get().z_global = _baroInitialized;
@@ -574,9 +675,9 @@ void BlockLocalPositionEstimator::publishLocalPos()
 		_pub_lpos.get().ref_lon = _map_ref.lon_rad * 180 / M_PI;
 		_pub_lpos.get().ref_alt = _sub_home.get().alt;
 		_pub_lpos.get().dist_bottom = agl();
-		_pub_lpos.get().dist_bottom_rate = -_x(X_vz);
+		_pub_lpos.get().dist_bottom_rate = - xLP(X_vz);
 		_pub_lpos.get().surface_bottom_timestamp = _timeStamp;
-		_pub_lpos.get().dist_bottom_valid = _canEstimateZ;
+		_pub_lpos.get().dist_bottom_valid = _validTZ && _validZ;
 		_pub_lpos.get().eph = sqrtf(_P(X_x, X_x) + _P(X_y, X_y));
 		_pub_lpos.get().epv = sqrtf(_P(X_z, X_z));
 		_pub_lpos.update();
@@ -595,13 +696,13 @@ void BlockLocalPositionEstimator::publishEstimatorStatus()
 	_pub_est_status.get().n_states = n_x;
 	_pub_est_status.get().nan_flags = 0;
 	_pub_est_status.get().health_flags =
-		((_baroFault > fault_lvl_disable) << SENSOR_BARO)
-		+ ((_gpsFault > fault_lvl_disable) << SENSOR_GPS)
-		+ ((_lidarFault > fault_lvl_disable) << SENSOR_LIDAR)
-		+ ((_flowFault > fault_lvl_disable) << SENSOR_FLOW)
-		+ ((_sonarFault > fault_lvl_disable) << SENSOR_SONAR)
-		+ ((_visionFault > fault_lvl_disable) << SENSOR_VISION)
-		+ ((_mocapFault > fault_lvl_disable) << SENSOR_MOCAP);
+		((_baroFault > FAULT_NONE) << SENSOR_BARO)
+		+ ((_gpsFault > FAULT_NONE) << SENSOR_GPS)
+		+ ((_lidarFault > FAULT_NONE) << SENSOR_LIDAR)
+		+ ((_flowFault > FAULT_NONE) << SENSOR_FLOW)
+		+ ((_sonarFault > FAULT_NONE) << SENSOR_SONAR)
+		+ ((_visionFault > FAULT_NONE) << SENSOR_VISION)
+		+ ((_mocapFault > FAULT_NONE) << SENSOR_MOCAP);
 	_pub_est_status.get().timeout_flags =
 		(_baroInitialized << SENSOR_BARO)
 		+ (_gpsInitialized << SENSOR_GPS)
@@ -618,26 +719,27 @@ void BlockLocalPositionEstimator::publishGlobalPos()
 	// publish global position
 	double lat = 0;
 	double lon = 0;
-	map_projection_reproject(&_map_ref, _x(X_x), _x(X_y), &lat, &lon);
-	float alt = -_x(X_z) + _altHome;
+	const Vector<float, n_x> &xLP = _xLowPass.getState();
+	map_projection_reproject(&_map_ref, xLP(X_x), xLP(X_y), &lat, &lon);
+	float alt = -xLP(X_z) + _altHome;
 
 	if (PX4_ISFINITE(lat) && PX4_ISFINITE(lon) && PX4_ISFINITE(alt) &&
-	    PX4_ISFINITE(_x(X_vx)) && PX4_ISFINITE(_x(X_vy)) &&
-	    PX4_ISFINITE(_x(X_vz))) {
+	    PX4_ISFINITE(xLP(X_vx)) && PX4_ISFINITE(xLP(X_vy)) &&
+	    PX4_ISFINITE(xLP(X_vz))) {
 		_pub_gpos.get().timestamp = _timeStamp;
 		_pub_gpos.get().time_utc_usec = _sub_gps.get().time_utc_usec;
 		_pub_gpos.get().lat = lat;
 		_pub_gpos.get().lon = lon;
 		_pub_gpos.get().alt = alt;
-		_pub_gpos.get().vel_n = _x(X_vx);
-		_pub_gpos.get().vel_e = _x(X_vy);
-		_pub_gpos.get().vel_d = _x(X_vz);
+		_pub_gpos.get().vel_n = xLP(X_vx);
+		_pub_gpos.get().vel_e = xLP(X_vy);
+		_pub_gpos.get().vel_d = xLP(X_vz);
 		_pub_gpos.get().yaw = _sub_att.get().yaw;
 		_pub_gpos.get().eph = sqrtf(_P(X_x, X_x) + _P(X_y, X_y));
 		_pub_gpos.get().epv = sqrtf(_P(X_z, X_z));
-		_pub_gpos.get().terrain_alt = _altHome - _x(X_tz);
-		_pub_gpos.get().terrain_alt_valid = _canEstimateT;
-		_pub_gpos.get().dead_reckoning = !_canEstimateXY && !_xyTimeout;
+		_pub_gpos.get().terrain_alt = _altHome - xLP(X_tz);
+		_pub_gpos.get().terrain_alt_valid = _validTZ;
+		_pub_gpos.get().dead_reckoning = !_validXY && !_xyTimeout;
 		_pub_gpos.get().pressure_alt = _sub_sensor.get().baro_alt_meter[0];
 		_pub_gpos.update();
 	}
@@ -646,23 +748,95 @@ void BlockLocalPositionEstimator::publishGlobalPos()
 void BlockLocalPositionEstimator::initP()
 {
 	_P.setZero();
-	_P(X_x, X_x) = 1;
-	_P(X_y, X_y) = 1;
-	_P(X_z, X_z) = 1;
+	_P(X_x, X_x) = 2 * EST_STDDEV_XY_VALID; // initialize to twice valid condition
+	_P(X_y, X_y) = 2 * EST_STDDEV_XY_VALID;
+	_P(X_z, X_z) = 2 * EST_STDDEV_Z_VALID;
 	_P(X_vx, X_vx) = 1;
 	_P(X_vy, X_vy) = 1;
 	_P(X_vz, X_vz) = 1;
 	_P(X_bx, X_bx) = 1e-6;
 	_P(X_by, X_by) = 1e-6;
 	_P(X_bz, X_bz) = 1e-6;
-	_P(X_tz, X_tz) = 1;
+	_P(X_tz, X_tz) = 2 * EST_STDDEV_TZ_VALID;
+}
+
+void BlockLocalPositionEstimator::initSS()
+{
+	initP();
+
+	// dynamics matrix
+	_A.setZero();
+	// derivative of position is velocity
+	_A(X_x, X_vx) = 1;
+	_A(X_y, X_vy) = 1;
+	_A(X_z, X_vz) = 1;
+
+	// input matrix
+	_B.setZero();
+	_B(X_vx, U_ax) = 1;
+	_B(X_vy, U_ay) = 1;
+	_B(X_vz, U_az) = 1;
+
+	// update components that depend on current state
+	updateSSStates();
+	updateSSParams();
+}
+
+void BlockLocalPositionEstimator::updateSSStates()
+{
+	// derivative of velocity is accelerometer acceleration
+	// (in input matrix) - bias (in body frame)
+	Matrix3f R_att(_sub_att.get().R);
+	_A(X_vx, X_bx) = -R_att(0, 0);
+	_A(X_vx, X_by) = -R_att(0, 1);
+	_A(X_vx, X_bz) = -R_att(0, 2);
+
+	_A(X_vy, X_bx) = -R_att(1, 0);
+	_A(X_vy, X_by) = -R_att(1, 1);
+	_A(X_vy, X_bz) = -R_att(1, 2);
+
+	_A(X_vz, X_bx) = -R_att(2, 0);
+	_A(X_vz, X_by) = -R_att(2, 1);
+	_A(X_vz, X_bz) = -R_att(2, 2);
+}
+
+void BlockLocalPositionEstimator::updateSSParams()
+{
+	// input noise covariance matrix
+	_R.setZero();
+	_R(U_ax, U_ax) = _accel_xy_stddev.get() * _accel_xy_stddev.get();
+	_R(U_ay, U_ay) = _accel_xy_stddev.get() * _accel_xy_stddev.get();
+	_R(U_az, U_az) = _accel_z_stddev.get() * _accel_z_stddev.get();
+
+	// process noise power matrix
+	_Q.setZero();
+	float pn_p_sq = _pn_p_noise_density.get() * _pn_p_noise_density.get();
+	float pn_v_sq = _pn_v_noise_density.get() * _pn_v_noise_density.get();
+	_Q(X_x, X_x) = pn_p_sq;
+	_Q(X_y, X_y) = pn_p_sq;
+	_Q(X_z, X_z) = pn_p_sq;
+	_Q(X_vx, X_vx) = pn_v_sq;
+	_Q(X_vy, X_vy) = pn_v_sq;
+	_Q(X_vz, X_vz) = pn_v_sq;
+
+	// technically, the noise is in the body frame,
+	// but the components are all the same, so
+	// ignoring for now
+	float pn_b_sq = _pn_b_noise_density.get() * _pn_b_noise_density.get();
+	_Q(X_bx, X_bx) = pn_b_sq;
+	_Q(X_by, X_by) = pn_b_sq;
+	_Q(X_bz, X_bz) = pn_b_sq;
+
+	// terrain random walk noise
+	float pn_t_sq = _pn_t_noise_density.get() * _pn_t_noise_density.get();
+	_Q(X_tz, X_tz) = pn_t_sq;
 }
 
 void BlockLocalPositionEstimator::predict()
 {
 	// if can't update anything, don't propagate
 	// state or covariance
-	if (!_canEstimateXY && !_canEstimateZ) { return; }
+	if (!_validXY && !_validZ) { return; }
 
 	if (_integrate.get() && _sub_att.get().R_valid) {
 		Matrix3f R_att(_sub_att.get().R);
@@ -674,86 +848,26 @@ void BlockLocalPositionEstimator::predict()
 		_u = Vector3f(0, 0, 0);
 	}
 
-	// dynamics matrix
-	Matrix<float, n_x, n_x>  A; // state dynamics matrix
-	A.setZero();
-	// derivative of position is velocity
-	A(X_x, X_vx) = 1;
-	A(X_y, X_vy) = 1;
-	A(X_z, X_vz) = 1;
-
-	// derivative of velocity is accelerometer acceleration
-	// (in input matrix) - bias (in body frame)
-	Matrix3f R_att(_sub_att.get().R);
-	A(X_vx, X_bx) = -R_att(0, 0);
-	A(X_vx, X_by) = -R_att(0, 1);
-	A(X_vx, X_bz) = -R_att(0, 2);
-
-	A(X_vy, X_bx) = -R_att(1, 0);
-	A(X_vy, X_by) = -R_att(1, 1);
-	A(X_vy, X_bz) = -R_att(1, 2);
-
-	A(X_vz, X_bx) = -R_att(2, 0);
-	A(X_vz, X_by) = -R_att(2, 1);
-	A(X_vz, X_bz) = -R_att(2, 2);
-
-	// input matrix
-	Matrix<float, n_x, n_u>  B; // input matrix
-	B.setZero();
-	B(X_vx, U_ax) = 1;
-	B(X_vy, U_ay) = 1;
-	B(X_vz, U_az) = 1;
-
-	// input noise covariance matrix
-	Matrix<float, n_u, n_u> R;
-	R.setZero();
-	R(U_ax, U_ax) = _accel_xy_stddev.get() * _accel_xy_stddev.get();
-	R(U_ay, U_ay) = _accel_xy_stddev.get() * _accel_xy_stddev.get();
-	R(U_az, U_az) = _accel_z_stddev.get() * _accel_z_stddev.get();
-
-	// process noise power matrix
-	Matrix<float, n_x, n_x>  Q;
-	Q.setZero();
-	float pn_p_sq = _pn_p_noise_density.get() * _pn_p_noise_density.get();
-	float pn_v_sq = _pn_v_noise_density.get() * _pn_v_noise_density.get();
-	Q(X_x, X_x) = pn_p_sq;
-	Q(X_y, X_y) = pn_p_sq;
-	Q(X_z, X_z) = pn_p_sq;
-	Q(X_vx, X_vx) = pn_v_sq;
-	Q(X_vy, X_vy) = pn_v_sq;
-	Q(X_vz, X_vz) = pn_v_sq;
-
-	// technically, the noise is in the body frame,
-	// but the components are all the same, so
-	// ignoring for now
-	float pn_b_sq = _pn_b_noise_density.get() * _pn_b_noise_density.get();
-	Q(X_bx, X_bx) = pn_b_sq;
-	Q(X_by, X_by) = pn_b_sq;
-	Q(X_bz, X_bz) = pn_b_sq;
-
-	// terrain random walk noise
-	float pn_t_sq = _pn_t_noise_density.get() * _pn_t_noise_density.get();
-	Q(X_tz, X_tz) = pn_t_sq;
+	// update state space based on new states
+	updateSSStates();
 
 	// continuous time kalman filter prediction
-	Vector<float, n_x> dx = (A * _x + B * _u) * getDt();
-
-	// only predict for components we have
-	// valid measurements for
-	if (!_canEstimateXY) {
-		dx(X_x) = 0;
-		dx(X_y) = 0;
-		dx(X_vx) = 0;
-		dx(X_vy) = 0;
-	}
-
-	if (!_canEstimateZ) {
-		dx(X_z) = 0;
-		dx(X_vz) = 0;
-	}
+	// integrate runge kutta 4th order
+	// TODO move rk4 algorithm to matrixlib
+	// https://en.wikipedia.org/wiki/Runge%E2%80%93Kutta_methods
+	float h = getDt();
+	Vector<float, n_x> k1, k2, k3, k4;
+	k1 = dynamics(0, _x, _u);
+	k2 = dynamics(h / 2, _x + k1 * h / 2, _u);
+	k3 = dynamics(h / 2, _x + k2 * h / 2, _u);
+	k4 = dynamics(h, _x + k3 * h, _u);
+	Vector<float, n_x> dx = (k1 + k2 * 2 + k3 * 2 + k4) * (h / 6);
 
 	// propagate
+	correctionLogic(dx);
 	_x += dx;
-	_P += (A * _P + _P * A.transpose() +
-	       B * R * B.transpose() + Q) * getDt();
+	_P += (_A * _P + _P * _A.transpose() +
+	       _B * _R * _B.transpose() +
+	       _Q) * getDt();
+	_xLowPass.update(_x);
 }
diff --git a/src/modules/local_position_estimator/BlockLocalPositionEstimator.hpp b/src/modules/local_position_estimator/BlockLocalPositionEstimator.hpp
index eba9d61355..92a3a9fc14 100644
--- a/src/modules/local_position_estimator/BlockLocalPositionEstimator.hpp
+++ b/src/modules/local_position_estimator/BlockLocalPositionEstimator.hpp
@@ -33,8 +33,9 @@
 using namespace matrix;
 using namespace control;
 
-static const float GPS_DELAY_MAX = 0.5; // seconds
-static const float HIST_STEP = 0.05; // 20 hz
+static const float GPS_DELAY_MAX = 0.5f; // seconds
+static const float HIST_STEP = 0.05f; // 20 hz
+static const float BIAS_MAX = 1e-1f;
 static const size_t HIST_LEN = 10; // GPS_DELAY_MAX / HIST_STEP;
 static const size_t N_DIST_SUBS = 4;
 
@@ -134,6 +135,10 @@ public:
 
 	BlockLocalPositionEstimator();
 	void update();
+	Vector<float, n_x> dynamics(
+		float t,
+		const Vector<float, n_x> &x,
+		const Vector<float, n_u> &u);
 	virtual ~BlockLocalPositionEstimator();
 
 private:
@@ -144,6 +149,9 @@ private:
 	// methods
 	// ----------------------------
 	void initP();
+	void initSS();
+	void updateSSStates();
+	void updateSSParams();
 
 	// predict the next state
 	void predict();
@@ -195,6 +203,7 @@ private:
 
 	// misc
 	float agl();
+	void correctionLogic(Vector<float, n_x> &dx);
 	void detectDistanceSensors();
 	void updateHome();
 
@@ -207,11 +216,8 @@ private:
 	// ----------------------------
 
 	// subscriptions
-	uORB::Subscription<vehicle_status_s> _sub_status;
 	uORB::Subscription<actuator_armed_s> _sub_armed;
-	uORB::Subscription<vehicle_control_mode_s> _sub_control_mode;
 	uORB::Subscription<vehicle_attitude_s> _sub_att;
-	uORB::Subscription<vehicle_attitude_setpoint_s> _sub_att_sp;
 	uORB::Subscription<optical_flow_s> _sub_flow;
 	uORB::Subscription<sensor_combined_s> _sub_sensor;
 	uORB::Subscription<parameter_update_s> _sub_param_update;
@@ -302,6 +308,9 @@ private:
 	BlockStats<float, n_y_mocap> _mocapStats;
 	BlockStats<double, n_y_gps> _gpsStats;
 
+	// low pass
+	BlockLowPassVector<float, n_x> _xLowPass;
+
 	// delay blocks
 	BlockDelay<float, n_x, 1, HIST_LEN> _xDelay;
 	BlockDelay<uint64_t, 1, 1, HIST_LEN> _tDelay;
@@ -346,9 +355,9 @@ private:
 	float _flowMeanQual;
 
 	// status
-	bool _canEstimateXY;
-	bool _canEstimateZ;
-	bool _canEstimateT;
+	bool _validXY;
+	bool _validZ;
+	bool _validTZ;
 	bool _xyTimeout;
 	bool _zTimeout;
 	bool _tzTimeout;
@@ -372,4 +381,8 @@ private:
 	Vector<float, n_x>  _x; // state vector
 	Vector<float, n_u>  _u; // input vector
 	Matrix<float, n_x, n_x>  _P; // state covariance matrix
+	Matrix<float, n_x, n_x>  _A; // dynamics matrix
+	Matrix<float, n_x, n_u>  _B; // input matrix
+	Matrix<float, n_u, n_u>  _R; // input covariance
+	Matrix<float, n_x, n_x>  _Q; // process noise covariance
 };
diff --git a/src/modules/local_position_estimator/CMakeLists.txt b/src/modules/local_position_estimator/CMakeLists.txt
index 7b118a18d0..1421be5969 100644
--- a/src/modules/local_position_estimator/CMakeLists.txt
+++ b/src/modules/local_position_estimator/CMakeLists.txt
@@ -35,7 +35,7 @@ px4_add_module(
 	MAIN local_position_estimator
 	COMPILE_FLAGS -Os
 	STACK_MAIN 5700
-	STACK_MAX 10000
+	STACK_MAX 13000
 	SRCS
 		local_position_estimator_main.cpp
 		BlockLocalPositionEstimator.cpp
diff --git a/src/modules/local_position_estimator/local_position_estimator_main.cpp b/src/modules/local_position_estimator/local_position_estimator_main.cpp
index 27256fc8e7..4ba3946076 100644
--- a/src/modules/local_position_estimator/local_position_estimator_main.cpp
+++ b/src/modules/local_position_estimator/local_position_estimator_main.cpp
@@ -112,7 +112,7 @@ int local_position_estimator_main(int argc, char *argv[])
 		deamon_task = px4_task_spawn_cmd("lp_estimator",
 						 SCHED_DEFAULT,
 						 SCHED_PRIORITY_MAX - 5,
-						 12000,
+						 13000,
 						 local_position_estimator_thread_main,
 						 (argv && argc > 2) ? (char *const *) &argv[2] : (char *const *) NULL);
 		return 0;
diff --git a/src/modules/local_position_estimator/params.c b/src/modules/local_position_estimator/params.c
index 4b85db9447..fb8fdd6172 100644
--- a/src/modules/local_position_estimator/params.c
+++ b/src/modules/local_position_estimator/params.c
@@ -88,34 +88,32 @@ PARAM_DEFINE_FLOAT(LPE_LDR_Z, 0.03f);
 PARAM_DEFINE_FLOAT(LPE_LDR_OFF_Z, 0.00f);
 
 /**
- * Accelerometer xy standard deviation
+ * Accelerometer xy noise density
  *
- * Data sheet sqrt(Noise power) = 150ug/sqrt(Hz)
- * std dev = (150*9.8*1e-6)*sqrt(1000 Hz) m/s^2
- * Since accels sampled at 1000 Hz.
+ * Data sheet noise density = 150ug/sqrt(Hz) = 0.0015 m/s^2/sqrt(Hz)
  *
- * should be 0.0464
+ * Larger than data sheet to account for tilt error.
  *
  * @group Local Position Estimator
- * @unit m/s^2
+ * @unit m/s^2/srqt(Hz)
  * @min 0.00001
  * @max 2
  * @decimal 4
  */
-PARAM_DEFINE_FLOAT(LPE_ACC_XY, 0.0454f);
+PARAM_DEFINE_FLOAT(LPE_ACC_XY, 0.0015f);
 
 /**
- * Accelerometer z standard deviation
+ * Accelerometer z noise density
  *
- * (see Accel x comments)
+ * Data sheet noise density = 150ug/sqrt(Hz) = 0.0015 m/s^2/sqrt(Hz)
  *
  * @group Local Position Estimator
- * @unit m/s^2
+ * @unit m/s^2/srqt(Hz)
  * @min 0.00001
  * @max 2
  * @decimal 4
  */
-PARAM_DEFINE_FLOAT(LPE_ACC_Z, 0.0454f);
+PARAM_DEFINE_FLOAT(LPE_ACC_Z, 0.0015f);
 
 /**
  * Barometric presssure altitude z standard deviation.
@@ -145,11 +143,11 @@ PARAM_DEFINE_INT32(LPE_GPS_ON, 1);
  * @max 0.4
  * @decimal 2
  */
-PARAM_DEFINE_FLOAT(LPE_GPS_DELAY, 0.25f);
+PARAM_DEFINE_FLOAT(LPE_GPS_DELAY, 0.29f);
 
 
 /**
- * GPS xy standard deviation.
+ * Minimum GPS xy standard deviation, uses reported EPH if greater.
  *
  * @group Local Position Estimator
  * @unit m
@@ -157,10 +155,10 @@ PARAM_DEFINE_FLOAT(LPE_GPS_DELAY, 0.25f);
  * @max 5
  * @decimal 2
  */
-PARAM_DEFINE_FLOAT(LPE_GPS_XY, 2.0f);
+PARAM_DEFINE_FLOAT(LPE_GPS_XY, 1.0f);
 
 /**
- * GPS z standard deviation.
+ * Minimum GPS z standard deviation, uses reported EPV if greater.
  *
  * @group Local Position Estimator
  * @unit m
@@ -168,10 +166,11 @@ PARAM_DEFINE_FLOAT(LPE_GPS_XY, 2.0f);
  * @max 200
  * @decimal 2
  */
-PARAM_DEFINE_FLOAT(LPE_GPS_Z, 100.0f);
+PARAM_DEFINE_FLOAT(LPE_GPS_Z, 3.0f);
 
 /**
  * GPS xy velocity standard deviation.
+ * EPV used if greater than this value.
  *
  * @group Local Position Estimator
  * @unit m/s
@@ -193,7 +192,7 @@ PARAM_DEFINE_FLOAT(LPE_GPS_VXY, 0.25f);
 PARAM_DEFINE_FLOAT(LPE_GPS_VZ, 0.25f);
 
 /**
- * GPS max eph
+ * Max EPH allowed for GPS initialization
  *
  * @group Local Position Estimator
  * @unit m
@@ -204,7 +203,7 @@ PARAM_DEFINE_FLOAT(LPE_GPS_VZ, 0.25f);
 PARAM_DEFINE_FLOAT(LPE_EPH_MAX, 3.0f);
 
 /**
- * GPS max epv
+ * Max EPV allowed for GPS initialization
  *
  * @group Local Position Estimator
  * @unit m
@@ -258,24 +257,30 @@ PARAM_DEFINE_FLOAT(LPE_VIC_P, 0.05f);
 /**
  * Position propagation noise density
  *
+ * Increase to trust measurements more.
+ * Decrease to trust model more.
+ *
  * @group Local Position Estimator
  * @unit m/s/sqrt(Hz)
  * @min 0
  * @max 1
  * @decimal 8
  */
-PARAM_DEFINE_FLOAT(LPE_PN_P, 0.0f);
+PARAM_DEFINE_FLOAT(LPE_PN_P, 0.1f);
 
 /**
  * Velocity propagation noise density
  *
+ * Increase to trust measurements more.
+ * Decrease to trust model more.
+ *
  * @group Local Position Estimator
  * @unit (m/s)/s/sqrt(Hz)
  * @min 0
  * @max 1
  * @decimal 8
  */
-PARAM_DEFINE_FLOAT(LPE_PN_V, 0.0f);
+PARAM_DEFINE_FLOAT(LPE_PN_V, 0.1f);
 
 /**
  * Accel bias propagation noise density
@@ -332,3 +337,14 @@ PARAM_DEFINE_FLOAT(LPE_LAT, 40.430f);
  */
 PARAM_DEFINE_FLOAT(LPE_LON, -86.929);
 
+/**
+ * Cut frequency for state publication
+ *
+ * @group Local Position Estimator
+ * @unit Hz
+ * @min 5
+ * @max 1000
+ * @decimal 0
+ */
+PARAM_DEFINE_FLOAT(LPE_X_LP, 5.0);
+
diff --git a/src/modules/local_position_estimator/sensors/baro.cpp b/src/modules/local_position_estimator/sensors/baro.cpp
index 351a134696..57fc87eb5f 100644
--- a/src/modules/local_position_estimator/sensors/baro.cpp
+++ b/src/modules/local_position_estimator/sensors/baro.cpp
@@ -76,8 +76,8 @@ void BlockLocalPositionEstimator::baroCorrect()
 
 	if (beta > BETA_TABLE[n_y_baro]) {
 		if (_baroFault < FAULT_MINOR) {
-			//mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] baro fault, r %5.2f m, beta %5.2f",
-			//double(r(0)), double(beta));
+			mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] baro fault, r %5.2f m, beta %5.2f",
+						     double(r(0)), double(beta));
 			_baroFault = FAULT_MINOR;
 		}
 
@@ -90,14 +90,7 @@ void BlockLocalPositionEstimator::baroCorrect()
 	if (_baroFault < fault_lvl_disable) {
 		Matrix<float, n_x, n_y_baro> K = _P * C.transpose() * S_I;
 		Vector<float, n_x> dx = K * r;
-
-		if (!_canEstimateXY) {
-			dx(X_x) = 0;
-			dx(X_y) = 0;
-			dx(X_vx) = 0;
-			dx(X_vy) = 0;
-		}
-
+		correctionLogic(dx);
 		_x += dx;
 		_P -= K * C * _P;
 	}
diff --git a/src/modules/local_position_estimator/sensors/flow.cpp b/src/modules/local_position_estimator/sensors/flow.cpp
index 31f29a1487..879da54b0a 100644
--- a/src/modules/local_position_estimator/sensors/flow.cpp
+++ b/src/modules/local_position_estimator/sensors/flow.cpp
@@ -49,7 +49,7 @@ int BlockLocalPositionEstimator::flowMeasure(Vector<float, n_y_flow> &y)
 	}
 
 	// calculate range to center of image for flow
-	if (!_canEstimateT) {
+	if (!_validTZ) {
 		return -1;
 	}
 
@@ -153,7 +153,9 @@ void BlockLocalPositionEstimator::flowCorrect()
 	if (_flowFault < fault_lvl_disable) {
 		Matrix<float, n_x, n_y_flow> K =
 			_P * C.transpose() * S_I;
-		_x += K * r;
+		Vector<float, n_x> dx = K * r;
+		correctionLogic(dx);
+		_x += dx;
 		_P -= K * C * _P;
 
 	} else {
diff --git a/src/modules/local_position_estimator/sensors/gps.cpp b/src/modules/local_position_estimator/sensors/gps.cpp
index 617849b50f..e08119dddb 100644
--- a/src/modules/local_position_estimator/sensors/gps.cpp
+++ b/src/modules/local_position_estimator/sensors/gps.cpp
@@ -8,11 +8,25 @@ extern orb_advert_t mavlink_log_pub;
 // to initialize
 static const uint32_t 		REQ_GPS_INIT_COUNT = 10;
 static const uint32_t 		GPS_TIMEOUT =      1000000; // 1.0 s
-static const float 			GPS_EPH_MIN =      2.0; // m, min allowed by gps self reporting
-static const float 			GPS_EPV_MIN =      2.0; // m, min allowed by gps self reporting
 
 void BlockLocalPositionEstimator::gpsInit()
 {
+	// check for good gps signal
+	uint8_t nSat = _sub_gps.get().satellites_used;
+	float eph = _sub_gps.get().eph;
+	float epv = _sub_gps.get().epv;
+	uint8_t fix_type = _sub_gps.get().fix_type;
+
+	if (
+		nSat < 6 ||
+		eph > _gps_eph_max.get() ||
+		epv > _gps_epv_max.get() ||
+		fix_type < 3
+	) {
+		_gpsStats.reset();
+		return;
+	}
+
 	// measure
 	Vector<double, n_y_gps> y;
 
@@ -50,21 +64,6 @@ void BlockLocalPositionEstimator::gpsInit()
 
 int BlockLocalPositionEstimator::gpsMeasure(Vector<double, n_y_gps> &y)
 {
-	// check for good gps signal
-	uint8_t nSat = _sub_gps.get().satellites_used;
-	float eph = _sub_gps.get().eph;
-	float epv = _sub_gps.get().epv;
-	uint8_t fix_type = _sub_gps.get().fix_type;
-
-	if (
-		nSat < 6 ||
-		eph > _gps_eph_max.get() ||
-		epv > _gps_epv_max.get() ||
-		fix_type < 3
-	) {
-		return -1;
-	}
-
 	// gps measurement
 	y.setZero();
 	y(0) = _sub_gps.get().lat * 1e-7;
@@ -125,11 +124,11 @@ void BlockLocalPositionEstimator::gpsCorrect()
 	float var_vz = _gps_vz_stddev.get() * _gps_vz_stddev.get();
 
 	// if field is not below minimum, set it to the value provided
-	if (_sub_gps.get().eph > GPS_EPH_MIN) {
+	if (_sub_gps.get().eph > _gps_xy_stddev.get()) {
 		var_xy = _sub_gps.get().eph * _sub_gps.get().eph;
 	}
 
-	if (_sub_gps.get().epv > GPS_EPV_MIN) {
+	if (_sub_gps.get().epv > _gps_z_stddev.get()) {
 		var_z = _sub_gps.get().epv * _sub_gps.get().epv;
 	}
 
@@ -170,13 +169,9 @@ void BlockLocalPositionEstimator::gpsCorrect()
 
 	if (beta > BETA_TABLE[n_y_gps]) {
 		if (_gpsFault < FAULT_MINOR) {
-			//mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] gps fault, beta: %5.2f", double(beta));
-			//mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] r: %5.2f %5.2f %5.2f %5.2f %5.2f %5.2f",
-			//double(r(0)),  double(r(1)), double(r(2)),
-			//double(r(3)), double(r(4)), double(r(5)));
-			//mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] S_I: %5.2f %5.2f %5.2f %5.2f %5.2f %5.2f",
-			//double(S_I(0, 0)),  double(S_I(1, 1)), double(S_I(2, 2)),
-			//double(S_I(3, 3)),  double(S_I(4, 4)), double(S_I(5, 5)));
+			mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] gps fault %3g %3g %3g %3g %3g %3g",
+						     double(r(0)*r(0) / S_I(0, 0)),  double(r(1)*r(1) / S_I(1, 1)), double(r(2)*r(2) / S_I(2, 2)),
+						     double(r(3)*r(3) / S_I(3, 3)),  double(r(4)*r(4) / S_I(4, 4)), double(r(5)*r(5) / S_I(5, 5)));
 			_gpsFault = FAULT_MINOR;
 		}
 
@@ -188,7 +183,9 @@ void BlockLocalPositionEstimator::gpsCorrect()
 	// kalman filter correction if no hard fault
 	if (_gpsFault < fault_lvl_disable) {
 		Matrix<float, n_x, n_y_gps> K = _P * C.transpose() * S_I;
-		_x += K * r;
+		Vector<float, n_x> dx = K * r;
+		correctionLogic(dx);
+		_x += dx;
 		_P -= K * C * _P;
 	}
 }
diff --git a/src/modules/local_position_estimator/sensors/lidar.cpp b/src/modules/local_position_estimator/sensors/lidar.cpp
index ecbdb4d733..67f623e946 100644
--- a/src/modules/local_position_estimator/sensors/lidar.cpp
+++ b/src/modules/local_position_estimator/sensors/lidar.cpp
@@ -109,14 +109,7 @@ void BlockLocalPositionEstimator::lidarCorrect()
 	if (_lidarFault < fault_lvl_disable) {
 		Matrix<float, n_x, n_y_lidar> K = _P * C.transpose() * S_I;
 		Vector<float, n_x> dx = K * r;
-
-		if (!_canEstimateXY) {
-			dx(X_x) = 0;
-			dx(X_y) = 0;
-			dx(X_vx) = 0;
-			dx(X_vy) = 0;
-		}
-
+		correctionLogic(dx);
 		_x += dx;
 		_P -= K * C * _P;
 	}
diff --git a/src/modules/local_position_estimator/sensors/mocap.cpp b/src/modules/local_position_estimator/sensors/mocap.cpp
index f3c26e4fde..8319fc403f 100644
--- a/src/modules/local_position_estimator/sensors/mocap.cpp
+++ b/src/modules/local_position_estimator/sensors/mocap.cpp
@@ -98,7 +98,9 @@ void BlockLocalPositionEstimator::mocapCorrect()
 	// kalman filter correction if no fault
 	if (_mocapFault < fault_lvl_disable) {
 		Matrix<float, n_x, n_y_mocap> K = _P * C.transpose() * S_I;
-		_x += K * r;
+		Vector<float, n_x> dx = K * r;
+		correctionLogic(dx);
+		_x += dx;
 		_P -= K * C * _P;
 	}
 }
diff --git a/src/modules/local_position_estimator/sensors/sonar.cpp b/src/modules/local_position_estimator/sensors/sonar.cpp
index d95ad5e346..e78dc45b0e 100644
--- a/src/modules/local_position_estimator/sensors/sonar.cpp
+++ b/src/modules/local_position_estimator/sensors/sonar.cpp
@@ -127,14 +127,7 @@ void BlockLocalPositionEstimator::sonarCorrect()
 		Matrix<float, n_x, n_y_sonar> K =
 			_P * C.transpose() * S_I;
 		Vector<float, n_x> dx = K * r;
-
-		if (!_canEstimateXY) {
-			dx(X_x) = 0;
-			dx(X_y) = 0;
-			dx(X_vx) = 0;
-			dx(X_vy) = 0;
-		}
-
+		correctionLogic(dx);
 		_x += dx;
 		_P -= K * C * _P;
 	}
diff --git a/src/modules/local_position_estimator/sensors/vision.cpp b/src/modules/local_position_estimator/sensors/vision.cpp
index 1b3c7ddbca..4a4fa6a2d4 100644
--- a/src/modules/local_position_estimator/sensors/vision.cpp
+++ b/src/modules/local_position_estimator/sensors/vision.cpp
@@ -96,7 +96,9 @@ void BlockLocalPositionEstimator::visionCorrect()
 	// kalman filter correction if no fault
 	if (_visionFault <  fault_lvl_disable) {
 		Matrix<float, n_x, n_y_vision> K = _P * C.transpose() * S_I;
-		_x += K * r;
+		Vector<float, n_x> dx = K * r;
+		correctionLogic(dx);
+		_x += dx;
 		_P -= K * C * _P;
 	}
 }
diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c
index 7543560eac..cd17ae121c 100644
--- a/src/modules/sdlog2/sdlog2.c
+++ b/src/modules/sdlog2/sdlog2.c
@@ -2138,6 +2138,7 @@ int sdlog2_thread_main(int argc, char *argv[])
 				memcpy(&(log_msg.body.log_EST2.cov), buf.estimator_status.covariances, maxcopy2);
 				log_msg.body.log_EST2.gps_check_fail_flags = buf.estimator_status.gps_check_fail_flags;
 				log_msg.body.log_EST2.control_mode_flags = buf.estimator_status.control_mode_flags;
+				log_msg.body.log_EST2.health_flags = buf.estimator_status.health_flags;
 				LOGBUFFER_WRITE_AND_COUNT(EST2);
 
 				log_msg.msg_type = LOG_EST3_MSG;
diff --git a/src/modules/sdlog2/sdlog2_messages.h b/src/modules/sdlog2/sdlog2_messages.h
index a6a253f921..58ae693b64 100644
--- a/src/modules/sdlog2/sdlog2_messages.h
+++ b/src/modules/sdlog2/sdlog2_messages.h
@@ -410,9 +410,10 @@ struct log_EST1_s {
 /* --- EST2 - ESTIMATOR STATUS --- */
 #define LOG_EST2_MSG 34
 struct log_EST2_s {
-    float cov[12];
-    uint16_t gps_check_fail_flags;
-    uint16_t control_mode_flags;
+	float cov[12];
+	uint16_t gps_check_fail_flags;
+	uint16_t control_mode_flags;
+	uint8_t health_flags;
 };
 
 /* --- EST3 - ESTIMATOR STATUS --- */
@@ -687,7 +688,7 @@ static const struct log_format_s log_formats[] = {
 	LOG_FORMAT_S(TEL3, TEL, "BBBBHHBQ",		"RSSI,RemRSSI,Noise,RemNoise,RXErr,Fixed,TXBuf,HbTime"),
 	LOG_FORMAT(EST0, "ffffffffffffBBHB",	"s0,s1,s2,s3,s4,s5,s6,s7,s8,s9,s10,s11,nStat,fNaN,fFault,fTOut"),
 	LOG_FORMAT(EST1, "ffffffffffffffff",	"s12,s13,s14,s15,s16,s17,s18,s19,s20,s21,s22,s23,s24,s25,s26,s27"),
-	LOG_FORMAT(EST2, "ffffffffffffHH",    "P0,P1,P2,P3,P4,P5,P6,P7,P8,P9,P10,P11,GCHK,CTRL"),
+	LOG_FORMAT(EST2, "ffffffffffffHHB",    "P0,P1,P2,P3,P4,P5,P6,P7,P8,P9,P10,P11,GCHK,CTRL,fHealth"),
 	LOG_FORMAT(EST3, "ffffffffffffffff",    "P12,P13,P14,P15,P16,P17,P18,P19,P20,P21,P22,P23,P24,P25,P26,P27"),
 	LOG_FORMAT(EST4, "ffffffffffff", "VxI,VyI,VzI,PxI,PyI,PzI,VxIV,VyIV,VzIV,PxIV,PyIV,PzIV"),
 	LOG_FORMAT(EST5, "ffffffffff", "MAGxI,MAGyI,MAGzI,MAGxIV,MAGyIV,MAGzIV,HeadI,HeadIV,AirI,AirIV"),

From 6268cdc86a227f37a9d13af702f155e44921c51b Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Fri, 1 Jul 2016 18:03:23 +0200
Subject: [PATCH 0870/1230] Solo is just a normal X quad

---
 ROMFS/px4fmu_common/init.d/4030_solo     | 2 +-
 ROMFS/px4fmu_common/mixers/solo.main.mix | 7 -------
 2 files changed, 1 insertion(+), 8 deletions(-)
 delete mode 100644 ROMFS/px4fmu_common/mixers/solo.main.mix

diff --git a/ROMFS/px4fmu_common/init.d/4030_solo b/ROMFS/px4fmu_common/init.d/4030_solo
index c2462ef4e6..77661c8596 100644
--- a/ROMFS/px4fmu_common/init.d/4030_solo
+++ b/ROMFS/px4fmu_common/init.d/4030_solo
@@ -79,7 +79,7 @@ then
     param set RC5_TRIM          1500
 fi
 
-set MIXER solo
+set MIXER quad_x
 
 set PWM_OUT 1234
 set MIXER_AUX none
diff --git a/ROMFS/px4fmu_common/mixers/solo.main.mix b/ROMFS/px4fmu_common/mixers/solo.main.mix
deleted file mode 100644
index 12a3bee20c..0000000000
--- a/ROMFS/px4fmu_common/mixers/solo.main.mix
+++ /dev/null
@@ -1,7 +0,0 @@
-Multirotor mixer for PX4FMU
-===========================
-
-This file defines a single mixer for a quadrotor in the X configuration.  All controls
-are mixed 100%.
-
-R: 4x 10000 10000 10000 0

From 37edb43b6060bc9e7285e4a8b66baf08f6e2fe09 Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Fri, 1 Jul 2016 18:03:38 +0200
Subject: [PATCH 0871/1230] ROMFS: Strip README files

---
 Tools/px_romfs_pruner.py | 5 +++++
 1 file changed, 5 insertions(+)

diff --git a/Tools/px_romfs_pruner.py b/Tools/px_romfs_pruner.py
index fa48019bd0..d6481e03ed 100644
--- a/Tools/px_romfs_pruner.py
+++ b/Tools/px_romfs_pruner.py
@@ -68,6 +68,11 @@ def main():
                 os.remove(file_path)
                 continue
 
+            # delete documentation
+            if file.startswith("README"):
+                os.remove(file_path)
+                continue
+
             # only prune text files
             if ".zip" in file or ".bin" in file or ".swp" in file \
                     or ".data" in file or ".DS_Store" in file:

From 82b2fa5ecbe97bc6393d9bd0aa5f7b408c5d3ef0 Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Fri, 1 Jul 2016 18:04:09 +0200
Subject: [PATCH 0872/1230] Commander should not depend on MAVLink

---
 src/modules/commander/commander.cpp | 28 ++++++++++++++++++++--------
 1 file changed, 20 insertions(+), 8 deletions(-)

diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp
index d543afc33c..81ccc4e852 100644
--- a/src/modules/commander/commander.cpp
+++ b/src/modules/commander/commander.cpp
@@ -104,7 +104,6 @@
 #include <uORB/topics/vehicle_local_position.h>
 #include <uORB/topics/vtol_vehicle_status.h>
 #include <uORB/uORB.h>
-#include <v2.0/common/mavlink.h>
 
 /* oddly, ERROR is not defined for c++ */
 #ifdef ERROR
@@ -112,6 +111,19 @@
 #endif
 static const int ERROR = -1;
 
+typedef enum VEHICLE_MODE_FLAG
+{
+	VEHICLE_MODE_FLAG_CUSTOM_MODE_ENABLED=1, /* 0b00000001 Reserved for future use. | */
+	VEHICLE_MODE_FLAG_TEST_ENABLED=2, /* 0b00000010 system has a test mode enabled. This flag is intended for temporary system tests and should not be used for stable implementations. | */
+	VEHICLE_MODE_FLAG_AUTO_ENABLED=4, /* 0b00000100 autonomous mode enabled, system finds its own goal positions. Guided flag can be set or not, depends on the actual implementation. | */
+	VEHICLE_MODE_FLAG_GUIDED_ENABLED=8, /* 0b00001000 guided mode enabled, system flies MISSIONs / mission items. | */
+	VEHICLE_MODE_FLAG_STABILIZE_ENABLED=16, /* 0b00010000 system stabilizes electronically its attitude (and optionally position). It needs however further control inputs to move around. | */
+	VEHICLE_MODE_FLAG_HIL_ENABLED=32, /* 0b00100000 hardware in the loop simulation. All motors / actuators are blocked, but internal software is full operational. | */
+	VEHICLE_MODE_FLAG_MANUAL_INPUT_ENABLED=64, /* 0b01000000 remote control input is enabled. | */
+	VEHICLE_MODE_FLAG_SAFETY_ARMED=128, /* 0b10000000 MAV safety set to armed. Motors are enabled / running / can start. Ready to fly. | */
+	VEHICLE_MODE_FLAG_ENUM_END=129, /*  | */
+} VEHICLE_MODE_FLAG;
+
 static constexpr uint8_t COMMANDER_MAX_GPS_NOISE = 60;		/**< Maximum percentage signal to noise ratio allowed for GPS reception */
 
 /* Decouple update interval and hysteresis counters, all depends on intervals */
@@ -688,11 +700,11 @@ bool handle_command(struct vehicle_status_s *status_local, const struct safety_s
 			transition_result_t main_ret = TRANSITION_NOT_CHANGED;
 
 			/* set HIL state */
-			hil_state_t new_hil_state = (base_mode & MAV_MODE_FLAG_HIL_ENABLED) ? vehicle_status_s::HIL_STATE_ON : vehicle_status_s::HIL_STATE_OFF;
+			hil_state_t new_hil_state = (base_mode & VEHICLE_MODE_FLAG_HIL_ENABLED) ? vehicle_status_s::HIL_STATE_ON : vehicle_status_s::HIL_STATE_OFF;
 			transition_result_t hil_ret = hil_state_transition(new_hil_state, status_pub, status_local, &mavlink_log_pub);
 
 			// Transition the arming state
-			bool cmd_arm = base_mode & MAV_MODE_FLAG_SAFETY_ARMED;
+			bool cmd_arm = base_mode & VEHICLE_MODE_FLAG_SAFETY_ARMED;
 
 			arming_ret = arm_disarm(cmd_arm, &mavlink_log_pub, "set mode command");
 
@@ -703,7 +715,7 @@ bool handle_command(struct vehicle_status_s *status_local, const struct safety_s
 				commander_set_home_position(*home_pub, *home, *local_pos, *global_pos, *attitude);
 			}
 
-			if (base_mode & MAV_MODE_FLAG_CUSTOM_MODE_ENABLED) {
+			if (base_mode & VEHICLE_MODE_FLAG_CUSTOM_MODE_ENABLED) {
 				/* use autopilot-specific mode */
 				if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_MANUAL) {
 					/* MANUAL */
@@ -769,16 +781,16 @@ bool handle_command(struct vehicle_status_s *status_local, const struct safety_s
 
 			} else {
 				/* use base mode */
-				if (base_mode & MAV_MODE_FLAG_AUTO_ENABLED) {
+				if (base_mode & VEHICLE_MODE_FLAG_AUTO_ENABLED) {
 					/* AUTO */
 					main_ret = main_state_transition(status_local, commander_state_s::MAIN_STATE_AUTO_MISSION, main_state_prev, &status_flags, &internal_state);
 
-				} else if (base_mode & MAV_MODE_FLAG_MANUAL_INPUT_ENABLED) {
-					if (base_mode & MAV_MODE_FLAG_GUIDED_ENABLED) {
+				} else if (base_mode & VEHICLE_MODE_FLAG_MANUAL_INPUT_ENABLED) {
+					if (base_mode & VEHICLE_MODE_FLAG_GUIDED_ENABLED) {
 						/* POSCTL */
 						main_ret = main_state_transition(status_local, commander_state_s::MAIN_STATE_POSCTL, main_state_prev, &status_flags, &internal_state);
 
-					} else if (base_mode & MAV_MODE_FLAG_STABILIZE_ENABLED) {
+					} else if (base_mode & VEHICLE_MODE_FLAG_STABILIZE_ENABLED) {
 						/* STABILIZED */
 						main_ret = main_state_transition(status_local, commander_state_s::MAIN_STATE_STAB, main_state_prev, &status_flags, &internal_state);
 					} else {

From 42d8459e87dbd54ab9c9a1127ef023441888357f Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Fri, 1 Jul 2016 18:04:47 +0200
Subject: [PATCH 0873/1230] We are not using the conversion scripts in this ZIP
 file any more

---
 ROMFS/px4fmu_common/logging/conv.zip | Bin 9922 -> 0 bytes
 1 file changed, 0 insertions(+), 0 deletions(-)
 delete mode 100644 ROMFS/px4fmu_common/logging/conv.zip

diff --git a/ROMFS/px4fmu_common/logging/conv.zip b/ROMFS/px4fmu_common/logging/conv.zip
deleted file mode 100644
index 7f485184c6d453a422f795489fe64684e9b179e3..0000000000000000000000000000000000000000
GIT binary patch
literal 0
HcmV?d00001

literal 9922
zcmZ{qLvStt)1}|owr$(K!5iboxv_2AwtZtKH@0otw#}(;7W02KHPf43bU#(SIGfXT
z6lK7`(E*?UXaJdqx)`^$oFFn50AOYc0KfnM0BbulBRgAHMw=@EE4vM;#2u*gpV<be
z5JS;1&sBL+$v#VCp1B$ZoWyKdH7jg(ap0z6Q7n>>qu0-weeZWq<vG-vw^RWdPZ-#A
zx)gdxM|(wkMLVIKe1vRjssiI<@3P(4>TQ*aF#Hxt3nh$?lLTm*MrG3l1i41$;j*Li
zO~UH6A&8AzJ3*$P!^!IN1rvqi+sNM95XL`zY>ovmg6j*E)}|{+kmz}W&(e*!ok`87
z6+L~Oh|bLgBFTO4*S+I=Z*Ckn$*&0@+-L(nD-r=&oetE;rJW#1iu?tmEn|RmUVTEM
zM0<0&*I*}G*NDCaPn4b?i#3?gZL{r$``J5X$0m~{1?#mO<yT+Wo$sFVC?#5W6Cu5l
z1mV^!*>|Cz8E(LZs$IfPs!d|?S%C5G5yi7L8iaAFTBQ-v5jl*N3C}XEjuMFG8PX$f
zof@$LGl(=&h(rlsg9~9L<87tq8#Jg+p~F@5RnLO7ufG`#mV8#-3Y@*+v#DCh^4AN9
ztgtQ7*&*>IVaD?4<`cka1}WSki*5mC6eQrEuD{b6QkxU7RnUV`TS3-@x4~*WMTL!7
zAitmdMP@?7(Z$faED?lxz+T-7&vPxV>@PfM*-9afzuH!AeYucM>_z}qX7o}oJMu50
zA(xs1(YHEI_XpE|&n|rCD+*E;?OJ~hWEYm27A=E9>~kP4Kzi)>zrIbc-Z5c+<W4m}
z@Yva~=IX++t~_fU;lV&kOx$ZU1bB&c$9-h{gqpSUv&|bnzvTNU^&|aYFI3cUvZ!*e
zi3iHCVI53S@y8mDCg}&jgUfzRZQd?(PW%Ivd&la^Vj158UsqN3$uK+vlD~5aYv3`B
zyEMh4PHrJ9f=<S>F>|9{Vxxxt^>30JY>03F8wTA%v6;w?^Y`R$nGs#UQ~;3c92Ocq
zq)<C|@9e=J`b;!eo&iy*fxCX?ifwl<<VfcfiQ6QP<#$z|ZwvaW%qZObUPT<`)45~M
zE!1{K6STmB9Z}A1?KGIqvXy;T)wHHe+jgX9`IQz1gJOiNEV}S!yun{EnU-oh-mNHo
zNmH;f)FPzGx@(VjvT|ZShuQ#0DxIxBj+_7-2VsbT-gYpVK$UYOZ^1z^*<p42i)8Lw
zFUmmy<E#Gr_p37(cJ=Sz=Hd~w`Khp&XxBu0xe(|y*kIUWSpF4LZH(*5{6;{@gsm7A
zE#tlLd2z4c9}#GiY2TRn5>&cdIWJukVf#`n<Yxx^(!y!8`Hw|BpA7F{1n-_*1(5}b
zcOT<_w{B_WCSezZ8o9mb$yaIY;1ZreDy;luSUKPR%v3Sq$0Bz8XE})tl_GBQM2Mpn
zH&sHtl`K4X2u!0R+hvlWU?7%uf>YlxS9&^#l4!@VxvrE1%C5b(fsoL7N?U9wfGRM?
z=Ab(5V2|fpqDYRHJprsoL~q#ZqpE8qwv*frP^vCz%c&rV>g_6JsrwydVJWayq`d*i
z1`SZfhaE9E+nK7j&7$F)nbd%R(wEAgnd`jiYih+YPlnaWs;lqxf!!F-BqgFtY#cw3
z>TcpZ&#f{0#+$W*Gekt*W;wNz>!2kGzg5W-W5BWT^%-4~3tCRcA`YT`)@iKJT}OLM
z*#mVks~>lbx_yXcEuKAzDCXrzM?`DrPEJO1Q~zA%siaSSo5?y`XN-J~xrpC)XsFbz
zzfeV}9~JK2{V+@bx;fg*YAFX*EMD>y)CAHv3VaOpYjGLvMTnmuJft5hv%1NCt-vbz
ziHQk<2z9u>5ZF5|SA%_1O%2glp7eK~l*x1t*fWQb9-=P|Zm|a0d^C#%E}2f$tXjl)
z0;G^KG?sohjMZF74FJJ;9eRT8Npn-%zo-#L2!|K4!K1z~^wUgR{ZispCQTgFDT@}x
zkf=pPOLY354pO$~;O>mIb?wBfk~=gbcry`ED$i${OM_$fS&Q`315&J2Tkos5IJ_4y
z)=IhO6?S0yo?YvaO@fu4CnF?xdQ#P5nRZGFr}Lbd`3p0y|91$}Wu#xwf)Tw-txj$E
z3NW`GsHjwiKJk~r-ST@rTQ%iSgBH2j&Qzb+A5)1u$KMJo8;I;e$?g%a*x2fQ<wJ+_
zNVT=6&x)I?!gbrmg?j9Zt6#^z6`QS=jT_{>@;6o1(F{#wQD7=ku_<5+3jAw3q1tDB
z>&Vr%#}$j^soKszgs0S&_fl<!dG2x#MU-4wD}bfG0Ef|Kq){7u%Yp3HdV6WvPGoug
zDO52b!%mwme%7@RbBon%cdlh8d`D)vcx$v`$uXv3aU%>FK4rCObJm<oAKA)$6k*Es
zfdcB|{FVawwjv_0lkWQK4*zADUZa`kxE8#-7CY73IVcvg!oyo~<Q7Km_XFq1BZg$O
zAJ>{Y%AbfmYuU_I0U1fGODAEmdNTl1$f#x3pVSn(co8DkfTkxv)gWB1v|=L-y-=TD
zvQsw5N+v7d2f*uUw51YUtf_`YvPzsEP+w1=_1IA*Th&_<)XJ;O(d%o^6x44L4N4C_
z&MNj-H0?JuwtoFGN_xRq@=#g$r4)*HfvmJrQSc>ma{VcXYeCDgRx1eGZ{>cEPjK&G
zdM>WrrOg1Ww2s8ac8pvn3^ilfrfTu_L<@{xEHPD)n`-6`d^)<=PI7lalG57xg5Yl#
zld4*)Si-LOz1yAfsgYXu`saEr=o!i2d>luG7n(czwp-V|xz|<ZOhcR}0pwMiwwI!5
zE<sVuy1~gMicw_orNQ>AEEG$Y9L<EV?r4#es4nBuQ2zYdr8JEi<W0EkHaq9T>f#@4
z)@F-q$Ss_X!pJc4SM6QQL5`tXa&7*aOXDRH!is>qhYn^D>>x)FxbvlT5P6B%*CCfl
zGs{#r2OK_%w)NyTTuqN*U07C0Sx&_PjfxV?J-6N6{((vBm8L&BdmqK(OFv$kxymTH
z_Hq_bJso0v%fKsr(qWkG5-LC!E?vZL?X|q_OXA(~_e9bwn9LyHL(y{`pJ-691aJa|
zOnjwZnw;DnYAb|-Til*S^El}zp571X&LW{Xn?J$SEst_u?z$8M#d2Z{#qulRQmL#Z
z>Gt>EY2+5kf75H&zEV{!D!Lj~Y~)zz)|_7o7?-0{d-4#&!BC4YGVc6E-YDxD(hifb
zs48tPM2qAPs4o5l^yu>02cC3QP#F#6dX-I(eSS^n8%FOAQ_lv~)0|e-x=GG~rb;Z~
zCK11lg6Oh#kDGgJ&PmzWF4U>y#EsGYleF9VtNWXg$<Ka>SS4K>>_*k%bL&C2^^KLI
zKMRVe3bNAA%_gh$1)j{v)lfC#%XHIZ!5TK<=(Oj_;m$)8DxX`E4WHE8UzQ$k5ix0u
zxX`^C`WltB`ge{jVuZ<|@+4OpqjgFBV0@4bc>`71JaUqA!RtWPnB=C#Nrqfj4~^kz
z8Pf1X$~N$ILze!4&{mJCNW-ccw8%yqo3Mgs+vul+PgG*CpBk2S)FDT6ASuaW1Zj|b
zIr-W1e#)LR-0wNcGKL)proPPYd6OZY4DiFTIsB;$H|Nph5+~yir-BWkdz6dJA=1e7
z_oW&DY=iG<Q$|~tCECa(&YCBD52Z)|kg(iQG(@=B7J}K0M8SQT!|~&H9DawzNf*K#
ze(W)@)_Fsx*5_K<QfA?m9}gHiu`PKgTMJI^i5I#gPsEt=mdQI7t&!%86$!QRUL|b1
z&-a|ldg7irQ>o{S*|Q7oYA&&CUc?Qjb#0$<bDG}KhiT@zfDTj7KNv{<igFj`rT%b_
zhN)$k(aSoXRB*a?=cCfIJ2}j{%J%NaN*bwdY0Ku{oE{s%<RS7UJ^-4r$~T-y6%Un?
z2TgB5^yR=rx0x8XD%K9AFPm_V-s@S3wD9Z1Et>6M-Hw8NzBtjgxE}`aV7s4(A5U9o
zV&43sAe_Bm4u?Z|ho7kqtfpsS(=<57n9RPB)|T6MhVZEAKj)Mgm!}mMc;VwnYj5;0
zc0wTdNXT&#^VR?w%0KdLfu4+U7K-YxA*mltW^iFUIn_X!I4o9D#Cn!jymyx*Vlhc+
z@~%)X@t%3kzwLX4OQ_UeDr8)nyss8YG<)j2)%`hH(;uk$N5E+<k-4v$VobtPC+AU{
z)B+TRP}61LV0jH_@3}YMC14wUoHQ$VFluH)ZZCgnk+u6rgZj1H#Wptdimy;e3CX~3
zC2?!5-~6eGo8i8SsSq~Ag*Gy2xpx1^R(Xo4u=l3QzTHs%7RL`CllwF|p1>JXj8NCz
zHjCwV=t7oZs~73=p)KD+-H?f_B8(b17-9r@4Xg0$!}whU$wvqiazm$=b^MDZ=nnd)
zo5sVK3odDkC7RS&tc7W)9fPVPLRudi`JWfcRwdX^4_Ct0Ch*z7pk9dI(-x)u-%eWK
zSEPz>xGRjrYiLL4mdp8BLr_emPixX;;gs=+MyyxR9fIe<o2jY_d~RV|ad|e{zc`!K
znADn>k~8u)ER5b$nv(9!`X^BF@GDf;L=v{V5jSo)403GaiH2xu=)Y>IS8x&9Z>3zW
zAkx{3qx6q+u6#c#Rrg5Zbj2r}8xH=8<KRM?K%s4V^<VS$`7fr2>(7WoL$+yZ`t)#*
z9|b!n<y__6RXydZNSRCT24AN(HO*l2k;E{a7sM-tQs?}Lcm9A(Z7^`9uO}}GQ;awm
z@r<>Z`g0cpH5a)13hiXgEriW+=PdfJStQu3sTJpe-Q*}E(S>nJ%K7Ugm@|2a6L4T~
z_mkz5D#4PFWj^~+GXSOS1!vh<%ZNciGns1$)5M*ld<%Xczd0zPK(9-`N21SnD)0*t
z?wOBv^`=$7XC*}ZIEJ1zEsseGC$KE?JhYD&UjO84W!MHw(p6}gMf~fZh)~w!L%c8H
z+>FutTc&Tfl4BwQAedTjx;mvmZf7?mTcX;P04{p;*cjsI3R5H@85;gsZYLuKMS2^O
zcAm_-;rp)zrLH+<mgMCCg)HSRCthoSt&vNR@&r-ZBs~UYJh)^TS*wLf9hnbu<obfS
zRND?tWjY(gui_3g*EHLgPt&UUP+L3_pYRwp>LClKpcD7C1#20np&SyZ>_xt<VW*0n
zUW=!Rrc)q;GD#p4C@*DLkEpuCkE+1F#--jb<_SGj-%pHM+RqB*8E)+RcLT~OYn?vm
zX76s0KnXoPmO!8UJc7*;d@es%seIceJbnSj9AK>YMxg3z5Rfx3GdygWm3qQ6p@JD$
zoWG~HN2bD-&539Q6;nu-_+HY$Z<JZ%#ew?-i5CBLUFO8uhi-CHn&QYQR6UsHU>o3$
zs|<sl*Thbg=mA<8vvnG9@nYkUUlQ8{K7(Wo<vb%v;xv&F%*YS3(lFYvanac>Qu-Fx
zzWhGp>dn}BnA8L1hm=f>vZyR<Uz-15Bkt6%I)K#4pxK`N8x#JAiQl(}<8QI>Ob(j<
z^<@WDFI9${tZ-q)?|yiHv0IWzuRx0!IoXa`tZ(POB$VD3D2wUN&xi_4+6`J<k4?z4
z2^C_W4ahS)^u$Dpwzdf4%*0!9N<+FSdXIXfOif=CBK8J!S;46=JQuJe6eV0kx+#9A
zdZYpmM-!so51%rEYm#_rnPJE5gisX+#69BorqaiX_Vp>Dgt40+P#kyMQ0gJWaZA+B
zMG0gLM%c@TtQ3d=OTWXllGoUo*P*k*0(ao>4J%{vEpF36vKwk@6V!ofau{PfH;{y{
zmdM1-hH*#&S$>Td)dTp7DsSq7j-+bCO)K{(wBX`(H&A`dYG1_2C?QmP$)q<naD&<6
z9t}oHp`?$2=9jyAzT4}aV*%VLen*O=A(L;P?x@M{y7}zSpv7%i)L~u$j*VE<BVGab
zo-KpNy%PzmEpL?SzoOXDO_ni9CqH9IFv3+jM~$uQXmaJRwH)h`ofKIR)49LP!7#mi
zPZ4rVqXTPmC+wJ%ZUahpGB|_PG<p}!Z>Qu-MMjmkG?NuD$*Mdup!jYIN3|&^_@8ep
zV&jn7HTqT}diu*=ICUng!<-Q?;shFd3fsRQ=U>odE@zlU>R%IS>UIJdv%&ZT4@Bf4
zW)KsoJ7;D-W-&1OEItGAteqykv~q0~^3a_U1Rkd!`f7|4Ksbt@-dY1|x~;fGY$um^
z-8YrZN0KC}OV%-qfF|V1{cajc0OExU4tsc>UfmN>PX7TjSu}TZmnV#0O=~!$PGR>r
z5j0+<C<@toG$Y`Tn8FLnZnznI8kR#GL=O+$u@~v=xPxpW_DV6nkEU=*m<VSUw!TjA
z<o-pAi6y|*<Y4#NL8Xjx(g|elV=y1RfKw1U444FNt6<6BW+irweRa6R5U`txpTJZU
zv4-=B6xoTM1skhj`GnM?gPMCjh*ufJN#doW;N(@Cz<ENVtRe9!9^oh=1sQsCg>h8#
z9glp<>Yc!pu}hczwp#azJ+8$hGvCbTL@M34GhKdKqz5X*36;J*yCcirrjE~!wWHS3
zA-2g>6;8dT91P+Gd*sVHuCb$9Z=WJP)D6;BkP)bEs8U%3ea9CfuP+>Pj(fXgJq4|C
zo|*1b-~{o2Tz6xCk$0iI`L0tWMqa<&4|=H<C*N<c(vOuqH&&q&oV0sK_Yb70PB6{^
z!UwzS6}PXxrPoiNM!D_qG(lVU!{a2zyoG$Q*DO3Pes4@9<YIu2ZU3|>z1%_Njv<$w
zSMEygMV%W=Y5O**k}uIjZVRP{B&rGgY;b+>t(THN@y{m}jZAnK+7k)~gHIL&^sK{L
zp%prqnA>ggIYW1m%RgoT@0SX)y0;i_!yC)45jSytAuuhh*VtHml1Y5hzeNu#Ql>Kj
z$+&Z1&MZW~lm!ehjMbpMGT0ZrNHT-I<#cA<?E0G{z9I6uvA15SjOF?TmdKu_2Yy>+
zy1}T3tgS>7mdC8V_>#S}c79^Nxc^~1CUjwi{TGMe1NzZ=3>>vH)U&-%d#2E3`d#uT
zBUIXZEw=Y~DP<-Uu?{tc9-ubWyE8DgG}{-^mDD(1SSo)8Ea`-rR9ObfsQ&u0Nd@vV
z(`Zs-I-`E$yM-WZzU%WrrQEB0PASYyG}vCy3LLbRLOqVf3H+V0YFiEJP6_Se89{Qs
zjY+gaI-WF6;R?(p_*>g}56A5tWn-=O;5{q_?#j>Fju4C-O%SIdL(CmxmI}s;*taau
zjqw=k2!X!5a8t=2MDi)|PN@2fYJ4prbahq9P5Kr)R)wY;D4~@#6fDMXJYYq=hWy^H
zFp<%52w@<fLG<?P<0Z0Pz3_)3jrz+r^y*u&emlorz+R^#CqZe98+N1PPipZWBBE_%
zeKb!Jkm7Nqf2LrD)Axo^u;urrAg}wqZu_&NQCR5lqblWZS&*&?ZlS?OqHAI=oj2>0
zJ*^}G+Lsisi_Lt*zU(!?+Yc_svkbRXymxD%-W<UGc)_%4XBI_Q1^PZjf;`*|CVHZQ
zrS0o|k(_Z*4tfR@3uQ_&LB{3O%w;nClI7iQN+CFnin!SNN3qoilI;~c;S45M&J$EA
zy=!2UZwuGA2c@Cq6rnH+`{z7S4KT}^dBiba*KY1}twbH$(nWPy!q-*Whv=;-SZ=*p
zeze-`Tu(tccGYc$T!-EJpI~8Gmz6hW2>kNq3L0{Y0Yj%SXSy;D4+&^nnVg@yM8<iX
zkjwN(DG^`L!o&7V5>;+ZADYM5Ai|enB?M$<SMP)T3%(BqN5zemd6mtACiq1C5mnec
z)r)fZUt^@xc=nlAr@4}sCd>AiW;XN%C6UKvZ8m5Ds-<{jv^z0xIDgVszmle}i6wW@
zVX<49VXWQX@o`-b6Wi3D3r`B{M2zVxoG2I2JJA!D%|}%O{+x>@B58Vd(H?)E{d_>4
z@_$aS-<PMse5vk)wErIbKq>y8x4FSz4Y8;oc|kvT0N|4q06+u)08Yl%c4n-4#x6GY
zjP@Q~+CO$1&8R<Z5br{Duz?y;`;@jjbvP6L?7$X7xh#&efo(%EMdR^hT9wRhZjR8e
zo10itio}9NV+K?7*r5Z0xRHac6KdC!vxbV%yshC&922}mn+etNTz=ns?(qeVyRS0Z
z%ItT+h!xGuH1Y`}HBmaaiPq~=?rF`YY3Ok1dj{Y<g{HE^Lw*sQV+TcAPuD7))IUIf
za1jqrTPLxIh<xq$$019=OwtHZq&~3J(0Xn`&wtXPnL?;{0o(XT$`+B+>9ydsi5M<(
z@S?SdN07C@g1k@9%F*FzVTwD_qMrNyH7|mTFz&>O2iKR#8v0F9F_A)KQh3p(iQGcf
zR268bmdvgZ&n}8Fl6Wx(;`5jrdKP*doE3WPk%?s_cV3Ws&fd;L92-Sq#tvHnK8Q7V
zho2Jz!<iiv)C|%j+Af_0fe1-icu}>PC>Q^`u?~3X{KvGGz>?SZwW64QEs=x49tJks
zIbn`D^czmumhC*jpW$4dAwVWcWn>286vYf?$Y24UppaN3vB=Gt1@c9h>Fz5(@Mx+p
zX_Uo5UIRwvi?y{na%m);XB9+N8$kEyk0^NnbAo28p6r2<N=VZTVHb0|;w~-(LwSF|
z!t?3;`iU%9i&)xZ=`o}+nVPm)n;W!v(OJM@AwsrqCZapLP>HJJ$pdQu?Y2HmQ@Z;K
z4|Pl)cL^bmJ>qrUI-1yue>H{pvY91TM<oZl&b40&2>-Dky_QlS3<D~nAD&+#SSQFN
zr@(!u*=^^1@9TAaf8k4fU7vmcx8UV>f4qGiUB9>eoW3!oW}cqO0$ur7w5U3tK7Jaj
zYqz_4xZ9qY6y*JQdN2#RKf1+w+I@fa9&uN0jwY~%=30hYm|#n<T)uiHoaR#EZnq5N
z&^A|Yv_SltOdV`<XmI*kB)3bV+jB)d$AC;WMHp1o)<t<dXJ&Xb$3?3Wo>RbDo1*X}
zQoINU)bu`4uL%o%O1wKa`OoEP9L^*@d?G@Nq6Je&0#qBy6_OmTq>K3NGM&%D1*Z43
z69xJA1&Q~RBwr3GzZE}kA$m?hq{36#+(J&H^ugs4hd=aR)bhmJq7PF#oN0HNqzJeU
z&});0T}X4fJGsPsQ%<<4z65J>wD7_{m{sK@h#O6mCM_Q<Q8GJ380-=4cw3jb3xTOQ
z-LL#LC$0J7qZl6-H<`_3WL$ei8o}Eelm!pi^2;KoDQVy>$Q@Pp;J}2z(rqf32na4g
z7K$ZuDXpW(M9`2To?{LvCsNeW%Y_7QZE+O6g$;(HLw=-YGA!~aQ`u;ifn<jG5mM@)
z*yyTXAYs2gF8!-XhRkU6aUzw2ClR^YwbLn=GHDt%`aWe|+huU2khjT9p%C(Sc&QD%
zbiUY+kDaUbm8?u)6umV9sMW@U<IqB^X`vdV%j+RVv@;iQqkh|=5G^Z4w}=hapsX9d
zLsdv#?>mY=cRN6TC5wDZ(#I!4xBNlHqHwJw@jvU^?a|L<i|}n_@+w*z$81q+@p#x^
zd}fHQl^Ud;B~t{4ep?{DF3{-SSHS|+et5L*qU>x$!u+>_>>%8TOnVR~UWA%?s1Vi4
z7XACCLV45*G)+7ocQJDZ8v8fiGNI@6KvrWB=7aS!dr-l)K0Js-v7JF2+?m{Dav%{*
zxh1oWZxo}$B4<LWi*d>&tx#PFj!9t&n{Z>tmw>vns$#~N+KA=Cu4Zm1azxR2l~FXW
zV22G|f#Sd?tU^~B3mCG~&smPW-vlu%v|Gyd+tYKu&)S`cQSi2aFPZqX!EoekszBsJ
z!Qq``t07z*Pa6yKLwM2-@AxDOF_s`w0kY1<a|ebN_7S#aXl10&;1D4hTFYH)NL9@?
z&ShqAYxW4m2y>vxd<*eu;kI=N!qr=e9P|RuS}jNYeN^(_h4Tzx1osygJE&c6e^AT0
z)JDNAETu1d{?g_&D~|q}P(5?Nlf;vT0>V;k-DizSW>JLIoW_fT&%ui`*JP`oZ0tK<
zC=H!a$$R#@5@+325){kg<Vl|BxDt}SI^-Lx?kx;yH}#mUtVCFztDjS;@r(UE-cO*0
zo(OD5u}zYg>zLHtsgDUH;TeTJ-ijY(u7UYXW6eem^tA%w4CJz#q25las-{G7>p9B?
z;@(kdK!;$<bw$I^EaFNzG}Q8Vy}F#4$PlqhE=eqbW);USf}<8b5JA2IWUhKjTgQj2
zn)jwDyubFS*z4^C>N%m<4L-zP15qBQ@M6qh%QuGvmk}g^pau5!z(1@?o9;NT8f|9W
zvKt#Y2w6ZL6Q5EhIC?~iA8WbN_*ghfM&B-;+3~~6l{Fp27Yg5IF?~M1_WH%hWWdTI
zuh<03V-H=<o9Z@7T4ncrG1u=l9K-ZmuuFxCC}x11q0Cx&V$!wzDuq&^R4d+LOvKE#
zVh~rdQ9~^d1_7N$3S#`u4u9&`cswt2K=K;!w4Iu6!yRU~QmN3(+>{i*0bWv_wR)kn
z1jD(Ec}m!{bGQqM84n7FxMIkBabKV=E0drb8h-y+oncIUs6;)l?V?_ftqIDc*6L{?
zY+sH?V0*fB;klkV;>ol+9@=~J_G9pl;Pu$L3se2v)#6?)n{PC9?y%&Fe`VVB5qe@G
zqI6ld_vLn!-f_gQhy~BZ#rZ<{Oe3L^+5+f<gnm<Dl1Zq~J0drZqSc^(CEX}rHp-Q}
z(I{xbW<FxV1-7;rzm)or*9VbZJ1L2`;8M(Ijhp42!|~NFv#QC0K&aWD{=MIaEsf8z
zbfY+oF%fusFbeMYG}f^aC2lYyK&67PiR9n+c6&%vqLdNWovIe81^a!>SV2mQVI<Ur
zHN32kn`X9bDhJKSsS9?ujm9>5f2u`9qJ@*Yft`u=2W}v$&HL}6d#qKiN_L60*n=`b
zv+NT@B2^{?);dAwbyfCc<)BMSvZ-~#9mQbXJfij5v`P0>UseJ<@-bggmi;zTSx5!o
zwm_GB%Y4fiSwmUdq@P6aEyVV1442UQWeFlRGrx@0yD!2{)#@U&k~nPM|HR896j7mY
zYxO9^@|5N^znoz6@6js=TFLyNg6{%<5qRfc*;9~7Mnny}M6L_6cpl5It;N#Zu+rDU
zQuym8*DFa3j;v7X^<!Pe*#1L9&G`MV*h}6s;oV&ceFfLq1K!;CC|)n=U&(7R0`tMp
z-pF(MSn!LN&fey(EQ0J4;Yf~HSD2ev?{s8rqaKG3aG?&(91R;SmNicV6W@2jn^&aw
zCGb`gQSI1S=OFKJe`FaldOH2B$X*>vchsqGufL>6Lbn-WE9BgZ93y~jTlWUYlGGV^
zJ76)7kJXIAHvy?vWxq*UWCXdyw2)>yN(I4?_eFTywk|P@il)Jui^%9Hl}J{apfENg
zTv%gI7<D&6x;*>CPfZk#{xKj`hcq~igfTuR4b1$ib=7#gz^nhJB3VqmAhKlnEQ83G
zg1hUTQ!jlVBD-mJ#-Zot;}D&K!Z=}}RD2|{@QHd$Y3<RLH3?Qb_kLZ;|M+-&Ml3f1
z2uU5p+IE+GyH&j_e&96emAcx&S{Y3HDpl7aKpmFm`12D?$1HPK_Wa<^8oWNbagSf{
zG$1f)+}>@rBKAri_K%@eFmRi6Z|(xGf8nTcZ}m6@poUYl`T96q8`XJxIXVdDu{0$j
zuTPE#O;vU7JdF#2IIFGOYCpT^Egnp*XQi=SEKOt_Ol9lR$V!W7Pj>H8n(hF)nsfSQ
z8}4P8jF*)mnT)iSKuhQtttQgcd+xzhcUcuGWxcxk?I8DD%N!Isjq|0j>rfZR&k6$P
z4V6TyaSR_T)Vo)x=P14pxnyrI#U0`cbO5yn>rC?E@N;pi*m6x<P90XKwrNlA=|=?_
zPtuu=NdqDW5vCtDEE<Sc89SF_Y5a}j5waytU*=F5mq2Y~mN^FNk<N;K;*CijSiTD*
zEK2NSMw`t@gU~jzq>lv4PJ`{I6NQ$h7u0*S#l$0z@SE1<*0{f1y)PJ&K7V!bU}wBN
z<20NVtaQP5+mc+T?F_*d%sg*F&7*RIC3sd>s|swRZ3_<6Y`_&O`O>vdos_EtH5H&f
zUBZ(P?CV9Qp8zMG@eTO9^iVv7Ak(7>P;I?`FC~emOxNSJoHNEyc%DaEi*(;o?F%Sl
zr?lO!eE4Lze4}dGgv?ycZ-!`?1}5e2Q?;FXrY>hUL9K55GX7*%+FTVk$JVdAa2yn5
zE^BUYMyAt!JNeH@4*dWD!;d^g_dXUj0d=|j-$MUmRsNIk+52ss`~m_1=z;+NumAu+
zSzP3ooH(PiyR&Mdp4~b#`p^#D6CeH&3_0|l7AkT=>PlX6YV%p!^$F)B?!iPO$!3_t
zjhl6#QW!C}-!)H<Q}z(4ciqnyLe>_XTF1)ENnY5~HOAfW{`d8_Z7B(LZT=*TJU3O$
zFg2HpP2>44za|rGJfpi!X6TA0RDIK8se4nrE%!r;Fdc0M9SfvL6r6PVFU4-%S_?{!
zA~9Qte}<7w5_0Wg^455v)xF}hA+p4tcm!sJp(U9)Z<l*}GQV-R2x(IIReiHVnMcY*
zKOeAyHc!=7(?^fgIJX0+e1_2dmOr4mkW}dL2<0w#V)rQhU^b2%8oY5K9riR-<0Q@O
zEbm#VhQPbRe##1z7KcbZY0(cOM!bh2ne43O|McfJU4IxB>iVIYW$QQX53o`ZRK!!+
z(7#{^|LH4>^NVuYDw2E2aNT)3$7H+K8wk%p?eh4*2HB`RAQ|(&g49!$heHW%01QLb
z|M4c1HsYb{G>BW;6L3x4Hz%%%f%#{ruB4?R$#tE+OWXG+#uxY}J!-)6gJ5{y!sE3s
zD>P>DgwAwTId;o{A0}t8+B_8h2jZ*qW&ss=lXF}n$NVG}dvV293rmjny&#oH%<-Fb
zaTHw(hxU~rGC$>I%A51zOW8;P&De}+!Wu{myObbCBF#%jl<u{>YBZ?u1EMGc0*Vg!
z|3|?7Qy>81|K0y95cc2L|H}gZ55|S{e;eZeX8&KB{2z9o?7tPNC<6icpBWec3{d%>
Kvnc<w`o92<MGN@=


From e43625cfc1e941c64b377b769c1e9aa6064e6197 Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Fri, 1 Jul 2016 18:08:32 +0200
Subject: [PATCH 0874/1230] ROMFS: Clear out SITL mixers from deployed image,
 delete unused mixers, move test mixers to test config

---
 ROMFS/px4fmu_common/mixers/quad_v.main.mix     | 18 ------------------
 .../mixers/quad_test.mix                       |  0
 .../mixers/plane_sitl.main.mix                 |  0
 .../mixers/standard_vtol_sitl.main.mix         |  0
 posix-configs/SITL/init/rcS_gazebo_plane       |  2 +-
 .../SITL/init/rcS_gazebo_standard_vtol         |  2 +-
 unittests/mixer_test.cpp                       |  2 +-
 7 files changed, 3 insertions(+), 21 deletions(-)
 delete mode 100644 ROMFS/px4fmu_common/mixers/quad_v.main.mix
 rename ROMFS/{px4fmu_common => px4fmu_test}/mixers/quad_test.mix (100%)
 rename ROMFS/{px4fmu_common => sitl}/mixers/plane_sitl.main.mix (100%)
 rename ROMFS/{px4fmu_common => sitl}/mixers/standard_vtol_sitl.main.mix (100%)

diff --git a/ROMFS/px4fmu_common/mixers/quad_v.main.mix b/ROMFS/px4fmu_common/mixers/quad_v.main.mix
deleted file mode 100644
index d14074694d..0000000000
--- a/ROMFS/px4fmu_common/mixers/quad_v.main.mix
+++ /dev/null
@@ -1,18 +0,0 @@
-Multirotor mixer for PX4FMU
-===========================
-
-This file defines a single mixer for a quadrotor in the V configuration.  All controls
-are mixed 100%.
-
-R: 4v 10000 10000 10000 0
-
-Gimbal / payload mixer for last two channels
------------------------------------------------------
-
-M: 1
-O:      10000  10000      0 -10000  10000
-S: 3 5  10000  10000      0 -10000  10000
-
-M: 1
-O:      10000  10000      0 -10000  10000
-S: 3 6  10000  10000      0 -10000  10000
diff --git a/ROMFS/px4fmu_common/mixers/quad_test.mix b/ROMFS/px4fmu_test/mixers/quad_test.mix
similarity index 100%
rename from ROMFS/px4fmu_common/mixers/quad_test.mix
rename to ROMFS/px4fmu_test/mixers/quad_test.mix
diff --git a/ROMFS/px4fmu_common/mixers/plane_sitl.main.mix b/ROMFS/sitl/mixers/plane_sitl.main.mix
similarity index 100%
rename from ROMFS/px4fmu_common/mixers/plane_sitl.main.mix
rename to ROMFS/sitl/mixers/plane_sitl.main.mix
diff --git a/ROMFS/px4fmu_common/mixers/standard_vtol_sitl.main.mix b/ROMFS/sitl/mixers/standard_vtol_sitl.main.mix
similarity index 100%
rename from ROMFS/px4fmu_common/mixers/standard_vtol_sitl.main.mix
rename to ROMFS/sitl/mixers/standard_vtol_sitl.main.mix
diff --git a/posix-configs/SITL/init/rcS_gazebo_plane b/posix-configs/SITL/init/rcS_gazebo_plane
index a431c1ed28..3a162cc678 100644
--- a/posix-configs/SITL/init/rcS_gazebo_plane
+++ b/posix-configs/SITL/init/rcS_gazebo_plane
@@ -42,7 +42,7 @@ ekf2 start
 fw_pos_control_l1 start
 fw_att_control start
 land_detector start fixedwing
-mixer load /dev/pwm_output0 ../../../../ROMFS/px4fmu_common/mixers/plane_sitl.main.mix
+mixer load /dev/pwm_output0 ../../../../ROMFS/sitl/mixers/plane_sitl.main.mix
 mavlink start -u 14556 -r 2000000
 mavlink start -u 14557 -r 2000000 -m onboard -o 14540
 mavlink stream -r 80 -s POSITION_TARGET_LOCAL_NED -u 14556
diff --git a/posix-configs/SITL/init/rcS_gazebo_standard_vtol b/posix-configs/SITL/init/rcS_gazebo_standard_vtol
index 2a663400ca..d9e06b6fbd 100644
--- a/posix-configs/SITL/init/rcS_gazebo_standard_vtol
+++ b/posix-configs/SITL/init/rcS_gazebo_standard_vtol
@@ -69,7 +69,7 @@ mc_pos_control start
 mc_att_control start
 fw_pos_control_l1 start
 fw_att_control start
-mixer load /dev/pwm_output0 ../../../../ROMFS/px4fmu_common/mixers/standard_vtol_sitl.main.mix
+mixer load /dev/pwm_output0 ../../../../ROMFS/sitl/mixers/standard_vtol_sitl.main.mix
 mavlink start -u 14556 -r 2000000
 mavlink start -u 14557 -r 2000000 -m onboard -o 14540
 mavlink stream -r 80 -s POSITION_TARGET_LOCAL_NED -u 14556
diff --git a/unittests/mixer_test.cpp b/unittests/mixer_test.cpp
index 6b47034e28..73df8c54f2 100644
--- a/unittests/mixer_test.cpp
+++ b/unittests/mixer_test.cpp
@@ -7,6 +7,6 @@
 
 TEST(MixerTest, Mixer)
 {
-	const char *args[] = {"empty", "../ROMFS/px4fmu_common/mixers/IO_pass.mix", "../ROMFS/px4fmu_common/mixers/quad_test.mix"};
+	const char *args[] = {"empty", "../ROMFS/px4fmu_test/mixers/IO_pass.mix", "../ROMFS/px4fmu_test/mixers/quad_test.mix"};
 	ASSERT_EQ(test_mixer(3, (char **)args), 0) << "IO_pass.mix failed";
 }

From 8162300522063b8c85740be51ef6e1485cae96fa Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Thu, 30 Jun 2016 18:23:52 +0200
Subject: [PATCH 0875/1230] Switch fixed wing to EKF2. Does not link yet.

---
 ROMFS/px4fmu_common/init.d/rc.fw_apps       | 2 +-
 cmake/configs/nuttx_px4fmu-v2_default.cmake | 3 ++-
 2 files changed, 3 insertions(+), 2 deletions(-)

diff --git a/ROMFS/px4fmu_common/init.d/rc.fw_apps b/ROMFS/px4fmu_common/init.d/rc.fw_apps
index 9e2d1f6ffe..04a82d506f 100644
--- a/ROMFS/px4fmu_common/init.d/rc.fw_apps
+++ b/ROMFS/px4fmu_common/init.d/rc.fw_apps
@@ -6,7 +6,7 @@
 #
 # Start the attitude and position estimator
 #
-ekf_att_pos_estimator start
+ekf2 start
 
 #
 # Start attitude controller
diff --git a/cmake/configs/nuttx_px4fmu-v2_default.cmake b/cmake/configs/nuttx_px4fmu-v2_default.cmake
index 72876e8800..d94391426a 100644
--- a/cmake/configs/nuttx_px4fmu-v2_default.cmake
+++ b/cmake/configs/nuttx_px4fmu-v2_default.cmake
@@ -85,9 +85,10 @@ set(config_module_list
 	# Estimation modules (EKF/ SO3 / other filters)
 	#
 	modules/attitude_estimator_q
-	modules/ekf_att_pos_estimator
+	#modules/ekf_att_pos_estimator
 	modules/position_estimator_inav
 	modules/local_position_estimator
+	modules/ekf2
 
 	#
 	# Vehicle Control

From fad07a45b92b7173c8cb683cd6f967ad45e12b5c Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Fri, 1 Jul 2016 18:21:35 +0200
Subject: [PATCH 0876/1230] Mathlib: Optimize for sixe

---
 src/lib/mathlib/CMakeLists.txt | 2 ++
 1 file changed, 2 insertions(+)

diff --git a/src/lib/mathlib/CMakeLists.txt b/src/lib/mathlib/CMakeLists.txt
index 6bfe964d79..e19201da74 100644
--- a/src/lib/mathlib/CMakeLists.txt
+++ b/src/lib/mathlib/CMakeLists.txt
@@ -32,6 +32,8 @@
 ############################################################################
 px4_add_module(
 	MODULE lib__mathlib
+	COMPILE_FLAGS
+		-Os
 	SRCS
 		math/test/test.cpp
 		math/Limits.cpp

From ed19d1ff6b607cea1dfb42d5cdf215c3ec1ce07e Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Fri, 1 Jul 2016 18:22:56 +0200
Subject: [PATCH 0877/1230] EKF2 wrapper: Optimize for size

---
 src/modules/ekf2/CMakeLists.txt | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/src/modules/ekf2/CMakeLists.txt b/src/modules/ekf2/CMakeLists.txt
index 5fae22e36e..148402e533 100644
--- a/src/modules/ekf2/CMakeLists.txt
+++ b/src/modules/ekf2/CMakeLists.txt
@@ -33,7 +33,7 @@
 px4_add_module(
 	MODULE modules__ekf2
 	MAIN ekf2
-	COMPILE_FLAGS
+	COMPILE_FLAGS -Os
 	STACK_MAIN 2500
 	STACK_MAX 4000
 	SRCS

From a0fdfb0c213bd395696fffd8f102dbccaa0af724 Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Fri, 1 Jul 2016 18:23:08 +0200
Subject: [PATCH 0878/1230] Strip ESC calib commandline tool

---
 cmake/configs/nuttx_px4fmu-v2_default.cmake | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/cmake/configs/nuttx_px4fmu-v2_default.cmake b/cmake/configs/nuttx_px4fmu-v2_default.cmake
index d94391426a..53c4f8c792 100644
--- a/cmake/configs/nuttx_px4fmu-v2_default.cmake
+++ b/cmake/configs/nuttx_px4fmu-v2_default.cmake
@@ -57,7 +57,7 @@ set(config_module_list
 	systemcmds/param
 	systemcmds/perf
 	systemcmds/pwm
-	systemcmds/esc_calib
+	#systemcmds/esc_calib
 	systemcmds/reboot
 	#systemcmds/topic_listener
 	systemcmds/top

From cb3120764a9b5e0ba796c0e2e0c600238ff57f90 Mon Sep 17 00:00:00 2001
From: James Goppert <jgoppert@users.noreply.github.com>
Date: Fri, 1 Jul 2016 13:27:29 -0400
Subject: [PATCH 0879/1230] Made LPE var pub threshold a parameter. (#4959)

---
 .../BlockLocalPositionEstimator.cpp           | 14 +++++----
 .../BlockLocalPositionEstimator.hpp           |  3 +-
 src/modules/local_position_estimator/params.c | 31 +++++++++++++------
 3 files changed, 32 insertions(+), 16 deletions(-)

diff --git a/src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp b/src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp
index b9b4c21fa5..77c2748921 100644
--- a/src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp
+++ b/src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp
@@ -14,6 +14,7 @@ static const uint32_t 		EST_SRC_TIMEOUT = 10000; // 0.01 s
 static const uint32_t 		EST_STDDEV_XY_VALID = 2.0; // 2.0 m
 static const uint32_t 		EST_STDDEV_Z_VALID = 2.0; // 2.0 m
 static const uint32_t 		EST_STDDEV_TZ_VALID = 2.0; // 2.0 m
+static const bool integrate = true; // use accel for integrating
 
 // minimum flow altitude
 static const float flow_min_agl = 0.3;
@@ -55,7 +56,8 @@ BlockLocalPositionEstimator::BlockLocalPositionEstimator() :
 	_map_ref(),
 
 	// block parameters
-	_integrate(this, "INTEGRATE"),
+	_xy_pub_thresh(this, "XY_PUB"),
+	_z_pub_thresh(this, "Z_PUB"),
 	_sonar_z_stddev(this, "SNR_Z"),
 	_sonar_z_offset(this, "SNR_OFF_Z"),
 	_lidar_z_stddev(this, "LDR_Z"),
@@ -303,7 +305,7 @@ void BlockLocalPositionEstimator::update()
 	}
 
 	// is xy valid?
-	bool xy_stddev_ok = sqrt(math::max(_P(X_x, X_x), _P(X_y, X_y))) < EST_STDDEV_XY_VALID;
+	bool xy_stddev_ok = sqrtf(math::max(_P(X_x, X_x), _P(X_y, X_y))) < _xy_pub_thresh.get();
 
 	if (_validXY) {
 		// if valid and gps has timed out, set to not valid
@@ -318,7 +320,7 @@ void BlockLocalPositionEstimator::update()
 	}
 
 	// is z valid?
-	bool z_stddev_ok = sqrt(_P(X_z, X_z)) < EST_STDDEV_Z_VALID;
+	bool z_stddev_ok = sqrtf(_P(X_z, X_z)) < _z_pub_thresh.get();
 
 	if (_validZ) {
 		// if valid and baro has timed out, set to not valid
@@ -333,7 +335,7 @@ void BlockLocalPositionEstimator::update()
 	}
 
 	// is terrain valid?
-	bool tz_stddev_ok = sqrt(_P(X_tz, X_tz)) < EST_STDDEV_TZ_VALID;
+	bool tz_stddev_ok = sqrtf(_P(X_tz, X_tz)) < _z_pub_thresh.get();
 
 	if (_validTZ) {
 		if (!tz_stddev_ok) {
@@ -555,7 +557,7 @@ float BlockLocalPositionEstimator::agl()
 void BlockLocalPositionEstimator::correctionLogic(Vector<float, n_x> &dx)
 {
 	// don't correct bias when rotating rapidly
-	float ang_speed = sqrt(
+	float ang_speed = sqrtf(
 				  _sub_att.get().rollspeed * _sub_att.get().rollspeed +
 				  _sub_att.get().pitchspeed * _sub_att.get().pitchspeed +
 				  _sub_att.get().yawspeed * _sub_att.get().yawspeed);
@@ -838,7 +840,7 @@ void BlockLocalPositionEstimator::predict()
 	// state or covariance
 	if (!_validXY && !_validZ) { return; }
 
-	if (_integrate.get() && _sub_att.get().R_valid) {
+	if (integrate && _sub_att.get().R_valid) {
 		Matrix3f R_att(_sub_att.get().R);
 		Vector3f a(_sub_sensor.get().accelerometer_m_s2);
 		_u = R_att * a;
diff --git a/src/modules/local_position_estimator/BlockLocalPositionEstimator.hpp b/src/modules/local_position_estimator/BlockLocalPositionEstimator.hpp
index 92a3a9fc14..7a5028424f 100644
--- a/src/modules/local_position_estimator/BlockLocalPositionEstimator.hpp
+++ b/src/modules/local_position_estimator/BlockLocalPositionEstimator.hpp
@@ -243,7 +243,8 @@ private:
 	struct map_projection_reference_s _map_ref;
 
 	// general parameters
-	BlockParamInt  _integrate;
+	BlockParamFloat  _xy_pub_thresh;
+	BlockParamFloat  _z_pub_thresh;
 
 	// sonar parameters
 	BlockParamFloat  _sonar_z_stddev;
diff --git a/src/modules/local_position_estimator/params.c b/src/modules/local_position_estimator/params.c
index fb8fdd6172..43e6988b97 100644
--- a/src/modules/local_position_estimator/params.c
+++ b/src/modules/local_position_estimator/params.c
@@ -3,14 +3,6 @@
 // 16 is max name length
 
 
-/**
- * Accelerometer integration for prediction.
- *
- * @boolean
- * @group Local Position Estimator
- */
-PARAM_DEFINE_INT32(LPE_INTEGRATE, 1);
-
 /**
  * Optical flow z offset from center
  *
@@ -346,5 +338,26 @@ PARAM_DEFINE_FLOAT(LPE_LON, -86.929);
  * @max 1000
  * @decimal 0
  */
-PARAM_DEFINE_FLOAT(LPE_X_LP, 5.0);
+PARAM_DEFINE_FLOAT(LPE_X_LP, 5.0f);
 
+/**
+ * Required xy standard deviation to publish position
+ *
+ * @group Local Position Estimator
+ * @unit m
+ * @min 0.3
+ * @max 5.0
+ * @decimal 1
+ */
+PARAM_DEFINE_FLOAT(LPE_XY_PUB, 1.0f);
+
+/**
+ * Required z standard deviation to publish altitude/ terrain
+ *
+ * @group Local Position Estimator
+ * @unit m
+ * @min 0.3
+ * @max 5.0
+ * @decimal 1
+ */
+PARAM_DEFINE_FLOAT(LPE_Z_PUB, 1.0f);

From 54e14cd4b60a4d71f5b439470a12dcbbef1a473a Mon Sep 17 00:00:00 2001
From: Daniel Agar <daniel@agar.ca>
Date: Fri, 1 Jul 2016 17:00:35 -0400
Subject: [PATCH 0880/1230] add latest version to README (#4960)

---
 README.md | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/README.md b/README.md
index 176bafd5d1..8b71befe58 100644
--- a/README.md
+++ b/README.md
@@ -1,6 +1,6 @@
 ## PX4 Pro Drone Autopilot ##
 
-[![DOI](https://zenodo.org/badge/22634/PX4/Firmware.svg)](https://zenodo.org/badge/latestdoi/22634/PX4/Firmware) [![Build Status](https://travis-ci.org/PX4/Firmware.svg?branch=master)](https://travis-ci.org/PX4/Firmware) [![Coverity Scan](https://scan.coverity.com/projects/3966/badge.svg?flat=1)](https://scan.coverity.com/projects/3966?tab=overview)
+[![Releases](https://img.shields.io/github/release/PX4/Firmware.svg)](https://github.com/PX4/Firmware/releases) [![DOI](https://zenodo.org/badge/22634/PX4/Firmware.svg)](https://zenodo.org/badge/latestdoi/22634/PX4/Firmware) [![Build Status](https://travis-ci.org/PX4/Firmware.svg?branch=master)](https://travis-ci.org/PX4/Firmware) [![Coverity Scan](https://scan.coverity.com/projects/3966/badge.svg?flat=1)](https://scan.coverity.com/projects/3966?tab=overview)
 
 [![Gitter](https://badges.gitter.im/Join%20Chat.svg)](https://gitter.im/PX4/Firmware?utm_source=badge&utm_medium=badge&utm_campaign=pr-badge&utm_content=badge)
 

From 76ee17e532fdbd28bc6e0a20bf16626c17f52288 Mon Sep 17 00:00:00 2001
From: Hidenori <hidenori.kobayashi@gmail.com>
Date: Wed, 29 Jun 2016 13:10:11 -0400
Subject: [PATCH 0881/1230] RC input and PWM output for Navio2

---
 cmake/configs/posix_rpi2_release.cmake        |   3 +
 posix-configs/rpi2/mainapp.config             |   2 +
 src/drivers/pwm_out/CMakeLists.txt            |  44 ++
 src/drivers/pwm_out/pwm_out.cpp               | 490 ++++++++++++++++++
 src/modules/commander/PreflightCheck.cpp      |   3 +
 src/modules/mavlink/mavlink_main.cpp          |   3 +
 src/modules/rcinput/CMakeLists.txt            |  43 ++
 src/modules/rcinput/rcinput.cpp               | 297 +++++++++++
 .../posix/px4_layer/px4_posix_tasks.cpp       |   6 +-
 9 files changed, 890 insertions(+), 1 deletion(-)
 create mode 100644 src/drivers/pwm_out/CMakeLists.txt
 create mode 100644 src/drivers/pwm_out/pwm_out.cpp
 create mode 100644 src/modules/rcinput/CMakeLists.txt
 create mode 100644 src/modules/rcinput/rcinput.cpp

diff --git a/cmake/configs/posix_rpi2_release.cmake b/cmake/configs/posix_rpi2_release.cmake
index 0778efa1ff..a796aa6f15 100644
--- a/cmake/configs/posix_rpi2_release.cmake
+++ b/cmake/configs/posix_rpi2_release.cmake
@@ -72,9 +72,12 @@ set(config_module_list
 	modules/navigator
 	modules/mavlink
 
+	modules/rcinput
+
 	#
 	# PX4 drivers
 	#
+	drivers/pwm_out
 
 	#
 	# Libraries
diff --git a/posix-configs/rpi2/mainapp.config b/posix-configs/rpi2/mainapp.config
index 752ea8f66b..4f7ddd9354 100644
--- a/posix-configs/rpi2/mainapp.config
+++ b/posix-configs/rpi2/mainapp.config
@@ -19,3 +19,5 @@ sleep 1
 mavlink stream -u 14556 -s HIGHRES_IMU -r 50
 mavlink stream -u 14556 -s ATTITUDE -r 50
 mavlink boot_complete
+rcinput start
+pwm_out start
diff --git a/src/drivers/pwm_out/CMakeLists.txt b/src/drivers/pwm_out/CMakeLists.txt
new file mode 100644
index 0000000000..ff6158c0cd
--- /dev/null
+++ b/src/drivers/pwm_out/CMakeLists.txt
@@ -0,0 +1,44 @@
+############################################################################
+#
+#   Copyright (c) 2015-2016 PX4 Development Team. All rights reserved.
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions
+# are met:
+#
+# 1. Redistributions of source code must retain the above copyright
+#    notice, this list of conditions and the following disclaimer.
+# 2. Redistributions in binary form must reproduce the above copyright
+#    notice, this list of conditions and the following disclaimer in
+#    the documentation and/or other materials provided with the
+#    distribution.
+# 3. Neither the name PX4 nor the names of its contributors may be
+#    used to endorse or promote products derived from this software
+#    without specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.
+#
+############################################################################
+px4_add_module(
+	MODULE drivers__pwm_out
+	MAIN pwm_out
+	COMPILE_FLAGS
+		-Os
+		-DMAVLINK_COMM_NUM_BUFFERS=1
+	SRCS
+		pwm_out.cpp
+	DEPENDS
+		platforms__common
+	)
+# vim: set noet ft=cmake fenc=utf-8 ff=unix :
diff --git a/src/drivers/pwm_out/pwm_out.cpp b/src/drivers/pwm_out/pwm_out.cpp
new file mode 100644
index 0000000000..0fb4ebd35e
--- /dev/null
+++ b/src/drivers/pwm_out/pwm_out.cpp
@@ -0,0 +1,490 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2015-2016 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+#include <stdint.h>
+
+#include <px4_tasks.h>
+#include <px4_getopt.h>
+#include <px4_posix.h>
+#include <errno.h>
+
+#include <uORB/uORB.h>
+#include <uORB/topics/actuator_controls.h>
+#include <uORB/topics/actuator_outputs.h>
+#include <uORB/topics/actuator_armed.h>
+
+#include <drivers/drv_hrt.h>
+#include <drivers/drv_mixer.h>
+#include <systemlib/mixer/mixer.h>
+#include <systemlib/mixer/mixer_multirotor.generated.h>
+#include <systemlib/param/param.h>
+#include <systemlib/pwm_limit/pwm_limit.h>
+
+namespace pwm_out
+{
+static px4_task_t _task_handle = -1;
+volatile bool _task_should_exit = false;
+static bool _is_running = false;
+
+static const int NUM_PWM = 4;
+static char _device[32] = "/sys/class/pwm/pwmchip0";
+static int  _pwm_fd[NUM_PWM];
+
+static const char *MIXER_FILENAME = "";
+
+// subscriptions
+int     _controls_sub;
+int     _armed_sub;
+
+// publications
+orb_advert_t    _outputs_pub = nullptr;
+orb_advert_t    _rc_pub = nullptr;
+
+// topic structures
+actuator_controls_s _controls;
+actuator_outputs_s  _outputs;
+actuator_armed_s    _armed;
+
+pwm_limit_t     _pwm_limit;
+
+// esc parameters
+int32_t _pwm_disarmed;
+int32_t _pwm_min;
+int32_t _pwm_max;
+
+MultirotorMixer *_mixer = nullptr;
+
+void usage();
+
+void start();
+
+void stop();
+
+int pwm_write_sysfs(char *path, int value);
+
+int pwm_initialize(const char *device);
+
+void pwm_deinitialize();
+
+void send_outputs_pwm(const uint16_t *pwm);
+
+void task_main_trampoline(int argc, char *argv[]);
+
+void task_main(int argc, char *argv[]);
+
+/* mixer initialization */
+int initialize_mixer(const char *mixer_filename);
+int mixer_control_callback(uintptr_t handle, uint8_t control_group, uint8_t control_index, float &input);
+
+
+int mixer_control_callback(uintptr_t handle,
+               uint8_t control_group,
+               uint8_t control_index,
+               float &input)
+{
+    const actuator_controls_s *controls = (actuator_controls_s *)handle;
+
+    input = controls[control_group].control[control_index];
+
+    return 0;
+}
+
+
+int initialize_mixer(const char *mixer_filename)
+{
+    char buf[2048];
+    size_t buflen = sizeof(buf);
+
+    PX4_INFO("Trying to initialize mixer from config file %s", mixer_filename);
+
+    int fd_load = ::open(mixer_filename, O_RDONLY);
+
+    if (fd_load != -1) {
+        int nRead = ::read(fd_load, buf, buflen);
+        close(fd_load);
+
+        if (nRead > 0) {
+            _mixer = MultirotorMixer::from_text(mixer_control_callback, (uintptr_t)&_controls, buf, buflen);
+
+            if (_mixer != nullptr) {
+                PX4_INFO("Successfully initialized mixer from config file");
+                return 0;
+
+            } else {
+                PX4_ERR("Unable to parse from mixer config file");
+                return -1;
+            }
+
+        } else {
+            PX4_WARN("Unable to read from mixer config file");
+            return -2;
+        }
+
+    } else {
+        PX4_WARN("No mixer config file found, using default mixer.");
+
+        /* Mixer file loading failed, fall back to default mixer configuration for
+        * QUAD_X airframe. */
+        float roll_scale = 1;
+        float pitch_scale = 1;
+        float yaw_scale = 1;
+        float deadband = 0;
+
+        _mixer = new MultirotorMixer(mixer_control_callback, (uintptr_t)&_controls,
+                         MultirotorGeometry::QUAD_X,
+                         roll_scale, pitch_scale, yaw_scale, deadband);
+
+        // TODO: temporary hack to make this compile
+        (void)_config_index[0];
+
+        if (_mixer == nullptr) {
+            PX4_ERR("Mixer initialization failed");
+            return -1;
+        }
+
+        return 0;
+    }
+}
+
+int pwm_write_sysfs(char *path, int value)
+{
+    int fd = ::open(path, O_WRONLY | O_CLOEXEC);
+    int n;
+    char *data;
+
+    ::free(path);
+    if (fd == -1) {
+        return -errno;
+    }
+    n = ::asprintf(&data, "%u", value);
+    if (n > 0) {
+        ::write(fd, data, n);
+        ::free(data);
+    }
+    ::close(fd);
+
+    return 0;
+}
+
+int pwm_initialize(const char *device)
+{
+    int i;
+    char *path;
+
+    for (i = 0; i < NUM_PWM; ++i) {
+        ::asprintf(&path, "%s/export", device);
+        if (pwm_write_sysfs(path, i) < 0) {
+            PX4_ERR("PWM export failed");
+        }
+    }
+
+    for (i = 0; i < NUM_PWM; ++i) {
+        ::asprintf(&path, "%s/pwm%u/enable", device, i);
+        if (pwm_write_sysfs(path, 1) < 0) {
+            PX4_ERR("PWM enable failed");
+        }
+    }
+
+    for (i = 0; i < NUM_PWM; ++i) {
+        ::asprintf(&path, "%s/pwm%u/period", device, i);
+        if (pwm_write_sysfs(path, (int)1e9/50)) {
+            PX4_ERR("PWM period failed");
+        }
+    }
+
+    for (i = 0; i < NUM_PWM; ++i) {
+        ::asprintf(&path, "%s/pwm%u/duty_cycle", device, i);
+        _pwm_fd[i] = ::open(path, O_WRONLY | O_CLOEXEC);
+        ::free(path);
+        if (_pwm_fd[i] == -1) {
+            PX4_ERR("PWM: Failed to open duty_cycle.");
+            return -errno;
+        }
+    }
+
+    return 0;
+}
+
+void pwm_deinitialize()
+{
+    for (int i = 0; i < NUM_PWM; ++i) {
+        if (_pwm_fd[i] != -1) {
+            ::close(_pwm_fd[i]);
+        }
+    }
+}
+
+void send_outputs_pwm(const uint16_t *pwm)
+{
+    int n;
+    char *data;
+
+    //convert this to duty_cycle in ns
+    for (unsigned i = 0; i < NUM_PWM; ++i) {
+        n = ::asprintf(&data, "%u", pwm[i] * 1000);
+        ::write(_pwm_fd[i], data, n);
+    }
+}
+
+
+
+void task_main(int argc, char *argv[])
+{
+    _is_running = true;
+
+    if (pwm_initialize(_device) < 0) {
+        PX4_ERR("Failed to initialize PWM.");
+        return;
+    }
+
+    // Subscribe for orb topics
+    _controls_sub = orb_subscribe(ORB_ID(actuator_controls_3));
+    _armed_sub = orb_subscribe(ORB_ID(actuator_armed));
+
+    // Start disarmed
+    _armed.armed = false;
+    _armed.prearmed = false;
+
+    // Set up poll topic
+    px4_pollfd_struct_t fds[1];
+    fds[0].fd     = _controls_sub;
+    fds[0].events = POLLIN;
+    /* Don't limit poll intervall for now, 250 Hz should be fine. */
+    //orb_set_interval(_controls_sub, 10);
+
+    // Set up mixer
+    if (initialize_mixer(MIXER_FILENAME) < 0) {
+        PX4_ERR("Mixer initialization failed.");
+        return;
+    }
+
+    pwm_limit_init(&_pwm_limit);
+
+    // Main loop
+    while (!_task_should_exit) {
+
+        int pret = px4_poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 10);
+
+        /* Timed out, do a periodic check for _task_should_exit. */
+        if (pret == 0) {
+            continue;
+        }
+
+        /* This is undesirable but not much we can do. */
+        if (pret < 0) {
+            PX4_WARN("poll error %d, %d", pret, errno);
+            /* sleep a bit before next try */
+            usleep(100000);
+            continue;
+        }
+
+        if (fds[0].revents & POLLIN) {
+            orb_copy(ORB_ID(actuator_controls_3), _controls_sub, &_controls);
+
+            _outputs.timestamp = _controls.timestamp;
+
+            /* do mixing */
+            _outputs.noutputs = _mixer->mix(_outputs.output,
+                                            0 /* not used */,
+                                            NULL);
+
+
+            /* disable unused ports by setting their output to NaN */
+            for (size_t i = _outputs.noutputs;
+                 i < sizeof(_outputs.output) / sizeof(_outputs.output[0]);
+                 i++) {
+                _outputs.output[i] = NAN;
+            }
+
+            const uint16_t reverse_mask = 0;
+            uint16_t disarmed_pwm[4];
+            uint16_t min_pwm[4];
+            uint16_t max_pwm[4];
+
+            for (unsigned int i = 0; i < 4; i++) {
+                disarmed_pwm[i] = _pwm_disarmed;
+                min_pwm[i] = _pwm_min;
+                max_pwm[i] = _pwm_max;
+            }
+
+            uint16_t pwm[4];
+
+            // TODO FIXME: pre-armed seems broken
+            pwm_limit_calc(_armed.armed,
+                           false/*_armed.prearmed*/,
+                           _outputs.noutputs,
+                           reverse_mask,
+                           disarmed_pwm,
+                           min_pwm,
+                           max_pwm,
+                           _outputs.output,
+                           pwm,
+                           &_pwm_limit);
+
+            send_outputs_pwm(pwm);
+
+            if (_outputs_pub != nullptr) {
+                orb_publish(ORB_ID(actuator_outputs), _outputs_pub, &_outputs);
+
+            } else {
+                _outputs_pub = orb_advertise(ORB_ID(actuator_outputs), &_outputs);
+            }
+        }
+
+        bool updated;
+        orb_check(_armed_sub, &updated);
+
+        if (updated) {
+            orb_copy(ORB_ID(actuator_armed), _armed_sub, &_armed);
+        }
+    }
+
+    pwm_deinitialize();
+    orb_unsubscribe(_controls_sub);
+    orb_unsubscribe(_armed_sub);
+
+    _is_running = false;
+
+}
+
+void task_main_trampoline(int argc, char *argv[])
+{
+    task_main(argc, argv);
+}
+
+void start()
+{
+    ASSERT(_task_handle == -1);
+
+    _task_should_exit = false;
+
+    /* start the task */
+    _task_handle = px4_task_spawn_cmd("pwm_out_main",
+                      SCHED_DEFAULT,
+                      SCHED_PRIORITY_MAX,
+                      1500,
+                      (px4_main_t)&task_main_trampoline,
+                      nullptr);
+
+    if (_task_handle < 0) {
+        warn("task start failed");
+        return;
+    }
+
+}
+
+void stop()
+{
+    _task_should_exit = true;
+
+    while (_is_running) {
+        usleep(200000);
+        PX4_INFO(".");
+    }
+
+    _task_handle = -1;
+}
+
+void usage()
+{
+    PX4_INFO("usage: pwm_out start -d /sys/class/pwm/pwmchip0");
+    PX4_INFO("       pwm_out stop");
+    PX4_INFO("       pwm_out status");
+}
+
+} // namespace pwm_out
+
+/* driver 'main' command */
+extern "C" __EXPORT int pwm_out_main(int argc, char *argv[]);
+
+int pwm_out_main(int argc, char *argv[])
+{
+    const char *device = nullptr;
+    int ch;
+    int myoptind = 1;
+    const char *myoptarg = nullptr;
+
+    char *verb = nullptr;
+
+    if (argc >= 2) {
+        verb = argv[1];
+    } else {
+        return 1;
+    }
+
+    while ((ch = px4_getopt(argc, argv, "d:", &myoptind, &myoptarg)) != EOF) {
+        switch (ch) {
+        case 'd':
+            device = myoptarg;
+            strncpy(pwm_out::_device, device, strlen(device));
+            break;
+        }
+    }
+
+    // gets the parameters for the esc's pwm
+    param_get(param_find("PWM_DISARMED"), &pwm_out::_pwm_disarmed);
+    param_get(param_find("PWM_MIN"), &pwm_out::_pwm_min);
+    param_get(param_find("PWM_MAX"), &pwm_out::_pwm_max);
+
+    /*
+     * Start/load the driver.
+     */
+    if (!strcmp(verb, "start")) {
+        if (pwm_out::_is_running) {
+            PX4_WARN("pwm_out already running");
+            return 1;
+        }
+
+        pwm_out::start();
+    }
+
+    else if (!strcmp(verb, "stop")) {
+        if (!pwm_out::_is_running) {
+            PX4_WARN("pwm_out is not running");
+            return 1;
+        }
+
+        pwm_out::stop();
+    }
+
+    else if (!strcmp(verb, "status")) {
+        PX4_WARN("pwm_out is %s", pwm_out::_is_running ? "running" : "not running");
+        return 0;
+
+    } else {
+        pwm_out::usage();
+        return 1;
+    }
+
+    return 0;
+}
diff --git a/src/modules/commander/PreflightCheck.cpp b/src/modules/commander/PreflightCheck.cpp
index 1f31586e19..a11432c994 100644
--- a/src/modules/commander/PreflightCheck.cpp
+++ b/src/modules/commander/PreflightCheck.cpp
@@ -391,6 +391,9 @@ bool preflightCheck(orb_advert_t *mavlink_log_pub, bool checkMag, bool checkAcc,
 	// all the sensors are supported
 	PX4_WARN("Preflight checks always pass on Snapdragon.");
 	return true;
+#elif defined(__LINUX)
+	PX4_WARN("Preflight checks always pass on Linux (RPI).");
+	return true;
 #endif
 
 	bool failed = false;
diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp
index 30ad39d3d3..42c17a8fdc 100644
--- a/src/modules/mavlink/mavlink_main.cpp
+++ b/src/modules/mavlink/mavlink_main.cpp
@@ -863,6 +863,9 @@ Mavlink::get_free_tx_buf()
 		// No FIONWRITE on Linux
 #if !defined(__PX4_LINUX) && !defined(__PX4_DARWIN)
 		(void) ioctl(_uart_fd, FIONWRITE, (unsigned long)&buf_free);
+#else
+        //Linux cp210x does not support TIOCOUTQ
+        buf_free = 256;
 #endif
 
 		if (get_flow_control_enabled() && buf_free < FLOW_CONTROL_DISABLE_THRESHOLD) {
diff --git a/src/modules/rcinput/CMakeLists.txt b/src/modules/rcinput/CMakeLists.txt
new file mode 100644
index 0000000000..5008242d19
--- /dev/null
+++ b/src/modules/rcinput/CMakeLists.txt
@@ -0,0 +1,43 @@
+############################################################################
+#
+#   Copyright (c) 2016 PX4 Development Team. All rights reserved.
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions
+# are met:
+#
+# 1. Redistributions of source code must retain the above copyright
+#    notice, this list of conditions and the following disclaimer.
+# 2. Redistributions in binary form must reproduce the above copyright
+#    notice, this list of conditions and the following disclaimer in
+#    the documentation and/or other materials provided with the
+#    distribution.
+# 3. Neither the name PX4 nor the names of its contributors may be
+#    used to endorse or promote products derived from this software
+#    without specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.
+#
+############################################################################
+px4_add_module(
+	MODULE modules__rcinput
+	MAIN rcinput
+	STACK_MAIN 1200
+	COMPILE_FLAGS -Os
+	SRCS
+		rcinput.cpp
+	DEPENDS
+		platforms__common
+	)
+# vim: set noet ft=cmake fenc=utf-8 ff=unix :
diff --git a/src/modules/rcinput/rcinput.cpp b/src/modules/rcinput/rcinput.cpp
new file mode 100644
index 0000000000..22a835ee0f
--- /dev/null
+++ b/src/modules/rcinput/rcinput.cpp
@@ -0,0 +1,297 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2015 Mark Charlebois. All rights reserved.
+ *   Copyright (C) 2016 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+#include <sys/types.h>
+#include <sys/stat.h>
+#include <fcntl.h>
+#include <stdio.h>
+#include <stdint.h>
+
+#include <px4_config.h>
+#include <px4_workqueue.h>
+#include <px4_defines.h>
+
+#include <drivers/drv_hrt.h>
+
+#include <uORB/uORB.h>
+#include <uORB/topics/input_rc.h>
+
+namespace rcinput
+{
+
+extern "C" __EXPORT int rcinput_main(int argc, char *argv[]);
+
+#define RCINPUT_DEVICE_PATH_BASE "/sys/kernel/rcio/rcin"
+
+#define RCINPUT_MEASURE_INTERVAL_US 20000 // microseconds
+
+
+class RcInput
+{
+public:
+	RcInput() :
+        _shouldExit(false),
+        _isRunning(false),
+        _work{},
+        _rcinput_pub(nullptr),
+        _channels(8), //D8R-II plus
+        _data{}
+    {
+        memset(_ch_fd, 0, sizeof(_ch_fd));
+    }
+    ~RcInput()
+    {
+        work_cancel(HPWORK, &_work);
+        _isRunning = false;
+    }
+
+	/* @return 0 on success, -errno on failure */
+	int start();
+
+	/* @return 0 on success, -errno on failure */
+	void stop();
+
+	/* Trampoline for the work queue. */
+	static void cycle_trampoline(void *arg);
+
+    bool isRunning() { return _isRunning; }
+
+private:
+    void _cycle();
+    void _measure();
+
+    bool _shouldExit;
+    bool _isRunning;
+    struct work_s _work;
+
+    orb_advert_t _rcinput_pub;
+
+    int _channels;
+    int _ch_fd[input_rc_s::RC_INPUT_MAX_CHANNELS];
+    struct input_rc_s _data;
+
+	int navio_rc_init();
+};
+
+int RcInput::navio_rc_init()
+{
+    int i;
+    char *buf;
+
+    for (i = 0; i < _channels; ++i) {
+        ::asprintf(&buf, "%s/ch%d", RCINPUT_DEVICE_PATH_BASE, i);
+        int fd = ::open(buf, O_RDONLY);
+        ::free(buf);
+        if (fd < 0) {
+            PX4_WARN("error: open %d failed", i);
+            break;
+        }
+        _ch_fd[i] = fd;
+    }
+    for (; i < input_rc_s::RC_INPUT_MAX_CHANNELS; ++i) {
+        _data.values[i] = UINT16_MAX;
+    }
+
+    _rcinput_pub = orb_advertise(ORB_ID(input_rc), &_data);
+    if (_rcinput_pub == nullptr) {
+        PX4_WARN("error: advertise failed");
+        return -1;
+    }
+
+	return 0;
+}
+
+int RcInput::start()
+{
+	int result = 0;
+
+	result = navio_rc_init();
+
+	if (result != 0) {
+		PX4_WARN("error: RC initialization failed");
+        return -1;
+	}
+
+    _isRunning = true;
+	result = work_queue(HPWORK, &_work, (worker_t)&RcInput::cycle_trampoline, this, 0);
+    if (result == -1) {
+        _isRunning = false;
+    }
+
+    return result;
+}
+
+void RcInput::stop()
+{
+    _shouldExit = true;
+}
+
+void RcInput::cycle_trampoline(void *arg)
+{
+    RcInput *dev = reinterpret_cast<RcInput *>(arg);
+    dev->_cycle();
+}
+
+void RcInput::_cycle()
+{
+    _measure();
+
+    if (!_shouldExit) {
+        work_queue(HPWORK, &_work, (worker_t)&RcInput::cycle_trampoline, this,
+                   USEC2TICK(RCINPUT_MEASURE_INTERVAL_US));
+    }
+}
+
+void RcInput::_measure(void)
+{
+    uint64_t ts;
+    char buf[12];
+
+    for (int i = 0; i < _channels; ++i) {
+        int res;
+        if ((res = ::pread(_ch_fd[i], buf, sizeof(buf) - 1, 0)) < 0) {
+            _data.values[i] = UINT16_MAX;
+            continue;
+        }
+        buf[sizeof(buf) -1] = '\0';
+
+        _data.values[i] = atoi(buf);
+    }
+
+    ts = hrt_absolute_time();
+    _data.timestamp_publication = ts;
+    _data.timestamp_last_signal = ts;
+    _data.channel_count = _channels;
+    _data.rssi = 100;
+    _data.rc_lost_frame_count = 0;
+    _data.rc_total_frame_count = 1;
+    _data.rc_ppm_frame_length = 100;
+    _data.rc_failsafe = false;
+    _data.rc_lost = false;
+    _data.input_source = input_rc_s::RC_INPUT_SOURCE_PX4IO_PPM;
+
+	orb_publish(ORB_ID(input_rc), _rcinput_pub, &_data);
+}
+
+/**
+ * Print the correct usage.
+ */
+static void usage(const char *reason);
+
+static void
+usage(const char *reason)
+{
+	if (reason) {
+		PX4_ERR("%s", reason);
+	}
+
+	PX4_INFO("usage: rcinput {start|stop|status}");
+}
+
+static RcInput *rc_input = nullptr;
+
+int rcinput_main(int argc, char *argv[])
+{
+	if (argc < 2) {
+		usage("missing command");
+		return 1;
+	}
+
+	if (!strcmp(argv[1], "start")) {
+
+		if (rc_input != nullptr && rc_input->isRunning()) {
+			PX4_WARN("already running");
+			/* this is not an error */
+			return 0;
+		}
+
+		rc_input = new RcInput();
+
+		// Check if alloc worked.
+		if (rc_input == nullptr) {
+			PX4_ERR("alloc failed");
+			return -1;
+		}
+
+		int ret = rc_input->start();
+
+		if (ret != 0) {
+			PX4_ERR("start failed");
+		}
+
+		return 0;
+	}
+
+	if (!strcmp(argv[1], "stop")) {
+
+		if (rc_input == nullptr || rc_input->isRunning()) {
+			PX4_WARN("not running");
+			/* this is not an error */
+			return 0;
+		}
+
+		rc_input->stop();
+
+		// Wait for task to die
+		int i = 0;
+
+		do {
+			/* wait up to 3s */
+			usleep(100000);
+
+		} while (rc_input->isRunning() && ++i < 30);
+
+		delete rc_input;
+		rc_input = nullptr;
+
+		return 0;
+	}
+
+	if (!strcmp(argv[1], "status")) {
+		if (rc_input != nullptr && rc_input->isRunning()) {
+			PX4_INFO("running");
+
+		} else {
+			PX4_INFO("not running\n");
+		}
+
+		return 0;
+	}
+
+	usage("unrecognized command");
+	return 1;
+
+}
+
+}; // namespace rcinput
diff --git a/src/platforms/posix/px4_layer/px4_posix_tasks.cpp b/src/platforms/posix/px4_layer/px4_posix_tasks.cpp
index 53cc821b2f..908a444802 100644
--- a/src/platforms/posix/px4_layer/px4_posix_tasks.cpp
+++ b/src/platforms/posix/px4_layer/px4_posix_tasks.cpp
@@ -90,11 +90,15 @@ static void *entry_adapter(void *ptr)
 	pthdata_t *data = (pthdata_t *) ptr;
 
 	int rv;
+
 	// set the threads name
 #ifdef __PX4_DARWIN
 	rv = pthread_setname_np(data->name);
 #else
-	rv = pthread_setname_np(pthread_self(), data->name);
+	char buf[17];
+	snprintf(buf, 16, "%s", data->name);
+	buf[16] = '0';
+	rv = pthread_setname_np(pthread_self(), buf);
 #endif
 
 	if (rv) {

From b871b322d2bded6fb2313803e7165fe6429c9fea Mon Sep 17 00:00:00 2001
From: Miguel Arroyo <miguel@cs.columbia.edu>
Date: Thu, 30 Jun 2016 10:21:33 -0400
Subject: [PATCH 0882/1230] Using Actuator Control Group 0 & Checkstyle fixes

---
 src/drivers/pwm_out/pwm_out.cpp | 528 ++++++++++++++++----------------
 1 file changed, 269 insertions(+), 259 deletions(-)

diff --git a/src/drivers/pwm_out/pwm_out.cpp b/src/drivers/pwm_out/pwm_out.cpp
index 0fb4ebd35e..12c4e7644b 100644
--- a/src/drivers/pwm_out/pwm_out.cpp
+++ b/src/drivers/pwm_out/pwm_out.cpp
@@ -60,6 +60,7 @@ static const int NUM_PWM = 4;
 static char _device[32] = "/sys/class/pwm/pwmchip0";
 static int  _pwm_fd[NUM_PWM];
 
+static const int FREQUENCY_PWM = 400;
 static const char *MIXER_FILENAME = "";
 
 // subscriptions
@@ -108,318 +109,326 @@ int mixer_control_callback(uintptr_t handle, uint8_t control_group, uint8_t cont
 
 
 int mixer_control_callback(uintptr_t handle,
-               uint8_t control_group,
-               uint8_t control_index,
-               float &input)
+			   uint8_t control_group,
+			   uint8_t control_index,
+			   float &input)
 {
-    const actuator_controls_s *controls = (actuator_controls_s *)handle;
+	const actuator_controls_s *controls = (actuator_controls_s *)handle;
 
-    input = controls[control_group].control[control_index];
+	input = controls[control_group].control[control_index];
 
-    return 0;
+	return 0;
 }
 
 
 int initialize_mixer(const char *mixer_filename)
 {
-    char buf[2048];
-    size_t buflen = sizeof(buf);
+	char buf[2048];
+	size_t buflen = sizeof(buf);
 
-    PX4_INFO("Trying to initialize mixer from config file %s", mixer_filename);
+	PX4_INFO("Trying to initialize mixer from config file %s", mixer_filename);
 
-    int fd_load = ::open(mixer_filename, O_RDONLY);
+	int fd_load = ::open(mixer_filename, O_RDONLY);
 
-    if (fd_load != -1) {
-        int nRead = ::read(fd_load, buf, buflen);
-        close(fd_load);
+	if (fd_load != -1) {
+		int nRead = ::read(fd_load, buf, buflen);
+		close(fd_load);
 
-        if (nRead > 0) {
-            _mixer = MultirotorMixer::from_text(mixer_control_callback, (uintptr_t)&_controls, buf, buflen);
+		if (nRead > 0) {
+			_mixer = MultirotorMixer::from_text(mixer_control_callback, (uintptr_t)&_controls, buf, buflen);
 
-            if (_mixer != nullptr) {
-                PX4_INFO("Successfully initialized mixer from config file");
-                return 0;
+			if (_mixer != nullptr) {
+				PX4_INFO("Successfully initialized mixer from config file");
+				return 0;
 
-            } else {
-                PX4_ERR("Unable to parse from mixer config file");
-                return -1;
-            }
+			} else {
+				PX4_ERR("Unable to parse from mixer config file");
+				return -1;
+			}
 
-        } else {
-            PX4_WARN("Unable to read from mixer config file");
-            return -2;
-        }
+		} else {
+			PX4_WARN("Unable to read from mixer config file");
+			return -2;
+		}
 
-    } else {
-        PX4_WARN("No mixer config file found, using default mixer.");
+	} else {
+		PX4_WARN("No mixer config file found, using default mixer.");
 
-        /* Mixer file loading failed, fall back to default mixer configuration for
-        * QUAD_X airframe. */
-        float roll_scale = 1;
-        float pitch_scale = 1;
-        float yaw_scale = 1;
-        float deadband = 0;
+		/* Mixer file loading failed, fall back to default mixer configuration for
+		* QUAD_X airframe. */
+		float roll_scale = 1;
+		float pitch_scale = 1;
+		float yaw_scale = 1;
+		float deadband = 0;
 
-        _mixer = new MultirotorMixer(mixer_control_callback, (uintptr_t)&_controls,
-                         MultirotorGeometry::QUAD_X,
-                         roll_scale, pitch_scale, yaw_scale, deadband);
+		_mixer = new MultirotorMixer(mixer_control_callback, (uintptr_t)&_controls,
+					     MultirotorGeometry::QUAD_X,
+					     roll_scale, pitch_scale, yaw_scale, deadband);
 
-        // TODO: temporary hack to make this compile
-        (void)_config_index[0];
+		// TODO: temporary hack to make this compile
+		(void)_config_index[0];
 
-        if (_mixer == nullptr) {
-            PX4_ERR("Mixer initialization failed");
-            return -1;
-        }
+		if (_mixer == nullptr) {
+			PX4_ERR("Mixer initialization failed");
+			return -1;
+		}
 
-        return 0;
-    }
+		return 0;
+	}
 }
 
 int pwm_write_sysfs(char *path, int value)
 {
-    int fd = ::open(path, O_WRONLY | O_CLOEXEC);
-    int n;
-    char *data;
+	int fd = ::open(path, O_WRONLY | O_CLOEXEC);
+	int n;
+	char *data;
 
-    ::free(path);
-    if (fd == -1) {
-        return -errno;
-    }
-    n = ::asprintf(&data, "%u", value);
-    if (n > 0) {
-        ::write(fd, data, n);
-        ::free(data);
-    }
-    ::close(fd);
+	::free(path);
 
-    return 0;
+	if (fd == -1) {
+		return -errno;
+	}
+
+	n = ::asprintf(&data, "%u", value);
+
+	if (n > 0) {
+		::write(fd, data, n);
+		::free(data);
+	}
+
+	::close(fd);
+
+	return 0;
 }
 
 int pwm_initialize(const char *device)
 {
-    int i;
-    char *path;
+	int i;
+	char *path;
 
-    for (i = 0; i < NUM_PWM; ++i) {
-        ::asprintf(&path, "%s/export", device);
-        if (pwm_write_sysfs(path, i) < 0) {
-            PX4_ERR("PWM export failed");
-        }
-    }
+	for (i = 0; i < NUM_PWM; ++i) {
+		::asprintf(&path, "%s/export", device);
 
-    for (i = 0; i < NUM_PWM; ++i) {
-        ::asprintf(&path, "%s/pwm%u/enable", device, i);
-        if (pwm_write_sysfs(path, 1) < 0) {
-            PX4_ERR("PWM enable failed");
-        }
-    }
+		if (pwm_write_sysfs(path, i) < 0) {
+			PX4_ERR("PWM export failed");
+		}
+	}
 
-    for (i = 0; i < NUM_PWM; ++i) {
-        ::asprintf(&path, "%s/pwm%u/period", device, i);
-        if (pwm_write_sysfs(path, (int)1e9/50)) {
-            PX4_ERR("PWM period failed");
-        }
-    }
+	for (i = 0; i < NUM_PWM; ++i) {
+		::asprintf(&path, "%s/pwm%u/enable", device, i);
 
-    for (i = 0; i < NUM_PWM; ++i) {
-        ::asprintf(&path, "%s/pwm%u/duty_cycle", device, i);
-        _pwm_fd[i] = ::open(path, O_WRONLY | O_CLOEXEC);
-        ::free(path);
-        if (_pwm_fd[i] == -1) {
-            PX4_ERR("PWM: Failed to open duty_cycle.");
-            return -errno;
-        }
-    }
+		if (pwm_write_sysfs(path, 1) < 0) {
+			PX4_ERR("PWM enable failed");
+		}
+	}
 
-    return 0;
+	for (i = 0; i < NUM_PWM; ++i) {
+		::asprintf(&path, "%s/pwm%u/period", device, i);
+
+		if (pwm_write_sysfs(path, (int)1e9 / FREQUENCY_PWM)) {
+			PX4_ERR("PWM period failed");
+		}
+	}
+
+	for (i = 0; i < NUM_PWM; ++i) {
+		::asprintf(&path, "%s/pwm%u/duty_cycle", device, i);
+		_pwm_fd[i] = ::open(path, O_WRONLY | O_CLOEXEC);
+		::free(path);
+
+		if (_pwm_fd[i] == -1) {
+			PX4_ERR("PWM: Failed to open duty_cycle.");
+			return -errno;
+		}
+	}
+
+	return 0;
 }
 
 void pwm_deinitialize()
 {
-    for (int i = 0; i < NUM_PWM; ++i) {
-        if (_pwm_fd[i] != -1) {
-            ::close(_pwm_fd[i]);
-        }
-    }
+	for (int i = 0; i < NUM_PWM; ++i) {
+		if (_pwm_fd[i] != -1) {
+			::close(_pwm_fd[i]);
+		}
+	}
 }
 
 void send_outputs_pwm(const uint16_t *pwm)
 {
-    int n;
-    char *data;
+	int n;
+	char *data;
 
-    //convert this to duty_cycle in ns
-    for (unsigned i = 0; i < NUM_PWM; ++i) {
-        n = ::asprintf(&data, "%u", pwm[i] * 1000);
-        ::write(_pwm_fd[i], data, n);
-    }
+	//convert this to duty_cycle in ns
+	for (unsigned i = 0; i < NUM_PWM; ++i) {
+		n = ::asprintf(&data, "%u", pwm[i] * 1000);
+		::write(_pwm_fd[i], data, n);
+	}
 }
 
 
 
 void task_main(int argc, char *argv[])
 {
-    _is_running = true;
+	_is_running = true;
 
-    if (pwm_initialize(_device) < 0) {
-        PX4_ERR("Failed to initialize PWM.");
-        return;
-    }
+	if (pwm_initialize(_device) < 0) {
+		PX4_ERR("Failed to initialize PWM.");
+		return;
+	}
 
-    // Subscribe for orb topics
-    _controls_sub = orb_subscribe(ORB_ID(actuator_controls_3));
-    _armed_sub = orb_subscribe(ORB_ID(actuator_armed));
+	// Subscribe for orb topics
+	_controls_sub = orb_subscribe(ORB_ID(actuator_controls_0));
+	_armed_sub = orb_subscribe(ORB_ID(actuator_armed));
 
-    // Start disarmed
-    _armed.armed = false;
-    _armed.prearmed = false;
+	// Start disarmed
+	_armed.armed = false;
+	_armed.prearmed = false;
 
-    // Set up poll topic
-    px4_pollfd_struct_t fds[1];
-    fds[0].fd     = _controls_sub;
-    fds[0].events = POLLIN;
-    /* Don't limit poll intervall for now, 250 Hz should be fine. */
-    //orb_set_interval(_controls_sub, 10);
+	// Set up poll topic
+	px4_pollfd_struct_t fds[1];
+	fds[0].fd     = _controls_sub;
+	fds[0].events = POLLIN;
+	/* Don't limit poll intervall for now, 250 Hz should be fine. */
+	//orb_set_interval(_controls_sub, 10);
 
-    // Set up mixer
-    if (initialize_mixer(MIXER_FILENAME) < 0) {
-        PX4_ERR("Mixer initialization failed.");
-        return;
-    }
+	// Set up mixer
+	if (initialize_mixer(MIXER_FILENAME) < 0) {
+		PX4_ERR("Mixer initialization failed.");
+		return;
+	}
 
-    pwm_limit_init(&_pwm_limit);
+	pwm_limit_init(&_pwm_limit);
 
-    // Main loop
-    while (!_task_should_exit) {
+	// Main loop
+	while (!_task_should_exit) {
 
-        int pret = px4_poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 10);
+		int pret = px4_poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 10);
 
-        /* Timed out, do a periodic check for _task_should_exit. */
-        if (pret == 0) {
-            continue;
-        }
+		/* Timed out, do a periodic check for _task_should_exit. */
+		if (pret == 0) {
+			continue;
+		}
 
-        /* This is undesirable but not much we can do. */
-        if (pret < 0) {
-            PX4_WARN("poll error %d, %d", pret, errno);
-            /* sleep a bit before next try */
-            usleep(100000);
-            continue;
-        }
+		/* This is undesirable but not much we can do. */
+		if (pret < 0) {
+			PX4_WARN("poll error %d, %d", pret, errno);
+			/* sleep a bit before next try */
+			usleep(100000);
+			continue;
+		}
 
-        if (fds[0].revents & POLLIN) {
-            orb_copy(ORB_ID(actuator_controls_3), _controls_sub, &_controls);
+		if (fds[0].revents & POLLIN) {
+			orb_copy(ORB_ID(actuator_controls_0), _controls_sub, &_controls);
 
-            _outputs.timestamp = _controls.timestamp;
+			_outputs.timestamp = _controls.timestamp;
 
-            /* do mixing */
-            _outputs.noutputs = _mixer->mix(_outputs.output,
-                                            0 /* not used */,
-                                            NULL);
+			/* do mixing */
+			_outputs.noutputs = _mixer->mix(_outputs.output,
+							0 /* not used */,
+							NULL);
 
 
-            /* disable unused ports by setting their output to NaN */
-            for (size_t i = _outputs.noutputs;
-                 i < sizeof(_outputs.output) / sizeof(_outputs.output[0]);
-                 i++) {
-                _outputs.output[i] = NAN;
-            }
+			/* disable unused ports by setting their output to NaN */
+			for (size_t i = _outputs.noutputs;
+			     i < sizeof(_outputs.output) / sizeof(_outputs.output[0]);
+			     i++) {
+				_outputs.output[i] = NAN;
+			}
 
-            const uint16_t reverse_mask = 0;
-            uint16_t disarmed_pwm[4];
-            uint16_t min_pwm[4];
-            uint16_t max_pwm[4];
+			const uint16_t reverse_mask = 0;
+			uint16_t disarmed_pwm[4];
+			uint16_t min_pwm[4];
+			uint16_t max_pwm[4];
 
-            for (unsigned int i = 0; i < 4; i++) {
-                disarmed_pwm[i] = _pwm_disarmed;
-                min_pwm[i] = _pwm_min;
-                max_pwm[i] = _pwm_max;
-            }
+			for (unsigned int i = 0; i < 4; i++) {
+				disarmed_pwm[i] = _pwm_disarmed;
+				min_pwm[i] = _pwm_min;
+				max_pwm[i] = _pwm_max;
+			}
 
-            uint16_t pwm[4];
+			uint16_t pwm[4];
 
-            // TODO FIXME: pre-armed seems broken
-            pwm_limit_calc(_armed.armed,
-                           false/*_armed.prearmed*/,
-                           _outputs.noutputs,
-                           reverse_mask,
-                           disarmed_pwm,
-                           min_pwm,
-                           max_pwm,
-                           _outputs.output,
-                           pwm,
-                           &_pwm_limit);
+			// TODO FIXME: pre-armed seems broken
+			pwm_limit_calc(_armed.armed,
+				       false/*_armed.prearmed*/,
+				       _outputs.noutputs,
+				       reverse_mask,
+				       disarmed_pwm,
+				       min_pwm,
+				       max_pwm,
+				       _outputs.output,
+				       pwm,
+				       &_pwm_limit);
 
-            send_outputs_pwm(pwm);
+			send_outputs_pwm(pwm);
 
-            if (_outputs_pub != nullptr) {
-                orb_publish(ORB_ID(actuator_outputs), _outputs_pub, &_outputs);
+			if (_outputs_pub != nullptr) {
+				orb_publish(ORB_ID(actuator_outputs), _outputs_pub, &_outputs);
 
-            } else {
-                _outputs_pub = orb_advertise(ORB_ID(actuator_outputs), &_outputs);
-            }
-        }
+			} else {
+				_outputs_pub = orb_advertise(ORB_ID(actuator_outputs), &_outputs);
+			}
+		}
 
-        bool updated;
-        orb_check(_armed_sub, &updated);
+		bool updated;
+		orb_check(_armed_sub, &updated);
 
-        if (updated) {
-            orb_copy(ORB_ID(actuator_armed), _armed_sub, &_armed);
-        }
-    }
+		if (updated) {
+			orb_copy(ORB_ID(actuator_armed), _armed_sub, &_armed);
+		}
+	}
 
-    pwm_deinitialize();
-    orb_unsubscribe(_controls_sub);
-    orb_unsubscribe(_armed_sub);
+	pwm_deinitialize();
+	orb_unsubscribe(_controls_sub);
+	orb_unsubscribe(_armed_sub);
 
-    _is_running = false;
+	_is_running = false;
 
 }
 
 void task_main_trampoline(int argc, char *argv[])
 {
-    task_main(argc, argv);
+	task_main(argc, argv);
 }
 
 void start()
 {
-    ASSERT(_task_handle == -1);
+	ASSERT(_task_handle == -1);
 
-    _task_should_exit = false;
+	_task_should_exit = false;
 
-    /* start the task */
-    _task_handle = px4_task_spawn_cmd("pwm_out_main",
-                      SCHED_DEFAULT,
-                      SCHED_PRIORITY_MAX,
-                      1500,
-                      (px4_main_t)&task_main_trampoline,
-                      nullptr);
+	/* start the task */
+	_task_handle = px4_task_spawn_cmd("pwm_out_main",
+					  SCHED_DEFAULT,
+					  SCHED_PRIORITY_MAX,
+					  1500,
+					  (px4_main_t)&task_main_trampoline,
+					  nullptr);
 
-    if (_task_handle < 0) {
-        warn("task start failed");
-        return;
-    }
+	if (_task_handle < 0) {
+		warn("task start failed");
+		return;
+	}
 
 }
 
 void stop()
 {
-    _task_should_exit = true;
+	_task_should_exit = true;
 
-    while (_is_running) {
-        usleep(200000);
-        PX4_INFO(".");
-    }
+	while (_is_running) {
+		usleep(200000);
+		PX4_INFO(".");
+	}
 
-    _task_handle = -1;
+	_task_handle = -1;
 }
 
 void usage()
 {
-    PX4_INFO("usage: pwm_out start -d /sys/class/pwm/pwmchip0");
-    PX4_INFO("       pwm_out stop");
-    PX4_INFO("       pwm_out status");
+	PX4_INFO("usage: pwm_out start -d /sys/class/pwm/pwmchip0");
+	PX4_INFO("       pwm_out stop");
+	PX4_INFO("       pwm_out status");
 }
 
 } // namespace pwm_out
@@ -429,62 +438,63 @@ extern "C" __EXPORT int pwm_out_main(int argc, char *argv[]);
 
 int pwm_out_main(int argc, char *argv[])
 {
-    const char *device = nullptr;
-    int ch;
-    int myoptind = 1;
-    const char *myoptarg = nullptr;
+	const char *device = nullptr;
+	int ch;
+	int myoptind = 1;
+	const char *myoptarg = nullptr;
 
-    char *verb = nullptr;
+	char *verb = nullptr;
 
-    if (argc >= 2) {
-        verb = argv[1];
-    } else {
-        return 1;
-    }
+	if (argc >= 2) {
+		verb = argv[1];
 
-    while ((ch = px4_getopt(argc, argv, "d:", &myoptind, &myoptarg)) != EOF) {
-        switch (ch) {
-        case 'd':
-            device = myoptarg;
-            strncpy(pwm_out::_device, device, strlen(device));
-            break;
-        }
-    }
+	} else {
+		return 1;
+	}
 
-    // gets the parameters for the esc's pwm
-    param_get(param_find("PWM_DISARMED"), &pwm_out::_pwm_disarmed);
-    param_get(param_find("PWM_MIN"), &pwm_out::_pwm_min);
-    param_get(param_find("PWM_MAX"), &pwm_out::_pwm_max);
+	while ((ch = px4_getopt(argc, argv, "d:", &myoptind, &myoptarg)) != EOF) {
+		switch (ch) {
+		case 'd':
+			device = myoptarg;
+			strncpy(pwm_out::_device, device, strlen(device));
+			break;
+		}
+	}
 
-    /*
-     * Start/load the driver.
-     */
-    if (!strcmp(verb, "start")) {
-        if (pwm_out::_is_running) {
-            PX4_WARN("pwm_out already running");
-            return 1;
-        }
+	// gets the parameters for the esc's pwm
+	param_get(param_find("PWM_DISARMED"), &pwm_out::_pwm_disarmed);
+	param_get(param_find("PWM_MIN"), &pwm_out::_pwm_min);
+	param_get(param_find("PWM_MAX"), &pwm_out::_pwm_max);
 
-        pwm_out::start();
-    }
+	/*
+	 * Start/load the driver.
+	 */
+	if (!strcmp(verb, "start")) {
+		if (pwm_out::_is_running) {
+			PX4_WARN("pwm_out already running");
+			return 1;
+		}
 
-    else if (!strcmp(verb, "stop")) {
-        if (!pwm_out::_is_running) {
-            PX4_WARN("pwm_out is not running");
-            return 1;
-        }
+		pwm_out::start();
+	}
 
-        pwm_out::stop();
-    }
+	else if (!strcmp(verb, "stop")) {
+		if (!pwm_out::_is_running) {
+			PX4_WARN("pwm_out is not running");
+			return 1;
+		}
 
-    else if (!strcmp(verb, "status")) {
-        PX4_WARN("pwm_out is %s", pwm_out::_is_running ? "running" : "not running");
-        return 0;
+		pwm_out::stop();
+	}
 
-    } else {
-        pwm_out::usage();
-        return 1;
-    }
+	else if (!strcmp(verb, "status")) {
+		PX4_WARN("pwm_out is %s", pwm_out::_is_running ? "running" : "not running");
+		return 0;
 
-    return 0;
+	} else {
+		pwm_out::usage();
+		return 1;
+	}
+
+	return 0;
 }

From 506d1855ffc9ae0e077a82d5e984c0d5f6b0c0f2 Mon Sep 17 00:00:00 2001
From: Hidenori <hidenori.kobayashi@gmail.com>
Date: Thu, 30 Jun 2016 14:54:58 -0400
Subject: [PATCH 0883/1230] rename files and add navio target

---
 Makefile                                      |   3 +
 cmake/configs/posix_navio2_release.cmake      | 114 +++++++++++
 .../CMakeLists.txt                            |   7 +-
 .../navio_sysfs_pwm_out.cpp}                  |  28 +--
 .../navio_sysfs_rc_in}/CMakeLists.txt         |   6 +-
 .../navio_sysfs_rc_in/navio_sysfs_rc_in.cpp}  | 177 +++++++++---------
 src/lib/version/version.h                     |   2 +-
 7 files changed, 230 insertions(+), 107 deletions(-)
 create mode 100644 cmake/configs/posix_navio2_release.cmake
 rename src/drivers/{pwm_out => navio_sysfs_pwm_out}/CMakeLists.txt (95%)
 rename src/drivers/{pwm_out/pwm_out.cpp => navio_sysfs_pwm_out/navio_sysfs_pwm_out.cpp} (93%)
 rename src/{modules/rcinput => drivers/navio_sysfs_rc_in}/CMakeLists.txt (95%)
 rename src/{modules/rcinput/rcinput.cpp => drivers/navio_sysfs_rc_in/navio_sysfs_rc_in.cpp} (62%)

diff --git a/Makefile b/Makefile
index 61ba857dbd..e97f0c43ce 100644
--- a/Makefile
+++ b/Makefile
@@ -211,6 +211,9 @@ posix_rpi2_release:
 posix_bebop_default:
 	$(call cmake-build,$@)
 
+posix_navio2_release:
+	$(call cmake-build,$@)
+
 posix: posix_sitl_default
 
 broadcast: posix_sitl_broadcast
diff --git a/cmake/configs/posix_navio2_release.cmake b/cmake/configs/posix_navio2_release.cmake
new file mode 100644
index 0000000000..402b74dba8
--- /dev/null
+++ b/cmake/configs/posix_navio2_release.cmake
@@ -0,0 +1,114 @@
+include(posix/px4_impl_posix)
+
+set(CMAKE_TOOLCHAIN_FILE ${CMAKE_SOURCE_DIR}/cmake/toolchains/Toolchain-arm-linux-gnueabihf-raspbian.cmake)
+
+set(CMAKE_PROGRAM_PATH
+	"${RPI_TOOLCHAIN_DIR}/gcc-linaro-arm-linux-gnueabihf-raspbian/bin"
+	${CMAKE_PROGRAM_PATH}
+)
+
+# This definition allows to differentiate if this just the usual POSIX build
+# or if it is for the RPi.
+add_definitions(
+	-D__PX4_POSIX_RPI2
+	-D__LINUX
+)
+
+set(config_module_list
+	#
+	# Board support modules
+	#
+	drivers/device
+	modules/sensors
+	platforms/posix/drivers/df_mpu9250_wrapper
+	platforms/posix/drivers/df_lsm9ds1_wrapper
+	platforms/posix/drivers/df_ms5611_wrapper
+	platforms/posix/drivers/df_hmc5883_wrapper
+	platforms/posix/drivers/df_trone_wrapper
+	platforms/posix/drivers/df_isl29501_wrapper
+
+	#
+	# System commands
+	#
+	systemcmds/param
+	systemcmds/mixer
+	systemcmds/ver
+	systemcmds/esc_calib
+	systemcmds/topic_listener
+	systemcmds/perf
+
+	#
+	# Estimation modules (EKF/ SO3 / other filters)
+	#
+	#modules/attitude_estimator_ekf
+	modules/ekf_att_pos_estimator
+	modules/attitude_estimator_q
+	modules/position_estimator_inav
+	modules/local_position_estimator
+	modules/ekf2
+
+	#
+	# Vehicle Control
+	#
+	modules/mc_att_control
+	modules/mc_pos_control
+	modules/fw_att_control
+	modules/fw_pos_control_l1	
+	modules/vtol_att_control
+
+	#
+	# Library modules
+	#
+	modules/sdlog2
+	modules/logger
+	modules/commander
+	modules/load_mon
+	modules/param
+	modules/systemlib
+	modules/systemlib/mixer
+	modules/uORB
+	modules/dataman
+	modules/land_detector
+	modules/navigator
+	modules/mavlink
+
+	#
+	# PX4 drivers
+	#
+	drivers/gps
+	drivers/navio_sysfs_rc_in
+	drivers/navio_sysfs_pwm_out
+
+	#
+	# Libraries
+	#
+	lib/controllib
+	lib/mathlib
+	lib/mathlib/math/filter
+	lib/geo
+	lib/ecl
+	lib/geo_lookup
+	lib/launchdetection
+	lib/external_lgpl
+	lib/conversion
+	lib/terrain_estimation
+	lib/runway_takeoff
+	lib/tailsitter_recovery
+	lib/DriverFramework/framework
+
+	#
+	# POSIX
+	#
+	platforms/common
+	platforms/posix/px4_layer
+	platforms/posix/work_queue
+)
+
+set(config_df_driver_list
+	mpu9250
+	lsm9ds1
+	ms5611
+	hmc5883
+	trone
+	isl29501
+)
diff --git a/src/drivers/pwm_out/CMakeLists.txt b/src/drivers/navio_sysfs_pwm_out/CMakeLists.txt
similarity index 95%
rename from src/drivers/pwm_out/CMakeLists.txt
rename to src/drivers/navio_sysfs_pwm_out/CMakeLists.txt
index ff6158c0cd..36841dbe6d 100644
--- a/src/drivers/pwm_out/CMakeLists.txt
+++ b/src/drivers/navio_sysfs_pwm_out/CMakeLists.txt
@@ -31,13 +31,12 @@
 #
 ############################################################################
 px4_add_module(
-	MODULE drivers__pwm_out
-	MAIN pwm_out
+	MODULE drivers__navio_sysfs_pwm_out
+	MAIN navio_sysfs_pwm_out
 	COMPILE_FLAGS
 		-Os
-		-DMAVLINK_COMM_NUM_BUFFERS=1
 	SRCS
-		pwm_out.cpp
+		navio_sysfs_pwm_out.cpp
 	DEPENDS
 		platforms__common
 	)
diff --git a/src/drivers/pwm_out/pwm_out.cpp b/src/drivers/navio_sysfs_pwm_out/navio_sysfs_pwm_out.cpp
similarity index 93%
rename from src/drivers/pwm_out/pwm_out.cpp
rename to src/drivers/navio_sysfs_pwm_out/navio_sysfs_pwm_out.cpp
index 12c4e7644b..2f794ceffe 100644
--- a/src/drivers/pwm_out/pwm_out.cpp
+++ b/src/drivers/navio_sysfs_pwm_out/navio_sysfs_pwm_out.cpp
@@ -50,7 +50,7 @@
 #include <systemlib/param/param.h>
 #include <systemlib/pwm_limit/pwm_limit.h>
 
-namespace pwm_out
+namespace navio_sysfs_pwm_out
 {
 static px4_task_t _task_handle = -1;
 volatile bool _task_should_exit = false;
@@ -431,12 +431,12 @@ void usage()
 	PX4_INFO("       pwm_out status");
 }
 
-} // namespace pwm_out
+} // namespace navio_sysfs_pwm_out
 
 /* driver 'main' command */
-extern "C" __EXPORT int pwm_out_main(int argc, char *argv[]);
+extern "C" __EXPORT int navio_sysfs_pwm_out_main(int argc, char *argv[]);
 
-int pwm_out_main(int argc, char *argv[])
+int navio_sysfs_pwm_out_main(int argc, char *argv[])
 {
 	const char *device = nullptr;
 	int ch;
@@ -456,43 +456,43 @@ int pwm_out_main(int argc, char *argv[])
 		switch (ch) {
 		case 'd':
 			device = myoptarg;
-			strncpy(pwm_out::_device, device, strlen(device));
+			strncpy(navio_sysfs_pwm_out::_device, device, strlen(device));
 			break;
 		}
 	}
 
 	// gets the parameters for the esc's pwm
-	param_get(param_find("PWM_DISARMED"), &pwm_out::_pwm_disarmed);
-	param_get(param_find("PWM_MIN"), &pwm_out::_pwm_min);
-	param_get(param_find("PWM_MAX"), &pwm_out::_pwm_max);
+	param_get(param_find("PWM_DISARMED"), &navio_sysfs_pwm_out::_pwm_disarmed);
+	param_get(param_find("PWM_MIN"), &navio_sysfs_pwm_out::_pwm_min);
+	param_get(param_find("PWM_MAX"), &navio_sysfs_pwm_out::_pwm_max);
 
 	/*
 	 * Start/load the driver.
 	 */
 	if (!strcmp(verb, "start")) {
-		if (pwm_out::_is_running) {
+		if (navio_sysfs_pwm_out::_is_running) {
 			PX4_WARN("pwm_out already running");
 			return 1;
 		}
 
-		pwm_out::start();
+		navio_sysfs_pwm_out::start();
 	}
 
 	else if (!strcmp(verb, "stop")) {
-		if (!pwm_out::_is_running) {
+		if (!navio_sysfs_pwm_out::_is_running) {
 			PX4_WARN("pwm_out is not running");
 			return 1;
 		}
 
-		pwm_out::stop();
+		navio_sysfs_pwm_out::stop();
 	}
 
 	else if (!strcmp(verb, "status")) {
-		PX4_WARN("pwm_out is %s", pwm_out::_is_running ? "running" : "not running");
+		PX4_WARN("pwm_out is %s", navio_sysfs_pwm_out::_is_running ? "running" : "not running");
 		return 0;
 
 	} else {
-		pwm_out::usage();
+		navio_sysfs_pwm_out::usage();
 		return 1;
 	}
 
diff --git a/src/modules/rcinput/CMakeLists.txt b/src/drivers/navio_sysfs_rc_in/CMakeLists.txt
similarity index 95%
rename from src/modules/rcinput/CMakeLists.txt
rename to src/drivers/navio_sysfs_rc_in/CMakeLists.txt
index 5008242d19..03910a27ff 100644
--- a/src/modules/rcinput/CMakeLists.txt
+++ b/src/drivers/navio_sysfs_rc_in/CMakeLists.txt
@@ -31,12 +31,12 @@
 #
 ############################################################################
 px4_add_module(
-	MODULE modules__rcinput
-	MAIN rcinput
+	MODULE drivers__navio_sysfs_rc_in
+	MAIN navio_sysfs_rc_in
 	STACK_MAIN 1200
 	COMPILE_FLAGS -Os
 	SRCS
-		rcinput.cpp
+		navio_sysfs_rc_in.cpp
 	DEPENDS
 		platforms__common
 	)
diff --git a/src/modules/rcinput/rcinput.cpp b/src/drivers/navio_sysfs_rc_in/navio_sysfs_rc_in.cpp
similarity index 62%
rename from src/modules/rcinput/rcinput.cpp
rename to src/drivers/navio_sysfs_rc_in/navio_sysfs_rc_in.cpp
index 22a835ee0f..598f354ba4 100644
--- a/src/modules/rcinput/rcinput.cpp
+++ b/src/drivers/navio_sysfs_rc_in/navio_sysfs_rc_in.cpp
@@ -47,10 +47,10 @@
 #include <uORB/uORB.h>
 #include <uORB/topics/input_rc.h>
 
-namespace rcinput
+namespace navio_sysfs_rc_in
 {
 
-extern "C" __EXPORT int rcinput_main(int argc, char *argv[]);
+extern "C" __EXPORT int navio_sysfs_rc_in_main(int argc, char *argv[]);
 
 #define RCINPUT_DEVICE_PATH_BASE "/sys/kernel/rcio/rcin"
 
@@ -61,20 +61,20 @@ class RcInput
 {
 public:
 	RcInput() :
-        _shouldExit(false),
-        _isRunning(false),
-        _work{},
-        _rcinput_pub(nullptr),
-        _channels(8), //D8R-II plus
-        _data{}
-    {
-        memset(_ch_fd, 0, sizeof(_ch_fd));
-    }
-    ~RcInput()
-    {
-        work_cancel(HPWORK, &_work);
-        _isRunning = false;
-    }
+		_shouldExit(false),
+		_isRunning(false),
+		_work{},
+		_rcinput_pub(nullptr),
+		_channels(8), //D8R-II plus
+		_data{}
+	{
+		memset(_ch_fd, 0, sizeof(_ch_fd));
+	}
+	~RcInput()
+	{
+		work_cancel(HPWORK, &_work);
+		_isRunning = false;
+	}
 
 	/* @return 0 on success, -errno on failure */
 	int start();
@@ -85,49 +85,53 @@ public:
 	/* Trampoline for the work queue. */
 	static void cycle_trampoline(void *arg);
 
-    bool isRunning() { return _isRunning; }
+	bool isRunning() { return _isRunning; }
 
 private:
-    void _cycle();
-    void _measure();
+	void _cycle();
+	void _measure();
 
-    bool _shouldExit;
-    bool _isRunning;
-    struct work_s _work;
+	bool _shouldExit;
+	bool _isRunning;
+	struct work_s _work;
 
-    orb_advert_t _rcinput_pub;
+	orb_advert_t _rcinput_pub;
 
-    int _channels;
-    int _ch_fd[input_rc_s::RC_INPUT_MAX_CHANNELS];
-    struct input_rc_s _data;
+	int _channels;
+	int _ch_fd[input_rc_s::RC_INPUT_MAX_CHANNELS];
+	struct input_rc_s _data;
 
 	int navio_rc_init();
 };
 
 int RcInput::navio_rc_init()
 {
-    int i;
-    char *buf;
+	int i;
+	char *buf;
 
-    for (i = 0; i < _channels; ++i) {
-        ::asprintf(&buf, "%s/ch%d", RCINPUT_DEVICE_PATH_BASE, i);
-        int fd = ::open(buf, O_RDONLY);
-        ::free(buf);
-        if (fd < 0) {
-            PX4_WARN("error: open %d failed", i);
-            break;
-        }
-        _ch_fd[i] = fd;
-    }
-    for (; i < input_rc_s::RC_INPUT_MAX_CHANNELS; ++i) {
-        _data.values[i] = UINT16_MAX;
-    }
+	for (i = 0; i < _channels; ++i) {
+		::asprintf(&buf, "%s/ch%d", RCINPUT_DEVICE_PATH_BASE, i);
+		int fd = ::open(buf, O_RDONLY);
+		::free(buf);
 
-    _rcinput_pub = orb_advertise(ORB_ID(input_rc), &_data);
-    if (_rcinput_pub == nullptr) {
-        PX4_WARN("error: advertise failed");
-        return -1;
-    }
+		if (fd < 0) {
+			PX4_WARN("error: open %d failed", i);
+			break;
+		}
+
+		_ch_fd[i] = fd;
+	}
+
+	for (; i < input_rc_s::RC_INPUT_MAX_CHANNELS; ++i) {
+		_data.values[i] = UINT16_MAX;
+	}
+
+	_rcinput_pub = orb_advertise(ORB_ID(input_rc), &_data);
+
+	if (_rcinput_pub == nullptr) {
+		PX4_WARN("error: advertise failed");
+		return -1;
+	}
 
 	return 0;
 }
@@ -140,66 +144,69 @@ int RcInput::start()
 
 	if (result != 0) {
 		PX4_WARN("error: RC initialization failed");
-        return -1;
+		return -1;
 	}
 
-    _isRunning = true;
+	_isRunning = true;
 	result = work_queue(HPWORK, &_work, (worker_t)&RcInput::cycle_trampoline, this, 0);
-    if (result == -1) {
-        _isRunning = false;
-    }
 
-    return result;
+	if (result == -1) {
+		_isRunning = false;
+	}
+
+	return result;
 }
 
 void RcInput::stop()
 {
-    _shouldExit = true;
+	_shouldExit = true;
 }
 
 void RcInput::cycle_trampoline(void *arg)
 {
-    RcInput *dev = reinterpret_cast<RcInput *>(arg);
-    dev->_cycle();
+	RcInput *dev = reinterpret_cast<RcInput *>(arg);
+	dev->_cycle();
 }
 
 void RcInput::_cycle()
 {
-    _measure();
+	_measure();
 
-    if (!_shouldExit) {
-        work_queue(HPWORK, &_work, (worker_t)&RcInput::cycle_trampoline, this,
-                   USEC2TICK(RCINPUT_MEASURE_INTERVAL_US));
-    }
+	if (!_shouldExit) {
+		work_queue(HPWORK, &_work, (worker_t)&RcInput::cycle_trampoline, this,
+			   USEC2TICK(RCINPUT_MEASURE_INTERVAL_US));
+	}
 }
 
 void RcInput::_measure(void)
 {
-    uint64_t ts;
-    char buf[12];
+	uint64_t ts;
+	char buf[12];
 
-    for (int i = 0; i < _channels; ++i) {
-        int res;
-        if ((res = ::pread(_ch_fd[i], buf, sizeof(buf) - 1, 0)) < 0) {
-            _data.values[i] = UINT16_MAX;
-            continue;
-        }
-        buf[sizeof(buf) -1] = '\0';
+	for (int i = 0; i < _channels; ++i) {
+		int res;
 
-        _data.values[i] = atoi(buf);
-    }
+		if ((res = ::pread(_ch_fd[i], buf, sizeof(buf) - 1, 0)) < 0) {
+			_data.values[i] = UINT16_MAX;
+			continue;
+		}
 
-    ts = hrt_absolute_time();
-    _data.timestamp_publication = ts;
-    _data.timestamp_last_signal = ts;
-    _data.channel_count = _channels;
-    _data.rssi = 100;
-    _data.rc_lost_frame_count = 0;
-    _data.rc_total_frame_count = 1;
-    _data.rc_ppm_frame_length = 100;
-    _data.rc_failsafe = false;
-    _data.rc_lost = false;
-    _data.input_source = input_rc_s::RC_INPUT_SOURCE_PX4IO_PPM;
+		buf[sizeof(buf) - 1] = '\0';
+
+		_data.values[i] = atoi(buf);
+	}
+
+	ts = hrt_absolute_time();
+	_data.timestamp_publication = ts;
+	_data.timestamp_last_signal = ts;
+	_data.channel_count = _channels;
+	_data.rssi = 100;
+	_data.rc_lost_frame_count = 0;
+	_data.rc_total_frame_count = 1;
+	_data.rc_ppm_frame_length = 100;
+	_data.rc_failsafe = false;
+	_data.rc_lost = false;
+	_data.input_source = input_rc_s::RC_INPUT_SOURCE_PX4IO_PPM;
 
 	orb_publish(ORB_ID(input_rc), _rcinput_pub, &_data);
 }
@@ -216,12 +223,12 @@ usage(const char *reason)
 		PX4_ERR("%s", reason);
 	}
 
-	PX4_INFO("usage: rcinput {start|stop|status}");
+	PX4_INFO("usage: navio_sysfs_rc_in {start|stop|status}");
 }
 
 static RcInput *rc_input = nullptr;
 
-int rcinput_main(int argc, char *argv[])
+int navio_sysfs_rc_in_main(int argc, char *argv[])
 {
 	if (argc < 2) {
 		usage("missing command");
@@ -294,4 +301,4 @@ int rcinput_main(int argc, char *argv[])
 
 }
 
-}; // namespace rcinput
+}; // namespace navio_sysfs_rc_in
diff --git a/src/lib/version/version.h b/src/lib/version/version.h
index 4a529f2751..ad3fc85cc7 100644
--- a/src/lib/version/version.h
+++ b/src/lib/version/version.h
@@ -59,7 +59,7 @@ __END_DECLS
 #  define	HW_ARCH "LINUXTEST"
 #elif defined(CONFIG_ARCH_BOARD_EXCELSIOR)
 #  define HW_ARCH "LINUXTEST"
-#elif defined(CONFIG_ARCH_BOARD_RPI2)
+#elif defined(CONFIG_ARCH_BOARD_RPI2) || defined(CONFIG_ARCH_BOARD_NAVIO2)
 #  define	HW_ARCH "LINUXTEST"
 #elif defined(CONFIG_ARCH_BOARD_BEBOP)
 #  define	HW_ARCH "LINUXTEST"

From ee581881622e0cc159e9ccf6f3357e9793047b35 Mon Sep 17 00:00:00 2001
From: Hidenori <hidenori.kobayashi@gmail.com>
Date: Fri, 1 Jul 2016 12:51:12 -0400
Subject: [PATCH 0884/1230] Split Navio2 specific parts from general RPI2 files

For Navio2, make posix_navio2_release and use navio2.config.
---
 cmake/configs/posix_rpi2_release.cmake |  3 ---
 posix-configs/rpi2/mainapp.config      |  2 --
 posix-configs/rpi2/navio2.config       | 24 ++++++++++++++++++++++++
 3 files changed, 24 insertions(+), 5 deletions(-)
 create mode 100644 posix-configs/rpi2/navio2.config

diff --git a/cmake/configs/posix_rpi2_release.cmake b/cmake/configs/posix_rpi2_release.cmake
index a796aa6f15..0778efa1ff 100644
--- a/cmake/configs/posix_rpi2_release.cmake
+++ b/cmake/configs/posix_rpi2_release.cmake
@@ -72,12 +72,9 @@ set(config_module_list
 	modules/navigator
 	modules/mavlink
 
-	modules/rcinput
-
 	#
 	# PX4 drivers
 	#
-	drivers/pwm_out
 
 	#
 	# Libraries
diff --git a/posix-configs/rpi2/mainapp.config b/posix-configs/rpi2/mainapp.config
index 4f7ddd9354..752ea8f66b 100644
--- a/posix-configs/rpi2/mainapp.config
+++ b/posix-configs/rpi2/mainapp.config
@@ -19,5 +19,3 @@ sleep 1
 mavlink stream -u 14556 -s HIGHRES_IMU -r 50
 mavlink stream -u 14556 -s ATTITUDE -r 50
 mavlink boot_complete
-rcinput start
-pwm_out start
diff --git a/posix-configs/rpi2/navio2.config b/posix-configs/rpi2/navio2.config
new file mode 100644
index 0000000000..3e14dbd5f1
--- /dev/null
+++ b/posix-configs/rpi2/navio2.config
@@ -0,0 +1,24 @@
+uorb start
+param set SYS_AUTOSTART 4001
+param set MAV_BROADCAST 1
+sleep 1
+param set MAV_TYPE 2
+df_lsm9ds1_wrapper start -R 4
+#df_mpu9250_wrapper start -R 10
+#df_hmc5883_wrapper start
+df_ms5611_wrapper start
+gps start -d /dev/pts/0
+sensors start
+commander start
+attitude_estimator_q start
+position_estimator_inav start
+land_detector start multicopter
+mc_pos_control start
+mc_att_control start
+mavlink start -d /dev/ttyUSB0
+sleep 1
+mavlink stream -d /dev/ttyUSB0 -s HIGHRES_IMU -r 50
+mavlink stream -d /dev/ttyUSB0 -s ATTITUDE -r 50
+mavlink boot_complete
+navio_sysfs_rcinput start
+navio_sysfs_pwm_out start

From 7e282f579b255d5b88ca514848fa4eca21570edf Mon Sep 17 00:00:00 2001
From: tumbili <roman@px4.io>
Date: Mon, 27 Jun 2016 14:06:01 +0200
Subject: [PATCH 0885/1230] sensors app: logic fixed and cleanup

- do not exit sensors app if sensor init failed
- do not spam console if we fail over first/second gyro

Signed-off-by: tumbili <roman@px4.io>
---
 src/modules/sensors/sensors.cpp | 15 ++++++---------
 1 file changed, 6 insertions(+), 9 deletions(-)

diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp
index 1a46c8ac2f..7e7defb882 100644
--- a/src/modules/sensors/sensors.cpp
+++ b/src/modules/sensors/sensors.cpp
@@ -2096,12 +2096,7 @@ Sensors::task_main()
 #endif
 
 	if (ret) {
-		warnx("sensor initialization failed");
-		_sensors_task = -1;
-
-		DevMgr::releaseHandle(_h_adc);
-
-		return;
+		PX4_ERR("sensor initialization failed");
 	}
 
 	struct sensor_combined_s raw = {};
@@ -2225,17 +2220,19 @@ Sensors::task_main()
 		/* Work out if main gyro timed out and fail over to alternate gyro.
 		 * However, don't do this if the secondary is not available. */
 		if (hrt_elapsed_time(&raw.gyro_timestamp[0]) > 20 * 1000 && _gyro_sub[1] >= 0) {
-			warnx("gyro has timed out");
+			if (fds[0].fd == _gyro_sub[0]) {
+				PX4_WARN("gyro0 has timed out");
+			}
 
 			/* If the secondary failed as well, go to the tertiary, also only if available. */
-			if (hrt_elapsed_time(&raw.gyro_timestamp[1]) > 20 * 1000 && _gyro_sub[2] >= 0) {
+			if (hrt_elapsed_time(&raw.gyro_timestamp[1]) > 20 * 1000 && _gyro_sub[2] >= 0 && (fds[0].fd != _gyro_sub[2])) {
 				fds[0].fd = _gyro_sub[2];
 
 				if (!_hil_enabled) {
 					warnx("failing over to third gyro");
 				}
 
-			} else if (_gyro_sub[1] >= 0) {
+			} else if (_gyro_sub[1] >= 0 && (fds[0].fd != _gyro_sub[1])) {
 				fds[0].fd = _gyro_sub[1];
 
 				if (!_hil_enabled) {

From 2a729028bd4182e81f58c6a5bd057e35175ae783 Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Sat, 2 Jul 2016 11:41:23 +0200
Subject: [PATCH 0886/1230] SITL: Set battery cells

---
 posix-configs/SITL/init/rcS_gazebo_iris              | 1 +
 posix-configs/SITL/init/rcS_gazebo_iris_opt_flow     | 1 +
 posix-configs/SITL/init/rcS_gazebo_plane             | 1 +
 posix-configs/SITL/init/rcS_gazebo_solo              | 1 +
 posix-configs/SITL/init/rcS_gazebo_standard_vtol     | 1 +
 posix-configs/SITL/init/rcS_gazebo_tailsitter        | 1 +
 posix-configs/SITL/init/rcS_gazebo_typhoon_h480      | 1 +
 posix-configs/SITL/init/rcS_jmavsim_iris             | 1 +
 posix-configs/SITL/init/rcS_lpe_gazebo_iris          | 1 +
 posix-configs/SITL/init/rcS_lpe_gazebo_iris_opt_flow | 1 +
 posix-configs/SITL/init/rcS_lpe_jmavsim_iris         | 1 +
 posix-configs/SITL/init/rcS_multiple_gazebo_iris     | 1 +
 12 files changed, 12 insertions(+)

diff --git a/posix-configs/SITL/init/rcS_gazebo_iris b/posix-configs/SITL/init/rcS_gazebo_iris
index b416d1f684..b84a93ee4a 100644
--- a/posix-configs/SITL/init/rcS_gazebo_iris
+++ b/posix-configs/SITL/init/rcS_gazebo_iris
@@ -4,6 +4,7 @@ param set MAV_TYPE 2
 param set SYS_AUTOSTART 4010
 param set SYS_RESTART_TYPE 2
 dataman start
+param set BAT_N_CELLS 3
 param set CAL_GYRO0_ID 2293768
 param set CAL_ACC0_ID 1376264
 param set CAL_ACC1_ID 1310728
diff --git a/posix-configs/SITL/init/rcS_gazebo_iris_opt_flow b/posix-configs/SITL/init/rcS_gazebo_iris_opt_flow
index 7aea1f3f40..d6a10f8d0e 100644
--- a/posix-configs/SITL/init/rcS_gazebo_iris_opt_flow
+++ b/posix-configs/SITL/init/rcS_gazebo_iris_opt_flow
@@ -4,6 +4,7 @@ param set MAV_TYPE 2
 param set SYS_AUTOSTART 4010
 param set SYS_RESTART_TYPE 2
 dataman start
+param set BAT_N_CELLS 3
 param set CAL_GYRO0_ID 2293768
 param set CAL_ACC0_ID 1376264
 param set CAL_ACC1_ID 1310728
diff --git a/posix-configs/SITL/init/rcS_gazebo_plane b/posix-configs/SITL/init/rcS_gazebo_plane
index 3a162cc678..42bb9fe76f 100644
--- a/posix-configs/SITL/init/rcS_gazebo_plane
+++ b/posix-configs/SITL/init/rcS_gazebo_plane
@@ -4,6 +4,7 @@ param set MAV_TYPE 1
 param set SYS_AUTOSTART 3033
 param set SYS_RESTART_TYPE 2
 dataman start
+param set BAT_N_CELLS 3
 param set CAL_GYRO0_ID 2293768
 param set CAL_ACC0_ID 1376264
 param set CAL_ACC1_ID 1310728
diff --git a/posix-configs/SITL/init/rcS_gazebo_solo b/posix-configs/SITL/init/rcS_gazebo_solo
index eeb85b54a6..8d9d09bf99 100644
--- a/posix-configs/SITL/init/rcS_gazebo_solo
+++ b/posix-configs/SITL/init/rcS_gazebo_solo
@@ -4,6 +4,7 @@ param set MAV_TYPE 2
 param set SYS_AUTOSTART 4010
 param set SYS_RESTART_TYPE 2
 dataman start
+param set BAT_N_CELLS 3
 param set CAL_GYRO0_ID 2293768
 param set CAL_ACC0_ID 1376264
 param set CAL_ACC1_ID 1310728
diff --git a/posix-configs/SITL/init/rcS_gazebo_standard_vtol b/posix-configs/SITL/init/rcS_gazebo_standard_vtol
index d9e06b6fbd..454071fc4d 100644
--- a/posix-configs/SITL/init/rcS_gazebo_standard_vtol
+++ b/posix-configs/SITL/init/rcS_gazebo_standard_vtol
@@ -5,6 +5,7 @@ param set VT_TYPE 2
 param set SYS_AUTOSTART 4010
 param set SYS_RESTART_TYPE 2
 dataman start
+param set BAT_N_CELLS 3
 param set CAL_GYRO0_ID 2293768
 param set CAL_ACC0_ID 1376264
 param set CAL_ACC1_ID 1310728
diff --git a/posix-configs/SITL/init/rcS_gazebo_tailsitter b/posix-configs/SITL/init/rcS_gazebo_tailsitter
index d5edadeea5..0882d895a0 100644
--- a/posix-configs/SITL/init/rcS_gazebo_tailsitter
+++ b/posix-configs/SITL/init/rcS_gazebo_tailsitter
@@ -5,6 +5,7 @@ param set VT_TYPE 0
 param set SYS_AUTOSTART 4010
 param set SYS_RESTART_TYPE 2
 dataman start
+param set BAT_N_CELLS 3
 param set CAL_GYRO0_ID 2293768
 param set CAL_ACC0_ID 1376264
 param set CAL_ACC1_ID 1310728
diff --git a/posix-configs/SITL/init/rcS_gazebo_typhoon_h480 b/posix-configs/SITL/init/rcS_gazebo_typhoon_h480
index 3688dae818..a122924f6c 100644
--- a/posix-configs/SITL/init/rcS_gazebo_typhoon_h480
+++ b/posix-configs/SITL/init/rcS_gazebo_typhoon_h480
@@ -4,6 +4,7 @@ param set MAV_TYPE 13
 param set SYS_AUTOSTART 6001
 param set SYS_RESTART_TYPE 2
 dataman start
+param set BAT_N_CELLS 3
 param set CAL_GYRO0_ID 2293768
 param set CAL_ACC0_ID 1376264
 param set CAL_ACC1_ID 1310728
diff --git a/posix-configs/SITL/init/rcS_jmavsim_iris b/posix-configs/SITL/init/rcS_jmavsim_iris
index dbcc533a81..d48ecbf394 100644
--- a/posix-configs/SITL/init/rcS_jmavsim_iris
+++ b/posix-configs/SITL/init/rcS_jmavsim_iris
@@ -4,6 +4,7 @@ param set MAV_TYPE 2
 param set SYS_AUTOSTART 4010
 param set SYS_RESTART_TYPE 2
 dataman start
+param set BAT_N_CELLS 3
 param set CAL_GYRO0_ID 2293768
 param set CAL_ACC0_ID 1376264
 param set CAL_ACC1_ID 1310728
diff --git a/posix-configs/SITL/init/rcS_lpe_gazebo_iris b/posix-configs/SITL/init/rcS_lpe_gazebo_iris
index 514ad7668a..f3e5dd55f8 100644
--- a/posix-configs/SITL/init/rcS_lpe_gazebo_iris
+++ b/posix-configs/SITL/init/rcS_lpe_gazebo_iris
@@ -4,6 +4,7 @@ param set MAV_TYPE 2
 param set SYS_AUTOSTART 4010
 param set SYS_RESTART_TYPE 2
 dataman start
+param set BAT_N_CELLS 3
 param set CAL_GYRO0_ID 2293768
 param set CAL_ACC0_ID 1376264
 param set CAL_ACC1_ID 1310728
diff --git a/posix-configs/SITL/init/rcS_lpe_gazebo_iris_opt_flow b/posix-configs/SITL/init/rcS_lpe_gazebo_iris_opt_flow
index 085dc77a7b..e7c0c33258 100644
--- a/posix-configs/SITL/init/rcS_lpe_gazebo_iris_opt_flow
+++ b/posix-configs/SITL/init/rcS_lpe_gazebo_iris_opt_flow
@@ -4,6 +4,7 @@ param set MAV_TYPE 2
 param set SYS_AUTOSTART 4010
 param set SYS_RESTART_TYPE 2
 dataman start
+param set BAT_N_CELLS 3
 param set CAL_GYRO0_ID 2293768
 param set CAL_ACC0_ID 1376264
 param set CAL_ACC1_ID 1310728
diff --git a/posix-configs/SITL/init/rcS_lpe_jmavsim_iris b/posix-configs/SITL/init/rcS_lpe_jmavsim_iris
index 4de8c96030..fa8d9bca2a 100644
--- a/posix-configs/SITL/init/rcS_lpe_jmavsim_iris
+++ b/posix-configs/SITL/init/rcS_lpe_jmavsim_iris
@@ -4,6 +4,7 @@ param set MAV_TYPE 2
 param set SYS_AUTOSTART 4010
 param set SYS_RESTART_TYPE 2
 dataman start
+param set BAT_N_CELLS 3
 param set CAL_GYRO0_ID 2293768
 param set CAL_ACC0_ID 1376264
 param set CAL_ACC1_ID 1310728
diff --git a/posix-configs/SITL/init/rcS_multiple_gazebo_iris b/posix-configs/SITL/init/rcS_multiple_gazebo_iris
index a791aa8174..fb4d23d9b2 100644
--- a/posix-configs/SITL/init/rcS_multiple_gazebo_iris
+++ b/posix-configs/SITL/init/rcS_multiple_gazebo_iris
@@ -5,6 +5,7 @@ param set MAV_TYPE 2
 param set SYS_AUTOSTART 4010
 param set SYS_RESTART_TYPE 2
 dataman start
+param set BAT_N_CELLS 3
 param set CAL_GYRO0_ID 2293768
 param set CAL_ACC0_ID 1376264
 param set CAL_ACC1_ID 1310728

From 972a6f7be8a7d716d263d1731b0c9b0a49848a97 Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Sat, 2 Jul 2016 11:43:24 +0200
Subject: [PATCH 0887/1230] Fix MAVLink reporting of Firmware version,
 implement dev / release version reporting

---
 src/modules/mavlink/mavlink_main.cpp   |  4 +-
 src/modules/systemlib/battery_params.c |  3 +-
 src/systemcmds/ver/ver.c               | 62 ++++++++++++++++++++++----
 3 files changed, 57 insertions(+), 12 deletions(-)

diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp
index 42c17a8fdc..b9ca1f2576 100644
--- a/src/modules/mavlink/mavlink_main.cpp
+++ b/src/modules/mavlink/mavlink_main.cpp
@@ -1222,8 +1222,8 @@ void Mavlink::send_autopilot_capabilites()
 		msg.capabilities |= MAV_PROTOCOL_CAPABILITY_SET_ATTITUDE_TARGET;
 		msg.capabilities |= MAV_PROTOCOL_CAPABILITY_SET_POSITION_TARGET_LOCAL_NED;
 		msg.capabilities |= MAV_PROTOCOL_CAPABILITY_SET_ACTUATOR_TARGET;
-		msg.flight_sw_version = version_tag_to_number(px4_git_version);
-		msg.middleware_sw_version = version_tag_to_number(px4_git_version);
+		msg.flight_sw_version = version_tag_to_number(px4_git_tag);
+		msg.middleware_sw_version = version_tag_to_number(px4_git_tag);
 		msg.os_sw_version = version_tag_to_number(os_git_tag);
 		msg.board_version = px4_board_version;
 		memcpy(&msg.flight_custom_version, &px4_git_version_binary, sizeof(msg.flight_custom_version));
diff --git a/src/modules/systemlib/battery_params.c b/src/modules/systemlib/battery_params.c
index fdf4826afd..e6d9b62030 100644
--- a/src/modules/systemlib/battery_params.c
+++ b/src/modules/systemlib/battery_params.c
@@ -125,6 +125,7 @@ PARAM_DEFINE_FLOAT(BAT_V_LOAD_DROP, 0.3f);
  *
  * @group Battery Calibration
  * @unit S
+ * @value 0 Unconfigured
  * @value 2 2S Battery
  * @value 3 3S Battery
  * @value 4 4S Battery
@@ -141,7 +142,7 @@ PARAM_DEFINE_FLOAT(BAT_V_LOAD_DROP, 0.3f);
  * @value 15 15S Battery
  * @value 16 16S Battery
  */
-PARAM_DEFINE_INT32(BAT_N_CELLS, 3);
+PARAM_DEFINE_INT32(BAT_N_CELLS, 0);
 
 /**
  * Battery capacity.
diff --git a/src/systemcmds/ver/ver.c b/src/systemcmds/ver/ver.c
index 0b7584c888..b69fd39e47 100644
--- a/src/systemcmds/ver/ver.c
+++ b/src/systemcmds/ver/ver.c
@@ -70,6 +70,19 @@ __EXPORT const char *os_git_tag = "";
 __EXPORT const uint32_t px4_board_version = 1;
 #endif
 
+// dev >= 0
+// alpha >= 64
+// beta >= 128
+// release candidate >= 192
+// release == 255
+enum FIRMWARE_TYPE {
+	FIRMWARE_TYPE_DEV = 0,
+	FIRMWARE_TYPE_ALPHA = 64,
+	FIRMWARE_TYPE_BETA = 128,
+	FIRMWARE_TYPE_RC = 192,
+	FIRMWARE_TYPE_RELEASE = 255
+};
+
 /**
  * Convert a version tag string to a number
  */
@@ -78,9 +91,16 @@ uint32_t version_tag_to_number(const char *tag)
 	uint32_t ver = 0;
 	unsigned len = strlen(tag);
 	unsigned mag = 0;
+	int32_t type = -1;
 	bool dotparsed = false;
+	unsigned dashcount = 0;
 
 	for (int i = len - 1; i >= 0; i--) {
+
+		if (tag[i] == '-') {
+			dashcount++;
+		}
+
 		if (tag[i] >= '0' && tag[i] <= '9') {
 			unsigned number = tag[i] - '0';
 
@@ -94,6 +114,29 @@ uint32_t version_tag_to_number(const char *tag)
 			/* this is a full version and we have enough digits */
 			return ver;
 
+		} else if (tag[i] == '-' && i > 3 && type == -1) {
+			/* scan until the first number */
+			const char *curr = &tag[i - 1];
+
+			// dev: v1.4.0rc3-7-g7e282f57
+
+			while (curr >= &tag[0] && (*curr <= '0' || *curr >= '9')) {
+				if (*curr == 'd') {
+					type = FIRMWARE_TYPE_DEV;
+					break;
+				} else if (*curr == 'a') {
+					type = FIRMWARE_TYPE_ALPHA;
+					break;
+				} else if (*curr == 'b') {
+					type = FIRMWARE_TYPE_BETA;
+					break;
+				} else if (*curr == 'r') {
+					type = FIRMWARE_TYPE_BETA;
+					break;
+				}
+				curr--;
+			}
+
 		} else if (tag[i] != 'v') {
 			/* reset, because we don't have a full tag but
 			 * are seeing non-numeric characters again
@@ -103,15 +146,16 @@ uint32_t version_tag_to_number(const char *tag)
 		}
 	}
 
-	// XXX not reporting patch version yet
-	// dev > 0
-	// alpha > 64
-	// beta > 128
-	// release candidate > 192
-	// release > 255
+	/* if the type is still uninitialized, check if there is a single dash in git describe */
+	if (type == -1 && dashcount == 1) {
+		type = FIRMWARE_TYPE_RELEASE;
+	} else if (type == -1) {
+		type = FIRMWARE_TYPE_DEV;
+	}
+
 	ver = (ver << 8);
 
-	return ver;
+	return ver | type;
 }
 
 static void usage(const char *reason)
@@ -167,8 +211,8 @@ int ver_main(int argc, char *argv[])
 				unsigned minor = (fwver >> (8 * 2)) & 0xFF;
 				unsigned patch = (fwver >> (8 * 1)) & 0xFF;
 				unsigned type = (fwver >> (8 * 0)) & 0xFF;
-				printf("FW version: %s (%u.%u.%u %s)\n", px4_git_tag, major, minor, patch,
-				       (type == 0) ? "dev" : "stable");
+				printf("FW version: %s (%u.%u.%u %u), %u\n", px4_git_tag, major, minor, patch,
+					type, fwver);
 				/* middleware is currently the same thing as firmware, so not printing yet */
 				printf("OS version: %s (%u)\n", os_git_tag, version_tag_to_number(os_git_tag));
 				ret = 0;

From fd17c87eb418b7e4bbc06a2b15753e380f44f4e8 Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Sat, 2 Jul 2016 12:00:56 +0200
Subject: [PATCH 0888/1230] Fix release parsing

---
 src/systemcmds/ver/ver.c | 15 ++++++++-------
 1 file changed, 8 insertions(+), 7 deletions(-)

diff --git a/src/systemcmds/ver/ver.c b/src/systemcmds/ver/ver.c
index b69fd39e47..183acc01e7 100644
--- a/src/systemcmds/ver/ver.c
+++ b/src/systemcmds/ver/ver.c
@@ -114,24 +114,25 @@ uint32_t version_tag_to_number(const char *tag)
 			/* this is a full version and we have enough digits */
 			return ver;
 
-		} else if (tag[i] == '-' && i > 3 && type == -1) {
+		} else if (i > 3 && type == -1) {
 			/* scan until the first number */
 			const char *curr = &tag[i - 1];
 
 			// dev: v1.4.0rc3-7-g7e282f57
+			// rc: v1.4.0rc4
 
-			while (curr >= &tag[0] && (*curr <= '0' || *curr >= '9')) {
-				if (*curr == 'd') {
+			while (curr > &tag[0]) {
+				if (*curr == 'v') {
 					type = FIRMWARE_TYPE_DEV;
 					break;
-				} else if (*curr == 'a') {
+				} else if (*curr == 'p') {
 					type = FIRMWARE_TYPE_ALPHA;
 					break;
-				} else if (*curr == 'b') {
+				} else if (*curr == 't') {
 					type = FIRMWARE_TYPE_BETA;
 					break;
 				} else if (*curr == 'r') {
-					type = FIRMWARE_TYPE_BETA;
+					type = FIRMWARE_TYPE_RC;
 					break;
 				}
 				curr--;
@@ -147,7 +148,7 @@ uint32_t version_tag_to_number(const char *tag)
 	}
 
 	/* if the type is still uninitialized, check if there is a single dash in git describe */
-	if (type == -1 && dashcount == 1) {
+	if (type == -1 && dashcount == 0) {
 		type = FIRMWARE_TYPE_RELEASE;
 	} else if (type == -1) {
 		type = FIRMWARE_TYPE_DEV;

From 88cf8f23f6796b44b8ee2a7cf5167a09e92e6273 Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Sat, 2 Jul 2016 12:08:30 +0200
Subject: [PATCH 0889/1230] Fix ver command

---
 src/systemcmds/ver/ver.c | 22 +++++++++++++++-------
 1 file changed, 15 insertions(+), 7 deletions(-)

diff --git a/src/systemcmds/ver/ver.c b/src/systemcmds/ver/ver.c
index 183acc01e7..c0c7fc6987 100644
--- a/src/systemcmds/ver/ver.c
+++ b/src/systemcmds/ver/ver.c
@@ -115,42 +115,50 @@ uint32_t version_tag_to_number(const char *tag)
 			return ver;
 
 		} else if (i > 3 && type == -1) {
-			/* scan until the first number */
+			/* scan and look for signature characters for each type */
 			const char *curr = &tag[i - 1];
 
 			// dev: v1.4.0rc3-7-g7e282f57
 			// rc: v1.4.0rc4
+			// release: v1.4.0
 
 			while (curr > &tag[0]) {
 				if (*curr == 'v') {
 					type = FIRMWARE_TYPE_DEV;
 					break;
+
 				} else if (*curr == 'p') {
 					type = FIRMWARE_TYPE_ALPHA;
 					break;
+
 				} else if (*curr == 't') {
 					type = FIRMWARE_TYPE_BETA;
 					break;
+
 				} else if (*curr == 'r') {
 					type = FIRMWARE_TYPE_RC;
 					break;
 				}
+
 				curr--;
 			}
 
+			/* looks like a release */
+			if (type == -1) {
+				type = FIRMWARE_TYPE_RELEASE;
+			}
+
 		} else if (tag[i] != 'v') {
 			/* reset, because we don't have a full tag but
-			 * are seeing non-numeric characters again
+			 * are seeing non-numeric characters
 			 */
 			ver = 0;
 			mag = 0;
 		}
 	}
 
-	/* if the type is still uninitialized, check if there is a single dash in git describe */
-	if (type == -1 && dashcount == 0) {
-		type = FIRMWARE_TYPE_RELEASE;
-	} else if (type == -1) {
+	/* if git describe contains dashes this is not a real tag */
+	if (dashcount > 0) {
 		type = FIRMWARE_TYPE_DEV;
 	}
 
@@ -213,7 +221,7 @@ int ver_main(int argc, char *argv[])
 				unsigned patch = (fwver >> (8 * 1)) & 0xFF;
 				unsigned type = (fwver >> (8 * 0)) & 0xFF;
 				printf("FW version: %s (%u.%u.%u %u), %u\n", px4_git_tag, major, minor, patch,
-					type, fwver);
+				       type, fwver);
 				/* middleware is currently the same thing as firmware, so not printing yet */
 				printf("OS version: %s (%u)\n", os_git_tag, version_tag_to_number(os_git_tag));
 				ret = 0;

From 9258bb2ae8e4ddc814dee8f73dd691e263209f70 Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Sat, 2 Jul 2016 12:08:57 +0200
Subject: [PATCH 0890/1230] v1.4.0 transitional support for battery count

---
 ROMFS/px4fmu_common/init.d/rcS | 10 ++++++++++
 1 file changed, 10 insertions(+)

diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS
index 6ef983e40c..834a4f655a 100644
--- a/ROMFS/px4fmu_common/init.d/rcS
+++ b/ROMFS/px4fmu_common/init.d/rcS
@@ -109,6 +109,16 @@ then
 		set AUTOCNF yes
 	else
 		set AUTOCNF no
+
+		#
+		# Release 1.4.0 transitional support:
+		# set to old default if unconfigured.
+		# this preserves the previous behaviour
+		#
+		if param compare BAT_N_CELLS 0
+		then
+			param set BAT_N_CELLS 3
+		fi
 	fi
 
 	#

From d85e41668049489bab450f3c1934cd3fdbc4479b Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Sat, 2 Jul 2016 12:59:49 +0200
Subject: [PATCH 0891/1230] Fix default handling for battery params

---
 src/modules/sensors/sensors.cpp | 8 ++++++--
 1 file changed, 6 insertions(+), 2 deletions(-)

diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp
index 7e7defb882..daf8d32d7c 100644
--- a/src/modules/sensors/sensors.cpp
+++ b/src/modules/sensors/sensors.cpp
@@ -919,7 +919,7 @@ Sensors::parameters_update()
 		warnx("%s", paramerr);
 		_parameters.battery_v_div = 0.0f;
 
-	} else if (_parameters.battery_v_div < 0.0f) {
+	} else if (_parameters.battery_v_div <= 0.0f) {
 		/* apply scaling according to defaults if set to default */
 #if defined (CONFIG_ARCH_BOARD_PX4FMU_V4)
 		_parameters.battery_v_div = 13.653333333f;
@@ -929,6 +929,8 @@ Sensors::parameters_update()
 		_parameters.battery_v_div = 7.8196363636f;
 #elif defined (CONFIG_ARCH_BOARD_PX4FMU_V1)
 		_parameters.battery_v_div = 5.7013919372f;
+#elif defined (CONFIG_ARCH_BOARD_SITL)
+		_parameters.battery_v_div = 10.177939394f;
 #else
 		/* ensure a missing default trips a low voltage lockdown */
 		_parameters.battery_v_div = 0.0f;
@@ -940,7 +942,7 @@ Sensors::parameters_update()
 		warnx("%s", paramerr);
 		_parameters.battery_a_per_v = 0.0f;
 
-	} else if (_parameters.battery_a_per_v < 0.0f) {
+	} else if (_parameters.battery_a_per_v <= 0.0f) {
 		/* apply scaling according to defaults if set to default */
 #if defined (CONFIG_ARCH_BOARD_PX4FMU_V4)
 		/* current scaling for ACSP4 */
@@ -948,6 +950,8 @@ Sensors::parameters_update()
 #elif defined (CONFIG_ARCH_BOARD_PX4FMU_V2) || defined (CONFIG_ARCH_BOARD_MINDPX_V2) || defined (CONFIG_ARCH_BOARD_AEROCORE) || defined (CONFIG_ARCH_BOARD_PX4FMU_V1)
 		/* current scaling for 3DR power brick */
 		_parameters.battery_a_per_v = 15.391030303f;
+#elif defined (CONFIG_ARCH_BOARD_SITL)
+		_parameters.battery_a_per_v = 15.391030303f;
 #else
 		/* ensure a missing default leads to an unrealistic current value */
 		_parameters.battery_a_per_v = 0.0f;

From bc6e83f5e3a3552afe8704ba43829c6682eb58ae Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Sat, 2 Jul 2016 13:00:07 +0200
Subject: [PATCH 0892/1230] Version reporting: Be more accurate about platforms

---
 src/lib/version/version.h | 10 +++++-----
 1 file changed, 5 insertions(+), 5 deletions(-)

diff --git a/src/lib/version/version.h b/src/lib/version/version.h
index ad3fc85cc7..a78be4fda7 100644
--- a/src/lib/version/version.h
+++ b/src/lib/version/version.h
@@ -54,15 +54,15 @@ __EXPORT const char *board_name(void);
 __END_DECLS
 
 #if defined(CONFIG_ARCH_BOARD_SITL)
-#  define	HW_ARCH "LINUXTEST"
+#  define	HW_ARCH "SITL"
 #elif defined(CONFIG_ARCH_BOARD_EAGLE)
-#  define	HW_ARCH "LINUXTEST"
+#  define	HW_ARCH "EAGLE"
 #elif defined(CONFIG_ARCH_BOARD_EXCELSIOR)
-#  define HW_ARCH "LINUXTEST"
+#  define HW_ARCH "EXCELSIOR"
 #elif defined(CONFIG_ARCH_BOARD_RPI2) || defined(CONFIG_ARCH_BOARD_NAVIO2)
-#  define	HW_ARCH "LINUXTEST"
+#  define	HW_ARCH "RPI"
 #elif defined(CONFIG_ARCH_BOARD_BEBOP)
-#  define	HW_ARCH "LINUXTEST"
+#  define	HW_ARCH "BEBOP"
 #else
 #define HW_ARCH (board_name())
 #endif

From 97d9fd38bee7ce07ed193ecc4d5844289c99a726 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Martin=20K=2E=20Schr=C3=B6der?= <mkschreder.uk@gmail.com>
Date: Sun, 3 Jul 2016 12:29:58 +0200
Subject: [PATCH 0893/1230] Update Matrix.hpp (#4966)

This was horribly wrong. Matrix is first cast into a matrix of size NxM (which is supposed to be the size of the result - NOT the starting point) so the transpose result becomes garbage. Instead make "Me" an MxN matrix as the original. Took me a whole evening to figure out this problem. Now my Kalman filter finally returns good results.
---
 src/lib/mathlib/math/Matrix.hpp | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/src/lib/mathlib/math/Matrix.hpp b/src/lib/mathlib/math/Matrix.hpp
index 0941f4b7b5..ce27ccfcc3 100644
--- a/src/lib/mathlib/math/Matrix.hpp
+++ b/src/lib/mathlib/math/Matrix.hpp
@@ -305,7 +305,7 @@ public:
 	 * transpose the matrix
 	 */
 	Matrix<N, M> transposed(void) const {
-		matrix::Matrix<float, N, M> Me(this->arm_mat.pData);
+		matrix::Matrix<float, M, N> Me(this->arm_mat.pData);
 		Matrix<N, M> res(Me.transpose().data());
 		return res;
 	}

From 00243a9842e4cfcb3138cad51476933f2a16a018 Mon Sep 17 00:00:00 2001
From: bharathr <bharathr.mail@gmail.com>
Date: Tue, 14 Jun 2016 20:27:43 -0700
Subject: [PATCH 0894/1230] Updated 200qx config files (removed calib files,
 cleaned up and renamed all files)

---
 .../eagle/200qx/mainapp-calib.config          |  9 ---
 .../{mainapp-flight.config => mainapp.config} |  0
 .../eagle/200qx/px4-flight-v2-board.config    | 80 -------------------
 ...ht-v1-board.config => px4-v1-board.config} | 37 ++++-----
 .../{px4-calib.config => px4-v2-board.config} | 65 ++++++++-------
 5 files changed, 50 insertions(+), 141 deletions(-)
 delete mode 100644 posix-configs/eagle/200qx/mainapp-calib.config
 rename posix-configs/eagle/200qx/{mainapp-flight.config => mainapp.config} (100%)
 delete mode 100644 posix-configs/eagle/200qx/px4-flight-v2-board.config
 rename posix-configs/eagle/200qx/{px4-flight-v1-board.config => px4-v1-board.config} (75%)
 rename posix-configs/eagle/200qx/{px4-calib.config => px4-v2-board.config} (77%)

diff --git a/posix-configs/eagle/200qx/mainapp-calib.config b/posix-configs/eagle/200qx/mainapp-calib.config
deleted file mode 100644
index eba7890720..0000000000
--- a/posix-configs/eagle/200qx/mainapp-calib.config
+++ /dev/null
@@ -1,9 +0,0 @@
-uorb start
-muorb start
-param set MAV_BROADCAST 1
-mavlink start -u 14556
-sleep 1
-mavlink stream -u 14556 -s HIGHRES_IMU -r 50
-mavlink stream -u 14556 -s ATTITUDE -r 50
-mavlink boot_complete
-commander start
diff --git a/posix-configs/eagle/200qx/mainapp-flight.config b/posix-configs/eagle/200qx/mainapp.config
similarity index 100%
rename from posix-configs/eagle/200qx/mainapp-flight.config
rename to posix-configs/eagle/200qx/mainapp.config
diff --git a/posix-configs/eagle/200qx/px4-flight-v2-board.config b/posix-configs/eagle/200qx/px4-flight-v2-board.config
deleted file mode 100644
index c201cee7fc..0000000000
--- a/posix-configs/eagle/200qx/px4-flight-v2-board.config
+++ /dev/null
@@ -1,80 +0,0 @@
-uorb start
-param set CAL_GYRO0_ID 2293760
-param set CAL_ACC0_ID 1310720
-param set CAL_ACC1_ID 1376256
-param set CAL_MAG0_ID 196608
-param set SYS_AUTOSTART 4001
-param set SYS_AUTOCONFIG 1
-param set MAV_TYPE 2
-param set MC_YAW_P 7.0
-param set MC_YAWRATE_P 0.08
-param set MC_YAWRATE_I 0.0
-param set MC_YAWRATE_D 0
-param set MC_PITCH_P 7.0
-param set MC_PITCHRATE_P 0.08
-param set MC_PITCHRATE_I 0.0
-param set MC_PITCHRATE_D 0.0001
-param set MC_ROLL_P 7.0
-param set MC_ROLLRATE_P 0.08
-param set MC_ROLLRATE_I 0.0
-param set MC_ROLLRATE_D 0.0001
-param set RC_MAP_THROTTLE 1
-param set RC_MAP_ROLL 2
-param set RC_MAP_PITCH 3
-param set RC_MAP_YAW 4
-param set RC_MAP_MODE_SW 5
-param set RC_MAP_POSCTL_SW 6
-param set RC1_MAX 1900
-param set RC1_MIN 1099
-param set RC1_TRIM 1099
-param set RC1_REV 1
-param set RC2_MAX 1900
-param set RC2_MIN 1099
-param set RC2_TRIM 1500
-param set RC2_REV -1
-param set RC3_MAX 1900
-param set RC3_MIN 1099
-param set RC3_TRIM 1500
-param set RC3_REV 1
-param set RC4_MAX 1900
-param set RC4_MIN 1099
-param set RC4_TRIM 1500
-param set RC4_REV -1
-param set RC5_MAX 1900
-param set RC5_MIN 1099
-param set RC5_TRIM 1500
-param set RC5_REV 1
-param set RC6_MAX 1900
-param set RC6_MIN 1099
-param set RC6_TRIM 1099
-param set RC6_REV 1
-param set ATT_W_MAG 0.00
-param set PE_MAG_NOISE 1.0f
-param set SENS_BOARD_ROT 0
-param set MPU_GYR_LPF_ENUM 4
-param set MPU_ACC_LPF_ENUM 4
-param set MPU_SMPRATE_ENUM 2
-param set UART_ESC_MODEL 0
-param set UART_ESC_BAUDRAT 250000
-param set UART_ESC_PX4MOT1 2
-param set UART_ESC_PX4MOT2 4
-param set UART_ESC_PX4MOT3 1
-param set UART_ESC_PX4MOT4 3
-sleep 1
-commander start
-rc_receiver start -D /dev/tty-1
-sleep 1
-mpu9x50 start -D /dev/spi-1
-df_bmp280_wrapper start
-sensors start
-attitude_estimator_q start
-position_estimator_inav start
-mc_pos_control start
-mc_att_control start
-land_detector start multicopter
-sleep 1
-uart_esc start -D /dev/tty-2
-list_devices
-list_files
-list_tasks
-list_topics
diff --git a/posix-configs/eagle/200qx/px4-flight-v1-board.config b/posix-configs/eagle/200qx/px4-v1-board.config
similarity index 75%
rename from posix-configs/eagle/200qx/px4-flight-v1-board.config
rename to posix-configs/eagle/200qx/px4-v1-board.config
index fa8ead0e46..1428b5197b 100644
--- a/posix-configs/eagle/200qx/px4-flight-v1-board.config
+++ b/posix-configs/eagle/200qx/px4-v1-board.config
@@ -1,10 +1,9 @@
 uorb start
-param set CAL_GYRO0_ID 2293760
-param set CAL_ACC0_ID 1310720
-param set CAL_ACC1_ID 1376256
-param set CAL_MAG0_ID 196608
+qshell start
+param set CAL_GYRO0_ID 100
+param set CAL_ACC0_ID 100
+param set CAL_MAG0_ID 101
 param set SYS_AUTOSTART 4001
-param set SYS_AUTOCONFIG 1
 param set MAV_TYPE 2
 param set MC_YAW_P 7.0
 param set MC_YAWRATE_P 0.08
@@ -51,29 +50,25 @@ param set RC6_REV 1
 param set ATT_W_MAG 0.00
 param set PE_MAG_NOISE 1.0f
 param set SENS_BOARD_ROT 6
-param set MPU_GYR_LPF_ENUM 4
-param set MPU_ACC_LPF_ENUM 4
-param set MPU_SMPRATE_ENUM 2
+param set RC_RECEIVER_TYPE 1
 param set UART_ESC_MODEL 0
-param set UART_ESC_BAUDRAT 250000
-param set UART_ESC_PX4MOT1 2
-param set UART_ESC_PX4MOT2 4
-param set UART_ESC_PX4MOT3 1
-param set UART_ESC_PX4MOT4 3
+param set UART_ESC_BAUDRTE 250000
+param set UART_ESC_MOTOR1 2
+param set UART_ESC_MOTOR2 4
+param set UART_ESC_MOTOR3 1
+param set UART_ESC_MOTOR4 3
 sleep 1
-commander start
-rc_receiver start -D /dev/tty-1
-sleep 1
-mpu9x50 start -D /dev/spi-1
+df_mpu9250_wrapper start
 df_bmp280_wrapper start
 sensors start
-attitude_estimator_q start
-position_estimator_inav start
+commander start
+ekf2 start
+land_detector start multicopter
 mc_pos_control start
 mc_att_control start
-land_detector start multicopter
-sleep 1
 uart_esc start -D /dev/tty-2
+rc_receiver start -D /dev/tty-1
+sleep 1
 list_devices
 list_files
 list_tasks
diff --git a/posix-configs/eagle/200qx/px4-calib.config b/posix-configs/eagle/200qx/px4-v2-board.config
similarity index 77%
rename from posix-configs/eagle/200qx/px4-calib.config
rename to posix-configs/eagle/200qx/px4-v2-board.config
index 74685189de..2d6c33f6ce 100644
--- a/posix-configs/eagle/200qx/px4-calib.config
+++ b/posix-configs/eagle/200qx/px4-v2-board.config
@@ -1,18 +1,22 @@
 uorb start
-param set CAL_GYRO0_ID 2293760
-param set CAL_ACC0_ID 1310720
-param set CAL_ACC1_ID 1376256
-param set CAL_MAG0_ID 196608
-param set RC_RECEIVER_TYPE 1
-rc_receiver start -D /dev/tty-1
-attitude_estimator_q start
-position_estimator_inav start
-mc_pos_control start
-mc_att_control start
-sleep 1
+qshell start
+param set CAL_GYRO0_ID 100
+param set CAL_ACC0_ID 100
+param set CAL_MAG0_ID 101
 param set SYS_AUTOSTART 4001
-param set SYS_AUTOCONFIG 1
-param set MAV_TYPE 2​
+param set MAV_TYPE 2
+param set MC_YAW_P 7.0
+param set MC_YAWRATE_P 0.08
+param set MC_YAWRATE_I 0.0
+param set MC_YAWRATE_D 0
+param set MC_PITCH_P 7.0
+param set MC_PITCHRATE_P 0.08
+param set MC_PITCHRATE_I 0.0
+param set MC_PITCHRATE_D 0.0001
+param set MC_ROLL_P 7.0
+param set MC_ROLLRATE_P 0.08
+param set MC_ROLLRATE_I 0.0
+param set MC_ROLLRATE_D 0.0001
 param set RC_MAP_THROTTLE 1
 param set RC_MAP_ROLL 2
 param set RC_MAP_PITCH 3
@@ -43,29 +47,28 @@ param set RC6_MAX 1900
 param set RC6_MIN 1099
 param set RC6_TRIM 1099
 param set RC6_REV 1
-sensors start
-param set MC_YAW_P 7.0
-param set MC_YAWRATE_P 0.08
-param set MC_YAWRATE_I 0.0
-param set MC_YAWRATE_D 0
-param set MC_PITCH_P 7.0
-param set MC_PITCHRATE_P 0.08
-param set MC_PITCHRATE_I 0.0
-param set MC_PITCHRATE_D 0.0001
-param set MC_ROLL_P 7.0
-param set MC_ROLLRATE_P 0.08
-param set MC_ROLLRATE_I 0.0
-param set MC_ROLLRATE_D 0.0001
 param set ATT_W_MAG 0.00
 param set PE_MAG_NOISE 1.0f
 param set SENS_BOARD_ROT 0
-param set MPU_GYR_LPF_ENUM 4
-param set MPU_ACC_LPF_ENUM 4
-param set MPU_SMPRATE_ENUM 2
+param set RC_RECEIVER_TYPE 1
+param set UART_ESC_MODEL 0
+param set UART_ESC_BAUDRTE 250000
+param set UART_ESC_MOTOR1 2
+param set UART_ESC_MOTOR2 4
+param set UART_ESC_MOTOR3 1
+param set UART_ESC_MOTOR4 3
 sleep 1
-mpu9x50 start -D /dev/spi-1
+df_mpu9250_wrapper start
+df_bmp280_wrapper start
+sensors start
+commander start
+ekf2 start
+land_detector start multicopter
+mc_pos_control start
+mc_att_control start
 uart_esc start -D /dev/tty-2
-csr_gps start -D /dev/tty-3
+rc_receiver start -D /dev/tty-1
+sleep 1
 list_devices
 list_files
 list_tasks

From 9131647566ba789968f935ffc58814e9f1d3d3b0 Mon Sep 17 00:00:00 2001
From: bharathr <bharathr.mail@gmail.com>
Date: Thu, 16 Jun 2016 18:11:43 -0700
Subject: [PATCH 0895/1230] Fixed UART_ESC_MODEL parameter in 200qx config file

---
 posix-configs/eagle/200qx/px4-v1-board.config | 2 +-
 posix-configs/eagle/200qx/px4-v2-board.config | 2 +-
 2 files changed, 2 insertions(+), 2 deletions(-)

diff --git a/posix-configs/eagle/200qx/px4-v1-board.config b/posix-configs/eagle/200qx/px4-v1-board.config
index 1428b5197b..f4ebfe96ee 100644
--- a/posix-configs/eagle/200qx/px4-v1-board.config
+++ b/posix-configs/eagle/200qx/px4-v1-board.config
@@ -51,7 +51,7 @@ param set ATT_W_MAG 0.00
 param set PE_MAG_NOISE 1.0f
 param set SENS_BOARD_ROT 6
 param set RC_RECEIVER_TYPE 1
-param set UART_ESC_MODEL 0
+param set UART_ESC_MODEL 2
 param set UART_ESC_BAUDRTE 250000
 param set UART_ESC_MOTOR1 2
 param set UART_ESC_MOTOR2 4
diff --git a/posix-configs/eagle/200qx/px4-v2-board.config b/posix-configs/eagle/200qx/px4-v2-board.config
index 2d6c33f6ce..9b2dac3d45 100644
--- a/posix-configs/eagle/200qx/px4-v2-board.config
+++ b/posix-configs/eagle/200qx/px4-v2-board.config
@@ -51,7 +51,7 @@ param set ATT_W_MAG 0.00
 param set PE_MAG_NOISE 1.0f
 param set SENS_BOARD_ROT 0
 param set RC_RECEIVER_TYPE 1
-param set UART_ESC_MODEL 0
+param set UART_ESC_MODEL 2
 param set UART_ESC_BAUDRTE 250000
 param set UART_ESC_MOTOR1 2
 param set UART_ESC_MOTOR2 4

From 29be7c30035177a46b1966d685327673ff4c586d Mon Sep 17 00:00:00 2001
From: bharathr <bharathr.mail@gmail.com>
Date: Thu, 23 Jun 2016 18:44:35 -0700
Subject: [PATCH 0896/1230] Updated motor-esc mappings in 200qx config file

---
 posix-configs/eagle/200qx/px4-v2-board.config | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/posix-configs/eagle/200qx/px4-v2-board.config b/posix-configs/eagle/200qx/px4-v2-board.config
index 9b2dac3d45..5bbd3c0703 100644
--- a/posix-configs/eagle/200qx/px4-v2-board.config
+++ b/posix-configs/eagle/200qx/px4-v2-board.config
@@ -53,8 +53,8 @@ param set SENS_BOARD_ROT 0
 param set RC_RECEIVER_TYPE 1
 param set UART_ESC_MODEL 2
 param set UART_ESC_BAUDRTE 250000
-param set UART_ESC_MOTOR1 2
-param set UART_ESC_MOTOR2 4
+param set UART_ESC_MOTOR1 4
+param set UART_ESC_MOTOR2 2
 param set UART_ESC_MOTOR3 1
 param set UART_ESC_MOTOR4 3
 sleep 1

From d96810f250d60481193cf15b7e99cd49d5106028 Mon Sep 17 00:00:00 2001
From: bharathr <bharathr.mail@gmail.com>
Date: Tue, 28 Jun 2016 22:15:27 -0700
Subject: [PATCH 0897/1230] Removed px4 config file for 200qx P1 board, updated
 P2 board version to support Qualcomm ESCs

---
 posix-configs/eagle/200qx/px4-v1-board.config | 75 -------------------
 .../200qx/{px4-v2-board.config => px4.config} |  0
 2 files changed, 75 deletions(-)
 delete mode 100644 posix-configs/eagle/200qx/px4-v1-board.config
 rename posix-configs/eagle/200qx/{px4-v2-board.config => px4.config} (100%)

diff --git a/posix-configs/eagle/200qx/px4-v1-board.config b/posix-configs/eagle/200qx/px4-v1-board.config
deleted file mode 100644
index f4ebfe96ee..0000000000
--- a/posix-configs/eagle/200qx/px4-v1-board.config
+++ /dev/null
@@ -1,75 +0,0 @@
-uorb start
-qshell start
-param set CAL_GYRO0_ID 100
-param set CAL_ACC0_ID 100
-param set CAL_MAG0_ID 101
-param set SYS_AUTOSTART 4001
-param set MAV_TYPE 2
-param set MC_YAW_P 7.0
-param set MC_YAWRATE_P 0.08
-param set MC_YAWRATE_I 0.0
-param set MC_YAWRATE_D 0
-param set MC_PITCH_P 7.0
-param set MC_PITCHRATE_P 0.08
-param set MC_PITCHRATE_I 0.0
-param set MC_PITCHRATE_D 0.0001
-param set MC_ROLL_P 7.0
-param set MC_ROLLRATE_P 0.08
-param set MC_ROLLRATE_I 0.0
-param set MC_ROLLRATE_D 0.0001
-param set RC_MAP_THROTTLE 1
-param set RC_MAP_ROLL 2
-param set RC_MAP_PITCH 3
-param set RC_MAP_YAW 4
-param set RC_MAP_MODE_SW 5
-param set RC_MAP_POSCTL_SW 6
-param set RC1_MAX 1900
-param set RC1_MIN 1099
-param set RC1_TRIM 1099
-param set RC1_REV 1
-param set RC2_MAX 1900
-param set RC2_MIN 1099
-param set RC2_TRIM 1500
-param set RC2_REV -1
-param set RC3_MAX 1900
-param set RC3_MIN 1099
-param set RC3_TRIM 1500
-param set RC3_REV 1
-param set RC4_MAX 1900
-param set RC4_MIN 1099
-param set RC4_TRIM 1500
-param set RC4_REV -1
-param set RC5_MAX 1900
-param set RC5_MIN 1099
-param set RC5_TRIM 1500
-param set RC5_REV 1
-param set RC6_MAX 1900
-param set RC6_MIN 1099
-param set RC6_TRIM 1099
-param set RC6_REV 1
-param set ATT_W_MAG 0.00
-param set PE_MAG_NOISE 1.0f
-param set SENS_BOARD_ROT 6
-param set RC_RECEIVER_TYPE 1
-param set UART_ESC_MODEL 2
-param set UART_ESC_BAUDRTE 250000
-param set UART_ESC_MOTOR1 2
-param set UART_ESC_MOTOR2 4
-param set UART_ESC_MOTOR3 1
-param set UART_ESC_MOTOR4 3
-sleep 1
-df_mpu9250_wrapper start
-df_bmp280_wrapper start
-sensors start
-commander start
-ekf2 start
-land_detector start multicopter
-mc_pos_control start
-mc_att_control start
-uart_esc start -D /dev/tty-2
-rc_receiver start -D /dev/tty-1
-sleep 1
-list_devices
-list_files
-list_tasks
-list_topics
diff --git a/posix-configs/eagle/200qx/px4-v2-board.config b/posix-configs/eagle/200qx/px4.config
similarity index 100%
rename from posix-configs/eagle/200qx/px4-v2-board.config
rename to posix-configs/eagle/200qx/px4.config

From 8d254058fabc0fe8fe50526bbcd0c3b34db2ad51 Mon Sep 17 00:00:00 2001
From: Andreas Antener <antener_a@gmx.ch>
Date: Sun, 3 Jul 2016 15:03:54 +0200
Subject: [PATCH 0898/1230] added square wave mode and PWM max parameter

---
 src/systemcmds/motor_ramp/motor_ramp.cpp | 101 +++++++++++++++--------
 1 file changed, 68 insertions(+), 33 deletions(-)

diff --git a/src/systemcmds/motor_ramp/motor_ramp.cpp b/src/systemcmds/motor_ramp/motor_ramp.cpp
index ea3e7ba272..a7b6968cf1 100644
--- a/src/systemcmds/motor_ramp/motor_ramp.cpp
+++ b/src/systemcmds/motor_ramp/motor_ramp.cpp
@@ -59,14 +59,6 @@
 #include "systemlib/systemlib.h"
 #include "systemlib/err.h"
 
-static bool _thread_should_exit = false;		/**< motor_ramp exit flag */
-static bool _thread_running = false;		/**< motor_ramp status flag */
-static int _motor_ramp_task;				/**< Handle of motor_ramp task / thread */
-static float _ramp_time;
-static int _min_pwm;
-static bool _sine_output = false;
-
-
 enum RampState {
 	RAMP_INIT,
 	RAMP_MIN,
@@ -74,6 +66,21 @@ enum RampState {
 	RAMP_WAIT
 };
 
+enum Mode {
+	RAMP,
+	SINE,
+	SQUARE
+};
+
+static bool _thread_should_exit = false;		/**< motor_ramp exit flag */
+static bool _thread_running = false;		/**< motor_ramp status flag */
+static int _motor_ramp_task;				/**< Handle of motor_ramp task / thread */
+static float _ramp_time;
+static int _min_pwm;
+static int _max_pwm;
+static Mode _mode;
+static char *_mode_c;
+
 /**
  * motor_ramp management function.
  */
@@ -86,6 +93,8 @@ int motor_ramp_thread_main(int argc, char *argv[]);
 
 bool min_pwm_valid(unsigned pwm_value);
 
+bool max_pwm_valid(unsigned pwm_value);
+
 int set_min_pwm(int fd, unsigned long max_channels, unsigned pwm_value);
 
 int set_out(int fd, unsigned long max_channels, float output);
@@ -105,12 +114,12 @@ usage(const char *reason)
 	}
 
 	PX4_WARN("\n\nWARNING: motors will ramp up to full speed!\n\n"
-		 "Usage: motor_ramp <min_pwm> <ramp_time> <-s>\n"
-		 "Setting option <-s> will enable sinus output with period <ramp_time>\n\n"
+		 "Usage: motor_ramp <mode> <min_pwm> <time> [<max_pwm>]\n"
+		 "<mode> can be one of (ramp|sine|square)\n\n"
 		 "Example:\n"
-		 "nsh> sdlog2 on\n"
-		 "nsh> mc_att_control stop\n"
-		 "nsh> motor_ramp 1100 0.5\n");
+		 "sdlog2 on\n"
+		 "mc_att_control stop\n"
+		 "motor_ramp sine 1100 0.5\n");
 }
 
 /**
@@ -123,7 +132,7 @@ usage(const char *reason)
  */
 int motor_ramp_main(int argc, char *argv[])
 {
-	if (argc < 3) {
+	if (argc < 4) {
 		usage("missing parameters");
 		return 1;
 	}
@@ -134,23 +143,41 @@ int motor_ramp_main(int argc, char *argv[])
 		return 0;
 	}
 
-	_min_pwm = atoi(argv[1]);
+	if (!strcmp(argv[1], "ramp")) {
+		_mode = RAMP;
 
-	if (!min_pwm_valid(_min_pwm)) {
-		usage("min pwm not in range");
+	} else if (!strcmp(argv[1], "sine")) {
+		_mode = SINE;
+
+	} else if (!strcmp(argv[1], "square")) {
+		_mode = SQUARE;
+
+	} else {
+		usage("selected mode not valid");
 		return 1;
 	}
 
-	_ramp_time = atof(argv[2]);
+	_mode_c = argv[1];
 
-	// check if we should apply sine ouput instead of ramp
-	if (argc > 3) {
-		if (strcmp(argv[3], "-s") == 0) {
-			_sine_output = true;
+	_min_pwm = atoi(argv[2]);
+
+	if (!min_pwm_valid(_min_pwm)) {
+		usage("min PWM not in range");
+		return 1;
+	}
+
+	_ramp_time = atof(argv[3]);
+
+	if (argc > 4) {
+		_max_pwm = atoi(argv[4]);
+
+		if (!max_pwm_valid(_max_pwm)) {
+			usage("max PWM not in range");
+			return 1;
 		}
 
 	} else {
-		_sine_output = false;
+		_max_pwm = 2000;
 	}
 
 	if (!(_ramp_time > 0)) {
@@ -173,7 +200,12 @@ int motor_ramp_main(int argc, char *argv[])
 
 bool min_pwm_valid(unsigned pwm_value)
 {
-	return pwm_value > 900 && pwm_value < 1300;
+	return pwm_value >= 900 && pwm_value <= 1500;
+}
+
+bool max_pwm_valid(unsigned pwm_value)
+{
+	return pwm_value <= 2100 && pwm_value > _min_pwm;
 }
 
 int set_min_pwm(int fd, unsigned long max_channels, unsigned pwm_value)
@@ -202,7 +234,7 @@ int set_min_pwm(int fd, unsigned long max_channels, unsigned pwm_value)
 int set_out(int fd, unsigned long max_channels, float output)
 {
 	int ret;
-	int pwm = (2000 - _min_pwm) * output + _min_pwm;
+	int pwm = (_max_pwm - _min_pwm) * output + _min_pwm;
 
 	for (unsigned i = 0; i < max_channels; i++) {
 
@@ -286,7 +318,7 @@ int motor_ramp_thread_main(int argc, char *argv[])
 
 	set_out(fd, max_channels, 0.0f);
 
-	float dt = 0.01f; // prevent division with 0
+	float dt = 0.001f; // prevent division with 0
 	float timer = 0.0f;
 	hrt_abstime start = 0;
 	hrt_abstime last_run = 0;
@@ -316,7 +348,7 @@ int motor_ramp_thread_main(int argc, char *argv[])
 
 		case RAMP_MIN: {
 				if (timer > 3.0f) {
-					PX4_WARN("starting %s: %.2f sec", _sine_output ? "sine" : "ramp", (double)_ramp_time);
+					PX4_WARN("starting %s: %.2f sec", _mode_c, (double)_ramp_time);
 					start = hrt_absolute_time();
 					ramp_state = RAMP_RAMP;
 				}
@@ -326,21 +358,24 @@ int motor_ramp_thread_main(int argc, char *argv[])
 			}
 
 		case RAMP_RAMP: {
-				if (!_sine_output) {
+				if (_mode == RAMP) {
 					output += dt / _ramp_time;
 
-				} else {
+				} else if (_mode == SINE) {
 					// sine outpout with period T = _ramp_time and magnitude between [0,1]
 					// phase shift makes sure that it starts at zero when timer is zero
 					output = 0.5f * (1.0f + sinf(M_TWOPI_F * timer / _ramp_time - M_PI_2_F));
+
+				} else if (_mode == SQUARE) {
+					output = fmodf(timer, _ramp_time) > (_ramp_time * 0.5f) ? 1.0f : 0.0f;
 				}
 
-				if ((output > 1.0f && !_sine_output) || (timer > 3.0f * _ramp_time && _sine_output)) {
-					// for ramp mode we set output to 1, for sine mode we just leave it as is
-					output = _sine_output ? output : 1.0f;
+				if ((output > 1.0f && _mode == RAMP) || (timer > 3.0f * _ramp_time)) {
+					// for ramp mode we set output to 1, for others we just leave it as is
+					output = _mode != RAMP ? output : 1.0f;
 					start = hrt_absolute_time();
 					ramp_state = RAMP_WAIT;
-					PX4_WARN("%s finished, waiting", _sine_output ? "sine" : "ramp");
+					PX4_WARN("%s finished, waiting", _mode_c);
 				}
 
 				set_out(fd, max_channels, output);

From a9a3050682b55c0ab1997caa8884dcb1a369bc11 Mon Sep 17 00:00:00 2001
From: Daniel Agar <daniel@agar.ca>
Date: Mon, 4 Jul 2016 02:18:15 -0400
Subject: [PATCH 0899/1230] EKF2 HIL gps decrease s_variance_m_s 5.0 -> 1.0
 (#4973)

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

diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp
index 0af8bb3139..52aa816968 100644
--- a/src/modules/mavlink/mavlink_receiver.cpp
+++ b/src/modules/mavlink/mavlink_receiver.cpp
@@ -1789,7 +1789,7 @@ MavlinkReceiver::handle_message_hil_gps(mavlink_message_t *msg)
 	hil_gps.eph = (float)gps.eph * 1e-2f; // from cm to m
 	hil_gps.epv = (float)gps.epv * 1e-2f; // from cm to m
 
-	hil_gps.s_variance_m_s = 5.0f;
+	hil_gps.s_variance_m_s = 1.0f;
 
 	hil_gps.vel_m_s = (float)gps.vel * 1e-2f; // from cm/s to m/s
 	hil_gps.vel_n_m_s = gps.vn * 1e-2f; // from cm to m

From 7f89994785716dfe69905fc424491ba867e2ea3d Mon Sep 17 00:00:00 2001
From: Daniel Agar <daniel@agar.ca>
Date: Mon, 4 Jul 2016 15:41:16 -0400
Subject: [PATCH 0900/1230] use NAV_ACC_RAD for vertical waypoint acceptance
 (#4978)

---
 ROMFS/px4fmu_common/init.d/rc.fw_defaults | 4 +++-
 src/modules/navigator/mission_block.cpp   | 9 ++++++---
 src/modules/navigator/navigator_params.c  | 1 +
 3 files changed, 10 insertions(+), 4 deletions(-)

diff --git a/ROMFS/px4fmu_common/init.d/rc.fw_defaults b/ROMFS/px4fmu_common/init.d/rc.fw_defaults
index f876353263..1ae8f81227 100644
--- a/ROMFS/px4fmu_common/init.d/rc.fw_defaults
+++ b/ROMFS/px4fmu_common/init.d/rc.fw_defaults
@@ -11,7 +11,9 @@ then
 	param set RTL_DESCEND_ALT 100
 	param set RTL_LAND_DELAY -1
 
-	param set NAV_ACC_RAD 50
+	# FW uses L1 distance for acceptance radius 
+	#  set a smaller NAV_ACC_RAD for vertical acceptance distance
+	param set NAV_ACC_RAD 10
 
 	param set PE_VELNE_NOISE 0.3
 	param set PE_VELD_NOISE 0.35
diff --git a/src/modules/navigator/mission_block.cpp b/src/modules/navigator/mission_block.cpp
index f00e05d1fe..cd838e3fa6 100644
--- a/src/modules/navigator/mission_block.cpp
+++ b/src/modules/navigator/mission_block.cpp
@@ -170,7 +170,8 @@ MissionBlock::is_mission_item_reached()
 			}
 		} else if (_mission_item.nav_cmd == NAV_CMD_TAKEOFF) {
 			/* for takeoff mission items use the parameter for the takeoff acceptance radius */
-			if (dist >= 0.0f && dist <= _navigator->get_acceptance_radius()) {
+			if (dist >= 0.0f && dist <= _navigator->get_acceptance_radius()
+				&& dist_z <= _navigator->get_default_acceptance_radius()) {
 				_waypoint_position_reached = true;
 			}
 		} else if (!_navigator->get_vstatus()->is_rotary_wing &&
@@ -183,7 +184,8 @@ MissionBlock::is_mission_item_reached()
 			 * Therefore the item is marked as reached once the system reaches the loiter
 			 * radius (+ some margin). Time inside and turn count is handled elsewhere.
 			 */
-			if (dist >= 0.0f && dist <= _navigator->get_acceptance_radius(_mission_item.loiter_radius * 1.2f)) {
+			if (dist >= 0.0f && dist <= _navigator->get_acceptance_radius(_mission_item.loiter_radius * 1.2f)
+				&& dist_z <= _navigator->get_default_acceptance_radius()) {
 				_waypoint_position_reached = true;
 			}
 		} else {
@@ -195,7 +197,8 @@ MissionBlock::is_mission_item_reached()
 				mission_acceptance_radius = _navigator->get_acceptance_radius();
 			}
 
-			if (dist >= 0.0f && dist <= mission_acceptance_radius) {
+			if (dist >= 0.0f && dist <= mission_acceptance_radius
+				&& dist_z <= _navigator->get_default_acceptance_radius()) {
 				_waypoint_position_reached = true;
 			}
 		}
diff --git a/src/modules/navigator/navigator_params.c b/src/modules/navigator/navigator_params.c
index 63701b4416..f2eaf9ba5c 100644
--- a/src/modules/navigator/navigator_params.c
+++ b/src/modules/navigator/navigator_params.c
@@ -58,6 +58,7 @@ PARAM_DEFINE_FLOAT(NAV_LOITER_RAD, 50.0f);
  * Acceptance Radius
  *
  * Default acceptance radius, overridden by acceptance radius of waypoint if set.
+ * For fixed wing NAV_ACC_RAD is the vertical acceptance, as the L1 turning distance is used for horizontal acceptance.
  *
  * @unit m
  * @min 0.05

From dc1f34350158046dc6e6b93f6260c37ceb9494b3 Mon Sep 17 00:00:00 2001
From: Eike <ecm@gmx.de>
Date: Mon, 4 Jul 2016 21:41:58 +0200
Subject: [PATCH 0901/1230] Change lower range limit of SF10a to 0.01m (#4977)

* SF10a driver added

* Remove Sf10a driver

* Set lower range boundary to 0.01m for LPE
---
 src/drivers/sf10a/sf10a.cpp | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/src/drivers/sf10a/sf10a.cpp b/src/drivers/sf10a/sf10a.cpp
index 21b0a8bcb7..ecfa4020d1 100644
--- a/src/drivers/sf10a/sf10a.cpp
+++ b/src/drivers/sf10a/sf10a.cpp
@@ -80,7 +80,7 @@
 #define SF10A_DEVICE_PATH	"/dev/sf10a"
 
 /* Device limits */
-#define SF10A_MIN_DISTANCE 	(0.0f)
+#define SF10A_MIN_DISTANCE 	(0.01f)
 #define SF10A_MAX_DISTANCE 	(25.0f)
 
 

From a455962e1737a99581815afdae302a2cea612f39 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Beat=20K=C3=BCng?= <beat-kueng@gmx.net>
Date: Mon, 4 Jul 2016 16:27:28 +0200
Subject: [PATCH 0902/1230] logger: only call write_add_logged_msg when
 sucessfully subscribed

---
 src/modules/logger/logger.cpp | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/src/modules/logger/logger.cpp b/src/modules/logger/logger.cpp
index f86d9eae8c..5da679b82b 100644
--- a/src/modules/logger/logger.cpp
+++ b/src/modules/logger/logger.cpp
@@ -420,12 +420,12 @@ bool Logger::copy_if_updated_multi(LoggerSubscription &sub, int multi_instance,
 		if (OK == orb_exists(sub.metadata, multi_instance)) {
 			handle = orb_subscribe_multi(sub.metadata, multi_instance);
 
-			write_add_logged_msg(sub, multi_instance);
-
 			//PX4_INFO("subscribed to instance %d of topic %s", multi_instance, topic->o_name);
 
 			/* copy first data */
 			if (handle >= 0) {
+				write_add_logged_msg(sub, multi_instance);
+
 				/* set to the same interval as the first instance */
 				unsigned int interval;
 

From c94fe845ec14c396efcb0f19077cefcb59dbcf4f Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Beat=20K=C3=BCng?= <beat-kueng@gmx.net>
Date: Mon, 4 Jul 2016 17:57:21 +0200
Subject: [PATCH 0903/1230] fix logger: remove space in format for changed
 parameters

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

diff --git a/src/modules/logger/logger.cpp b/src/modules/logger/logger.cpp
index 5da679b82b..653b3d076e 100644
--- a/src/modules/logger/logger.cpp
+++ b/src/modules/logger/logger.cpp
@@ -1249,7 +1249,7 @@ void Logger::write_changed_parameters()
 			}
 
 			/* format parameter key (type and name) */
-			msg->key_len = snprintf(msg->key, sizeof(msg->key), "%s %s ", type_str, param_name(param));
+			msg->key_len = snprintf(msg->key, sizeof(msg->key), "%s %s", type_str, param_name(param));
 			size_t msg_size = sizeof(*msg) - sizeof(msg->key) + msg->key_len;
 
 			/* copy parameter value directly to buffer */

From c4e77cf41113ae775b87078104b239d88bd28603 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Beat=20K=C3=BCng?= <beat-kueng@gmx.net>
Date: Tue, 5 Jul 2016 13:40:01 +0200
Subject: [PATCH 0904/1230] logger: fix '-r 0' parameter: should be unlimited
 rate instead of 1Hz

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

diff --git a/src/modules/logger/logger.cpp b/src/modules/logger/logger.cpp
index 653b3d076e..609333011f 100644
--- a/src/modules/logger/logger.cpp
+++ b/src/modules/logger/logger.cpp
@@ -252,7 +252,7 @@ void Logger::run_trampoline(int argc, char *argv[])
 				unsigned long r = strtoul(myoptarg, NULL, 10);
 
 				if (r <= 0) {
-					r = 1;
+					r = 1e6;
 				}
 
 				log_interval = 1e6 / r;

From e34229920496238c95418d5ffe24cb592491a1ec Mon Sep 17 00:00:00 2001
From: Ivan Dimitrov <ivand@hobbylab.org>
Date: Tue, 5 Jul 2016 15:17:32 +0300
Subject: [PATCH 0905/1230] Fixed filename in the Doxygen field. See issue
 "wrong doxygen descriptor (file name) #4979"

---
 src/modules/vtol_att_control/tailsitter.h | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/src/modules/vtol_att_control/tailsitter.h b/src/modules/vtol_att_control/tailsitter.h
index 17fdcae965..456efab568 100644
--- a/src/modules/vtol_att_control/tailsitter.h
+++ b/src/modules/vtol_att_control/tailsitter.h
@@ -32,7 +32,7 @@
  ****************************************************************************/
 
 /**
-* @file tiltrotor.h
+* @file tailsitter.h
 *
 * @author Roman Bapst 		<bapstroman@gmail.com>
 * @author David Vorsin     <davidvorsin@gmail.com>

From 3524fd7d2434e04c001134fe136a6ce441addb15 Mon Sep 17 00:00:00 2001
From: Julian Oes <julian@oes.ch>
Date: Tue, 5 Jul 2016 17:40:37 +0200
Subject: [PATCH 0906/1230] ekf2: don't ignore function argument (#4990)

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

diff --git a/src/modules/ekf2/ekf2_main.cpp b/src/modules/ekf2/ekf2_main.cpp
index a02e1ceeb3..e1f746e58d 100644
--- a/src/modules/ekf2/ekf2_main.cpp
+++ b/src/modules/ekf2/ekf2_main.cpp
@@ -116,7 +116,7 @@ public:
 	 */
 	int		start();
 
-	void 	set_replay_mode(bool replay) {_replay_mode = true;};
+	void 	set_replay_mode(bool replay) {_replay_mode = replay;};
 
 	static void	task_main_trampoline(int argc, char *argv[]);
 

From 09ddc24801b4a630ff261e9eafab97cc4d951ba0 Mon Sep 17 00:00:00 2001
From: James Goppert <jgoppert@users.noreply.github.com>
Date: Tue, 5 Jul 2016 16:54:08 -0400
Subject: [PATCH 0907/1230] Added agl smoothing to LPE. (#4976)

---
 .../BlockLocalPositionEstimator.cpp                      | 9 ++++++---
 .../BlockLocalPositionEstimator.hpp                      | 1 +
 2 files changed, 7 insertions(+), 3 deletions(-)

diff --git a/src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp b/src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp
index 77c2748921..fb9c8ea712 100644
--- a/src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp
+++ b/src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp
@@ -106,6 +106,8 @@ BlockLocalPositionEstimator::BlockLocalPositionEstimator() :
 
 	// low pass
 	_xLowPass(this, "X_LP"),
+	// use same lp constant for agl
+	_aglLowPass(this, "X_LP"),
 
 	// delay
 	_xDelay(this, ""),
@@ -275,6 +277,7 @@ void BlockLocalPositionEstimator::update()
 
 		// reset lowpass filter as well
 		_xLowPass.setState(_x);
+		_aglLowPass.setState(0);
 	}
 
 	_lastArmedState = armedState;
@@ -550,8 +553,7 @@ void BlockLocalPositionEstimator::checkTimeouts()
 
 float BlockLocalPositionEstimator::agl()
 {
-	const Vector<float, n_x> &xLP = _xLowPass.getState();
-	return xLP(X_tz) - xLP(X_z);
+	return _x(X_tz) - _x(X_z);
 }
 
 void BlockLocalPositionEstimator::correctionLogic(Vector<float, n_x> &dx)
@@ -676,7 +678,7 @@ void BlockLocalPositionEstimator::publishLocalPos()
 		_pub_lpos.get().ref_lat = _map_ref.lat_rad * 180 / M_PI;
 		_pub_lpos.get().ref_lon = _map_ref.lon_rad * 180 / M_PI;
 		_pub_lpos.get().ref_alt = _sub_home.get().alt;
-		_pub_lpos.get().dist_bottom = agl();
+		_pub_lpos.get().dist_bottom = _aglLowPass.getState();
 		_pub_lpos.get().dist_bottom_rate = - xLP(X_vz);
 		_pub_lpos.get().surface_bottom_timestamp = _timeStamp;
 		_pub_lpos.get().dist_bottom_valid = _validTZ && _validZ;
@@ -872,4 +874,5 @@ void BlockLocalPositionEstimator::predict()
 	       _B * _R * _B.transpose() +
 	       _Q) * getDt();
 	_xLowPass.update(_x);
+	_aglLowPass.update(agl());
 }
diff --git a/src/modules/local_position_estimator/BlockLocalPositionEstimator.hpp b/src/modules/local_position_estimator/BlockLocalPositionEstimator.hpp
index 7a5028424f..c541f53e03 100644
--- a/src/modules/local_position_estimator/BlockLocalPositionEstimator.hpp
+++ b/src/modules/local_position_estimator/BlockLocalPositionEstimator.hpp
@@ -311,6 +311,7 @@ private:
 
 	// low pass
 	BlockLowPassVector<float, n_x> _xLowPass;
+	BlockLowPass _aglLowPass;
 
 	// delay blocks
 	BlockDelay<float, n_x, 1, HIST_LEN> _xDelay;

From 9c0ed52bb835c668533fe6e359c4b2ae8721e456 Mon Sep 17 00:00:00 2001
From: James Goppert <jgoppert@users.noreply.github.com>
Date: Tue, 5 Jul 2016 17:38:35 -0400
Subject: [PATCH 0908/1230] Updated matrix lib to 1.0.0. (#4991)

* Updated matrix lib to 1.0.0.

* Bump matrix version.
---
 src/lib/matrix | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/src/lib/matrix b/src/lib/matrix
index 3c87ae78ff..3bc7c19c70 160000
--- a/src/lib/matrix
+++ b/src/lib/matrix
@@ -1 +1 @@
-Subproject commit 3c87ae78ff4d80d1643b72afd2783478e87b8e5b
+Subproject commit 3bc7c19c70bd72b58b07700bff282ae30d73d399

From c38f23c0e177df5bd9ade4a1a74d8da177c1afde Mon Sep 17 00:00:00 2001
From: James Goppert <jgoppert@users.noreply.github.com>
Date: Tue, 5 Jul 2016 21:20:17 -0400
Subject: [PATCH 0909/1230] Bump matrix version. (#4993)

---
 src/lib/matrix | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/src/lib/matrix b/src/lib/matrix
index 3bc7c19c70..0b1bed6b00 160000
--- a/src/lib/matrix
+++ b/src/lib/matrix
@@ -1 +1 @@
-Subproject commit 3bc7c19c70bd72b58b07700bff282ae30d73d399
+Subproject commit 0b1bed6b00096248b81d65ec2be45843e20620b5

From 7e883809e33de34647dc4b485373ca341d1de29a Mon Sep 17 00:00:00 2001
From: James Goppert <james.goppert@gmail.com>
Date: Wed, 6 Jul 2016 01:43:59 -0400
Subject: [PATCH 0910/1230] Bump matrix version to 1.0.1.

---
 src/lib/matrix | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/src/lib/matrix b/src/lib/matrix
index 0b1bed6b00..cbb7e06a1f 160000
--- a/src/lib/matrix
+++ b/src/lib/matrix
@@ -1 +1 @@
-Subproject commit 0b1bed6b00096248b81d65ec2be45843e20620b5
+Subproject commit cbb7e06a1fa7e20304c3ca3099b18c704da6c3a4

From 09ecc84cc7a5a253fc777eb8cd3cd02d306fba40 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Beat=20K=C3=BCng?= <beat-kueng@gmx.net>
Date: Wed, 6 Jul 2016 09:32:37 +0200
Subject: [PATCH 0911/1230] gps file dump: re-implement with an uORB topic &
 write to the log file (#4987)

Drawbacks of the previous method: when writing to the SD card, there are
high delays in the write() call of several 100ms, every now and then. The
frequency and length of these events depend on:
- SD card
- used logger bandwidth
- bandwidth of gps data (RTCM)
Since the whole gps thread was blocked during this period, it lead to
gps timeouts and lost module.

What we do now is: publish an orb topic with queuing. This makes it async
and the logger takes care of buffering. This means it's best to:
- use high logger rate
- use large logger buffer
- reduce logger bandwith by disabling unused topics
---
 msg/CMakeLists.txt            |   1 +
 msg/gps_dump.msg              |   6 ++
 src/drivers/gps/gps.cpp       | 175 ++++++++++++----------------------
 src/drivers/gps/params.c      |   6 +-
 src/modules/logger/logger.cpp |   1 +
 5 files changed, 71 insertions(+), 118 deletions(-)
 create mode 100644 msg/gps_dump.msg

diff --git a/msg/CMakeLists.txt b/msg/CMakeLists.txt
index dedd4983ac..5a94a5ba54 100644
--- a/msg/CMakeLists.txt
+++ b/msg/CMakeLists.txt
@@ -61,6 +61,7 @@ set(msg_file_names
 	fw_virtual_attitude_setpoint.msg
 	fw_virtual_rates_setpoint.msg
 	geofence_result.msg
+	gps_dump.msg
 	gps_inject_data.msg
 	hil_sensor.msg
 	home_position.msg
diff --git a/msg/gps_dump.msg b/msg/gps_dump.msg
new file mode 100644
index 0000000000..8a91149d46
--- /dev/null
+++ b/msg/gps_dump.msg
@@ -0,0 +1,6 @@
+# This message is used to dump the raw gps communication to the log.
+# Set the parameter GPS_DUMP_COMM to 1 to use this.
+
+uint8 len			# length of data, MSB bit set = message to the gps device,
+				# clear = message from the device
+uint8[79] data			# data to write to the log
diff --git a/src/drivers/gps/gps.cpp b/src/drivers/gps/gps.cpp
index 6d9d0f07c8..bd3eb10807 100644
--- a/src/drivers/gps/gps.cpp
+++ b/src/drivers/gps/gps.cpp
@@ -76,9 +76,9 @@
 #include <drivers/drv_gps.h>
 #include <uORB/uORB.h>
 #include <uORB/topics/vehicle_gps_position.h>
-#include <uORB/topics/vehicle_status.h>
 #include <uORB/topics/satellite_info.h>
 #include <uORB/topics/gps_inject_data.h>
+#include <uORB/topics/gps_dump.h>
 
 #include <board_config.h>
 
@@ -142,16 +142,9 @@ private:
 	int _orb_inject_data_fd[_orb_inject_data_fd_count];
 	int _orb_inject_data_next = 0;
 
-	// dump communication to file
-#ifdef __PX4_POSIX_EAGLE
-	static constexpr const char	*DUMP_ROOT = PX4_ROOTFSDIR;
-#else
-	static constexpr const char 	*DUMP_ROOT = PX4_ROOTFSDIR"/fs/microsd";
-#endif
-	int _dump_to_gps_device_fd = -1;              ///< file descriptors, to safe all device communication to a file
-	int _dump_from_gps_device_fd = -1;
-	int _vehicle_status_sub = -1;
-	bool _is_armed = false;                       ///< current arming state (only updated if communication dump is enabled)
+	orb_advert_t _dump_communication_pub;			///< if non-null, dump communication
+	gps_dump_s *_dump_to_device;
+	gps_dump_s *_dump_from_device;
 
 	/**
 	 * Try to configure the GPS, handle outgoing communication to the GPS
@@ -225,13 +218,14 @@ private:
 	static int callback(GPSCallbackType type, void *data1, int data2, void *user);
 
 	/**
-	 * Setup dumping of the GPS device input and output stream to a file.
+	 * Dump gps communication.
+	 * @param data message
+	 * @param len length of the message
+	 * @param msg_to_gps_device if true, this is a message sent to the gps device, otherwise it's from the device
 	 */
+	void dumpGpsData(uint8_t *data, size_t len, bool msg_to_gps_device);
+
 	void initializeCommunicationDump();
-
-	int getDumpFileName(char *file_name, size_t file_name_size, const char *suffix);
-
-	static bool fileExist(const char *filename);
 };
 
 
@@ -264,7 +258,10 @@ GPS::GPS(const char *uart_path, bool fake_gps, bool enable_sat_info, int gps_num
 	_rate_rtcm_injection(0.0f),
 	_last_rate_rtcm_injection_count(0),
 	_fake_gps(fake_gps),
-	_gps_num(gps_num)
+	_gps_num(gps_num),
+	_dump_communication_pub(nullptr),
+	_dump_to_device(nullptr),
+	_dump_from_device(nullptr)
 {
 	/* store port name */
 	strncpy(_port, uart_path, sizeof(_port));
@@ -305,6 +302,13 @@ GPS::~GPS()
 		delete(_sat_info);
 	}
 
+	if (_dump_to_device) {
+		delete(_dump_to_device);
+	}
+
+	if (_dump_from_device) {
+		delete(_dump_from_device);
+	}
 
 }
 
@@ -340,21 +344,15 @@ int GPS::callback(GPSCallbackType type, void *data1, int data2, void *user)
 	case GPSCallbackType::readDeviceData: {
 			int num_read = gps->pollOrRead((uint8_t *)data1, data2, *((int *)data1));
 
-			if (num_read > 0 && gps->_dump_from_gps_device_fd >= 0) {
-				if (write(gps->_dump_from_gps_device_fd, data1, (size_t)num_read) != (size_t)num_read) {
-					PX4_WARN("gps dump failed");
-				}
+			if (num_read > 0) {
+				gps->dumpGpsData((uint8_t *)data1, (size_t)num_read, false);
 			}
 
 			return num_read;
 		}
 
 	case GPSCallbackType::writeDeviceData:
-		if (gps->_dump_to_gps_device_fd >= 0) {
-			if (write(gps->_dump_to_gps_device_fd, data1, (size_t)data2) != (size_t)data2) {
-				PX4_WARN("gps dump failed");
-			}
-		}
+		gps->dumpGpsData((uint8_t *)data1, (size_t)data2, true);
 
 		return write(gps->_serial_fd, data1, (size_t)data2);
 
@@ -457,11 +455,7 @@ void GPS::handleInjectDataTopic()
 
 bool GPS::injectData(uint8_t *data, size_t len)
 {
-	if (_dump_to_gps_device_fd >= 0) {
-		if (write(_dump_to_gps_device_fd, data, len) != len) {
-			PX4_WARN("gps dump failed");
-		}
-	}
+	dumpGpsData(data, len, true);
 
 	size_t written = ::write(_serial_fd, data, len);
 	::fsync(_serial_fd);
@@ -580,17 +574,6 @@ int GPS::setBaudrate(unsigned baud)
 	return 0;
 }
 
-bool GPS::fileExist(const char *filename)
-{
-#ifdef __PX4_QURT
-	//FIXME: QuRT neither has stat() nor access(). Just pretend file does not exist.
-	return false;
-#else
-	struct stat buffer;
-	return stat(filename, &buffer) == 0;
-#endif
-}
-
 void GPS::initializeCommunicationDump()
 {
 	param_t gps_dump_comm_ph = param_find("GPS_DUMP_COMM");
@@ -604,53 +587,54 @@ void GPS::initializeCommunicationDump()
 		return; //dumping disabled
 	}
 
-	char to_device_file_name[128] = "";
-	char from_device_file_name[128] = "";
+	_dump_from_device = new gps_dump_s();
+	_dump_to_device = new gps_dump_s();
 
-	if (getDumpFileName(to_device_file_name, sizeof(to_device_file_name), "to")
-	    || getDumpFileName(from_device_file_name, sizeof(from_device_file_name), "from")) {
-		PX4_ERR("Failed to get GPS dump file name");
+	if (!_dump_from_device || !_dump_to_device) {
+		PX4_ERR("failed to allocated dump data");
 		return;
 	}
 
-	//open files
-	if ((_dump_to_gps_device_fd = open(to_device_file_name, O_CREAT | O_WRONLY | O_TRUNC, PX4_O_MODE_666)) < 0) {
-		return;
-	}
+	memset(_dump_to_device, 0, sizeof(gps_dump_s));
+	memset(_dump_from_device, 0, sizeof(gps_dump_s));
 
-	if ((_dump_from_gps_device_fd = open(from_device_file_name, O_CREAT | O_WRONLY | O_TRUNC, PX4_O_MODE_666)) < 0) {
-		close(_dump_to_gps_device_fd);
-		_dump_to_gps_device_fd = -1;
-		return;
-	}
-
-	PX4_INFO("Dumping GPS comm to files %s, %s", to_device_file_name, from_device_file_name);
-
-	_vehicle_status_sub = orb_subscribe(ORB_ID(vehicle_status));
+	int instance;
+	//make sure to use a large enough queue size, so that we don't lose messages. You may also want
+	//to increase the logger rate for that.
+	_dump_communication_pub = orb_advertise_multi_queue(ORB_ID(gps_dump), _dump_from_device, &instance,
+				  ORB_PRIO_DEFAULT, 8);
 }
 
-int GPS::getDumpFileName(char *file_name, size_t file_name_size, const char *suffix)
+void GPS::dumpGpsData(uint8_t *data, size_t len, bool msg_to_gps_device)
 {
-	uint16_t file_number = 1; // start with file gps_dump_<num>_<suffix>_dev_001.dat
+	if (!_dump_communication_pub) {
+		return;
+	}
 
-	const uint16_t max_num_files = 999;
+	gps_dump_s *dump_data = msg_to_gps_device ? _dump_to_device : _dump_from_device;
 
-	while (file_number <= max_num_files) {
-		snprintf(file_name, file_name_size, "%s/gps_dump_%u_%s_dev_%03u.dat",
-			 DUMP_ROOT, _gps_num, suffix, file_number);
+	while (len > 0) {
+		size_t write_len = len;
 
-		if (!fileExist(file_name)) {
-			break;
+		if (write_len > sizeof(dump_data->data) - dump_data->len) {
+			write_len = sizeof(dump_data->data) - dump_data->len;
 		}
 
-		file_number++;
-	}
+		memcpy(dump_data->data + dump_data->len, data, write_len);
+		data += write_len;
+		dump_data->len += write_len;
+		len -= write_len;
 
-	if (file_number > max_num_files) {
-		return -1;
-	}
+		if (dump_data->len >= sizeof(dump_data->data)) {
+			if (msg_to_gps_device) {
+				dump_data->len |= 1 << 7;
+			}
 
-	return 0;
+			dump_data->timestamp = hrt_absolute_time();
+			orb_publish(ORB_ID(gps_dump), _dump_communication_pub, dump_data);
+			dump_data->len = 0;
+		}
+	}
 }
 
 void
@@ -817,32 +801,6 @@ GPS::task_main()
 //						PX4_WARN("module found: %s", mode_str);
 						_healthy = true;
 					}
-
-					//check for disarming
-					if (_vehicle_status_sub != -1 && _dump_from_gps_device_fd != -1) {
-						bool updated;
-						orb_check(_vehicle_status_sub, &updated);
-
-						if (updated) {
-							vehicle_status_s vehicle_status;
-							orb_copy(ORB_ID(vehicle_status), _vehicle_status_sub, &vehicle_status);
-							bool armed = (vehicle_status.arming_state == vehicle_status_s::ARMING_STATE_ARMED) ||
-								     (vehicle_status.arming_state == vehicle_status_s::ARMING_STATE_ARMED_ERROR);
-
-							if (armed) {
-								_is_armed = true;
-
-							} else if (_is_armed) {
-								//disable communication dump when disarming
-								close(_dump_from_gps_device_fd);
-								_dump_from_gps_device_fd = -1;
-								close(_dump_to_gps_device_fd);
-								_dump_to_gps_device_fd = -1;
-								_is_armed = false;
-							}
-
-						}
-					}
 				}
 
 				if (_healthy) {
@@ -874,26 +832,15 @@ GPS::task_main()
 
 	}
 
-	PX4_WARN("exiting");
+	PX4_INFO("exiting");
 
 	for (size_t i = 0; i < _orb_inject_data_fd_count; ++i) {
 		orb_unsubscribe(_orb_inject_data_fd[i]);
 		_orb_inject_data_fd[i] = -1;
 	}
 
-	if (_dump_to_gps_device_fd != -1) {
-		close(_dump_to_gps_device_fd);
-		_dump_to_gps_device_fd = -1;
-	}
-
-	if (_dump_from_gps_device_fd != -1) {
-		close(_dump_from_gps_device_fd);
-		_dump_from_gps_device_fd = -1;
-	}
-
-	if (_vehicle_status_sub != -1) {
-		orb_unsubscribe(_vehicle_status_sub);
-		_vehicle_status_sub = -1;
+	if (_dump_communication_pub) {
+		orb_unadvertise(_dump_communication_pub);
 	}
 
 	::close(_serial_fd);
diff --git a/src/drivers/gps/params.c b/src/drivers/gps/params.c
index 9baba96507..75f4c3dc22 100644
--- a/src/drivers/gps/params.c
+++ b/src/drivers/gps/params.c
@@ -34,10 +34,8 @@
 /**
  * Dump GPS communication to a file.
  *
- * If this is set to 1, all GPS communication data will be written to a
- * file. Two files will be created, for reading and writing. All
- * communication from startup until device disarm will be dumped.
- *
+ * If this is set to 1, all GPS communication data will be published via uORB,
+ * and written to the log file as gps_dump message.
  * @min 0
  * @max 1
  * @value 0 Disable
diff --git a/src/modules/logger/logger.cpp b/src/modules/logger/logger.cpp
index 609333011f..4412809f79 100644
--- a/src/modules/logger/logger.cpp
+++ b/src/modules/logger/logger.cpp
@@ -469,6 +469,7 @@ void Logger::add_default_topics()
 	add_topic("servorail_status", 200);
 	add_topic("mc_att_ctrl_status", 50);
 	add_topic("vehicle_status", 200);
+	add_topic("gps_dump"); //this will only be published if GPS_DUMP_COMM is set
 }
 
 int Logger::add_topics_from_file(const char *fname)

From 96f053273c381dbef66f09a4b60e7eeed5a711ff Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Wed, 6 Jul 2016 13:30:58 +0200
Subject: [PATCH 0912/1230] Always start default mc apps

---
 ROMFS/px4fmu_common/init.d/rc.mc_apps | 14 ++------------
 1 file changed, 2 insertions(+), 12 deletions(-)

diff --git a/ROMFS/px4fmu_common/init.d/rc.mc_apps b/ROMFS/px4fmu_common/init.d/rc.mc_apps
index 56a349457f..399f4398ac 100644
--- a/ROMFS/px4fmu_common/init.d/rc.mc_apps
+++ b/ROMFS/px4fmu_common/init.d/rc.mc_apps
@@ -29,19 +29,9 @@ then
 fi
 #---------------------------------------
 
-if mc_att_control start
-then
-else
-	# try the multiplatform version
-	mc_att_control_m start
-fi
+mc_att_control start
 
-if mc_pos_control start
-then
-else
-	# try the multiplatform version
-	mc_pos_control_m start
-fi
+mc_pos_control start
 
 #
 # Start Land Detector

From 8a2155f2af9192f3cd76d2e3396fd4265087a4c2 Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Wed, 6 Jul 2016 13:31:11 +0200
Subject: [PATCH 0913/1230] UAVCAN is now in main RCs

---
 ROMFS/px4fmu_common/init.d/rc.uavcan | 29 ----------------------------
 1 file changed, 29 deletions(-)
 delete mode 100644 ROMFS/px4fmu_common/init.d/rc.uavcan

diff --git a/ROMFS/px4fmu_common/init.d/rc.uavcan b/ROMFS/px4fmu_common/init.d/rc.uavcan
deleted file mode 100644
index ebecea2de4..0000000000
--- a/ROMFS/px4fmu_common/init.d/rc.uavcan
+++ /dev/null
@@ -1,29 +0,0 @@
-#!nsh
-#
-# UAVCAN initialization script.
-#
-
-#
-# Starting stuff according to UAVCAN_ENABLE value
-#
-if param greater UAVCAN_ENABLE 0
-then
-	if uavcan start
-	then
-		echo "[i] UAVCAN started"
-	else
-		echo "[i] ERROR: Could not start UAVCAN"
-		tone_alarm $TUNE_ERR
-	fi
-fi
-
-if param greater UAVCAN_ENABLE 1
-then
-	if uavcan start fw
-	then
-		echo "[i] UAVCAN servers started"
-	else
-		echo "[i] ERROR: Could not start UAVCAN servers"
-		tone_alarm $TUNE_ERR
-	fi
-fi

From 7ed0eba50ea257861c22b3d5594091545199afee Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Wed, 6 Jul 2016 13:31:24 +0200
Subject: [PATCH 0914/1230] USB config is now in main boot

---
 ROMFS/px4fmu_common/init.d/rc.usb | 9 ---------
 1 file changed, 9 deletions(-)
 delete mode 100644 ROMFS/px4fmu_common/init.d/rc.usb

diff --git a/ROMFS/px4fmu_common/init.d/rc.usb b/ROMFS/px4fmu_common/init.d/rc.usb
deleted file mode 100644
index 4a157a683f..0000000000
--- a/ROMFS/px4fmu_common/init.d/rc.usb
+++ /dev/null
@@ -1,9 +0,0 @@
-#!nsh
-#
-# USB MAVLink start
-#
-
-mavlink start -r 800000 -d /dev/ttyACM0 -m config -x
-
-# Exit shell to make it available to MAVLink
-exit

From 332f669d9be2ccdfb915b9d5bb6cd4f040667950 Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Wed, 6 Jul 2016 13:32:10 +0200
Subject: [PATCH 0915/1230] Add tap-v1 config

---
 Images/tap-v1.prototype                      |  12 +
 Makefile                                     |   4 +
 ROMFS/tap_common/init.d/4001_quad_x          |  18 +
 ROMFS/tap_common/init.d/6001_hexa_x          |  19 +
 ROMFS/tap_common/init.d/rc.fw_apps           |  20 +
 ROMFS/tap_common/init.d/rc.fw_defaults       |  28 +
 ROMFS/tap_common/init.d/rc.interface         |  83 ++
 ROMFS/tap_common/init.d/rc.mc_apps           |  39 +
 ROMFS/tap_common/init.d/rc.mc_defaults       |  49 +
 ROMFS/tap_common/init.d/rc.sensors           |  46 +
 ROMFS/tap_common/init.d/rc.vtol_apps         |  44 +
 ROMFS/tap_common/init.d/rc.vtol_defaults     |  49 +
 ROMFS/tap_common/init.d/rcS                  | 482 +++++++++
 ROMFS/tap_common/mixers/README.md            | 105 ++
 ROMFS/tap_common/mixers/hexa_x.main.mix      |   3 +
 ROMFS/tap_common/mixers/quad_x.main.mix      |   7 +
 cmake/configs/nuttx_tap-v1_default.cmake     | 125 +++
 cmake/nuttx/px4_impl_nuttx.cmake             |  59 +-
 nuttx-configs/tap-v1/include/board.h         | 448 +++++++++
 nuttx-configs/tap-v1/include/nsh_romfsimg.h  |  42 +
 nuttx-configs/tap-v1/nsh/Make.defs           | 160 +++
 nuttx-configs/tap-v1/nsh/defconfig           | 982 +++++++++++++++++++
 nuttx-configs/tap-v1/nsh/setenv.sh           |  75 ++
 nuttx-configs/tap-v1/scripts/ld.script       | 149 +++
 nuttx-configs/tap-v1/src/Makefile            |  84 ++
 nuttx-configs/tap-v1/src/empty.c             |   4 +
 src/drivers/boards/tap-v1/CMakeLists.txt     |  48 +
 src/drivers/boards/tap-v1/board_config.h     | 276 ++++++
 src/drivers/boards/tap-v1/tap_init.c         | 204 ++++
 src/drivers/boards/tap-v1/tap_led.c          | 117 +++
 src/drivers/boards/tap-v1/tap_spi.c          |  88 ++
 src/drivers/boards/tap-v1/tap_timer_config.c | 100 ++
 src/drivers/boards/tap-v1/tap_usb.c          | 105 ++
 src/drivers/drv_gpio.h                       |  23 +-
 src/drivers/drv_sensor.h                     |   2 +-
 35 files changed, 4047 insertions(+), 52 deletions(-)
 create mode 100644 Images/tap-v1.prototype
 create mode 100644 ROMFS/tap_common/init.d/4001_quad_x
 create mode 100644 ROMFS/tap_common/init.d/6001_hexa_x
 create mode 100644 ROMFS/tap_common/init.d/rc.fw_apps
 create mode 100644 ROMFS/tap_common/init.d/rc.fw_defaults
 create mode 100644 ROMFS/tap_common/init.d/rc.interface
 create mode 100644 ROMFS/tap_common/init.d/rc.mc_apps
 create mode 100644 ROMFS/tap_common/init.d/rc.mc_defaults
 create mode 100644 ROMFS/tap_common/init.d/rc.sensors
 create mode 100644 ROMFS/tap_common/init.d/rc.vtol_apps
 create mode 100644 ROMFS/tap_common/init.d/rc.vtol_defaults
 create mode 100644 ROMFS/tap_common/init.d/rcS
 create mode 100644 ROMFS/tap_common/mixers/README.md
 create mode 100644 ROMFS/tap_common/mixers/hexa_x.main.mix
 create mode 100644 ROMFS/tap_common/mixers/quad_x.main.mix
 create mode 100644 cmake/configs/nuttx_tap-v1_default.cmake
 create mode 100644 nuttx-configs/tap-v1/include/board.h
 create mode 100644 nuttx-configs/tap-v1/include/nsh_romfsimg.h
 create mode 100644 nuttx-configs/tap-v1/nsh/Make.defs
 create mode 100644 nuttx-configs/tap-v1/nsh/defconfig
 create mode 100755 nuttx-configs/tap-v1/nsh/setenv.sh
 create mode 100644 nuttx-configs/tap-v1/scripts/ld.script
 create mode 100644 nuttx-configs/tap-v1/src/Makefile
 create mode 100644 nuttx-configs/tap-v1/src/empty.c
 create mode 100644 src/drivers/boards/tap-v1/CMakeLists.txt
 create mode 100644 src/drivers/boards/tap-v1/board_config.h
 create mode 100644 src/drivers/boards/tap-v1/tap_init.c
 create mode 100644 src/drivers/boards/tap-v1/tap_led.c
 create mode 100644 src/drivers/boards/tap-v1/tap_spi.c
 create mode 100644 src/drivers/boards/tap-v1/tap_timer_config.c
 create mode 100644 src/drivers/boards/tap-v1/tap_usb.c

diff --git a/Images/tap-v1.prototype b/Images/tap-v1.prototype
new file mode 100644
index 0000000000..8876ec5e88
--- /dev/null
+++ b/Images/tap-v1.prototype
@@ -0,0 +1,12 @@
+{
+    "board_id": 78,
+    "magic": "PX4FWv1",
+    "description": "Firmware for the TAPv1 board",
+    "image": "",
+    "build_time": 0,
+    "summary": "TAPv1",
+    "version": "0.1",
+    "image_size": 0,
+    "git_identity": "",
+    "board_revision": 0
+}
diff --git a/Makefile b/Makefile
index e97f0c43ce..31978311b2 100644
--- a/Makefile
+++ b/Makefile
@@ -137,6 +137,9 @@ endef
 # --------------------------------------------------------------------
 #  Do not put any spaces between function arguments.
 
+tap-v1_default:
+	$(call cmake-build,nuttx_tap-v1_default)
+
 px4fmu-v1_default:
 	$(call cmake-build,nuttx_px4fmu-v1_default)
 
@@ -249,6 +252,7 @@ checks_defaults: \
 	check_px4fmu-v2_default \
 	check_mindpx-v2_default \
 	check_px4-stm32f4discovery_default \
+	check_tap-v1_default \
 
 checks_bootloaders: \
 
diff --git a/ROMFS/tap_common/init.d/4001_quad_x b/ROMFS/tap_common/init.d/4001_quad_x
new file mode 100644
index 0000000000..8f7c8da356
--- /dev/null
+++ b/ROMFS/tap_common/init.d/4001_quad_x
@@ -0,0 +1,18 @@
+#!nsh
+#
+# @name Generic Quadrotor X config
+#
+# @type Quadrotor x
+#
+# @output AUX1 feed-through of RC AUX1 channel
+# @output AUX2 feed-through of RC AUX2 channel
+# @output AUX3 feed-through of RC AUX3 channel
+#
+# @maintainer Lorenz Meier <lorenz@px4.io>
+#
+
+sh /etc/init.d/rc.mc_defaults
+
+set MIXER quad_x
+
+set PWM_OUT 1234
diff --git a/ROMFS/tap_common/init.d/6001_hexa_x b/ROMFS/tap_common/init.d/6001_hexa_x
new file mode 100644
index 0000000000..584c4a381c
--- /dev/null
+++ b/ROMFS/tap_common/init.d/6001_hexa_x
@@ -0,0 +1,19 @@
+#!nsh
+#
+# @name Generic Hexarotor x geometry
+#
+# @type Hexarotor x
+#
+# @output AUX1 feed-through of RC AUX1 channel
+# @output AUX2 feed-through of RC AUX2 channel
+# @output AUX3 feed-through of RC AUX3 channel
+#
+# @maintainer Anton Babushkin <anton@px4.io>
+#
+
+sh /etc/init.d/rc.mc_defaults
+
+set MIXER hexa_x
+
+# Need to set all 8 channels
+set PWM_OUT 12345678
diff --git a/ROMFS/tap_common/init.d/rc.fw_apps b/ROMFS/tap_common/init.d/rc.fw_apps
new file mode 100644
index 0000000000..040bcbde03
--- /dev/null
+++ b/ROMFS/tap_common/init.d/rc.fw_apps
@@ -0,0 +1,20 @@
+#!nsh
+#
+# Standard apps for fixed wing
+#
+
+#
+# Start the attitude and position estimator
+#
+ekf2 start
+
+#
+# Start attitude controller
+#
+fw_att_control start
+fw_pos_control_l1 start
+
+#
+# Start Land Detector
+#
+land_detector start fixedwing
diff --git a/ROMFS/tap_common/init.d/rc.fw_defaults b/ROMFS/tap_common/init.d/rc.fw_defaults
new file mode 100644
index 0000000000..f876353263
--- /dev/null
+++ b/ROMFS/tap_common/init.d/rc.fw_defaults
@@ -0,0 +1,28 @@
+#!nsh
+
+set VEHICLE_TYPE fw
+
+if [ $AUTOCNF == yes ]
+then
+#
+# Default parameters for FW
+#
+	param set RTL_RETURN_ALT 100
+	param set RTL_DESCEND_ALT 100
+	param set RTL_LAND_DELAY -1
+
+	param set NAV_ACC_RAD 50
+
+	param set PE_VELNE_NOISE 0.3
+	param set PE_VELD_NOISE 0.35
+	param set PE_POSNE_NOISE 0.5
+	param set PE_POSD_NOISE 1.0
+fi
+
+# This is the gimbal pass mixer
+set MIXER_AUX pass
+set PWM_AUX_RATE 50
+set PWM_AUX_OUT 1234
+set PWM_AUX_DISARMED 1500
+set PWM_AUX_MIN 1000
+set PWM_AUX_MAX 2000
diff --git a/ROMFS/tap_common/init.d/rc.interface b/ROMFS/tap_common/init.d/rc.interface
new file mode 100644
index 0000000000..bfa105b537
--- /dev/null
+++ b/ROMFS/tap_common/init.d/rc.interface
@@ -0,0 +1,83 @@
+#!nsh
+#
+# Script to configure control interface
+#
+
+set SDCARD_MIXERS_PATH /fs/microsd/etc/mixers
+
+if [ $MIXER != none -a $MIXER != skip ]
+then
+	#
+	# Load main mixer
+	#
+
+	# Use the mixer file
+	set MIXER_FILE /etc/mixers/$MIXER.main.mix
+
+	set OUTPUT_DEV /dev/pwm_output0
+
+	if [ $OUTPUT_MODE == uavcan_esc ]
+	then
+		set OUTPUT_DEV /dev/uavcan/esc
+	fi
+
+	if mixer load $OUTPUT_DEV $MIXER_FILE
+	then
+		echo "INFO  [init] Mixer: $MIXER_FILE on $OUTPUT_DEV"
+	else
+		echo "ERROR [init] Error loading mixer: $MIXER_FILE"
+		echo "ERROR:[init] Could not load mixer: $MIXER_FILE" >> $LOG_FILE
+		tone_alarm $TUNE_ERR
+	fi
+
+	unset MIXER_FILE
+else
+	if [ $MIXER != skip ]
+	then
+		echo "ERROR [init] Mixer not defined"
+		echo "ERROR [init] Mixer not defined" >> $LOG_FILE
+		tone_alarm $TUNE_ERR
+	fi
+fi
+
+if [ $OUTPUT_MODE == fmu -o $OUTPUT_MODE == io ]
+then
+	if [ $PWM_OUT != none ]
+	then
+		#
+		# Set PWM output frequency
+		#
+		if [ $PWM_RATE != none ]
+		then
+			pwm rate -c $PWM_OUT -r $PWM_RATE
+		fi
+
+		#
+		# Set disarmed, min and max PWM values
+		#
+		if [ $PWM_DISARMED != none ]
+		then
+			pwm disarmed -c $PWM_OUT -p $PWM_DISARMED
+		fi
+		if [ $PWM_MIN != none ]
+		then
+			pwm min -c $PWM_OUT -p $PWM_MIN
+		fi
+		if [ $PWM_MAX != none ]
+		then
+			pwm max -c $PWM_OUT -p $PWM_MAX
+		fi
+	fi
+
+	if [ $FAILSAFE != none ]
+	then
+		pwm failsafe -d $OUTPUT_DEV $FAILSAFE
+	fi
+fi
+
+unset PWM_OUT
+unset PWM_RATE
+unset PWM_MIN
+unset PWM_MAX
+unset FAILSAFE
+unset OUTPUT_DEV
diff --git a/ROMFS/tap_common/init.d/rc.mc_apps b/ROMFS/tap_common/init.d/rc.mc_apps
new file mode 100644
index 0000000000..399f4398ac
--- /dev/null
+++ b/ROMFS/tap_common/init.d/rc.mc_apps
@@ -0,0 +1,39 @@
+#!nsh
+#
+# Standard apps for multirotors:
+# att & pos estimator, att & pos control.
+#
+
+
+#---------------------------------------
+# Estimator group selction
+#
+# INAV
+if param compare SYS_MC_EST_GROUP 0
+then
+	attitude_estimator_q start
+	position_estimator_inav start
+fi
+
+# LPE
+if param compare SYS_MC_EST_GROUP 1
+then
+	attitude_estimator_q start
+	local_position_estimator start
+fi
+
+# EKF
+if param compare SYS_MC_EST_GROUP 2
+then
+	ekf2 start
+fi
+#---------------------------------------
+
+mc_att_control start
+
+mc_pos_control start
+
+#
+# Start Land Detector
+#
+land_detector start multicopter
diff --git a/ROMFS/tap_common/init.d/rc.mc_defaults b/ROMFS/tap_common/init.d/rc.mc_defaults
new file mode 100644
index 0000000000..0bf41dcac6
--- /dev/null
+++ b/ROMFS/tap_common/init.d/rc.mc_defaults
@@ -0,0 +1,49 @@
+#!nsh
+
+set VEHICLE_TYPE mc
+
+if [ $AUTOCNF == yes ]
+then
+	param set PE_VELNE_NOISE 0.5
+	param set PE_VELD_NOISE 0.35
+	param set PE_POSNE_NOISE 0.5
+	param set PE_POSD_NOISE 1.25
+
+	param set NAV_ACC_RAD 2.0
+	param set RTL_RETURN_ALT 30.0
+	param set RTL_DESCEND_ALT 10.0
+	param set PWM_DISARMED 900
+	param set PWM_MIN 1075
+	param set PWM_MAX 1950
+
+	param set RTL_LAND_DELAY 0
+fi
+
+# set environment variables (!= parameters)
+set PWM_RATE 400
+# tell the mixer to use parameters for these instead
+set PWM_DISARMED p:PWM_DISARMED
+set PWM_MIN p:PWM_MIN
+set PWM_MAX p:PWM_MAX
+
+# This is the gimbal pass mixer
+set MIXER_AUX pass
+set PWM_AUX_RATE 50
+set PWM_AUX_OUT 1234
+set PWM_AUX_DISARMED 1500
+set PWM_AUX_MIN 1000
+set PWM_AUX_MAX 2000
+
+# Transitional support: ensure suitable PWM min/max param values
+if param compare PWM_MIN 1000
+then
+	param set PWM_MIN 1075
+fi
+if param compare PWM_MAX 2000
+then
+	param set PWM_MAX 1950
+fi
+if param compare PWM_DISARMED 0
+then
+	param set PWM_DISARMED 900
+fi
diff --git a/ROMFS/tap_common/init.d/rc.sensors b/ROMFS/tap_common/init.d/rc.sensors
new file mode 100644
index 0000000000..fdb835baff
--- /dev/null
+++ b/ROMFS/tap_common/init.d/rc.sensors
@@ -0,0 +1,46 @@
+#!nsh
+#
+# Standard startup script for TAP v1 onboard sensor drivers.
+#
+
+if adc start
+then
+fi
+
+# External I2C bus
+if hmc5883 -C -T -X start
+then
+fi
+
+if lis3mdl -R 2 start
+then
+fi
+
+# Internal SPI bus is rotated 90 deg yaw
+if hmc5883 -C -T -S -R 2 start
+then
+fi
+
+# Internal SPI bus ICM-20608-G is rotated 90 deg yaw
+if mpu6000 -R 2 -T 20608 start
+then
+fi
+
+# Internal SPI bus mpu9250 is rotated 90 deg yaw
+if mpu9250 -R 2 start
+then
+fi
+
+if meas_airspeed start
+then
+fi
+
+if sf10a start
+then
+fi
+
+# Wait 20 ms for sensors (because we need to wait for the HRT and work queue callbacks to fire)
+usleep 20000
+if sensors start
+then
+fi
diff --git a/ROMFS/tap_common/init.d/rc.vtol_apps b/ROMFS/tap_common/init.d/rc.vtol_apps
new file mode 100644
index 0000000000..c3d54fd299
--- /dev/null
+++ b/ROMFS/tap_common/init.d/rc.vtol_apps
@@ -0,0 +1,44 @@
+#!nsh
+#
+# Standard apps for vtol:
+# att & pos estimator, att & pos control.
+#
+#
+
+
+#---------------------------------------
+# Estimator group selction
+#
+# INAV
+if param compare SYS_MC_EST_GROUP 0
+then
+	attitude_estimator_q start
+	position_estimator_inav start
+fi
+
+# LPE
+if param compare SYS_MC_EST_GROUP 1
+then
+	attitude_estimator_q start
+	local_position_estimator start
+fi
+
+# EKF
+if param compare SYS_MC_EST_GROUP 2
+then
+	ekf2 start
+fi
+#---------------------------------------
+
+
+vtol_att_control start
+mc_att_control start
+mc_pos_control start
+fw_att_control start
+fw_pos_control_l1 start
+
+#
+# Start Land Detector
+# Multicopter for now until we have something for VTOL
+#
+land_detector start vtol
diff --git a/ROMFS/tap_common/init.d/rc.vtol_defaults b/ROMFS/tap_common/init.d/rc.vtol_defaults
new file mode 100644
index 0000000000..d8dab16cf0
--- /dev/null
+++ b/ROMFS/tap_common/init.d/rc.vtol_defaults
@@ -0,0 +1,49 @@
+#!nsh
+
+set VEHICLE_TYPE vtol
+
+if [ $AUTOCNF == yes ]
+then
+	param set MC_ROLL_P 6.0
+	param set MC_PITCH_P 6.0
+	param set MC_YAW_P 4
+
+	param set PE_VELNE_NOISE 0.5
+	param set PE_VELD_NOISE 0.3
+	param set PE_POSNE_NOISE 0.5
+	param set PE_POSD_NOISE 1.25
+	param set PE_ABIAS_PNOISE 0.0001
+
+	#
+	# Default parameters for mission and position handling
+	#
+	param set NAV_ACC_RAD 3
+	param set MPC_TKO_SPEED 1.0
+	param set MPC_LAND_SPEED 0.7
+	param set MPC_Z_VEL_MAX 1.5
+	param set MPC_XY_VEL_MAX 4.0
+	param set MIS_YAW_TMT 10
+	param set MPC_ACC_HOR_MAX 2.0
+	param set RTL_LAND_DELAY 0
+fi
+
+# set environment variables (!= parameters)
+set PWM_RATE 400
+# tell the mixer to use parameters for these instead
+set PWM_DISARMED p:PWM_DISARMED
+set PWM_MIN p:PWM_MIN
+set PWM_MAX p:PWM_MAX
+
+# Transitional support: ensure suitable PWM min/max param values
+if param compare PWM_MIN 1000
+then
+	param set PWM_MIN 1075
+fi
+if param compare PWM_MAX 2000
+then
+	param set PWM_MAX 1950
+fi
+if param compare PWM_DISARMED 0
+then
+	param set PWM_DISARMED 900
+fi
diff --git a/ROMFS/tap_common/init.d/rcS b/ROMFS/tap_common/init.d/rcS
new file mode 100644
index 0000000000..643e421bae
--- /dev/null
+++ b/ROMFS/tap_common/init.d/rcS
@@ -0,0 +1,482 @@
+#!nsh
+#
+# PX4FMU startup script.
+#
+#  NOTE: COMMENT LINES ARE REMOVED BEFORE STORED IN ROMFS.
+#
+
+#
+# Start CDC/ACM serial driver
+#
+sercon
+
+#
+# Default to auto-start mode.
+#
+set MODE autostart
+
+set TUNE_ERR ML<<CP4CP4CP4CP4CP4
+set LOG_FILE /fs/microsd/bootlog.txt
+
+#
+# Try to mount the microSD card.
+#
+# REBOOTWORK this needs to start after the flight control loop
+if mount -t vfat /dev/mmcsd0 /fs/microsd
+then
+	# Start playing the startup tune
+	tone_alarm start
+else
+	tone_alarm MBAGP
+	if mkfatfs /dev/mmcsd0
+	then
+		if mount -t vfat /dev/mmcsd0 /fs/microsd
+		then
+			echo "INFO  [init] MicroSD card formatted"
+		else
+			echo "ERROR [init] Format failed"
+			tone_alarm MNBG
+			set LOG_FILE /dev/null
+		fi
+	else
+		set LOG_FILE /dev/null
+	fi
+fi
+
+#
+# Look for an init script on the microSD card.
+# Disable autostart if the script found.
+#
+set FRC /fs/microsd/etc/rc.txt
+if [ -f $FRC ]
+then
+	echo "INFO  [init] Executing script: $FRC"
+	sh $FRC
+	set MODE custom
+fi
+unset FRC
+
+if [ $MODE == autostart ]
+then
+
+	#
+	# Start the ORB (first app to start)
+	#
+	uorb start
+
+	#
+	# Load parameters
+	#
+	set PARAM_FILE /fs/microsd/params
+	if mtd start
+	then
+		set PARAM_FILE /fs/mtd_params
+	fi
+
+	param select $PARAM_FILE
+	if param load
+	then
+	else
+		if param reset
+		then
+		fi
+	fi
+
+	#
+	# Start system state indicator
+	#
+	if rgbled start
+	then
+	fi
+
+	#
+	# Set AUTOCNF flag to use it in AUTOSTART scripts
+	#
+	if param compare SYS_AUTOCONFIG 1
+	then
+		# Wipe out params except RC*
+		param reset_nostart RC*
+		set AUTOCNF yes
+	else
+		set AUTOCNF no
+	fi
+
+	#
+	# Set default values
+	#
+	set VEHICLE_TYPE none
+	set MIXER none
+	set OUTPUT_MODE none
+	set PWM_OUT none
+	set PWM_RATE none
+	set PWM_DISARMED none
+	set PWM_MIN none
+	set PWM_MAX none
+	set FMU_MODE pwm
+	set MAVLINK_F default
+	set EXIT_ON_END no
+	set MAV_TYPE none
+	set FAILSAFE none
+
+	#
+	# Set parameters and env variables for selected AUTOSTART
+	#
+	if param compare SYS_AUTOSTART 0
+	then
+		echo "INFO  [init] No autostart"
+	else
+		sh /etc/init.d/rc.autostart
+	fi
+	unset MODE
+
+	#
+	# If autoconfig parameter was set, reset it and save parameters
+	#
+	if [ $AUTOCNF == yes ]
+	then
+		param set SYS_AUTOCONFIG 0
+		param save
+	fi
+	unset AUTOCNF
+
+	#
+	# Set default output if not set
+	#
+	if [ $OUTPUT_MODE == none ]
+	then
+		if [ $USE_IO == yes ]
+		then
+			set OUTPUT_MODE io
+		else
+			set OUTPUT_MODE fmu
+		fi
+	fi
+
+	# waypoint storage
+	# REBOOTWORK this needs to start in parallel
+	if dataman start
+	then
+	fi
+
+	#
+	# Sensors System (start before Commander so Preflight checks are properly run)
+	#
+	sh /etc/init.d/rc.sensors
+
+	commander start
+
+	#
+	# Start CPU load monitor
+	#
+	load_mon start
+
+	#
+	# Start primary output
+	#
+	set TTYS1_BUSY no
+
+	#
+	# Check if UAVCAN is enabled, default to it for ESCs
+	#
+	if param greater UAVCAN_ENABLE 2
+	then
+		set OUTPUT_MODE uavcan_esc
+	fi
+
+	# If OUTPUT_MODE == none then something is wrong with setup and we shouldn't try to enable output
+	if [ $OUTPUT_MODE != none ]
+	then
+		if [ $OUTPUT_MODE == uavcan_esc ]
+		then
+			if param compare UAVCAN_ENABLE 0
+			then
+				echo "INFO  [init] OVERRIDING UAVCAN_ENABLE = 1" >> $LOG_FILE
+				param set UAVCAN_ENABLE 1
+			fi
+		fi
+
+		if [ $OUTPUT_MODE == fmu ]
+		then
+			if fmu mode_$FMU_MODE
+			then
+			else
+				echo "ERR  [init] FMU start failed" >> $LOG_FILE
+				tone_alarm $TUNE_ERR
+			fi
+		fi
+
+		if fmu mode_pwm4
+		then
+		else
+			echo "ERROR [init] FMU mode_$FMU_MODE start failed" >> $LOG_FILE
+			tone_alarm $TUNE_ERR
+		fi
+	fi
+
+	mavlink start -r 1200 -d /dev/ttyS1
+
+	#
+	# Starting stuff according to UAVCAN_ENABLE value
+	#
+	if param greater UAVCAN_ENABLE 0
+	then
+		if uavcan start
+		then
+		else
+			tone_alarm $TUNE_ERR
+		fi
+	fi
+
+	if param greater UAVCAN_ENABLE 1
+	then
+		if uavcan start fw
+		then
+		else
+			tone_alarm $TUNE_ERR
+		fi
+	fi
+
+	#
+	# Optional drivers
+	#
+
+	# Sensors on the PWM interface bank
+	if param compare SENS_EN_LL40LS 1
+	then
+		if pwm_input start
+		then
+			if ll40ls start pwm
+			then
+			fi
+		fi
+	fi
+
+	# sf0x lidar sensor
+	if param compare SENS_EN_SF0X 1
+	then
+		sf0x start
+	fi
+
+	# Start USB shell if no microSD present, MAVLink else
+	if [ $LOG_FILE == /dev/null ]
+	then
+		# Try to get an USB console
+		nshterm /dev/ttyACM0 &
+	else
+		mavlink start -r 800000 -d /dev/ttyACM0 -m config -x
+	fi
+
+	#
+	# Logging
+	#
+	if param compare SYS_LOGGER 0
+	then
+		if sdlog2 start -r 100 -a -b 9 -t
+		then
+		fi
+	else
+		if logger start -b 12
+		then
+		fi
+	fi
+
+	#
+	# Fixed wing setup
+	#
+	if [ $VEHICLE_TYPE == fw ]
+	then
+		echo "INFO  [init] Fixedwing"
+
+		if [ $MIXER == none ]
+		then
+			# Set default mixer for fixed wing if not defined
+			set MIXER AERT
+		fi
+
+		if [ $MAV_TYPE == none ]
+		then
+			# Use MAV_TYPE = 1 (fixed wing) if not defined
+			set MAV_TYPE 1
+		fi
+
+		param set MAV_TYPE $MAV_TYPE
+
+		# Load mixer and configure outputs
+		sh /etc/init.d/rc.interface
+
+		# Start standard fixedwing apps
+		sh /etc/init.d/rc.fw_apps
+	fi
+
+	#
+	# Multicopters setup
+	#
+	if [ $VEHICLE_TYPE == mc ]
+	then
+		echo "INFO  [init] Multicopter"
+
+		if [ $MIXER == none ]
+		then
+			echo "INFO  [init] Mixer undefined"
+		fi
+
+		if [ $MAV_TYPE == none ]
+		then
+			# Use mixer to detect vehicle type
+			if [ $MIXER == quad_x -o $MIXER == quad_+ ]
+			then
+				set MAV_TYPE 2
+			fi
+			if [ $MIXER == quad_w ]
+			then
+				set MAV_TYPE 2
+			fi
+			if [ $MIXER == quad_h ]
+			then
+				set MAV_TYPE 2
+			fi
+			if [ $MIXER == tri_y_yaw- -o $MIXER == tri_y_yaw+ ]
+			then
+				set MAV_TYPE 15
+			fi
+			if [ $MIXER == hexa_x -o $MIXER == hexa_+ ]
+			then
+				set MAV_TYPE 13
+			fi
+			if [ $MIXER == hexa_cox ]
+			then
+				set MAV_TYPE 13
+			fi
+			if [ $MIXER == octo_x -o $MIXER == octo_+ ]
+			then
+				set MAV_TYPE 14
+			fi
+		fi
+
+		# Still no MAV_TYPE found
+		if [ $MAV_TYPE == none ]
+		then
+			echo "WARN  [init] Unknown MAV_TYPE"
+			param set MAV_TYPE 2
+		else
+			param set MAV_TYPE $MAV_TYPE
+		fi
+
+		# Load mixer and configure outputs
+		sh /etc/init.d/rc.interface
+
+		# Start standard multicopter apps
+		sh /etc/init.d/rc.mc_apps
+	fi
+
+	#
+	# VTOL setup
+	#
+	if [ $VEHICLE_TYPE == vtol ]
+	then
+		echo "INFO  [init] VTOL"
+
+		if [ $MIXER == none ]
+		then
+			echo "WARN  [init] VTOL mixer undefined"
+		fi
+
+		if [ $MAV_TYPE == none ]
+		then
+			# Use mixer to detect vehicle type
+			if [ $MIXER == caipirinha_vtol ]
+			then
+				set MAV_TYPE 19
+			fi
+			if [ $MIXER == firefly6 ]
+			then
+				set MAV_TYPE 21
+			fi
+			if [ $MIXER == quad_x_pusher_vtol ]
+			then
+				set MAV_TYPE 22
+			fi
+		fi
+
+		# Still no MAV_TYPE found
+		if [ $MAV_TYPE == none ]
+		then
+			echo "WARN  [init] Unknown MAV_TYPE"
+			param set MAV_TYPE 19
+		else
+			param set MAV_TYPE $MAV_TYPE
+		fi
+
+		# Load mixer and configure outputs
+		sh /etc/init.d/rc.interface
+
+		# Start standard vtol apps
+		sh /etc/init.d/rc.vtol_apps
+	fi
+
+	#
+	# Rover setup
+	#
+	if [ $VEHICLE_TYPE == rover ]
+	then
+		# 10 is MAV_TYPE_GROUND_ROVER
+		set MAV_TYPE 10
+
+		# Load mixer and configure outputs
+		sh /etc/init.d/rc.interface
+
+		# Start standard rover apps
+		sh /etc/init.d/rc.axialracing_ax10_apps
+
+		param set MAV_TYPE 10
+	fi
+
+	unset MIXER
+	unset MAV_TYPE
+	unset OUTPUT_MODE
+
+	#
+	# Start the navigator
+	#
+	navigator start
+
+	#
+	# Generic setup (autostart ID not found)
+	#
+	if [ $VEHICLE_TYPE == none ]
+	then
+		echo "WARN  [init] No autostart ID found"
+	fi
+
+	# Start any custom addons
+	set FEXTRAS /fs/microsd/etc/extras.txt
+	if [ -f $FEXTRAS ]
+	then
+		echo "INFO  [init] Addons script: $FEXTRAS"
+		sh $FEXTRAS
+	fi
+	unset FEXTRAS
+
+	# Run no SD alarm
+	if [ $LOG_FILE == /dev/null ]
+	then
+		# Play SOS
+		tone_alarm error
+	fi
+
+# End of autostart
+fi
+
+# There is no further script processing, so we can free some RAM
+# XXX potentially unset all script variables.
+unset TUNE_ERR
+
+# Boot is complete, inform MAVLink app(s) that the system is now fully up and running
+mavlink boot_complete
+
+if [ $EXIT_ON_END == yes ]
+then
+	echo "INFO  [init] NSH exit"
+	exit
+fi
+unset EXIT_ON_END
diff --git a/ROMFS/tap_common/mixers/README.md b/ROMFS/tap_common/mixers/README.md
new file mode 100644
index 0000000000..0e591ca288
--- /dev/null
+++ b/ROMFS/tap_common/mixers/README.md
@@ -0,0 +1,105 @@
+## PX4 mixer definitions ##
+
+Files in this directory implement example mixers that can be used as a basis
+for customisation, or for general testing purposes.
+
+For a detailed description of the mixing architecture and examples see:
+http://px4.io/dev/mixing
+
+### Syntax ###
+
+Mixer definitions are text files; lines beginning with a single capital letter
+followed by a colon are significant. All other lines are ignored, meaning that
+explanatory text can be freely mixed with the definitions.
+
+Each file may define more than one mixer; the allocation of mixers to actuators
+is specific to the device reading the mixer definition, and the number of
+actuator outputs generated by a mixer is specific to the mixer.
+
+For example: each simple or null mixer is assigned to outputs 1 to x
+in the order they appear in the mixer file.
+
+A mixer begins with a line of the form
+
+	<tag>: <mixer arguments>
+
+The tag selects the mixer type; 'M' for a simple summing mixer, 'R' for a 
+multirotor mixer, etc.
+
+#### Null Mixer ####
+
+A null mixer consumes no controls and generates a single actuator output whose
+value is always zero.  Typically a null mixer is used as a placeholder in a
+collection of mixers in order to achieve a specific pattern of actuator outputs.
+
+The null mixer definition has the form:
+
+	Z:
+
+#### Simple Mixer ####
+
+A simple mixer combines zero or more control inputs into a single actuator
+output.  Inputs are scaled, and the mixing function sums the result before
+applying an output scaler.
+
+A simple mixer definition begins with:
+
+	M: <control count>
+	O: <-ve scale> <+ve scale> <offset> <lower limit> <upper limit>
+
+If <control count> is zero, the sum is effectively zero and the mixer will
+output a fixed value that is <offset> constrained by <lower limit> and <upper
+limit>.
+
+The second line defines the output scaler with scaler parameters as discussed
+above. Whilst the calculations are performed as floating-point operations, the
+values stored in the definition file are scaled by a factor of 10000; i.e. an
+offset of -0.5 is encoded as -5000.
+
+The definition continues with <control count> entries describing the control
+inputs and their scaling, in the form:
+
+	S: <group> <index> <-ve scale> <+ve scale> <offset> <lower limit> <upper limit>
+
+The <group> value identifies the control group from which the scaler will read,
+and the <index> value an offset within that group.  These values are specific to
+the device reading the mixer definition.
+
+When used to mix vehicle controls, mixer group zero is the vehicle attitude
+control group, and index values zero through three are normally roll, pitch,
+yaw and thrust respectively.
+
+The remaining fields on the line configure the control scaler with parameters as
+discussed above. Whilst the calculations are performed as floating-point
+operations, the values stored in the definition file are scaled by a factor of
+10000; i.e. an offset of -0.5 is encoded as -5000.
+
+#### Multirotor Mixer ####
+
+The multirotor mixer combines four control inputs (roll, pitch, yaw, thrust)
+into a set of actuator outputs intended to drive motor speed controllers.
+
+The mixer definition is a single line of the form:
+
+	R: <geometry> <roll scale> <pitch scale> <yaw scale> <deadband>
+
+The supported geometries include:
+
+ * 4x - quadrotor in X configuration
+ * 4+ - quadrotor in + configuration
+ * 6x - hexcopter in X configuration
+ * 6+ - hexcopter in + configuration
+ * 8x - octocopter in X configuration
+ * 8+ - octocopter in + configuration
+  
+Each of the roll, pitch and yaw scale values determine scaling of the roll,
+pitch and yaw controls relative to the thrust control.  Whilst the calculations
+are performed as floating-point operations, the values stored in the definition
+file are scaled by a factor of 10000; i.e. an factor of 0.5 is encoded as 5000.
+
+Roll, pitch and yaw inputs are expected to range from -1.0 to 1.0, whilst the
+thrust input ranges from 0.0 to 1.0.  Output for each actuator is in the 
+range -1.0 to 1.0.
+
+In the case where an actuator saturates, all actuator values are rescaled so that 
+the saturating actuator is limited to 1.0.
diff --git a/ROMFS/tap_common/mixers/hexa_x.main.mix b/ROMFS/tap_common/mixers/hexa_x.main.mix
new file mode 100644
index 0000000000..16e6e22f90
--- /dev/null
+++ b/ROMFS/tap_common/mixers/hexa_x.main.mix
@@ -0,0 +1,3 @@
+# Hexa X
+
+R: 6x 10000 10000 10000 0
diff --git a/ROMFS/tap_common/mixers/quad_x.main.mix b/ROMFS/tap_common/mixers/quad_x.main.mix
new file mode 100644
index 0000000000..bf034e7c1f
--- /dev/null
+++ b/ROMFS/tap_common/mixers/quad_x.main.mix
@@ -0,0 +1,7 @@
+R: 4x 10000 10000 10000 0
+M: 1
+O:      10000  10000      0 -10000  10000
+S: 3 5  10000  10000      0 -10000  10000
+M: 1
+O:      10000  10000      0 -10000  10000
+S: 3 6  10000  10000      0 -10000  10000
diff --git a/cmake/configs/nuttx_tap-v1_default.cmake b/cmake/configs/nuttx_tap-v1_default.cmake
new file mode 100644
index 0000000000..d4753fdaca
--- /dev/null
+++ b/cmake/configs/nuttx_tap-v1_default.cmake
@@ -0,0 +1,125 @@
+include(nuttx/px4_impl_nuttx)
+
+set(CMAKE_TOOLCHAIN_FILE ${CMAKE_SOURCE_DIR}/cmake/toolchains/Toolchain-arm-none-eabi.cmake)
+
+set(config_module_list
+	#
+	# Board support modules
+	#
+	drivers/device
+	drivers/stm32
+	drivers/stm32/adc
+	drivers/stm32/tone_alarm
+	drivers/led
+	drivers/px4fmu
+	drivers/boards/tap-v1
+	drivers/rgbled_pwm
+	#drivers/mpu6500
+	drivers/ms5611
+	drivers/gps
+	drivers/airspeed
+	drivers/meas_airspeed
+	modules/sensors
+	drivers/camera_trigger
+
+	#
+	# System commands
+	#
+	systemcmds/bl_update
+	systemcmds/mixer
+	systemcmds/param
+	systemcmds/perf
+	systemcmds/pwm
+	systemcmds/reboot
+	systemcmds/top
+	systemcmds/config
+	systemcmds/nshterm
+	systemcmds/mtd
+	systemcmds/dumpfile
+	systemcmds/ver
+
+	#
+	# General system control
+	#
+	modules/commander
+	modules/load_mon
+	modules/navigator
+	modules/mavlink
+	modules/land_detector
+
+	#
+	# Estimation modules (EKF/ SO3 / other filters)
+	#
+	modules/ekf2
+
+	#
+	# Vehicle Control
+	#
+	modules/fw_pos_control_l1
+	modules/fw_att_control
+	modules/mc_att_control
+	modules/mc_pos_control
+	modules/vtol_att_control
+
+	#
+	# Logging
+	#
+	modules/logger
+
+	#
+	# Library modules
+	#
+	modules/param
+	modules/systemlib
+	modules/systemlib/mixer
+	modules/uORB
+	modules/dataman
+
+	#
+	# Libraries
+	#
+	lib/controllib
+	lib/mathlib
+	lib/mathlib/math/filter
+	lib/ecl
+	lib/external_lgpl
+	lib/geo
+	lib/geo_lookup
+	lib/conversion
+	lib/launchdetection
+	lib/terrain_estimation
+	lib/runway_takeoff
+	lib/tailsitter_recovery
+	lib/DriverFramework/framework
+	platforms/nuttx
+
+	# had to add for cmake, not sure why wasn't in original config
+	platforms/common
+	platforms/nuttx/px4_layer
+)
+
+set(config_extra_builtin_cmds
+	serdis
+	sercon
+	)
+
+set(config_io_board
+	)
+
+set(config_extra_libs
+	)
+
+set(config_io_extra_libs
+	)
+
+add_custom_target(sercon)
+set_target_properties(sercon PROPERTIES
+	PRIORITY "SCHED_PRIORITY_DEFAULT"
+	MAIN "sercon"
+	STACK_MAIN "2048")
+
+add_custom_target(serdis)
+set_target_properties(serdis PROPERTIES
+	PRIORITY "SCHED_PRIORITY_DEFAULT"
+	MAIN "serdis"
+	STACK_MAIN "2048")
diff --git a/cmake/nuttx/px4_impl_nuttx.cmake b/cmake/nuttx/px4_impl_nuttx.cmake
index 8ceddfe44a..c969263be1 100644
--- a/cmake/nuttx/px4_impl_nuttx.cmake
+++ b/cmake/nuttx/px4_impl_nuttx.cmake
@@ -450,55 +450,8 @@ function(px4_os_add_flags)
 	endif()
 
 	set(cpu_flags)
-	if (${BOARD} STREQUAL "px4fmu-v1")
-		set(cpu_flags
-			-mcpu=cortex-m4
-			-mthumb
-			-march=armv7e-m
-			-mfpu=fpv4-sp-d16
-			-mfloat-abi=hard
-			)
-	elseif (${BOARD} STREQUAL "px4fmu-v2")
-		set(cpu_flags
-			-mcpu=cortex-m4
-			-mthumb
-			-march=armv7e-m
-			-mfpu=fpv4-sp-d16
-			-mfloat-abi=hard
-			)
-	elseif (${BOARD} STREQUAL "px4fmu-v4")
-		set(cpu_flags
-			-mcpu=cortex-m4
-			-mthumb
-			-march=armv7e-m
-			-mfpu=fpv4-sp-d16
-			-mfloat-abi=hard
-			)
-	elseif (${BOARD} STREQUAL "px4-stm32f4discovery")
-		set(cpu_flags
-			-mcpu=cortex-m4
-			-mthumb
-			-march=armv7e-m
-			-mfpu=fpv4-sp-d16
-			-mfloat-abi=hard
-			)
-	elseif (${BOARD} STREQUAL "aerocore")
-		set(cpu_flags
-			-mcpu=cortex-m4
-			-mthumb
-			-march=armv7e-m
-			-mfpu=fpv4-sp-d16
-			-mfloat-abi=hard
-			)
-	elseif (${BOARD} STREQUAL "mindpx-v2")
-			set(cpu_flags
-			-mcpu=cortex-m4
-			-mthumb
-			-march=armv7e-m
-			-mfpu=fpv4-sp-d16
-			-mfloat-abi=hard
-			)
-	elseif (${BOARD} STREQUAL "px4io-v1")
+	# Handle non-F4 boards specifically here
+	if (${BOARD} STREQUAL "px4io-v1")
 		set(cpu_flags
 			-mcpu=cortex-m3
 			-mthumb
@@ -510,6 +463,14 @@ function(px4_os_add_flags)
 			-mthumb
 			-march=armv7-m
 			)
+	else ()
+			set(cpu_flags
+			-mcpu=cortex-m4
+			-mthumb
+			-march=armv7e-m
+			-mfpu=fpv4-sp-d16
+			-mfloat-abi=hard
+			)
 	endif()
 	list(APPEND c_flags ${cpu_flags})
 	list(APPEND cxx_flags ${cpu_flags})
diff --git a/nuttx-configs/tap-v1/include/board.h b/nuttx-configs/tap-v1/include/board.h
new file mode 100644
index 0000000000..8df4099124
--- /dev/null
+++ b/nuttx-configs/tap-v1/include/board.h
@@ -0,0 +1,448 @@
+/************************************************************************************
+ * configs/tap-v1/include/board.h
+ * include/arch/board/board.h
+ *
+ *   Copyright (C) 2012-2016 Gregory Nutt. All rights reserved.
+ *   Author: Gregory Nutt <gnutt@nuttx.org>
+ *           David Sidrane <david_s5@nscdg.com>
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name NuttX nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ************************************************************************************/
+
+#ifndef __CONFIG_TAP_V1_INCLUDE_BOARD_H
+#define __CONFIG_TAP_V1_INCLUDE_BOARD_H
+
+/************************************************************************************
+ * Included Files
+ ************************************************************************************/
+
+#include <nuttx/config.h>
+
+#ifndef __ASSEMBLY__
+# include <stdint.h>
+#endif
+
+#include "stm32_rcc.h"
+#include "stm32_sdio.h"
+#include "stm32.h"
+
+/************************************************************************************
+ * Definitions
+ ************************************************************************************/
+
+/* Clocking *************************************************************************/
+/* The TAP V1 uses a 16MHz crystal connected to the HSE.
+ *
+ * This is the canonical configuration:
+ *   System Clock source           : PLL (HSE)
+ *   SYSCLK(Hz)                    : 168000000    Determined by PLL configuration
+ *   HCLK(Hz)                      : 168000000    (STM32_RCC_CFGR_HPRE)
+ *   AHB Prescaler                 : 1            (STM32_RCC_CFGR_HPRE)
+ *   APB1 Prescaler                : 4            (STM32_RCC_CFGR_PPRE1)
+ *   APB2 Prescaler                : 2            (STM32_RCC_CFGR_PPRE2)
+ *   HSE Frequency(Hz)             : 16000000     (STM32_BOARD_XTAL)
+ *   PLLM                          : 8            (STM32_PLLCFG_PLLM)
+ *   PLLN                          : 168          (STM32_PLLCFG_PLLN)
+ *   PLLP                          : 2            (STM32_PLLCFG_PLLP)
+ *   PLLQ                          : 7            (STM32_PLLCFG_PLLQ)
+ *   Main regulator output voltage : Scale1 mode  Needed for high speed SYSCLK
+ *   Flash Latency(WS)             : 5
+ *   Prefetch Buffer               : OFF
+ *   Instruction cache             : ON
+ *   Data cache                    : ON
+ *   Require 48MHz for USB OTG FS, : Enabled
+ *   SDIO and RNG clock
+ */
+
+/* HSI - 16 MHz RC factory-trimmed
+ * LSI - 32 KHz RC
+ * HSE - On-board crystal frequency is 16MHz
+ * LSE - not installed
+ */
+
+#define STM32_BOARD_XTAL        16000000ul
+
+#define STM32_HSI_FREQUENCY     16000000ul
+#define STM32_LSI_FREQUENCY     32000
+#define STM32_HSE_FREQUENCY     STM32_BOARD_XTAL
+
+/* Main PLL Configuration.
+ *
+ * PLL source is HSE
+ * PLL_VCO = (STM32_HSE_FREQUENCY / PLLM) * PLLN
+ *         = (16,000,000 / 8) * 168
+ *         = 336,000,000
+ * SYSCLK  = PLL_VCO / PLLP
+ *         = 336,000,000 / 2 = 168,000,000
+ * USB OTG FS, SDIO and RNG Clock
+ *         =  PLL_VCO / PLLQ
+ *         =  336,000,000 / 7
+ *         = 48,000,000
+ */
+
+#define STM32_PLLCFG_PLLM       RCC_PLLCFG_PLLM(8)
+#define STM32_PLLCFG_PLLN       RCC_PLLCFG_PLLN(168)
+#define STM32_PLLCFG_PLLP       RCC_PLLCFG_PLLP_2
+#define STM32_PLLCFG_PLLQ       RCC_PLLCFG_PLLQ(7)
+
+#define STM32_SYSCLK_FREQUENCY  168000000ul
+
+/* AHB clock (HCLK) is SYSCLK (168MHz) */
+
+#define STM32_RCC_CFGR_HPRE     RCC_CFGR_HPRE_SYSCLK  /* HCLK  = SYSCLK / 1 */
+#define STM32_HCLK_FREQUENCY    STM32_SYSCLK_FREQUENCY
+#define STM32_BOARD_HCLK        STM32_HCLK_FREQUENCY  /* same as above, to satisfy compiler */
+
+/* APB1 clock (PCLK1) is HCLK/4 (42MHz) */
+
+#define STM32_RCC_CFGR_PPRE1    RCC_CFGR_PPRE1_HCLKd4     /* PCLK1 = HCLK / 4 */
+#define STM32_PCLK1_FREQUENCY   (STM32_HCLK_FREQUENCY/4)
+
+/* Timers driven from APB1 will be twice PCLK1 */
+
+#define STM32_APB1_TIM2_CLKIN   (2*STM32_PCLK1_FREQUENCY)
+#define STM32_APB1_TIM3_CLKIN   (2*STM32_PCLK1_FREQUENCY)
+#define STM32_APB1_TIM4_CLKIN   (2*STM32_PCLK1_FREQUENCY)
+#define STM32_APB1_TIM5_CLKIN   (2*STM32_PCLK1_FREQUENCY)
+#define STM32_APB1_TIM6_CLKIN   (2*STM32_PCLK1_FREQUENCY)
+#define STM32_APB1_TIM7_CLKIN   (2*STM32_PCLK1_FREQUENCY)
+#define STM32_APB1_TIM12_CLKIN  (2*STM32_PCLK1_FREQUENCY)
+#define STM32_APB1_TIM13_CLKIN  (2*STM32_PCLK1_FREQUENCY)
+#define STM32_APB1_TIM14_CLKIN  (2*STM32_PCLK1_FREQUENCY)
+
+/* APB2 clock (PCLK2) is HCLK/2 (84MHz) */
+
+#define STM32_RCC_CFGR_PPRE2    RCC_CFGR_PPRE2_HCLKd2     /* PCLK2 = HCLK / 2 */
+#define STM32_PCLK2_FREQUENCY   (STM32_HCLK_FREQUENCY/2)
+
+/* Timers driven from APB2 will be twice PCLK2 */
+
+#define STM32_APB2_TIM1_CLKIN   (2*STM32_PCLK2_FREQUENCY)
+#define STM32_APB2_TIM8_CLKIN   (2*STM32_PCLK2_FREQUENCY)
+#define STM32_APB2_TIM9_CLKIN   (2*STM32_PCLK2_FREQUENCY)
+#define STM32_APB2_TIM10_CLKIN  (2*STM32_PCLK2_FREQUENCY)
+#define STM32_APB2_TIM11_CLKIN  (2*STM32_PCLK2_FREQUENCY)
+
+/* Timer Frequencies, if APBx is set to 1, frequency is same to APBx
+ * otherwise frequency is 2xAPBx.
+ * Note: TIM1,8 are on APB2, others on APB1
+ */
+
+#define STM32_TIM18_FREQUENCY   (2*STM32_PCLK2_FREQUENCY)
+#define STM32_TIM27_FREQUENCY   (2*STM32_PCLK1_FREQUENCY)
+
+/* LED definitions ******************************************************************/
+/* If CONFIG_ARCH_LEDS is not defined, then the user can control the LEDs in any
+ * way.  The following definitions are used to access individual LEDs.
+ */
+
+/* LED index values for use with stm32_setled()
+ *
+ * PC4     BLUE_LED                  D4 Blue LED cathode
+ * PC5     RED_LED                   D5 Red LED cathode
+*/
+#define BOARD_RED         0
+#define BOARD_LED2        1
+#define BOARD_NLEDS       2
+
+#define BOARD_LED_BLUE    BOARD_LED1
+#define BOARD_LED_RED     BOARD_LED2
+
+/* LED bits for use with stm32_setleds() */
+
+#define BOARD_LED1_BIT    (1 << BOARD_LED1)
+#define BOARD_LED2_BIT    (1 << BOARD_LED2)
+
+/* If CONFIG_ARCH_LEDs is defined, then NuttX will control the 2 LEDs on board
+ * the tap-v1.  The following definitions describe how NuttX controls
+ * the LEDs:
+ */
+
+#define LED_STARTED       0  /* BLUE */
+#define LED_HEAPALLOCATE  1  /* LED2 */
+#define LED_IRQSENABLED   2  /* BLUE */
+#define LED_STACKCREATED  3  /* BLUE + RED  */
+#define LED_INIRQ         4  /* BLUE */
+#define LED_SIGNAL        5  /* RED  */
+#define LED_ASSERTION     6  /* BLUE + RED  */
+#define LED_PANIC         7  /* BLUE + RED  */
+
+/* Alternate function pin selections ************************************************/
+
+/*
+ * USARTs.
+ *
+ *
+ * Peripheral   Port     Signal Name               CONN
+ * USART1_TX    PB6     GPS_USART1_TX            JP1-15,16
+ * USART1_RX    PB7     GPS_USART1_RX            JP1-13,14
+ * USART2_TX    PA2     GB_USART2_TX             JP2-19,20
+ * USART2_RX    PA3     GB_USART2_RX             JP2-21,22
+ * USART3_TX    PC10    RF2_USART3_TX            J3-2
+ * USART3_RX    PC11    RF2_USART3_RX            J3-1
+ * USART6_TX    PC6     RF_USART6_TX             JP2-15,16
+ * USART6_RX    PC7     RF_USART6_RX             JP2-17,18
+*/
+
+#define GPIO_USART1_TX	GPIO_USART1_TX_2
+#define GPIO_USART1_RX	GPIO_USART1_RX_2
+
+#define GPIO_USART2_TX	GPIO_USART2_TX_1
+#define GPIO_USART2_RX	GPIO_USART2_RX_1
+
+#define GPIO_USART3_TX	GPIO_USART3_TX_2
+#define GPIO_USART3_RX	GPIO_USART3_RX_2
+
+#define GPIO_USART6_TX	GPIO_USART6_TX_1
+#define GPIO_USART6_RX	GPIO_USART6_RX_1
+
+/* USART DMA configuration for USART 1 and 6 */
+
+#define DMAMAP_USART1_RX DMAMAP_USART1_RX_2
+#define DMAMAP_USART6_RX DMAMAP_USART6_RX_2
+
+/*
+ * UARTs.
+ *
+ *  N.B. The 's' is here to match the wrong labeling on Schematic
+ *
+ * Peripheral   Port     Signal Name               CONN
+ * UART4_TX     PA0     OFS_UsART4_TX             JP1-19,20
+ * UART4_RX     PA1     OFS_UsART4_RX             JP1-17,18
+ * UART5_TX     PC12    ESC_UsART5_TX             U7-HCT244 etal ESC
+ * UART5_RX     PD2     ESC_UsART5_RX             U8-5 74HCT151
+ *
+ * Note that UART5 has no optional pinout, so it is not listed here.
+ *
+*/
+
+#define GPIO_UART4_TX	GPIO_UART4_TX_1
+#define GPIO_UART4_RX	GPIO_UART4_RX_1
+
+/*
+ * I2C
+ *
+ * Peripheral   Port     Signal Name               CONN
+ * I2C1_SDA     PB9     I2C1_SDA                  J2-4,9,16,21 mpu6050, U4 MS6507
+ * I2C1_SDL     PB8     I2C1_SCL                  J2-3,10,15,22 mpu6050, U4 MS6507
+ * I2C2_SDA     PB11    Sonar Echo/I2C_SDA        JP2-31,32
+ * I2C2_SDL     PB10    Sonar Trig/I2C_SCL        JP2-29,30
+ * I2C3_SDA     PC9     COMPASS_I2C3_SDA          JP1-27,28
+ * I2C3_SDL     PA8     COMPASS_I2C3_SCL          JP1-25,26
+ *
+ * The optional _GPIO configurations allow the I2C driver to manually
+ * reset the bus to clear stuck slaves.  They match the pin configuration,
+ * but are normally-high GPIOs.
+ */
+#define GPIO_I2C1_SDA		GPIO_I2C1_SDA_2
+#define GPIO_I2C1_SCL		GPIO_I2C1_SCL_2
+#define GPIO_I2C1_SDA_GPIO	(GPIO_OUTPUT|GPIO_OPENDRAIN|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN9)
+#define GPIO_I2C1_SCL_GPIO	(GPIO_OUTPUT|GPIO_OPENDRAIN|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN8)
+
+#define GPIO_I2C2_SDA		GPIO_I2C2_SDA_1
+#define GPIO_I2C2_SCL		GPIO_I2C2_SCL_1
+#define GPIO_I2C2_SDA_GPIO	(GPIO_OUTPUT|GPIO_OPENDRAIN|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN11)
+#define GPIO_I2C2_SCL_GPIO	(GPIO_OUTPUT|GPIO_OPENDRAIN|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN10)
+
+#define GPIO_I2C3_SDA		GPIO_I2C3_SDA_1
+#define GPIO_I2C3_SCL		GPIO_I2C3_SCL_1
+#define GPIO_I2C3_SDA_GPIO	(GPIO_OUTPUT|GPIO_OPENDRAIN|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTC|GPIO_PIN9)
+#define GPIO_I2C3_SCL_GPIO	(GPIO_OUTPUT|GPIO_OPENDRAIN|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTA|GPIO_PIN8)
+
+/*
+ * SPI
+ *
+ * Peripheral   Port     Signal Name               CONN
+ * SPI2_NSS       PB12    SD_SPI2_NSS               SD-2 CS
+ * SPI2_SCK       PB13    SD_SPI2_SCK               SD-5 CLK
+ * SPI2_MISO      PB14    SD_SPI2_MISO              SD-7 D0
+ * SPI2_MOSI      PB15    SD_SPI2_MOSI              SD-3 DI
+ *
+ */
+
+#define GPIO_SPI2_NSS	(GPIO_SPI2_NSS_1  | GPIO_SPEED_50MHz)
+#define GPIO_SPI2_SCK	(GPIO_SPI2_SCK_2  | GPIO_SPEED_50MHz)
+#define GPIO_SPI2_MISO	(GPIO_SPI2_MISO_1 | GPIO_SPEED_50MHz)
+#define GPIO_SPI2_MOSI	(GPIO_SPI2_MOSI_1 | GPIO_SPEED_50MHz)
+
+/* SPI DMA configuration for SPI2 (microSD) */
+#define DMACHAN_SPI2_RX DMAMAP_SPI2_RX
+#define DMACHAN_SPI2_TX DMAMAP_SPI2_TX
+
+/* The following Pin Mapping is just for completeness */
+/*
+ * JTAG
+ *
+ * We will only enable SW-DP, JTAG-DP will be disabled
+ *
+ * Function     Port     Signal Name               CONN
+ * SWDIO        PA13    DAT                       J10-3,J7
+ * SWCLK        PA14    CLK                       J10-4,J8
+ *
+ */
+
+/*
+ *  BOOT
+ *
+ * Function     Port     Signal Name               CONN
+ * BOOT0        NA       BOOT0              GND via 10 K
+ * BOOT1        PB2      BOOT1              V3.3 - 10 K
+ *
+ *   As jumpered the device can only boot from FLASH.
+ *
+ *   It can be booted to:
+ *
+ * 	   SRAM if BOOT0 is pulled High with 1K.
+ *     System memory if:
+ *       BOOT0 is pulled High with 1K and
+ *       BOOT1 is pulled Low with  1K
+*/
+
+/*
+ * Timer PWM
+ *
+ * Peripheral   Port     Signal Name               CONN
+ * TIM3_CH1     PA6     LED_R                     JP2-23,24
+ * TIM3_CH2     PA7     LED_G                     JP2-25,26
+ * TIM3_CH3     PB0     LED_B                     JP2-27,28
+ * TIM3_CH4     PB1     nPWM_1 AUX1(Landing Gear) JP1-21,22
+ */
+
+/*
+ * GPIO
+ * todo:Revisit - cannnot tell from schematic - some could be ADC
+ * Port     Signal Name               CONN
+ * PA4     POWER                     JP1-23,24
+ * PB4     TEMP_CONT                 J2-2,11,14,23 MPU6050 TEMP
+ * PC0     VOLTAGE                   JP2-13,14
+ * PC1     KEY_AD                    JP1-31,32
+ * PC2     SD_SW                     SD-9 SW
+ * PC3     PCON_RADIO                JP1-29,30
+ * PC13    S2                        U8-9 74HCT151
+ * PC14    S1                        U8-10 74HCT151
+ * PC15    S0                        U8-11 74HCT151
+ */
+
+/*
+ * USB
+ *
+ * Port     Signal Name               CONN
+ * PA9     OTG_FS_VBUS               J1-1
+ * PA10    OTG_FS_ID                 J1-4
+ * PA11    OTG_FS_DM                 J1-2
+ * PA12    OTG_FS_DP                 J1-3
+ */
+
+/*
+ * UNUSED PINS - In an idle world - these would have been tied to pads to
+ * facilitate debugging probs.
+ * Port
+ * PA15
+ * PB3
+ * PB5
+ * PC8
+*/
+
+/* Board provides GPIO or other Hardware for signaling to timing analyzer */
+
+#if defined(CONFIG_BOARD_USE_PROBES)
+# define PROBE_N(n) (1<<((n)-1))
+# define PROBE_1	(GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTA|GPIO_PIN15)
+# define PROBE_2	(GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN3)
+# define PROBE_3	(GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN5)
+# define PROBE_4	(GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTC|GPIO_PIN8)
+
+# define PROBE_INIT(mask) \
+	do { \
+		if ((mask)& PROBE_N(1)) { stm32_configgpio(PROBE_1); } \
+		if ((mask)& PROBE_N(2)) { stm32_configgpio(PROBE_2); } \
+		if ((mask)& PROBE_N(3)) { stm32_configgpio(PROBE_3); } \
+		if ((mask)& PROBE_N(4)) { stm32_configgpio(PROBE_4); } \
+	} while(0)
+
+# define PROBE(n,s)  do {stm32_gpiowrite(PROBE_##n,(s));}while(0)
+# define PROBE_MARK(n) PROBE(n,false);PROBE(n,true)
+#else
+# define PROBE_INIT(mask)
+# define PROBE(n,s)
+# define PROBE_MARK(n)
+#endif
+
+/************************************************************************************
+ * Public Data
+ ************************************************************************************/
+
+#ifndef __ASSEMBLY__
+
+#undef EXTERN
+#if defined(__cplusplus)
+#define EXTERN extern "C"
+extern "C" {
+#else
+#define EXTERN extern
+#endif
+
+/************************************************************************************
+ * Public Function Prototypes
+ ************************************************************************************/
+/************************************************************************************
+ * Name: stm32_boardinitialize
+ *
+ * Description:
+ *   All STM32 architectures must provide the following entry point.  This entry point
+ *   is called early in the intitialization -- after all memory has been configured
+ *   and mapped but before any devices have been initialized.
+ *
+ ************************************************************************************/
+
+EXTERN void stm32_boardinitialize(void);
+
+/************************************************************************************
+ * Name:  stm32_ledinit, stm32_setled, and stm32_setleds
+ *
+ * Description:
+ *   If CONFIG_ARCH_LEDS is defined, then NuttX will control the on-board LEDs.  If
+ *   CONFIG_ARCH_LEDS is not defined, then the following interfacesare available to
+ *   control the LEDs from user applications.
+ *
+ ************************************************************************************/
+
+#ifndef CONFIG_ARCH_LEDS
+EXTERN void stm32_ledinit(void);
+EXTERN void stm32_setled(int led, bool ledon);
+EXTERN void stm32_setleds(uint8_t ledset);
+#endif
+
+#undef EXTERN
+#if defined(__cplusplus)
+}
+#endif
+
+#endif /* __ASSEMBLY__ */
+#endif  /* __CONFIG_TAP_V1_INCLUDE_BOARD_H */
diff --git a/nuttx-configs/tap-v1/include/nsh_romfsimg.h b/nuttx-configs/tap-v1/include/nsh_romfsimg.h
new file mode 100644
index 0000000000..beb1a924cf
--- /dev/null
+++ b/nuttx-configs/tap-v1/include/nsh_romfsimg.h
@@ -0,0 +1,42 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2016 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * nsh_romfsetc.h
+ *
+ * This file is a stub for 'make export' purposes; the actual ROMFS
+ * must be supplied by the library client.
+ */
+
+extern unsigned char romfs_img[];
+extern unsigned int romfs_img_len;
diff --git a/nuttx-configs/tap-v1/nsh/Make.defs b/nuttx-configs/tap-v1/nsh/Make.defs
new file mode 100644
index 0000000000..7601fcc095
--- /dev/null
+++ b/nuttx-configs/tap-v1/nsh/Make.defs
@@ -0,0 +1,160 @@
+############################################################################
+# configs/tap-v1/nsh/Make.defs
+#
+#   Copyright (C) 2011 Gregory Nutt. All rights reserved.
+#   Author: Gregory Nutt <gnutt@nuttx.org>
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions
+# are met:
+#
+# 1. Redistributions of source code must retain the above copyright
+#    notice, this list of conditions and the following disclaimer.
+# 2. Redistributions in binary form must reproduce the above copyright
+#    notice, this list of conditions and the following disclaimer in
+#    the documentation and/or other materials provided with the
+#    distribution.
+# 3. Neither the name NuttX nor the names of its contributors may be
+#    used to endorse or promote products derived from this software
+#    without specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.
+#
+############################################################################
+
+include ${TOPDIR}/.config
+include ${TOPDIR}/tools/Config.mk
+
+include $(TOPDIR)/PX4_Warnings.mk
+
+#
+# We only support building with the ARM bare-metal toolchain from
+# https://launchpad.net/gcc-arm-embedded on Windows, Linux or Mac OS.
+#
+CONFIG_ARMV7M_TOOLCHAIN	:= GNU_EABI
+
+include ${TOPDIR}/arch/arm/src/armv7-m/Toolchain.defs
+
+CC			 = $(CROSSDEV)gcc
+CXX			 = $(CROSSDEV)g++
+CPP			 = $(CROSSDEV)gcc -E
+LD			 = $(CROSSDEV)ld
+AR			 = $(CROSSDEV)ar rcs
+NM			 = $(CROSSDEV)nm
+OBJCOPY			 = $(CROSSDEV)objcopy
+OBJDUMP			 = $(CROSSDEV)objdump
+
+MAXOPTIMIZATION		 = -Os
+ARCHCPUFLAGS		 = -mcpu=cortex-m4 \
+			   -mthumb \
+			   -march=armv7e-m \
+			   -mfpu=fpv4-sp-d16 \
+			   -mfloat-abi=hard
+
+
+# enable precise stack overflow tracking
+ifeq ($(CONFIG_ARMV7M_STACKCHECK),y)
+INSTRUMENTATIONDEFINES	 = -finstrument-functions -ffixed-r10
+endif
+
+# pull in *just* libm from the toolchain ... this is grody
+LIBM			 = "${shell $(CC) $(ARCHCPUFLAGS) -print-file-name=libm.a}"
+EXTRA_LIBS		+= $(LIBM)
+
+# use our linker script
+LDSCRIPT		 = ld.script
+
+ifeq ($(WINTOOL),y)
+  # Windows-native toolchains
+  DIRLINK		 = $(TOPDIR)/tools/copydir.sh
+  DIRUNLINK		 = $(TOPDIR)/tools/unlink.sh
+  MKDEP			 = $(TOPDIR)/tools/mknulldeps.sh
+  ARCHINCLUDES		 = -I. -isystem "${shell cygpath -w $(TOPDIR)/include}"
+  ARCHXXINCLUDES	 = -I. -isystem "${shell cygpath -w $(TOPDIR)/include}" -isystem "${shell cygpath -w $(TOPDIR)/include/cxx}"
+  ARCHSCRIPT		 = -T "${shell cygpath -w $(TOPDIR)/configs/$(CONFIG_ARCH_BOARD)/scripts/$(LDSCRIPT)}"
+else
+  ifeq ($(PX4_WINTOOL),y)
+    # Windows-native toolchains (MSYS)
+    DIRLINK		 = $(TOPDIR)/tools/copydir.sh
+    DIRUNLINK		 = $(TOPDIR)/tools/unlink.sh
+    MKDEP		 = $(TOPDIR)/tools/mknulldeps.sh
+    ARCHINCLUDES	 = -I. -isystem $(TOPDIR)/include
+    ARCHXXINCLUDES	 = -I. -isystem $(TOPDIR)/include -isystem $(TOPDIR)/include/cxx
+    ARCHSCRIPT		 = -T$(TOPDIR)/configs/$(CONFIG_ARCH_BOARD)/scripts/$(LDSCRIPT)
+  else
+    # Linux/Cygwin-native toolchain
+    MKDEP		 = $(TOPDIR)/tools/mkdeps.sh
+    ARCHINCLUDES	 = -I. -isystem $(TOPDIR)/include
+    ARCHXXINCLUDES	 = -I. -isystem $(TOPDIR)/include -isystem $(TOPDIR)/include/cxx
+    ARCHSCRIPT		 = -T$(TOPDIR)/configs/$(CONFIG_ARCH_BOARD)/scripts/$(LDSCRIPT)
+  endif
+endif
+
+# tool versions
+ARCHCCVERSION		 = ${shell $(CC) -v 2>&1 | sed -n '/^gcc version/p' | sed -e 's/^gcc version \([0-9\.]\)/\1/g' -e 's/[-\ ].*//g' -e '1q'}
+ARCHCCMAJOR		 = ${shell echo $(ARCHCCVERSION) | cut -d'.' -f1}
+
+# optimisation flags
+ARCHOPTIMIZATION	 = $(MAXOPTIMIZATION) \
+			   -fno-strict-aliasing \
+			   -fno-strength-reduce \
+			   -fomit-frame-pointer \
+			   -funsafe-math-optimizations \
+			   -fno-builtin-printf \
+			   -ffunction-sections \
+			   -fdata-sections
+
+ifeq ("${CONFIG_DEBUG_SYMBOLS}","y")
+ARCHOPTIMIZATION	+= -g
+endif
+
+ARCHCFLAGS		 = -std=gnu99
+ARCHCXXFLAGS		 = -fno-exceptions -fno-rtti -std=gnu++0x
+ARCHWARNINGS		 = $(PX4_ARCHWARNINGS)
+ARCHCWARNINGS		 = $(PX4_ARCHWARNINGS) $(PX4_ARCHCWARNINGS)
+ARCHWARNINGSXX		 = $(ARCHWARNINGS) $(PX4_ARCHWARNINGSXX)
+ARCHDEFINES		 =
+ARCHPICFLAGS		 = -fpic -msingle-pic-base -mpic-register=r10
+
+# this seems to be the only way to add linker flags
+EXTRA_LIBS		+= --warn-common \
+			   --gc-sections
+
+CFLAGS			 = $(ARCHCFLAGS) $(ARCHCWARNINGS) $(ARCHOPTIMIZATION) $(ARCHCPUFLAGS) $(ARCHINCLUDES) $(INSTRUMENTATIONDEFINES) $(ARCHDEFINES) $(EXTRADEFINES) -pipe -fno-common
+CPICFLAGS		 = $(ARCHPICFLAGS) $(CFLAGS)
+CXXFLAGS		 = $(ARCHCXXFLAGS) $(ARCHWARNINGSXX) $(ARCHOPTIMIZATION) $(ARCHCPUFLAGS) $(ARCHXXINCLUDES) $(INSTRUMENTATIONDEFINES) $(ARCHDEFINES) $(EXTRADEFINES) -pipe
+CXXPICFLAGS		 = $(ARCHPICFLAGS) $(CXXFLAGS)
+CPPFLAGS		 = $(ARCHINCLUDES) $(INSTRUMENTATIONDEFINES) $(ARCHDEFINES) $(EXTRADEFINES)
+AFLAGS			 = $(CFLAGS) -D__ASSEMBLY__
+
+NXFLATLDFLAGS1		 = -r -d -warn-common
+NXFLATLDFLAGS2		 = $(NXFLATLDFLAGS1) -T$(TOPDIR)/binfmt/libnxflat/gnu-nxflat.ld -no-check-sections
+LDNXFLATFLAGS		 = -e main -s 2048
+
+OBJEXT			 = .o
+LIBEXT			 = .a
+EXEEXT			 =
+
+
+# produce partially-linked $1 from files in $2
+define PRELINK
+	@echo "PRELINK: $1"
+	$(Q) $(LD) -Ur -o $1 $2 && $(OBJCOPY) --localize-hidden $1
+endef
+
+HOSTCC			 = gcc
+HOSTINCLUDES		 = -I.
+HOSTCFLAGS		 = -Wall -Wstrict-prototypes -Wshadow -g -pipe
+HOSTLDFLAGS		 =
+
diff --git a/nuttx-configs/tap-v1/nsh/defconfig b/nuttx-configs/tap-v1/nsh/defconfig
new file mode 100644
index 0000000000..c55ddf53cd
--- /dev/null
+++ b/nuttx-configs/tap-v1/nsh/defconfig
@@ -0,0 +1,982 @@
+#
+# Automatically generated file; DO NOT EDIT.
+# Nuttx/ Configuration
+#
+CONFIG_NUTTX_NEWCONFIG=y
+
+#
+# Build Setup
+#
+# CONFIG_EXPERIMENTAL is not set
+CONFIG_HOST_LINUX=y
+# CONFIG_HOST_OSX is not set
+# CONFIG_HOST_WINDOWS is not set
+# CONFIG_HOST_OTHER is not set
+
+#
+# Build Configuration
+#
+CONFIG_APPS_DIR="../apps"
+# CONFIG_BUILD_2PASS is not set
+
+#
+# Binary Output Formats
+#
+# CONFIG_RRLOAD_BINARY is not set
+# CONFIG_INTELHEX_BINARY is not set
+# CONFIG_MOTOROLA_SREC is not set
+CONFIG_RAW_BINARY=y
+
+#
+# Customize Header Files
+#
+# CONFIG_ARCH_STDBOOL_H is not set
+CONFIG_ARCH_MATH_H=y
+# CONFIG_ARCH_FLOAT_H is not set
+# CONFIG_ARCH_STDARG_H is not set
+
+#
+# Debug Options
+#
+# CONFIG_DEBUG is not set
+CONFIG_DEBUG_SYMBOLS=y
+
+#
+# System Type
+#
+# CONFIG_ARCH_8051 is not set
+CONFIG_ARCH_ARM=y
+# CONFIG_ARCH_AVR is not set
+# CONFIG_ARCH_HC is not set
+# CONFIG_ARCH_MIPS is not set
+# CONFIG_ARCH_RGMP is not set
+# CONFIG_ARCH_SH is not set
+# CONFIG_ARCH_SIM is not set
+# CONFIG_ARCH_X86 is not set
+# CONFIG_ARCH_Z16 is not set
+# CONFIG_ARCH_Z80 is not set
+CONFIG_ARCH="arm"
+
+#
+# ARM Options
+#
+# CONFIG_ARCH_CHIP_C5471 is not set
+# CONFIG_ARCH_CHIP_CALYPSO is not set
+# CONFIG_ARCH_CHIP_DM320 is not set
+# CONFIG_ARCH_CHIP_IMX is not set
+# CONFIG_ARCH_CHIP_KINETIS is not set
+# CONFIG_ARCH_CHIP_KL is not set
+# CONFIG_ARCH_CHIP_LM is not set
+# CONFIG_ARCH_CHIP_LPC17XX is not set
+# CONFIG_ARCH_CHIP_LPC214X is not set
+# CONFIG_ARCH_CHIP_LPC2378 is not set
+# CONFIG_ARCH_CHIP_LPC31XX is not set
+# CONFIG_ARCH_CHIP_LPC43XX is not set
+# CONFIG_ARCH_CHIP_NUC1XX is not set
+# CONFIG_ARCH_CHIP_SAM34 is not set
+CONFIG_ARCH_CHIP_STM32=y
+# CONFIG_ARCH_CHIP_STR71X is not set
+CONFIG_ARCH_CORTEXM4=y
+CONFIG_ARCH_FAMILY="armv7-m"
+CONFIG_ARCH_CHIP="stm32"
+CONFIG_ARMV7M_USEBASEPRI=y
+CONFIG_ARCH_HAVE_CMNVECTOR=y
+CONFIG_ARMV7M_CMNVECTOR=y
+CONFIG_ARCH_HAVE_FPU=y
+CONFIG_ARCH_FPU=y
+CONFIG_ARCH_HAVE_MPU=y
+# CONFIG_ARMV7M_MPU is not set
+
+#
+# ARMV7M Configuration Options
+#
+# CONFIG_ARMV7M_TOOLCHAIN_BUILDROOT is not set
+# CONFIG_ARMV7M_TOOLCHAIN_CODEREDL is not set
+# CONFIG_ARMV7M_TOOLCHAIN_CODESOURCERYL is not set
+CONFIG_ARMV7M_TOOLCHAIN_GNU_EABI=y
+# CONFIG_ARMV7M_STACKCHECK is not set
+CONFIG_SERIAL_TERMIOS=y
+
+#
+# STM32 Configuration Options
+#
+# CONFIG_ARCH_CHIP_STM32L151C6 is not set
+# CONFIG_ARCH_CHIP_STM32L151C8 is not set
+# CONFIG_ARCH_CHIP_STM32L151CB is not set
+# CONFIG_ARCH_CHIP_STM32L151R6 is not set
+# CONFIG_ARCH_CHIP_STM32L151R8 is not set
+# CONFIG_ARCH_CHIP_STM32L151RB is not set
+# CONFIG_ARCH_CHIP_STM32L151V6 is not set
+# CONFIG_ARCH_CHIP_STM32L151V8 is not set
+# CONFIG_ARCH_CHIP_STM32L151VB is not set
+# CONFIG_ARCH_CHIP_STM32L152C6 is not set
+# CONFIG_ARCH_CHIP_STM32L152C8 is not set
+# CONFIG_ARCH_CHIP_STM32L152CB is not set
+# CONFIG_ARCH_CHIP_STM32L152R6 is not set
+# CONFIG_ARCH_CHIP_STM32L152R8 is not set
+# CONFIG_ARCH_CHIP_STM32L152RB is not set
+# CONFIG_ARCH_CHIP_STM32L152V6 is not set
+# CONFIG_ARCH_CHIP_STM32L152V8 is not set
+# CONFIG_ARCH_CHIP_STM32L152VB is not set
+# CONFIG_ARCH_CHIP_STM32F100C8 is not set
+# CONFIG_ARCH_CHIP_STM32F100CB is not set
+# CONFIG_ARCH_CHIP_STM32F100R8 is not set
+# CONFIG_ARCH_CHIP_STM32F100RB is not set
+# CONFIG_ARCH_CHIP_STM32F100RC is not set
+# CONFIG_ARCH_CHIP_STM32F100RD is not set
+# CONFIG_ARCH_CHIP_STM32F100RE is not set
+# CONFIG_ARCH_CHIP_STM32F100V8 is not set
+# CONFIG_ARCH_CHIP_STM32F100VB is not set
+# CONFIG_ARCH_CHIP_STM32F100VC is not set
+# CONFIG_ARCH_CHIP_STM32F100VD is not set
+# CONFIG_ARCH_CHIP_STM32F100VE is not set
+# CONFIG_ARCH_CHIP_STM32F102CB is not set
+# CONFIG_ARCH_CHIP_STM32F103C4 is not set
+# CONFIG_ARCH_CHIP_STM32F103C8 is not set
+# CONFIG_ARCH_CHIP_STM32F103RET6 is not set
+# CONFIG_ARCH_CHIP_STM32F103VCT6 is not set
+# CONFIG_ARCH_CHIP_STM32F103VET6 is not set
+# CONFIG_ARCH_CHIP_STM32F103ZET6 is not set
+# CONFIG_ARCH_CHIP_STM32F105VBT7 is not set
+# CONFIG_ARCH_CHIP_STM32F107VC is not set
+# CONFIG_ARCH_CHIP_STM32F207IG is not set
+# CONFIG_ARCH_CHIP_STM32F302CB is not set
+# CONFIG_ARCH_CHIP_STM32F302CC is not set
+# CONFIG_ARCH_CHIP_STM32F302RB is not set
+# CONFIG_ARCH_CHIP_STM32F302RC is not set
+# CONFIG_ARCH_CHIP_STM32F302VB is not set
+# CONFIG_ARCH_CHIP_STM32F302VC is not set
+# CONFIG_ARCH_CHIP_STM32F303CB is not set
+# CONFIG_ARCH_CHIP_STM32F303CC is not set
+# CONFIG_ARCH_CHIP_STM32F303RB is not set
+# CONFIG_ARCH_CHIP_STM32F303RC is not set
+# CONFIG_ARCH_CHIP_STM32F303VB is not set
+# CONFIG_ARCH_CHIP_STM32F303VC is not set
+CONFIG_ARCH_CHIP_STM32F405RG=y
+# CONFIG_ARCH_CHIP_STM32F405VG is not set
+# CONFIG_ARCH_CHIP_STM32F405ZG is not set
+# CONFIG_ARCH_CHIP_STM32F407VE is not set
+# CONFIG_ARCH_CHIP_STM32F407VG is not set
+# CONFIG_ARCH_CHIP_STM32F407ZE is not set
+# CONFIG_ARCH_CHIP_STM32F407ZG is not set
+# CONFIG_ARCH_CHIP_STM32F407IE is not set
+# CONFIG_ARCH_CHIP_STM32F407IG is not set
+# CONFIG_ARCH_CHIP_STM32F427V is not set
+# CONFIG_ARCH_CHIP_STM32F427Z is not set
+# CONFIG_ARCH_CHIP_STM32F427I is not set
+# CONFIG_STM32_STM32L15XX is not set
+# CONFIG_STM32_ENERGYLITE is not set
+# CONFIG_STM32_STM32F10XX is not set
+# CONFIG_STM32_VALUELINE is not set
+# CONFIG_STM32_CONNECTIVITYLINE is not set
+# CONFIG_STM32_PERFORMANCELINE is not set
+# CONFIG_STM32_HIGHDENSITY is not set
+# CONFIG_STM32_MEDIUMDENSITY is not set
+# CONFIG_STM32_LOWDENSITY is not set
+# CONFIG_STM32_STM32F20XX is not set
+# CONFIG_STM32_STM32F30XX is not set
+CONFIG_STM32_STM32F40XX=y
+# CONFIG_STM32_DFU is not set
+
+#
+# STM32 Peripheral Support
+#
+CONFIG_STM32_ADC1=y
+# CONFIG_STM32_ADC2 is not set
+# CONFIG_STM32_ADC3 is not set
+CONFIG_STM32_BKPSRAM=y
+# CONFIG_STM32_CAN1 is not set
+# CONFIG_STM32_CAN2 is not set
+CONFIG_STM32_CCMDATARAM=y
+# CONFIG_STM32_CRC is not set
+# CONFIG_STM32_CRYP is not set
+CONFIG_STM32_DMA1=y
+CONFIG_STM32_DMA2=y
+# CONFIG_STM32_DAC1 is not set
+# CONFIG_STM32_DAC2 is not set
+# CONFIG_STM32_DCMI is not set
+# CONFIG_STM32_ETHMAC is not set
+# CONFIG_STM32_FSMC is not set
+# CONFIG_STM32_HASH is not set
+CONFIG_STM32_I2C1=y
+CONFIG_STM32_I2C2=y
+CONFIG_STM32_I2C3=y
+CONFIG_STM32_OTGFS=y
+# CONFIG_STM32_OTGHS is not set
+CONFIG_STM32_PWR=y
+# CONFIG_STM32_RNG is not set
+# CONFIG_STM32_SDIO is not set
+# CONFIG_STM32_SPI1 is not set
+CONFIG_STM32_SPI2=y
+# CONFIG_STM32_SPI3 is not set
+CONFIG_STM32_SYSCFG=y
+# CONFIG_STM32_TIM1 is not set
+# CONFIG_STM32_TIM2 is not set
+# CONFIG_STM32_TIM3 is not set
+CONFIG_STM32_TIM4=y
+CONFIG_STM32_TIM5=y
+CONFIG_STM32_TIM6=y
+CONFIG_STM32_TIM7=y
+# CONFIG_STM32_TIM8 is not set
+CONFIG_STM32_TIM9=y
+CONFIG_STM32_TIM10=y
+CONFIG_STM32_TIM11=y
+CONFIG_STM32_TIM12=y
+CONFIG_STM32_TIM13=y
+CONFIG_STM32_TIM14=y
+CONFIG_STM32_USART1=y
+CONFIG_STM32_USART2=y
+CONFIG_STM32_USART3=y
+CONFIG_STM32_UART4=y
+CONFIG_STM32_UART5=y
+CONFIG_STM32_USART6=y
+# CONFIG_STM32_IWDG is not set
+CONFIG_STM32_WWDG=y
+CONFIG_STM32_ADC=y
+CONFIG_STM32_SPI=y
+CONFIG_STM32_I2C=y
+
+#
+# Alternate Pin Mapping
+#
+CONFIG_STM32_FLASH_PREFETCH=y
+# CONFIG_STM32_JTAG_DISABLE is not set
+# CONFIG_STM32_JTAG_FULL_ENABLE is not set
+# CONFIG_STM32_JTAG_NOJNTRST_ENABLE is not set
+CONFIG_STM32_JTAG_SW_ENABLE=y
+CONFIG_STM32_DISABLE_IDLE_SLEEP_DURING_DEBUG=y
+# CONFIG_STM32_FORCEPOWER is not set
+# CONFIG_ARCH_BOARD_STM32_CUSTOM_CLOCKCONFIG is not set
+# CONFIG_STM32_CCMEXCLUDE is not set
+CONFIG_STM32_DMACAPABLE=y
+# CONFIG_STM32_TIM4_PWM is not set
+# CONFIG_STM32_TIM5_PWM is not set
+# CONFIG_STM32_TIM9_PWM is not set
+# CONFIG_STM32_TIM10_PWM is not set
+# CONFIG_STM32_TIM11_PWM is not set
+# CONFIG_STM32_TIM12_PWM is not set
+# CONFIG_STM32_TIM13_PWM is not set
+# CONFIG_STM32_TIM14_PWM is not set
+# CONFIG_STM32_TIM4_ADC is not set
+# CONFIG_STM32_TIM5_ADC is not set
+CONFIG_STM32_USART=y
+
+#
+# U[S]ART Configuration
+#
+# CONFIG_USART1_RS485 is not set
+CONFIG_USART1_RXDMA=y
+# CONFIG_USART2_RS485 is not set
+CONFIG_USART2_RXDMA=y
+# CONFIG_USART3_RS485 is not set
+CONFIG_USART3_RXDMA=y
+# CONFIG_UART4_RS485 is not set
+CONFIG_UART4_RXDMA=y
+# CONFIG_UART5_RS485 is not set
+CONFIG_UART5_RXDMA=y
+# CONFIG_USART6_RS485 is not set
+CONFIG_USART6_RXDMA=y
+CONFIG_SERIAL_DISABLE_REORDERING=y
+CONFIG_STM32_USART_SINGLEWIRE=y
+
+#
+# SPI Configuration
+#
+# CONFIG_STM32_SPI_INTERRUPTS is not set
+# CONFIG_STM32_SPI_DMA is not set
+
+#
+# I2C Configuration
+#
+# CONFIG_STM32_I2C_DYNTIMEO is not set
+CONFIG_STM32_I2CTIMEOSEC=0
+CONFIG_STM32_I2CTIMEOMS=10
+# CONFIG_STM32_I2CTIMEOTICKS is not set (PX4 Codbase has this)
+# CONFIG_STM32_I2C_DUTY16_9 is not set
+
+#
+# USB Host Configuration
+#
+
+#
+# USB Device Configuration
+#
+
+#
+# External Memory Configuration
+#
+
+#
+# Architecture Options
+#
+# CONFIG_ARCH_NOINTC is not set
+# CONFIG_ARCH_VECNOTIRQ is not set
+CONFIG_ARCH_DMA=y
+CONFIG_ARCH_IRQPRIO=y
+# CONFIG_CUSTOM_STACK is not set
+# CONFIG_ADDRENV is not set
+CONFIG_ARCH_HAVE_VFORK=y
+CONFIG_ARCH_STACKDUMP=y
+# CONFIG_ENDIAN_BIG is not set
+# CONFIG_ARCH_HAVE_RAMFUNCS is not set
+CONFIG_ARCH_HAVE_RAMVECTORS=y
+# CONFIG_ARCH_RAMVECTORS is not set
+
+#
+# Board Settings
+#
+CONFIG_BOARD_LOOPSPERMSEC=16717
+# CONFIG_ARCH_CALIBRATION is not set
+CONFIG_DRAM_START=0x20000000
+CONFIG_DRAM_SIZE=196608
+CONFIG_ARCH_HAVE_INTERRUPTSTACK=y
+CONFIG_ARCH_INTERRUPTSTACK=750
+
+#
+# Boot options
+#
+# CONFIG_BOOT_RUNFROMEXTSRAM is not set
+CONFIG_BOOT_RUNFROMFLASH=y
+# CONFIG_BOOT_RUNFROMISRAM is not set
+# CONFIG_BOOT_RUNFROMSDRAM is not set
+# CONFIG_BOOT_COPYTORAM is not set
+
+#
+# Board Selection
+#
+CONFIG_ARCH_BOARD_TAP_V1=y
+CONFIG_ARCH_BOARD="tap-v1"
+
+#
+# Common Board Options
+#
+CONFIG_NSH_MMCSDMINOR=0
+CONFIG_NSH_MMCSDSLOTNO=0
+CONFIG_NSH_MMCSDSPIPORTNO=2
+
+#
+# Board-Specific Options
+#
+
+#
+# RTOS Features
+#
+# CONFIG_BOARD_INITIALIZE is not set
+CONFIG_MSEC_PER_TICK=1
+CONFIG_RR_INTERVAL=0
+CONFIG_SCHED_INSTRUMENTATION=y
+CONFIG_TASK_NAME_SIZE=24
+# CONFIG_SCHED_HAVE_PARENT is not set
+# CONFIG_JULIAN_TIME is not set
+CONFIG_START_YEAR=1970
+CONFIG_START_MONTH=1
+CONFIG_START_DAY=1
+CONFIG_DEV_CONSOLE=y
+# CONFIG_MUTEX_TYPES is not set
+CONFIG_PRIORITY_INHERITANCE=y
+CONFIG_SEM_PREALLOCHOLDERS=0
+CONFIG_SEM_NNESTPRIO=8
+# CONFIG_FDCLONE_DISABLE is not set
+CONFIG_FDCLONE_STDIO=y
+CONFIG_SDCLONE_DISABLE=y
+CONFIG_SCHED_WAITPID=y
+# CONFIG_SCHED_STARTHOOK is not set
+CONFIG_SCHED_ATEXIT=y
+CONFIG_SCHED_ATEXIT_MAX=1
+# CONFIG_SCHED_ONEXIT is not set
+CONFIG_USER_ENTRYPOINT="nsh_main"
+# CONFIG_DISABLE_OS_API is not set
+
+#
+# Signal Numbers
+#
+CONFIG_SIG_SIGUSR1=1
+CONFIG_SIG_SIGUSR2=2
+CONFIG_SIG_SIGALARM=3
+CONFIG_SIG_SIGCONDTIMEDOUT=16
+CONFIG_SIG_SIGWORK=4
+
+#
+# Sizes of configurable things (0 disables)
+#
+CONFIG_MAX_TASKS=32
+CONFIG_MAX_TASK_ARGS=10
+CONFIG_NPTHREAD_KEYS=4
+CONFIG_NFILE_DESCRIPTORS=42
+CONFIG_NFILE_STREAMS=8
+CONFIG_NAME_MAX=32
+CONFIG_PREALLOC_MQ_MSGS=4
+CONFIG_MQ_MAXMSGSIZE=32
+CONFIG_MAX_WDOGPARMS=2
+CONFIG_PREALLOC_WDOGS=50
+CONFIG_PREALLOC_TIMERS=50
+
+#
+# Stack and heap information
+#
+CONFIG_IDLETHREAD_STACKSIZE=1000
+CONFIG_USERMAIN_STACKSIZE=2500
+CONFIG_PTHREAD_STACK_MIN=512
+CONFIG_PTHREAD_STACK_DEFAULT=2048
+
+#
+# Device Drivers
+#
+# CONFIG_DISABLE_POLL is not set
+CONFIG_DEV_NULL=y
+# CONFIG_DEV_ZERO is not set
+# CONFIG_LOOP is not set
+# CONFIG_RAMDISK is not set
+# CONFIG_CAN is not set
+# CONFIG_PWM is not set
+CONFIG_I2C=y
+# CONFIG_I2C_SLAVE is not set
+CONFIG_I2C_TRANSFER=y
+# CONFIG_I2C_WRITEREAD is not set
+# CONFIG_I2C_POLLED is not set
+# CONFIG_I2C_TRACE is not set
+CONFIG_ARCH_HAVE_I2CRESET=y
+CONFIG_I2C_RESET=y
+CONFIG_SPI=y
+# CONFIG_SPI_OWNBUS is not set
+CONFIG_SPI_EXCHANGE=y
+# CONFIG_SPI_CMDDATA is not set
+# CONFIG_RTC is not set
+CONFIG_WATCHDOG=y
+# CONFIG_ANALOG is not set
+# CONFIG_AUDIO_DEVICES is not set
+# CONFIG_BCH is not set
+# CONFIG_INPUT is not set
+# CONFIG_LCD is not set
+CONFIG_MMCSD=y
+CONFIG_MMCSD_NSLOTS=1
+# CONFIG_MMCSD_READONLY is not set
+# CONFIG_MMCSD_MULTIBLOCK_DISABLE is not set
+# CONFIG_MMCSD_MMCSUPPORT is not set
+# CONFIG_MMCSD_HAVECARDDETECT is not set
+# CONFIG_MMCSD_HAVE_SDIOWAIT_WRCOMPLETE is not set
+CONFIG_MMCSD_SPI=y
+CONFIG_MMCSD_SPICLOCK=24000000
+# CONFIG_MMCSD_SDIO is not set
+# CONFIG_MTD is not set
+CONFIG_PIPES=y
+# CONFIG_PM is not set
+# CONFIG_POWER is not set
+# CONFIG_SENSORS is not set
+CONFIG_SERIAL=y
+# CONFIG_DEV_LOWCONSOLE is not set
+CONFIG_SERIAL_REMOVABLE=y
+# CONFIG_16550_UART is not set
+CONFIG_ARCH_HAVE_UART4=y
+CONFIG_ARCH_HAVE_UART5=y
+CONFIG_ARCH_HAVE_USART1=y
+CONFIG_ARCH_HAVE_USART2=y
+CONFIG_ARCH_HAVE_USART3=y
+CONFIG_ARCH_HAVE_USART6=y
+CONFIG_MCU_SERIAL=y
+CONFIG_STANDARD_SERIAL=y
+CONFIG_SERIAL_NPOLLWAITERS=2
+# CONFIG_USART1_SERIAL_CONSOLE is not set
+# CONFIG_USART2_SERIAL_CONSOLE is not set
+CONFIG_USART3_SERIAL_CONSOLE=y
+# CONFIG_UART4_SERIAL_CONSOLE is not set
+# CONFIG_UART5_SERIAL_CONSOLE is not set
+# CONFIG_USART6_SERIAL_CONSOLE is not set
+# CONFIG_NO_SERIAL_CONSOLE is not set
+
+#
+# USART1 Configuration
+#
+CONFIG_USART1_RXBUFSIZE=128
+CONFIG_USART1_TXBUFSIZE=128
+CONFIG_USART1_BAUD=57600
+CONFIG_USART1_BITS=8
+CONFIG_USART1_PARITY=0
+CONFIG_USART1_2STOP=0
+# CONFIG_USART1_IFLOWCONTROL is not set
+# CONFIG_USART1_OFLOWCONTROL is not set
+
+#
+# USART2 Configuration
+#
+CONFIG_USART2_RXBUFSIZE=300
+CONFIG_USART2_TXBUFSIZE=300
+CONFIG_USART2_BAUD=57600
+CONFIG_USART2_BITS=8
+CONFIG_USART2_PARITY=0
+CONFIG_USART2_2STOP=0
+# CONFIG_USART2_IFLOWCONTROL is not set
+# CONFIG_USART2_OFLOWCONTROL is not set
+
+#
+# USART3 Configuration
+#
+CONFIG_USART3_RXBUFSIZE=256
+CONFIG_USART3_TXBUFSIZE=256
+CONFIG_USART3_BAUD=115200
+CONFIG_USART3_BITS=8
+CONFIG_USART3_PARITY=0
+CONFIG_USART3_2STOP=0
+# CONFIG_USART3_IFLOWCONTROL is not set
+# CONFIG_USART3_OFLOWCONTROL is not set
+
+#
+# UART4 Configuration
+#
+CONFIG_UART4_RXBUFSIZE=256
+CONFIG_UART4_TXBUFSIZE=256
+CONFIG_UART4_BAUD=115200
+CONFIG_UART4_BITS=8
+CONFIG_UART4_PARITY=0
+CONFIG_UART4_2STOP=0
+# CONFIG_UART4_IFLOWCONTROL is not set
+# CONFIG_UART4_OFLOWCONTROL is not set
+
+#
+# UART5 Configuration
+#
+CONFIG_UART5_RXBUFSIZE=300
+CONFIG_UART5_TXBUFSIZE=300
+CONFIG_UART5_BAUD=57600
+CONFIG_UART5_BITS=8
+CONFIG_UART5_PARITY=0
+CONFIG_UART5_2STOP=0
+# CONFIG_UART5_IFLOWCONTROL is not set
+# CONFIG_UART5_OFLOWCONTROL is not set
+
+#
+# USART6 Configuration
+#
+CONFIG_USART6_RXBUFSIZE=128
+CONFIG_USART6_TXBUFSIZE=64
+CONFIG_USART6_BAUD=57600
+CONFIG_USART6_BITS=8
+CONFIG_USART6_PARITY=0
+CONFIG_USART6_2STOP=0
+# CONFIG_USART6_IFLOWCONTROL is not set
+# CONFIG_USART6_OFLOWCONTROL is not set
+# CONFIG_SERIAL_IFLOWCONTROL is not set
+# CONFIG_SERIAL_OFLOWCONTROL is not set
+CONFIG_USBDEV=y
+
+#
+# USB Device Controller Driver Options
+#
+# CONFIG_USBDEV_ISOCHRONOUS is not set
+# CONFIG_USBDEV_DUALSPEED is not set
+# CONFIG_USBDEV_SELFPOWERED is not set
+CONFIG_USBDEV_BUSPOWERED=y
+CONFIG_USBDEV_MAXPOWER=500
+# CONFIG_USBDEV_DMA is not set
+# CONFIG_USBDEV_TRACE is not set
+
+#
+# USB Device Class Driver Options
+#
+# CONFIG_USBDEV_COMPOSITE is not set
+# CONFIG_PL2303 is not set
+CONFIG_CDCACM=y
+# CONFIG_CDCACM_CONSOLE is not set
+CONFIG_CDCACM_EP0MAXPACKET=64
+CONFIG_CDCACM_EPINTIN=1
+CONFIG_CDCACM_EPINTIN_FSSIZE=64
+CONFIG_CDCACM_EPINTIN_HSSIZE=64
+CONFIG_CDCACM_EPBULKOUT=3
+CONFIG_CDCACM_EPBULKOUT_FSSIZE=64
+CONFIG_CDCACM_EPBULKOUT_HSSIZE=512
+CONFIG_CDCACM_EPBULKIN=2
+CONFIG_CDCACM_EPBULKIN_FSSIZE=64
+CONFIG_CDCACM_EPBULKIN_HSSIZE=512
+CONFIG_CDCACM_NWRREQS=4
+CONFIG_CDCACM_NRDREQS=4
+CONFIG_CDCACM_BULKIN_REQLEN=96
+CONFIG_CDCACM_RXBUFSIZE=300
+CONFIG_CDCACM_TXBUFSIZE=1000
+CONFIG_CDCACM_VENDORID=0x26ac
+CONFIG_CDCACM_PRODUCTID=0x0010
+CONFIG_CDCACM_VENDORSTR="The Autopilot"
+CONFIG_CDCACM_PRODUCTSTR="PX4 TAP v1.x"
+# CONFIG_USBMSC is not set
+# CONFIG_USBHOST is not set
+# CONFIG_WIRELESS is not set
+
+#
+# System Logging Device Options
+#
+
+#
+# System Logging
+#
+# CONFIG_RAMLOG is not set
+
+#
+# Networking Support
+#
+# CONFIG_NET is not set
+
+#
+# File Systems
+#
+
+#
+# File system configuration
+#
+# CONFIG_DISABLE_MOUNTPOINT is not set
+# CONFIG_FS_RAMMAP is not set
+CONFIG_FS_FAT=y
+CONFIG_FAT_LCNAMES=y
+CONFIG_FAT_LFN=y
+CONFIG_FAT_MAXFNAME=32
+CONFIG_FS_FATTIME=y
+# CONFIG_FAT_DMAMEMORY is not set
+# CONFIG_FS_NXFFS is not set
+CONFIG_FS_ROMFS=y
+# CONFIG_FS_SMARTFS is not set
+CONFIG_FS_BINFS=y
+
+#
+# System Logging
+#
+# CONFIG_SYSLOG_ENABLE is not set
+# CONFIG_SYSLOG is not set
+
+#
+# Graphics Support
+#
+# CONFIG_NX is not set
+
+#
+# Memory Management
+#
+# CONFIG_MM_MULTIHEAP is not set
+# CONFIG_MM_SMALL is not set
+CONFIG_MM_REGIONS=2
+CONFIG_GRAN=y
+# CONFIG_GRAN_SINGLE is not set
+# CONFIG_GRAN_INTR is not set
+
+#
+# Audio Support
+#
+# CONFIG_AUDIO is not set
+
+#
+# Binary Formats
+#
+# CONFIG_BINFMT_DISABLE is not set
+# CONFIG_BINFMT_EXEPATH is not set
+# CONFIG_NXFLAT is not set
+# CONFIG_ELF is not set
+CONFIG_BUILTIN=y
+# CONFIG_PIC is not set
+# CONFIG_SYMTAB_ORDEREDBYNAME is not set
+
+#
+# Library Routines
+#
+
+#
+# Standard C Library Options
+#
+CONFIG_STDIO_BUFFER_SIZE=180
+CONFIG_STDIO_LINEBUFFER=y
+CONFIG_NUNGET_CHARS=2
+CONFIG_LIB_HOMEDIR="/"
+# CONFIG_NOPRINTF_FIELDWIDTH is not set
+CONFIG_LIBC_FLOATINGPOINT=y
+CONFIG_LIB_RAND_ORDER=1
+# CONFIG_EOL_IS_CR is not set
+# CONFIG_EOL_IS_LF is not set
+# CONFIG_EOL_IS_BOTH_CRLF is not set
+CONFIG_EOL_IS_EITHER_CRLF=y
+# CONFIG_LIBC_EXECFUNCS is not set
+CONFIG_POSIX_SPAWN_PROXY_STACKSIZE=1024
+CONFIG_TASK_SPAWN_DEFAULT_STACKSIZE=2048
+CONFIG_LIBC_STRERROR=y
+# CONFIG_LIBC_STRERROR_SHORT is not set
+# CONFIG_LIBC_PERROR_STDOUT is not set
+CONFIG_ARCH_LOWPUTC=y
+CONFIG_LIB_SENDFILE_BUFSIZE=512
+# CONFIG_ARCH_ROMGETC is not set
+CONFIG_ARCH_OPTIMIZED_FUNCTIONS=y
+CONFIG_ARCH_MEMCPY=y
+# CONFIG_ARCH_MEMCMP is not set
+# CONFIG_ARCH_MEMMOVE is not set
+# CONFIG_ARCH_MEMSET is not set
+# CONFIG_MEMSET_OPTSPEED is not set
+# CONFIG_ARCH_STRCHR is not set
+# CONFIG_ARCH_STRCMP is not set
+# CONFIG_ARCH_STRCPY is not set
+# CONFIG_ARCH_STRNCPY is not set
+# CONFIG_ARCH_STRLEN is not set
+# CONFIG_ARCH_STRNLEN is not set
+# CONFIG_ARCH_BZERO is not set
+
+#
+# Non-standard Library Support
+#
+CONFIG_SCHED_WORKQUEUE=y
+CONFIG_SCHED_HPWORK=y
+CONFIG_SCHED_WORKPRIORITY=192
+CONFIG_SCHED_WORKPERIOD=5000
+CONFIG_SCHED_WORKSTACKSIZE=1600
+CONFIG_SCHED_LPWORK=y
+CONFIG_SCHED_LPWORKPRIORITY=50
+CONFIG_SCHED_LPWORKPERIOD=50000
+CONFIG_SCHED_LPWORKSTACKSIZE=1600
+# CONFIG_LIB_KBDCODEC is not set
+# CONFIG_LIB_SLCDCODEC is not set
+
+#
+# Basic CXX Support
+#
+CONFIG_C99_BOOL8=y
+CONFIG_HAVE_CXX=y
+CONFIG_HAVE_CXXINITIALIZE=y
+# CONFIG_CXX_NEWLONG is not set
+
+#
+# uClibc++ Standard C++ Library
+#
+# CONFIG_UCLIBCXX is not set
+
+#
+# Application Configuration
+#
+
+#
+# Built-In Applications
+#
+CONFIG_BUILTIN_PROXY_STACKSIZE=1024
+
+#
+# Examples
+#
+# CONFIG_EXAMPLES_BUTTONS is not set
+# CONFIG_EXAMPLES_CAN is not set
+CONFIG_EXAMPLES_CDCACM=y
+# CONFIG_EXAMPLES_COMPOSITE is not set
+# CONFIG_EXAMPLES_CXXTEST is not set
+# CONFIG_EXAMPLES_DHCPD is not set
+# CONFIG_EXAMPLES_ELF is not set
+# CONFIG_EXAMPLES_FTPC is not set
+# CONFIG_EXAMPLES_FTPD is not set
+# CONFIG_EXAMPLES_HELLO is not set
+# CONFIG_EXAMPLES_HELLOXX is not set
+# CONFIG_EXAMPLES_JSON is not set
+# CONFIG_EXAMPLES_HIDKBD is not set
+# CONFIG_EXAMPLES_KEYPADTEST is not set
+# CONFIG_EXAMPLES_IGMP is not set
+# CONFIG_EXAMPLES_LCDRW is not set
+# CONFIG_EXAMPLES_MM is not set
+# CONFIG_EXAMPLES_MODBUS is not set
+CONFIG_EXAMPLES_MOUNT=y
+# CONFIG_EXAMPLES_NRF24L01TERM is not set
+CONFIG_EXAMPLES_NSH=y
+# CONFIG_EXAMPLES_NULL is not set
+# CONFIG_EXAMPLES_NX is not set
+# CONFIG_EXAMPLES_NXCONSOLE is not set
+# CONFIG_EXAMPLES_NXFFS is not set
+# CONFIG_EXAMPLES_NXFLAT is not set
+# CONFIG_EXAMPLES_NXHELLO is not set
+# CONFIG_EXAMPLES_NXIMAGE is not set
+# CONFIG_EXAMPLES_NXLINES is not set
+# CONFIG_EXAMPLES_NXTEXT is not set
+# CONFIG_EXAMPLES_OSTEST is not set
+# CONFIG_EXAMPLES_PASHELLO is not set
+# CONFIG_EXAMPLES_PIPE is not set
+# CONFIG_EXAMPLES_POSIXSPAWN is not set
+# CONFIG_EXAMPLES_QENCODER is not set
+# CONFIG_EXAMPLES_RGMP is not set
+# CONFIG_EXAMPLES_ROMFS is not set
+# CONFIG_EXAMPLES_SENDMAIL is not set
+# CONFIG_EXAMPLES_SERLOOP is not set
+# CONFIG_EXAMPLES_SLCD is not set
+# CONFIG_EXAMPLES_SMART_TEST is not set
+# CONFIG_EXAMPLES_SMART is not set
+# CONFIG_EXAMPLES_TCPECHO is not set
+# CONFIG_EXAMPLES_TELNETD is not set
+# CONFIG_EXAMPLES_THTTPD is not set
+# CONFIG_EXAMPLES_TIFF is not set
+# CONFIG_EXAMPLES_TOUCHSCREEN is not set
+# CONFIG_EXAMPLES_UDP is not set
+# CONFIG_EXAMPLES_UIP is not set
+# CONFIG_EXAMPLES_USBSERIAL is not set
+# CONFIG_EXAMPLES_USBMSC is not set
+# CONFIG_EXAMPLES_USBTERM is not set
+# CONFIG_EXAMPLES_WATCHDOG is not set
+
+#
+# Graphics Support
+#
+# CONFIG_TIFF is not set
+
+#
+# Interpreters
+#
+# CONFIG_INTERPRETERS_FICL is not set
+# CONFIG_INTERPRETERS_PCODE is not set
+
+#
+# Network Utilities
+#
+
+#
+# Networking Utilities
+#
+# CONFIG_NETUTILS_CODECS is not set
+# CONFIG_NETUTILS_DHCPC is not set
+# CONFIG_NETUTILS_DHCPD is not set
+# CONFIG_NETUTILS_FTPC is not set
+# CONFIG_NETUTILS_FTPD is not set
+# CONFIG_NETUTILS_JSON is not set
+# CONFIG_NETUTILS_RESOLV is not set
+# CONFIG_NETUTILS_SMTP is not set
+# CONFIG_NETUTILS_TELNETD is not set
+# CONFIG_NETUTILS_TFTPC is not set
+# CONFIG_NETUTILS_THTTPD is not set
+# CONFIG_NETUTILS_UIPLIB is not set
+# CONFIG_NETUTILS_WEBCLIENT is not set
+
+#
+# FreeModBus
+#
+# CONFIG_MODBUS is not set
+
+#
+# NSH Library
+#
+CONFIG_NSH_LIBRARY=y
+CONFIG_NSH_BUILTIN_APPS=y
+
+#
+# Disable Individual commands
+#
+# CONFIG_NSH_DISABLE_CAT is not set
+# CONFIG_NSH_DISABLE_CD is not set
+# CONFIG_NSH_DISABLE_CP is not set
+# CONFIG_NSH_DISABLE_DD is not set
+# CONFIG_NSH_DISABLE_ECHO is not set
+# CONFIG_NSH_DISABLE_EXEC is not set
+# CONFIG_NSH_DISABLE_EXIT is not set
+# CONFIG_NSH_DISABLE_FREE is not set
+# CONFIG_NSH_DISABLE_GET is not set
+# CONFIG_NSH_DISABLE_HELP is not set
+# CONFIG_NSH_DISABLE_HEXDUMP is not set
+CONFIG_NSH_DISABLE_IFCONFIG=y
+# CONFIG_NSH_DISABLE_KILL is not set
+CONFIG_NSH_DISABLE_LOSETUP=y
+# CONFIG_NSH_DISABLE_LS is not set
+# CONFIG_NSH_DISABLE_MB is not set
+# CONFIG_NSH_DISABLE_MKDIR is not set
+# CONFIG_NSH_DISABLE_MKFATFS is not set
+# CONFIG_NSH_DISABLE_MKFIFO is not set
+# CONFIG_NSH_DISABLE_MKRD is not set
+# CONFIG_NSH_DISABLE_MH is not set
+# CONFIG_NSH_DISABLE_MOUNT is not set
+# CONFIG_NSH_DISABLE_MW is not set
+CONFIG_NSH_DISABLE_NSFMOUNT=y
+# CONFIG_NSH_DISABLE_PS is not set
+CONFIG_NSH_DISABLE_PING=y
+CONFIG_NSH_DISABLE_PUT=y
+# CONFIG_NSH_DISABLE_PWD is not set
+# CONFIG_NSH_DISABLE_RM is not set
+# CONFIG_NSH_DISABLE_RMDIR is not set
+# CONFIG_NSH_DISABLE_SET is not set
+# CONFIG_NSH_DISABLE_SH is not set
+# CONFIG_NSH_DISABLE_SLEEP is not set
+# CONFIG_NSH_DISABLE_TEST is not set
+# CONFIG_NSH_DISABLE_UMOUNT is not set
+# CONFIG_NSH_DISABLE_UNSET is not set
+# CONFIG_NSH_DISABLE_USLEEP is not set
+CONFIG_NSH_DISABLE_WGET=y
+# CONFIG_NSH_DISABLE_XD is not set
+
+#
+# Configure Command Options
+#
+# CONFIG_NSH_CMDOPT_DF_H is not set
+CONFIG_NSH_CODECS_BUFSIZE=128
+CONFIG_NSH_FILEIOSIZE=512
+CONFIG_NSH_STRERROR=y
+CONFIG_NSH_LINELEN=128
+CONFIG_NSH_MAXARGUMENTS=12
+CONFIG_NSH_NESTDEPTH=8
+# CONFIG_NSH_DISABLESCRIPT is not set
+# CONFIG_NSH_DISABLEBG is not set
+CONFIG_NSH_ROMFSETC=y
+# CONFIG_NSH_ROMFSRC is not set
+CONFIG_NSH_ROMFSMOUNTPT="/etc"
+CONFIG_NSH_INITSCRIPT="init.d/rcS"
+CONFIG_NSH_ROMFSDEVNO=0
+CONFIG_NSH_ROMFSSECTSIZE=128
+CONFIG_NSH_ARCHROMFS=y
+CONFIG_NSH_FATDEVNO=1
+CONFIG_NSH_FATSECTSIZE=512
+CONFIG_NSH_FATNSECTORS=1024
+CONFIG_NSH_FATMOUNTPT="/tmp"
+CONFIG_NSH_CONSOLE=y
+# CONFIG_NSH_USBCONSOLE is not set
+
+#
+# USB Trace Support
+#
+# CONFIG_NSH_CONDEV is not set
+CONFIG_NSH_ARCHINIT=y
+
+#
+# NxWidgets/NxWM
+#
+
+#
+# System NSH Add-Ons
+#
+
+#
+# Custom Free Memory Command
+#
+# CONFIG_SYSTEM_FREE is not set
+
+#
+# I2C tool
+#
+# CONFIG_SYSTEM_I2CTOOL is not set
+
+#
+# FLASH Program Installation
+#
+# CONFIG_SYSTEM_INSTALL is not set
+
+#
+# FLASH Erase-all Command
+#
+
+#
+# readline()
+#
+CONFIG_SYSTEM_READLINE=y
+CONFIG_READLINE_ECHO=y
+
+#
+# Power Off
+#
+# CONFIG_SYSTEM_POWEROFF is not set
+
+#
+# RAMTRON
+#
+# CONFIG_SYSTEM_RAMTRON is not set
+
+#
+# SD Card
+#
+# CONFIG_SYSTEM_SDCARD is not set
+
+#
+# Sysinfo
+#
+CONFIG_SYSTEM_SYSINFO=y
+
+#
+# USB Monitor
+#
diff --git a/nuttx-configs/tap-v1/nsh/setenv.sh b/nuttx-configs/tap-v1/nsh/setenv.sh
new file mode 100755
index 0000000000..db372217cd
--- /dev/null
+++ b/nuttx-configs/tap-v1/nsh/setenv.sh
@@ -0,0 +1,75 @@
+#!/bin/bash
+# configs/px4fmu-v1/usbnsh/setenv.sh
+#
+#   Copyright (C) 2013 Gregory Nutt. All rights reserved.
+#   Author: Gregory Nutt <gnutt@nuttx.org>
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions
+# are met:
+#
+# 1. Redistributions of source code must retain the above copyright
+#    notice, this list of conditions and the following disclaimer.
+# 2. Redistributions in binary form must reproduce the above copyright
+#    notice, this list of conditions and the following disclaimer in
+#    the documentation and/or other materials provided with the
+#    distribution.
+# 3. Neither the name NuttX nor the names of its contributors may be
+#    used to endorse or promote products derived from this software
+#    without specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.
+#
+
+if [ "$_" = "$0" ] ; then
+  echo "You must source this script, not run it!" 1>&2
+  exit 1
+fi
+
+WD=`pwd`
+if [ ! -x "setenv.sh" ]; then
+  echo "This script must be executed from the top-level NuttX build directory"
+  exit 1
+fi
+
+if [ -z "${PATH_ORIG}" ]; then
+  export PATH_ORIG="${PATH}"
+fi
+
+# This is the Cygwin path to the location where I installed the RIDE
+# toolchain under windows.  You will also have to edit this if you install
+# the RIDE toolchain in any other location
+#export TOOLCHAIN_BIN="/cygdrive/c/Program Files (x86)/Raisonance/Ride/arm-gcc/bin"
+
+# This is the Cygwin path to the location where I installed the CodeSourcery
+# toolchain under windows.  You will also have to edit this if you install
+# the CodeSourcery toolchain in any other location
+export TOOLCHAIN_BIN="/cygdrive/c/Program Files (x86)/CodeSourcery/Sourcery G++ Lite/bin"
+
+# These are the Cygwin paths to the locations where I installed the Atollic
+# toolchain under windows.  You will also have to edit this if you install
+# the Atollic toolchain in any other location.  /usr/bin is added before
+# the Atollic bin path because there is are binaries named gcc.exe and g++.exe
+# at those locations as well.
+#export TOOLCHAIN_BIN="/usr/bin:/cygdrive/c/Program Files (x86)/Atollic/TrueSTUDIO for ARM Pro 2.3.0/ARMTools/bin"
+#export TOOLCHAIN_BIN="/usr/bin:/cygdrive/c/Program Files (x86)/Atollic/TrueSTUDIO for STMicroelectronics STM32 Lite 2.3.0/ARMTools/bin"
+
+# This is the Cygwin path to the location where I build the buildroot
+# toolchain.
+#export TOOLCHAIN_BIN="${WD}/../misc/buildroot/build_arm_nofpu/staging_dir/bin"
+
+# Add the path to the toolchain to the PATH varialble
+export PATH="${TOOLCHAIN_BIN}:/sbin:/usr/sbin:${PATH_ORIG}"
+
+echo "PATH : ${PATH}"
diff --git a/nuttx-configs/tap-v1/scripts/ld.script b/nuttx-configs/tap-v1/scripts/ld.script
new file mode 100644
index 0000000000..4cf52e0a4b
--- /dev/null
+++ b/nuttx-configs/tap-v1/scripts/ld.script
@@ -0,0 +1,149 @@
+/****************************************************************************
+ * configs/tap-v1/scripts/ld.script
+ *
+ *   Copyright (C) 2011 Gregory Nutt. All rights reserved.
+ *   Author: Gregory Nutt <gnutt@nuttx.org>
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name NuttX nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/* The STM32F405RG has 1024Kb of FLASH beginning at address 0x0800:0000 and
+ * 192Kb of SRAM. SRAM is split up into three blocks:
+ *
+ * 1) 112Kb of SRAM beginning at address 0x2000:0000
+ * 2)  16Kb of SRAM beginning at address 0x2001:c000
+ * 3)  64Kb of CCM SRAM beginning at address 0x1000:0000
+ *
+ * When booting from FLASH, FLASH memory is aliased to address 0x0000:0000
+ * where the code expects to begin execution by jumping to the entry point in
+ * the 0x0800:0000 address range.
+ *
+ * The first 0x4000 of flash is reserved for the bootloader.
+ */
+
+MEMORY
+{
+    flash (rx)   : ORIGIN = 0x08004000, LENGTH = 1008K
+    sram (rwx)   : ORIGIN = 0x20000000, LENGTH = 128K
+    ccsram (rwx) : ORIGIN = 0x10000000, LENGTH = 64K
+}
+
+OUTPUT_ARCH(arm)
+
+ENTRY(__start)		/* treat __start as the anchor for dead code stripping */
+EXTERN(_vectors)	/* force the vectors to be included in the output */
+
+/*
+ * Ensure that abort() is present in the final object.  The exception handling
+ * code pulled in by libgcc.a requires it (and that code cannot be easily avoided).
+ */
+EXTERN(abort)
+
+SECTIONS
+{
+	.text : {
+		_stext = ABSOLUTE(.);
+		*(.vectors)
+		*(.text .text.*)
+		*(.fixup)
+		*(.gnu.warning)
+		*(.rodata .rodata.*)
+		*(.gnu.linkonce.t.*)
+		*(.got)
+		*(.gcc_except_table)
+		*(.gnu.linkonce.r.*)
+		_etext = ABSOLUTE(.);
+
+		/*
+		 * This is a hack to make the newlib libm __errno() call
+		 * use the NuttX get_errno_ptr() function.
+		 */
+		__errno = get_errno_ptr;
+	} > flash
+
+	/*
+	 * Init functions (static constructors and the like)
+	 */
+        .init_section : {
+                _sinit = ABSOLUTE(.);
+                KEEP(*(.init_array .init_array.*))
+                _einit = ABSOLUTE(.);
+        } > flash
+
+	/*
+	 * Construction data for parameters.
+	 */
+	__param ALIGN(4): {
+		__param_start = ABSOLUTE(.);
+		KEEP(*(__param*))
+		__param_end = ABSOLUTE(.);
+	} > flash
+
+	.ARM.extab : {
+		*(.ARM.extab*)
+	} > flash
+
+	__exidx_start = ABSOLUTE(.);
+	.ARM.exidx : {
+		*(.ARM.exidx*)
+	} > flash
+	__exidx_end = ABSOLUTE(.);
+
+	_eronly = ABSOLUTE(.);
+
+	.data : {
+		_sdata = ABSOLUTE(.);
+		*(.data .data.*)
+		*(.gnu.linkonce.d.*)
+		CONSTRUCTORS
+		_edata = ABSOLUTE(.);
+	} > sram AT > flash
+
+	.bss : {
+		_sbss = ABSOLUTE(.);
+		*(.bss .bss.*)
+		*(.gnu.linkonce.b.*)
+		*(COMMON)
+		_ebss = ABSOLUTE(.);
+	} > sram
+
+	/* Stabs debugging sections. */
+	.stab 0 : { *(.stab) }
+	.stabstr 0 : { *(.stabstr) }
+	.stab.excl 0 : { *(.stab.excl) }
+	.stab.exclstr 0 : { *(.stab.exclstr) }
+	.stab.index 0 : { *(.stab.index) }
+	.stab.indexstr 0 : { *(.stab.indexstr) }
+	.comment 0 : { *(.comment) }
+	.debug_abbrev 0 : { *(.debug_abbrev) }
+	.debug_info 0 : { *(.debug_info) }
+	.debug_line 0 : { *(.debug_line) }
+	.debug_pubnames 0 : { *(.debug_pubnames) }
+	.debug_aranges 0 : { *(.debug_aranges) }
+}
diff --git a/nuttx-configs/tap-v1/src/Makefile b/nuttx-configs/tap-v1/src/Makefile
new file mode 100644
index 0000000000..8f86d0ada1
--- /dev/null
+++ b/nuttx-configs/tap-v1/src/Makefile
@@ -0,0 +1,84 @@
+############################################################################
+# configs/tap-v1/src/Makefile
+#
+#   Copyright (C) 2013 Gregory Nutt. All rights reserved.
+#   Author: Gregory Nutt <gnutt@nuttx.org>
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions
+# are met:
+#
+# 1. Redistributions of source code must retain the above copyright
+#    notice, this list of conditions and the following disclaimer.
+# 2. Redistributions in binary form must reproduce the above copyright
+#    notice, this list of conditions and the following disclaimer in
+#    the documentation and/or other materials provided with the
+#    distribution.
+# 3. Neither the name NuttX nor the names of its contributors may be
+#    used to endorse or promote products derived from this software
+#    without specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.
+#
+############################################################################
+
+-include $(TOPDIR)/Make.defs
+
+CFLAGS		+= -I$(TOPDIR)/sched
+
+ASRCS		=
+AOBJS		= $(ASRCS:.S=$(OBJEXT))
+
+CSRCS		= empty.c
+COBJS		= $(CSRCS:.c=$(OBJEXT))
+
+SRCS		= $(ASRCS) $(CSRCS)
+OBJS		= $(AOBJS) $(COBJS)
+
+ARCH_SRCDIR	= $(TOPDIR)/arch/$(CONFIG_ARCH)/src
+ifeq ($(WINTOOL),y)
+  CFLAGS	+= -I "${shell cygpath -w $(ARCH_SRCDIR)/chip}" \
+             -I "${shell cygpath -w $(ARCH_SRCDIR)/common}" \
+             -I "${shell cygpath -w $(ARCH_SRCDIR)/armv7-m}"
+else
+  CFLAGS	+= -I$(ARCH_SRCDIR)/chip -I$(ARCH_SRCDIR)/common -I$(ARCH_SRCDIR)/armv7-m
+endif
+
+all: libboard$(LIBEXT)
+
+$(AOBJS): %$(OBJEXT): %.S
+	$(call ASSEMBLE, $<, $@)
+
+$(COBJS) $(LINKOBJS): %$(OBJEXT): %.c
+	$(call COMPILE, $<, $@)
+
+libboard$(LIBEXT): $(OBJS)
+	$(call ARCHIVE, $@, $(OBJS))
+
+.depend: Makefile $(SRCS)
+	$(Q) $(MKDEP) $(CC) -- $(CFLAGS) -- $(SRCS) >Make.dep
+	$(Q) touch $@
+
+depend: .depend
+
+clean:
+	$(call DELFILE, libboard$(LIBEXT))
+	$(call CLEAN)
+
+distclean: clean
+	$(call DELFILE, Make.dep)
+	$(call DELFILE, .depend)
+
+-include Make.dep
+
diff --git a/nuttx-configs/tap-v1/src/empty.c b/nuttx-configs/tap-v1/src/empty.c
new file mode 100644
index 0000000000..5de10699fb
--- /dev/null
+++ b/nuttx-configs/tap-v1/src/empty.c
@@ -0,0 +1,4 @@
+/*
+ * There are no source files here, but libboard.a can't be empty, so
+ * we have this empty source file to keep it company.
+ */
diff --git a/src/drivers/boards/tap-v1/CMakeLists.txt b/src/drivers/boards/tap-v1/CMakeLists.txt
new file mode 100644
index 0000000000..ca3baa3e31
--- /dev/null
+++ b/src/drivers/boards/tap-v1/CMakeLists.txt
@@ -0,0 +1,48 @@
+############################################################################
+#
+#   Copyright (c) 2016 PX4 Development Team. All rights reserved.
+#         Author: David Sidrane <david_s5@nscdg.com>
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions
+# are met:
+#
+# 1. Redistributions of source code must retain the above copyright
+#    notice, this list of conditions and the following disclaimer.
+# 2. Redistributions in binary form must reproduce the above copyright
+#    notice, this list of conditions and the following disclaimer in
+#    the documentation and/or other materials provided with the
+#    distribution.
+# 3. Neither the name PX4 nor the names of its contributors may be
+#    used to endorse or promote products derived from this software
+#    without specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.
+#
+############################################################################
+px4_add_module(
+	MODULE drivers__boards__tap-v1
+	COMPILE_FLAGS
+		-Os
+	SRCS
+		../common/board_name.c
+		tap_init.c
+		tap_timer_config.c
+		tap_spi.c
+		tap_usb.c
+		tap_led.c
+	DEPENDS
+		platforms__common
+	)
+# vim: set noet ft=cmake fenc=utf-8 ff=unix : 
diff --git a/src/drivers/boards/tap-v1/board_config.h b/src/drivers/boards/tap-v1/board_config.h
new file mode 100644
index 0000000000..0111e20114
--- /dev/null
+++ b/src/drivers/boards/tap-v1/board_config.h
@@ -0,0 +1,276 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2012-2016 PX4 Development Team. All rights reserved.
+ *         Author: David Sidrane <david_s5@nscdg.com>
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file board_config.h
+ *
+ * TAP_V1 internal definitions
+ */
+
+#pragma once
+
+/****************************************************************************************************
+ * Included Files
+ ****************************************************************************************************/
+
+#include <px4_config.h>
+#include <nuttx/compiler.h>
+#include <stdint.h>
+
+__BEGIN_DECLS
+
+/* these headers are not C++ safe */
+#include <stm32.h>
+#include <arch/board/board.h>
+
+/****************************************************************************************************
+ * Definitions
+ ****************************************************************************************************/
+/* Configuration ************************************************************************************/
+
+#define UDID_START		0x1FFF7A10
+
+/* PX4FMU GPIOs ***********************************************************************************/
+/* LEDs
+ *
+ * PC4     BLUE_LED                  D4 Blue LED cathode
+ * PC5     RED_LED                   D5 Red LED cathode
+*/
+
+#define GPIO_LED1		(GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTC|GPIO_PIN4)
+#define GPIO_LED2		(GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTC|GPIO_PIN5)
+#define GPIO_BLUE_LED GPIO_LED1
+#define GPIO_RED_LED  GPIO_LED1
+/*
+ * SPI
+ *
+ * Peripheral   Port     Signal Name               CONN
+ * SPI2_NSS       PB12    SD_SPI2_NSS               SD-2 CS
+ * SPI2_SCK       PB13    SD_SPI2_SCK               SD-5 CLK
+ * SPI2_MISO      PB14    SD_SPI2_MISO              SD-7 D0
+ * SPI2_MOSI      PB15    SD_SPI2_MOSI              SD-3 DI
+ *
+ *                PC2     SD_SW                     SD-9 SW
+ *
+ */
+#define GPIO_SPI_CS_SDCARD	(GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN12)
+#define GPIO_SPI_SD_SW	    (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTC|GPIO_PIN2)
+
+
+/*
+ * I2C busses
+ *
+ * Peripheral   Port     Signal Name               CONN
+ * I2C1_SDA     PB9     I2C1_SDA                  J2-4,9,16,21 mpu6050, U4 MS6507
+ * I2C1_SDL     PB8     I2C1_SCL                  J2-3,10,15,22 mpu6050, U4 MS6507
+ *
+ * I2C2_SDA     PB11    Sonar Echo/I2C_SDA        JP2-31,32
+ * I2C2_SDL     PB10    Sonar Trig/I2C_SCL        JP2-29,30
+ *
+ * I2C3_SDA     PC9     COMPASS_I2C3_SDA          JP1-27,28
+ * I2C3_SDL     PA8     COMPASS_I2C3_SCL          JP1-25,26
+ *
+ */
+#define PX4_I2C_BUS_ONBOARD    1
+#define PX4_I2C_BUS_SONAR      2
+#define PX4_I2C_BUS_EXPANSION  3
+
+/*
+ * Devices on the onboard bus.
+ *
+ * Note that these are unshifted addresses (not includinf R/W).
+ */
+
+/* todo:
+ * Cannot tell from the schematic if there is one or 2 MPU6050
+ * The slave address of the MPU-60X0 is b110100X which is 7 bits long.
+ * The LSB bit of the 7 bit address is determined by the logic level
+ * on pin AD0. This allows two MPU-60X0s to be connected to the same I2C bus.
+ * When used in this configuration, the address of the one of the devices
+ * should be b1101000 (pin AD0 is logic low) and the address of the other
+ * should be b1101001 (pin AD0 is logic high).
+ */
+#define PX4_I2C_ON_BOARD_MPU6050_ADDRS {0x68,0x69}
+
+
+/*
+ * ADC channels
+ *
+ * These are the channel numbers of the ADCs of the microcontroller that can be used by the Px4 Firmware in the adc driver
+ */
+#define ADC_CHANNELS (1 << 10)
+/* todo:Revisit - cannnot tell from schematic - some could be ADC */
+
+// ADC defines to be used in sensors.cpp to read from a particular channel
+#define ADC_BATTERY_VOLTAGE_CHANNEL	10
+#define ADC_BATTERY_CURRENT_CHANNEL	((uint8_t)(-1))
+
+
+/* User GPIOs
+ *
+ * TIM3_CH1     PA6     LED_R                     JP2-23,24
+ * TIM3_CH2     PA7     LED_G                     JP2-25,26
+ * TIM3_CH3     PB0     LED_B                     JP2-27,28
+ * TIM3_CH4     PB1     nPWM_1 AUX1(Landing Gear) JP1-21,22
+ *
+ * I2C2_SDA     PB11    Sonar Echo/I2C_SDA        JP2-31,32
+ * I2C2_SDL     PB10    Sonar Trig/I2C_SCL        JP2-29,30
+ *
+ */
+#define GPIO_GPIO0_INPUT	(GPIO_INPUT|GPIO_PULLUP|GPIO_PORTA|GPIO_PIN6)
+#define GPIO_GPIO1_INPUT	(GPIO_INPUT|GPIO_PULLUP|GPIO_PORTA|GPIO_PIN7)
+#define GPIO_GPIO2_INPUT	(GPIO_INPUT|GPIO_PULLUP|GPIO_PORTB|GPIO_PIN0)
+#define GPIO_GPIO3_INPUT	(GPIO_INPUT|GPIO_PULLUP|GPIO_PORTB|GPIO_PIN1)
+#define GPIO_GPIO4_INPUT	(GPIO_INPUT|GPIO_PULLUP|GPIO_PORTB|GPIO_PIN10)
+#define GPIO_GPIO5_INPUT	(GPIO_INPUT|GPIO_PULLUP|GPIO_PORTB|GPIO_PIN11)
+
+#define GPIO_GPIO0_OUTPUT	(GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTA|GPIO_PIN6)
+#define GPIO_GPIO1_OUTPUT	(GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTA|GPIO_PIN7)
+#define GPIO_GPIO2_OUTPUT	(GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN0)
+#define GPIO_GPIO3_OUTPUT	(GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN1)
+#define GPIO_GPIO4_OUTPUT	(GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN10)
+#define GPIO_GPIO5_OUTPUT	(GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN11)
+
+/*
+ * Tone alarm output
+ */
+/* todo:Revisit - cannnot tell from schematic - one could be tone alarm*/
+#define TONE_ALARM_TIMER	3	/* timer 3 */
+#define TONE_ALARM_CHANNEL	3	/* channel 3 */
+#define GPIO_TONE_ALARM_IDLE	(GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTC|GPIO_PIN8)
+#define GPIO_TONE_ALARM		(GPIO_ALT|GPIO_AF2|GPIO_SPEED_2MHz|GPIO_FLOAT|GPIO_PUSHPULL|GPIO_PORTC|GPIO_PIN8)
+
+/*
+ * PWM
+ *
+ * Four PWM outputs can be configured on pins
+ *
+ *
+ * Peripheral   Port     Signal Name               CONN
+ * TIM3_CH1     PA6     LED_R                     JP2-23,24
+ * TIM3_CH2     PA7     LED_G                     JP2-25,26
+ * TIM3_CH3     PB0     LED_B                     JP2-27,28
+ * TIM3_CH4     PB1     nPWM_1 AUX1(Landing Gear) JP1-21,22
+ *
+ */
+#define GPIO_TIM3_CH1OUT	GPIO_TIM3_CH1OUT_1
+#define GPIO_TIM3_CH2OUT	GPIO_TIM3_CH2OUT_1
+#define GPIO_TIM3_CH3OUT	GPIO_TIM3_CH3OUT_1
+#define GPIO_TIM3_CH4OUT	GPIO_TIM3_CH4OUT_1
+#define DIRECT_PWM_OUTPUT_CHANNELS	4
+
+/* USB OTG FS
+ *
+ * PA9  OTG_FS_VBUS VBUS sensing (also connected to the green LED)
+ */
+#define GPIO_OTGFS_VBUS (GPIO_INPUT|GPIO_FLOAT|GPIO_SPEED_100MHz|GPIO_OPENDRAIN|GPIO_PORTA|GPIO_PIN9)
+
+/* High-resolution timer
+ */
+#define HRT_TIMER           1  /* use timer1 for the HRT */
+#define HRT_TIMER_CHANNEL   1  /* use capture/compare channel */
+
+#define	BOARD_NAME "TAP_V1"
+
+/* By Providing BOARD_ADC_USB_CONNECTED this board support the ADC
+ * system_power interface, and herefore provides the true logic
+ * GPIO BOARD_ADC_xxxx macros.
+ */
+#define BOARD_ADC_USB_CONNECTED (px4_arch_gpioread(GPIO_OTGFS_VBUS))
+#define BOARD_ADC_BRICK_VALID   (1)
+#define BOARD_ADC_SERVO_VALID   (1)
+#define BOARD_ADC_PERIPH_5V_OC  (0)
+#define BOARD_ADC_HIPOWER_5V_OC (0)
+
+#define BOARD_HAS_PWM	DIRECT_PWM_OUTPUT_CHANNELS
+
+#define BOARD_FMU_GPIO_TAB { \
+		{GPIO_GPIO0_INPUT,       GPIO_GPIO0_OUTPUT,       0}, \
+		{GPIO_GPIO1_INPUT,       GPIO_GPIO1_OUTPUT,       0}, \
+		{GPIO_GPIO2_INPUT,       GPIO_GPIO2_OUTPUT,       0}, \
+		{GPIO_GPIO3_INPUT,       GPIO_GPIO3_OUTPUT,       0}, \
+		{GPIO_GPIO4_INPUT,       GPIO_GPIO4_OUTPUT,       0}, \
+		{GPIO_GPIO5_INPUT,       GPIO_GPIO5_OUTPUT,       0}, }
+
+/****************************************************************************************************
+ * Public Types
+ ****************************************************************************************************/
+
+/****************************************************************************************************
+ * Public data
+ ****************************************************************************************************/
+
+#ifndef __ASSEMBLY__
+
+/****************************************************************************************************
+ * Public Functions
+ ****************************************************************************************************/
+
+/****************************************************************************************************
+ * Name: stm32_spiinitialize
+ *
+ * Description:
+ *   Called to configure SPI chip select GPIO pins for the PX4FMU board.
+ *
+ ****************************************************************************************************/
+
+extern void stm32_spiinitialize(void);
+
+#define board_spi_reset(ms)
+#define board_peripheral_reset(ms)
+
+extern void stm32_usbinitialize(void);
+
+/****************************************************************************
+ * Name: nsh_archinitialize
+ *
+ * Description:
+ *   Perform architecture specific initialization for NSH.
+ *
+ *   CONFIG_NSH_ARCHINIT=y :
+ *     Called from the NSH library
+ *
+ *   CONFIG_BOARD_INITIALIZE=y, CONFIG_NSH_LIBRARY=y, &&
+ *   CONFIG_NSH_ARCHINIT=n :
+ *     Called from board_initialize().
+ *
+ ****************************************************************************/
+
+#ifdef CONFIG_NSH_LIBRARY
+int nsh_archinitialize(void);
+#endif
+
+#endif /* __ASSEMBLY__ */
+
+__END_DECLS
diff --git a/src/drivers/boards/tap-v1/tap_init.c b/src/drivers/boards/tap-v1/tap_init.c
new file mode 100644
index 0000000000..9bcfcdabc2
--- /dev/null
+++ b/src/drivers/boards/tap-v1/tap_init.c
@@ -0,0 +1,204 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2012-2016 PX4 Development Team. All rights reserved.
+ *         Author: David Sidrane <david_s5@nscdg.com>
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file tap-v1_init.c
+ *
+ * tap-v1-specific early startup code.  This file implements the
+ * nsh_archinitialize() function that is called early by nsh during startup.
+ *
+ * Code here is run before the rcS script is invoked; it should start required
+ * subsystems and perform board-specific initialisation.
+ */
+
+/****************************************************************************
+ * Included Files
+ ****************************************************************************/
+
+#include <px4_config.h>
+
+#include <stdbool.h>
+#include <stdio.h>
+#include <debug.h>
+#include <errno.h>
+
+#include <nuttx/arch.h>
+#include <nuttx/spi.h>
+#include <nuttx/i2c.h>
+#include <nuttx/mmcsd.h>
+#include <nuttx/analog/adc.h>
+
+#include "stm32.h"
+#include "board_config.h"
+#include "stm32_uart.h"
+
+#include <arch/board/board.h>
+
+#include <drivers/drv_hrt.h>
+#include <drivers/drv_led.h>
+
+#include <systemlib/cpuload.h>
+
+/****************************************************************************
+ * Pre-Processor Definitions
+ ****************************************************************************/
+
+/* Configuration ************************************************************/
+
+/* Debug ********************************************************************/
+
+#ifdef CONFIG_CPP_HAVE_VARARGS
+#  ifdef CONFIG_DEBUG
+#    define message(...) lowsyslog(__VA_ARGS__)
+#  else
+#    define message(...) printf(__VA_ARGS__)
+#  endif
+#else
+#  ifdef CONFIG_DEBUG
+#    define message lowsyslog
+#  else
+#    define message printf
+#  endif
+#endif
+
+/*
+ * Ideally we'd be able to get these from up_internal.h,
+ * but since we want to be able to disable the NuttX use
+ * of leds for system indication at will and there is no
+ * separate switch, we need to build independent of the
+ * CONFIG_ARCH_LEDS configuration switch.
+ */
+__BEGIN_DECLS
+extern void led_init(void);
+extern void led_on(int led);
+extern void led_off(int led);
+__END_DECLS
+
+/****************************************************************************
+ * Protected Functions
+ ****************************************************************************/
+
+/****************************************************************************
+ * Public Functions
+ ****************************************************************************/
+
+/************************************************************************************
+ * Name: stm32_boardinitialize
+ *
+ * Description:
+ *   All STM32 architectures must provide the following entry point.  This entry point
+ *   is called early in the intitialization -- after all memory has been configured
+ *   and mapped but before any devices have been initialized.
+ *
+ ************************************************************************************/
+
+__EXPORT void stm32_boardinitialize(void)
+{
+	/* configure always-on ADC pins */
+	stm32_configgpio(GPIO_ADC1_IN10);
+
+	/* configure SPI interfaces */
+	stm32_spiinitialize();
+
+	/* configure LEDs (empty call to NuttX' ledinit) */
+	up_ledinit();
+}
+
+/****************************************************************************
+ * Name: nsh_archinitialize
+ *
+ * Description:
+ *   Perform architecture specific initialization
+ *
+ ****************************************************************************/
+
+static struct spi_dev_s *spi;
+
+#include <math.h>
+
+__EXPORT int nsh_archinitialize(void)
+{
+	int result;
+
+	/* configure the high-resolution time/callout interface */
+	hrt_init();
+
+	/* configure CPU load estimation */
+#ifdef CONFIG_SCHED_INSTRUMENTATION
+	cpuload_initialize_once();
+#endif
+
+	/* set up the serial DMA polling */
+	static struct hrt_call serial_dma_call;
+	struct timespec ts;
+
+	/*
+	 * Poll at 1ms intervals for received bytes that have not triggered
+	 * a DMA event.
+	 */
+	ts.tv_sec = 0;
+	ts.tv_nsec = 1000000;
+
+	hrt_call_every(&serial_dma_call,
+		       ts_to_abstime(&ts),
+		       ts_to_abstime(&ts),
+		       (hrt_callout)stm32_serial_dma_poll,
+		       NULL);
+
+	/* initial LED state */
+	drv_led_start();
+	led_off(LED_AMBER);
+	led_off(LED_BLUE);
+
+	/* Get the SPI port for the microSD slot */
+
+	spi = up_spiinitialize(CONFIG_NSH_MMCSDSPIPORTNO);
+
+	if (!spi) {
+		message("[boot] FAILED to initialize SPI port %d\n", CONFIG_NSH_MMCSDSPIPORTNO);
+		up_ledon(LED_AMBER);
+		return -ENODEV;
+	}
+
+	/* Now bind the SPI interface to the MMCSD driver */
+	result = mmcsd_spislotinitialize(CONFIG_NSH_MMCSDMINOR, CONFIG_NSH_MMCSDSLOTNO, spi);
+
+	if (result != OK) {
+		message("[boot] FAILED to bind SPI port 2 to the MMCSD driver\n");
+		up_ledon(LED_AMBER);
+		return -ENODEV;
+	}
+
+	return OK;
+}
diff --git a/src/drivers/boards/tap-v1/tap_led.c b/src/drivers/boards/tap-v1/tap_led.c
new file mode 100644
index 0000000000..d39e8f4a84
--- /dev/null
+++ b/src/drivers/boards/tap-v1/tap_led.c
@@ -0,0 +1,117 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2012-2016 PX4 Development Team. All rights reserved.
+ *         Author: David Sidrane <david_s5@nscdg.com>
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file tap-v1_led.c
+ *
+ * TAP_V1 LED backend.
+ */
+
+#include <px4_config.h>
+
+#include <stdbool.h>
+
+#include "stm32.h"
+#include "board_config.h"
+
+#include <arch/board/board.h>
+
+/*
+ * Ideally we'd be able to get these from up_internal.h,
+ * but since we want to be able to disable the NuttX use
+ * of leds for system indication at will and there is no
+ * separate switch, we need to build independent of the
+ * CONFIG_ARCH_LEDS configuration switch.
+ */
+__BEGIN_DECLS
+extern void led_init(void);
+extern void led_on(int led);
+extern void led_off(int led);
+extern void led_toggle(int led);
+__END_DECLS
+
+__EXPORT void led_init(void)
+{
+	/* Configure LED1-2 GPIOs for output */
+
+	stm32_configgpio(GPIO_BLUE_LED);
+	stm32_configgpio(GPIO_RED_LED);
+}
+
+__EXPORT void led_on(int led)
+{
+	if (led == 0) {
+		/* Pull down to switch on */
+		stm32_gpiowrite(GPIO_BLUE_LED, false);
+	}
+
+	if (led == 1) {
+		/* Pull down to switch on */
+		stm32_gpiowrite(GPIO_RED_LED, false);
+	}
+}
+
+__EXPORT void led_off(int led)
+{
+	if (led == 0) {
+		/* Pull up to switch off */
+		stm32_gpiowrite(GPIO_BLUE_LED, true);
+	}
+
+	if (led == 1) {
+		/* Pull up to switch off */
+		stm32_gpiowrite(GPIO_RED_LED, true);
+	}
+}
+
+__EXPORT void led_toggle(int led)
+{
+	if (led == 0) {
+		if (stm32_gpioread(GPIO_BLUE_LED)) {
+			stm32_gpiowrite(GPIO_BLUE_LED, false);
+
+		} else {
+			stm32_gpiowrite(GPIO_BLUE_LED, true);
+		}
+	}
+
+	if (led == 1) {
+		if (stm32_gpioread(GPIO_RED_LED)) {
+			stm32_gpiowrite(GPIO_RED_LED, false);
+
+		} else {
+			stm32_gpiowrite(GPIO_RED_LED, true);
+		}
+	}
+}
diff --git a/src/drivers/boards/tap-v1/tap_spi.c b/src/drivers/boards/tap-v1/tap_spi.c
new file mode 100644
index 0000000000..87f4df5110
--- /dev/null
+++ b/src/drivers/boards/tap-v1/tap_spi.c
@@ -0,0 +1,88 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2012-2016 PX4 Development Team. All rights reserved.
+ *         Author: David Sidrane <david_s5@nscdg.com>
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file tap-v1_spi.c
+ *
+ * Board-specific SPI functions.
+ */
+
+/************************************************************************************
+ * Included Files
+ ************************************************************************************/
+
+#include <px4_config.h>
+
+#include <stdint.h>
+#include <stdbool.h>
+#include <debug.h>
+
+#include <nuttx/spi.h>
+#include <arch/board/board.h>
+
+#include "up_arch.h"
+#include "chip.h"
+#include "stm32.h"
+#include "board_config.h"
+
+/************************************************************************************
+ * Public Functions
+ ************************************************************************************/
+
+/************************************************************************************
+ * Name: stm32_spiinitialize
+ *
+ * Description:
+ *   Called to configure SPI chip select GPIO pins for the tap-v1 board.
+ *
+ ************************************************************************************/
+
+__EXPORT void stm32_spiinitialize(void)
+{
+	stm32_configgpio(GPIO_SPI_CS_SDCARD);
+	stm32_configgpio(GPIO_SPI_SD_SW);
+}
+
+
+__EXPORT void stm32_spi2select(FAR struct spi_dev_s *dev, enum spi_dev_e devid, bool selected)
+{
+	/* there can only be one device on this bus, so always select it */
+	stm32_gpiowrite(GPIO_SPI_CS_SDCARD, !selected);
+}
+
+__EXPORT uint8_t stm32_spi2status(FAR struct spi_dev_s *dev, enum spi_dev_e devid)
+{
+	return stm32_gpioread(GPIO_SPI_SD_SW);
+}
+
diff --git a/src/drivers/boards/tap-v1/tap_timer_config.c b/src/drivers/boards/tap-v1/tap_timer_config.c
new file mode 100644
index 0000000000..e59fdfe641
--- /dev/null
+++ b/src/drivers/boards/tap-v1/tap_timer_config.c
@@ -0,0 +1,100 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2012-2016 PX4 Development Team. All rights reserved.
+ *         Author: David Sidrane <david_s5@nscdg.com>
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/*
+ * @file tap-v1_timer_config.c
+ *
+ * Configuration data for the stm32 pwm_servo, input capture and pwm input driver.
+ *
+ * Note that these arrays must always be fully-sized.
+ */
+
+#include <stdint.h>
+
+#include <stm32.h>
+#include <stm32_gpio.h>
+#include <stm32_tim.h>
+
+#include <drivers/drv_pwm_output.h>
+#include <drivers/stm32/drv_io_timer.h>
+
+#include "board_config.h"
+
+__EXPORT const io_timers_t io_timers[MAX_IO_TIMERS] = {
+	{
+		.base = STM32_TIM3_BASE,
+		.clock_register = STM32_RCC_APB1ENR,
+		.clock_bit = RCC_APB1ENR_TIM3EN,
+		.clock_freq = STM32_APB1_TIM3_CLKIN,
+		.first_channel_index = 0,
+		.last_channel_index = 3,
+		.handler = io_timer_handler0,
+		.vectorno =  STM32_IRQ_TIM3,
+	}
+};
+
+__EXPORT const timer_io_channels_t timer_io_channels[MAX_TIMER_IO_CHANNELS] = {
+	{
+		.gpio_out = GPIO_TIM3_CH1OUT,
+		.gpio_in = 0,
+		.timer_index = 0,
+		.timer_channel = 1,
+		.ccr_offset = STM32_GTIM_CCR1_OFFSET,
+		.masks  = GTIM_SR_CC1IF | GTIM_SR_CC1OF
+	},
+	{
+		.gpio_out = GPIO_TIM3_CH2OUT,
+		.gpio_in = 0,
+		.timer_index = 0,
+		.timer_channel = 2,
+		.ccr_offset = STM32_GTIM_CCR2_OFFSET,
+		.masks  = GTIM_SR_CC2IF | GTIM_SR_CC2OF
+	},
+	{
+		.gpio_out = GPIO_TIM3_CH3OUT,
+		.gpio_in = 0,
+		.timer_index = 0,
+		.timer_channel = 3,
+		.ccr_offset = STM32_GTIM_CCR3_OFFSET,
+		.masks  = GTIM_SR_CC3IF | GTIM_SR_CC3OF
+	},
+	{
+		.gpio_out = GPIO_TIM3_CH4OUT,
+		.gpio_in = 0,
+		.timer_index = 0,
+		.timer_channel = 4,
+		.ccr_offset = STM32_GTIM_CCR4_OFFSET,
+		.masks  = GTIM_SR_CC4IF | GTIM_SR_CC4OF
+	}
+};
diff --git a/src/drivers/boards/tap-v1/tap_usb.c b/src/drivers/boards/tap-v1/tap_usb.c
new file mode 100644
index 0000000000..2afd1e44e4
--- /dev/null
+++ b/src/drivers/boards/tap-v1/tap_usb.c
@@ -0,0 +1,105 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2012-2016 PX4 Development Team. All rights reserved.
+ *         Author: David Sidrane <david_s5@nscdg.com>
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file tap-v1_usb.c
+ *
+ * Board-specific USB functions.
+ */
+
+/************************************************************************************
+ * Included Files
+ ************************************************************************************/
+
+#include <px4_config.h>
+
+#include <sys/types.h>
+#include <stdint.h>
+#include <stdbool.h>
+#include <debug.h>
+
+#include <nuttx/usb/usbdev.h>
+#include <nuttx/usb/usbdev_trace.h>
+
+#include "up_arch.h"
+#include "stm32.h"
+#include "board_config.h"
+
+/************************************************************************************
+ * Definitions
+ ************************************************************************************/
+
+/************************************************************************************
+ * Private Functions
+ ************************************************************************************/
+
+/************************************************************************************
+ * Public Functions
+ ************************************************************************************/
+
+/************************************************************************************
+ * Name: stm32_usbinitialize
+ *
+ * Description:
+ *   Called to setup USB-related GPIO pins for the tap-v1 board.
+ *
+ ************************************************************************************/
+
+__EXPORT void stm32_usbinitialize(void)
+{
+	/* The OTG FS has an internal soft pull-up */
+
+	/* Configure the OTG FS VBUS sensing GPIO, Power On, and Overcurrent GPIOs */
+
+#ifdef CONFIG_STM32_OTGFS
+	stm32_configgpio(GPIO_OTGFS_VBUS);
+#endif
+}
+
+/************************************************************************************
+ * Name:  stm32_usbsuspend
+ *
+ * Description:
+ *   Board logic must provide the stm32_usbsuspend logic if the USBDEV driver is
+ *   used.  This function is called whenever the USB enters or leaves suspend mode.
+ *   This is an opportunity for the board logic to shutdown clocks, power, etc.
+ *   while the USB is suspended.
+ *
+ ************************************************************************************/
+
+__EXPORT void stm32_usbsuspend(FAR struct usbdev_s *dev, bool resume)
+{
+	//ulldbg("resume: %d\n", resume);
+}
+
diff --git a/src/drivers/drv_gpio.h b/src/drivers/drv_gpio.h
index 856f604fec..6b600fbe2d 100644
--- a/src/drivers/drv_gpio.h
+++ b/src/drivers/drv_gpio.h
@@ -117,6 +117,24 @@
 
 #endif
 
+#ifdef CONFIG_ARCH_BOARD_TAP_V1
+/*
+ * PX4FMUv3 GPIO numbers.
+ *
+ * There are no alternate functions on this board.
+ */
+# define GPIO_SERVO_1			(1<<0)		/**< servo 1 output */
+# define GPIO_SERVO_2			(1<<1)		/**< servo 2 output */
+# define GPIO_SERVO_3			(1<<2)		/**< servo 3 output */
+# define GPIO_SERVO_4			(1<<3)		/**< servo 4 output */
+
+/**
+ * Device paths for things that support the GPIO ioctl protocol.
+ */
+# define PX4FMU_DEVICE_PATH	"/dev/px4fmu"
+
+#endif
+
 #ifdef CONFIG_ARCH_BOARD_AEROCORE
 /*
  * AeroCore GPIO numbers and configuration.
@@ -144,8 +162,9 @@
 #if !defined(CONFIG_ARCH_BOARD_PX4IO_V1) && !defined(CONFIG_ARCH_BOARD_PX4IO_V2)  && \
 	!defined(CONFIG_ARCH_BOARD_PX4FMU_V1) && !defined(CONFIG_ARCH_BOARD_PX4FMU_V2) && \
 	!defined(CONFIG_ARCH_BOARD_AEROCORE) && !defined(CONFIG_ARCH_BOARD_PX4_STM32F4DISCOVERY) && \
-	!defined(CONFIG_ARCH_BOARD_MINDPX_V2) &&\
-	!defined(CONFIG_ARCH_BOARD_PX4FMU_V4) && !defined(CONFIG_ARCH_BOARD_SITL)
+	!defined(CONFIG_ARCH_BOARD_MINDPX_V2) && \
+	!defined(CONFIG_ARCH_BOARD_PX4FMU_V4) && !defined(CONFIG_ARCH_BOARD_SITL) && \
+	!defined(CONFIG_ARCH_BOARD_TAP_V1)
 # error No CONFIG_ARCH_BOARD_xxxx set
 #endif
 /*
diff --git a/src/drivers/drv_sensor.h b/src/drivers/drv_sensor.h
index a1100287c2..fac3dc0eff 100644
--- a/src/drivers/drv_sensor.h
+++ b/src/drivers/drv_sensor.h
@@ -72,7 +72,7 @@
 #define DRV_RNG_DEVTYPE_MB12XX   0x31
 #define DRV_RNG_DEVTYPE_LL40LS   0x32
 
-#ifdef CONFIG_ARCH_BOARD_MINDPX_V2
+#if defined (CONFIG_ARCH_BOARD_MINDPX_V2) || defined (CONFIG_ARCH_BOARD_TAP_V1)
 #define DRV_ACC_DEVTYPE_MPU6050 0x14
 #define DRV_ACC_DEVTYPE_MPU6500 0x13
 

From 703141d650c95205655cb21476a5f0cf1a31334c Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Wed, 6 Jul 2016 13:37:46 +0200
Subject: [PATCH 0916/1230] Estimator cleanup

---
 ROMFS/tap_common/init.d/rc.mc_apps   | 24 +-----------------------
 ROMFS/tap_common/init.d/rc.vtol_apps | 22 +---------------------
 2 files changed, 2 insertions(+), 44 deletions(-)

diff --git a/ROMFS/tap_common/init.d/rc.mc_apps b/ROMFS/tap_common/init.d/rc.mc_apps
index 399f4398ac..607dffd5b0 100644
--- a/ROMFS/tap_common/init.d/rc.mc_apps
+++ b/ROMFS/tap_common/init.d/rc.mc_apps
@@ -5,29 +5,7 @@
 #
 
 
-#---------------------------------------
-# Estimator group selction
-#
-# INAV
-if param compare SYS_MC_EST_GROUP 0
-then
-	attitude_estimator_q start
-	position_estimator_inav start
-fi
-
-# LPE
-if param compare SYS_MC_EST_GROUP 1
-then
-	attitude_estimator_q start
-	local_position_estimator start
-fi
-
-# EKF
-if param compare SYS_MC_EST_GROUP 2
-then
-	ekf2 start
-fi
-#---------------------------------------
+ekf2 start
 
 mc_att_control start
 
diff --git a/ROMFS/tap_common/init.d/rc.vtol_apps b/ROMFS/tap_common/init.d/rc.vtol_apps
index c3d54fd299..0ae3f28950 100644
--- a/ROMFS/tap_common/init.d/rc.vtol_apps
+++ b/ROMFS/tap_common/init.d/rc.vtol_apps
@@ -9,27 +9,7 @@
 #---------------------------------------
 # Estimator group selction
 #
-# INAV
-if param compare SYS_MC_EST_GROUP 0
-then
-	attitude_estimator_q start
-	position_estimator_inav start
-fi
-
-# LPE
-if param compare SYS_MC_EST_GROUP 1
-then
-	attitude_estimator_q start
-	local_position_estimator start
-fi
-
-# EKF
-if param compare SYS_MC_EST_GROUP 2
-then
-	ekf2 start
-fi
-#---------------------------------------
-
+ekf2 start
 
 vtol_att_control start
 mc_att_control start

From 056f73f5d279435a6bb2f77174bb8bfa1e556416 Mon Sep 17 00:00:00 2001
From: James Goppert <jgoppert@users.noreply.github.com>
Date: Wed, 6 Jul 2016 10:31:55 -0400
Subject: [PATCH 0917/1230] Changed LPE distance sensor timeout logic. (#4996)

---
 .../local_position_estimator/sensors/lidar.cpp       | 12 ++++++++----
 .../local_position_estimator/sensors/sonar.cpp       |  7 ++++++-
 2 files changed, 14 insertions(+), 5 deletions(-)

diff --git a/src/modules/local_position_estimator/sensors/lidar.cpp b/src/modules/local_position_estimator/sensors/lidar.cpp
index 67f623e946..1efb0a234e 100644
--- a/src/modules/local_position_estimator/sensors/lidar.cpp
+++ b/src/modules/local_position_estimator/sensors/lidar.cpp
@@ -33,19 +33,23 @@ void BlockLocalPositionEstimator::lidarInit()
 int BlockLocalPositionEstimator::lidarMeasure(Vector<float, n_y_lidar> &y)
 {
 	// measure
-	float d = _sub_lidar->get().current_distance + _lidar_z_offset.get();
-	warnx("d %10.2g, lidar z offset %10.2g\n", double(d), double(_lidar_z_offset.get()));
-	float eps = 0.01f;
+	float d = _sub_lidar->get().current_distance;
+	float eps = 0.01f; // 1 cm
 	float min_dist = _sub_lidar->get().min_distance + eps;
 	float max_dist = _sub_lidar->get().max_distance - eps;
 
+	// prevent driver from setting min dist below eps
+	if (min_dist < eps) {
+		min_dist = eps;
+	}
+
 	// check for bad data
 	if (d > max_dist || d < min_dist) {
 		return -1;
 	}
 
 	// update stats
-	_lidarStats.update(Scalarf(d));
+	_lidarStats.update(Scalarf(d + _lidar_z_offset.get()));
 	_time_last_lidar = _timeStamp;
 	y.setZero();
 	y(0) = d;
diff --git a/src/modules/local_position_estimator/sensors/sonar.cpp b/src/modules/local_position_estimator/sensors/sonar.cpp
index e78dc45b0e..c263cb0791 100644
--- a/src/modules/local_position_estimator/sensors/sonar.cpp
+++ b/src/modules/local_position_estimator/sensors/sonar.cpp
@@ -48,10 +48,15 @@ int BlockLocalPositionEstimator::sonarMeasure(Vector<float, n_y_sonar> &y)
 {
 	// measure
 	float d = _sub_sonar->get().current_distance;
-	float eps = 0.01f;
+	float eps = 0.01f; // 1 cm
 	float min_dist = _sub_sonar->get().min_distance + eps;
 	float max_dist = _sub_sonar->get().max_distance - eps;
 
+	// prevent driver from setting min dist below eps
+	if (min_dist < eps) {
+		min_dist = eps;
+	}
+
 	// check for bad data
 	if (d > max_dist || d < min_dist) {
 		return -1;

From 1d5c5497b510ae7d833b4ef1fb45734f0c99d9c8 Mon Sep 17 00:00:00 2001
From: David Sidrane <david_s5@nscdg.com>
Date: Wed, 6 Jul 2016 11:52:08 -1000
Subject: [PATCH 0918/1230] Reduces wasted FLASH by > 4K (#4994)

* Reduces wasted FLASH by > 4K

* Removed PX4_IMPLEMENT_PX4_LOG_MODULENAME

* Moved implamentation of px4_log_modulename to px4_log.c
---
 src/platforms/posix/px4_layer/px4_log.c | 18 +++++++++++
 src/platforms/px4_log.h                 | 41 +++++--------------------
 2 files changed, 26 insertions(+), 33 deletions(-)

diff --git a/src/platforms/posix/px4_layer/px4_log.c b/src/platforms/posix/px4_layer/px4_log.c
index 37d08564b1..792caa3012 100644
--- a/src/platforms/posix/px4_layer/px4_log.c
+++ b/src/platforms/posix/px4_layer/px4_log.c
@@ -30,3 +30,21 @@ void px4_backtrace()
 	free(callstack);
 #endif
 }
+
+__EXPORT void px4_log_modulename(int level, const char *moduleName, const char *fmt, ...)
+{
+	if (level <= __px4_log_level_current) {
+		PX4_LOG_COLOR_START
+		printf(__px4__log_level_fmt __px4__log_level_arg(level));
+		PX4_LOG_COLOR_MODULE
+		printf(__px4__log_modulename_pfmt, moduleName);
+		PX4_LOG_COLOR_MESSAGE
+		va_list argptr;
+		va_start(argptr, fmt);
+		vprintf(fmt, argptr);
+		va_end(argptr);
+		PX4_LOG_COLOR_END
+		printf("\n");
+	}
+}
+
diff --git a/src/platforms/px4_log.h b/src/platforms/px4_log.h
index b912f7dad3..53af0bb2ed 100644
--- a/src/platforms/px4_log.h
+++ b/src/platforms/px4_log.h
@@ -134,6 +134,8 @@ __EXPORT extern const char *__px4_log_level_str[_PX4_LOG_LEVEL_PANIC + 1];
 __EXPORT extern const char *__px4_log_level_color[_PX4_LOG_LEVEL_PANIC + 1];
 __EXPORT extern int __px4_log_level_current;
 __EXPORT extern void px4_backtrace(void);
+__EXPORT void px4_log_modulename(int level, const char *moduleName, const char *fmt, ...);
+
 __END_DECLS
 
 #define PX4_BACKTRACE() px4_backtrace()
@@ -166,6 +168,7 @@ __END_DECLS
 #define __px4__log_thread_fmt		"%#X "
 #define __px4__log_thread_arg		,(unsigned int)pthread_self()
 #define __px4__log_modulename_fmt	"%-10s "
+#define __px4__log_modulename_pfmt	"[%s] "
 #define __px4__log_modulename_arg	,"[" MODULE_NAME "]"
 
 #define __px4__log_file_and_line_fmt 	" (file %s line %u)"
@@ -246,42 +249,14 @@ __END_DECLS
  * Convert a message in the form:
  * 	PX4_WARN("val is %d", val);
  * to
- * 	printf("%-5s [%-10s] val is %d\n", __px4_log_level_str[3],
+ * 	printf("%-5s [%s] val is %d\n", __px4_log_level_str[3],
  *		MODULENAME, val);
  ****************************************************************************/
 
-/* It turns out the macro below uses a lot more flash space than a static
- * inline function. */
-#if 0
-#define __px4_log_modulename(level, FMT, ...) \
-	__px4__log_printline(level,\
-			     __px4__log_level_fmt\
-			     __px4__log_modulename_fmt\
-			     FMT\
-			     __px4__log_end_fmt\
-			     __px4__log_level_arg(level)\
-			     __px4__log_modulename_arg\
-			     , ##__VA_ARGS__\
-			    )
-#endif
-
-static inline void __px4_log_modulename(int level, const char *fmt, ...)
-{
-	if (level <= __px4_log_level_current) {
-		PX4_LOG_COLOR_START
-		printf(__px4__log_level_fmt __px4__log_level_arg(level));
-		PX4_LOG_COLOR_MODULE
-		printf(__px4__log_modulename_fmt __px4__log_modulename_arg);
-		PX4_LOG_COLOR_MESSAGE
-		va_list argptr;
-		va_start(argptr, fmt);
-		vprintf(fmt, argptr);
-		va_end(argptr);
-		PX4_LOG_COLOR_END
-		printf("\n");
-	}
-}
-
+#define __px4_log_modulename(level, fmt, ...) \
+	do { \
+		px4_log_modulename(level, MODULE_NAME, fmt, ##__VA_ARGS__); \
+	} while(0)
 /****************************************************************************
  * __px4_log_timestamp:
  * Convert a message in the form:

From ddc4d70d5196ec912830df346293bb35482956fb Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Beat=20K=C3=BCng?= <beat-kueng@gmx.net>
Date: Tue, 21 Jun 2016 12:47:43 +0200
Subject: [PATCH 0919/1230] airspeed_calibration: remove unused include

---
 src/modules/commander/airspeed_calibration.cpp | 1 -
 1 file changed, 1 deletion(-)

diff --git a/src/modules/commander/airspeed_calibration.cpp b/src/modules/commander/airspeed_calibration.cpp
index 0556e04b0a..94ca6e33dd 100644
--- a/src/modules/commander/airspeed_calibration.cpp
+++ b/src/modules/commander/airspeed_calibration.cpp
@@ -50,7 +50,6 @@
 #include <cmath>
 #include <drivers/drv_hrt.h>
 #include <drivers/drv_airspeed.h>
-#include <uORB/topics/sensor_combined.h>
 #include <uORB/topics/differential_pressure.h>
 #include <systemlib/mavlink_log.h>
 #include <systemlib/param/param.h>

From 78b45c4778df78924d36b3b5b97829f074cbafb0 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Beat=20K=C3=BCng?= <beat-kueng@gmx.net>
Date: Tue, 21 Jun 2016 12:48:10 +0200
Subject: [PATCH 0920/1230] sensors: remove duplicated initialization

---
 src/modules/sensors/sensors.cpp | 6 ------
 1 file changed, 6 deletions(-)

diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp
index daf8d32d7c..a9ea9343d0 100644
--- a/src/modules/sensors/sensors.cpp
+++ b/src/modules/sensors/sensors.cpp
@@ -525,12 +525,6 @@ Sensors::Sensors() :
 	_hil_enabled(false),
 	_publishing(true),
 	_armed(false),
-
-	/* subscriptions */
-	_gyro_sub{ -1, -1, -1},
-	_accel_sub{ -1, -1, -1},
-	_mag_sub{ -1, -1, -1},
-	_baro_sub{ -1, -1, -1},
 	_gyro_count(0),
 	_accel_count(0),
 	_mag_count(0),

From ac45c9001bcfe312274ca9b17974f8d160bbc76f Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Beat=20K=C3=BCng?= <beat-kueng@gmx.net>
Date: Tue, 21 Jun 2016 12:49:40 +0200
Subject: [PATCH 0921/1230] sensors: cleanup syntax: replace &x[0] with x

---
 src/modules/sensors/sensors.cpp | 35 +++++++++++++++------------------
 1 file changed, 16 insertions(+), 19 deletions(-)

diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp
index a9ea9343d0..29d248287e 100644
--- a/src/modules/sensors/sensors.cpp
+++ b/src/modules/sensors/sensors.cpp
@@ -2115,17 +2115,14 @@ Sensors::task_main()
 
 	unsigned bcount_prev = _baro_count;
 
-	_gyro_count = init_sensor_class(ORB_ID(sensor_gyro), &_gyro_sub[0],
-					&raw.gyro_priority[0], &raw.gyro_errcount[0]);
+	_gyro_count = init_sensor_class(ORB_ID(sensor_gyro), _gyro_sub, raw.gyro_priority, raw.gyro_errcount);
 
-	_mag_count = init_sensor_class(ORB_ID(sensor_mag), &_mag_sub[0],
-				       &raw.magnetometer_priority[0], &raw.magnetometer_errcount[0]);
+	_mag_count = init_sensor_class(ORB_ID(sensor_mag), _mag_sub, raw.magnetometer_priority, raw.magnetometer_errcount);
 
-	_accel_count = init_sensor_class(ORB_ID(sensor_accel), &_accel_sub[0],
-					 &raw.accelerometer_priority[0], &raw.accelerometer_errcount[0]);
+	_accel_count = init_sensor_class(ORB_ID(sensor_accel), _accel_sub, raw.accelerometer_priority,
+					 raw.accelerometer_errcount);
 
-	_baro_count = init_sensor_class(ORB_ID(sensor_baro), &_baro_sub[0],
-					&raw.baro_priority[0], &raw.baro_errcount[0]);
+	_baro_count = init_sensor_class(ORB_ID(sensor_baro), _baro_sub, raw.baro_priority, raw.baro_errcount);
 
 	if (gcount_prev != _gyro_count ||
 	    mcount_prev != _mag_count ||
@@ -2184,7 +2181,7 @@ Sensors::task_main()
 	while (!_task_should_exit) {
 
 		/* wait for up to 50ms for data */
-		int pret = px4_poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 50);
+		int pret = px4_poll(fds, (sizeof(fds) / sizeof(fds[0])), 50);
 
 		/* if pret == 0 it timed out - periodic check for _task_should_exit, etc. */
 
@@ -2194,8 +2191,8 @@ Sensors::task_main()
 			 * then attempt to subscribe once again
 			 */
 			if (_gyro_count == 0) {
-				_gyro_count = init_sensor_class(ORB_ID(sensor_gyro), &_gyro_sub[0],
-								&raw.gyro_priority[0], &raw.gyro_errcount[0]);
+				_gyro_count = init_sensor_class(ORB_ID(sensor_gyro), _gyro_sub,
+								raw.gyro_priority, raw.gyro_errcount);
 				fds[0].fd = _gyro_sub[0];
 			}
 
@@ -2253,17 +2250,17 @@ Sensors::task_main()
 		 * when not adding sensors poll for param updates
 		 */
 		if (!_armed && hrt_elapsed_time(&_last_config_update) > 500 * 1000) {
-			_gyro_count = init_sensor_class(ORB_ID(sensor_gyro), &_gyro_sub[0],
-							&raw.gyro_priority[0], &raw.gyro_errcount[0]);
+			_gyro_count = init_sensor_class(ORB_ID(sensor_gyro), _gyro_sub,
+							raw.gyro_priority, raw.gyro_errcount);
 
-			_mag_count = init_sensor_class(ORB_ID(sensor_mag), &_mag_sub[0],
-						       &raw.magnetometer_priority[0], &raw.magnetometer_errcount[0]);
+			_mag_count = init_sensor_class(ORB_ID(sensor_mag), _mag_sub,
+						       raw.magnetometer_priority, raw.magnetometer_errcount);
 
-			_accel_count = init_sensor_class(ORB_ID(sensor_accel), &_accel_sub[0],
-							 &raw.accelerometer_priority[0], &raw.accelerometer_errcount[0]);
+			_accel_count = init_sensor_class(ORB_ID(sensor_accel), _accel_sub,
+							 raw.accelerometer_priority, raw.accelerometer_errcount);
 
-			_baro_count = init_sensor_class(ORB_ID(sensor_baro), &_baro_sub[0],
-							&raw.baro_priority[0], &raw.baro_errcount[0]);
+			_baro_count = init_sensor_class(ORB_ID(sensor_baro), _baro_sub,
+							raw.baro_priority, raw.baro_errcount);
 
 			_last_config_update = hrt_absolute_time();
 

From 2c2477a07d0c9803305a365f82ec8b1d7cf39505 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Beat=20K=C3=BCng?= <beat-kueng@gmx.net>
Date: Tue, 21 Jun 2016 12:50:35 +0200
Subject: [PATCH 0922/1230] fix sensors leaks: unsubscribe topics when exiting

---
 src/modules/sensors/sensors.cpp | 27 +++++++++++++++++++++++++++
 1 file changed, 27 insertions(+)

diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp
index 29d248287e..4053fde20e 100644
--- a/src/modules/sensors/sensors.cpp
+++ b/src/modules/sensors/sensors.cpp
@@ -2279,6 +2279,33 @@ Sensors::task_main()
 		perf_end(_loop_perf);
 	}
 
+	for (unsigned i = 0; i < SENSOR_COUNT_MAX; i++) {
+		if (_gyro_sub[i] >= 0) {
+			orb_unsubscribe(_gyro_sub[i]);
+		}
+
+		if (_accel_sub[i] >= 0) {
+			orb_unsubscribe(_accel_sub[i]);
+		}
+
+		if (_mag_sub[i] >= 0) {
+			orb_unsubscribe(_mag_sub[i]);
+		}
+
+		if (_baro_sub[i] >= 0) {
+			orb_unsubscribe(_baro_sub[i]);
+		}
+	}
+
+	orb_unsubscribe(_rc_sub);
+	orb_unsubscribe(_diff_pres_sub);
+	orb_unsubscribe(_vcontrol_mode_sub);
+	orb_unsubscribe(_params_sub);
+	orb_unsubscribe(_rc_parameter_map_sub);
+	orb_unsubscribe(_manual_control_sub);
+	orb_unsubscribe(_actuator_ctrl_0_sub);
+	orb_unadvertise(_sensor_pub);
+
 	warnx("exiting.");
 	_sensors_task = -1;
 	px4_task_exit(ret);

From c407123a72f0438177227a3b25ad27dfda9ffc8b Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Beat=20K=C3=BCng?= <beat-kueng@gmx.net>
Date: Tue, 21 Jun 2016 14:12:13 +0200
Subject: [PATCH 0923/1230] cleanup sensor_combined: remove adc &
 differential_pressure fields

These are not really used. differential_pressure is just copied from the
topic with the same name.

for sdlog2 we assume no one needs the diff pressure fields and set it to 0.
We plan to switch to the new logger soon anyway.
---
 msg/sensor_combined.msg                  | 10 ----------
 src/modules/mavlink/mavlink_messages.cpp | 10 +++++++++-
 src/modules/mavlink/mavlink_receiver.cpp |  8 --------
 src/modules/sdlog2/sdlog2.c              | 10 ++--------
 src/modules/sensors/sensors.cpp          | 13 -------------
 5 files changed, 11 insertions(+), 40 deletions(-)

diff --git a/msg/sensor_combined.msg b/msg/sensor_combined.msg
index fab3dcad02..206258e31d 100644
--- a/msg/sensor_combined.msg
+++ b/msg/sensor_combined.msg
@@ -23,7 +23,6 @@ uint32 SENSOR_PRIO_MAX = 255
 # Actual data, this is specific to the type of data which is stored in this struct
 # A line containing L0GME will be added by the Python logging code generator to the logged dataset.
 #
-# NOTE: Ordering of fields optimized to align to 32 bit / 4 bytes Change with consideration only
 
 uint64[3] gyro_timestamp			# Gyro timestamps
 int16[9] gyro_raw			# Raw sensor values of angular velocity
@@ -62,12 +61,3 @@ uint64[3] baro_timestamp		# Barometer timestamp
 uint32[3] baro_priority			# Sensor priority
 uint32[3] baro_errcount			# Error count in communication
 
-float32[10] adc_voltage_v		# ADC voltages of ADC Chan 10/11/12/13 or -1
-uint16[10] adc_mapping		# Channel indices of each of these values
-float32 mcu_temp_celcius			# Internal temperature measurement of MCU
-
-float32[3] differential_pressure_pa			# Airspeed sensor differential pressure
-uint64[3] differential_pressure_timestamp	# Last measurement timestamp
-float32[3] differential_pressure_filtered_pa	# Low pass filtered airspeed sensor differential pressure reading
-uint32[3] differential_pressure_priority			# Sensor priority
-uint32[3] differential_pressure_errcount			# Error count in communication
diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp
index b62f150dbc..a20d99294b 100644
--- a/src/modules/mavlink/mavlink_messages.cpp
+++ b/src/modules/mavlink/mavlink_messages.cpp
@@ -63,6 +63,7 @@
 #include <uORB/topics/camera_trigger.h>
 #include <uORB/topics/cpuload.h>
 #include <uORB/topics/debug_key_value.h>
+#include <uORB/topics/differential_pressure.h>
 #include <uORB/topics/distance_sensor.h>
 #include <uORB/topics/estimator_status.h>
 #include <uORB/topics/fw_pos_ctrl_status.h>
@@ -691,6 +692,9 @@ private:
 	MavlinkOrbSubscription *_sensor_sub;
 	uint64_t _sensor_time;
 
+	MavlinkOrbSubscription *_differential_pressure_sub;
+	uint64_t _differential_pressure_time;
+
 	uint64_t _accel_timestamp;
 	uint64_t _gyro_timestamp;
 	uint64_t _mag_timestamp;
@@ -704,6 +708,8 @@ protected:
 	explicit MavlinkStreamHighresIMU(Mavlink *mavlink) : MavlinkStream(mavlink),
 		_sensor_sub(_mavlink->add_orb_subscription(ORB_ID(sensor_combined))),
 		_sensor_time(0),
+		_differential_pressure_sub(_mavlink->add_orb_subscription(ORB_ID(differential_pressure))),
+		_differential_pressure_time(0),
 		_accel_timestamp(0),
 		_gyro_timestamp(0),
 		_mag_timestamp(0),
@@ -713,6 +719,7 @@ protected:
 	void send(const hrt_abstime t)
 	{
 		struct sensor_combined_s sensor;
+		struct differential_pressure_s differential_pressure;
 
 		if (_sensor_sub->update(&_sensor_time, &sensor)) {
 			uint16_t fields_updated = 0;
@@ -740,6 +747,7 @@ protected:
 				fields_updated |= (1 << 9) | (1 << 11) | (1 << 12);
 				_baro_timestamp = sensor.baro_timestamp[0];
 			}
+			_differential_pressure_sub->update(&_differential_pressure_time, &differential_pressure);
 
 			mavlink_highres_imu_t msg;
 
@@ -754,7 +762,7 @@ protected:
 			msg.ymag = sensor.magnetometer_ga[1];
 			msg.zmag = sensor.magnetometer_ga[2];
 			msg.abs_pressure = sensor.baro_pres_mbar[0];
-			msg.diff_pressure = sensor.differential_pressure_pa[0];
+			msg.diff_pressure = differential_pressure.differential_pressure_raw_pa;
 			msg.pressure_alt = sensor.baro_alt_meter[0];
 			msg.temperature = sensor.baro_temp_celcius[0];
 			msg.fields_updated = fields_updated;
diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp
index 52aa816968..c7baa5da05 100644
--- a/src/modules/mavlink/mavlink_receiver.cpp
+++ b/src/modules/mavlink/mavlink_receiver.cpp
@@ -1705,10 +1705,6 @@ MavlinkReceiver::handle_message_hil_sensor(mavlink_message_t *msg)
 		hil_sensors.accelerometer_timestamp[0] = timestamp;
 		hil_sensors.accelerometer_priority[0] = ORB_PRIO_HIGH;
 
-		hil_sensors.adc_voltage_v[0] = 0.0f;
-		hil_sensors.adc_voltage_v[1] = 0.0f;
-		hil_sensors.adc_voltage_v[2] = 0.0f;
-
 		hil_sensors.magnetometer_raw[0] = imu.xmag * 1000.0f;
 		hil_sensors.magnetometer_raw[1] = imu.ymag * 1000.0f;
 		hil_sensors.magnetometer_raw[2] = imu.zmag * 1000.0f;
@@ -1726,10 +1722,6 @@ MavlinkReceiver::handle_message_hil_sensor(mavlink_message_t *msg)
 		hil_sensors.baro_temp_celcius[0] = imu.temperature;
 		hil_sensors.baro_timestamp[0] = timestamp;
 
-		hil_sensors.differential_pressure_pa[0] = imu.diff_pressure * 1e2f; //from hPa to Pa
-		hil_sensors.differential_pressure_filtered_pa[0] = hil_sensors.differential_pressure_pa[0];
-		hil_sensors.differential_pressure_timestamp[0] = timestamp;
-
 		/* publish combined sensor topic */
 		if (_sensors_pub == nullptr) {
 			_sensors_pub = orb_advertise(ORB_ID(sensor_combined), &hil_sensors);
diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c
index cd17ae121c..261b319f6e 100644
--- a/src/modules/sdlog2/sdlog2.c
+++ b/src/modules/sdlog2/sdlog2.c
@@ -1374,7 +1374,6 @@ int sdlog2_thread_main(int argc, char *argv[])
 	hrt_abstime accelerometer_timestamp[3] = {0, 0, 0};
 	hrt_abstime magnetometer_timestamp[3] = {0, 0, 0};
 	hrt_abstime barometer_timestamp[3] = {0, 0, 0};
-	hrt_abstime differential_pressure_timestamp[3] = {0, 0, 0};
 
 	/* initialize calculated mean SNR */
 	float snr_mean = 0.0f;
@@ -1659,11 +1658,6 @@ int sdlog2_thread_main(int argc, char *argv[])
 					write_SENS = true;
 				}
 
-				if (buf.sensor.differential_pressure_timestamp[i] != differential_pressure_timestamp[i]) {
-					differential_pressure_timestamp[i] = buf.sensor.differential_pressure_timestamp[i];
-					write_SENS = true;
-				}
-
 				if (write_IMU) {
 					switch (i) {
 						case 0:
@@ -1708,8 +1702,8 @@ int sdlog2_thread_main(int argc, char *argv[])
 					log_msg.body.log_SENS.baro_pres = buf.sensor.baro_pres_mbar[i];
 					log_msg.body.log_SENS.baro_alt = buf.sensor.baro_alt_meter[i];
 					log_msg.body.log_SENS.baro_temp = buf.sensor.baro_temp_celcius[i];
-					log_msg.body.log_SENS.diff_pres = buf.sensor.differential_pressure_pa[i];
-					log_msg.body.log_SENS.diff_pres_filtered = buf.sensor.differential_pressure_filtered_pa[i];
+					log_msg.body.log_SENS.diff_pres = 0;
+					log_msg.body.log_SENS.diff_pres_filtered = 0;
 					LOGBUFFER_WRITE_AND_COUNT(SENS);
 				}
 			}
diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp
index 4053fde20e..35fbb2ed7f 100644
--- a/src/modules/sensors/sensors.cpp
+++ b/src/modules/sensors/sensors.cpp
@@ -1154,10 +1154,6 @@ Sensors::diff_pres_poll(struct sensor_combined_s &raw)
 	if (updated) {
 		orb_copy(ORB_ID(differential_pressure), _diff_pres_sub, &_diff_pres);
 
-		raw.differential_pressure_pa[0] = _diff_pres.differential_pressure_raw_pa;
-		raw.differential_pressure_timestamp[0] = _diff_pres.timestamp;
-		raw.differential_pressure_filtered_pa[0] = _diff_pres.differential_pressure_filtered_pa;
-
 		float air_temperature_celsius = (_diff_pres.temperature > -300.0f) ? _diff_pres.temperature :
 						(raw.baro_temp_celcius[0] - PCB_TEMP_ESTIMATE_DEG);
 
@@ -1671,11 +1667,6 @@ Sensors::adc_poll(struct sensor_combined_s &raw)
 
 			/* Read add channels we got */
 			for (unsigned i = 0; i < ret / sizeof(buf_adc[0]); i++) {
-				/* Save raw voltage values */
-				if (i < (sizeof(raw.adc_voltage_v) / sizeof(raw.adc_voltage_v[0]))) {
-					raw.adc_voltage_v[i] = buf_adc[i].am_data / (4096.0f / 3.3f);
-					raw.adc_mapping[i] = buf_adc[i].am_channel;
-				}
 
 				/* look for specific channels and process the raw voltage to measurement data */
 				if (ADC_BATTERY_VOLTAGE_CHANNEL == buf_adc[i].am_channel) {
@@ -2145,10 +2136,6 @@ Sensors::task_main()
 	 * do advertisements
 	 */
 	raw.timestamp = hrt_absolute_time();
-	raw.adc_voltage_v[0] = 0.0f;
-	raw.adc_voltage_v[1] = 0.0f;
-	raw.adc_voltage_v[2] = 0.0f;
-	raw.adc_voltage_v[3] = 0.0f;
 
 	_battery.reset(&_battery_status);
 

From b4ecc5a8d961d31ef7eb66b8f5ea097568e914f0 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Beat=20K=C3=BCng?= <beat-kueng@gmx.net>
Date: Fri, 24 Jun 2016 12:52:16 +0200
Subject: [PATCH 0924/1230] sensor_combined cleanup: remove many unneeded
 fields

Decreases the message size from 780 to 280 bytes.
In particular, all modules using sensor_combined must use the integral now.
The sensor value can easily be reconstructed by dividing with dt.

Voters now need to be moved into sensors module, because error count and
priority is removed from the topic.

Any module that requires additional data from a sensor can subscribe to
the raw sensor topics.

At two places, values are set to zero instead of subscribing to the raw
sensors (with the assumption that no one reads them):
- mavlink mavlink_highres_imu_t::abs_pressure
- sdlog2: sensor temperatures
---
 msg/sensor_combined.msg                       |  49 +------
 src/drivers/frsky_telemetry/frsky_data.c      |  14 +-
 src/examples/px4_simple_app/px4_simple_app.c  |  15 +-
 .../terrain_estimation/terrain_estimator.cpp  |   6 +-
 .../attitude_estimator_ekf_main.cpp           |  36 +++--
 .../attitude_estimator_q_main.cpp             |  31 ++--
 .../commander/calibration_routines.cpp        |  13 +-
 src/modules/ekf2/ekf2_main.cpp                |  23 ++-
 .../ekf_att_pos_estimator_main.cpp            |  74 ++++------
 .../fw_pos_control_l1_main.cpp                |   8 +-
 .../BlockLocalPositionEstimator.cpp           |   6 +-
 src/modules/mavlink/mavlink_messages.cpp      |  16 ++-
 src/modules/mavlink/mavlink_receiver.cpp      |  32 +----
 .../position_estimator_inav_main.cpp          |  11 +-
 src/modules/sdlog2/sdlog2.c                   |  22 +--
 src/modules/sensors/sensors.cpp               | 132 +++++++++---------
 16 files changed, 216 insertions(+), 272 deletions(-)

diff --git a/msg/sensor_combined.msg b/msg/sensor_combined.msg
index 206258e31d..790cdf3c68 100644
--- a/msg/sensor_combined.msg
+++ b/msg/sensor_combined.msg
@@ -1,63 +1,22 @@
-# Definition of the sensor_combined uORB topic.
-
-int32 MAGNETOMETER_MODE_NORMAL = 0
-int32 MAGNETOMETER_MODE_POSITIVE_BIAS = 1
-int32 MAGNETOMETER_MODE_NEGATIVE_BIAS = 2
-
-uint32 SENSOR_PRIO_MIN = 0
-uint32 SENSOR_PRIO_VERY_LOW = 25
-uint32 SENSOR_PRIO_LOW = 50
-uint32 SENSOR_PRIO_DEFAULT = 75
-uint32 SENSOR_PRIO_HIGH = 100
-uint32 SENSOR_PRIO_VERY_HIGH = 125
-uint32 SENSOR_PRIO_MAX = 255
-
-# Sensor readings in raw and SI-unit form.
 #
-# These values are read from the sensors. Raw values are in sensor-specific units,
-# the scaled values are in SI-units, as visible from the ending of the variable
-# or the comments. The use of the SI fields is in general advised, as these fields
-# are scaled and offset-compensated where possible and do not change with board
-# revisions and sensor updates.
+# Sensor readings in SI-unit form.
 #
-# Actual data, this is specific to the type of data which is stored in this struct
-# A line containing L0GME will be added by the Python logging code generator to the logged dataset.
+# These fields are scaled and offset-compensated where possible and do not
+# change with board revisions and sensor updates.
 #
 
 uint64[3] gyro_timestamp			# Gyro timestamps
-int16[9] gyro_raw			# Raw sensor values of angular velocity
-float32[9] gyro_rad_s			# Angular velocity in radian per seconds
-uint32[3] gyro_priority			# Sensor priority
-float32[9] gyro_integral_rad		# delta angle in radians
+float32[9] gyro_integral_rad		# delta angle in the NED body frame in rad in the integration time frame
 uint64[3] gyro_integral_dt			# delta time for gyro integral in us
-uint32[3] gyro_errcount			# Error counter for gyro 0
-float32[3] gyro_temp			# Temperature of gyro 0
 
-int16[9] accelerometer_raw		# Raw acceleration in NED body frame
-float32[9] accelerometer_m_s2		# Acceleration in NED body frame, in m/s^2
 float32[9] accelerometer_integral_m_s		# velocity in NED body frame, in m/s^2
 uint64[3] accelerometer_integral_dt		# delta time for accel integral in us
-int16[3] accelerometer_mode			# Accelerometer measurement mode
-float32[3] accelerometer_range_m_s2		# Accelerometer measurement range in m/s^2
 uint64[3] accelerometer_timestamp	# Accelerometer timestamp
-uint32[3] accelerometer_priority	# Sensor priority
-uint32[3] accelerometer_errcount	# Error counter for accel 0
-float32[3] accelerometer_temp		# Temperature of accel 0
 
-int16[9] magnetometer_raw		# Raw magnetic field in NED body frame
 float32[9] magnetometer_ga		# Magnetic field in NED body frame, in Gauss
-int16[3] magnetometer_mode			# Magnetometer measurement mode
-float32[3] magnetometer_range_ga		# measurement range in Gauss
-float32[3] magnetometer_cuttoff_freq_hz	# Internal analog low pass frequency of sensor
 uint64[3] magnetometer_timestamp	# Magnetometer timestamp
-uint32[3] magnetometer_priority			# Sensor priority
-uint32[3] magnetometer_errcount		# Error counter for mag 0
-float32[3] magnetometer_temp		# Temperature of mag 0
 
-float32[3] baro_pres_mbar			# Barometric pressure, already temp. comp.
 float32[3] baro_alt_meter			# Altitude, already temp. comp.
 float32[3] baro_temp_celcius		# Temperature in degrees celsius
 uint64[3] baro_timestamp		# Barometer timestamp
-uint32[3] baro_priority			# Sensor priority
-uint32[3] baro_errcount			# Error count in communication
 
diff --git a/src/drivers/frsky_telemetry/frsky_data.c b/src/drivers/frsky_telemetry/frsky_data.c
index a5304603fd..246ad91d42 100644
--- a/src/drivers/frsky_telemetry/frsky_data.c
+++ b/src/drivers/frsky_telemetry/frsky_data.c
@@ -204,12 +204,14 @@ void frsky_update_topics()
 void frsky_send_frame1(int uart)
 {
 	/* send formatted frame */
-	frsky_send_data(uart, FRSKY_ID_ACCEL_X,
-			roundf(sensor_combined->accelerometer_m_s2[0] * 1000.0f));
-	frsky_send_data(uart, FRSKY_ID_ACCEL_Y,
-			roundf(sensor_combined->accelerometer_m_s2[1] * 1000.0f));
-	frsky_send_data(uart, FRSKY_ID_ACCEL_Z,
-			roundf(sensor_combined->accelerometer_m_s2[2] * 1000.0f));
+	float acceleration[3];
+	float accel_dt = sensor_combined->accelerometer_integral_dt[0] / 1.e6f;
+	acceleration[0] = sensor_combined->accelerometer_integral_m_s[0] / accel_dt;
+	acceleration[1] = sensor_combined->accelerometer_integral_m_s[1] / accel_dt;
+	acceleration[2] = sensor_combined->accelerometer_integral_m_s[2] / accel_dt;
+	frsky_send_data(uart, FRSKY_ID_ACCEL_X, roundf(acceleration[0] * 1000.0f));
+	frsky_send_data(uart, FRSKY_ID_ACCEL_Y, roundf(acceleration[1] * 1000.0f));
+	frsky_send_data(uart, FRSKY_ID_ACCEL_Z, roundf(acceleration[2] * 1000.0f));
 
 	frsky_send_data(uart, FRSKY_ID_BARO_ALT_BP,
 			sensor_combined->baro_alt_meter[0]);
diff --git a/src/examples/px4_simple_app/px4_simple_app.c b/src/examples/px4_simple_app/px4_simple_app.c
index 23cd124130..a382c2224a 100644
--- a/src/examples/px4_simple_app/px4_simple_app.c
+++ b/src/examples/px4_simple_app/px4_simple_app.c
@@ -101,15 +101,18 @@ int px4_simple_app_main(int argc, char *argv[])
 				struct sensor_combined_s raw;
 				/* copy sensors raw data into local buffer */
 				orb_copy(ORB_ID(sensor_combined), sensor_sub_fd, &raw);
+				float sensor_accel[3];
+				float accel_dt = raw.accelerometer_integral_dt[0] / 1.e6f;
+				sensor_accel[0] = raw.accelerometer_integral_m_s[0] / accel_dt;
+				sensor_accel[1] = raw.accelerometer_integral_m_s[1] / accel_dt;
+				sensor_accel[2] = raw.accelerometer_integral_m_s[2] / accel_dt;
 				PX4_WARN("[px4_simple_app] Accelerometer:\t%8.4f\t%8.4f\t%8.4f",
-					 (double)raw.accelerometer_m_s2[0],
-					 (double)raw.accelerometer_m_s2[1],
-					 (double)raw.accelerometer_m_s2[2]);
+					 (double)sensor_accel[0], (double)sensor_accel[1], (double)sensor_accel[2]);
 
 				/* set att and publish this information for other apps */
-				att.roll = raw.accelerometer_m_s2[0];
-				att.pitch = raw.accelerometer_m_s2[1];
-				att.yaw = raw.accelerometer_m_s2[2];
+				att.roll = sensor_accel[0];
+				att.pitch = sensor_accel[1];
+				att.yaw = sensor_accel[2];
 				orb_publish(ORB_ID(vehicle_attitude), att_pub, &att);
 			}
 
diff --git a/src/lib/terrain_estimation/terrain_estimator.cpp b/src/lib/terrain_estimation/terrain_estimator.cpp
index 20bdabf6e5..d8c88e618d 100644
--- a/src/lib/terrain_estimation/terrain_estimator.cpp
+++ b/src/lib/terrain_estimation/terrain_estimator.cpp
@@ -67,7 +67,11 @@ void TerrainEstimator::predict(float dt, const struct vehicle_attitude_s *attitu
 {
 	if (attitude->R_valid) {
 		matrix::Matrix<float, 3, 3> R_att(attitude->R);
-		matrix::Vector<float, 3> a(&sensor->accelerometer_m_s2[0]);
+		matrix::Vector<float, 3> a;
+		float accel_dt = sensor->accelerometer_integral_dt[0] / 1.e6f;
+		a(0) = sensor->accelerometer_integral_m_s[0] / accel_dt;
+		a(1) = sensor->accelerometer_integral_m_s[1] / accel_dt;
+		a(2) = sensor->accelerometer_integral_m_s[2] / accel_dt;
 		matrix::Vector<float, 3> u;
 		u = R_att * a;
 		_u_z = u(2) + 9.81f; // compensate for gravity
diff --git a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp
index 4aa6abca72..fa22ad165f 100755
--- a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp
+++ b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp
@@ -395,13 +395,19 @@ int attitude_estimator_ekf_thread_main(int argc, char *argv[])
 					orb_copy(ORB_ID(vehicle_global_position), sub_global_pos, &global_pos);
 				}
 
+				float gyro_rad_s[3];
+				float gyro_dt = raw.gyro_integral_dt[0] / 1.e6f;
+				gyro_rad_s[0] = raw.gyro_integral_rad[0] / gyro_dt;
+				gyro_rad_s[1] = raw.gyro_integral_rad[1] / gyro_dt;
+				gyro_rad_s[2] = raw.gyro_integral_rad[2] / gyro_dt;
+
 				if (!initialized) {
 					// XXX disabling init for now
 					initialized = true;
 
-					// gyro_offsets[0] += raw.gyro_rad_s[0];
-					// gyro_offsets[1] += raw.gyro_rad_s[1];
-					// gyro_offsets[2] += raw.gyro_rad_s[2];
+					// gyro_offsets[0] += gyro_rad_s[0];
+					// gyro_offsets[1] += gyro_rad_s[1];
+					// gyro_offsets[2] += gyro_rad_s[2];
 					// offset_count++;
 
 					// if (hrt_absolute_time() - start_time > 3000000LL) {
@@ -427,9 +433,9 @@ int attitude_estimator_ekf_thread_main(int argc, char *argv[])
 						sensor_last_timestamp[0] = raw.gyro_timestamp[0];
 					}
 
-					z_k[0] =  raw.gyro_rad_s[0] - gyro_offsets[0];
-					z_k[1] =  raw.gyro_rad_s[1] - gyro_offsets[1];
-					z_k[2] =  raw.gyro_rad_s[2] - gyro_offsets[2];
+					z_k[0] =  gyro_rad_s[0] - gyro_offsets[0];
+					z_k[1] =  gyro_rad_s[1] - gyro_offsets[1];
+					z_k[2] =  gyro_rad_s[2] - gyro_offsets[2];
 
 					/* update accelerometer measurements */
 					if (sensor_last_timestamp[1] != raw.accelerometer_timestamp[0]) {
@@ -470,9 +476,15 @@ int attitude_estimator_ekf_thread_main(int argc, char *argv[])
 						last_vel_t = 0;
 					}
 
-					z_k[3] = raw.accelerometer_m_s2[0] - acc(0);
-					z_k[4] = raw.accelerometer_m_s2[1] - acc(1);
-					z_k[5] = raw.accelerometer_m_s2[2] - acc(2);
+					matrix::Vector3f raw_accel;
+					float accel_dt = raw.accelerometer_integral_dt[0] / 1.e6f;
+					raw_accel(0) = raw.accelerometer_integral_m_s[0] / accel_dt;
+					raw_accel(1) = raw.accelerometer_integral_m_s[1] / accel_dt;
+					raw_accel(2) = raw.accelerometer_integral_m_s[2] / accel_dt;
+
+					z_k[3] = raw_accel(0) - acc(0);
+					z_k[4] = raw_accel(1) - acc(1);
+					z_k[5] = raw_accel(2) - acc(2);
 
 					/* update magnetometer measurements */
 					if (sensor_last_timestamp[2] != raw.magnetometer_timestamp[0] &&
@@ -616,9 +628,9 @@ int attitude_estimator_ekf_thread_main(int argc, char *argv[])
 					att.pitchacc = x_aposteriori[4];
 					att.yawacc = x_aposteriori[5];
 
-					att.g_comp[0] = raw.accelerometer_m_s2[0] - acc(0);
-					att.g_comp[1] = raw.accelerometer_m_s2[1] - acc(1);
-					att.g_comp[2] = raw.accelerometer_m_s2[2] - acc(2);
+					att.g_comp[0] = raw_accel(0) - acc(0);
+					att.g_comp[1] = raw_accel(1) - acc(1);
+					att.g_comp[2] = raw_accel(2) - acc(2);
 
 					/* copy offsets */
 					memcpy(&att.rate_offsets, &(x_aposteriori[3]), sizeof(att.rate_offsets));
diff --git a/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp b/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp
index b2b6c6bdda..90ed78025c 100644
--- a/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp
+++ b/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp
@@ -359,30 +359,27 @@ void AttitudeEstimatorQ::task_main()
 				if (sensors.gyro_timestamp[i] > 0) {
 
 					float gyro[3];
+					float gyro_dt = sensors.gyro_integral_dt[i] / 1e6;
+					gyro[0] = sensors.gyro_integral_rad[i * 3 + 0] / gyro_dt;
+					gyro[1] = sensors.gyro_integral_rad[i * 3 + 1] / gyro_dt;
+					gyro[2] = sensors.gyro_integral_rad[i * 3 + 2] / gyro_dt;
 
-					for (unsigned j = 0; j < 3; j++) {
-						if (sensors.gyro_integral_dt[i] > 0) {
-							gyro[j] = (double)sensors.gyro_integral_rad[i * 3 + j] / (sensors.gyro_integral_dt[i] / 1e6);
-
-						} else {
-							/* fall back to angular rate */
-							gyro[j] = sensors.gyro_rad_s[i * 3 + j];
-						}
-					}
-
-					_voter_gyro.put(i, sensors.gyro_timestamp[i], &gyro[0], sensors.gyro_errcount[i], sensors.gyro_priority[i]);
+					//TODO: note: voter will be moved into sensors module
+					//_voter_gyro.put(i, sensors.gyro_timestamp[i], &gyro[0], sensors.gyro_errcount[i], sensors.gyro_priority[i]);
+					_voter_gyro.put(i, sensors.gyro_timestamp[i], &gyro[0], 0, 75);
 				}
 
-				/* ignore empty fields */
 				if (sensors.accelerometer_timestamp[i] > 0) {
-					_voter_accel.put(i, sensors.accelerometer_timestamp[i], &sensors.accelerometer_m_s2[i * 3],
-							 sensors.accelerometer_errcount[i], sensors.accelerometer_priority[i]);
+					float acceleration[3];
+					float accel_dt = sensors.accelerometer_integral_dt[i] / 1.e6f;
+					acceleration[0] = sensors.accelerometer_integral_m_s[i * 3 + 0] / accel_dt;
+					acceleration[1] = sensors.accelerometer_integral_m_s[i * 3 + 1] / accel_dt;
+					acceleration[2] = sensors.accelerometer_integral_m_s[i * 3 + 2] / accel_dt;
+					_voter_accel.put(i, sensors.accelerometer_timestamp[i], acceleration, 0, 75);
 				}
 
-				/* ignore empty fields */
 				if (sensors.magnetometer_timestamp[i] > 0) {
-					_voter_mag.put(i, sensors.magnetometer_timestamp[i], &sensors.magnetometer_ga[i * 3],
-						       sensors.magnetometer_errcount[i], sensors.magnetometer_priority[i]);
+					_voter_mag.put(i, sensors.magnetometer_timestamp[i], &sensors.magnetometer_ga[i * 3], 0, 75);
 				}
 			}
 
diff --git a/src/modules/commander/calibration_routines.cpp b/src/modules/commander/calibration_routines.cpp
index b351ac1bd3..39dc65155e 100644
--- a/src/modules/commander/calibration_routines.cpp
+++ b/src/modules/commander/calibration_routines.cpp
@@ -273,18 +273,7 @@ enum detect_orientation_return detect_orientation(orb_advert_t *mavlink_log_pub,
 
 			for (unsigned i = 0; i < ndim; i++) {
 
-				float di = 0.0f;
-				switch (i) {
-					case 0:
-						di = sensor.accelerometer_m_s2[0];
-						break;
-					case 1:
-						di = sensor.accelerometer_m_s2[1];
-						break;
-					case 2:
-						di = sensor.accelerometer_m_s2[2];
-						break;
-				}
+				float di = sensor.accelerometer_integral_m_s[i] / (sensor.accelerometer_integral_dt[0] / 1.e6f);
 
 				float d = di - accel_ema[i];
 				accel_ema[i] += d * w;
diff --git a/src/modules/ekf2/ekf2_main.cpp b/src/modules/ekf2/ekf2_main.cpp
index e1f746e58d..ddc471e234 100644
--- a/src/modules/ekf2/ekf2_main.cpp
+++ b/src/modules/ekf2/ekf2_main.cpp
@@ -614,10 +614,15 @@ void Ekf2::task_main()
 			control_state_s ctrl_state = {};
 			float gyro_bias[3] = {};
 			_ekf.get_gyro_bias(gyro_bias);
+			float gyro_rad_s[3];
+			float gyro_dt = sensors.gyro_integral_dt[0] / 1.e6f;
+			gyro_rad_s[0] = sensors.gyro_integral_rad[0] / gyro_dt - gyro_bias[0];
+			gyro_rad_s[1] = sensors.gyro_integral_rad[1] / gyro_dt - gyro_bias[1];
+			gyro_rad_s[2] = sensors.gyro_integral_rad[2] / gyro_dt - gyro_bias[2];
 			ctrl_state.timestamp = hrt_absolute_time();
-			ctrl_state.roll_rate = _lp_roll_rate.apply(sensors.gyro_rad_s[0]) - gyro_bias[0];
-			ctrl_state.pitch_rate = _lp_pitch_rate.apply(sensors.gyro_rad_s[1]) - gyro_bias[1];
-			ctrl_state.yaw_rate = _lp_yaw_rate.apply(sensors.gyro_rad_s[2]) - gyro_bias[2];
+			ctrl_state.roll_rate = _lp_roll_rate.apply(gyro_rad_s[0]);
+			ctrl_state.pitch_rate = _lp_pitch_rate.apply(gyro_rad_s[1]);
+			ctrl_state.yaw_rate = _lp_yaw_rate.apply(gyro_rad_s[2]);
 
 			// Velocity in body frame
 			float velocity[3];
@@ -644,7 +649,11 @@ void Ekf2::task_main()
 			ctrl_state.q[3] = q(3);
 
 			// Acceleration data
-			matrix::Vector<float, 3> acceleration = {&sensors.accelerometer_m_s2[0]};
+			matrix::Vector<float, 3> acceleration;
+			float accel_dt = sensors.accelerometer_integral_dt[0] / 1.e6f;
+			acceleration(0) = sensors.accelerometer_integral_m_s[0] / accel_dt;
+			acceleration(1) = sensors.accelerometer_integral_m_s[1] / accel_dt;
+			acceleration(2) = sensors.accelerometer_integral_m_s[2] / accel_dt;
 
 			float accel_bias[3];
 			_ekf.get_accel_bias(accel_bias);
@@ -705,9 +714,9 @@ void Ekf2::task_main()
 			att.q[3] = q(3);
 			att.q_valid = true;
 
-			att.rollspeed = sensors.gyro_rad_s[0];
-			att.pitchspeed = sensors.gyro_rad_s[1];
-			att.yawspeed = sensors.gyro_rad_s[2];
+			att.rollspeed = gyro_rad_s[0];
+			att.pitchspeed = gyro_rad_s[1];
+			att.yawspeed = gyro_rad_s[2];
 
 			// publish vehicle attitude data
 			if (_att_pub == nullptr) {
diff --git a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp
index 4fde0bea22..ac19b192fb 100644
--- a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp
+++ b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp
@@ -1384,13 +1384,21 @@ void AttitudePositionEstimatorEKF::pollData()
 
 	// Feed validator with recent sensor data
 
+	//TODO: note, we will move voters into sensors module
 	for (unsigned i = 0; i < (sizeof(_sensor_combined.gyro_timestamp) / sizeof(_sensor_combined.gyro_timestamp[0])); i++) {
-		_voter_gyro.put(i, _sensor_combined.gyro_timestamp[i], &_sensor_combined.gyro_rad_s[i * 3],
-				_sensor_combined.gyro_errcount[i], _sensor_combined.gyro_priority[i]);
-		_voter_accel.put(i, _sensor_combined.accelerometer_timestamp[i], &_sensor_combined.accelerometer_m_s2[i * 3],
-				 _sensor_combined.accelerometer_errcount[i], _sensor_combined.accelerometer_priority[i]);
-		_voter_mag.put(i, _sensor_combined.magnetometer_timestamp[i], &_sensor_combined.magnetometer_ga[i * 3],
-			       _sensor_combined.magnetometer_errcount[i], _sensor_combined.magnetometer_priority[i]);
+		float gyro_rad_s[3];
+		float gyro_dt = _sensor_combined.gyro_integral_dt[i] / 1.e6f;
+		gyro_rad_s[0] = _sensor_combined.gyro_integral_rad[i * 3 + 0] / gyro_dt;
+		gyro_rad_s[1] = _sensor_combined.gyro_integral_rad[i * 3 + 1] / gyro_dt;
+		gyro_rad_s[2] = _sensor_combined.gyro_integral_rad[i * 3 + 2] / gyro_dt;
+		_voter_gyro.put(i, _sensor_combined.gyro_timestamp[i], gyro_rad_s, 0, 75);
+		float acceleration[3];
+		float accel_dt = _sensor_combined.accelerometer_integral_dt[i] / 1.e6f;
+		acceleration[0] = _sensor_combined.accelerometer_integral_m_s[i * 3 + 0] / accel_dt;
+		acceleration[1] = _sensor_combined.accelerometer_integral_m_s[i * 3 + 1] / accel_dt;
+		acceleration[2] = _sensor_combined.accelerometer_integral_m_s[i * 3 + 2] / accel_dt;
+		_voter_accel.put(i, _sensor_combined.accelerometer_timestamp[i], acceleration, 0, 75);
+		_voter_mag.put(i, _sensor_combined.magnetometer_timestamp[i], &_sensor_combined.magnetometer_ga[i * 3], 0, 75);
 	}
 
 	// Get best measurement values
@@ -1399,28 +1407,13 @@ void AttitudePositionEstimatorEKF::pollData()
 
 	if (_gyro_main >= 0) {
 
-		// Use pre-integrated values if possible
-		if (_sensor_combined.gyro_integral_dt[_gyro_main] > 0) {
-			_ekf->dAngIMU.x = _sensor_combined.gyro_integral_rad[_gyro_main * 3 + 0];
-			_ekf->dAngIMU.y = _sensor_combined.gyro_integral_rad[_gyro_main * 3 + 1];
-			_ekf->dAngIMU.z = _sensor_combined.gyro_integral_rad[_gyro_main * 3 + 2];
+		_ekf->dAngIMU.x = _sensor_combined.gyro_integral_rad[_gyro_main * 3 + 0];
+		_ekf->dAngIMU.y = _sensor_combined.gyro_integral_rad[_gyro_main * 3 + 1];
+		_ekf->dAngIMU.z = _sensor_combined.gyro_integral_rad[_gyro_main * 3 + 2];
 
-		} else {
-			float dt_gyro = _sensor_combined.gyro_integral_dt[_gyro_main] / 1e6f;
+		float gyro_dt = _sensor_combined.gyro_integral_dt[_gyro_main] / 1.e6f;
+		_ekf->angRate = _ekf->dAngIMU / gyro_dt;
 
-			if (PX4_ISFINITE(dt_gyro) && (dt_gyro < 0.5f) && (dt_gyro > 0.00001f)) {
-				deltaT = dt_gyro;
-				_ekf->dtIMU = deltaT;
-			}
-
-			_ekf->dAngIMU.x = 0.5f * (_ekf->angRate.x + _sensor_combined.gyro_rad_s[_gyro_main * 3 + 0]) * _ekf->dtIMU;
-			_ekf->dAngIMU.y = 0.5f * (_ekf->angRate.y + _sensor_combined.gyro_rad_s[_gyro_main * 3 + 1]) * _ekf->dtIMU;
-			_ekf->dAngIMU.z = 0.5f * (_ekf->angRate.z + _sensor_combined.gyro_rad_s[_gyro_main * 3 + 2]) * _ekf->dtIMU;
-		}
-
-		_ekf->angRate.x = _sensor_combined.gyro_rad_s[_gyro_main * 3 + 0];
-		_ekf->angRate.y = _sensor_combined.gyro_rad_s[_gyro_main * 3 + 1];
-		_ekf->angRate.z = _sensor_combined.gyro_rad_s[_gyro_main * 3 + 2];
 		perf_count(_perf_gyro);
 	}
 
@@ -1428,21 +1421,13 @@ void AttitudePositionEstimatorEKF::pollData()
 
 	if (_accel_main >= 0 && (_last_accel != _sensor_combined.accelerometer_timestamp[_accel_main])) {
 
-		// Use pre-integrated values if possible
-		if (_sensor_combined.accelerometer_integral_dt[_accel_main] > 0) {
-			_ekf->dVelIMU.x = _sensor_combined.accelerometer_integral_m_s[_accel_main * 3 + 0];
-			_ekf->dVelIMU.y = _sensor_combined.accelerometer_integral_m_s[_accel_main * 3 + 1];
-			_ekf->dVelIMU.z = _sensor_combined.accelerometer_integral_m_s[_accel_main * 3 + 2];
+		_ekf->dVelIMU.x = _sensor_combined.accelerometer_integral_m_s[_accel_main * 3 + 0];
+		_ekf->dVelIMU.y = _sensor_combined.accelerometer_integral_m_s[_accel_main * 3 + 1];
+		_ekf->dVelIMU.z = _sensor_combined.accelerometer_integral_m_s[_accel_main * 3 + 2];
 
-		} else {
-			_ekf->dVelIMU.x = 0.5f * (_ekf->accel.x + _sensor_combined.accelerometer_m_s2[_accel_main * 3 + 0]) * _ekf->dtIMU;
-			_ekf->dVelIMU.y = 0.5f * (_ekf->accel.y + _sensor_combined.accelerometer_m_s2[_accel_main * 3 + 1]) * _ekf->dtIMU;
-			_ekf->dVelIMU.z = 0.5f * (_ekf->accel.z + _sensor_combined.accelerometer_m_s2[_accel_main * 3 + 2]) * _ekf->dtIMU;
-		}
+		float accel_dt = _sensor_combined.accelerometer_integral_dt[_accel_main] / 1.e6f;
+		_ekf->accel = _ekf->dVelIMU / accel_dt;
 
-		_ekf->accel.x = _sensor_combined.accelerometer_m_s2[_accel_main * 3 + 0];
-		_ekf->accel.y = _sensor_combined.accelerometer_m_s2[_accel_main * 3 + 1];
-		_ekf->accel.z = _sensor_combined.accelerometer_m_s2[_accel_main * 3 + 2];
 		_last_accel = _sensor_combined.accelerometer_timestamp[_accel_main];
 	}
 
@@ -1518,20 +1503,9 @@ void AttitudePositionEstimatorEKF::pollData()
 			 (double)(_sensor_combined.accelerometer_integral_m_s[1]),
 			 (double)(_sensor_combined.accelerometer_integral_m_s[2]));
 
-		PX4_WARN("DRV: dang: %8.4f %8.4f dvel: %8.4f %8.4f %8.4f",
-			 (double)(_sensor_combined.gyro_rad_s[0] * deltaT), (double)(_sensor_combined.gyro_rad_s[2] * deltaT),
-			 (double)(_sensor_combined.accelerometer_m_s2[0] * deltaT),
-			 (double)(_sensor_combined.accelerometer_m_s2[1] * deltaT),
-			 (double)(_sensor_combined.accelerometer_m_s2[2] * deltaT));
-
 		PX4_WARN("EKF rate: %8.4f, %8.4f, %8.4f",
 			 (double)_att.rollspeed, (double)_att.pitchspeed, (double)_att.yawspeed);
 
-		PX4_WARN("DRV rate: %8.4f, %8.4f, %8.4f",
-			 (double)_sensor_combined.gyro_rad_s[0],
-			 (double)_sensor_combined.gyro_rad_s[1],
-			 (double)_sensor_combined.gyro_rad_s[2]);
-
 		lastprint = hrt_absolute_time();
 	}
 
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 0788056104..3fddf2c7b6 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
@@ -1230,7 +1230,11 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
 	float eas2tas = 1.0f; // XXX calculate actual number based on current measurements
 
 	/* filter speed and altitude for controller */
-	math::Vector<3> accel_body(_sensor_combined.accelerometer_m_s2);
+	math::Vector<3> accel_body;
+	float accel_dt = _sensor_combined.accelerometer_integral_dt[0] / 1.e6f;
+	accel_body(0) = _sensor_combined.accelerometer_integral_m_s[0] / accel_dt;
+	accel_body(1) = _sensor_combined.accelerometer_integral_m_s[1] / accel_dt;
+	accel_body(2) = _sensor_combined.accelerometer_integral_m_s[2] / accel_dt;
 	math::Vector<3> accel_earth = _R_nb * accel_body;
 
 	/* tell TECS to update its state, but let it know when it cannot actually control the plane */
@@ -1703,7 +1707,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
 					}
 
 					/* Detect launch */
-					launchDetector.update(_sensor_combined.accelerometer_m_s2[0]);
+					launchDetector.update(accel_body(0));
 
 					/* update our copy of the launch detection state */
 					launch_detection_state = launchDetector.getLaunchDetected();
diff --git a/src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp b/src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp
index fb9c8ea712..493a12e997 100644
--- a/src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp
+++ b/src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp
@@ -844,7 +844,11 @@ void BlockLocalPositionEstimator::predict()
 
 	if (integrate && _sub_att.get().R_valid) {
 		Matrix3f R_att(_sub_att.get().R);
-		Vector3f a(_sub_sensor.get().accelerometer_m_s2);
+		Vector3f a;
+		float accel_dt = _sub_sensor.get().accelerometer_integral_dt[0] / 1.e6f;
+		a(0) = _sub_sensor.get().accelerometer_integral_m_s[0] / accel_dt;
+		a(1) = _sub_sensor.get().accelerometer_integral_m_s[1] / accel_dt;
+		a(2) = _sub_sensor.get().accelerometer_integral_m_s[2] / accel_dt;
 		_u = R_att * a;
 		_u(U_az) += 9.81f; // add g
 
diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp
index a20d99294b..a699974ca1 100644
--- a/src/modules/mavlink/mavlink_messages.cpp
+++ b/src/modules/mavlink/mavlink_messages.cpp
@@ -752,16 +752,18 @@ protected:
 			mavlink_highres_imu_t msg;
 
 			msg.time_usec = sensor.timestamp;
-			msg.xacc = sensor.accelerometer_m_s2[0];
-			msg.yacc = sensor.accelerometer_m_s2[1];
-			msg.zacc = sensor.accelerometer_m_s2[2];
-			msg.xgyro = sensor.gyro_rad_s[0];
-			msg.ygyro = sensor.gyro_rad_s[1];
-			msg.zgyro = sensor.gyro_rad_s[2];
+			float accel_dt = sensor.accelerometer_integral_dt[0] / 1.e6f;
+			msg.xacc = sensor.accelerometer_integral_m_s[0] / accel_dt;
+			msg.yacc = sensor.accelerometer_integral_m_s[1] / accel_dt;
+			msg.zacc = sensor.accelerometer_integral_m_s[2] / accel_dt;
+			float gyro_dt = sensor.gyro_integral_dt[0] / 1.e6f;
+			msg.xgyro = sensor.gyro_integral_rad[0] / gyro_dt;
+			msg.ygyro = sensor.gyro_integral_rad[1] / gyro_dt;
+			msg.zgyro = sensor.gyro_integral_rad[2] / gyro_dt;
 			msg.xmag = sensor.magnetometer_ga[0];
 			msg.ymag = sensor.magnetometer_ga[1];
 			msg.zmag = sensor.magnetometer_ga[2];
-			msg.abs_pressure = sensor.baro_pres_mbar[0];
+			msg.abs_pressure = 0;
 			msg.diff_pressure = differential_pressure.differential_pressure_raw_pa;
 			msg.pressure_alt = sensor.baro_alt_meter[0];
 			msg.temperature = sensor.baro_temp_celcius[0];
diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp
index c7baa5da05..0d62fb2307 100644
--- a/src/modules/mavlink/mavlink_receiver.cpp
+++ b/src/modules/mavlink/mavlink_receiver.cpp
@@ -1675,49 +1675,29 @@ MavlinkReceiver::handle_message_hil_sensor(mavlink_message_t *msg)
 
 		hil_sensors.timestamp = timestamp;
 
-		hil_sensors.gyro_raw[0] = imu.xgyro * 1000.0f;
-		hil_sensors.gyro_raw[1] = imu.ygyro * 1000.0f;
-		hil_sensors.gyro_raw[2] = imu.zgyro * 1000.0f;
-		hil_sensors.gyro_rad_s[0] = imu.xgyro;
-		hil_sensors.gyro_rad_s[1] = imu.ygyro;
-		hil_sensors.gyro_rad_s[2] = imu.zgyro;
 		hil_sensors.gyro_integral_rad[0] = 0.5f * (imu.xgyro + _hil_prev_gyro[0]) * dt;
 		hil_sensors.gyro_integral_rad[1] = 0.5f * (imu.ygyro + _hil_prev_gyro[1]) * dt;
 		hil_sensors.gyro_integral_rad[2] = 0.5f * (imu.zgyro + _hil_prev_gyro[2]) * dt;
-		memcpy(&_hil_prev_gyro[0], &hil_sensors.gyro_rad_s[0], sizeof(_hil_prev_gyro));
+		_hil_prev_gyro[0] = imu.xgyro;
+		_hil_prev_gyro[1] = imu.ygyro;
+		_hil_prev_gyro[2] = imu.zgyro;
 		hil_sensors.gyro_integral_dt[0] = dt * 1e6f;
 		hil_sensors.gyro_timestamp[0] = timestamp;
-		hil_sensors.gyro_priority[0] = ORB_PRIO_HIGH;
 
-		hil_sensors.accelerometer_raw[0] = imu.xacc / mg2ms2;
-		hil_sensors.accelerometer_raw[1] = imu.yacc / mg2ms2;
-		hil_sensors.accelerometer_raw[2] = imu.zacc / mg2ms2;
-		hil_sensors.accelerometer_m_s2[0] = imu.xacc;
-		hil_sensors.accelerometer_m_s2[1] = imu.yacc;
-		hil_sensors.accelerometer_m_s2[2] = imu.zacc;
 		hil_sensors.accelerometer_integral_m_s[0] = 0.5f * (imu.xacc + _hil_prev_accel[0]) * dt;
 		hil_sensors.accelerometer_integral_m_s[1] = 0.5f * (imu.yacc + _hil_prev_accel[1]) * dt;
 		hil_sensors.accelerometer_integral_m_s[2] = 0.5f * (imu.zacc + _hil_prev_accel[2]) * dt;
-		memcpy(&_hil_prev_accel[0], &hil_sensors.accelerometer_m_s2[0], sizeof(_hil_prev_accel));
+		_hil_prev_accel[0] = imu.xacc;
+		_hil_prev_accel[1] = imu.yacc;
+		_hil_prev_accel[2] = imu.zacc;
 		hil_sensors.accelerometer_integral_dt[0] = dt * 1e6f;
-		hil_sensors.accelerometer_mode[0] = 0; // TODO what is this?
-		hil_sensors.accelerometer_range_m_s2[0] = 32.7f; // int16
 		hil_sensors.accelerometer_timestamp[0] = timestamp;
-		hil_sensors.accelerometer_priority[0] = ORB_PRIO_HIGH;
 
-		hil_sensors.magnetometer_raw[0] = imu.xmag * 1000.0f;
-		hil_sensors.magnetometer_raw[1] = imu.ymag * 1000.0f;
-		hil_sensors.magnetometer_raw[2] = imu.zmag * 1000.0f;
 		hil_sensors.magnetometer_ga[0] = imu.xmag;
 		hil_sensors.magnetometer_ga[1] = imu.ymag;
 		hil_sensors.magnetometer_ga[2] = imu.zmag;
-		hil_sensors.magnetometer_range_ga[0] = 32.7f; // int16
-		hil_sensors.magnetometer_mode[0] = 0; // TODO what is this
-		hil_sensors.magnetometer_cuttoff_freq_hz[0] = 50.0f;
 		hil_sensors.magnetometer_timestamp[0] = timestamp;
-		hil_sensors.magnetometer_priority[0] = ORB_PRIO_HIGH;
 
-		hil_sensors.baro_pres_mbar[0] = imu.abs_pressure;
 		hil_sensors.baro_alt_meter[0] = imu.pressure_alt;
 		hil_sensors.baro_temp_celcius[0] = imu.temperature;
 		hil_sensors.baro_timestamp[0] = timestamp;
diff --git a/src/modules/position_estimator_inav/position_estimator_inav_main.cpp b/src/modules/position_estimator_inav/position_estimator_inav_main.cpp
index 191b5be099..96f02298a1 100644
--- a/src/modules/position_estimator_inav/position_estimator_inav_main.cpp
+++ b/src/modules/position_estimator_inav/position_estimator_inav_main.cpp
@@ -504,17 +504,18 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
 
 				if (sensor.accelerometer_timestamp[0] != accel_timestamp) {
 					if (att.R_valid) {
-						/* correct accel bias */
-						sensor.accelerometer_m_s2[0] -= acc_bias[0];
-						sensor.accelerometer_m_s2[1] -= acc_bias[1];
-						sensor.accelerometer_m_s2[2] -= acc_bias[2];
+						float sensor_accel[3];
+						float accel_dt = sensor.accelerometer_integral_dt[0] / 1.e6f;
+						sensor_accel[0] = sensor.accelerometer_integral_m_s[0] / accel_dt - acc_bias[0];
+						sensor_accel[1] = sensor.accelerometer_integral_m_s[1] / accel_dt - acc_bias[1];
+						sensor_accel[2] = sensor.accelerometer_integral_m_s[2] / accel_dt - acc_bias[2];
 
 						/* transform acceleration vector from body frame to NED frame */
 						for (int i = 0; i < 3; i++) {
 							acc[i] = 0.0f;
 
 							for (int j = 0; j < 3; j++) {
-								acc[i] += PX4_R(att.R, i, j) * sensor.accelerometer_m_s2[j];
+								acc[i] += PX4_R(att.R, i, j) * sensor_accel[j];
 							}
 						}
 
diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c
index 261b319f6e..9d0245dcaf 100644
--- a/src/modules/sdlog2/sdlog2.c
+++ b/src/modules/sdlog2/sdlog2.c
@@ -1671,18 +1671,20 @@ int sdlog2_thread_main(int argc, char *argv[])
 							break;
 					}
 
-					log_msg.body.log_IMU.gyro_x = buf.sensor.gyro_rad_s[i * 3 + 0];
-					log_msg.body.log_IMU.gyro_y = buf.sensor.gyro_rad_s[i * 3 + 1];
-					log_msg.body.log_IMU.gyro_z = buf.sensor.gyro_rad_s[i * 3 + 2];
-					log_msg.body.log_IMU.acc_x = buf.sensor.accelerometer_m_s2[i * 3 + 0];
-					log_msg.body.log_IMU.acc_y = buf.sensor.accelerometer_m_s2[i * 3 + 1];
-					log_msg.body.log_IMU.acc_z = buf.sensor.accelerometer_m_s2[i * 3 + 2];
+					float gyro_dt = buf.sensor.gyro_integral_dt[i] / 1.e6f;
+					log_msg.body.log_IMU.gyro_x = buf.sensor.gyro_integral_rad[i * 3 + 0] / gyro_dt;
+					log_msg.body.log_IMU.gyro_y = buf.sensor.gyro_integral_rad[i * 3 + 1] / gyro_dt;
+					log_msg.body.log_IMU.gyro_z = buf.sensor.gyro_integral_rad[i * 3 + 2] / gyro_dt;
+					float accel_dt = buf.sensor.accelerometer_integral_dt[i] / 1.e6f;
+					log_msg.body.log_IMU.acc_x = buf.sensor.accelerometer_integral_m_s[i * 3 + 0] / accel_dt;
+					log_msg.body.log_IMU.acc_y = buf.sensor.accelerometer_integral_m_s[i * 3 + 1] / accel_dt;
+					log_msg.body.log_IMU.acc_z = buf.sensor.accelerometer_integral_m_s[i * 3 + 2] / accel_dt;
 					log_msg.body.log_IMU.mag_x = buf.sensor.magnetometer_ga[i * 3 + 0];
 					log_msg.body.log_IMU.mag_y = buf.sensor.magnetometer_ga[i * 3 + 1];
 					log_msg.body.log_IMU.mag_z = buf.sensor.magnetometer_ga[i * 3 + 2];
-					log_msg.body.log_IMU.temp_gyro = buf.sensor.gyro_temp[i];
-					log_msg.body.log_IMU.temp_acc = buf.sensor.accelerometer_temp[i];
-					log_msg.body.log_IMU.temp_mag = buf.sensor.magnetometer_temp[i];
+					log_msg.body.log_IMU.temp_gyro = 0;
+					log_msg.body.log_IMU.temp_acc = 0;
+					log_msg.body.log_IMU.temp_mag = 0;
 					LOGBUFFER_WRITE_AND_COUNT(IMU);
 				}
 
@@ -1699,7 +1701,7 @@ int sdlog2_thread_main(int argc, char *argv[])
 							break;
 					}
 
-					log_msg.body.log_SENS.baro_pres = buf.sensor.baro_pres_mbar[i];
+					log_msg.body.log_SENS.baro_pres = 0;
 					log_msg.body.log_SENS.baro_alt = buf.sensor.baro_alt_meter[i];
 					log_msg.body.log_SENS.baro_temp = buf.sensor.baro_temp_celcius[i];
 					log_msg.body.log_SENS.diff_pres = 0;
diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp
index 35fbb2ed7f..e5fe8a7ecf 100644
--- a/src/modules/sensors/sensors.cpp
+++ b/src/modules/sensors/sensors.cpp
@@ -252,6 +252,10 @@ private:
 
 	Battery		_battery;			/**< Helper lib to publish battery_status topic. */
 
+	float		_latest_baro_pressure[SENSOR_COUNT_MAX];
+	hrt_abstime	_last_accel_timestamp[SENSOR_COUNT_MAX];
+	hrt_abstime	_last_gyro_timestamp[SENSOR_COUNT_MAX];
+
 	struct {
 		float min[_rc_max_chan_count];
 		float trim[_rc_max_chan_count];
@@ -393,8 +397,7 @@ private:
 	}		_parameter_handles;		/**< handles for interesting parameters */
 
 
-	int		init_sensor_class(const struct orb_metadata *meta, int *subs,
-					  uint32_t *priorities, uint32_t *errcount);
+	int		init_sensor_class(const struct orb_metadata *meta, int *subs);
 
 	/**
 	 * Update our local parameter cache.
@@ -564,6 +567,9 @@ Sensors::Sensors() :
 	memset(&_diff_pres, 0, sizeof(_diff_pres));
 	memset(&_parameters, 0, sizeof(_parameters));
 	memset(&_rc_parameter_map, 0, sizeof(_rc_parameter_map));
+	memset(&_latest_baro_pressure, 0, sizeof(_latest_baro_pressure));
+	memset(&_last_accel_timestamp, 0, sizeof(_last_accel_timestamp));
+	memset(&_last_gyro_timestamp, 0, sizeof(_last_gyro_timestamp));
 
 	/* basic r/c parameters */
 	for (unsigned i = 0; i < _rc_max_chan_count; i++) {
@@ -1023,29 +1029,34 @@ Sensors::accel_poll(struct sensor_combined_s &raw)
 
 			orb_copy(ORB_ID(sensor_accel), _accel_sub[i], &accel_report);
 
-			math::Vector<3> vect(accel_report.x, accel_report.y, accel_report.z);
-			vect = _board_rotation * vect;
+			if (accel_report.integral_dt != 0) {
+				math::Vector<3> vect_int(accel_report.x_integral, accel_report.y_integral, accel_report.z_integral);
+				vect_int = _board_rotation * vect_int;
 
-			raw.accelerometer_m_s2[i * 3 + 0] = vect(0);
-			raw.accelerometer_m_s2[i * 3 + 1] = vect(1);
-			raw.accelerometer_m_s2[i * 3 + 2] = vect(2);
+				raw.accelerometer_integral_m_s[i * 3 + 0] = vect_int(0);
+				raw.accelerometer_integral_m_s[i * 3 + 1] = vect_int(1);
+				raw.accelerometer_integral_m_s[i * 3 + 2] = vect_int(2);
 
-			math::Vector<3> vect_int(accel_report.x_integral, accel_report.y_integral, accel_report.z_integral);
-			vect_int = _board_rotation * vect_int;
+				raw.accelerometer_integral_dt[i] = accel_report.integral_dt;
 
-			raw.accelerometer_integral_m_s[i * 3 + 0] = vect_int(0);
-			raw.accelerometer_integral_m_s[i * 3 + 1] = vect_int(1);
-			raw.accelerometer_integral_m_s[i * 3 + 2] = vect_int(2);
+			} else {
+				//this is undesirable: a driver did not set the integral, so we have to construct it ourselves
+				math::Vector<3> vect_val(accel_report.x, accel_report.y, accel_report.z);
+				vect_val = _board_rotation * vect_val;
 
-			raw.accelerometer_integral_dt[i] = accel_report.integral_dt;
+				if (_last_accel_timestamp[i] == 0) {
+					_last_accel_timestamp[i] = accel_report.timestamp - 1000;
+				}
 
-			raw.accelerometer_raw[i * 3 + 0] = accel_report.x_raw;
-			raw.accelerometer_raw[i * 3 + 1] = accel_report.y_raw;
-			raw.accelerometer_raw[i * 3 + 2] = accel_report.z_raw;
+				raw.accelerometer_integral_dt[i] = accel_report.timestamp - _last_accel_timestamp[i];
+				_last_accel_timestamp[i] = accel_report.timestamp;
+				float dt = raw.accelerometer_integral_dt[i] / 1.e6f;
+				raw.accelerometer_integral_m_s[i * 3 + 0] = vect_val(0) * dt;
+				raw.accelerometer_integral_m_s[i * 3 + 1] = vect_val(1) * dt;
+				raw.accelerometer_integral_m_s[i * 3 + 2] = vect_val(2) * dt;
+			}
 
 			raw.accelerometer_timestamp[i] = accel_report.timestamp;
-			raw.accelerometer_errcount[i] = accel_report.error_count;
-			raw.accelerometer_temp[i] = accel_report.temperature;
 		}
 	}
 }
@@ -1062,34 +1073,38 @@ Sensors::gyro_poll(struct sensor_combined_s &raw)
 
 			orb_copy(ORB_ID(sensor_gyro), _gyro_sub[i], &gyro_report);
 
-			math::Vector<3> vect(gyro_report.x, gyro_report.y, gyro_report.z);
-			vect = _board_rotation * vect;
+			if (gyro_report.integral_dt != 0) {
+				math::Vector<3> vect_int(gyro_report.x_integral, gyro_report.y_integral, gyro_report.z_integral);
+				vect_int = _board_rotation * vect_int;
 
-			raw.gyro_rad_s[i * 3 + 0] = vect(0);
-			raw.gyro_rad_s[i * 3 + 1] = vect(1);
-			raw.gyro_rad_s[i * 3 + 2] = vect(2);
+				raw.gyro_integral_rad[i * 3 + 0] = vect_int(0);
+				raw.gyro_integral_rad[i * 3 + 1] = vect_int(1);
+				raw.gyro_integral_rad[i * 3 + 2] = vect_int(2);
 
-			math::Vector<3> vect_int(gyro_report.x_integral, gyro_report.y_integral, gyro_report.z_integral);
-			vect_int = _board_rotation * vect_int;
+				raw.gyro_integral_dt[i] = gyro_report.integral_dt;
+				raw.gyro_timestamp[i] = gyro_report.timestamp;
 
-			raw.gyro_integral_rad[i * 3 + 0] = vect_int(0);
-			raw.gyro_integral_rad[i * 3 + 1] = vect_int(1);
-			raw.gyro_integral_rad[i * 3 + 2] = vect_int(2);
+			} else {
+				//this is undesirable: a driver did not set the integral, so we have to construct it ourselves
+				math::Vector<3> vect_val(gyro_report.x, gyro_report.y, gyro_report.z);
+				vect_val = _board_rotation * vect_val;
 
-			raw.gyro_integral_dt[i] = gyro_report.integral_dt;
+				if (_last_gyro_timestamp[i] == 0) {
+					_last_gyro_timestamp[i] = gyro_report.timestamp - 1000;
+				}
 
-			raw.gyro_raw[i * 3 + 0] = gyro_report.x_raw;
-			raw.gyro_raw[i * 3 + 1] = gyro_report.y_raw;
-			raw.gyro_raw[i * 3 + 2] = gyro_report.z_raw;
+				raw.gyro_integral_dt[i] = gyro_report.timestamp - _last_gyro_timestamp[i];
+				_last_gyro_timestamp[i] = gyro_report.timestamp;
+				float dt = raw.gyro_integral_dt[i] / 1.e6f;
+				raw.gyro_integral_rad[i * 3 + 0] = vect_val(0) * dt;
+				raw.gyro_integral_rad[i * 3 + 1] = vect_val(1) * dt;
+				raw.gyro_integral_rad[i * 3 + 2] = vect_val(2) * dt;
 
-			raw.gyro_timestamp[i] = gyro_report.timestamp;
+			}
 
 			if (i == 0) {
 				raw.timestamp = gyro_report.timestamp;
 			}
-
-			raw.gyro_errcount[i] = gyro_report.error_count;
-			raw.gyro_temp[i] = gyro_report.temperature;
 		}
 	}
 }
@@ -1114,13 +1129,7 @@ Sensors::mag_poll(struct sensor_combined_s &raw)
 			raw.magnetometer_ga[i * 3 + 1] = vect(1);
 			raw.magnetometer_ga[i * 3 + 2] = vect(2);
 
-			raw.magnetometer_raw[i * 3 + 0] = mag_report.x_raw;
-			raw.magnetometer_raw[i * 3 + 1] = mag_report.y_raw;
-			raw.magnetometer_raw[i * 3 + 2] = mag_report.z_raw;
-
 			raw.magnetometer_timestamp[i] = mag_report.timestamp;
-			raw.magnetometer_errcount[i] = mag_report.error_count;
-			raw.magnetometer_temp[i] = mag_report.temperature;
 		}
 	}
 }
@@ -1136,7 +1145,7 @@ Sensors::baro_poll(struct sensor_combined_s &raw)
 
 			orb_copy(ORB_ID(sensor_baro), _baro_sub[i], &_barometer);
 
-			raw.baro_pres_mbar[i] = _barometer.pressure; // Pressure in mbar
+			_latest_baro_pressure[i] = _barometer.pressure;
 			raw.baro_alt_meter[i] = _barometer.altitude; // Altitude in meters
 			raw.baro_temp_celcius[i] = _barometer.temperature; // Temperature in degrees celcius
 
@@ -1172,12 +1181,13 @@ Sensors::diff_pres_poll(struct sensor_combined_s &raw)
 		_airspeed.indicated_airspeed_m_s = math::max(0.0f,
 						   calc_indicated_airspeed(_diff_pres.differential_pressure_filtered_pa));
 
+		//FIXME: we just use the baro pressure from the first baro. we should do voting instead.
 		_airspeed.true_airspeed_m_s = math::max(0.0f,
-							calc_true_airspeed(_diff_pres.differential_pressure_filtered_pa + raw.baro_pres_mbar[0] * 1e2f,
-									raw.baro_pres_mbar[0] * 1e2f, air_temperature_celsius));
+							calc_true_airspeed(_diff_pres.differential_pressure_filtered_pa + _latest_baro_pressure[0] * 1e2f,
+									_latest_baro_pressure[0] * 1e2f, air_temperature_celsius));
 		_airspeed.true_airspeed_unfiltered_m_s = math::max(0.0f,
-				calc_true_airspeed(_diff_pres.differential_pressure_raw_pa + raw.baro_pres_mbar[0] * 1e2f,
-						   raw.baro_pres_mbar[0] * 1e2f, air_temperature_celsius));
+				calc_true_airspeed(_diff_pres.differential_pressure_raw_pa + _latest_baro_pressure[0] * 1e2f,
+						   _latest_baro_pressure[0] * 1e2f, air_temperature_celsius));
 
 		_airspeed.air_temperature_celsius = air_temperature_celsius;
 
@@ -2050,8 +2060,7 @@ Sensors::task_main_trampoline(int argc, char *argv[])
 }
 
 int
-Sensors::init_sensor_class(const struct orb_metadata *meta, int *subs,
-			   uint32_t *priorities, uint32_t *errcount)
+Sensors::init_sensor_class(const struct orb_metadata *meta, int *subs)
 {
 	unsigned group_count = orb_group_count(meta);
 
@@ -2062,7 +2071,6 @@ Sensors::init_sensor_class(const struct orb_metadata *meta, int *subs,
 	for (unsigned i = 0; i < group_count; i++) {
 		if (subs[i] < 0) {
 			subs[i] = orb_subscribe_multi(meta, i);
-			orb_priority(subs[i], (int32_t *)&priorities[i]);
 		}
 	}
 
@@ -2106,14 +2114,13 @@ Sensors::task_main()
 
 	unsigned bcount_prev = _baro_count;
 
-	_gyro_count = init_sensor_class(ORB_ID(sensor_gyro), _gyro_sub, raw.gyro_priority, raw.gyro_errcount);
+	_gyro_count = init_sensor_class(ORB_ID(sensor_gyro), _gyro_sub);
 
-	_mag_count = init_sensor_class(ORB_ID(sensor_mag), _mag_sub, raw.magnetometer_priority, raw.magnetometer_errcount);
+	_mag_count = init_sensor_class(ORB_ID(sensor_mag), _mag_sub);
 
-	_accel_count = init_sensor_class(ORB_ID(sensor_accel), _accel_sub, raw.accelerometer_priority,
-					 raw.accelerometer_errcount);
+	_accel_count = init_sensor_class(ORB_ID(sensor_accel), _accel_sub);
 
-	_baro_count = init_sensor_class(ORB_ID(sensor_baro), _baro_sub, raw.baro_priority, raw.baro_errcount);
+	_baro_count = init_sensor_class(ORB_ID(sensor_baro), _baro_sub);
 
 	if (gcount_prev != _gyro_count ||
 	    mcount_prev != _mag_count ||
@@ -2178,8 +2185,7 @@ Sensors::task_main()
 			 * then attempt to subscribe once again
 			 */
 			if (_gyro_count == 0) {
-				_gyro_count = init_sensor_class(ORB_ID(sensor_gyro), _gyro_sub,
-								raw.gyro_priority, raw.gyro_errcount);
+				_gyro_count = init_sensor_class(ORB_ID(sensor_gyro), _gyro_sub);
 				fds[0].fd = _gyro_sub[0];
 			}
 
@@ -2237,17 +2243,13 @@ Sensors::task_main()
 		 * when not adding sensors poll for param updates
 		 */
 		if (!_armed && hrt_elapsed_time(&_last_config_update) > 500 * 1000) {
-			_gyro_count = init_sensor_class(ORB_ID(sensor_gyro), _gyro_sub,
-							raw.gyro_priority, raw.gyro_errcount);
+			_gyro_count = init_sensor_class(ORB_ID(sensor_gyro), _gyro_sub);
 
-			_mag_count = init_sensor_class(ORB_ID(sensor_mag), _mag_sub,
-						       raw.magnetometer_priority, raw.magnetometer_errcount);
+			_mag_count = init_sensor_class(ORB_ID(sensor_mag), _mag_sub);
 
-			_accel_count = init_sensor_class(ORB_ID(sensor_accel), _accel_sub,
-							 raw.accelerometer_priority, raw.accelerometer_errcount);
+			_accel_count = init_sensor_class(ORB_ID(sensor_accel), _accel_sub);
 
-			_baro_count = init_sensor_class(ORB_ID(sensor_baro), _baro_sub,
-							raw.baro_priority, raw.baro_errcount);
+			_baro_count = init_sensor_class(ORB_ID(sensor_baro), _baro_sub);
 
 			_last_config_update = hrt_absolute_time();
 

From 0c30ee8d37e573701a539461934ccd5f19460c6c Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Beat=20K=C3=BCng?= <beat-kueng@gmx.net>
Date: Thu, 23 Jun 2016 21:32:24 +0200
Subject: [PATCH 0925/1230] fix resource leak in attitude_estimator_q_main:
 unsubscribe topics

---
 .../attitude_estimator_q/attitude_estimator_q_main.cpp     | 7 +++++++
 1 file changed, 7 insertions(+)

diff --git a/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp b/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp
index 90ed78025c..2a6a743661 100644
--- a/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp
+++ b/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp
@@ -671,6 +671,13 @@ void AttitudeEstimatorQ::task_main()
 			// orb_publish_auto(ORB_ID(estimator_status), &_est_state_pub, &est, &est_inst, ORB_PRIO_HIGH);
 		}
 	}
+
+	orb_unsubscribe(_sensors_sub);
+	orb_unsubscribe(_vision_sub);
+	orb_unsubscribe(_mocap_sub);
+	orb_unsubscribe(_airspeed_sub);
+	orb_unsubscribe(_params_sub);
+	orb_unsubscribe(_global_pos_sub);
 }
 
 void AttitudeEstimatorQ::update_parameters(bool force)

From c50d267bfb9248fe2178fabe78deff78abe04048 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Beat=20K=C3=BCng?= <beat-kueng@gmx.net>
Date: Fri, 24 Jun 2016 17:16:42 +0200
Subject: [PATCH 0926/1230] sensors: cleanup includes and logging (warnx ->
 PX4_WARN/ERR/INFO)

---
 src/modules/sensors/sensors.cpp | 127 +++++++++++++++-----------------
 1 file changed, 59 insertions(+), 68 deletions(-)

diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp
index e5fe8a7ecf..e03ee4f3a1 100644
--- a/src/modules/sensors/sensors.cpp
+++ b/src/modules/sensors/sensors.cpp
@@ -48,9 +48,10 @@
 
 #include <board_config.h>
 
+#include <px4_adc.h>
 #include <px4_config.h>
-#include <px4_tasks.h>
 #include <px4_posix.h>
+#include <px4_tasks.h>
 #include <px4_time.h>
 
 #include <fcntl.h>
@@ -64,8 +65,6 @@
 #include <math.h>
 #include <mathlib/mathlib.h>
 
-#include <px4_adc.h>
-
 #include <drivers/drv_hrt.h>
 #include <drivers/drv_accel.h>
 #include <drivers/drv_gyro.h>
@@ -76,14 +75,14 @@
 #include <drivers/drv_airspeed.h>
 #include <drivers/drv_px4flow.h>
 
+#include <systemlib/airspeed.h>
 #include <systemlib/systemlib.h>
 #include <systemlib/param/param.h>
 #include <systemlib/err.h>
 #include <systemlib/perf_counter.h>
 #include <systemlib/battery.h>
-#include <conversion/rotation.h>
 
-#include <systemlib/airspeed.h>
+#include <conversion/rotation.h>
 
 #include <lib/ecl/validation/data_validator.h>
 
@@ -136,12 +135,6 @@ using namespace DriverFramework;
 
 #define SENSOR_COUNT_MAX		3
 
-/* oddly, ERROR is not defined for c++ */
-#ifdef ERROR
-# undef ERROR
-#endif
-static const int ERROR = -1;
-
 #define CAL_ERROR_APPLY_CAL_MSG "FAILED APPLYING %s CAL #%u"
 
 /**
@@ -727,6 +720,7 @@ Sensors::parameters_update()
 	bool rc_valid = true;
 	float tmpScaleFactor = 0.0f;
 	float tmpRevFactor = 0.0f;
+	int ret = PX4_OK;
 
 	/* rc values */
 	for (unsigned int i = 0; i < _rc_max_chan_count; i++) {
@@ -744,7 +738,7 @@ Sensors::parameters_update()
 		if (!PX4_ISFINITE(tmpScaleFactor) ||
 		    (tmpRevFactor < 0.000001f) ||
 		    (tmpRevFactor > 0.2f)) {
-			warnx("RC chan %u not sane, scaling: %8.6f, rev: %d", i, (double)tmpScaleFactor, (int)(_parameters.rev[i]));
+			PX4_WARN("RC chan %u not sane, scaling: %8.6f, rev: %d", i, (double)tmpScaleFactor, (int)(_parameters.rev[i]));
 			/* scaling factors do not make sense, lock them down */
 			_parameters.scaling_factor[i] = 0.0f;
 			rc_valid = false;
@@ -756,66 +750,66 @@ Sensors::parameters_update()
 
 	/* handle wrong values */
 	if (!rc_valid) {
-		warnx("WARNING     WARNING     WARNING\n\nRC CALIBRATION NOT SANE!\n\n");
+		PX4_ERR("WARNING     WARNING     WARNING\n\nRC CALIBRATION NOT SANE!\n\n");
 	}
 
 	const char *paramerr = "FAIL PARM LOAD";
 
 	/* channel mapping */
 	if (param_get(_parameter_handles.rc_map_roll, &(_parameters.rc_map_roll)) != OK) {
-		warnx("%s", paramerr);
+		PX4_WARN("%s", paramerr);
 	}
 
 	if (param_get(_parameter_handles.rc_map_pitch, &(_parameters.rc_map_pitch)) != OK) {
-		warnx("%s", paramerr);
+		PX4_WARN("%s", paramerr);
 	}
 
 	if (param_get(_parameter_handles.rc_map_yaw, &(_parameters.rc_map_yaw)) != OK) {
-		warnx("%s", paramerr);
+		PX4_WARN("%s", paramerr);
 	}
 
 	if (param_get(_parameter_handles.rc_map_throttle, &(_parameters.rc_map_throttle)) != OK) {
-		warnx("%s", paramerr);
+		PX4_WARN("%s", paramerr);
 	}
 
 	if (param_get(_parameter_handles.rc_map_failsafe, &(_parameters.rc_map_failsafe)) != OK) {
-		warnx("%s", paramerr);
+		PX4_WARN("%s", paramerr);
 	}
 
 	if (param_get(_parameter_handles.rc_map_mode_sw, &(_parameters.rc_map_mode_sw)) != OK) {
-		warnx("%s", paramerr);
+		PX4_WARN("%s", paramerr);
 	}
 
 	if (param_get(_parameter_handles.rc_map_return_sw, &(_parameters.rc_map_return_sw)) != OK) {
-		warnx("%s", paramerr);
+		PX4_WARN("%s", paramerr);
 	}
 
 	if (param_get(_parameter_handles.rc_map_rattitude_sw, &(_parameters.rc_map_rattitude_sw)) != OK) {
-		warnx("%s", paramerr);
+		PX4_WARN("%s", paramerr);
 	}
 
 	if (param_get(_parameter_handles.rc_map_posctl_sw, &(_parameters.rc_map_posctl_sw)) != OK) {
-		warnx("%s", paramerr);
+		PX4_WARN("%s", paramerr);
 	}
 
 	if (param_get(_parameter_handles.rc_map_loiter_sw, &(_parameters.rc_map_loiter_sw)) != OK) {
-		warnx("%s", paramerr);
+		PX4_WARN("%s", paramerr);
 	}
 
 	if (param_get(_parameter_handles.rc_map_acro_sw, &(_parameters.rc_map_acro_sw)) != OK) {
-		warnx("%s", paramerr);
+		PX4_WARN("%s", paramerr);
 	}
 
 	if (param_get(_parameter_handles.rc_map_offboard_sw, &(_parameters.rc_map_offboard_sw)) != OK) {
-		warnx("%s", paramerr);
+		PX4_WARN("%s", paramerr);
 	}
 
 	if (param_get(_parameter_handles.rc_map_kill_sw, &(_parameters.rc_map_kill_sw)) != OK) {
-		warnx("%s", paramerr);
+		PX4_WARN("%s", paramerr);
 	}
 
 	if (param_get(_parameter_handles.rc_map_flaps, &(_parameters.rc_map_flaps)) != OK) {
-		warnx("%s", paramerr);
+		PX4_WARN("%s", paramerr);
 	}
 
 	param_get(_parameter_handles.rc_map_aux1, &(_parameters.rc_map_aux1));
@@ -892,7 +886,7 @@ Sensors::parameters_update()
 
 	/* scaling of ADC ticks to battery voltage */
 	if (param_get(_parameter_handles.battery_voltage_scaling, &(_parameters.battery_voltage_scaling)) != OK) {
-		warnx("%s", paramerr);
+		PX4_WARN("%s", paramerr);
 
 	} else if (_parameters.battery_voltage_scaling < 0.0f) {
 		/* apply scaling according to defaults if set to default */
@@ -902,7 +896,7 @@ Sensors::parameters_update()
 
 	/* scaling of ADC ticks to battery current */
 	if (param_get(_parameter_handles.battery_current_scaling, &(_parameters.battery_current_scaling)) != OK) {
-		warnx("%s", paramerr);
+		PX4_WARN("%s", paramerr);
 
 	} else if (_parameters.battery_current_scaling < 0.0f) {
 		/* apply scaling according to defaults if set to default */
@@ -911,12 +905,12 @@ Sensors::parameters_update()
 	}
 
 	if (param_get(_parameter_handles.battery_current_offset, &(_parameters.battery_current_offset)) != OK) {
-		warnx("%s", paramerr);
+		PX4_WARN("%s", paramerr);
 
 	}
 
 	if (param_get(_parameter_handles.battery_v_div, &(_parameters.battery_v_div)) != OK) {
-		warnx("%s", paramerr);
+		PX4_WARN("%s", paramerr);
 		_parameters.battery_v_div = 0.0f;
 
 	} else if (_parameters.battery_v_div <= 0.0f) {
@@ -939,7 +933,7 @@ Sensors::parameters_update()
 	}
 
 	if (param_get(_parameter_handles.battery_a_per_v, &(_parameters.battery_a_per_v)) != OK) {
-		warnx("%s", paramerr);
+		PX4_WARN("%s", paramerr);
 		_parameters.battery_a_per_v = 0.0f;
 
 	} else if (_parameters.battery_a_per_v <= 0.0f) {
@@ -985,21 +979,21 @@ Sensors::parameters_update()
 
 	// TODO: this needs fixing for QURT and Raspberry Pi
 	if (!h_baro.isValid()) {
-		warnx("ERROR: no barometer found on %s (%d)", BARO0_DEVICE_PATH, h_baro.getError());
-		return ERROR;
+		PX4_ERR("no barometer found on %s (%d)", BARO0_DEVICE_PATH, h_baro.getError());
+		ret = PX4_ERROR;
 
 	} else {
 		int baroret = h_baro.ioctl(BAROIOCSMSLPRESSURE, (unsigned long)(_parameters.baro_qnh * 100));
 
 		if (baroret) {
-			warnx("qnh could not be set");
-			return ERROR;
+			PX4_ERR("qnh for baro could not be set");
+			ret = PX4_ERROR;
 		}
 	}
 
 #endif
 
-	return OK;
+	return ret;
 }
 
 
@@ -1010,8 +1004,8 @@ Sensors::adc_init()
 	DevMgr::getHandle(ADC0_DEVICE_PATH, _h_adc);
 
 	if (!_h_adc.isValid()) {
-		warnx("FATAL: no ADC found: %s (%d)", ADC0_DEVICE_PATH, _h_adc.getError());
-		return ERROR;
+		PX4_ERR("no ADC found: %s (%d)", ADC0_DEVICE_PATH, _h_adc.getError());
+		return PX4_ERROR;
 	}
 
 	return OK;
@@ -1301,14 +1295,14 @@ Sensors::parameter_update_poll(bool forced)
 					failed = failed || (OK != param_get(param_find(str), &gscale.z_scale));
 
 					if (failed) {
-						warnx(CAL_ERROR_APPLY_CAL_MSG, "gyro", i);
+						PX4_ERR(CAL_ERROR_APPLY_CAL_MSG, "gyro", i);
 
 					} else {
 						/* apply new scaling and offsets */
 						config_ok = apply_gyro_calibration(h, &gscale, device_id);
 
 						if (!config_ok) {
-							warnx(CAL_ERROR_APPLY_CAL_MSG, "gyro ", i);
+							PX4_ERR(CAL_ERROR_APPLY_CAL_MSG, "gyro ", i);
 						}
 					}
 
@@ -1369,14 +1363,14 @@ Sensors::parameter_update_poll(bool forced)
 					failed = failed || (OK != param_get(param_find(str), &ascale.z_scale));
 
 					if (failed) {
-						warnx(CAL_ERROR_APPLY_CAL_MSG, "accel", i);
+						PX4_ERR(CAL_ERROR_APPLY_CAL_MSG, "accel", i);
 
 					} else {
 						/* apply new scaling and offsets */
 						config_ok = apply_accel_calibration(h, &ascale, device_id);
 
 						if (!config_ok) {
-							warnx(CAL_ERROR_APPLY_CAL_MSG, "accel ", i);
+							PX4_ERR(CAL_ERROR_APPLY_CAL_MSG, "accel ", i);
 						}
 					}
 
@@ -1502,7 +1496,7 @@ Sensors::parameter_update_poll(bool forced)
 					}
 
 					if (failed) {
-						warnx(CAL_ERROR_APPLY_CAL_MSG, "mag", i);
+						PX4_ERR(CAL_ERROR_APPLY_CAL_MSG, "mag", i);
 
 					} else {
 
@@ -1510,7 +1504,7 @@ Sensors::parameter_update_poll(bool forced)
 						config_ok = apply_mag_calibration(h, &mscale, device_id);
 
 						if (!config_ok) {
-							warnx(CAL_ERROR_APPLY_CAL_MSG, "mag ", i);
+							PX4_ERR(CAL_ERROR_APPLY_CAL_MSG, "mag ", i);
 						}
 					}
 
@@ -1540,8 +1534,6 @@ Sensors::parameter_update_poll(bool forced)
 			px4_close(fd);
 		}
 
-		/* do not output this for now, as its covered in preflight checks */
-		// warnx("valid configs: %u gyros, %u mags, %u accels", gyro_count, mag_count, accel_count);
 		_battery.updateParams();
 	}
 }
@@ -1637,17 +1629,17 @@ Sensors::rc_parameter_map_poll(bool forced)
 
 		}
 
-		warnx("rc to parameter map updated");
+		PX4_DEBUG("rc to parameter map updated");
 
 		for (int i = 0; i < rc_parameter_map_s::RC_PARAM_MAP_NCHAN; i++) {
-			warnx("\ti %d param_id %s scale %.3f value0 %.3f, min %.3f, max %.3f",
-			      i,
-			      &_rc_parameter_map.param_id[i * (rc_parameter_map_s::PARAM_ID_LEN + 1)],
-			      (double)_rc_parameter_map.scale[i],
-			      (double)_rc_parameter_map.value0[i],
-			      (double)_rc_parameter_map.value_min[i],
-			      (double)_rc_parameter_map.value_max[i]
-			     );
+			PX4_DEBUG("\ti %d param_id %s scale %.3f value0 %.3f, min %.3f, max %.3f",
+				  i,
+				  &_rc_parameter_map.param_id[i * (rc_parameter_map_s::PARAM_ID_LEN + 1)],
+				  (double)_rc_parameter_map.scale[i],
+				  (double)_rc_parameter_map.value0[i],
+				  (double)_rc_parameter_map.value_min[i],
+				  (double)_rc_parameter_map.value_max[i]
+				 );
 		}
 	}
 }
@@ -2217,14 +2209,14 @@ Sensors::task_main()
 				fds[0].fd = _gyro_sub[2];
 
 				if (!_hil_enabled) {
-					warnx("failing over to third gyro");
+					PX4_WARN("failing over to third gyro");
 				}
 
 			} else if (_gyro_sub[1] >= 0 && (fds[0].fd != _gyro_sub[1])) {
 				fds[0].fd = _gyro_sub[1];
 
 				if (!_hil_enabled) {
-					warnx("failing over to second gyro");
+					PX4_WARN("failing over to second gyro");
 				}
 			}
 		}
@@ -2295,7 +2287,6 @@ Sensors::task_main()
 	orb_unsubscribe(_actuator_ctrl_0_sub);
 	orb_unadvertise(_sensor_pub);
 
-	warnx("exiting.");
 	_sensors_task = -1;
 	px4_task_exit(ret);
 }
@@ -2319,7 +2310,7 @@ Sensors::start()
 	}
 
 	if (_sensors_task < 0) {
-		return -ERROR;
+		return -PX4_ERROR;
 	}
 
 	return OK;
@@ -2328,28 +2319,28 @@ Sensors::start()
 int sensors_main(int argc, char *argv[])
 {
 	if (argc < 2) {
-		warnx("usage: sensors {start|stop|status}");
+		PX4_INFO("usage: sensors {start|stop|status}");
 		return 0;
 	}
 
 	if (!strcmp(argv[1], "start")) {
 
 		if (sensors::g_sensors != nullptr) {
-			warnx("already running");
+			PX4_INFO("already running");
 			return 0;
 		}
 
 		sensors::g_sensors = new Sensors;
 
 		if (sensors::g_sensors == nullptr) {
-			warnx("alloc failed");
+			PX4_ERR("alloc failed");
 			return 1;
 		}
 
 		if (OK != sensors::g_sensors->start()) {
 			delete sensors::g_sensors;
 			sensors::g_sensors = nullptr;
-			warnx("start failed");
+			PX4_ERR("start failed");
 			return 1;
 		}
 
@@ -2358,7 +2349,7 @@ int sensors_main(int argc, char *argv[])
 
 	if (!strcmp(argv[1], "stop")) {
 		if (sensors::g_sensors == nullptr) {
-			warnx("not running");
+			PX4_INFO("not running");
 			return 1;
 		}
 
@@ -2369,15 +2360,15 @@ int sensors_main(int argc, char *argv[])
 
 	if (!strcmp(argv[1], "status")) {
 		if (sensors::g_sensors) {
-			warnx("is running");
+			PX4_INFO("is running");
 			return 0;
 
 		} else {
-			warnx("not running");
+			PX4_INFO("not running");
 			return 1;
 		}
 	}
 
-	warnx("unrecognized command");
+	PX4_ERR("unrecognized command");
 	return 1;
 }

From d846ad5dacfa4ab89fd611116d0ea8d21b9bc181 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Beat=20K=C3=BCng?= <beat-kueng@gmx.net>
Date: Sat, 25 Jun 2016 12:28:02 +0200
Subject: [PATCH 0927/1230] sensors: move voting into sensors module

- voting is now at a central place instead of duplicated within the
  estimators
  -> this also means that estimators that did not do voting so far,
     now have voting, like ekf2
- estimators requiring more than that can still subscribe to the raw
  sensors
- allows sensors_combined to be 3 times smaller
  - reduces logger, memcpy (cache) & RAM overhead
- all modules requiring only 1 or 2 sensor values now automatically get
  the voted result
- this also adds voting to baro
---
 msg/sensor_combined.msg                       |  22 +-
 src/drivers/frsky_telemetry/frsky_data.c      |   8 +-
 src/drivers/frsky_telemetry/sPort_data.c      |   2 +-
 src/drivers/hott/messages.cpp                 |   4 +-
 src/examples/px4_simple_app/px4_simple_app.c  |   2 +-
 .../terrain_estimation/terrain_estimator.cpp  |   2 +-
 .../attitude_estimator_ekf_main.cpp           |  16 +-
 .../attitude_estimator_q_main.cpp             | 173 +-----
 .../commander/calibration_routines.cpp        |   2 +-
 src/modules/commander/commander.cpp           |   2 +-
 src/modules/ekf2/ekf2_main.cpp                |  30 +-
 src/modules/ekf2_replay/ekf2_replay_main.cpp  |  10 +-
 .../AttitudePositionEstimatorEKF.h            |  11 -
 .../ekf_att_pos_estimator_main.cpp            | 136 +----
 .../fw_pos_control_l1_main.cpp                |   2 +-
 .../BlockLocalPositionEstimator.cpp           |   4 +-
 .../local_position_estimator/sensors/baro.cpp |   2 +-
 src/modules/mavlink/mavlink_messages.cpp      |  28 +-
 src/modules/mavlink/mavlink_receiver.cpp      |  18 +-
 src/modules/navigator/navigator_main.cpp      |   2 +-
 .../position_estimator_inav_main.cpp          |  22 +-
 src/modules/sdlog2/sdlog2.c                   | 118 ++--
 src/modules/sensors/sensors.cpp               | 576 +++++++++++-------
 23 files changed, 560 insertions(+), 632 deletions(-)

diff --git a/msg/sensor_combined.msg b/msg/sensor_combined.msg
index 790cdf3c68..8a7045f9d7 100644
--- a/msg/sensor_combined.msg
+++ b/msg/sensor_combined.msg
@@ -5,18 +5,18 @@
 # change with board revisions and sensor updates.
 #
 
-uint64[3] gyro_timestamp			# Gyro timestamps
-float32[9] gyro_integral_rad		# delta angle in the NED body frame in rad in the integration time frame
-uint64[3] gyro_integral_dt			# delta time for gyro integral in us
+# gyro timstamp is equal to the timestamp of the message
+float32[3] gyro_integral_rad		# delta angle in the NED body frame in rad in the integration time frame
+uint64 gyro_integral_dt			# delta time for gyro integral in us
 
-float32[9] accelerometer_integral_m_s		# velocity in NED body frame, in m/s^2
-uint64[3] accelerometer_integral_dt		# delta time for accel integral in us
-uint64[3] accelerometer_timestamp	# Accelerometer timestamp
+uint64 accelerometer_timestamp	# Accelerometer timestamp
+float32[3] accelerometer_integral_m_s		# velocity in NED body frame, in m/s^2
+uint64 accelerometer_integral_dt		# delta time for accel integral in us
 
-float32[9] magnetometer_ga		# Magnetic field in NED body frame, in Gauss
-uint64[3] magnetometer_timestamp	# Magnetometer timestamp
+uint64 magnetometer_timestamp	# Magnetometer timestamp
+float32[3] magnetometer_ga		# Magnetic field in NED body frame, in Gauss
 
-float32[3] baro_alt_meter			# Altitude, already temp. comp.
-float32[3] baro_temp_celcius		# Temperature in degrees celsius
-uint64[3] baro_timestamp		# Barometer timestamp
+uint64 baro_timestamp		# Barometer timestamp
+float32 baro_alt_meter			# Altitude, already temp. comp.
+float32 baro_temp_celcius		# Temperature in degrees celsius
 
diff --git a/src/drivers/frsky_telemetry/frsky_data.c b/src/drivers/frsky_telemetry/frsky_data.c
index 246ad91d42..47330774a3 100644
--- a/src/drivers/frsky_telemetry/frsky_data.c
+++ b/src/drivers/frsky_telemetry/frsky_data.c
@@ -205,7 +205,7 @@ void frsky_send_frame1(int uart)
 {
 	/* send formatted frame */
 	float acceleration[3];
-	float accel_dt = sensor_combined->accelerometer_integral_dt[0] / 1.e6f;
+	float accel_dt = sensor_combined->accelerometer_integral_dt / 1.e6f;
 	acceleration[0] = sensor_combined->accelerometer_integral_m_s[0] / accel_dt;
 	acceleration[1] = sensor_combined->accelerometer_integral_m_s[1] / accel_dt;
 	acceleration[2] = sensor_combined->accelerometer_integral_m_s[2] / accel_dt;
@@ -214,12 +214,12 @@ void frsky_send_frame1(int uart)
 	frsky_send_data(uart, FRSKY_ID_ACCEL_Z, roundf(acceleration[2] * 1000.0f));
 
 	frsky_send_data(uart, FRSKY_ID_BARO_ALT_BP,
-			sensor_combined->baro_alt_meter[0]);
+			sensor_combined->baro_alt_meter);
 	frsky_send_data(uart, FRSKY_ID_BARO_ALT_AP,
-			roundf(frac(sensor_combined->baro_alt_meter[0]) * 100.0f));
+			roundf(frac(sensor_combined->baro_alt_meter) * 100.0f));
 
 	frsky_send_data(uart, FRSKY_ID_TEMP1,
-			roundf(sensor_combined->baro_temp_celcius[0]));
+			roundf(sensor_combined->baro_temp_celcius));
 
 	frsky_send_data(uart, FRSKY_ID_VFAS,
 			roundf(battery_status->voltage_v * 10.0f));
diff --git a/src/drivers/frsky_telemetry/sPort_data.c b/src/drivers/frsky_telemetry/sPort_data.c
index c266b84924..c728ea6f8a 100644
--- a/src/drivers/frsky_telemetry/sPort_data.c
+++ b/src/drivers/frsky_telemetry/sPort_data.c
@@ -236,7 +236,7 @@ void sPort_send_CUR(int uart)
 void sPort_send_ALT(int uart)
 {
 	/* send data */
-	uint32_t alt = (int)(100 * sensor_combined->baro_alt_meter[0]);
+	uint32_t alt = (int)(100 * sensor_combined->baro_alt_meter);
 	sPort_send_data(uart, SMARTPORT_ID_ALT, alt);
 }
 
diff --git a/src/drivers/hott/messages.cpp b/src/drivers/hott/messages.cpp
index 1150fd0722..92b1664ccf 100644
--- a/src/drivers/hott/messages.cpp
+++ b/src/drivers/hott/messages.cpp
@@ -149,12 +149,12 @@ build_eam_response(uint8_t *buffer, size_t *size)
 	msg.eam_sensor_id = EAM_SENSOR_ID;
 	msg.sensor_text_id = EAM_SENSOR_TEXT_ID;
 
-	msg.temperature1 = (uint8_t)(raw.baro_temp_celcius[0] + 20);
+	msg.temperature1 = (uint8_t)(raw.baro_temp_celcius + 20);
 	msg.temperature2 = msg.temperature1 - BOARD_TEMP_OFFSET_DEG;
 
 	msg.main_voltage_L = (uint8_t)(battery.voltage_v * 10);
 
-	uint16_t alt = (uint16_t)(raw.baro_alt_meter[0] + 500);
+	uint16_t alt = (uint16_t)(raw.baro_alt_meter + 500);
 	msg.altitude_L = (uint8_t)alt & 0xff;
 	msg.altitude_H = (uint8_t)(alt >> 8) & 0xff;
 
diff --git a/src/examples/px4_simple_app/px4_simple_app.c b/src/examples/px4_simple_app/px4_simple_app.c
index a382c2224a..58ec077170 100644
--- a/src/examples/px4_simple_app/px4_simple_app.c
+++ b/src/examples/px4_simple_app/px4_simple_app.c
@@ -102,7 +102,7 @@ int px4_simple_app_main(int argc, char *argv[])
 				/* copy sensors raw data into local buffer */
 				orb_copy(ORB_ID(sensor_combined), sensor_sub_fd, &raw);
 				float sensor_accel[3];
-				float accel_dt = raw.accelerometer_integral_dt[0] / 1.e6f;
+				float accel_dt = raw.accelerometer_integral_dt / 1.e6f;
 				sensor_accel[0] = raw.accelerometer_integral_m_s[0] / accel_dt;
 				sensor_accel[1] = raw.accelerometer_integral_m_s[1] / accel_dt;
 				sensor_accel[2] = raw.accelerometer_integral_m_s[2] / accel_dt;
diff --git a/src/lib/terrain_estimation/terrain_estimator.cpp b/src/lib/terrain_estimation/terrain_estimator.cpp
index d8c88e618d..d42f57b27e 100644
--- a/src/lib/terrain_estimation/terrain_estimator.cpp
+++ b/src/lib/terrain_estimation/terrain_estimator.cpp
@@ -68,7 +68,7 @@ void TerrainEstimator::predict(float dt, const struct vehicle_attitude_s *attitu
 	if (attitude->R_valid) {
 		matrix::Matrix<float, 3, 3> R_att(attitude->R);
 		matrix::Vector<float, 3> a;
-		float accel_dt = sensor->accelerometer_integral_dt[0] / 1.e6f;
+		float accel_dt = sensor->accelerometer_integral_dt / 1.e6f;
 		a(0) = sensor->accelerometer_integral_m_s[0] / accel_dt;
 		a(1) = sensor->accelerometer_integral_m_s[1] / accel_dt;
 		a(2) = sensor->accelerometer_integral_m_s[2] / accel_dt;
diff --git a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp
index fa22ad165f..1a66b19f94 100755
--- a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp
+++ b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp
@@ -396,7 +396,7 @@ int attitude_estimator_ekf_thread_main(int argc, char *argv[])
 				}
 
 				float gyro_rad_s[3];
-				float gyro_dt = raw.gyro_integral_dt[0] / 1.e6f;
+				float gyro_dt = raw.gyro_integral_dt / 1.e6f;
 				gyro_rad_s[0] = raw.gyro_integral_rad[0] / gyro_dt;
 				gyro_rad_s[1] = raw.gyro_integral_rad[1] / gyro_dt;
 				gyro_rad_s[2] = raw.gyro_integral_rad[2] / gyro_dt;
@@ -427,10 +427,10 @@ int attitude_estimator_ekf_thread_main(int argc, char *argv[])
 					uint8_t update_vect[3] = {0, 0, 0};
 
 					/* Fill in gyro measurements */
-					if (sensor_last_timestamp[0] != raw.gyro_timestamp[0]) {
+					if (sensor_last_timestamp[0] != raw.timestamp) {
 						update_vect[0] = 1;
 						// sensor_update_hz[0] = 1e6f / (raw.timestamp - sensor_last_timestamp[0]);
-						sensor_last_timestamp[0] = raw.gyro_timestamp[0];
+						sensor_last_timestamp[0] = raw.timestamp;
 					}
 
 					z_k[0] =  gyro_rad_s[0] - gyro_offsets[0];
@@ -438,10 +438,10 @@ int attitude_estimator_ekf_thread_main(int argc, char *argv[])
 					z_k[2] =  gyro_rad_s[2] - gyro_offsets[2];
 
 					/* update accelerometer measurements */
-					if (sensor_last_timestamp[1] != raw.accelerometer_timestamp[0]) {
+					if (sensor_last_timestamp[1] != raw.accelerometer_timestamp) {
 						update_vect[1] = 1;
 						// sensor_update_hz[1] = 1e6f / (raw.timestamp - sensor_last_timestamp[1]);
-						sensor_last_timestamp[1] = raw.accelerometer_timestamp[0];
+						sensor_last_timestamp[1] = raw.accelerometer_timestamp;
 					}
 
 					hrt_abstime vel_t = 0;
@@ -477,7 +477,7 @@ int attitude_estimator_ekf_thread_main(int argc, char *argv[])
 					}
 
 					matrix::Vector3f raw_accel;
-					float accel_dt = raw.accelerometer_integral_dt[0] / 1.e6f;
+					float accel_dt = raw.accelerometer_integral_dt / 1.e6f;
 					raw_accel(0) = raw.accelerometer_integral_m_s[0] / accel_dt;
 					raw_accel(1) = raw.accelerometer_integral_m_s[1] / accel_dt;
 					raw_accel(2) = raw.accelerometer_integral_m_s[2] / accel_dt;
@@ -487,14 +487,14 @@ int attitude_estimator_ekf_thread_main(int argc, char *argv[])
 					z_k[5] = raw_accel(2) - acc(2);
 
 					/* update magnetometer measurements */
-					if (sensor_last_timestamp[2] != raw.magnetometer_timestamp[0] &&
+					if (sensor_last_timestamp[2] != raw.magnetometer_timestamp &&
 						/* check that the mag vector is > 0 */
 						fabsf(sqrtf(raw.magnetometer_ga[0] * raw.magnetometer_ga[0] +
 							raw.magnetometer_ga[1] * raw.magnetometer_ga[1] +
 							raw.magnetometer_ga[2] * raw.magnetometer_ga[2])) > 0.1f) {
 						update_vect[2] = 1;
 						// sensor_update_hz[2] = 1e6f / (raw.timestamp - sensor_last_timestamp[2]);
-						sensor_last_timestamp[2] = raw.magnetometer_timestamp[0];
+						sensor_last_timestamp[2] = raw.magnetometer_timestamp;
 					}
 
 					bool vision_updated = false;
diff --git a/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp b/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp
index 2a6a743661..f56a6793aa 100644
--- a/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp
+++ b/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp
@@ -67,7 +67,6 @@
 #include <mathlib/mathlib.h>
 #include <mathlib/math/filter/LowPassFilter2p.hpp>
 #include <lib/geo/geo.h>
-#include <lib/ecl/validation/data_validator_group.h>
 
 #include <systemlib/systemlib.h>
 #include <systemlib/param/param.h>
@@ -139,7 +138,6 @@ private:
 		param_t	mag_decl_auto;
 		param_t	acc_comp;
 		param_t	bias_max;
-		param_t vibe_thresh;
 		param_t	ext_hdg_mode;
 		param_t airspeed_mode;
 	}		_params_handles;		/**< handles for interesting parameters */
@@ -152,8 +150,6 @@ private:
 	bool		_mag_decl_auto = false;
 	bool		_acc_comp = false;
 	float		_bias_max = 0.0f;
-	float		_vibration_warning_threshold = 2.0f;
-	hrt_abstime	_vibration_warning_timestamp = 0;
 	int		_ext_hdg_mode = 0;
 	int 	_airspeed_mode = 0;
 
@@ -177,10 +173,6 @@ private:
 	Vector<3>	_vel_prev;
 	Vector<3>	_pos_acc;
 
-	DataValidatorGroup _voter_gyro;
-	DataValidatorGroup _voter_accel;
-	DataValidatorGroup _voter_mag;
-
 	/* Low pass filter for attitude rates */
 	math::LowPassFilter2p _lp_roll_rate;
 	math::LowPassFilter2p _lp_pitch_rate;
@@ -190,8 +182,6 @@ private:
 
 	bool		_inited = false;
 	bool		_data_good = false;
-	bool		_failsafe = false;
-	bool		_vibration_warning = false;
 	bool		_ext_hdg_good = false;
 
 	orb_advert_t	_mavlink_log_pub = nullptr;
@@ -215,15 +205,10 @@ private:
 AttitudeEstimatorQ::AttitudeEstimatorQ() :
 	_vel_prev(0, 0, 0),
 	_pos_acc(0, 0, 0),
-	_voter_gyro(3),
-	_voter_accel(3),
-	_voter_mag(3),
 	_lp_roll_rate(250.0f, 30.0f),
 	_lp_pitch_rate(250.0f, 30.0f),
 	_lp_yaw_rate(250.0f, 20.0f)
 {
-	_voter_mag.set_timeout(200000);
-
 	_params_handles.w_acc		= param_find("ATT_W_ACC");
 	_params_handles.w_mag		= param_find("ATT_W_MAG");
 	_params_handles.w_ext_hdg	= param_find("ATT_W_EXT_HDG");
@@ -232,7 +217,6 @@ AttitudeEstimatorQ::AttitudeEstimatorQ() :
 	_params_handles.mag_decl_auto	= param_find("ATT_MAG_DECL_A");
 	_params_handles.acc_comp	= param_find("ATT_ACC_COMP");
 	_params_handles.bias_max	= param_find("ATT_BIAS_MAX");
-	_params_handles.vibe_thresh	= param_find("ATT_VIBE_THRESH");
 	_params_handles.ext_hdg_mode	= param_find("ATT_EXT_HDG_M");
 	_params_handles.airspeed_mode = param_find("FW_ARSP_MODE");
 }
@@ -286,12 +270,6 @@ int AttitudeEstimatorQ::start()
 
 void AttitudeEstimatorQ::print()
 {
-	warnx("gyro status:");
-	_voter_gyro.print();
-	warnx("accel status:");
-	_voter_accel.print();
-	warnx("mag status:");
-	_voter_mag.print();
 }
 
 void AttitudeEstimatorQ::task_main_trampoline(int argc, char *argv[])
@@ -346,129 +324,40 @@ void AttitudeEstimatorQ::task_main()
 		// Update sensors
 		sensor_combined_s sensors;
 
-		int best_gyro = 0;
-		int best_accel = 0;
-		int best_mag = 0;
-
 		if (!orb_copy(ORB_ID(sensor_combined), _sensors_sub, &sensors)) {
 			// Feed validator with recent sensor data
 
-			for (unsigned i = 0; i < (sizeof(sensors.gyro_timestamp) / sizeof(sensors.gyro_timestamp[0])); i++) {
+			if (sensors.timestamp > 0) {
+				float gyro_dt = sensors.gyro_integral_dt / 1e6;
+				_gyro(0) = sensors.gyro_integral_rad[0] / gyro_dt;
+				_gyro(1) = sensors.gyro_integral_rad[1] / gyro_dt;
+				_gyro(2) = sensors.gyro_integral_rad[2] / gyro_dt;
+			}
 
-				/* ignore empty fields */
-				if (sensors.gyro_timestamp[i] > 0) {
+			if (sensors.accelerometer_timestamp > 0) {
+				float accel_dt = sensors.accelerometer_integral_dt / 1.e6f;
+				_accel(0) = sensors.accelerometer_integral_m_s[0] / accel_dt;
+				_accel(1) = sensors.accelerometer_integral_m_s[1] / accel_dt;
+				_accel(2) = sensors.accelerometer_integral_m_s[2] / accel_dt;
 
-					float gyro[3];
-					float gyro_dt = sensors.gyro_integral_dt[i] / 1e6;
-					gyro[0] = sensors.gyro_integral_rad[i * 3 + 0] / gyro_dt;
-					gyro[1] = sensors.gyro_integral_rad[i * 3 + 1] / gyro_dt;
-					gyro[2] = sensors.gyro_integral_rad[i * 3 + 2] / gyro_dt;
-
-					//TODO: note: voter will be moved into sensors module
-					//_voter_gyro.put(i, sensors.gyro_timestamp[i], &gyro[0], sensors.gyro_errcount[i], sensors.gyro_priority[i]);
-					_voter_gyro.put(i, sensors.gyro_timestamp[i], &gyro[0], 0, 75);
-				}
-
-				if (sensors.accelerometer_timestamp[i] > 0) {
-					float acceleration[3];
-					float accel_dt = sensors.accelerometer_integral_dt[i] / 1.e6f;
-					acceleration[0] = sensors.accelerometer_integral_m_s[i * 3 + 0] / accel_dt;
-					acceleration[1] = sensors.accelerometer_integral_m_s[i * 3 + 1] / accel_dt;
-					acceleration[2] = sensors.accelerometer_integral_m_s[i * 3 + 2] / accel_dt;
-					_voter_accel.put(i, sensors.accelerometer_timestamp[i], acceleration, 0, 75);
-				}
-
-				if (sensors.magnetometer_timestamp[i] > 0) {
-					_voter_mag.put(i, sensors.magnetometer_timestamp[i], &sensors.magnetometer_ga[i * 3], 0, 75);
+				if (_accel.length() < 0.01f) {
+					warnx("WARNING: degenerate accel!");
+					continue;
 				}
 			}
 
-			// Get best measurement values
-			hrt_abstime curr_time = hrt_absolute_time();
-			_gyro.set(_voter_gyro.get_best(curr_time, &best_gyro));
-			_accel.set(_voter_accel.get_best(curr_time, &best_accel));
-			_mag.set(_voter_mag.get_best(curr_time, &best_mag));
+			if (sensors.magnetometer_timestamp > 0) {
+				_mag(0) = sensors.magnetometer_ga[0];
+				_mag(1) = sensors.magnetometer_ga[1];
+				_mag(2) = sensors.magnetometer_ga[2];
 
-			if (_accel.length() < 0.01f) {
-				warnx("WARNING: degenerate accel!");
-				continue;
-			}
-
-			if (_mag.length() < 0.01f) {
-				warnx("WARNING: degenerate mag!");
-				continue;
+				if (_mag.length() < 0.01f) {
+					warnx("WARNING: degenerate mag!");
+					continue;
+				}
 			}
 
 			_data_good = true;
-
-			if (!_failsafe) {
-				uint32_t flags = DataValidator::ERROR_FLAG_NO_ERROR;
-
-#ifdef __PX4_POSIX
-				perf_end(_perf_accel);
-				perf_end(_perf_mpu);
-				perf_end(_perf_mag);
-#endif
-
-				if (_voter_gyro.failover_count() > 0) {
-					_failsafe = true;
-					flags = _voter_gyro.failover_state();
-					mavlink_and_console_log_emergency(&_mavlink_log_pub, "Gyro #%i failure :%s%s%s%s%s!",
-									  _voter_gyro.failover_index(),
-									  ((flags & DataValidator::ERROR_FLAG_NO_DATA) ? " No data" : ""),
-									  ((flags & DataValidator::ERROR_FLAG_STALE_DATA) ? " Stale data" : ""),
-									  ((flags & DataValidator::ERROR_FLAG_TIMEOUT) ? " Data timeout" : ""),
-									  ((flags & DataValidator::ERROR_FLAG_HIGH_ERRCOUNT) ? " High error count" : ""),
-									  ((flags & DataValidator::ERROR_FLAG_HIGH_ERRDENSITY) ? " High error density" : ""));
-				}
-
-				if (_voter_accel.failover_count() > 0) {
-					_failsafe = true;
-					flags = _voter_accel.failover_state();
-					mavlink_and_console_log_emergency(&_mavlink_log_pub, "Accel #%i failure :%s%s%s%s%s!",
-									  _voter_accel.failover_index(),
-									  ((flags & DataValidator::ERROR_FLAG_NO_DATA) ? " No data" : ""),
-									  ((flags & DataValidator::ERROR_FLAG_STALE_DATA) ? " Stale data" : ""),
-									  ((flags & DataValidator::ERROR_FLAG_TIMEOUT) ? " Data timeout" : ""),
-									  ((flags & DataValidator::ERROR_FLAG_HIGH_ERRCOUNT) ? " High error count" : ""),
-									  ((flags & DataValidator::ERROR_FLAG_HIGH_ERRDENSITY) ? " High error density" : ""));
-				}
-
-				if (_voter_mag.failover_count() > 0) {
-					_failsafe = true;
-					flags = _voter_mag.failover_state();
-					mavlink_and_console_log_emergency(&_mavlink_log_pub, "Mag #%i failure :%s%s%s%s%s!",
-									  _voter_mag.failover_index(),
-									  ((flags & DataValidator::ERROR_FLAG_NO_DATA) ? " No data" : ""),
-									  ((flags & DataValidator::ERROR_FLAG_STALE_DATA) ? " Stale data" : ""),
-									  ((flags & DataValidator::ERROR_FLAG_TIMEOUT) ? " Data timeout" : ""),
-									  ((flags & DataValidator::ERROR_FLAG_HIGH_ERRCOUNT) ? " High error count" : ""),
-									  ((flags & DataValidator::ERROR_FLAG_HIGH_ERRDENSITY) ? " High error density" : ""));
-				}
-
-				if (_failsafe) {
-					mavlink_and_console_log_emergency(&_mavlink_log_pub, "SENSOR FAILSAFE! RETURN TO LAND IMMEDIATELY");
-				}
-			}
-
-			if (!_vibration_warning && (_voter_gyro.get_vibration_factor(curr_time) > _vibration_warning_threshold ||
-						    _voter_accel.get_vibration_factor(curr_time) > _vibration_warning_threshold ||
-						    _voter_mag.get_vibration_factor(curr_time) > _vibration_warning_threshold)) {
-
-				if (_vibration_warning_timestamp == 0) {
-					_vibration_warning_timestamp = curr_time;
-
-				} else if (hrt_elapsed_time(&_vibration_warning_timestamp) > 10000000) {
-					_vibration_warning = true;
-					// mavlink_and_console_log_critical(&_mavlink_log_pub, "HIGH VIBRATION! g: %d a: %d m: %d",
-					// 				 (int)(100 * _voter_gyro.get_vibration_factor(curr_time)),
-					// 				 (int)(100 * _voter_accel.get_vibration_factor(curr_time)),
-					// 				 (int)(100 * _voter_mag.get_vibration_factor(curr_time)));
-				}
-
-			} else {
-				_vibration_warning_timestamp = 0;
-			}
 		}
 
 		// Update vision and motion capture heading
@@ -596,10 +485,6 @@ void AttitudeEstimatorQ::task_main()
 		memcpy(&att.q[0], _q.data, sizeof(att.q));
 		att.q_valid = true;
 
-		att.rate_vibration = _voter_gyro.get_vibration_factor(hrt_absolute_time());
-		att.accel_vibration = _voter_accel.get_vibration_factor(hrt_absolute_time());
-		att.mag_vibration = _voter_mag.get_vibration_factor(hrt_absolute_time());
-
 		/* the instance count is not used here */
 		int att_inst;
 		orb_publish_auto(ORB_ID(vehicle_attitude), &_att_pub, &att, &att_inst, ORB_PRIO_HIGH);
@@ -656,12 +541,9 @@ void AttitudeEstimatorQ::task_main()
 		}
 
 		{
-			struct estimator_status_s est = {};
+			//struct estimator_status_s est = {};
 
-			est.timestamp = sensors.timestamp;
-			est.vibe[0] = _voter_accel.get_vibration_offset(est.timestamp, 0);
-			est.vibe[1] = _voter_accel.get_vibration_offset(est.timestamp, 1);
-			est.vibe[2] = _voter_accel.get_vibration_offset(est.timestamp, 2);
+			//est.timestamp = sensors.timestamp;
 
 			/* the instance count is not used here */
 			//int est_inst;
@@ -672,6 +554,12 @@ void AttitudeEstimatorQ::task_main()
 		}
 	}
 
+#ifdef __PX4_POSIX
+	perf_end(_perf_accel);
+	perf_end(_perf_mpu);
+	perf_end(_perf_mag);
+#endif
+
 	orb_unsubscribe(_sensors_sub);
 	orb_unsubscribe(_vision_sub);
 	orb_unsubscribe(_mocap_sub);
@@ -706,7 +594,6 @@ void AttitudeEstimatorQ::update_parameters(bool force)
 		param_get(_params_handles.acc_comp, &acc_comp_int);
 		_acc_comp = acc_comp_int != 0;
 		param_get(_params_handles.bias_max, &_bias_max);
-		param_get(_params_handles.vibe_thresh, &_vibration_warning_threshold);
 		param_get(_params_handles.ext_hdg_mode, &_ext_hdg_mode);
 		param_get(_params_handles.airspeed_mode, &_airspeed_mode);
 	}
diff --git a/src/modules/commander/calibration_routines.cpp b/src/modules/commander/calibration_routines.cpp
index 39dc65155e..7138f34309 100644
--- a/src/modules/commander/calibration_routines.cpp
+++ b/src/modules/commander/calibration_routines.cpp
@@ -273,7 +273,7 @@ enum detect_orientation_return detect_orientation(orb_advert_t *mavlink_log_pub,
 
 			for (unsigned i = 0; i < ndim; i++) {
 
-				float di = sensor.accelerometer_integral_m_s[i] / (sensor.accelerometer_integral_dt[0] / 1.e6f);
+				float di = sensor.accelerometer_integral_m_s[i] / (sensor.accelerometer_integral_dt / 1.e6f);
 
 				float d = di - accel_ema[i];
 				accel_ema[i] += d * w;
diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp
index 81ccc4e852..c55c2d57d0 100644
--- a/src/modules/commander/commander.cpp
+++ b/src/modules/commander/commander.cpp
@@ -1788,7 +1788,7 @@ int commander_thread_main(int argc, char *argv[])
 			 * vertical separation from other airtraffic the operator has to know when the
 			 * barometer is inoperational.
 			 * */
-			if (hrt_elapsed_time(&sensors.baro_timestamp[0]) < FAILSAFE_DEFAULT_TIMEOUT) {
+			if (hrt_elapsed_time(&sensors.baro_timestamp) < FAILSAFE_DEFAULT_TIMEOUT) {
 				/* handle the case where baro was regained */
 				if (status_flags.barometer_failure) {
 					status_flags.barometer_failure = false;
diff --git a/src/modules/ekf2/ekf2_main.cpp b/src/modules/ekf2/ekf2_main.cpp
index ddc471e234..cb6e21d3aa 100644
--- a/src/modules/ekf2/ekf2_main.cpp
+++ b/src/modules/ekf2/ekf2_main.cpp
@@ -505,14 +505,14 @@ void Ekf2::task_main()
 		}
 
 		// push imu data into estimator
-		_ekf.setIMUData(now, sensors.gyro_integral_dt[0], sensors.accelerometer_integral_dt[0],
-				&sensors.gyro_integral_rad[0], &sensors.accelerometer_integral_m_s[0]);
+		_ekf.setIMUData(now, sensors.gyro_integral_dt, sensors.accelerometer_integral_dt,
+				sensors.gyro_integral_rad, sensors.accelerometer_integral_m_s);
 
 		// read mag data
-		_ekf.setMagData(sensors.magnetometer_timestamp[0], &sensors.magnetometer_ga[0]);
+		_ekf.setMagData(sensors.magnetometer_timestamp, sensors.magnetometer_ga);
 
 		// read baro data
-		_ekf.setBaroData(sensors.baro_timestamp[0], &sensors.baro_alt_meter[0]);
+		_ekf.setBaroData(sensors.baro_timestamp, &sensors.baro_alt_meter);
 
 		// read gps data if available
 		if (gps_updated) {
@@ -615,7 +615,7 @@ void Ekf2::task_main()
 			float gyro_bias[3] = {};
 			_ekf.get_gyro_bias(gyro_bias);
 			float gyro_rad_s[3];
-			float gyro_dt = sensors.gyro_integral_dt[0] / 1.e6f;
+			float gyro_dt = sensors.gyro_integral_dt / 1.e6f;
 			gyro_rad_s[0] = sensors.gyro_integral_rad[0] / gyro_dt - gyro_bias[0];
 			gyro_rad_s[1] = sensors.gyro_integral_rad[1] / gyro_dt - gyro_bias[1];
 			gyro_rad_s[2] = sensors.gyro_integral_rad[2] / gyro_dt - gyro_bias[2];
@@ -650,7 +650,7 @@ void Ekf2::task_main()
 
 			// Acceleration data
 			matrix::Vector<float, 3> acceleration;
-			float accel_dt = sensors.accelerometer_integral_dt[0] / 1.e6f;
+			float accel_dt = sensors.accelerometer_integral_dt / 1.e6f;
 			acceleration(0) = sensors.accelerometer_integral_m_s[0] / accel_dt;
 			acceleration(1) = sensors.accelerometer_integral_m_s[1] / accel_dt;
 			acceleration(2) = sensors.accelerometer_integral_m_s[2] / accel_dt;
@@ -811,7 +811,7 @@ void Ekf2::task_main()
 				// TODO use innovatun consistency check timouts to set this
 				global_pos.dead_reckoning = false; // True if this position is estimated through dead-reckoning
 
-				global_pos.pressure_alt = sensors.baro_alt_meter[0]; // Pressure altitude AMSL (m)
+				global_pos.pressure_alt = sensors.baro_alt_meter; // Pressure altitude AMSL (m)
 
 				if (_vehicle_global_position_pub == nullptr) {
 					_vehicle_global_position_pub = orb_advertise(ORB_ID(vehicle_global_position), &global_pos);
@@ -905,15 +905,15 @@ void Ekf2::task_main()
 		if (publish_replay_message) {
 			struct ekf2_replay_s replay = {};
 			replay.time_ref = now;
-			replay.gyro_integral_dt = sensors.gyro_integral_dt[0];
-			replay.accelerometer_integral_dt = sensors.accelerometer_integral_dt[0];
-			replay.magnetometer_timestamp = sensors.magnetometer_timestamp[0];
-			replay.baro_timestamp = sensors.baro_timestamp[0];
-			memcpy(&replay.gyro_integral_rad[0], &sensors.gyro_integral_rad[0], sizeof(replay.gyro_integral_rad));
-			memcpy(&replay.accelerometer_integral_m_s[0], &sensors.accelerometer_integral_m_s[0],
+			replay.gyro_integral_dt = sensors.gyro_integral_dt;
+			replay.accelerometer_integral_dt = sensors.accelerometer_integral_dt;
+			replay.magnetometer_timestamp = sensors.magnetometer_timestamp;
+			replay.baro_timestamp = sensors.baro_timestamp;
+			memcpy(replay.gyro_integral_rad, sensors.gyro_integral_rad, sizeof(replay.gyro_integral_rad));
+			memcpy(replay.accelerometer_integral_m_s, sensors.accelerometer_integral_m_s,
 			       sizeof(replay.accelerometer_integral_m_s));
-			memcpy(&replay.magnetometer_ga[0], &sensors.magnetometer_ga[0], sizeof(replay.magnetometer_ga));
-			replay.baro_alt_meter = sensors.baro_alt_meter[0];
+			memcpy(replay.magnetometer_ga, sensors.magnetometer_ga, sizeof(replay.magnetometer_ga));
+			replay.baro_alt_meter = sensors.baro_alt_meter;
 
 			// only write gps data if we had a gps update.
 			if (gps_updated) {
diff --git a/src/modules/ekf2_replay/ekf2_replay_main.cpp b/src/modules/ekf2_replay/ekf2_replay_main.cpp
index 7faae2f89f..b92014081f 100644
--- a/src/modules/ekf2_replay/ekf2_replay_main.cpp
+++ b/src/modules/ekf2_replay/ekf2_replay_main.cpp
@@ -377,10 +377,10 @@ void Ekf2Replay::setEstimatorInput(uint8_t *data, uint8_t type)
 		uint8_t *dest_ptr = (uint8_t *)&replay_part1.time_ref;
 		parseMessage(data, dest_ptr, type);
 		_sensors.timestamp = replay_part1.time_ref;
-		_sensors.gyro_integral_dt[0] = replay_part1.gyro_integral_dt;
-		_sensors.accelerometer_integral_dt[0] = replay_part1.accelerometer_integral_dt;
-		_sensors.magnetometer_timestamp[0] = replay_part1.magnetometer_timestamp;
-		_sensors.baro_timestamp[0] = replay_part1.baro_timestamp;
+		_sensors.gyro_integral_dt = replay_part1.gyro_integral_dt;
+		_sensors.accelerometer_integral_dt = replay_part1.accelerometer_integral_dt;
+		_sensors.magnetometer_timestamp = replay_part1.magnetometer_timestamp;
+		_sensors.baro_timestamp = replay_part1.baro_timestamp;
 		_sensors.gyro_integral_rad[0] = replay_part1.gyro_integral_x_rad;
 		_sensors.gyro_integral_rad[1] = replay_part1.gyro_integral_y_rad;
 		_sensors.gyro_integral_rad[2] = replay_part1.gyro_integral_z_rad;
@@ -390,7 +390,7 @@ void Ekf2Replay::setEstimatorInput(uint8_t *data, uint8_t type)
 		_sensors.magnetometer_ga[0] = replay_part1.magnetometer_x_ga;
 		_sensors.magnetometer_ga[1] = replay_part1.magnetometer_y_ga;
 		_sensors.magnetometer_ga[2] = replay_part1.magnetometer_z_ga;
-		_sensors.baro_alt_meter[0] = replay_part1.baro_alt_meter;
+		_sensors.baro_alt_meter = replay_part1.baro_alt_meter;
 		_part1_counter_ref = _message_counter;
 
 	} else if (type == LOG_RPL2_MSG) {
diff --git a/src/modules/ekf_att_pos_estimator/AttitudePositionEstimatorEKF.h b/src/modules/ekf_att_pos_estimator/AttitudePositionEstimatorEKF.h
index b66025c30b..92a729a413 100644
--- a/src/modules/ekf_att_pos_estimator/AttitudePositionEstimatorEKF.h
+++ b/src/modules/ekf_att_pos_estimator/AttitudePositionEstimatorEKF.h
@@ -71,7 +71,6 @@
 #include <geo/geo.h>
 #include <terrain_estimation/terrain_estimator.h>
 #include <systemlib/perf_counter.h>
-#include <lib/ecl/validation/data_validator_group.h>
 #include "estimator_22states.h"
 
 #include <controllib/blocks.hpp>
@@ -191,8 +190,6 @@ private:
     float                       _filter_ref_offset;   /**< offset between initial baro reference and GPS init baro altitude */
     float                       _baro_gps_offset;   /**< offset between baro altitude (at GPS init time) and GPS altitude */
     hrt_abstime                 _last_debug_print = 0;
-    float       _vibration_warning_threshold = 2.0f;
-    hrt_abstime _vibration_warning_timestamp = 0;
 
     perf_counter_t  _loop_perf;         ///< loop performance counter
     perf_counter_t  _loop_intvl;        ///< loop rate counter
@@ -213,15 +210,7 @@ private:
     hrt_abstime     _filter_start_time;
     hrt_abstime     _last_sensor_timestamp;
     hrt_abstime     _distance_last_valid;
-    DataValidatorGroup _voter_gyro;
-    DataValidatorGroup _voter_accel;
-    DataValidatorGroup _voter_mag;
-    int             _gyro_main;         ///< index of the main gyroscope
-    int             _accel_main;        ///< index of the main accelerometer
-    int             _mag_main;          ///< index of the main magnetometer
     bool            _data_good;         ///< all required filter data is ok
-    bool            _failsafe;          ///< failsafe on one of the sensors
-    bool            _vibration_warning; ///< high vibration levels detected
     bool            _ekf_logging;       ///< log EKF state
     unsigned        _debug;             ///< debug level - default 0
     bool            _was_landed;        ///< indicates if was landed in previous iteration
diff --git a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp
index ac19b192fb..d426c8d3ea 100644
--- a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp
+++ b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp
@@ -205,15 +205,7 @@ AttitudePositionEstimatorEKF::AttitudePositionEstimatorEKF() :
 	_filter_start_time(0),
 	_last_sensor_timestamp(hrt_absolute_time()),
 	_distance_last_valid(0),
-	_voter_gyro(3),
-	_voter_accel(3),
-	_voter_mag(3),
-	_gyro_main(-1),
-	_accel_main(-1),
-	_mag_main(-1),
 	_data_good(false),
-	_failsafe(false),
-	_vibration_warning(false),
 	_ekf_logging(true),
 	_debug(0),
 	_was_landed(true),
@@ -236,8 +228,6 @@ AttitudePositionEstimatorEKF::AttitudePositionEstimatorEKF() :
 	_LP_att_Q(250.0f, 20.0f),
 	_LP_att_R(250.0f, 20.0f)
 {
-	_voter_mag.set_timeout(200000);
-
 	_terrain_estimator = new TerrainEstimator();
 
 	_parameter_handles.vel_delay_ms = param_find("PE_VEL_DELAY_MS");
@@ -620,19 +610,9 @@ void AttitudePositionEstimatorEKF::task_main()
 				/* system is in HIL now, wait for measurements to come in one last round */
 				usleep(60000);
 
-				/* HIL is slow, set permissive timeouts */
-				_voter_gyro.set_timeout(500000);
-				_voter_accel.set_timeout(500000);
-				_voter_mag.set_timeout(500000);
-
 				/* now read all sensor publications to ensure all real sensor data is purged */
 				orb_copy(ORB_ID(sensor_combined), _sensor_combined_sub, &_sensor_combined);
 
-				/* set sensors to de-initialized state */
-				_gyro_main = -1;
-				_accel_main = -1;
-				_mag_main = -1;
-
 				_baro_init = false;
 				_gps_initialized = false;
 
@@ -664,7 +644,8 @@ void AttitudePositionEstimatorEKF::task_main()
 			 *    We run the filter only once all data has been fetched
 			 **/
 
-			if (_baro_init && (_gyro_main >= 0) && (_accel_main >= 0) && (_mag_main >= 0)) {
+			if (_baro_init && _sensor_combined.accelerometer_timestamp && _sensor_combined.timestamp
+					&& _sensor_combined.magnetometer_timestamp) {
 
 				// maintain filtered baro and gps altitudes to calculate weather offset
 				// baro sample rate is ~70Hz and measurement bandwidth is high
@@ -1343,13 +1324,6 @@ void AttitudePositionEstimatorEKF::print_status()
 		 (_ekf->useAirspeed) ? "USE_AIRSPD" : "IGN_AIRSPD",
 		 (_ekf->useCompass) ? "USE_COMPASS" : "IGN_COMPASS",
 		 (_ekf->staticMode) ? "STATIC_MODE" : "DYNAMIC_MODE");
-
-	PX4_INFO("gyro status:");
-	_voter_gyro.print();
-	PX4_INFO("accel status:");
-	_voter_accel.print();
-	PX4_INFO("mag status:");
-	_voter_mag.print();
 }
 
 void AttitudePositionEstimatorEKF::pollData()
@@ -1382,97 +1356,37 @@ void AttitudePositionEstimatorEKF::pollData()
 	/* fill in last data set */
 	_ekf->dtIMU = deltaT;
 
-	// Feed validator with recent sensor data
+	_ekf->dAngIMU.x = _sensor_combined.gyro_integral_rad[0];
+	_ekf->dAngIMU.y = _sensor_combined.gyro_integral_rad[1];
+	_ekf->dAngIMU.z = _sensor_combined.gyro_integral_rad[2];
 
-	//TODO: note, we will move voters into sensors module
-	for (unsigned i = 0; i < (sizeof(_sensor_combined.gyro_timestamp) / sizeof(_sensor_combined.gyro_timestamp[0])); i++) {
-		float gyro_rad_s[3];
-		float gyro_dt = _sensor_combined.gyro_integral_dt[i] / 1.e6f;
-		gyro_rad_s[0] = _sensor_combined.gyro_integral_rad[i * 3 + 0] / gyro_dt;
-		gyro_rad_s[1] = _sensor_combined.gyro_integral_rad[i * 3 + 1] / gyro_dt;
-		gyro_rad_s[2] = _sensor_combined.gyro_integral_rad[i * 3 + 2] / gyro_dt;
-		_voter_gyro.put(i, _sensor_combined.gyro_timestamp[i], gyro_rad_s, 0, 75);
-		float acceleration[3];
-		float accel_dt = _sensor_combined.accelerometer_integral_dt[i] / 1.e6f;
-		acceleration[0] = _sensor_combined.accelerometer_integral_m_s[i * 3 + 0] / accel_dt;
-		acceleration[1] = _sensor_combined.accelerometer_integral_m_s[i * 3 + 1] / accel_dt;
-		acceleration[2] = _sensor_combined.accelerometer_integral_m_s[i * 3 + 2] / accel_dt;
-		_voter_accel.put(i, _sensor_combined.accelerometer_timestamp[i], acceleration, 0, 75);
-		_voter_mag.put(i, _sensor_combined.magnetometer_timestamp[i], &_sensor_combined.magnetometer_ga[i * 3], 0, 75);
-	}
+	float gyro_dt = _sensor_combined.gyro_integral_dt / 1.e6f;
+	_ekf->angRate = _ekf->dAngIMU / gyro_dt;
 
-	// Get best measurement values
-	hrt_abstime curr_time = hrt_absolute_time();
-	(void)_voter_gyro.get_best(curr_time, &_gyro_main);
+	perf_count(_perf_gyro);
 
-	if (_gyro_main >= 0) {
+	if (_last_accel != _sensor_combined.accelerometer_timestamp) {
 
-		_ekf->dAngIMU.x = _sensor_combined.gyro_integral_rad[_gyro_main * 3 + 0];
-		_ekf->dAngIMU.y = _sensor_combined.gyro_integral_rad[_gyro_main * 3 + 1];
-		_ekf->dAngIMU.z = _sensor_combined.gyro_integral_rad[_gyro_main * 3 + 2];
+		_ekf->dVelIMU.x = _sensor_combined.accelerometer_integral_m_s[0];
+		_ekf->dVelIMU.y = _sensor_combined.accelerometer_integral_m_s[1];
+		_ekf->dVelIMU.z = _sensor_combined.accelerometer_integral_m_s[2];
 
-		float gyro_dt = _sensor_combined.gyro_integral_dt[_gyro_main] / 1.e6f;
-		_ekf->angRate = _ekf->dAngIMU / gyro_dt;
-
-		perf_count(_perf_gyro);
-	}
-
-	(void)_voter_accel.get_best(curr_time, &_accel_main);
-
-	if (_accel_main >= 0 && (_last_accel != _sensor_combined.accelerometer_timestamp[_accel_main])) {
-
-		_ekf->dVelIMU.x = _sensor_combined.accelerometer_integral_m_s[_accel_main * 3 + 0];
-		_ekf->dVelIMU.y = _sensor_combined.accelerometer_integral_m_s[_accel_main * 3 + 1];
-		_ekf->dVelIMU.z = _sensor_combined.accelerometer_integral_m_s[_accel_main * 3 + 2];
-
-		float accel_dt = _sensor_combined.accelerometer_integral_dt[_accel_main] / 1.e6f;
+		float accel_dt = _sensor_combined.accelerometer_integral_dt / 1.e6f;
 		_ekf->accel = _ekf->dVelIMU / accel_dt;
 
-		_last_accel = _sensor_combined.accelerometer_timestamp[_accel_main];
+		_last_accel = _sensor_combined.accelerometer_timestamp;
 	}
 
-	(void)_voter_mag.get_best(curr_time, &_mag_main);
+	Vector3f mag(_sensor_combined.magnetometer_ga[0], _sensor_combined.magnetometer_ga[1],
+			_sensor_combined.magnetometer_ga[2]);
 
-	if (_mag_main >= 0) {
-		Vector3f mag(_sensor_combined.magnetometer_ga[_mag_main * 3 + 0], _sensor_combined.magnetometer_ga[_mag_main * 3 + 1],
-			     _sensor_combined.magnetometer_ga[_mag_main * 3 + 2]);
-
-		/* fail over to the 2nd mag if we know the first is down */
-		if (mag.length() > 0.1f && (_last_mag != _sensor_combined.magnetometer_timestamp[_mag_main])) {
-			_ekf->magData.x = mag.x;
-			_ekf->magData.y = mag.y;
-			_ekf->magData.z = mag.z;
-			_newDataMag = true;
-			_last_mag = _sensor_combined.magnetometer_timestamp[_mag_main];
-			perf_count(_perf_mag);
-		}
-	}
-
-	if (!_failsafe && (_voter_gyro.failover_count() > 0 ||
-			   _voter_accel.failover_count() > 0 ||
-			   _voter_mag.failover_count() > 0)) {
-
-		_failsafe = true;
-		mavlink_and_console_log_emergency(&_mavlink_log_pub, "SENSOR FAILSAFE! RETURN TO LAND IMMEDIATELY");
-	}
-
-	if (!_vibration_warning && (_voter_gyro.get_vibration_factor(curr_time) > _vibration_warning_threshold ||
-				    _voter_accel.get_vibration_factor(curr_time) > _vibration_warning_threshold ||
-				    _voter_mag.get_vibration_factor(curr_time) > _vibration_warning_threshold)) {
-
-		if (_vibration_warning_timestamp == 0) {
-			_vibration_warning_timestamp = curr_time;
-
-		} else if (hrt_elapsed_time(&_vibration_warning_timestamp) > 10000000) {
-			_vibration_warning = true;
-			// mavlink_and_console_log_critical(&_mavlink_log_pub, "HIGH VIBRATION! g: %d a: %d m: %d",
-			// 				 (int)(100 * _voter_gyro.get_vibration_factor(curr_time)),
-			// 				 (int)(100 * _voter_accel.get_vibration_factor(curr_time)),
-			// 				 (int)(100 * _voter_mag.get_vibration_factor(curr_time)));
-		}
-
-	} else {
-		_vibration_warning_timestamp = 0;
+	if (mag.length() > 0.1f && _last_mag != _sensor_combined.magnetometer_timestamp) {
+		_ekf->magData.x = mag.x;
+		_ekf->magData.y = mag.y;
+		_ekf->magData.z = mag.z;
+		_newDataMag = true;
+		_last_mag = _sensor_combined.magnetometer_timestamp;
+		perf_count(_perf_mag);
 	}
 
 	_last_sensor_timestamp = _sensor_combined.timestamp;
@@ -1481,8 +1395,8 @@ void AttitudePositionEstimatorEKF::pollData()
 	// leave this in as long as larger improvements are still being made.
 #if 0
 
-	float deltaTIntegral = (_sensor_combined.gyro_integral_dt[0]) / 1e6f;
-	float deltaTIntAcc = (_sensor_combined.accelerometer_integral_dt[0]) / 1e6f;
+	float deltaTIntegral = _sensor_combined.gyro_integral_dt / 1e6f;
+	float deltaTIntAcc = _sensor_combined.accelerometer_integral_dt / 1e6f;
 
 	static unsigned dtoverflow5 = 0;
 	static unsigned dtoverflow10 = 0;
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 3fddf2c7b6..f604581a75 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
@@ -1231,7 +1231,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
 
 	/* filter speed and altitude for controller */
 	math::Vector<3> accel_body;
-	float accel_dt = _sensor_combined.accelerometer_integral_dt[0] / 1.e6f;
+	float accel_dt = _sensor_combined.accelerometer_integral_dt / 1.e6f;
 	accel_body(0) = _sensor_combined.accelerometer_integral_m_s[0] / accel_dt;
 	accel_body(1) = _sensor_combined.accelerometer_integral_m_s[1] / accel_dt;
 	accel_body(2) = _sensor_combined.accelerometer_integral_m_s[2] / accel_dt;
diff --git a/src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp b/src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp
index 493a12e997..cb0301f8a0 100644
--- a/src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp
+++ b/src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp
@@ -744,7 +744,7 @@ void BlockLocalPositionEstimator::publishGlobalPos()
 		_pub_gpos.get().terrain_alt = _altHome - xLP(X_tz);
 		_pub_gpos.get().terrain_alt_valid = _validTZ;
 		_pub_gpos.get().dead_reckoning = !_validXY && !_xyTimeout;
-		_pub_gpos.get().pressure_alt = _sub_sensor.get().baro_alt_meter[0];
+		_pub_gpos.get().pressure_alt = _sub_sensor.get().baro_alt_meter;
 		_pub_gpos.update();
 	}
 }
@@ -845,7 +845,7 @@ void BlockLocalPositionEstimator::predict()
 	if (integrate && _sub_att.get().R_valid) {
 		Matrix3f R_att(_sub_att.get().R);
 		Vector3f a;
-		float accel_dt = _sub_sensor.get().accelerometer_integral_dt[0] / 1.e6f;
+		float accel_dt = _sub_sensor.get().accelerometer_integral_dt / 1.e6f;
 		a(0) = _sub_sensor.get().accelerometer_integral_m_s[0] / accel_dt;
 		a(1) = _sub_sensor.get().accelerometer_integral_m_s[1] / accel_dt;
 		a(2) = _sub_sensor.get().accelerometer_integral_m_s[2] / accel_dt;
diff --git a/src/modules/local_position_estimator/sensors/baro.cpp b/src/modules/local_position_estimator/sensors/baro.cpp
index 57fc87eb5f..ffe8e51611 100644
--- a/src/modules/local_position_estimator/sensors/baro.cpp
+++ b/src/modules/local_position_estimator/sensors/baro.cpp
@@ -41,7 +41,7 @@ int BlockLocalPositionEstimator::baroMeasure(Vector<float, n_y_baro> &y)
 {
 	//measure
 	y.setZero();
-	y(0) = _sub_sensor.get().baro_alt_meter[0];
+	y(0) = _sub_sensor.get().baro_alt_meter;
 	_baroStats.update(y);
 	_time_last_baro = _timeStamp;
 	return OK;
diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp
index a699974ca1..da77fd07ba 100644
--- a/src/modules/mavlink/mavlink_messages.cpp
+++ b/src/modules/mavlink/mavlink_messages.cpp
@@ -724,39 +724,39 @@ protected:
 		if (_sensor_sub->update(&_sensor_time, &sensor)) {
 			uint16_t fields_updated = 0;
 
-			if (_accel_timestamp != sensor.accelerometer_timestamp[0]) {
+			if (_accel_timestamp != sensor.accelerometer_timestamp) {
 				/* mark first three dimensions as changed */
 				fields_updated |= (1 << 0) | (1 << 1) | (1 << 2);
-				_accel_timestamp = sensor.accelerometer_timestamp[0];
+				_accel_timestamp = sensor.accelerometer_timestamp;
 			}
 
-			if (_gyro_timestamp != sensor.gyro_timestamp[0]) {
+			if (_gyro_timestamp != sensor.timestamp) {
 				/* mark second group dimensions as changed */
 				fields_updated |= (1 << 3) | (1 << 4) | (1 << 5);
-				_gyro_timestamp = sensor.gyro_timestamp[0];
+				_gyro_timestamp = sensor.timestamp;
 			}
 
-			if (_mag_timestamp != sensor.magnetometer_timestamp[0]) {
+			if (_mag_timestamp != sensor.magnetometer_timestamp) {
 				/* mark third group dimensions as changed */
 				fields_updated |= (1 << 6) | (1 << 7) | (1 << 8);
-				_mag_timestamp = sensor.magnetometer_timestamp[0];
+				_mag_timestamp = sensor.magnetometer_timestamp;
 			}
 
-			if (_baro_timestamp != sensor.baro_timestamp[0]) {
+			if (_baro_timestamp != sensor.baro_timestamp) {
 				/* mark last group dimensions as changed */
 				fields_updated |= (1 << 9) | (1 << 11) | (1 << 12);
-				_baro_timestamp = sensor.baro_timestamp[0];
+				_baro_timestamp = sensor.baro_timestamp;
 			}
 			_differential_pressure_sub->update(&_differential_pressure_time, &differential_pressure);
 
 			mavlink_highres_imu_t msg;
 
 			msg.time_usec = sensor.timestamp;
-			float accel_dt = sensor.accelerometer_integral_dt[0] / 1.e6f;
+			float accel_dt = sensor.accelerometer_integral_dt / 1.e6f;
 			msg.xacc = sensor.accelerometer_integral_m_s[0] / accel_dt;
 			msg.yacc = sensor.accelerometer_integral_m_s[1] / accel_dt;
 			msg.zacc = sensor.accelerometer_integral_m_s[2] / accel_dt;
-			float gyro_dt = sensor.gyro_integral_dt[0] / 1.e6f;
+			float gyro_dt = sensor.gyro_integral_dt / 1.e6f;
 			msg.xgyro = sensor.gyro_integral_rad[0] / gyro_dt;
 			msg.ygyro = sensor.gyro_integral_rad[1] / gyro_dt;
 			msg.zgyro = sensor.gyro_integral_rad[2] / gyro_dt;
@@ -765,8 +765,8 @@ protected:
 			msg.zmag = sensor.magnetometer_ga[2];
 			msg.abs_pressure = 0;
 			msg.diff_pressure = differential_pressure.differential_pressure_raw_pa;
-			msg.pressure_alt = sensor.baro_alt_meter[0];
-			msg.temperature = sensor.baro_temp_celcius[0];
+			msg.pressure_alt = sensor.baro_alt_meter;
+			msg.temperature = sensor.baro_temp_celcius;
 			msg.fields_updated = fields_updated;
 
 			mavlink_msg_highres_imu_send_struct(_mavlink->get_channel(), &msg);
@@ -1014,7 +1014,7 @@ protected:
 				/* fall back to baro altitude */
 				sensor_combined_s sensor;
 				(void)_sensor_sub->update(&_sensor_time, &sensor);
-				msg.alt = sensor.baro_alt_meter[0];
+				msg.alt = sensor.baro_alt_meter;
 			}
 			msg.climb = -pos.vel_d;
 
@@ -3121,7 +3121,7 @@ protected:
 
 			msg.time_usec = hrt_absolute_time();
 
-			msg.altitude_monotonic = (_sensor_time > 0) ? sensor.baro_alt_meter[0] : NAN;
+			msg.altitude_monotonic = (_sensor_time > 0) ? sensor.baro_alt_meter : NAN;
 			msg.altitude_amsl = (_global_pos_time > 0) ? global_pos.alt : NAN;
 			msg.altitude_local = (_local_pos_time > 0) ? -local_pos.z : NAN;
 			msg.altitude_relative = (_home_time > 0) ? (global_pos.alt - home.alt) : NAN;
diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp
index 0d62fb2307..62cf3b3f9c 100644
--- a/src/modules/mavlink/mavlink_receiver.cpp
+++ b/src/modules/mavlink/mavlink_receiver.cpp
@@ -1673,16 +1673,14 @@ MavlinkReceiver::handle_message_hil_sensor(mavlink_message_t *msg)
 	{
 		struct sensor_combined_s hil_sensors = {};
 
-		hil_sensors.timestamp = timestamp;
-
 		hil_sensors.gyro_integral_rad[0] = 0.5f * (imu.xgyro + _hil_prev_gyro[0]) * dt;
 		hil_sensors.gyro_integral_rad[1] = 0.5f * (imu.ygyro + _hil_prev_gyro[1]) * dt;
 		hil_sensors.gyro_integral_rad[2] = 0.5f * (imu.zgyro + _hil_prev_gyro[2]) * dt;
 		_hil_prev_gyro[0] = imu.xgyro;
 		_hil_prev_gyro[1] = imu.ygyro;
 		_hil_prev_gyro[2] = imu.zgyro;
-		hil_sensors.gyro_integral_dt[0] = dt * 1e6f;
-		hil_sensors.gyro_timestamp[0] = timestamp;
+		hil_sensors.gyro_integral_dt = dt * 1e6f;
+		hil_sensors.timestamp = timestamp;
 
 		hil_sensors.accelerometer_integral_m_s[0] = 0.5f * (imu.xacc + _hil_prev_accel[0]) * dt;
 		hil_sensors.accelerometer_integral_m_s[1] = 0.5f * (imu.yacc + _hil_prev_accel[1]) * dt;
@@ -1690,17 +1688,17 @@ MavlinkReceiver::handle_message_hil_sensor(mavlink_message_t *msg)
 		_hil_prev_accel[0] = imu.xacc;
 		_hil_prev_accel[1] = imu.yacc;
 		_hil_prev_accel[2] = imu.zacc;
-		hil_sensors.accelerometer_integral_dt[0] = dt * 1e6f;
-		hil_sensors.accelerometer_timestamp[0] = timestamp;
+		hil_sensors.accelerometer_integral_dt = dt * 1e6f;
+		hil_sensors.accelerometer_timestamp = timestamp;
 
 		hil_sensors.magnetometer_ga[0] = imu.xmag;
 		hil_sensors.magnetometer_ga[1] = imu.ymag;
 		hil_sensors.magnetometer_ga[2] = imu.zmag;
-		hil_sensors.magnetometer_timestamp[0] = timestamp;
+		hil_sensors.magnetometer_timestamp = timestamp;
 
-		hil_sensors.baro_alt_meter[0] = imu.pressure_alt;
-		hil_sensors.baro_temp_celcius[0] = imu.temperature;
-		hil_sensors.baro_timestamp[0] = timestamp;
+		hil_sensors.baro_alt_meter = imu.pressure_alt;
+		hil_sensors.baro_temp_celcius = imu.temperature;
+		hil_sensors.baro_timestamp = timestamp;
 
 		/* publish combined sensor topic */
 		if (_sensors_pub == nullptr) {
diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp
index 163238da75..c3e606da0d 100644
--- a/src/modules/navigator/navigator_main.cpp
+++ b/src/modules/navigator/navigator_main.cpp
@@ -500,7 +500,7 @@ Navigator::task_main()
 		if (have_geofence_position_data &&
 			(_geofence.getGeofenceAction() != geofence_result_s::GF_ACTION_NONE) &&
 			(hrt_elapsed_time(&last_geofence_check) > GEOFENCE_CHECK_INTERVAL)) {
-			bool inside = _geofence.inside(_global_pos, _gps_pos, _sensor_combined.baro_alt_meter[0], _home_pos, home_position_valid());
+			bool inside = _geofence.inside(_global_pos, _gps_pos, _sensor_combined.baro_alt_meter, _home_pos, home_position_valid());
 			last_geofence_check = hrt_absolute_time();
 			have_geofence_position_data = false;
 
diff --git a/src/modules/position_estimator_inav/position_estimator_inav_main.cpp b/src/modules/position_estimator_inav/position_estimator_inav_main.cpp
index 96f02298a1..7ef5cd7106 100644
--- a/src/modules/position_estimator_inav/position_estimator_inav_main.cpp
+++ b/src/modules/position_estimator_inav/position_estimator_inav_main.cpp
@@ -425,14 +425,14 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
 			if (fds_init[0].revents & POLLIN) {
 				orb_copy(ORB_ID(sensor_combined), sensor_combined_sub, &sensor);
 
-				if (wait_baro && sensor.baro_timestamp[0] != baro_timestamp) {
-					baro_timestamp = sensor.baro_timestamp[0];
+				if (wait_baro && sensor.baro_timestamp != baro_timestamp) {
+					baro_timestamp = sensor.baro_timestamp;
 					baro_wait_for_sample_time = hrt_absolute_time();
 
 					/* mean calculation over several measurements */
 					if (baro_init_cnt < baro_init_num) {
-						if (PX4_ISFINITE(sensor.baro_alt_meter[0])) {
-							baro_offset += sensor.baro_alt_meter[0];
+						if (PX4_ISFINITE(sensor.baro_alt_meter)) {
+							baro_offset += sensor.baro_alt_meter;
 							baro_init_cnt++;
 						}
 
@@ -502,10 +502,10 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
 			if (updated) {
 				orb_copy(ORB_ID(sensor_combined), sensor_combined_sub, &sensor);
 
-				if (sensor.accelerometer_timestamp[0] != accel_timestamp) {
+				if (sensor.accelerometer_timestamp != accel_timestamp) {
 					if (att.R_valid) {
 						float sensor_accel[3];
-						float accel_dt = sensor.accelerometer_integral_dt[0] / 1.e6f;
+						float accel_dt = sensor.accelerometer_integral_dt / 1.e6f;
 						sensor_accel[0] = sensor.accelerometer_integral_m_s[0] / accel_dt - acc_bias[0];
 						sensor_accel[1] = sensor.accelerometer_integral_m_s[1] / accel_dt - acc_bias[1];
 						sensor_accel[2] = sensor.accelerometer_integral_m_s[2] / accel_dt - acc_bias[2];
@@ -525,13 +525,13 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
 						memset(acc, 0, sizeof(acc));
 					}
 
-					accel_timestamp = sensor.accelerometer_timestamp[0];
+					accel_timestamp = sensor.accelerometer_timestamp;
 					accel_updates++;
 				}
 
-				if (sensor.baro_timestamp[0] != baro_timestamp) {
-					corr_baro = baro_offset - sensor.baro_alt_meter[0] - z_est[0];
-					baro_timestamp = sensor.baro_timestamp[0];
+				if (sensor.baro_timestamp != baro_timestamp) {
+					corr_baro = baro_offset - sensor.baro_alt_meter - z_est[0];
+					baro_timestamp = sensor.baro_timestamp;
 					baro_updates++;
 				}
 			}
@@ -1363,7 +1363,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
 					global_pos.terrain_alt_valid = false;
 				}
 
-				global_pos.pressure_alt = sensor.baro_alt_meter[0];
+				global_pos.pressure_alt = sensor.baro_alt_meter;
 
 				if (vehicle_global_position_pub == NULL) {
 					vehicle_global_position_pub = orb_advertise(ORB_ID(vehicle_global_position), &global_pos);
diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c
index 9d0245dcaf..3ad1925c6f 100644
--- a/src/modules/sdlog2/sdlog2.c
+++ b/src/modules/sdlog2/sdlog2.c
@@ -1370,10 +1370,10 @@ int sdlog2_thread_main(int argc, char *argv[])
 	pthread_cond_init(&logbuffer_cond, NULL);
 
 	/* track changes in sensor_combined topic */
-	hrt_abstime gyro_timestamp[3] = {0, 0, 0};
-	hrt_abstime accelerometer_timestamp[3] = {0, 0, 0};
-	hrt_abstime magnetometer_timestamp[3] = {0, 0, 0};
-	hrt_abstime barometer_timestamp[3] = {0, 0, 0};
+	hrt_abstime gyro_timestamp = 0;
+	hrt_abstime accelerometer_timestamp = 0;
+	hrt_abstime magnetometer_timestamp = 0;
+	hrt_abstime barometer_timestamp = 0;
 
 	/* initialize calculated mean SNR */
 	float snr_mean = 0.0f;
@@ -1634,80 +1634,58 @@ int sdlog2_thread_main(int argc, char *argv[])
 			// but we need to copy it again because we are re-using the buffer.
 			orb_copy(ORB_ID(sensor_combined), subs.sensor_sub, &buf.sensor);
 
-			for (unsigned i = 0; i < 3; i++) {
-				bool write_IMU = false;
-				bool write_SENS = false;
+			bool write_IMU = false;
+			bool write_SENS = false;
 
-				if (buf.sensor.gyro_timestamp[i] != gyro_timestamp[i]) {
-					gyro_timestamp[i] = buf.sensor.gyro_timestamp[i];
-					write_IMU = true;
-				}
+			if (buf.sensor.timestamp != gyro_timestamp) {
+				gyro_timestamp = buf.sensor.timestamp;
+				write_IMU = true;
+			}
 
-				if (buf.sensor.accelerometer_timestamp[i] != accelerometer_timestamp[i]) {
-					accelerometer_timestamp[i] = buf.sensor.accelerometer_timestamp[i];
-					write_IMU = true;
-				}
+			if (buf.sensor.accelerometer_timestamp != accelerometer_timestamp) {
+				accelerometer_timestamp = buf.sensor.accelerometer_timestamp;
+				write_IMU = true;
+			}
 
-				if (buf.sensor.magnetometer_timestamp[i] != magnetometer_timestamp[i]) {
-					magnetometer_timestamp[i] = buf.sensor.magnetometer_timestamp[i];
-					write_IMU = true;
-				}
+			if (buf.sensor.magnetometer_timestamp != magnetometer_timestamp) {
+				magnetometer_timestamp = buf.sensor.magnetometer_timestamp;
+				write_IMU = true;
+			}
 
-				if (buf.sensor.baro_timestamp[i] != barometer_timestamp[i]) {
-					barometer_timestamp[i] = buf.sensor.baro_timestamp[i];
-					write_SENS = true;
-				}
+			if (buf.sensor.baro_timestamp != barometer_timestamp) {
+				barometer_timestamp = buf.sensor.baro_timestamp;
+				write_SENS = true;
+			}
 
-				if (write_IMU) {
-					switch (i) {
-						case 0:
-							log_msg.msg_type = LOG_IMU_MSG;
-							break;
-						case 1:
-							log_msg.msg_type = LOG_IMU1_MSG;
-							break;
-						case 2:
-							log_msg.msg_type = LOG_IMU2_MSG;
-							break;
-					}
+			if (write_IMU) {
+				log_msg.msg_type = LOG_IMU_MSG;
 
-					float gyro_dt = buf.sensor.gyro_integral_dt[i] / 1.e6f;
-					log_msg.body.log_IMU.gyro_x = buf.sensor.gyro_integral_rad[i * 3 + 0] / gyro_dt;
-					log_msg.body.log_IMU.gyro_y = buf.sensor.gyro_integral_rad[i * 3 + 1] / gyro_dt;
-					log_msg.body.log_IMU.gyro_z = buf.sensor.gyro_integral_rad[i * 3 + 2] / gyro_dt;
-					float accel_dt = buf.sensor.accelerometer_integral_dt[i] / 1.e6f;
-					log_msg.body.log_IMU.acc_x = buf.sensor.accelerometer_integral_m_s[i * 3 + 0] / accel_dt;
-					log_msg.body.log_IMU.acc_y = buf.sensor.accelerometer_integral_m_s[i * 3 + 1] / accel_dt;
-					log_msg.body.log_IMU.acc_z = buf.sensor.accelerometer_integral_m_s[i * 3 + 2] / accel_dt;
-					log_msg.body.log_IMU.mag_x = buf.sensor.magnetometer_ga[i * 3 + 0];
-					log_msg.body.log_IMU.mag_y = buf.sensor.magnetometer_ga[i * 3 + 1];
-					log_msg.body.log_IMU.mag_z = buf.sensor.magnetometer_ga[i * 3 + 2];
-					log_msg.body.log_IMU.temp_gyro = 0;
-					log_msg.body.log_IMU.temp_acc = 0;
-					log_msg.body.log_IMU.temp_mag = 0;
-					LOGBUFFER_WRITE_AND_COUNT(IMU);
-				}
+				float gyro_dt = buf.sensor.gyro_integral_dt / 1.e6f;
+				log_msg.body.log_IMU.gyro_x = buf.sensor.gyro_integral_rad[0] / gyro_dt;
+				log_msg.body.log_IMU.gyro_y = buf.sensor.gyro_integral_rad[1] / gyro_dt;
+				log_msg.body.log_IMU.gyro_z = buf.sensor.gyro_integral_rad[2] / gyro_dt;
+				float accel_dt = buf.sensor.accelerometer_integral_dt / 1.e6f;
+				log_msg.body.log_IMU.acc_x = buf.sensor.accelerometer_integral_m_s[0] / accel_dt;
+				log_msg.body.log_IMU.acc_y = buf.sensor.accelerometer_integral_m_s[1] / accel_dt;
+				log_msg.body.log_IMU.acc_z = buf.sensor.accelerometer_integral_m_s[2] / accel_dt;
+				log_msg.body.log_IMU.mag_x = buf.sensor.magnetometer_ga[0];
+				log_msg.body.log_IMU.mag_y = buf.sensor.magnetometer_ga[1];
+				log_msg.body.log_IMU.mag_z = buf.sensor.magnetometer_ga[2];
+				log_msg.body.log_IMU.temp_gyro = 0;
+				log_msg.body.log_IMU.temp_acc = 0;
+				log_msg.body.log_IMU.temp_mag = 0;
+				LOGBUFFER_WRITE_AND_COUNT(IMU);
+			}
 
-				if (write_SENS) {
-					switch (i) {
-						case 0:
-							log_msg.msg_type = LOG_SENS_MSG;
-							break;
-						case 1:
-							log_msg.msg_type = LOG_AIR1_MSG;
-							break;
-						case 2:
-							continue;
-							break;
-					}
+			if (write_SENS) {
+				log_msg.msg_type = LOG_SENS_MSG;
 
-					log_msg.body.log_SENS.baro_pres = 0;
-					log_msg.body.log_SENS.baro_alt = buf.sensor.baro_alt_meter[i];
-					log_msg.body.log_SENS.baro_temp = buf.sensor.baro_temp_celcius[i];
-					log_msg.body.log_SENS.diff_pres = 0;
-					log_msg.body.log_SENS.diff_pres_filtered = 0;
-					LOGBUFFER_WRITE_AND_COUNT(SENS);
-				}
+				log_msg.body.log_SENS.baro_pres = 0;
+				log_msg.body.log_SENS.baro_alt = buf.sensor.baro_alt_meter;
+				log_msg.body.log_SENS.baro_temp = buf.sensor.baro_temp_celcius;
+				log_msg.body.log_SENS.diff_pres = 0;
+				log_msg.body.log_SENS.diff_pres_filtered = 0;
+				LOGBUFFER_WRITE_AND_COUNT(SENS);
 			}
 
 			/* --- VTOL VEHICLE STATUS --- */
diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp
index e03ee4f3a1..658998d879 100644
--- a/src/modules/sensors/sensors.cpp
+++ b/src/modules/sensors/sensors.cpp
@@ -76,6 +76,7 @@
 #include <drivers/drv_px4flow.h>
 
 #include <systemlib/airspeed.h>
+#include <systemlib/mavlink_log.h>
 #include <systemlib/systemlib.h>
 #include <systemlib/param/param.h>
 #include <systemlib/err.h>
@@ -85,6 +86,7 @@
 #include <conversion/rotation.h>
 
 #include <lib/ecl/validation/data_validator.h>
+#include <lib/ecl/validation/data_validator_group.h>
 
 #include <uORB/uORB.h>
 #include <uORB/topics/sensor_combined.h>
@@ -164,6 +166,9 @@ public:
 	 */
 	int		start();
 
+
+	void	print_status();
+
 private:
 	static const unsigned _rc_max_chan_count =
 		input_rc_s::RC_INPUT_MAX_CHANNELS;	/**< maximum number of r/c channels we handle */
@@ -200,19 +205,33 @@ private:
 	int 		_sensors_task;			/**< task handle for sensor task */
 
 	bool		_hil_enabled;			/**< if true, HIL is active */
-	bool		_publishing;			/**< if true, we are publishing sensor data */
+	bool		_publishing;			/**< if true, we are publishing sensor data (in HIL mode, we don't) */
 	bool		_armed;				/**< arming status of the vehicle */
 
-	int		_gyro_sub[SENSOR_COUNT_MAX];	/**< raw gyro data subscription */
-	int		_accel_sub[SENSOR_COUNT_MAX];	/**< raw accel data subscription */
-	int		_mag_sub[SENSOR_COUNT_MAX];	/**< raw mag data subscription */
-	int		_baro_sub[SENSOR_COUNT_MAX];	/**< raw baro data subscription */
-	int		_actuator_ctrl_0_sub;		/**< attitude controls sub */
-	unsigned	_gyro_count;			/**< raw gyro data count */
-	unsigned	_accel_count;			/**< raw accel data count */
-	unsigned	_mag_count;			/**< raw mag data count */
-	unsigned	_baro_count;			/**< raw baro data count */
+	struct SensorData {
+		SensorData()
+			: subscription_count(0),
+			  voter(SENSOR_COUNT_MAX),
+			  last_failover_count(0)
+		{
+			for (unsigned i = 0; i < SENSOR_COUNT_MAX; i++) {
+				subscription[i] = -1;
+			}
+		}
 
+		int subscription[SENSOR_COUNT_MAX]; /**< raw sensor data subscription */
+		uint8_t priority[SENSOR_COUNT_MAX]; /**< sensor priority */
+		int subscription_count;
+		DataValidatorGroup voter;
+		unsigned int last_failover_count;
+	};
+
+	SensorData _gyro;
+	SensorData _accel;
+	SensorData _mag;
+	SensorData _baro;
+
+	int		_actuator_ctrl_0_sub;		/**< attitude controls sub */
 	int 		_rc_sub;			/**< raw rc channels data subscription */
 	int		_diff_pres_sub;			/**< raw differential pressure subscription */
 	int		_vcontrol_mode_sub;		/**< vehicle control mode subscription */
@@ -227,6 +246,7 @@ private:
 	orb_advert_t	_battery_pub;			/**< battery status */
 	orb_advert_t	_airspeed_pub;			/**< airspeed */
 	orb_advert_t	_diff_pres_pub;			/**< differential_pressure */
+	orb_advert_t	_mavlink_log_pub;
 
 	perf_counter_t	_loop_perf;			/**< loop performance counter */
 
@@ -234,7 +254,6 @@ private:
 
 	struct rc_channels_s _rc;			/**< r/c channel data */
 	struct battery_status_s _battery_status;	/**< battery status */
-	struct baro_report _barometer;			/**< barometer data */
 	struct differential_pressure_s _diff_pres;
 	struct airspeed_s _airspeed;
 	struct rc_parameter_map_s _rc_parameter_map;
@@ -245,9 +264,12 @@ private:
 
 	Battery		_battery;			/**< Helper lib to publish battery_status topic. */
 
-	float		_latest_baro_pressure[SENSOR_COUNT_MAX];
-	hrt_abstime	_last_accel_timestamp[SENSOR_COUNT_MAX];
-	hrt_abstime	_last_gyro_timestamp[SENSOR_COUNT_MAX];
+	float _last_baro_pressure[SENSOR_COUNT_MAX]; /**< pressure from last baro sensors */
+	float _last_best_baro_pressure; /**< pressure from last best baro */
+	sensor_combined_s _last_sensor_data[SENSOR_COUNT_MAX]; /**< latest sensor data from all sensors instances */
+
+	hrt_abstime _vibration_warning_timestamp;
+	bool _vibration_warning;
 
 	struct {
 		float min[_rc_max_chan_count];
@@ -320,6 +342,8 @@ private:
 
 		float baro_qnh;
 
+		float vibration_warning_threshold;
+
 	}		_parameters;			/**< local copies of interesting parameters */
 
 	struct {
@@ -387,10 +411,12 @@ private:
 
 		param_t baro_qnh;
 
+		param_t vibe_thresh; /**< vibration threshold */
+
 	}		_parameter_handles;		/**< handles for interesting parameters */
 
 
-	int		init_sensor_class(const struct orb_metadata *meta, int *subs);
+	void	init_sensor_class(const struct orb_metadata *meta, SensorData &sensor_data);
 
 	/**
 	 * Update our local parameter cache.
@@ -495,6 +521,18 @@ private:
 	 */
 	void		adc_poll(struct sensor_combined_s &raw);
 
+	/**
+	 * Check & handle failover of a sensor
+	 * @return true if a switch occured (could be for a non-critical reason)
+	 */
+	bool check_failover(SensorData &sensor, const char *sensor_name);
+
+	/**
+	 * check vibration levels and output a warning if they're high
+	 * @return true on high vibration
+	 */
+	bool check_vibration();
+
 	/**
 	 * Shim for calling task_main from task_create.
 	 */
@@ -521,10 +559,6 @@ Sensors::Sensors() :
 	_hil_enabled(false),
 	_publishing(true),
 	_armed(false),
-	_gyro_count(0),
-	_accel_count(0),
-	_mag_count(0),
-	_baro_count(0),
 	_rc_sub(-1),
 	_vcontrol_mode_sub(-1),
 	_params_sub(-1),
@@ -539,6 +573,7 @@ Sensors::Sensors() :
 	_battery_pub(nullptr),
 	_airspeed_pub(nullptr),
 	_diff_pres_pub(nullptr),
+	_mavlink_log_pub(nullptr),
 
 	/* performance counters */
 	_loop_perf(perf_alloc(PC_ELAPSED, "sensors")),
@@ -546,23 +581,20 @@ Sensors::Sensors() :
 
 	_param_rc_values{},
 	_board_rotation{},
-	_mag_rotation{}
+	_mag_rotation{},
+
+	_last_best_baro_pressure(0.f),
+
+	_vibration_warning_timestamp(0),
+	_vibration_warning(false)
 {
-	/* initialize subscriptions */
-	for (unsigned i = 0; i < SENSOR_COUNT_MAX; i++) {
-		_gyro_sub[i] = -1;
-		_accel_sub[i] = -1;
-		_mag_sub[i] = -1;
-		_baro_sub[i] = -1;
-	}
+	_mag.voter.set_timeout(200000);
 
 	memset(&_rc, 0, sizeof(_rc));
 	memset(&_diff_pres, 0, sizeof(_diff_pres));
 	memset(&_parameters, 0, sizeof(_parameters));
 	memset(&_rc_parameter_map, 0, sizeof(_rc_parameter_map));
-	memset(&_latest_baro_pressure, 0, sizeof(_latest_baro_pressure));
-	memset(&_last_accel_timestamp, 0, sizeof(_last_accel_timestamp));
-	memset(&_last_gyro_timestamp, 0, sizeof(_last_gyro_timestamp));
+	memset(&_last_sensor_data, 0, sizeof(_last_sensor_data));
 
 	/* basic r/c parameters */
 	for (unsigned i = 0; i < _rc_max_chan_count; i++) {
@@ -661,6 +693,8 @@ Sensors::Sensors() :
 	/* Barometer QNH */
 	_parameter_handles.baro_qnh = param_find("SENS_BARO_QNH");
 
+	_parameter_handles.vibe_thresh = param_find("ATT_VIBE_THRESH");
+
 	// These are parameters for which QGroundControl always expects to be returned in a list request.
 	// We do a param_find here to force them into the list.
 	(void)param_find("RC_CHAN_CNT");
@@ -993,6 +1027,8 @@ Sensors::parameters_update()
 
 #endif
 
+	param_get(_parameter_handles.vibe_thresh, &_parameters.vibration_warning_threshold);
+
 	return ret;
 }
 
@@ -1014,43 +1050,72 @@ Sensors::adc_init()
 void
 Sensors::accel_poll(struct sensor_combined_s &raw)
 {
-	for (unsigned i = 0; i < _accel_count; i++) {
+	bool got_update = false;
+
+	for (unsigned i = 0; i < _accel.subscription_count; i++) {
 		bool accel_updated;
-		orb_check(_accel_sub[i], &accel_updated);
+		orb_check(_accel.subscription[i], &accel_updated);
 
 		if (accel_updated) {
-			struct accel_report	accel_report;
+			struct accel_report accel_report;
 
-			orb_copy(ORB_ID(sensor_accel), _accel_sub[i], &accel_report);
+			orb_copy(ORB_ID(sensor_accel), _accel.subscription[i], &accel_report);
+
+			if (accel_report.timestamp == 0) {
+				continue; //ignore invalid data
+			}
+
+			got_update = true;
+			math::Vector<3> sensor_value;
 
 			if (accel_report.integral_dt != 0) {
 				math::Vector<3> vect_int(accel_report.x_integral, accel_report.y_integral, accel_report.z_integral);
 				vect_int = _board_rotation * vect_int;
 
-				raw.accelerometer_integral_m_s[i * 3 + 0] = vect_int(0);
-				raw.accelerometer_integral_m_s[i * 3 + 1] = vect_int(1);
-				raw.accelerometer_integral_m_s[i * 3 + 2] = vect_int(2);
+				_last_sensor_data[i].accelerometer_integral_m_s[0] = vect_int(0);
+				_last_sensor_data[i].accelerometer_integral_m_s[1] = vect_int(1);
+				_last_sensor_data[i].accelerometer_integral_m_s[2] = vect_int(2);
 
-				raw.accelerometer_integral_dt[i] = accel_report.integral_dt;
+				_last_sensor_data[i].accelerometer_integral_dt = accel_report.integral_dt;
+
+				float dt = accel_report.integral_dt / 1.e6f;
+				sensor_value = vect_int / dt;
 
 			} else {
 				//this is undesirable: a driver did not set the integral, so we have to construct it ourselves
 				math::Vector<3> vect_val(accel_report.x, accel_report.y, accel_report.z);
 				vect_val = _board_rotation * vect_val;
 
-				if (_last_accel_timestamp[i] == 0) {
-					_last_accel_timestamp[i] = accel_report.timestamp - 1000;
+				sensor_value = vect_val;
+
+				if (_last_sensor_data[i].accelerometer_timestamp == 0) {
+					_last_sensor_data[i].accelerometer_timestamp = accel_report.timestamp - 1000;
 				}
 
-				raw.accelerometer_integral_dt[i] = accel_report.timestamp - _last_accel_timestamp[i];
-				_last_accel_timestamp[i] = accel_report.timestamp;
-				float dt = raw.accelerometer_integral_dt[i] / 1.e6f;
-				raw.accelerometer_integral_m_s[i * 3 + 0] = vect_val(0) * dt;
-				raw.accelerometer_integral_m_s[i * 3 + 1] = vect_val(1) * dt;
-				raw.accelerometer_integral_m_s[i * 3 + 2] = vect_val(2) * dt;
+				_last_sensor_data[i].accelerometer_integral_dt =
+					accel_report.timestamp - _last_sensor_data[i].accelerometer_timestamp;
+				float dt = _last_sensor_data[i].accelerometer_integral_dt / 1.e6f;
+				_last_sensor_data[i].accelerometer_integral_m_s[0] = vect_val(0) * dt;
+				_last_sensor_data[i].accelerometer_integral_m_s[1] = vect_val(1) * dt;
+				_last_sensor_data[i].accelerometer_integral_m_s[2] = vect_val(2) * dt;
 			}
 
-			raw.accelerometer_timestamp[i] = accel_report.timestamp;
+			_last_sensor_data[i].accelerometer_timestamp = accel_report.timestamp;
+			_accel.voter.put(i, accel_report.timestamp, sensor_value.data,
+					 accel_report.error_count, _accel.priority[i]);
+		}
+	}
+
+	if (got_update) {
+		int best_index;
+		_accel.voter.get_best(hrt_absolute_time(), &best_index);
+
+		if (best_index >= 0) {
+			raw.accelerometer_integral_m_s[0] = _last_sensor_data[best_index].accelerometer_integral_m_s[0];
+			raw.accelerometer_integral_m_s[1] = _last_sensor_data[best_index].accelerometer_integral_m_s[1];
+			raw.accelerometer_integral_m_s[2] = _last_sensor_data[best_index].accelerometer_integral_m_s[2];
+			raw.accelerometer_integral_dt = _last_sensor_data[best_index].accelerometer_integral_dt;
+			raw.accelerometer_timestamp = _last_sensor_data[best_index].accelerometer_timestamp;
 		}
 	}
 }
@@ -1058,47 +1123,72 @@ Sensors::accel_poll(struct sensor_combined_s &raw)
 void
 Sensors::gyro_poll(struct sensor_combined_s &raw)
 {
-	for (unsigned i = 0; i < _gyro_count; i++) {
+	bool got_update = false;
+
+	for (unsigned i = 0; i < _gyro.subscription_count; i++) {
 		bool gyro_updated;
-		orb_check(_gyro_sub[i], &gyro_updated);
+		orb_check(_gyro.subscription[i], &gyro_updated);
 
 		if (gyro_updated) {
-			struct gyro_report	gyro_report;
+			struct gyro_report gyro_report;
 
-			orb_copy(ORB_ID(sensor_gyro), _gyro_sub[i], &gyro_report);
+			orb_copy(ORB_ID(sensor_gyro), _gyro.subscription[i], &gyro_report);
+
+			if (gyro_report.timestamp == 0) {
+				continue; //ignore invalid data
+			}
+
+			got_update = true;
+			math::Vector<3> sensor_value;
 
 			if (gyro_report.integral_dt != 0) {
 				math::Vector<3> vect_int(gyro_report.x_integral, gyro_report.y_integral, gyro_report.z_integral);
 				vect_int = _board_rotation * vect_int;
 
-				raw.gyro_integral_rad[i * 3 + 0] = vect_int(0);
-				raw.gyro_integral_rad[i * 3 + 1] = vect_int(1);
-				raw.gyro_integral_rad[i * 3 + 2] = vect_int(2);
+				_last_sensor_data[i].gyro_integral_rad[0] = vect_int(0);
+				_last_sensor_data[i].gyro_integral_rad[1] = vect_int(1);
+				_last_sensor_data[i].gyro_integral_rad[2] = vect_int(2);
 
-				raw.gyro_integral_dt[i] = gyro_report.integral_dt;
-				raw.gyro_timestamp[i] = gyro_report.timestamp;
+				_last_sensor_data[i].gyro_integral_dt = gyro_report.integral_dt;
+
+				float dt = gyro_report.integral_dt / 1.e6f;
+				sensor_value = vect_int / dt;
 
 			} else {
 				//this is undesirable: a driver did not set the integral, so we have to construct it ourselves
 				math::Vector<3> vect_val(gyro_report.x, gyro_report.y, gyro_report.z);
 				vect_val = _board_rotation * vect_val;
 
-				if (_last_gyro_timestamp[i] == 0) {
-					_last_gyro_timestamp[i] = gyro_report.timestamp - 1000;
+				sensor_value = vect_val;
+
+				if (_last_sensor_data[i].timestamp == 0) {
+					_last_sensor_data[i].timestamp = gyro_report.timestamp - 1000;
 				}
 
-				raw.gyro_integral_dt[i] = gyro_report.timestamp - _last_gyro_timestamp[i];
-				_last_gyro_timestamp[i] = gyro_report.timestamp;
-				float dt = raw.gyro_integral_dt[i] / 1.e6f;
-				raw.gyro_integral_rad[i * 3 + 0] = vect_val(0) * dt;
-				raw.gyro_integral_rad[i * 3 + 1] = vect_val(1) * dt;
-				raw.gyro_integral_rad[i * 3 + 2] = vect_val(2) * dt;
-
+				_last_sensor_data[i].gyro_integral_dt =
+					gyro_report.timestamp - _last_sensor_data[i].timestamp;
+				float dt = _last_sensor_data[i].gyro_integral_dt / 1.e6f;
+				_last_sensor_data[i].gyro_integral_rad[0] = vect_val(0) * dt;
+				_last_sensor_data[i].gyro_integral_rad[1] = vect_val(1) * dt;
+				_last_sensor_data[i].gyro_integral_rad[2] = vect_val(2) * dt;
 			}
 
-			if (i == 0) {
-				raw.timestamp = gyro_report.timestamp;
-			}
+			_last_sensor_data[i].timestamp = gyro_report.timestamp;
+			_gyro.voter.put(i, gyro_report.timestamp, sensor_value.data,
+					gyro_report.error_count, _gyro.priority[i]);
+		}
+	}
+
+	if (got_update) {
+		int best_index;
+		_gyro.voter.get_best(hrt_absolute_time(), &best_index);
+
+		if (best_index >= 0) {
+			raw.gyro_integral_rad[0] = _last_sensor_data[best_index].gyro_integral_rad[0];
+			raw.gyro_integral_rad[1] = _last_sensor_data[best_index].gyro_integral_rad[1];
+			raw.gyro_integral_rad[2] = _last_sensor_data[best_index].gyro_integral_rad[2];
+			raw.gyro_integral_dt = _last_sensor_data[best_index].gyro_integral_dt;
+			raw.timestamp = _last_sensor_data[best_index].timestamp;
 		}
 	}
 }
@@ -1106,24 +1196,44 @@ Sensors::gyro_poll(struct sensor_combined_s &raw)
 void
 Sensors::mag_poll(struct sensor_combined_s &raw)
 {
-	for (unsigned i = 0; i < _mag_count; i++) {
+	bool got_update = false;
+
+	for (unsigned i = 0; i < _mag.subscription_count; i++) {
 		bool mag_updated;
-		orb_check(_mag_sub[i], &mag_updated);
+		orb_check(_mag.subscription[i], &mag_updated);
 
 		if (mag_updated) {
-			struct mag_report	mag_report;
+			struct mag_report mag_report;
 
-			orb_copy(ORB_ID(sensor_mag), _mag_sub[i], &mag_report);
+			orb_copy(ORB_ID(sensor_mag), _mag.subscription[i], &mag_report);
 
+			if (mag_report.timestamp == 0) {
+				continue; //ignore invalid data
+			}
+
+			got_update = true;
 			math::Vector<3> vect(mag_report.x, mag_report.y, mag_report.z);
-
 			vect = _mag_rotation[i] * vect;
 
-			raw.magnetometer_ga[i * 3 + 0] = vect(0);
-			raw.magnetometer_ga[i * 3 + 1] = vect(1);
-			raw.magnetometer_ga[i * 3 + 2] = vect(2);
+			_last_sensor_data[i].magnetometer_ga[0] = vect(0);
+			_last_sensor_data[i].magnetometer_ga[1] = vect(1);
+			_last_sensor_data[i].magnetometer_ga[2] = vect(2);
 
-			raw.magnetometer_timestamp[i] = mag_report.timestamp;
+			_last_sensor_data[i].magnetometer_timestamp = mag_report.timestamp;
+			_mag.voter.put(i, mag_report.timestamp, vect.data,
+				       mag_report.error_count, _mag.priority[i]);
+		}
+	}
+
+	if (got_update) {
+		int best_index;
+		_mag.voter.get_best(hrt_absolute_time(), &best_index);
+
+		if (best_index >= 0) {
+			raw.magnetometer_ga[0] = _last_sensor_data[best_index].magnetometer_ga[0];
+			raw.magnetometer_ga[1] = _last_sensor_data[best_index].magnetometer_ga[1];
+			raw.magnetometer_ga[2] = _last_sensor_data[best_index].magnetometer_ga[2];
+			raw.magnetometer_timestamp = _last_sensor_data[best_index].magnetometer_timestamp;
 		}
 	}
 }
@@ -1131,19 +1241,43 @@ Sensors::mag_poll(struct sensor_combined_s &raw)
 void
 Sensors::baro_poll(struct sensor_combined_s &raw)
 {
-	for (unsigned i = 0; i < _baro_count; i++) {
+	bool got_update = false;
+
+	for (unsigned i = 0; i < _baro.subscription_count; i++) {
 		bool baro_updated;
-		orb_check(_baro_sub[i], &baro_updated);
+		orb_check(_baro.subscription[i], &baro_updated);
 
 		if (baro_updated) {
+			struct baro_report baro_report;
 
-			orb_copy(ORB_ID(sensor_baro), _baro_sub[i], &_barometer);
+			orb_copy(ORB_ID(sensor_baro), _baro.subscription[i], &baro_report);
 
-			_latest_baro_pressure[i] = _barometer.pressure;
-			raw.baro_alt_meter[i] = _barometer.altitude; // Altitude in meters
-			raw.baro_temp_celcius[i] = _barometer.temperature; // Temperature in degrees celcius
+			if (baro_report.timestamp == 0) {
+				continue; //ignore invalid data
+			}
 
-			raw.baro_timestamp[i] = _barometer.timestamp;
+			got_update = true;
+			math::Vector<3> vect(baro_report.altitude, 0.f, 0.f);
+
+			_last_sensor_data[i].baro_alt_meter = baro_report.altitude;
+			_last_sensor_data[i].baro_temp_celcius = baro_report.temperature;
+			_last_baro_pressure[i] = baro_report.pressure;
+
+			_last_sensor_data[i].baro_timestamp = baro_report.timestamp;
+			_baro.voter.put(i, baro_report.timestamp, vect.data,
+					baro_report.error_count, _baro.priority[i]);
+		}
+	}
+
+	if (got_update) {
+		int best_index;
+		_baro.voter.get_best(hrt_absolute_time(), &best_index);
+
+		if (best_index >= 0) {
+			raw.baro_alt_meter = _last_sensor_data[best_index].baro_alt_meter;
+			raw.baro_temp_celcius = _last_sensor_data[best_index].baro_temp_celcius;
+			raw.baro_timestamp = _last_sensor_data[best_index].baro_timestamp;
+			_last_best_baro_pressure = _last_baro_pressure[best_index];
 		}
 	}
 }
@@ -1158,7 +1292,7 @@ Sensors::diff_pres_poll(struct sensor_combined_s &raw)
 		orb_copy(ORB_ID(differential_pressure), _diff_pres_sub, &_diff_pres);
 
 		float air_temperature_celsius = (_diff_pres.temperature > -300.0f) ? _diff_pres.temperature :
-						(raw.baro_temp_celcius[0] - PCB_TEMP_ESTIMATE_DEG);
+						(raw.baro_temp_celcius - PCB_TEMP_ESTIMATE_DEG);
 
 		_airspeed.timestamp = _diff_pres.timestamp;
 
@@ -1175,13 +1309,12 @@ Sensors::diff_pres_poll(struct sensor_combined_s &raw)
 		_airspeed.indicated_airspeed_m_s = math::max(0.0f,
 						   calc_indicated_airspeed(_diff_pres.differential_pressure_filtered_pa));
 
-		//FIXME: we just use the baro pressure from the first baro. we should do voting instead.
 		_airspeed.true_airspeed_m_s = math::max(0.0f,
-							calc_true_airspeed(_diff_pres.differential_pressure_filtered_pa + _latest_baro_pressure[0] * 1e2f,
-									_latest_baro_pressure[0] * 1e2f, air_temperature_celsius));
+							calc_true_airspeed(_diff_pres.differential_pressure_filtered_pa + _last_best_baro_pressure * 1e2f,
+									_last_best_baro_pressure * 1e2f, air_temperature_celsius));
 		_airspeed.true_airspeed_unfiltered_m_s = math::max(0.0f,
-				calc_true_airspeed(_diff_pres.differential_pressure_raw_pa + _latest_baro_pressure[0] * 1e2f,
-						   _latest_baro_pressure[0] * 1e2f, air_temperature_celsius));
+				calc_true_airspeed(_diff_pres.differential_pressure_raw_pa + _last_best_baro_pressure * 1e2f,
+						   _last_best_baro_pressure * 1e2f, air_temperature_celsius));
 
 		_airspeed.air_temperature_celsius = air_temperature_celsius;
 
@@ -1544,14 +1677,7 @@ Sensors::apply_gyro_calibration(DevHandle &h, const struct gyro_calibration_s *g
 #if !defined(__PX4_QURT) && !defined(__PX4_POSIX_RPI2)
 
 	/* On most systems, we can just use the IOCTL call to set the calibration params. */
-	const int res = h.ioctl(GYROIOCSSCALE, (long unsigned int)gcal);
-
-	if (res) {
-		return false;
-
-	} else {
-		return true;
-	}
+	return !h.ioctl(GYROIOCSSCALE, (long unsigned int)gcal);
 
 #else
 	/* On QURT, the params are read directly in the respective wrappers. */
@@ -1565,14 +1691,7 @@ Sensors::apply_accel_calibration(DevHandle &h, const struct accel_calibration_s
 #if !defined(__PX4_QURT) && !defined(__PX4_POSIX_RPI2)
 
 	/* On most systems, we can just use the IOCTL call to set the calibration params. */
-	const int res = h.ioctl(ACCELIOCSSCALE, (long unsigned int)acal);
-
-	if (res) {
-		return false;
-
-	} else {
-		return true;
-	}
+	return !h.ioctl(ACCELIOCSSCALE, (long unsigned int)acal);
 
 #else
 	/* On QURT, the params are read directly in the respective wrappers. */
@@ -1586,14 +1705,7 @@ Sensors::apply_mag_calibration(DevHandle &h, const struct mag_calibration_s *mca
 #if !defined(__PX4_QURT) && !defined(__PX4_POSIX_RPI2)
 
 	/* On most systems, we can just use the IOCTL call to set the calibration params. */
-	const int res = h.ioctl(MAGIOCSSCALE, (long unsigned int)mcal);
-
-	if (res) {
-		return false;
-
-	} else {
-		return true;
-	}
+	return !h.ioctl(MAGIOCSSCALE, (long unsigned int)mcal);
 
 #else
 	/* On QURT, the params are read directly in the respective wrappers. */
@@ -2045,14 +2157,72 @@ Sensors::rc_poll()
 	}
 }
 
+bool
+Sensors::check_failover(SensorData &sensor, const char *sensor_name)
+{
+	if (sensor.last_failover_count != sensor.voter.failover_count()) {
+
+		uint32_t flags = sensor.voter.failover_state();
+
+		if (flags == DataValidator::ERROR_FLAG_NO_ERROR) {
+			//we switched due to a non-critical reason. No need to panic.
+			PX4_INFO("%s sensor switch from #%i", sensor_name, sensor.voter.failover_index());
+
+		} else {
+			mavlink_and_console_log_emergency(&_mavlink_log_pub, "%s #%i failure :%s%s%s%s%s!",
+							  sensor_name,
+							  sensor.voter.failover_index(),
+							  ((flags & DataValidator::ERROR_FLAG_NO_DATA) ? " No data" : ""),
+							  ((flags & DataValidator::ERROR_FLAG_STALE_DATA) ? " Stale data" : ""),
+							  ((flags & DataValidator::ERROR_FLAG_TIMEOUT) ? " Data timeout" : ""),
+							  ((flags & DataValidator::ERROR_FLAG_HIGH_ERRCOUNT) ? " High error count" : ""),
+							  ((flags & DataValidator::ERROR_FLAG_HIGH_ERRDENSITY) ? " High error density" : ""));
+		}
+
+		sensor.last_failover_count = sensor.voter.failover_count();
+		return true;
+	}
+
+	return false;
+}
+
+bool
+Sensors::check_vibration()
+{
+	bool ret = false;
+	hrt_abstime cur_time = hrt_absolute_time();
+
+	if (!_vibration_warning && (_gyro.voter.get_vibration_factor(cur_time) > _parameters.vibration_warning_threshold ||
+				    _accel.voter.get_vibration_factor(cur_time) > _parameters.vibration_warning_threshold ||
+				    _mag.voter.get_vibration_factor(cur_time) > _parameters.vibration_warning_threshold)) {
+
+		if (_vibration_warning_timestamp == 0) {
+			_vibration_warning_timestamp = cur_time;
+
+		} else if (hrt_elapsed_time(&_vibration_warning_timestamp) > 10000 * 1000) {
+			_vibration_warning = true;
+			mavlink_and_console_log_critical(&_mavlink_log_pub, "HIGH VIBRATION! g: %d a: %d m: %d",
+							 (int)(100 * _gyro.voter.get_vibration_factor(cur_time)),
+							 (int)(100 * _accel.voter.get_vibration_factor(cur_time)),
+							 (int)(100 * _mag.voter.get_vibration_factor(cur_time)));
+			ret = true;
+		}
+
+	} else {
+		_vibration_warning_timestamp = 0;
+	}
+
+	return ret;
+}
+
 void
 Sensors::task_main_trampoline(int argc, char *argv[])
 {
 	sensors::g_sensors->task_main();
 }
 
-int
-Sensors::init_sensor_class(const struct orb_metadata *meta, int *subs)
+void
+Sensors::init_sensor_class(const struct orb_metadata *meta, SensorData &sensor_data)
 {
 	unsigned group_count = orb_group_count(meta);
 
@@ -2061,12 +2231,19 @@ Sensors::init_sensor_class(const struct orb_metadata *meta, int *subs)
 	}
 
 	for (unsigned i = 0; i < group_count; i++) {
-		if (subs[i] < 0) {
-			subs[i] = orb_subscribe_multi(meta, i);
+		if (sensor_data.subscription[i] < 0) {
+			sensor_data.subscription[i] = orb_subscribe_multi(meta, i);
+			int32_t priority;
+			orb_priority(sensor_data.subscription[i], &priority);
+			sensor_data.priority[i] = (uint8_t)priority;
 		}
+
+		int32_t priority;
+		orb_priority(sensor_data.subscription[i], &priority);
+		sensor_data.priority[i] = (uint8_t)priority;
 	}
 
-	return group_count;
+	sensor_data.subscription_count = group_count;
 }
 
 void
@@ -2090,84 +2267,79 @@ Sensors::task_main()
 
 	struct sensor_combined_s raw = {};
 
-	/* ensure no overflows can occur */
-	static_assert((sizeof(raw.gyro_timestamp) / sizeof(raw.gyro_timestamp[0])) >= SENSOR_COUNT_MAX,
-		      "SENSOR_COUNT_MAX larger than sensor_combined datastructure fields. Overflow would occur");
-
 	/*
 	 * do subscriptions
 	 */
+	init_sensor_class(ORB_ID(sensor_gyro), _gyro);
 
-	unsigned gcount_prev = _gyro_count;
+	init_sensor_class(ORB_ID(sensor_mag), _mag);
 
-	unsigned mcount_prev = _mag_count;
+	init_sensor_class(ORB_ID(sensor_accel), _accel);
 
-	unsigned acount_prev = _accel_count;
+	init_sensor_class(ORB_ID(sensor_baro), _baro);
 
-	unsigned bcount_prev = _baro_count;
-
-	_gyro_count = init_sensor_class(ORB_ID(sensor_gyro), _gyro_sub);
-
-	_mag_count = init_sensor_class(ORB_ID(sensor_mag), _mag_sub);
-
-	_accel_count = init_sensor_class(ORB_ID(sensor_accel), _accel_sub);
-
-	_baro_count = init_sensor_class(ORB_ID(sensor_baro), _baro_sub);
-
-	if (gcount_prev != _gyro_count ||
-	    mcount_prev != _mag_count ||
-	    acount_prev != _accel_count ||
-	    bcount_prev != _baro_count) {
-
-		/* reload calibration params */
-		parameter_update_poll(true);
-	}
+	/* reload calibration params */
+	parameter_update_poll(true);
 
 	_rc_sub = orb_subscribe(ORB_ID(input_rc));
+
 	_diff_pres_sub = orb_subscribe(ORB_ID(differential_pressure));
+
 	_vcontrol_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode));
+
 	_params_sub = orb_subscribe(ORB_ID(parameter_update));
+
 	_rc_parameter_map_sub = orb_subscribe(ORB_ID(rc_parameter_map));
+
 	_manual_control_sub = orb_subscribe(ORB_ID(manual_control_setpoint));
+
 	_actuator_ctrl_0_sub = orb_subscribe(ORB_ID(actuator_controls_0));
 
-	/*
-	 * do advertisements
-	 */
-	raw.timestamp = hrt_absolute_time();
+	raw.timestamp = 0;
 
 	_battery.reset(&_battery_status);
 
 	/* get a set of initial values */
 	accel_poll(raw);
+
 	gyro_poll(raw);
+
 	mag_poll(raw);
+
 	baro_poll(raw);
+
 	diff_pres_poll(raw);
 
 	parameter_update_poll(true /* forced */);
+
 	rc_parameter_map_poll(true /* forced */);
 
 	/* advertise the sensor_combined topic and make the initial publication */
 	_sensor_pub = orb_advertise(ORB_ID(sensor_combined), &raw);
 
 	/* wakeup source(s) */
-	px4_pollfd_struct_t fds[1] = {};
+	px4_pollfd_struct_t fds[SENSOR_COUNT_MAX] = {};
 
-	/* use the gyro to pace output */
-	fds[0].fd = _gyro_sub[0];
-	fds[0].events = POLLIN;
+	int num_poll_fds = 0;
 
 	_task_should_exit = false;
 
-	raw.timestamp = 0;
-
 	uint64_t _last_config_update = hrt_absolute_time();
 
 	while (!_task_should_exit) {
 
+		/* use the gyro(s) to pace output */
+		if (num_poll_fds != _gyro.subscription_count) { //happens the first time we enter, or when new gyro added
+			num_poll_fds = _gyro.subscription_count;
+
+			for (int i = 0; i < _gyro.subscription_count; ++i) {
+				fds[i].fd = _gyro.subscription[i];
+				fds[i].events = POLLIN;
+			}
+		}
+
 		/* wait for up to 50ms for data */
-		int pret = px4_poll(fds, (sizeof(fds) / sizeof(fds[0])), 50);
+		int pret = px4_poll(fds, num_poll_fds, 50);
 
 		/* if pret == 0 it timed out - periodic check for _task_should_exit, etc. */
 
@@ -2176,11 +2348,12 @@ Sensors::task_main()
 			/* if the polling operation failed because no gyro sensor is available yet,
 			 * then attempt to subscribe once again
 			 */
-			if (_gyro_count == 0) {
-				_gyro_count = init_sensor_class(ORB_ID(sensor_gyro), _gyro_sub);
-				fds[0].fd = _gyro_sub[0];
+			if (_gyro.subscription_count == 0) {
+				init_sensor_class(ORB_ID(sensor_gyro), _gyro);
 			}
 
+			usleep(1000);
+
 			continue;
 		}
 
@@ -2189,60 +2362,38 @@ Sensors::task_main()
 		/* check vehicle status for changes to publication state */
 		vehicle_control_mode_poll();
 
-		/* the timestamp of the raw struct is updated by the gyro_poll() method */
-		/* copy most recent sensor data */
+		/* the timestamp of the raw struct is updated by the gyro_poll() method (this makes the gyro
+		 * a mandatory sensor) */
 		gyro_poll(raw);
 		accel_poll(raw);
 		mag_poll(raw);
 		baro_poll(raw);
 
-		// FIXME TODO: this needs more thinking, otherwise we spam the console and keep switching.
-		/* Work out if main gyro timed out and fail over to alternate gyro.
-		 * However, don't do this if the secondary is not available. */
-		if (hrt_elapsed_time(&raw.gyro_timestamp[0]) > 20 * 1000 && _gyro_sub[1] >= 0) {
-			if (fds[0].fd == _gyro_sub[0]) {
-				PX4_WARN("gyro0 has timed out");
-			}
-
-			/* If the secondary failed as well, go to the tertiary, also only if available. */
-			if (hrt_elapsed_time(&raw.gyro_timestamp[1]) > 20 * 1000 && _gyro_sub[2] >= 0 && (fds[0].fd != _gyro_sub[2])) {
-				fds[0].fd = _gyro_sub[2];
-
-				if (!_hil_enabled) {
-					PX4_WARN("failing over to third gyro");
-				}
-
-			} else if (_gyro_sub[1] >= 0 && (fds[0].fd != _gyro_sub[1])) {
-				fds[0].fd = _gyro_sub[1];
-
-				if (!_hil_enabled) {
-					PX4_WARN("failing over to second gyro");
-				}
-			}
-		}
 
 		/* check battery voltage */
 		adc_poll(raw);
 
 		diff_pres_poll(raw);
 
-		/* Inform other processes that new data is available to copy */
 		if (_publishing && raw.timestamp > 0) {
 			orb_publish(ORB_ID(sensor_combined), _sensor_pub, &raw);
+
+			check_failover(_accel, "Accel");
+			check_failover(_gyro, "Gyro");
+			check_failover(_mag, "Mag");
+			check_failover(_baro, "Baro");
+
+			//check_vibration(); //disabled for now, as it does not seem to be reliable
 		}
 
 		/* keep adding sensors as long as we are not armed,
 		 * when not adding sensors poll for param updates
 		 */
 		if (!_armed && hrt_elapsed_time(&_last_config_update) > 500 * 1000) {
-			_gyro_count = init_sensor_class(ORB_ID(sensor_gyro), _gyro_sub);
-
-			_mag_count = init_sensor_class(ORB_ID(sensor_mag), _mag_sub);
-
-			_accel_count = init_sensor_class(ORB_ID(sensor_accel), _accel_sub);
-
-			_baro_count = init_sensor_class(ORB_ID(sensor_baro), _baro_sub);
-
+			init_sensor_class(ORB_ID(sensor_gyro), _gyro);
+			init_sensor_class(ORB_ID(sensor_mag), _mag);
+			init_sensor_class(ORB_ID(sensor_accel), _accel);
+			init_sensor_class(ORB_ID(sensor_baro), _baro);
 			_last_config_update = hrt_absolute_time();
 
 		} else {
@@ -2260,22 +2411,20 @@ Sensors::task_main()
 		perf_end(_loop_perf);
 	}
 
-	for (unsigned i = 0; i < SENSOR_COUNT_MAX; i++) {
-		if (_gyro_sub[i] >= 0) {
-			orb_unsubscribe(_gyro_sub[i]);
-		}
+	for (unsigned i = 0; i < _gyro.subscription_count; i++) {
+		orb_unsubscribe(_gyro.subscription[i]);
+	}
 
-		if (_accel_sub[i] >= 0) {
-			orb_unsubscribe(_accel_sub[i]);
-		}
+	for (unsigned i = 0; i < _accel.subscription_count; i++) {
+		orb_unsubscribe(_accel.subscription[i]);
+	}
 
-		if (_mag_sub[i] >= 0) {
-			orb_unsubscribe(_mag_sub[i]);
-		}
+	for (unsigned i = 0; i < _mag.subscription_count; i++) {
+		orb_unsubscribe(_mag.subscription[i]);
+	}
 
-		if (_baro_sub[i] >= 0) {
-			orb_unsubscribe(_baro_sub[i]);
-		}
+	for (unsigned i = 0; i < _baro.subscription_count; i++) {
+		orb_unsubscribe(_baro.subscription[i]);
 	}
 
 	orb_unsubscribe(_rc_sub);
@@ -2316,6 +2465,19 @@ Sensors::start()
 	return OK;
 }
 
+void Sensors::print_status()
+{
+	PX4_INFO("gyro status:");
+	_gyro.voter.print();
+	PX4_INFO("accel status:");
+	_accel.voter.print();
+	PX4_INFO("mag status:");
+	_mag.voter.print();
+	PX4_INFO("baro status:");
+	_baro.voter.print();
+}
+
+
 int sensors_main(int argc, char *argv[])
 {
 	if (argc < 2) {
@@ -2360,7 +2522,7 @@ int sensors_main(int argc, char *argv[])
 
 	if (!strcmp(argv[1], "status")) {
 		if (sensors::g_sensors) {
-			PX4_INFO("is running");
+			sensors::g_sensors->print_status();
 			return 0;
 
 		} else {

From 30301187f077a2db8c2b6bd63103e3707ec499b1 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Beat=20K=C3=BCng?= <beat-kueng@gmx.net>
Date: Sat, 25 Jun 2016 12:28:45 +0200
Subject: [PATCH 0928/1230] cleanup sensors_init: remove the static const int
 ERROR and use PX4_ERROR

---
 src/modules/sensors/sensors_init.cpp | 16 +++++-----------
 1 file changed, 5 insertions(+), 11 deletions(-)

diff --git a/src/modules/sensors/sensors_init.cpp b/src/modules/sensors/sensors_init.cpp
index 447d569d50..93742333bd 100644
--- a/src/modules/sensors/sensors_init.cpp
+++ b/src/modules/sensors/sensors_init.cpp
@@ -66,12 +66,6 @@ sensors_init(void)
 
 #else
 
-/* oddly, ERROR is not defined for c++ */
-#ifdef ERROR
-# undef ERROR
-#endif
-static const int ERROR = -1;
-
 /**
  * Do accel-related initialisation.
  */
@@ -126,7 +120,7 @@ accel_init()
 
 	if (!h_accel.isValid()) {
 		warnx("FATAL: no accelerometer found: %s (%d)", ACCEL0_DEVICE_PATH, h_accel.getError());
-		return ERROR;
+		return PX4_ERROR;
 
 	} else {
 
@@ -148,7 +142,7 @@ gyro_init()
 
 	if (!h_gyro.isValid()) {
 		warnx("FATAL: no gyro found: %s (%d)", GYRO0_DEVICE_PATH, h_gyro.getError());
-		return ERROR;
+		return PX4_ERROR;
 
 	}
 
@@ -171,7 +165,7 @@ mag_init()
 
 	if (!h_mag.isValid()) {
 		warnx("FATAL: no magnetometer found: %s (%d)", MAG0_DEVICE_PATH, h_mag.getError());
-		return ERROR;
+		return PX4_ERROR;
 	}
 
 	/* try different mag sampling rates */
@@ -193,7 +187,7 @@ mag_init()
 
 		} else {
 			warnx("FATAL: mag sampling rate could not be set");
-			return ERROR;
+			return PX4_ERROR;
 		}
 	}
 
@@ -208,7 +202,7 @@ baro_init()
 
 	if (!h_baro.isValid()) {
 		warnx("FATAL: No barometer found: %s (%d)", BARO0_DEVICE_PATH, h_baro.getError());
-		return ERROR;
+		return PX4_ERROR;
 	}
 
 	/* set the driver to poll at 150Hz */

From c66f26245cdb46d0d93efcfb64b1b8261a365896 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Beat=20K=C3=BCng?= <beat-kueng@gmx.net>
Date: Sat, 25 Jun 2016 14:19:32 +0200
Subject: [PATCH 0929/1230] sensor_combined.msg: use uint32 for integral_dt

There is no reason to make this 64 bit. The same should be done in
the sensor raw messages, together with further cleanup.
---
 msg/sensor_combined.msg                      | 4 ++--
 src/modules/ekf2_replay/ekf2_replay_main.cpp | 2 +-
 src/modules/sensors/sensors.cpp              | 8 ++++----
 3 files changed, 7 insertions(+), 7 deletions(-)

diff --git a/msg/sensor_combined.msg b/msg/sensor_combined.msg
index 8a7045f9d7..f474900a34 100644
--- a/msg/sensor_combined.msg
+++ b/msg/sensor_combined.msg
@@ -7,11 +7,11 @@
 
 # gyro timstamp is equal to the timestamp of the message
 float32[3] gyro_integral_rad		# delta angle in the NED body frame in rad in the integration time frame
-uint64 gyro_integral_dt			# delta time for gyro integral in us
+uint32 gyro_integral_dt			# delta time for gyro integral in us
 
 uint64 accelerometer_timestamp	# Accelerometer timestamp
 float32[3] accelerometer_integral_m_s		# velocity in NED body frame, in m/s^2
-uint64 accelerometer_integral_dt		# delta time for accel integral in us
+uint32 accelerometer_integral_dt		# delta time for accel integral in us
 
 uint64 magnetometer_timestamp	# Magnetometer timestamp
 float32[3] magnetometer_ga		# Magnetic field in NED body frame, in Gauss
diff --git a/src/modules/ekf2_replay/ekf2_replay_main.cpp b/src/modules/ekf2_replay/ekf2_replay_main.cpp
index b92014081f..a29a21774d 100644
--- a/src/modules/ekf2_replay/ekf2_replay_main.cpp
+++ b/src/modules/ekf2_replay/ekf2_replay_main.cpp
@@ -378,7 +378,7 @@ void Ekf2Replay::setEstimatorInput(uint8_t *data, uint8_t type)
 		parseMessage(data, dest_ptr, type);
 		_sensors.timestamp = replay_part1.time_ref;
 		_sensors.gyro_integral_dt = replay_part1.gyro_integral_dt;
-		_sensors.accelerometer_integral_dt = replay_part1.accelerometer_integral_dt;
+		_sensors.accelerometer_integral_dt = (uint32_t)replay_part1.accelerometer_integral_dt;
 		_sensors.magnetometer_timestamp = replay_part1.magnetometer_timestamp;
 		_sensors.baro_timestamp = replay_part1.baro_timestamp;
 		_sensors.gyro_integral_rad[0] = replay_part1.gyro_integral_x_rad;
diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp
index 658998d879..a268d9b24b 100644
--- a/src/modules/sensors/sensors.cpp
+++ b/src/modules/sensors/sensors.cpp
@@ -1076,7 +1076,7 @@ Sensors::accel_poll(struct sensor_combined_s &raw)
 				_last_sensor_data[i].accelerometer_integral_m_s[1] = vect_int(1);
 				_last_sensor_data[i].accelerometer_integral_m_s[2] = vect_int(2);
 
-				_last_sensor_data[i].accelerometer_integral_dt = accel_report.integral_dt;
+				_last_sensor_data[i].accelerometer_integral_dt = (uint32_t)accel_report.integral_dt;
 
 				float dt = accel_report.integral_dt / 1.e6f;
 				sensor_value = vect_int / dt;
@@ -1093,7 +1093,7 @@ Sensors::accel_poll(struct sensor_combined_s &raw)
 				}
 
 				_last_sensor_data[i].accelerometer_integral_dt =
-					accel_report.timestamp - _last_sensor_data[i].accelerometer_timestamp;
+					(uint32_t)(accel_report.timestamp - _last_sensor_data[i].accelerometer_timestamp);
 				float dt = _last_sensor_data[i].accelerometer_integral_dt / 1.e6f;
 				_last_sensor_data[i].accelerometer_integral_m_s[0] = vect_val(0) * dt;
 				_last_sensor_data[i].accelerometer_integral_m_s[1] = vect_val(1) * dt;
@@ -1149,7 +1149,7 @@ Sensors::gyro_poll(struct sensor_combined_s &raw)
 				_last_sensor_data[i].gyro_integral_rad[1] = vect_int(1);
 				_last_sensor_data[i].gyro_integral_rad[2] = vect_int(2);
 
-				_last_sensor_data[i].gyro_integral_dt = gyro_report.integral_dt;
+				_last_sensor_data[i].gyro_integral_dt = (uint32_t)gyro_report.integral_dt;
 
 				float dt = gyro_report.integral_dt / 1.e6f;
 				sensor_value = vect_int / dt;
@@ -1166,7 +1166,7 @@ Sensors::gyro_poll(struct sensor_combined_s &raw)
 				}
 
 				_last_sensor_data[i].gyro_integral_dt =
-					gyro_report.timestamp - _last_sensor_data[i].timestamp;
+					(uint32_t)(gyro_report.timestamp - _last_sensor_data[i].timestamp);
 				float dt = _last_sensor_data[i].gyro_integral_dt / 1.e6f;
 				_last_sensor_data[i].gyro_integral_rad[0] = vect_val(0) * dt;
 				_last_sensor_data[i].gyro_integral_rad[1] = vect_val(1) * dt;

From c5ea4b43be86d11901bcf50cdd369bdc0307b212 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Beat=20K=C3=BCng?= <beat-kueng@gmx.net>
Date: Sat, 25 Jun 2016 15:57:03 +0200
Subject: [PATCH 0930/1230] sensor_combined.msg: make timestamps relative

This is needed for the new logger & saves some space as well.
---
 msg/sensor_combined.msg                       |  8 +--
 .../attitude_estimator_ekf_main.cpp           |  8 +--
 .../attitude_estimator_q_main.cpp             |  4 +-
 src/modules/commander/commander.cpp           |  3 +-
 src/modules/ekf2/ekf2_main.cpp                | 16 ++++--
 src/modules/ekf2_replay/ekf2_replay_main.cpp  |  4 +-
 .../ekf_att_pos_estimator_main.cpp            | 13 ++---
 src/modules/mavlink/mavlink_messages.cpp      | 12 ++---
 src/modules/mavlink/mavlink_receiver.cpp      |  6 +--
 .../position_estimator_inav_main.cpp          | 12 ++---
 src/modules/sdlog2/sdlog2.c                   | 12 ++---
 src/modules/sensors/sensors.cpp               | 49 +++++++++++++++----
 12 files changed, 94 insertions(+), 53 deletions(-)

diff --git a/msg/sensor_combined.msg b/msg/sensor_combined.msg
index f474900a34..12300a233e 100644
--- a/msg/sensor_combined.msg
+++ b/msg/sensor_combined.msg
@@ -5,18 +5,20 @@
 # change with board revisions and sensor updates.
 #
 
+int32 RELATIVE_TIMESTAMP_INVALID = 2147483647 # (0x7fffffff) If one of the relative timestamp is set to this value, it means the associated sensor values are invalid
+
 # gyro timstamp is equal to the timestamp of the message
 float32[3] gyro_integral_rad		# delta angle in the NED body frame in rad in the integration time frame
 uint32 gyro_integral_dt			# delta time for gyro integral in us
 
-uint64 accelerometer_timestamp	# Accelerometer timestamp
+int32 accelerometer_timestamp_relative	# timestamp + accelerometer_timestamp_relative = Accelerometer timestamp
 float32[3] accelerometer_integral_m_s		# velocity in NED body frame, in m/s^2
 uint32 accelerometer_integral_dt		# delta time for accel integral in us
 
-uint64 magnetometer_timestamp	# Magnetometer timestamp
+int32 magnetometer_timestamp_relative	# timestamp + magnetometer_timestamp_relative = Magnetometer timestamp
 float32[3] magnetometer_ga		# Magnetic field in NED body frame, in Gauss
 
-uint64 baro_timestamp		# Barometer timestamp
+int32 baro_timestamp_relative		# timestamp + baro_timestamp_relative = Barometer timestamp
 float32 baro_alt_meter			# Altitude, already temp. comp.
 float32 baro_temp_celcius		# Temperature in degrees celsius
 
diff --git a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp
index 1a66b19f94..689d44e6aa 100755
--- a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp
+++ b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp
@@ -438,10 +438,10 @@ int attitude_estimator_ekf_thread_main(int argc, char *argv[])
 					z_k[2] =  gyro_rad_s[2] - gyro_offsets[2];
 
 					/* update accelerometer measurements */
-					if (sensor_last_timestamp[1] != raw.accelerometer_timestamp) {
+					if (sensor_last_timestamp[1] != raw.timestamp + raw.accelerometer_timestamp_relative) {
 						update_vect[1] = 1;
 						// sensor_update_hz[1] = 1e6f / (raw.timestamp - sensor_last_timestamp[1]);
-						sensor_last_timestamp[1] = raw.accelerometer_timestamp;
+						sensor_last_timestamp[1] = raw.timestamp + raw.accelerometer_timestamp_relative;
 					}
 
 					hrt_abstime vel_t = 0;
@@ -487,14 +487,14 @@ int attitude_estimator_ekf_thread_main(int argc, char *argv[])
 					z_k[5] = raw_accel(2) - acc(2);
 
 					/* update magnetometer measurements */
-					if (sensor_last_timestamp[2] != raw.magnetometer_timestamp &&
+					if (sensor_last_timestamp[2] != raw.timestamp + raw.magnetometer_timestamp_relative &&
 						/* check that the mag vector is > 0 */
 						fabsf(sqrtf(raw.magnetometer_ga[0] * raw.magnetometer_ga[0] +
 							raw.magnetometer_ga[1] * raw.magnetometer_ga[1] +
 							raw.magnetometer_ga[2] * raw.magnetometer_ga[2])) > 0.1f) {
 						update_vect[2] = 1;
 						// sensor_update_hz[2] = 1e6f / (raw.timestamp - sensor_last_timestamp[2]);
-						sensor_last_timestamp[2] = raw.magnetometer_timestamp;
+						sensor_last_timestamp[2] = raw.timestamp + raw.magnetometer_timestamp_relative;
 					}
 
 					bool vision_updated = false;
diff --git a/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp b/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp
index f56a6793aa..60641f1bc0 100644
--- a/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp
+++ b/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp
@@ -334,7 +334,7 @@ void AttitudeEstimatorQ::task_main()
 				_gyro(2) = sensors.gyro_integral_rad[2] / gyro_dt;
 			}
 
-			if (sensors.accelerometer_timestamp > 0) {
+			if (sensors.accelerometer_timestamp_relative != sensor_combined_s::RELATIVE_TIMESTAMP_INVALID) {
 				float accel_dt = sensors.accelerometer_integral_dt / 1.e6f;
 				_accel(0) = sensors.accelerometer_integral_m_s[0] / accel_dt;
 				_accel(1) = sensors.accelerometer_integral_m_s[1] / accel_dt;
@@ -346,7 +346,7 @@ void AttitudeEstimatorQ::task_main()
 				}
 			}
 
-			if (sensors.magnetometer_timestamp > 0) {
+			if (sensors.magnetometer_timestamp_relative != sensor_combined_s::RELATIVE_TIMESTAMP_INVALID) {
 				_mag(0) = sensors.magnetometer_ga[0];
 				_mag(1) = sensors.magnetometer_ga[1];
 				_mag(2) = sensors.magnetometer_ga[2];
diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp
index c55c2d57d0..7af9ce2f6e 100644
--- a/src/modules/commander/commander.cpp
+++ b/src/modules/commander/commander.cpp
@@ -1788,7 +1788,8 @@ int commander_thread_main(int argc, char *argv[])
 			 * vertical separation from other airtraffic the operator has to know when the
 			 * barometer is inoperational.
 			 * */
-			if (hrt_elapsed_time(&sensors.baro_timestamp) < FAILSAFE_DEFAULT_TIMEOUT) {
+			hrt_abstime baro_timestamp = sensors.timestamp + sensors.baro_timestamp_relative;
+			if (hrt_elapsed_time(&baro_timestamp) < FAILSAFE_DEFAULT_TIMEOUT) {
 				/* handle the case where baro was regained */
 				if (status_flags.barometer_failure) {
 					status_flags.barometer_failure = false;
diff --git a/src/modules/ekf2/ekf2_main.cpp b/src/modules/ekf2/ekf2_main.cpp
index cb6e21d3aa..ea0a1ffada 100644
--- a/src/modules/ekf2/ekf2_main.cpp
+++ b/src/modules/ekf2/ekf2_main.cpp
@@ -509,10 +509,18 @@ void Ekf2::task_main()
 				sensors.gyro_integral_rad, sensors.accelerometer_integral_m_s);
 
 		// read mag data
-		_ekf.setMagData(sensors.magnetometer_timestamp, sensors.magnetometer_ga);
+		if (sensors.magnetometer_timestamp_relative == sensor_combined_s::RELATIVE_TIMESTAMP_INVALID) {
+			_ekf.setMagData(0, sensors.magnetometer_ga);
+		} else {
+			_ekf.setMagData(sensors.timestamp + sensors.magnetometer_timestamp_relative, sensors.magnetometer_ga);
+		}
 
 		// read baro data
-		_ekf.setBaroData(sensors.baro_timestamp, &sensors.baro_alt_meter);
+		if (sensors.baro_timestamp_relative == sensor_combined_s::RELATIVE_TIMESTAMP_INVALID) {
+			_ekf.setBaroData(0, &sensors.baro_alt_meter);
+		} else {
+			_ekf.setBaroData(sensors.timestamp + sensors.baro_timestamp_relative, &sensors.baro_alt_meter);
+		}
 
 		// read gps data if available
 		if (gps_updated) {
@@ -907,8 +915,8 @@ void Ekf2::task_main()
 			replay.time_ref = now;
 			replay.gyro_integral_dt = sensors.gyro_integral_dt;
 			replay.accelerometer_integral_dt = sensors.accelerometer_integral_dt;
-			replay.magnetometer_timestamp = sensors.magnetometer_timestamp;
-			replay.baro_timestamp = sensors.baro_timestamp;
+			replay.magnetometer_timestamp = sensors.timestamp + sensors.magnetometer_timestamp_relative;
+			replay.baro_timestamp = sensors.timestamp + sensors.baro_timestamp_relative;
 			memcpy(replay.gyro_integral_rad, sensors.gyro_integral_rad, sizeof(replay.gyro_integral_rad));
 			memcpy(replay.accelerometer_integral_m_s, sensors.accelerometer_integral_m_s,
 			       sizeof(replay.accelerometer_integral_m_s));
diff --git a/src/modules/ekf2_replay/ekf2_replay_main.cpp b/src/modules/ekf2_replay/ekf2_replay_main.cpp
index a29a21774d..bdf823d335 100644
--- a/src/modules/ekf2_replay/ekf2_replay_main.cpp
+++ b/src/modules/ekf2_replay/ekf2_replay_main.cpp
@@ -379,8 +379,8 @@ void Ekf2Replay::setEstimatorInput(uint8_t *data, uint8_t type)
 		_sensors.timestamp = replay_part1.time_ref;
 		_sensors.gyro_integral_dt = replay_part1.gyro_integral_dt;
 		_sensors.accelerometer_integral_dt = (uint32_t)replay_part1.accelerometer_integral_dt;
-		_sensors.magnetometer_timestamp = replay_part1.magnetometer_timestamp;
-		_sensors.baro_timestamp = replay_part1.baro_timestamp;
+		_sensors.magnetometer_timestamp_relative = (int32_t)(replay_part1.magnetometer_timestamp - _sensors.timestamp);
+		_sensors.baro_timestamp_relative = (int32_t)(replay_part1.baro_timestamp - _sensors.timestamp);
 		_sensors.gyro_integral_rad[0] = replay_part1.gyro_integral_x_rad;
 		_sensors.gyro_integral_rad[1] = replay_part1.gyro_integral_y_rad;
 		_sensors.gyro_integral_rad[2] = replay_part1.gyro_integral_z_rad;
diff --git a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp
index d426c8d3ea..d1588f5a81 100644
--- a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp
+++ b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp
@@ -644,8 +644,9 @@ void AttitudePositionEstimatorEKF::task_main()
 			 *    We run the filter only once all data has been fetched
 			 **/
 
-			if (_baro_init && _sensor_combined.accelerometer_timestamp && _sensor_combined.timestamp
-					&& _sensor_combined.magnetometer_timestamp) {
+			if (_baro_init && _sensor_combined.timestamp &&
+					_sensor_combined.accelerometer_timestamp_relative != sensor_combined_s::RELATIVE_TIMESTAMP_INVALID &&
+					_sensor_combined.magnetometer_timestamp_relative != sensor_combined_s::RELATIVE_TIMESTAMP_INVALID) {
 
 				// maintain filtered baro and gps altitudes to calculate weather offset
 				// baro sample rate is ~70Hz and measurement bandwidth is high
@@ -1365,7 +1366,7 @@ void AttitudePositionEstimatorEKF::pollData()
 
 	perf_count(_perf_gyro);
 
-	if (_last_accel != _sensor_combined.accelerometer_timestamp) {
+	if (_last_accel != _sensor_combined.timestamp + _sensor_combined.accelerometer_timestamp_relative) {
 
 		_ekf->dVelIMU.x = _sensor_combined.accelerometer_integral_m_s[0];
 		_ekf->dVelIMU.y = _sensor_combined.accelerometer_integral_m_s[1];
@@ -1374,18 +1375,18 @@ void AttitudePositionEstimatorEKF::pollData()
 		float accel_dt = _sensor_combined.accelerometer_integral_dt / 1.e6f;
 		_ekf->accel = _ekf->dVelIMU / accel_dt;
 
-		_last_accel = _sensor_combined.accelerometer_timestamp;
+		_last_accel = _sensor_combined.timestamp + _sensor_combined.accelerometer_timestamp_relative;
 	}
 
 	Vector3f mag(_sensor_combined.magnetometer_ga[0], _sensor_combined.magnetometer_ga[1],
 			_sensor_combined.magnetometer_ga[2]);
 
-	if (mag.length() > 0.1f && _last_mag != _sensor_combined.magnetometer_timestamp) {
+	if (mag.length() > 0.1f && _last_mag != _sensor_combined.timestamp + _sensor_combined.magnetometer_timestamp_relative) {
 		_ekf->magData.x = mag.x;
 		_ekf->magData.y = mag.y;
 		_ekf->magData.z = mag.z;
 		_newDataMag = true;
-		_last_mag = _sensor_combined.magnetometer_timestamp;
+		_last_mag = _sensor_combined.timestamp + _sensor_combined.magnetometer_timestamp_relative;
 		perf_count(_perf_mag);
 	}
 
diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp
index da77fd07ba..3c5f24b925 100644
--- a/src/modules/mavlink/mavlink_messages.cpp
+++ b/src/modules/mavlink/mavlink_messages.cpp
@@ -724,10 +724,10 @@ protected:
 		if (_sensor_sub->update(&_sensor_time, &sensor)) {
 			uint16_t fields_updated = 0;
 
-			if (_accel_timestamp != sensor.accelerometer_timestamp) {
+			if (_accel_timestamp != sensor.timestamp + sensor.accelerometer_timestamp_relative) {
 				/* mark first three dimensions as changed */
 				fields_updated |= (1 << 0) | (1 << 1) | (1 << 2);
-				_accel_timestamp = sensor.accelerometer_timestamp;
+				_accel_timestamp = sensor.timestamp + sensor.accelerometer_timestamp_relative;
 			}
 
 			if (_gyro_timestamp != sensor.timestamp) {
@@ -736,16 +736,16 @@ protected:
 				_gyro_timestamp = sensor.timestamp;
 			}
 
-			if (_mag_timestamp != sensor.magnetometer_timestamp) {
+			if (_mag_timestamp != sensor.timestamp + sensor.magnetometer_timestamp_relative) {
 				/* mark third group dimensions as changed */
 				fields_updated |= (1 << 6) | (1 << 7) | (1 << 8);
-				_mag_timestamp = sensor.magnetometer_timestamp;
+				_mag_timestamp = sensor.timestamp + sensor.magnetometer_timestamp_relative;
 			}
 
-			if (_baro_timestamp != sensor.baro_timestamp) {
+			if (_baro_timestamp != sensor.timestamp + sensor.baro_timestamp_relative) {
 				/* mark last group dimensions as changed */
 				fields_updated |= (1 << 9) | (1 << 11) | (1 << 12);
-				_baro_timestamp = sensor.baro_timestamp;
+				_baro_timestamp = sensor.timestamp + sensor.baro_timestamp_relative;
 			}
 			_differential_pressure_sub->update(&_differential_pressure_time, &differential_pressure);
 
diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp
index 62cf3b3f9c..edb2a8f73b 100644
--- a/src/modules/mavlink/mavlink_receiver.cpp
+++ b/src/modules/mavlink/mavlink_receiver.cpp
@@ -1689,16 +1689,16 @@ MavlinkReceiver::handle_message_hil_sensor(mavlink_message_t *msg)
 		_hil_prev_accel[1] = imu.yacc;
 		_hil_prev_accel[2] = imu.zacc;
 		hil_sensors.accelerometer_integral_dt = dt * 1e6f;
-		hil_sensors.accelerometer_timestamp = timestamp;
+		hil_sensors.accelerometer_timestamp_relative = 0;
 
 		hil_sensors.magnetometer_ga[0] = imu.xmag;
 		hil_sensors.magnetometer_ga[1] = imu.ymag;
 		hil_sensors.magnetometer_ga[2] = imu.zmag;
-		hil_sensors.magnetometer_timestamp = timestamp;
+		hil_sensors.magnetometer_timestamp_relative = 0;
 
 		hil_sensors.baro_alt_meter = imu.pressure_alt;
 		hil_sensors.baro_temp_celcius = imu.temperature;
-		hil_sensors.baro_timestamp = timestamp;
+		hil_sensors.baro_timestamp_relative = 0;
 
 		/* publish combined sensor topic */
 		if (_sensors_pub == nullptr) {
diff --git a/src/modules/position_estimator_inav/position_estimator_inav_main.cpp b/src/modules/position_estimator_inav/position_estimator_inav_main.cpp
index 7ef5cd7106..d084517b3e 100644
--- a/src/modules/position_estimator_inav/position_estimator_inav_main.cpp
+++ b/src/modules/position_estimator_inav/position_estimator_inav_main.cpp
@@ -425,8 +425,8 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
 			if (fds_init[0].revents & POLLIN) {
 				orb_copy(ORB_ID(sensor_combined), sensor_combined_sub, &sensor);
 
-				if (wait_baro && sensor.baro_timestamp != baro_timestamp) {
-					baro_timestamp = sensor.baro_timestamp;
+				if (wait_baro && sensor.timestamp + sensor.baro_timestamp_relative != baro_timestamp) {
+					baro_timestamp = sensor.timestamp + sensor.baro_timestamp_relative;
 					baro_wait_for_sample_time = hrt_absolute_time();
 
 					/* mean calculation over several measurements */
@@ -502,7 +502,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
 			if (updated) {
 				orb_copy(ORB_ID(sensor_combined), sensor_combined_sub, &sensor);
 
-				if (sensor.accelerometer_timestamp != accel_timestamp) {
+				if (sensor.timestamp + sensor.accelerometer_timestamp_relative != accel_timestamp) {
 					if (att.R_valid) {
 						float sensor_accel[3];
 						float accel_dt = sensor.accelerometer_integral_dt / 1.e6f;
@@ -525,13 +525,13 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
 						memset(acc, 0, sizeof(acc));
 					}
 
-					accel_timestamp = sensor.accelerometer_timestamp;
+					accel_timestamp = sensor.timestamp + sensor.accelerometer_timestamp_relative;
 					accel_updates++;
 				}
 
-				if (sensor.baro_timestamp != baro_timestamp) {
+				if (sensor.timestamp + sensor.baro_timestamp_relative != baro_timestamp) {
 					corr_baro = baro_offset - sensor.baro_alt_meter - z_est[0];
-					baro_timestamp = sensor.baro_timestamp;
+					baro_timestamp = sensor.timestamp + sensor.baro_timestamp_relative;
 					baro_updates++;
 				}
 			}
diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c
index 3ad1925c6f..4accf90a79 100644
--- a/src/modules/sdlog2/sdlog2.c
+++ b/src/modules/sdlog2/sdlog2.c
@@ -1642,18 +1642,18 @@ int sdlog2_thread_main(int argc, char *argv[])
 				write_IMU = true;
 			}
 
-			if (buf.sensor.accelerometer_timestamp != accelerometer_timestamp) {
-				accelerometer_timestamp = buf.sensor.accelerometer_timestamp;
+			if (buf.sensor.timestamp + buf.sensor.accelerometer_timestamp_relative != accelerometer_timestamp) {
+				accelerometer_timestamp = buf.sensor.timestamp + buf.sensor.accelerometer_timestamp_relative;
 				write_IMU = true;
 			}
 
-			if (buf.sensor.magnetometer_timestamp != magnetometer_timestamp) {
-				magnetometer_timestamp = buf.sensor.magnetometer_timestamp;
+			if (buf.sensor.timestamp + buf.sensor.magnetometer_timestamp_relative != magnetometer_timestamp) {
+				magnetometer_timestamp = buf.sensor.timestamp + buf.sensor.magnetometer_timestamp_relative;
 				write_IMU = true;
 			}
 
-			if (buf.sensor.baro_timestamp != barometer_timestamp) {
-				barometer_timestamp = buf.sensor.baro_timestamp;
+			if (buf.sensor.timestamp + buf.sensor.baro_timestamp_relative != barometer_timestamp) {
+				barometer_timestamp = buf.sensor.timestamp + buf.sensor.baro_timestamp_relative;
 				write_SENS = true;
 			}
 
diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp
index a268d9b24b..4b24e78f5a 100644
--- a/src/modules/sensors/sensors.cpp
+++ b/src/modules/sensors/sensors.cpp
@@ -210,7 +210,8 @@ private:
 
 	struct SensorData {
 		SensorData()
-			: subscription_count(0),
+			: last_best_vote(0),
+			  subscription_count(0),
 			  voter(SENSOR_COUNT_MAX),
 			  last_failover_count(0)
 		{
@@ -221,6 +222,7 @@ private:
 
 		int subscription[SENSOR_COUNT_MAX]; /**< raw sensor data subscription */
 		uint8_t priority[SENSOR_COUNT_MAX]; /**< sensor priority */
+		uint8_t last_best_vote; /**< index of the latest best vote */
 		int subscription_count;
 		DataValidatorGroup voter;
 		unsigned int last_failover_count;
@@ -267,6 +269,9 @@ private:
 	float _last_baro_pressure[SENSOR_COUNT_MAX]; /**< pressure from last baro sensors */
 	float _last_best_baro_pressure; /**< pressure from last best baro */
 	sensor_combined_s _last_sensor_data[SENSOR_COUNT_MAX]; /**< latest sensor data from all sensors instances */
+	uint64_t _last_accel_timestamp[SENSOR_COUNT_MAX]; /**< latest full timestamp */
+	uint64_t _last_mag_timestamp[SENSOR_COUNT_MAX]; /**< latest full timestamp */
+	uint64_t _last_baro_timestamp[SENSOR_COUNT_MAX]; /**< latest full timestamp */
 
 	hrt_abstime _vibration_warning_timestamp;
 	bool _vibration_warning;
@@ -595,6 +600,9 @@ Sensors::Sensors() :
 	memset(&_parameters, 0, sizeof(_parameters));
 	memset(&_rc_parameter_map, 0, sizeof(_rc_parameter_map));
 	memset(&_last_sensor_data, 0, sizeof(_last_sensor_data));
+	memset(&_last_accel_timestamp, 0, sizeof(_last_accel_timestamp));
+	memset(&_last_mag_timestamp, 0, sizeof(_last_mag_timestamp));
+	memset(&_last_baro_timestamp, 0, sizeof(_last_baro_timestamp));
 
 	/* basic r/c parameters */
 	for (unsigned i = 0; i < _rc_max_chan_count; i++) {
@@ -1088,19 +1096,19 @@ Sensors::accel_poll(struct sensor_combined_s &raw)
 
 				sensor_value = vect_val;
 
-				if (_last_sensor_data[i].accelerometer_timestamp == 0) {
-					_last_sensor_data[i].accelerometer_timestamp = accel_report.timestamp - 1000;
+				if (_last_accel_timestamp[i] == 0) {
+					_last_accel_timestamp[i] = accel_report.timestamp - 1000;
 				}
 
 				_last_sensor_data[i].accelerometer_integral_dt =
-					(uint32_t)(accel_report.timestamp - _last_sensor_data[i].accelerometer_timestamp);
+					(uint32_t)(accel_report.timestamp - _last_accel_timestamp[i]);
 				float dt = _last_sensor_data[i].accelerometer_integral_dt / 1.e6f;
 				_last_sensor_data[i].accelerometer_integral_m_s[0] = vect_val(0) * dt;
 				_last_sensor_data[i].accelerometer_integral_m_s[1] = vect_val(1) * dt;
 				_last_sensor_data[i].accelerometer_integral_m_s[2] = vect_val(2) * dt;
 			}
 
-			_last_sensor_data[i].accelerometer_timestamp = accel_report.timestamp;
+			_last_accel_timestamp[i] = accel_report.timestamp;
 			_accel.voter.put(i, accel_report.timestamp, sensor_value.data,
 					 accel_report.error_count, _accel.priority[i]);
 		}
@@ -1115,7 +1123,7 @@ Sensors::accel_poll(struct sensor_combined_s &raw)
 			raw.accelerometer_integral_m_s[1] = _last_sensor_data[best_index].accelerometer_integral_m_s[1];
 			raw.accelerometer_integral_m_s[2] = _last_sensor_data[best_index].accelerometer_integral_m_s[2];
 			raw.accelerometer_integral_dt = _last_sensor_data[best_index].accelerometer_integral_dt;
-			raw.accelerometer_timestamp = _last_sensor_data[best_index].accelerometer_timestamp;
+			_accel.last_best_vote = (uint8_t)best_index;
 		}
 	}
 }
@@ -1189,6 +1197,7 @@ Sensors::gyro_poll(struct sensor_combined_s &raw)
 			raw.gyro_integral_rad[2] = _last_sensor_data[best_index].gyro_integral_rad[2];
 			raw.gyro_integral_dt = _last_sensor_data[best_index].gyro_integral_dt;
 			raw.timestamp = _last_sensor_data[best_index].timestamp;
+			_gyro.last_best_vote = (uint8_t)best_index;
 		}
 	}
 }
@@ -1219,7 +1228,7 @@ Sensors::mag_poll(struct sensor_combined_s &raw)
 			_last_sensor_data[i].magnetometer_ga[1] = vect(1);
 			_last_sensor_data[i].magnetometer_ga[2] = vect(2);
 
-			_last_sensor_data[i].magnetometer_timestamp = mag_report.timestamp;
+			_last_mag_timestamp[i] = mag_report.timestamp;
 			_mag.voter.put(i, mag_report.timestamp, vect.data,
 				       mag_report.error_count, _mag.priority[i]);
 		}
@@ -1233,7 +1242,7 @@ Sensors::mag_poll(struct sensor_combined_s &raw)
 			raw.magnetometer_ga[0] = _last_sensor_data[best_index].magnetometer_ga[0];
 			raw.magnetometer_ga[1] = _last_sensor_data[best_index].magnetometer_ga[1];
 			raw.magnetometer_ga[2] = _last_sensor_data[best_index].magnetometer_ga[2];
-			raw.magnetometer_timestamp = _last_sensor_data[best_index].magnetometer_timestamp;
+			_mag.last_best_vote = (uint8_t)best_index;
 		}
 	}
 }
@@ -1263,7 +1272,7 @@ Sensors::baro_poll(struct sensor_combined_s &raw)
 			_last_sensor_data[i].baro_temp_celcius = baro_report.temperature;
 			_last_baro_pressure[i] = baro_report.pressure;
 
-			_last_sensor_data[i].baro_timestamp = baro_report.timestamp;
+			_last_baro_timestamp[i] = baro_report.timestamp;
 			_baro.voter.put(i, baro_report.timestamp, vect.data,
 					baro_report.error_count, _baro.priority[i]);
 		}
@@ -1276,8 +1285,8 @@ Sensors::baro_poll(struct sensor_combined_s &raw)
 		if (best_index >= 0) {
 			raw.baro_alt_meter = _last_sensor_data[best_index].baro_alt_meter;
 			raw.baro_temp_celcius = _last_sensor_data[best_index].baro_temp_celcius;
-			raw.baro_timestamp = _last_sensor_data[best_index].baro_timestamp;
 			_last_best_baro_pressure = _last_baro_pressure[best_index];
+			_baro.last_best_vote = (uint8_t)best_index;
 		}
 	}
 }
@@ -2267,6 +2276,12 @@ Sensors::task_main()
 
 	struct sensor_combined_s raw = {};
 
+	raw.accelerometer_timestamp_relative = sensor_combined_s::RELATIVE_TIMESTAMP_INVALID;
+
+	raw.magnetometer_timestamp_relative = sensor_combined_s::RELATIVE_TIMESTAMP_INVALID;
+
+	raw.baro_timestamp_relative = sensor_combined_s::RELATIVE_TIMESTAMP_INVALID;
+
 	/*
 	 * do subscriptions
 	 */
@@ -2376,6 +2391,20 @@ Sensors::task_main()
 		diff_pres_poll(raw);
 
 		if (_publishing && raw.timestamp > 0) {
+
+			/* construct relative timestamps */
+			if (_last_accel_timestamp[_accel.last_best_vote]) {
+				raw.accelerometer_timestamp_relative = (int32_t)(_last_accel_timestamp[_accel.last_best_vote] - raw.timestamp);
+			}
+
+			if (_last_mag_timestamp[_mag.last_best_vote]) {
+				raw.magnetometer_timestamp_relative = (int32_t)(_last_mag_timestamp[_mag.last_best_vote] - raw.timestamp);
+			}
+
+			if (_last_baro_timestamp[_baro.last_best_vote]) {
+				raw.baro_timestamp_relative = (int32_t)(_last_baro_timestamp[_baro.last_best_vote] - raw.timestamp);
+			}
+
 			orb_publish(ORB_ID(sensor_combined), _sensor_pub, &raw);
 
 			check_failover(_accel, "Accel");

From 8e136779ec807ead35c77a989ba4879fc746a74d Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Beat=20K=C3=BCng?= <beat-kueng@gmx.net>
Date: Sat, 25 Jun 2016 18:00:50 +0200
Subject: [PATCH 0931/1230] stack sizes: reduce stack sizes for modules that
 use sensor_combined

The sensor_combined topic got reduced from ~780 bytes to 72 bytes.
---
 .../attitude_estimator_ekf/attitude_estimator_ekf_main.cpp      | 2 +-
 src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp  | 2 +-
 src/modules/commander/commander.cpp                             | 2 +-
 src/modules/ekf2/ekf2_main.cpp                                  | 2 +-
 .../position_estimator_inav/position_estimator_inav_main.cpp    | 2 +-
 src/modules/sensors/sensors.cpp                                 | 2 +-
 6 files changed, 6 insertions(+), 6 deletions(-)

diff --git a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp
index 689d44e6aa..48d4cde362 100755
--- a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp
+++ b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp
@@ -175,7 +175,7 @@ int attitude_estimator_ekf_main(int argc, char *argv[])
 		attitude_estimator_ekf_task = px4_task_spawn_cmd("attitude_estimator_ekf",
 					      SCHED_DEFAULT,
 					      SCHED_PRIORITY_MAX - 5,
-					      7700,
+					      7000,
 					      attitude_estimator_ekf_thread_main,
 					      (argv) ? (char * const *)&argv[2] : (char * const *)NULL);
 		return 0;
diff --git a/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp b/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp
index 60641f1bc0..25acbf1ff4 100644
--- a/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp
+++ b/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp
@@ -256,7 +256,7 @@ int AttitudeEstimatorQ::start()
 	_control_task = px4_task_spawn_cmd("attitude_estimator_q",
 					   SCHED_DEFAULT,
 					   SCHED_PRIORITY_MAX - 5,
-					   2500,
+					   2000,
 					   (px4_main_t)&AttitudeEstimatorQ::task_main_trampoline,
 					   nullptr);
 
diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp
index 7af9ce2f6e..9fd1534e15 100644
--- a/src/modules/commander/commander.cpp
+++ b/src/modules/commander/commander.cpp
@@ -297,7 +297,7 @@ int commander_main(int argc, char *argv[])
 		daemon_task = px4_task_spawn_cmd("commander",
 					     SCHED_DEFAULT,
 					     SCHED_PRIORITY_DEFAULT + 40,
-					     3600,
+					     3000,
 					     commander_thread_main,
 					     (char * const *)&argv[0]);
 
diff --git a/src/modules/ekf2/ekf2_main.cpp b/src/modules/ekf2/ekf2_main.cpp
index ea0a1ffada..059d55f427 100644
--- a/src/modules/ekf2/ekf2_main.cpp
+++ b/src/modules/ekf2/ekf2_main.cpp
@@ -1018,7 +1018,7 @@ int Ekf2::start()
 	// On the DSP we seem to get random crashes with a stack size below 13000.
 	const unsigned stack_size = 15000;
 #else
-	const unsigned stack_size = 9000;
+	const unsigned stack_size = 8500;
 #endif
 
 
diff --git a/src/modules/position_estimator_inav/position_estimator_inav_main.cpp b/src/modules/position_estimator_inav/position_estimator_inav_main.cpp
index d084517b3e..8a1bd9319b 100644
--- a/src/modules/position_estimator_inav/position_estimator_inav_main.cpp
+++ b/src/modules/position_estimator_inav/position_estimator_inav_main.cpp
@@ -151,7 +151,7 @@ int position_estimator_inav_main(int argc, char *argv[])
 
 		thread_should_exit = false;
 		position_estimator_inav_task = px4_task_spawn_cmd("position_estimator_inav",
-					       SCHED_DEFAULT, SCHED_PRIORITY_MAX - 5, 5300,
+					       SCHED_DEFAULT, SCHED_PRIORITY_MAX - 5, 4600,
 					       position_estimator_inav_thread_main,
 					       (argv && argc > 2) ? (char *const *) &argv[2] : (char *const *) NULL);
 		return 0;
diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp
index 4b24e78f5a..7437c2eaa2 100644
--- a/src/modules/sensors/sensors.cpp
+++ b/src/modules/sensors/sensors.cpp
@@ -2478,7 +2478,7 @@ Sensors::start()
 	_sensors_task = px4_task_spawn_cmd("sensors",
 					   SCHED_DEFAULT,
 					   SCHED_PRIORITY_MAX - 5,
-					   2200,
+					   1500,
 					   (px4_main_t)&Sensors::task_main_trampoline,
 					   nullptr);
 

From 9c73eae941c2ae569559878160ff66810161fad5 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Beat=20K=C3=BCng?= <beat-kueng@gmx.net>
Date: Thu, 7 Jul 2016 10:35:03 +0200
Subject: [PATCH 0932/1230] sensor_combined: replace accel & gyro integral with
 value, use float for dt

Reason: the value is easier to read & handle (for example plotting). In
most places the value is needed, not the integral.

Note that this breaks the replay format for sdlog2 replay
---
 msg/ekf2_replay.msg                           |  8 +--
 msg/sensor_combined.msg                       | 11 +--
 src/drivers/frsky_telemetry/frsky_data.c      | 11 +--
 src/examples/px4_simple_app/px4_simple_app.c  | 15 ++--
 .../terrain_estimation/terrain_estimator.cpp  |  6 +-
 .../attitude_estimator_ekf_main.cpp           | 36 ++++------
 .../attitude_estimator_q_main.cpp             | 14 ++--
 .../commander/calibration_routines.cpp        |  2 +-
 src/modules/ekf2/ekf2_main.cpp                | 44 ++++++------
 src/modules/ekf2_replay/ekf2_replay_main.cpp  | 14 ++--
 .../ekf_att_pos_estimator_main.cpp            | 32 ++++-----
 .../fw_pos_control_l1_main.cpp                |  6 +-
 .../BlockLocalPositionEstimator.cpp           |  6 +-
 src/modules/mavlink/mavlink_messages.cpp      | 14 ++--
 src/modules/mavlink/mavlink_receiver.cpp      | 16 ++---
 .../position_estimator_inav_main.cpp          | 11 ++-
 src/modules/sdlog2/sdlog2.c                   | 26 ++++---
 src/modules/sdlog2/sdlog2_messages.h          | 16 ++---
 src/modules/sensors/sensors.cpp               | 68 ++++++++-----------
 19 files changed, 154 insertions(+), 202 deletions(-)

diff --git a/msg/ekf2_replay.msg b/msg/ekf2_replay.msg
index 834230da1a..61f0d1d27d 100644
--- a/msg/ekf2_replay.msg
+++ b/msg/ekf2_replay.msg
@@ -1,7 +1,7 @@
 uint64 time_ref 			# ekf2 reference time. This is a timestamp passed to the
 					# estimator which it uses a absolute reference.
-uint64 gyro_integral_dt			# gyro integration period in us
-uint64 accelerometer_integral_dt 	# accelerometer integration period in us
+float32 gyro_integral_dt		# gyro integration period in s
+float32 accelerometer_integral_dt 	# accelerometer integration period in s
 uint64 magnetometer_timestamp 		# timestamp of magnetometer measurement in us
 uint64 baro_timestamp			# timestamp of barometer measurement in us
 uint64 rng_timestamp			# timestamp of range finder measurement in us
@@ -9,8 +9,8 @@ uint64 flow_timestamp			# timestamp of optical flow measurement in us
 uint64 asp_timestamp			# timestamp of airspeed measurement in us
 uint64 ev_timestamp			# timestamp of external vision measurement in us
 
-float32[3] gyro_integral_rad		# integrated gyro vector in rad
-float32[3] accelerometer_integral_m_s  	# integrated accelerometer vector in m/s
+float32[3] gyro_rad			# gyro vector in rad
+float32[3] accelerometer_m_s2  	# accelerometer vector in m/s^2
 
 float32[3] magnetometer_ga 		# magnetometer measurement vector (body fixed NED) in ga
 float32 baro_alt_meter			# barometer altitude measurement in m
diff --git a/msg/sensor_combined.msg b/msg/sensor_combined.msg
index 12300a233e..86682c0f11 100644
--- a/msg/sensor_combined.msg
+++ b/msg/sensor_combined.msg
@@ -5,15 +5,16 @@
 # change with board revisions and sensor updates.
 #
 
-int32 RELATIVE_TIMESTAMP_INVALID = 2147483647 # (0x7fffffff) If one of the relative timestamp is set to this value, it means the associated sensor values are invalid
+int32 RELATIVE_TIMESTAMP_INVALID = 2147483647 # (0x7fffffff) If one of the relative timestamps is set to this value, it means the associated sensor values are invalid
+
 
 # gyro timstamp is equal to the timestamp of the message
-float32[3] gyro_integral_rad		# delta angle in the NED body frame in rad in the integration time frame
-uint32 gyro_integral_dt			# delta time for gyro integral in us
+float32[3] gyro_rad				# delta angle in the NED body frame in rad
+float32 gyro_integral_dt			# delta time for gyro integral in s
 
 int32 accelerometer_timestamp_relative	# timestamp + accelerometer_timestamp_relative = Accelerometer timestamp
-float32[3] accelerometer_integral_m_s		# velocity in NED body frame, in m/s^2
-uint32 accelerometer_integral_dt		# delta time for accel integral in us
+float32[3] accelerometer_m_s2			# velocity in NED body frame, in m/s^2
+float32 accelerometer_integral_dt		# delta time for accel integral in s
 
 int32 magnetometer_timestamp_relative	# timestamp + magnetometer_timestamp_relative = Magnetometer timestamp
 float32[3] magnetometer_ga		# Magnetic field in NED body frame, in Gauss
diff --git a/src/drivers/frsky_telemetry/frsky_data.c b/src/drivers/frsky_telemetry/frsky_data.c
index 47330774a3..cfff8cfac0 100644
--- a/src/drivers/frsky_telemetry/frsky_data.c
+++ b/src/drivers/frsky_telemetry/frsky_data.c
@@ -204,14 +204,9 @@ void frsky_update_topics()
 void frsky_send_frame1(int uart)
 {
 	/* send formatted frame */
-	float acceleration[3];
-	float accel_dt = sensor_combined->accelerometer_integral_dt / 1.e6f;
-	acceleration[0] = sensor_combined->accelerometer_integral_m_s[0] / accel_dt;
-	acceleration[1] = sensor_combined->accelerometer_integral_m_s[1] / accel_dt;
-	acceleration[2] = sensor_combined->accelerometer_integral_m_s[2] / accel_dt;
-	frsky_send_data(uart, FRSKY_ID_ACCEL_X, roundf(acceleration[0] * 1000.0f));
-	frsky_send_data(uart, FRSKY_ID_ACCEL_Y, roundf(acceleration[1] * 1000.0f));
-	frsky_send_data(uart, FRSKY_ID_ACCEL_Z, roundf(acceleration[2] * 1000.0f));
+	frsky_send_data(uart, FRSKY_ID_ACCEL_X, roundf(sensor_combined->accelerometer_m_s2[0] * 1000.0f));
+	frsky_send_data(uart, FRSKY_ID_ACCEL_Y, roundf(sensor_combined->accelerometer_m_s2[1] * 1000.0f));
+	frsky_send_data(uart, FRSKY_ID_ACCEL_Z, roundf(sensor_combined->accelerometer_m_s2[2] * 1000.0f));
 
 	frsky_send_data(uart, FRSKY_ID_BARO_ALT_BP,
 			sensor_combined->baro_alt_meter);
diff --git a/src/examples/px4_simple_app/px4_simple_app.c b/src/examples/px4_simple_app/px4_simple_app.c
index 58ec077170..23cd124130 100644
--- a/src/examples/px4_simple_app/px4_simple_app.c
+++ b/src/examples/px4_simple_app/px4_simple_app.c
@@ -101,18 +101,15 @@ int px4_simple_app_main(int argc, char *argv[])
 				struct sensor_combined_s raw;
 				/* copy sensors raw data into local buffer */
 				orb_copy(ORB_ID(sensor_combined), sensor_sub_fd, &raw);
-				float sensor_accel[3];
-				float accel_dt = raw.accelerometer_integral_dt / 1.e6f;
-				sensor_accel[0] = raw.accelerometer_integral_m_s[0] / accel_dt;
-				sensor_accel[1] = raw.accelerometer_integral_m_s[1] / accel_dt;
-				sensor_accel[2] = raw.accelerometer_integral_m_s[2] / accel_dt;
 				PX4_WARN("[px4_simple_app] Accelerometer:\t%8.4f\t%8.4f\t%8.4f",
-					 (double)sensor_accel[0], (double)sensor_accel[1], (double)sensor_accel[2]);
+					 (double)raw.accelerometer_m_s2[0],
+					 (double)raw.accelerometer_m_s2[1],
+					 (double)raw.accelerometer_m_s2[2]);
 
 				/* set att and publish this information for other apps */
-				att.roll = sensor_accel[0];
-				att.pitch = sensor_accel[1];
-				att.yaw = sensor_accel[2];
+				att.roll = raw.accelerometer_m_s2[0];
+				att.pitch = raw.accelerometer_m_s2[1];
+				att.yaw = raw.accelerometer_m_s2[2];
 				orb_publish(ORB_ID(vehicle_attitude), att_pub, &att);
 			}
 
diff --git a/src/lib/terrain_estimation/terrain_estimator.cpp b/src/lib/terrain_estimation/terrain_estimator.cpp
index d42f57b27e..1b2f075a5a 100644
--- a/src/lib/terrain_estimation/terrain_estimator.cpp
+++ b/src/lib/terrain_estimation/terrain_estimator.cpp
@@ -67,11 +67,7 @@ void TerrainEstimator::predict(float dt, const struct vehicle_attitude_s *attitu
 {
 	if (attitude->R_valid) {
 		matrix::Matrix<float, 3, 3> R_att(attitude->R);
-		matrix::Vector<float, 3> a;
-		float accel_dt = sensor->accelerometer_integral_dt / 1.e6f;
-		a(0) = sensor->accelerometer_integral_m_s[0] / accel_dt;
-		a(1) = sensor->accelerometer_integral_m_s[1] / accel_dt;
-		a(2) = sensor->accelerometer_integral_m_s[2] / accel_dt;
+		matrix::Vector<float, 3> a(sensor->accelerometer_m_s2);
 		matrix::Vector<float, 3> u;
 		u = R_att * a;
 		_u_z = u(2) + 9.81f; // compensate for gravity
diff --git a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp
index 48d4cde362..be703728e2 100755
--- a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp
+++ b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp
@@ -395,19 +395,13 @@ int attitude_estimator_ekf_thread_main(int argc, char *argv[])
 					orb_copy(ORB_ID(vehicle_global_position), sub_global_pos, &global_pos);
 				}
 
-				float gyro_rad_s[3];
-				float gyro_dt = raw.gyro_integral_dt / 1.e6f;
-				gyro_rad_s[0] = raw.gyro_integral_rad[0] / gyro_dt;
-				gyro_rad_s[1] = raw.gyro_integral_rad[1] / gyro_dt;
-				gyro_rad_s[2] = raw.gyro_integral_rad[2] / gyro_dt;
-
 				if (!initialized) {
 					// XXX disabling init for now
 					initialized = true;
 
-					// gyro_offsets[0] += gyro_rad_s[0];
-					// gyro_offsets[1] += gyro_rad_s[1];
-					// gyro_offsets[2] += gyro_rad_s[2];
+					// gyro_offsets[0] += raw.gyro_rad[0];
+					// gyro_offsets[1] += raw.gyro_rad[1];
+					// gyro_offsets[2] += raw.gyro_rad[2];
 					// offset_count++;
 
 					// if (hrt_absolute_time() - start_time > 3000000LL) {
@@ -433,9 +427,9 @@ int attitude_estimator_ekf_thread_main(int argc, char *argv[])
 						sensor_last_timestamp[0] = raw.timestamp;
 					}
 
-					z_k[0] =  gyro_rad_s[0] - gyro_offsets[0];
-					z_k[1] =  gyro_rad_s[1] - gyro_offsets[1];
-					z_k[2] =  gyro_rad_s[2] - gyro_offsets[2];
+					z_k[0] =  raw.gyro_rad[0] - gyro_offsets[0];
+					z_k[1] =  raw.gyro_rad[1] - gyro_offsets[1];
+					z_k[2] =  raw.gyro_rad[2] - gyro_offsets[2];
 
 					/* update accelerometer measurements */
 					if (sensor_last_timestamp[1] != raw.timestamp + raw.accelerometer_timestamp_relative) {
@@ -476,15 +470,9 @@ int attitude_estimator_ekf_thread_main(int argc, char *argv[])
 						last_vel_t = 0;
 					}
 
-					matrix::Vector3f raw_accel;
-					float accel_dt = raw.accelerometer_integral_dt / 1.e6f;
-					raw_accel(0) = raw.accelerometer_integral_m_s[0] / accel_dt;
-					raw_accel(1) = raw.accelerometer_integral_m_s[1] / accel_dt;
-					raw_accel(2) = raw.accelerometer_integral_m_s[2] / accel_dt;
-
-					z_k[3] = raw_accel(0) - acc(0);
-					z_k[4] = raw_accel(1) - acc(1);
-					z_k[5] = raw_accel(2) - acc(2);
+					z_k[3] = raw.accelerometer_m_s2[0] - acc(0);
+					z_k[4] = raw.accelerometer_m_s2[1] - acc(1);
+					z_k[5] = raw.accelerometer_m_s2[2] - acc(2);
 
 					/* update magnetometer measurements */
 					if (sensor_last_timestamp[2] != raw.timestamp + raw.magnetometer_timestamp_relative &&
@@ -628,9 +616,9 @@ int attitude_estimator_ekf_thread_main(int argc, char *argv[])
 					att.pitchacc = x_aposteriori[4];
 					att.yawacc = x_aposteriori[5];
 
-					att.g_comp[0] = raw_accel(0) - acc(0);
-					att.g_comp[1] = raw_accel(1) - acc(1);
-					att.g_comp[2] = raw_accel(2) - acc(2);
+					att.g_comp[0] = raw.accelerometer_m_s2[0] - acc(0);
+					att.g_comp[1] = raw.accelerometer_m_s2[1] - acc(1);
+					att.g_comp[2] = raw.accelerometer_m_s2[2] - acc(2);
 
 					/* copy offsets */
 					memcpy(&att.rate_offsets, &(x_aposteriori[3]), sizeof(att.rate_offsets));
diff --git a/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp b/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp
index 25acbf1ff4..c093682044 100644
--- a/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp
+++ b/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp
@@ -328,17 +328,15 @@ void AttitudeEstimatorQ::task_main()
 			// Feed validator with recent sensor data
 
 			if (sensors.timestamp > 0) {
-				float gyro_dt = sensors.gyro_integral_dt / 1e6;
-				_gyro(0) = sensors.gyro_integral_rad[0] / gyro_dt;
-				_gyro(1) = sensors.gyro_integral_rad[1] / gyro_dt;
-				_gyro(2) = sensors.gyro_integral_rad[2] / gyro_dt;
+				_gyro(0) = sensors.gyro_rad[0];
+				_gyro(1) = sensors.gyro_rad[1];
+				_gyro(2) = sensors.gyro_rad[2];
 			}
 
 			if (sensors.accelerometer_timestamp_relative != sensor_combined_s::RELATIVE_TIMESTAMP_INVALID) {
-				float accel_dt = sensors.accelerometer_integral_dt / 1.e6f;
-				_accel(0) = sensors.accelerometer_integral_m_s[0] / accel_dt;
-				_accel(1) = sensors.accelerometer_integral_m_s[1] / accel_dt;
-				_accel(2) = sensors.accelerometer_integral_m_s[2] / accel_dt;
+				_accel(0) = sensors.accelerometer_m_s2[0];
+				_accel(1) = sensors.accelerometer_m_s2[1];
+				_accel(2) = sensors.accelerometer_m_s2[2];
 
 				if (_accel.length() < 0.01f) {
 					warnx("WARNING: degenerate accel!");
diff --git a/src/modules/commander/calibration_routines.cpp b/src/modules/commander/calibration_routines.cpp
index 7138f34309..b1ee6f5aea 100644
--- a/src/modules/commander/calibration_routines.cpp
+++ b/src/modules/commander/calibration_routines.cpp
@@ -273,7 +273,7 @@ enum detect_orientation_return detect_orientation(orb_advert_t *mavlink_log_pub,
 
 			for (unsigned i = 0; i < ndim; i++) {
 
-				float di = sensor.accelerometer_integral_m_s[i] / (sensor.accelerometer_integral_dt / 1.e6f);
+				float di = sensor.accelerometer_m_s2[i];
 
 				float d = di - accel_ema[i];
 				accel_ema[i] += d * w;
diff --git a/src/modules/ekf2/ekf2_main.cpp b/src/modules/ekf2/ekf2_main.cpp
index 059d55f427..b408227217 100644
--- a/src/modules/ekf2/ekf2_main.cpp
+++ b/src/modules/ekf2/ekf2_main.cpp
@@ -505,8 +505,16 @@ void Ekf2::task_main()
 		}
 
 		// push imu data into estimator
-		_ekf.setIMUData(now, sensors.gyro_integral_dt, sensors.accelerometer_integral_dt,
-				sensors.gyro_integral_rad, sensors.accelerometer_integral_m_s);
+		float gyro_integral[3];
+		gyro_integral[0] = sensors.gyro_rad[0] * sensors.gyro_integral_dt;
+		gyro_integral[1] = sensors.gyro_rad[1] * sensors.gyro_integral_dt;
+		gyro_integral[2] = sensors.gyro_rad[2] * sensors.gyro_integral_dt;
+		float accel_integral[3];
+		accel_integral[0] = sensors.accelerometer_m_s2[0] * sensors.accelerometer_integral_dt;
+		accel_integral[1] = sensors.accelerometer_m_s2[1] * sensors.accelerometer_integral_dt;
+		accel_integral[2] = sensors.accelerometer_m_s2[2] * sensors.accelerometer_integral_dt;
+		_ekf.setIMUData(now, sensors.gyro_integral_dt * 1.e6f, sensors.accelerometer_integral_dt * 1.e6f,
+				gyro_integral, accel_integral);
 
 		// read mag data
 		if (sensors.magnetometer_timestamp_relative == sensor_combined_s::RELATIVE_TIMESTAMP_INVALID) {
@@ -622,15 +630,14 @@ void Ekf2::task_main()
 			control_state_s ctrl_state = {};
 			float gyro_bias[3] = {};
 			_ekf.get_gyro_bias(gyro_bias);
-			float gyro_rad_s[3];
-			float gyro_dt = sensors.gyro_integral_dt / 1.e6f;
-			gyro_rad_s[0] = sensors.gyro_integral_rad[0] / gyro_dt - gyro_bias[0];
-			gyro_rad_s[1] = sensors.gyro_integral_rad[1] / gyro_dt - gyro_bias[1];
-			gyro_rad_s[2] = sensors.gyro_integral_rad[2] / gyro_dt - gyro_bias[2];
 			ctrl_state.timestamp = hrt_absolute_time();
-			ctrl_state.roll_rate = _lp_roll_rate.apply(gyro_rad_s[0]);
-			ctrl_state.pitch_rate = _lp_pitch_rate.apply(gyro_rad_s[1]);
-			ctrl_state.yaw_rate = _lp_yaw_rate.apply(gyro_rad_s[2]);
+			float gyro_rad[3];
+			gyro_rad[0] = sensors.gyro_rad[0] - gyro_bias[0];
+			gyro_rad[1] = sensors.gyro_rad[1] - gyro_bias[1];
+			gyro_rad[2] = sensors.gyro_rad[2] - gyro_bias[2];
+			ctrl_state.roll_rate = _lp_roll_rate.apply(gyro_rad[0]);
+			ctrl_state.pitch_rate = _lp_pitch_rate.apply(gyro_rad[1]);
+			ctrl_state.yaw_rate = _lp_yaw_rate.apply(gyro_rad[2]);
 
 			// Velocity in body frame
 			float velocity[3];
@@ -657,11 +664,7 @@ void Ekf2::task_main()
 			ctrl_state.q[3] = q(3);
 
 			// Acceleration data
-			matrix::Vector<float, 3> acceleration;
-			float accel_dt = sensors.accelerometer_integral_dt / 1.e6f;
-			acceleration(0) = sensors.accelerometer_integral_m_s[0] / accel_dt;
-			acceleration(1) = sensors.accelerometer_integral_m_s[1] / accel_dt;
-			acceleration(2) = sensors.accelerometer_integral_m_s[2] / accel_dt;
+			matrix::Vector<float, 3> acceleration(sensors.accelerometer_m_s2);
 
 			float accel_bias[3];
 			_ekf.get_accel_bias(accel_bias);
@@ -722,9 +725,9 @@ void Ekf2::task_main()
 			att.q[3] = q(3);
 			att.q_valid = true;
 
-			att.rollspeed = gyro_rad_s[0];
-			att.pitchspeed = gyro_rad_s[1];
-			att.yawspeed = gyro_rad_s[2];
+			att.rollspeed = gyro_rad[0];
+			att.pitchspeed = gyro_rad[1];
+			att.yawspeed = gyro_rad[2];
 
 			// publish vehicle attitude data
 			if (_att_pub == nullptr) {
@@ -917,9 +920,8 @@ void Ekf2::task_main()
 			replay.accelerometer_integral_dt = sensors.accelerometer_integral_dt;
 			replay.magnetometer_timestamp = sensors.timestamp + sensors.magnetometer_timestamp_relative;
 			replay.baro_timestamp = sensors.timestamp + sensors.baro_timestamp_relative;
-			memcpy(replay.gyro_integral_rad, sensors.gyro_integral_rad, sizeof(replay.gyro_integral_rad));
-			memcpy(replay.accelerometer_integral_m_s, sensors.accelerometer_integral_m_s,
-			       sizeof(replay.accelerometer_integral_m_s));
+			memcpy(replay.gyro_rad, sensors.gyro_rad, sizeof(replay.gyro_rad));
+			memcpy(replay.accelerometer_m_s2, sensors.accelerometer_m_s2, sizeof(replay.accelerometer_m_s2));
 			memcpy(replay.magnetometer_ga, sensors.magnetometer_ga, sizeof(replay.magnetometer_ga));
 			replay.baro_alt_meter = sensors.baro_alt_meter;
 
diff --git a/src/modules/ekf2_replay/ekf2_replay_main.cpp b/src/modules/ekf2_replay/ekf2_replay_main.cpp
index bdf823d335..4c30fe350d 100644
--- a/src/modules/ekf2_replay/ekf2_replay_main.cpp
+++ b/src/modules/ekf2_replay/ekf2_replay_main.cpp
@@ -378,15 +378,15 @@ void Ekf2Replay::setEstimatorInput(uint8_t *data, uint8_t type)
 		parseMessage(data, dest_ptr, type);
 		_sensors.timestamp = replay_part1.time_ref;
 		_sensors.gyro_integral_dt = replay_part1.gyro_integral_dt;
-		_sensors.accelerometer_integral_dt = (uint32_t)replay_part1.accelerometer_integral_dt;
+		_sensors.accelerometer_integral_dt = replay_part1.accelerometer_integral_dt;
 		_sensors.magnetometer_timestamp_relative = (int32_t)(replay_part1.magnetometer_timestamp - _sensors.timestamp);
 		_sensors.baro_timestamp_relative = (int32_t)(replay_part1.baro_timestamp - _sensors.timestamp);
-		_sensors.gyro_integral_rad[0] = replay_part1.gyro_integral_x_rad;
-		_sensors.gyro_integral_rad[1] = replay_part1.gyro_integral_y_rad;
-		_sensors.gyro_integral_rad[2] = replay_part1.gyro_integral_z_rad;
-		_sensors.accelerometer_integral_m_s[0] = replay_part1.accelerometer_integral_x_m_s;
-		_sensors.accelerometer_integral_m_s[1] = replay_part1.accelerometer_integral_y_m_s;
-		_sensors.accelerometer_integral_m_s[2] = replay_part1.accelerometer_integral_z_m_s;
+		_sensors.gyro_rad[0] = replay_part1.gyro_x_rad;
+		_sensors.gyro_rad[1] = replay_part1.gyro_y_rad;
+		_sensors.gyro_rad[2] = replay_part1.gyro_z_rad;
+		_sensors.accelerometer_m_s2[0] = replay_part1.accelerometer_x_m_s2;
+		_sensors.accelerometer_m_s2[1] = replay_part1.accelerometer_y_m_s2;
+		_sensors.accelerometer_m_s2[2] = replay_part1.accelerometer_z_m_s2;
 		_sensors.magnetometer_ga[0] = replay_part1.magnetometer_x_ga;
 		_sensors.magnetometer_ga[1] = replay_part1.magnetometer_y_ga;
 		_sensors.magnetometer_ga[2] = replay_part1.magnetometer_z_ga;
diff --git a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp
index d1588f5a81..92e49b3eab 100644
--- a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp
+++ b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp
@@ -1357,23 +1357,23 @@ void AttitudePositionEstimatorEKF::pollData()
 	/* fill in last data set */
 	_ekf->dtIMU = deltaT;
 
-	_ekf->dAngIMU.x = _sensor_combined.gyro_integral_rad[0];
-	_ekf->dAngIMU.y = _sensor_combined.gyro_integral_rad[1];
-	_ekf->dAngIMU.z = _sensor_combined.gyro_integral_rad[2];
+	_ekf->angRate.x = _sensor_combined.gyro_rad[0];
+	_ekf->angRate.y = _sensor_combined.gyro_rad[1];
+	_ekf->angRate.z = _sensor_combined.gyro_rad[2];
 
-	float gyro_dt = _sensor_combined.gyro_integral_dt / 1.e6f;
-	_ekf->angRate = _ekf->dAngIMU / gyro_dt;
+	float gyro_dt = _sensor_combined.gyro_integral_dt;
+	_ekf->dAngIMU = _ekf->angRate * gyro_dt;
 
 	perf_count(_perf_gyro);
 
 	if (_last_accel != _sensor_combined.timestamp + _sensor_combined.accelerometer_timestamp_relative) {
 
-		_ekf->dVelIMU.x = _sensor_combined.accelerometer_integral_m_s[0];
-		_ekf->dVelIMU.y = _sensor_combined.accelerometer_integral_m_s[1];
-		_ekf->dVelIMU.z = _sensor_combined.accelerometer_integral_m_s[2];
+		_ekf->accel.x = _sensor_combined.accelerometer_m_s2[0];
+		_ekf->accel.y = _sensor_combined.accelerometer_m_s2[1];
+		_ekf->accel.z = _sensor_combined.accelerometer_m_s2[2];
 
-		float accel_dt = _sensor_combined.accelerometer_integral_dt / 1.e6f;
-		_ekf->accel = _ekf->dVelIMU / accel_dt;
+		float accel_dt = _sensor_combined.accelerometer_integral_dt;
+		_ekf->dVelIMU = _ekf->accel * accel_dt;
 
 		_last_accel = _sensor_combined.timestamp + _sensor_combined.accelerometer_timestamp_relative;
 	}
@@ -1396,8 +1396,8 @@ void AttitudePositionEstimatorEKF::pollData()
 	// leave this in as long as larger improvements are still being made.
 #if 0
 
-	float deltaTIntegral = _sensor_combined.gyro_integral_dt / 1e6f;
-	float deltaTIntAcc = _sensor_combined.accelerometer_integral_dt / 1e6f;
+	float deltaTIntegral = _sensor_combined.gyro_integral_dt;
+	float deltaTIntAcc = _sensor_combined.accelerometer_integral_dt;
 
 	static unsigned dtoverflow5 = 0;
 	static unsigned dtoverflow10 = 0;
@@ -1413,10 +1413,10 @@ void AttitudePositionEstimatorEKF::pollData()
 			 (double)_ekf->dVelIMU.x, (double)_ekf->dVelIMU.y, (double)_ekf->dVelIMU.z);
 
 		PX4_WARN("INT: dang: %8.4f %8.4f dvel: %8.4f %8.4f %8.4f",
-			 (double)(_sensor_combined.gyro_integral_rad[0]), (double)(_sensor_combined.gyro_integral_rad[2]),
-			 (double)(_sensor_combined.accelerometer_integral_m_s[0]),
-			 (double)(_sensor_combined.accelerometer_integral_m_s[1]),
-			 (double)(_sensor_combined.accelerometer_integral_m_s[2]));
+			 (double)(_sensor_combined.gyro_rad[0]), (double)(_sensor_combined.gyro_rad[2]),
+			 (double)(_sensor_combined.accelerometer_m_s2[0]),
+			 (double)(_sensor_combined.accelerometer_m_s2[1]),
+			 (double)(_sensor_combined.accelerometer_m_s2[2]));
 
 		PX4_WARN("EKF rate: %8.4f, %8.4f, %8.4f",
 			 (double)_att.rollspeed, (double)_att.pitchspeed, (double)_att.yawspeed);
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 f604581a75..ae38b77095 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
@@ -1230,11 +1230,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
 	float eas2tas = 1.0f; // XXX calculate actual number based on current measurements
 
 	/* filter speed and altitude for controller */
-	math::Vector<3> accel_body;
-	float accel_dt = _sensor_combined.accelerometer_integral_dt / 1.e6f;
-	accel_body(0) = _sensor_combined.accelerometer_integral_m_s[0] / accel_dt;
-	accel_body(1) = _sensor_combined.accelerometer_integral_m_s[1] / accel_dt;
-	accel_body(2) = _sensor_combined.accelerometer_integral_m_s[2] / accel_dt;
+	math::Vector<3> accel_body(_sensor_combined.accelerometer_m_s2);
 	math::Vector<3> accel_earth = _R_nb * accel_body;
 
 	/* tell TECS to update its state, but let it know when it cannot actually control the plane */
diff --git a/src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp b/src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp
index cb0301f8a0..50f47a9621 100644
--- a/src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp
+++ b/src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp
@@ -844,11 +844,7 @@ void BlockLocalPositionEstimator::predict()
 
 	if (integrate && _sub_att.get().R_valid) {
 		Matrix3f R_att(_sub_att.get().R);
-		Vector3f a;
-		float accel_dt = _sub_sensor.get().accelerometer_integral_dt / 1.e6f;
-		a(0) = _sub_sensor.get().accelerometer_integral_m_s[0] / accel_dt;
-		a(1) = _sub_sensor.get().accelerometer_integral_m_s[1] / accel_dt;
-		a(2) = _sub_sensor.get().accelerometer_integral_m_s[2] / accel_dt;
+		Vector3f a(_sub_sensor.get().accelerometer_m_s2);
 		_u = R_att * a;
 		_u(U_az) += 9.81f; // add g
 
diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp
index 3c5f24b925..038796de31 100644
--- a/src/modules/mavlink/mavlink_messages.cpp
+++ b/src/modules/mavlink/mavlink_messages.cpp
@@ -752,14 +752,12 @@ protected:
 			mavlink_highres_imu_t msg;
 
 			msg.time_usec = sensor.timestamp;
-			float accel_dt = sensor.accelerometer_integral_dt / 1.e6f;
-			msg.xacc = sensor.accelerometer_integral_m_s[0] / accel_dt;
-			msg.yacc = sensor.accelerometer_integral_m_s[1] / accel_dt;
-			msg.zacc = sensor.accelerometer_integral_m_s[2] / accel_dt;
-			float gyro_dt = sensor.gyro_integral_dt / 1.e6f;
-			msg.xgyro = sensor.gyro_integral_rad[0] / gyro_dt;
-			msg.ygyro = sensor.gyro_integral_rad[1] / gyro_dt;
-			msg.zgyro = sensor.gyro_integral_rad[2] / gyro_dt;
+			msg.xacc = sensor.accelerometer_m_s2[0];
+			msg.yacc = sensor.accelerometer_m_s2[1];
+			msg.zacc = sensor.accelerometer_m_s2[2];
+			msg.xgyro = sensor.gyro_rad[0];
+			msg.ygyro = sensor.gyro_rad[1];
+			msg.zgyro = sensor.gyro_rad[2];
 			msg.xmag = sensor.magnetometer_ga[0];
 			msg.ymag = sensor.magnetometer_ga[1];
 			msg.zmag = sensor.magnetometer_ga[2];
diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp
index edb2a8f73b..dc9b711385 100644
--- a/src/modules/mavlink/mavlink_receiver.cpp
+++ b/src/modules/mavlink/mavlink_receiver.cpp
@@ -1673,22 +1673,22 @@ MavlinkReceiver::handle_message_hil_sensor(mavlink_message_t *msg)
 	{
 		struct sensor_combined_s hil_sensors = {};
 
-		hil_sensors.gyro_integral_rad[0] = 0.5f * (imu.xgyro + _hil_prev_gyro[0]) * dt;
-		hil_sensors.gyro_integral_rad[1] = 0.5f * (imu.ygyro + _hil_prev_gyro[1]) * dt;
-		hil_sensors.gyro_integral_rad[2] = 0.5f * (imu.zgyro + _hil_prev_gyro[2]) * dt;
+		hil_sensors.gyro_rad[0] = 0.5f * (imu.xgyro + _hil_prev_gyro[0]);
+		hil_sensors.gyro_rad[1] = 0.5f * (imu.ygyro + _hil_prev_gyro[1]);
+		hil_sensors.gyro_rad[2] = 0.5f * (imu.zgyro + _hil_prev_gyro[2]);
 		_hil_prev_gyro[0] = imu.xgyro;
 		_hil_prev_gyro[1] = imu.ygyro;
 		_hil_prev_gyro[2] = imu.zgyro;
-		hil_sensors.gyro_integral_dt = dt * 1e6f;
+		hil_sensors.gyro_integral_dt = dt;
 		hil_sensors.timestamp = timestamp;
 
-		hil_sensors.accelerometer_integral_m_s[0] = 0.5f * (imu.xacc + _hil_prev_accel[0]) * dt;
-		hil_sensors.accelerometer_integral_m_s[1] = 0.5f * (imu.yacc + _hil_prev_accel[1]) * dt;
-		hil_sensors.accelerometer_integral_m_s[2] = 0.5f * (imu.zacc + _hil_prev_accel[2]) * dt;
+		hil_sensors.accelerometer_m_s2[0] = 0.5f * (imu.xacc + _hil_prev_accel[0]);
+		hil_sensors.accelerometer_m_s2[1] = 0.5f * (imu.yacc + _hil_prev_accel[1]);
+		hil_sensors.accelerometer_m_s2[2] = 0.5f * (imu.zacc + _hil_prev_accel[2]);
 		_hil_prev_accel[0] = imu.xacc;
 		_hil_prev_accel[1] = imu.yacc;
 		_hil_prev_accel[2] = imu.zacc;
-		hil_sensors.accelerometer_integral_dt = dt * 1e6f;
+		hil_sensors.accelerometer_integral_dt = dt;
 		hil_sensors.accelerometer_timestamp_relative = 0;
 
 		hil_sensors.magnetometer_ga[0] = imu.xmag;
diff --git a/src/modules/position_estimator_inav/position_estimator_inav_main.cpp b/src/modules/position_estimator_inav/position_estimator_inav_main.cpp
index 8a1bd9319b..66c75c8fcf 100644
--- a/src/modules/position_estimator_inav/position_estimator_inav_main.cpp
+++ b/src/modules/position_estimator_inav/position_estimator_inav_main.cpp
@@ -504,18 +504,17 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
 
 				if (sensor.timestamp + sensor.accelerometer_timestamp_relative != accel_timestamp) {
 					if (att.R_valid) {
-						float sensor_accel[3];
-						float accel_dt = sensor.accelerometer_integral_dt / 1.e6f;
-						sensor_accel[0] = sensor.accelerometer_integral_m_s[0] / accel_dt - acc_bias[0];
-						sensor_accel[1] = sensor.accelerometer_integral_m_s[1] / accel_dt - acc_bias[1];
-						sensor_accel[2] = sensor.accelerometer_integral_m_s[2] / accel_dt - acc_bias[2];
+						/* correct accel bias */
+						sensor.accelerometer_m_s2[0] -= acc_bias[0];
+						sensor.accelerometer_m_s2[1] -= acc_bias[1];
+						sensor.accelerometer_m_s2[2] -= acc_bias[2];
 
 						/* transform acceleration vector from body frame to NED frame */
 						for (int i = 0; i < 3; i++) {
 							acc[i] = 0.0f;
 
 							for (int j = 0; j < 3; j++) {
-								acc[i] += PX4_R(att.R, i, j) * sensor_accel[j];
+								acc[i] += PX4_R(att.R, i, j) * sensor.accelerometer_m_s2[j];
 							}
 						}
 
diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c
index 4accf90a79..1f282d4787 100644
--- a/src/modules/sdlog2/sdlog2.c
+++ b/src/modules/sdlog2/sdlog2.c
@@ -1551,12 +1551,12 @@ int sdlog2_thread_main(int argc, char *argv[])
 				log_msg.body.log_RPL1.accelerometer_integral_dt = buf.replay.accelerometer_integral_dt;
 				log_msg.body.log_RPL1.magnetometer_timestamp = buf.replay.magnetometer_timestamp;
 				log_msg.body.log_RPL1.baro_timestamp = buf.replay.baro_timestamp;
-				log_msg.body.log_RPL1.gyro_integral_x_rad = buf.replay.gyro_integral_rad[0];
-				log_msg.body.log_RPL1.gyro_integral_y_rad = buf.replay.gyro_integral_rad[1];
-				log_msg.body.log_RPL1.gyro_integral_z_rad = buf.replay.gyro_integral_rad[2];
-				log_msg.body.log_RPL1.accelerometer_integral_x_m_s = buf.replay.accelerometer_integral_m_s[0];
-				log_msg.body.log_RPL1.accelerometer_integral_y_m_s = buf.replay.accelerometer_integral_m_s[1];
-				log_msg.body.log_RPL1.accelerometer_integral_z_m_s = buf.replay.accelerometer_integral_m_s[2];
+				log_msg.body.log_RPL1.gyro_x_rad = buf.replay.gyro_rad[0];
+				log_msg.body.log_RPL1.gyro_y_rad = buf.replay.gyro_rad[1];
+				log_msg.body.log_RPL1.gyro_z_rad = buf.replay.gyro_rad[2];
+				log_msg.body.log_RPL1.accelerometer_x_m_s2 = buf.replay.accelerometer_m_s2[0];
+				log_msg.body.log_RPL1.accelerometer_y_m_s2 = buf.replay.accelerometer_m_s2[1];
+				log_msg.body.log_RPL1.accelerometer_z_m_s2 = buf.replay.accelerometer_m_s2[2];
 				log_msg.body.log_RPL1.magnetometer_x_ga = buf.replay.magnetometer_ga[0];
 				log_msg.body.log_RPL1.magnetometer_y_ga = buf.replay.magnetometer_ga[1];
 				log_msg.body.log_RPL1.magnetometer_z_ga = buf.replay.magnetometer_ga[2];
@@ -1660,14 +1660,12 @@ int sdlog2_thread_main(int argc, char *argv[])
 			if (write_IMU) {
 				log_msg.msg_type = LOG_IMU_MSG;
 
-				float gyro_dt = buf.sensor.gyro_integral_dt / 1.e6f;
-				log_msg.body.log_IMU.gyro_x = buf.sensor.gyro_integral_rad[0] / gyro_dt;
-				log_msg.body.log_IMU.gyro_y = buf.sensor.gyro_integral_rad[1] / gyro_dt;
-				log_msg.body.log_IMU.gyro_z = buf.sensor.gyro_integral_rad[2] / gyro_dt;
-				float accel_dt = buf.sensor.accelerometer_integral_dt / 1.e6f;
-				log_msg.body.log_IMU.acc_x = buf.sensor.accelerometer_integral_m_s[0] / accel_dt;
-				log_msg.body.log_IMU.acc_y = buf.sensor.accelerometer_integral_m_s[1] / accel_dt;
-				log_msg.body.log_IMU.acc_z = buf.sensor.accelerometer_integral_m_s[2] / accel_dt;
+				log_msg.body.log_IMU.gyro_x = buf.sensor.gyro_rad[0];
+				log_msg.body.log_IMU.gyro_y = buf.sensor.gyro_rad[1];
+				log_msg.body.log_IMU.gyro_z = buf.sensor.gyro_rad[2];
+				log_msg.body.log_IMU.acc_x = buf.sensor.accelerometer_m_s2[0];
+				log_msg.body.log_IMU.acc_y = buf.sensor.accelerometer_m_s2[1];
+				log_msg.body.log_IMU.acc_z = buf.sensor.accelerometer_m_s2[2];
 				log_msg.body.log_IMU.mag_x = buf.sensor.magnetometer_ga[0];
 				log_msg.body.log_IMU.mag_y = buf.sensor.magnetometer_ga[1];
 				log_msg.body.log_IMU.mag_z = buf.sensor.magnetometer_ga[2];
diff --git a/src/modules/sdlog2/sdlog2_messages.h b/src/modules/sdlog2/sdlog2_messages.h
index 58ae693b64..8cbcdba6e8 100644
--- a/src/modules/sdlog2/sdlog2_messages.h
+++ b/src/modules/sdlog2/sdlog2_messages.h
@@ -520,16 +520,16 @@ struct log_EST5_s {
 #define LOG_RPL1_MSG 51
 struct log_RPL1_s {
 	uint64_t time_ref;
-	uint64_t gyro_integral_dt;
-	uint64_t accelerometer_integral_dt;
+	float gyro_integral_dt;
+	float accelerometer_integral_dt;
 	uint64_t magnetometer_timestamp;
 	uint64_t baro_timestamp;
-	float gyro_integral_x_rad;
-	float gyro_integral_y_rad;
-	float gyro_integral_z_rad;
-	float accelerometer_integral_x_m_s;
-	float accelerometer_integral_y_m_s;
-	float accelerometer_integral_z_m_s;
+	float gyro_x_rad;
+	float gyro_y_rad;
+	float gyro_z_rad;
+	float accelerometer_x_m_s2;
+	float accelerometer_y_m_s2;
+	float accelerometer_z_m_s2;
 	float magnetometer_x_ga;
 	float magnetometer_y_ga;
 	float magnetometer_z_ga;
diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp
index 7437c2eaa2..db605504fd 100644
--- a/src/modules/sensors/sensors.cpp
+++ b/src/modules/sensors/sensors.cpp
@@ -1074,42 +1074,36 @@ Sensors::accel_poll(struct sensor_combined_s &raw)
 			}
 
 			got_update = true;
-			math::Vector<3> sensor_value;
 
 			if (accel_report.integral_dt != 0) {
 				math::Vector<3> vect_int(accel_report.x_integral, accel_report.y_integral, accel_report.z_integral);
 				vect_int = _board_rotation * vect_int;
 
-				_last_sensor_data[i].accelerometer_integral_m_s[0] = vect_int(0);
-				_last_sensor_data[i].accelerometer_integral_m_s[1] = vect_int(1);
-				_last_sensor_data[i].accelerometer_integral_m_s[2] = vect_int(2);
-
-				_last_sensor_data[i].accelerometer_integral_dt = (uint32_t)accel_report.integral_dt;
-
 				float dt = accel_report.integral_dt / 1.e6f;
-				sensor_value = vect_int / dt;
+				_last_sensor_data[i].accelerometer_integral_dt = dt;
+
+				_last_sensor_data[i].accelerometer_m_s2[0] = vect_int(0) / dt;
+				_last_sensor_data[i].accelerometer_m_s2[1] = vect_int(1) / dt;
+				_last_sensor_data[i].accelerometer_m_s2[2] = vect_int(2) / dt;
 
 			} else {
-				//this is undesirable: a driver did not set the integral, so we have to construct it ourselves
+				//using the value instead of the integral (the integral is the prefered choice)
 				math::Vector<3> vect_val(accel_report.x, accel_report.y, accel_report.z);
 				vect_val = _board_rotation * vect_val;
 
-				sensor_value = vect_val;
-
 				if (_last_accel_timestamp[i] == 0) {
 					_last_accel_timestamp[i] = accel_report.timestamp - 1000;
 				}
 
 				_last_sensor_data[i].accelerometer_integral_dt =
-					(uint32_t)(accel_report.timestamp - _last_accel_timestamp[i]);
-				float dt = _last_sensor_data[i].accelerometer_integral_dt / 1.e6f;
-				_last_sensor_data[i].accelerometer_integral_m_s[0] = vect_val(0) * dt;
-				_last_sensor_data[i].accelerometer_integral_m_s[1] = vect_val(1) * dt;
-				_last_sensor_data[i].accelerometer_integral_m_s[2] = vect_val(2) * dt;
+					(accel_report.timestamp - _last_accel_timestamp[i]) / 1.e6f;
+				_last_sensor_data[i].accelerometer_m_s2[0] = vect_val(0);
+				_last_sensor_data[i].accelerometer_m_s2[1] = vect_val(1);
+				_last_sensor_data[i].accelerometer_m_s2[2] = vect_val(2);
 			}
 
 			_last_accel_timestamp[i] = accel_report.timestamp;
-			_accel.voter.put(i, accel_report.timestamp, sensor_value.data,
+			_accel.voter.put(i, accel_report.timestamp, _last_sensor_data[i].accelerometer_m_s2,
 					 accel_report.error_count, _accel.priority[i]);
 		}
 	}
@@ -1119,9 +1113,9 @@ Sensors::accel_poll(struct sensor_combined_s &raw)
 		_accel.voter.get_best(hrt_absolute_time(), &best_index);
 
 		if (best_index >= 0) {
-			raw.accelerometer_integral_m_s[0] = _last_sensor_data[best_index].accelerometer_integral_m_s[0];
-			raw.accelerometer_integral_m_s[1] = _last_sensor_data[best_index].accelerometer_integral_m_s[1];
-			raw.accelerometer_integral_m_s[2] = _last_sensor_data[best_index].accelerometer_integral_m_s[2];
+			raw.accelerometer_m_s2[0] = _last_sensor_data[best_index].accelerometer_m_s2[0];
+			raw.accelerometer_m_s2[1] = _last_sensor_data[best_index].accelerometer_m_s2[1];
+			raw.accelerometer_m_s2[2] = _last_sensor_data[best_index].accelerometer_m_s2[2];
 			raw.accelerometer_integral_dt = _last_sensor_data[best_index].accelerometer_integral_dt;
 			_accel.last_best_vote = (uint8_t)best_index;
 		}
@@ -1147,42 +1141,36 @@ Sensors::gyro_poll(struct sensor_combined_s &raw)
 			}
 
 			got_update = true;
-			math::Vector<3> sensor_value;
 
 			if (gyro_report.integral_dt != 0) {
 				math::Vector<3> vect_int(gyro_report.x_integral, gyro_report.y_integral, gyro_report.z_integral);
 				vect_int = _board_rotation * vect_int;
 
-				_last_sensor_data[i].gyro_integral_rad[0] = vect_int(0);
-				_last_sensor_data[i].gyro_integral_rad[1] = vect_int(1);
-				_last_sensor_data[i].gyro_integral_rad[2] = vect_int(2);
-
-				_last_sensor_data[i].gyro_integral_dt = (uint32_t)gyro_report.integral_dt;
-
 				float dt = gyro_report.integral_dt / 1.e6f;
-				sensor_value = vect_int / dt;
+				_last_sensor_data[i].gyro_integral_dt = dt;
+
+				_last_sensor_data[i].gyro_rad[0] = vect_int(0) / dt;
+				_last_sensor_data[i].gyro_rad[1] = vect_int(1) / dt;
+				_last_sensor_data[i].gyro_rad[2] = vect_int(2) / dt;
 
 			} else {
-				//this is undesirable: a driver did not set the integral, so we have to construct it ourselves
+				//using the value instead of the integral (the integral is the prefered choice)
 				math::Vector<3> vect_val(gyro_report.x, gyro_report.y, gyro_report.z);
 				vect_val = _board_rotation * vect_val;
 
-				sensor_value = vect_val;
-
 				if (_last_sensor_data[i].timestamp == 0) {
 					_last_sensor_data[i].timestamp = gyro_report.timestamp - 1000;
 				}
 
 				_last_sensor_data[i].gyro_integral_dt =
-					(uint32_t)(gyro_report.timestamp - _last_sensor_data[i].timestamp);
-				float dt = _last_sensor_data[i].gyro_integral_dt / 1.e6f;
-				_last_sensor_data[i].gyro_integral_rad[0] = vect_val(0) * dt;
-				_last_sensor_data[i].gyro_integral_rad[1] = vect_val(1) * dt;
-				_last_sensor_data[i].gyro_integral_rad[2] = vect_val(2) * dt;
+					(gyro_report.timestamp - _last_sensor_data[i].timestamp) / 1.e6f;
+				_last_sensor_data[i].gyro_rad[0] = vect_val(0);
+				_last_sensor_data[i].gyro_rad[1] = vect_val(1);
+				_last_sensor_data[i].gyro_rad[2] = vect_val(2);
 			}
 
 			_last_sensor_data[i].timestamp = gyro_report.timestamp;
-			_gyro.voter.put(i, gyro_report.timestamp, sensor_value.data,
+			_gyro.voter.put(i, gyro_report.timestamp, _last_sensor_data[i].gyro_rad,
 					gyro_report.error_count, _gyro.priority[i]);
 		}
 	}
@@ -1192,9 +1180,9 @@ Sensors::gyro_poll(struct sensor_combined_s &raw)
 		_gyro.voter.get_best(hrt_absolute_time(), &best_index);
 
 		if (best_index >= 0) {
-			raw.gyro_integral_rad[0] = _last_sensor_data[best_index].gyro_integral_rad[0];
-			raw.gyro_integral_rad[1] = _last_sensor_data[best_index].gyro_integral_rad[1];
-			raw.gyro_integral_rad[2] = _last_sensor_data[best_index].gyro_integral_rad[2];
+			raw.gyro_rad[0] = _last_sensor_data[best_index].gyro_rad[0];
+			raw.gyro_rad[1] = _last_sensor_data[best_index].gyro_rad[1];
+			raw.gyro_rad[2] = _last_sensor_data[best_index].gyro_rad[2];
 			raw.gyro_integral_dt = _last_sensor_data[best_index].gyro_integral_dt;
 			raw.timestamp = _last_sensor_data[best_index].timestamp;
 			_gyro.last_best_vote = (uint8_t)best_index;

From 4683e20187a482d0b2203e5bbb8bf6e537d348ea Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Wed, 29 Jun 2016 12:49:08 +0200
Subject: [PATCH 0933/1230] Clean up camera trigger interface code

---
 ROMFS/px4fmu_common/init.d/rcS | 22 +++-------------------
 1 file changed, 3 insertions(+), 19 deletions(-)

diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS
index 834a4f655a..30fbf5856f 100644
--- a/ROMFS/px4fmu_common/init.d/rcS
+++ b/ROMFS/px4fmu_common/init.d/rcS
@@ -308,22 +308,6 @@ then
 		set FMU_MODE gpio_serial
 	fi
 
-	if param greater TRIG_MODE 0
-	then
-		if [ $PWM_OUT == 1234 ]
-		then
-			if ver hwcmp PX4FMU_V4
-			then
-				set FMU_MODE pwm4
-			fi
-
-			if ver hwcmp PX4FMU_V2
-			then
-				set FMU_MODE pwm4
-			fi
-		fi
-	fi
-
 	if [ $HIL == yes ]
 	then
 		set OUTPUT_MODE hil
@@ -556,13 +540,13 @@ then
 		# clear pins 5 and 6
 		if param compare SENS_EN_LL40LS 1
 		then
+			set FMU_MODE pwm4
 			set AUX_MODE pwm4
 		fi
 		if param greater TRIG_MODE 0
 		then
-			# Get FMU driver out of the way
-			set MIXER_AUX none
-			set AUX_MODE none
+			set FMU_MODE pwm4
+			set AUX_MODE pwm4
 			camera_trigger start
 		fi
 	fi

From 78f7f00ae2268ba70bb04ecd34baa44db9148afd Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Wed, 29 Jun 2016 12:49:29 +0200
Subject: [PATCH 0934/1230] Camera trigger: Make interface dependent on
 parameter, not command line

---
 src/drivers/camera_trigger/camera_trigger.cpp | 70 ++++++++-----------
 .../camera_trigger/camera_trigger_params.c    | 18 ++++-
 2 files changed, 44 insertions(+), 44 deletions(-)

diff --git a/src/drivers/camera_trigger/camera_trigger.cpp b/src/drivers/camera_trigger/camera_trigger.cpp
index 8bb18e3466..876943342e 100644
--- a/src/drivers/camera_trigger/camera_trigger.cpp
+++ b/src/drivers/camera_trigger/camera_trigger.cpp
@@ -77,7 +77,7 @@ extern "C" __EXPORT int camera_trigger_main(int argc, char *argv[]);
 typedef enum {
 	CAMERA_INTERFACE_MODE_NONE = 0,
 	CAMERA_INTERFACE_MODE_RELAY,
-	CAMERA_INTERFACE_MODE_PWM
+	CAMERA_INTERFACE_MODE_SEAGULL_MAP2_PWM
 } camera_interface_mode_t;
 
 class CameraTrigger
@@ -86,7 +86,7 @@ public:
 	/**
 	 * Constructor
 	 */
-	CameraTrigger(camera_interface_mode_t camera_interface_mode);
+	CameraTrigger();
 
 	/**
 	 * Destructor, also kills task.
@@ -145,6 +145,7 @@ private:
 	param_t _p_interval;
 	param_t _p_distance;
 	param_t _p_pin;
+	param_t _p_interface;
 
 	camera_interface_mode_t _camera_interface_mode;
 	CameraInterface *_camera_interface;  ///< instance of camera interface
@@ -172,7 +173,7 @@ namespace camera_trigger
 CameraTrigger	*g_camera_trigger;
 }
 
-CameraTrigger::CameraTrigger(camera_interface_mode_t camera_interface_mode) :
+CameraTrigger::CameraTrigger() :
 	_engagecall {},
 	_disengagecall {},
 	_gpio_fd(-1),
@@ -187,7 +188,7 @@ CameraTrigger::CameraTrigger(camera_interface_mode_t camera_interface_mode) :
 	_vcommand_sub(-1),
 	_vlposition_sub(-1),
 	_trigger_pub(nullptr),
-	_camera_interface_mode(camera_interface_mode),
+	_camera_interface_mode(CAMERA_INTERFACE_MODE_RELAY),
 	_camera_interface(nullptr)
 {
 	//Initiate Camera interface basedon camera_interface_mode
@@ -197,19 +198,6 @@ CameraTrigger::CameraTrigger(camera_interface_mode_t camera_interface_mode) :
 		_camera_interface = nullptr;
 	}
 
-	switch (_camera_interface_mode) {
-	case CAMERA_INTERFACE_MODE_RELAY:
-		_camera_interface = new CameraInterfaceRelay;
-		break;
-
-	case CAMERA_INTERFACE_MODE_PWM:
-		_camera_interface = new CameraInterfacePWM;
-		break;
-
-	default:
-		break;
-	}
-
 	memset(&_work, 0, sizeof(_work));
 
 	// Parameters
@@ -217,15 +205,26 @@ CameraTrigger::CameraTrigger(camera_interface_mode_t camera_interface_mode) :
 	_p_distance = param_find("TRIG_DISTANCE");
 	_p_activation_time = param_find("TRIG_ACT_TIME");
 	_p_mode = param_find("TRIG_MODE");
+	_p_interface = param_find("TRIG_INTERFACE");
 
 	param_get(_p_activation_time, &_activation_time);
 	param_get(_p_interval, &_interval);
 	param_get(_p_distance, &_distance);
 	param_get(_p_mode, &_mode);
+	param_get(_p_interface, &_camera_interface_mode);
 
-	struct camera_trigger_s	camera_trigger = {};
+	switch (_camera_interface_mode) {
+	case CAMERA_INTERFACE_MODE_RELAY:
+		_camera_interface = new CameraInterfaceRelay;
+		break;
 
-	_trigger_pub = orb_advertise(ORB_ID(camera_trigger), &camera_trigger);
+	case CAMERA_INTERFACE_MODE_SEAGULL_MAP2_PWM:
+		_camera_interface = new CameraInterfacePWM;
+		break;
+
+	default:
+		break;
+	}
 }
 
 CameraTrigger::~CameraTrigger()
@@ -420,7 +419,9 @@ CameraTrigger::engage(void *arg)
 
 	report.seq = trig->_trigger_seq++;
 
-	orb_publish(ORB_ID(camera_trigger), trig->_trigger_pub, &report);
+	int instance;
+
+	orb_publish_auto(ORB_ID(camera_trigger), &trig->_trigger_pub, &report, &instance, ORB_PRIO_HIGH);
 }
 
 void
@@ -456,39 +457,24 @@ int camera_trigger_main(int argc, char *argv[])
 	if (!strcmp(argv[1], "start")) {
 
 		if (camera_trigger::g_camera_trigger != nullptr) {
-			errx(0, "already running");
+			PX4_WARN("already running");
+			return 0;
 		}
 
-		if (argc >= 3) {
-			if (!strcmp(argv[2], "--relay")) {
-				camera_trigger::g_camera_trigger = new CameraTrigger(CAMERA_INTERFACE_MODE_RELAY);
-				warnx("started with camera interface mode : relay");
-
-			} else if (!strcmp(argv[2], "--pwm")) {
-				camera_trigger::g_camera_trigger = new CameraTrigger(CAMERA_INTERFACE_MODE_PWM);
-				warnx("started with camera interface mode : pwm");
-
-			} else {
-				usage();
-				return 0;
-			}
-
-		} else {
-			camera_trigger::g_camera_trigger = new CameraTrigger(CAMERA_INTERFACE_MODE_RELAY);
-			warnx("started with default camera interface mode : relay");
-		}
+		camera_trigger::g_camera_trigger = new CameraTrigger();
 
 		if (camera_trigger::g_camera_trigger == nullptr) {
-			errx(1, "alloc failed");
+			PX4_WARN("alloc failed");
+			return 1;
 		}
 
 		camera_trigger::g_camera_trigger->start();
-
 		return 0;
 	}
 
 	if (camera_trigger::g_camera_trigger == nullptr) {
-		errx(1, "not running");
+		PX4_WARN("not running");
+		return 1;
 
 	} else if (!strcmp(argv[1], "stop")) {
 		camera_trigger::g_camera_trigger->stop();
diff --git a/src/drivers/camera_trigger/camera_trigger_params.c b/src/drivers/camera_trigger/camera_trigger_params.c
index eb1352245e..d8692eaea6 100644
--- a/src/drivers/camera_trigger/camera_trigger_params.c
+++ b/src/drivers/camera_trigger/camera_trigger_params.c
@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2015 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2015-2016 PX4 Development Team. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -41,6 +41,20 @@
 #include <nuttx/config.h>
 #include <systemlib/param/param.h>
 
+/**
+* Camera trigger Interface
+*
+* Selects the trigger interface
+*
+* @value 1 GPIO
+* @value 2 Seagull MAP2 (PWM)
+*
+* @reboot_required true
+*
+* @group Camera trigger
+*/
+PARAM_DEFINE_INT32(TRIG_INTERFACE, 2);
+
 /**
  * Camera trigger interval
  *
@@ -106,7 +120,7 @@ PARAM_DEFINE_INT32(TRIG_MODE, 0);
  * @reboot_required true
  * @group Camera trigger
  */
-PARAM_DEFINE_INT32(TRIG_PINS, 12);
+PARAM_DEFINE_INT32(TRIG_PINS, 6);
 
 /**
  * Camera trigger distance

From 50b93b161ce4ea24c2e77fe90b2b1129189f268b Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Wed, 29 Jun 2016 12:49:29 +0200
Subject: [PATCH 0935/1230] Camera trigger: Make interface dependent on
 parameter, not command line

---
 src/drivers/camera_trigger/camera_trigger_params.c | 14 ++++++++++++++
 1 file changed, 14 insertions(+)

diff --git a/src/drivers/camera_trigger/camera_trigger_params.c b/src/drivers/camera_trigger/camera_trigger_params.c
index d8692eaea6..7fcc3817f7 100644
--- a/src/drivers/camera_trigger/camera_trigger_params.c
+++ b/src/drivers/camera_trigger/camera_trigger_params.c
@@ -41,6 +41,20 @@
 #include <nuttx/config.h>
 #include <systemlib/param/param.h>
 
+ /**
+ * Camera trigger Interface
+ *
+ * Selects the trigger interface
+ *
+ * @value 1 GPIO
+ * @value 2 Seagull MAP2 (PWM)
+ *
+ * @reboot_required true
+ *
+ * @group Camera trigger
+ */
+PARAM_DEFINE_INT32(TRIG_INTERFACE, 2);
+
 /**
 * Camera trigger Interface
 *

From aa77e8ee23a21a47d85153d7318ade8db188d2e6 Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lorenz@px4.io>
Date: Thu, 30 Jun 2016 10:10:22 +0200
Subject: [PATCH 0936/1230] Cleanup

---
 src/drivers/camera_trigger/camera_trigger.cpp | 76 ++++++++++++-------
 .../camera_trigger/interfaces/src/pwm.cpp     |  3 +-
 2 files changed, 51 insertions(+), 28 deletions(-)

diff --git a/src/drivers/camera_trigger/camera_trigger.cpp b/src/drivers/camera_trigger/camera_trigger.cpp
index 876943342e..521aa796e8 100644
--- a/src/drivers/camera_trigger/camera_trigger.cpp
+++ b/src/drivers/camera_trigger/camera_trigger.cpp
@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2015 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2015-2016 PX4 Development Team. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -118,6 +118,11 @@ public:
 	 */
 	void		info();
 
+	/**
+	 * Trigger one image
+	 */
+	void		test();
+
 private:
 
 	struct hrt_call		_engagecall;
@@ -126,13 +131,13 @@ private:
 
 	int 			_gpio_fd;
 
-	int				_mode;
-	float 			_activation_time;
-	float  			_interval;
-	float  			_distance;
+	int			_mode;
+	float			_activation_time;
+	float			_interval;
+	float 			_distance;
 	uint32_t 		_trigger_seq;
-	bool	 		_trigger_enabled;
-	math::Vector<2>	_last_shoot_position;
+	bool			_trigger_enabled;
+	math::Vector<2>		_last_shoot_position;
 	bool			_valid_position;
 
 	int			_vcommand_sub;
@@ -140,15 +145,15 @@ private:
 
 	orb_advert_t		_trigger_pub;
 
-	param_t _p_mode;
-	param_t _p_activation_time;
-	param_t _p_interval;
-	param_t _p_distance;
-	param_t _p_pin;
-	param_t _p_interface;
+	param_t			_p_mode;
+	param_t			_p_activation_time;
+	param_t			_p_interval;
+	param_t			_p_distance;
+	param_t			_p_pin;
+	param_t			_p_interface;
 
-	camera_interface_mode_t _camera_interface_mode;
-	CameraInterface *_camera_interface;  ///< instance of camera interface
+	camera_interface_mode_t	_camera_interface_mode;
+	CameraInterface		*_camera_interface;  ///< instance of camera interface
 
 	/**
 	 * Vehicle command handler
@@ -225,6 +230,9 @@ CameraTrigger::CameraTrigger() :
 	default:
 		break;
 	}
+
+	struct camera_trigger_s	report = {};
+	_trigger_pub = orb_advertise(ORB_ID(camera_trigger), &report);
 }
 
 CameraTrigger::~CameraTrigger()
@@ -298,6 +306,18 @@ CameraTrigger::stop()
 	}
 }
 
+void
+CameraTrigger::test()
+{
+	struct vehicle_command_s cmd = {};
+	cmd.command = vehicle_command_s::VEHICLE_CMD_DO_DIGICAM_CONTROL;
+	cmd.param5 = 1.0f;
+
+	orb_advert_t pub;
+	pub = orb_advertise(ORB_ID(vehicle_command), &cmd);
+	(void)orb_unadvertise(pub);
+}
+
 void
 CameraTrigger::cycle_trampoline(void *arg)
 {
@@ -419,9 +439,7 @@ CameraTrigger::engage(void *arg)
 
 	report.seq = trig->_trigger_seq++;
 
-	int instance;
-
-	orb_publish_auto(ORB_ID(camera_trigger), &trig->_trigger_pub, &report, &instance, ORB_PRIO_HIGH);
+	orb_publish(ORB_ID(camera_trigger), &(trig->_trigger_pub), &report);
 }
 
 void
@@ -435,23 +453,24 @@ CameraTrigger::disengage(void *arg)
 void
 CameraTrigger::info()
 {
-	warnx("state : %s", _trigger_enabled ? "enabled" : "disabled");
-	warnx("mode : %i", _mode);
-	warnx("interval : %.2f [ms]", (double)_interval);
-	warnx("distance : %.2f [m]", (double)_distance);
-	warnx("activation time : %.2f [ms]", (double)_activation_time);
+	PX4_INFO("state : %s", _trigger_enabled ? "enabled" : "disabled");
+	PX4_INFO("mode : %i", _mode);
+	PX4_INFO("interval : %.2f [ms]", (double)_interval);
+	PX4_INFO("distance : %.2f [m]", (double)_distance);
+	PX4_INFO("activation time : %.2f [ms]", (double)_activation_time);
 	_camera_interface->info();
 }
 
-static void usage()
+static int usage()
 {
-	errx(1, "usage: camera_trigger {start {--relay|--pwm}|stop|info}\n");
+	PX4_ERR("usage: camera_trigger {start|stop|info|test}\n");
+	return 1;
 }
 
 int camera_trigger_main(int argc, char *argv[])
 {
 	if (argc < 2) {
-		usage();
+		return usage();
 	}
 
 	if (!strcmp(argv[1], "start")) {
@@ -488,8 +507,11 @@ int camera_trigger_main(int argc, char *argv[])
 	} else if (!strcmp(argv[1], "disable")) {
 		camera_trigger::g_camera_trigger->control(false);
 
+	} else if (!strcmp(argv[1], "test")) {
+		camera_trigger::g_camera_trigger->test();
+
 	} else {
-		usage();
+		return usage();
 	}
 
 	return 0;
diff --git a/src/drivers/camera_trigger/interfaces/src/pwm.cpp b/src/drivers/camera_trigger/interfaces/src/pwm.cpp
index 502f57713c..ceffb07e86 100644
--- a/src/drivers/camera_trigger/interfaces/src/pwm.cpp
+++ b/src/drivers/camera_trigger/interfaces/src/pwm.cpp
@@ -75,6 +75,7 @@ void CameraInterfacePWM::trigger(bool enable)
 		for (unsigned i = 0; i < sizeof(_pins) / sizeof(_pins[0]); i++) {
 			if (_pins[i] >= 0) {
 				up_pwm_servo_set(_pins[i], math::constrain(PWM_CAMERA_INSTANT_SHOOT, 1000, 2000));
+				PX4_WARN("S");
 			}
 		}
 
@@ -122,5 +123,5 @@ int CameraInterfacePWM::powerOff()
 
 void CameraInterfacePWM::info()
 {
-	warnx("PWM - camera triggering, pins 1-3 : %d,%d,%d", _pins[0], _pins[1], _pins[2]);
+	warnx("PWM - interface, pin config: %d,%d,%d", _pins[0] + 1, _pins[1] + 1, _pins[2] + 1);
 }

From 15880f8d1320fdfd7d55de3720a526751855f27f Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lorenz@px4.io>
Date: Thu, 30 Jun 2016 10:13:22 +0200
Subject: [PATCH 0937/1230] Less verbose

---
 src/drivers/camera_trigger/interfaces/src/pwm.cpp | 1 -
 1 file changed, 1 deletion(-)

diff --git a/src/drivers/camera_trigger/interfaces/src/pwm.cpp b/src/drivers/camera_trigger/interfaces/src/pwm.cpp
index ceffb07e86..5dafcc7775 100644
--- a/src/drivers/camera_trigger/interfaces/src/pwm.cpp
+++ b/src/drivers/camera_trigger/interfaces/src/pwm.cpp
@@ -75,7 +75,6 @@ void CameraInterfacePWM::trigger(bool enable)
 		for (unsigned i = 0; i < sizeof(_pins) / sizeof(_pins[0]); i++) {
 			if (_pins[i] >= 0) {
 				up_pwm_servo_set(_pins[i], math::constrain(PWM_CAMERA_INSTANT_SHOOT, 1000, 2000));
-				PX4_WARN("S");
 			}
 		}
 

From 9974b6f7475b2c390c2de54c379a4784ae112c6b Mon Sep 17 00:00:00 2001
From: Andreas Bircher <bircher@gmx.ch>
Date: Thu, 7 Jul 2016 11:37:45 +0200
Subject: [PATCH 0938/1230] Camera trigger update (#4998)

* updating the camera driver, correct init and keepAlive function

* removing debug output
---
 src/drivers/camera_trigger/camera_trigger.cpp | 72 ++++++++++++++++-
 .../camera_trigger/camera_trigger_params.c    | 21 ++---
 .../interfaces/src/camera_interface.h         |  5 ++
 .../camera_trigger/interfaces/src/pwm.cpp     | 77 +++++++++++++++----
 .../camera_trigger/interfaces/src/pwm.h       |  1 +
 5 files changed, 143 insertions(+), 33 deletions(-)

diff --git a/src/drivers/camera_trigger/camera_trigger.cpp b/src/drivers/camera_trigger/camera_trigger.cpp
index 521aa796e8..ecd7282a28 100644
--- a/src/drivers/camera_trigger/camera_trigger.cpp
+++ b/src/drivers/camera_trigger/camera_trigger.cpp
@@ -103,6 +103,11 @@ public:
 	 */
 	void		shootOnce();
 
+	/**
+	 * Toggle keep camera alive functionality
+	 */
+	void		keepAlive(bool on);
+
 	/**
 	 * Start the task.
 	 */
@@ -127,6 +132,9 @@ private:
 
 	struct hrt_call		_engagecall;
 	struct hrt_call		_disengagecall;
+	struct hrt_call		_keepalivecall_up;
+	struct hrt_call		_keepalivecall_down;
+
 	static struct work_s	_work;
 
 	int 			_gpio_fd;
@@ -167,6 +175,14 @@ private:
 	 * Resets trigger
 	 */
 	static void	disengage(void *arg);
+	/**
+	 * Fires trigger
+	 */
+	static void	keep_alive_up(void *arg);
+	/**
+	 * Resets trigger
+	 */
+	static void	keep_alive_down(void *arg);
 
 };
 
@@ -232,6 +248,7 @@ CameraTrigger::CameraTrigger() :
 	}
 
 	struct camera_trigger_s	report = {};
+
 	_trigger_pub = orb_advertise(ORB_ID(camera_trigger), &report);
 }
 
@@ -269,6 +286,26 @@ CameraTrigger::control(bool on)
 	_trigger_enabled = on;
 }
 
+void
+CameraTrigger::keepAlive(bool on)
+{
+	if (on) {
+		// schedule keep-alive up and down calls
+		hrt_call_every(&_keepalivecall_up, 0, (60000 * 1000),
+			       (hrt_callout)&CameraTrigger::keep_alive_up, this);
+
+		// schedule keep-alive up and down calls
+		hrt_call_every(&_keepalivecall_down, 0 + (30000 * 1000), (60000 * 1000),
+			       (hrt_callout)&CameraTrigger::keep_alive_down, this);
+
+	} else {
+		// cancel all calls
+		hrt_cancel(&_keepalivecall_up);
+		hrt_cancel(&_keepalivecall_down);
+	}
+
+}
+
 void
 CameraTrigger::shootOnce()
 {
@@ -289,6 +326,14 @@ CameraTrigger::start()
 		control(true);
 	}
 
+	// Prevent camera from sleeping, if triggering is enabled
+	if (_mode > 0) {
+		keepAlive(true);
+
+	} else {
+		keepAlive(false);
+	}
+
 	// start to monitor at high rate for trigger enable command
 	work_queue(LPWORK, &_work, (worker_t)&CameraTrigger::cycle_trampoline, this, USEC2TICK(1));
 
@@ -300,6 +345,8 @@ CameraTrigger::stop()
 	work_cancel(LPWORK, &_work);
 	hrt_cancel(&_engagecall);
 	hrt_cancel(&_disengagecall);
+	hrt_cancel(&_keepalivecall_up);
+	hrt_cancel(&_keepalivecall_down);
 
 	if (camera_trigger::g_camera_trigger != nullptr) {
 		delete(camera_trigger::g_camera_trigger);
@@ -390,7 +437,14 @@ CameraTrigger::cycle_trampoline(void *arg)
 
 				if (cmd.command == vehicle_command_s::VEHICLE_CMD_DO_SET_CAM_TRIGG_DIST) {
 
-					// Set trigger to false if the set distance is not positive
+					// Set trigger to disabled if the set distance is not positive
+					if (cmd.param1 > 0.0f && !trig->_trigger_enabled) {
+						trig->_camera_interface->powerOn();
+
+					} else if (cmd.param1 <= 0.0f && trig->_trigger_enabled) {
+						trig->_camera_interface->powerOff();
+					}
+
 					trig->_trigger_enabled = cmd.param1 > 0.0f;
 					trig->_distance = cmd.param1;
 				}
@@ -450,6 +504,22 @@ CameraTrigger::disengage(void *arg)
 	trig->_camera_interface->trigger(false);
 }
 
+void
+CameraTrigger::keep_alive_up(void *arg)
+{
+	CameraTrigger *trig = reinterpret_cast<CameraTrigger *>(arg);
+
+	trig->_camera_interface->keep_alive(true);
+}
+
+void
+CameraTrigger::keep_alive_down(void *arg)
+{
+	CameraTrigger *trig = reinterpret_cast<CameraTrigger *>(arg);
+
+	trig->_camera_interface->keep_alive(false);
+}
+
 void
 CameraTrigger::info()
 {
diff --git a/src/drivers/camera_trigger/camera_trigger_params.c b/src/drivers/camera_trigger/camera_trigger_params.c
index 7fcc3817f7..09b3cbabc5 100644
--- a/src/drivers/camera_trigger/camera_trigger_params.c
+++ b/src/drivers/camera_trigger/camera_trigger_params.c
@@ -36,25 +36,12 @@
  * Camera trigger parameters
  *
  * @author Mohammed Kabir <mhkabir98@gmail.com>
+ * @author Andreas Bircher <andreas@wingtra.com>
  */
 
 #include <nuttx/config.h>
 #include <systemlib/param/param.h>
 
- /**
- * Camera trigger Interface
- *
- * Selects the trigger interface
- *
- * @value 1 GPIO
- * @value 2 Seagull MAP2 (PWM)
- *
- * @reboot_required true
- *
- * @group Camera trigger
- */
-PARAM_DEFINE_INT32(TRIG_INTERFACE, 2);
-
 /**
 * Camera trigger Interface
 *
@@ -102,7 +89,7 @@ PARAM_DEFINE_INT32(TRIG_POLARITY, 0);
  *
  * @unit ms
  * @min 0.1
- * @max 3
+ * @max 3000
  * @decimal 1
  * @group Camera trigger
  */
@@ -126,7 +113,9 @@ PARAM_DEFINE_INT32(TRIG_MODE, 0);
 /**
  * Camera trigger pin
  *
- * Selects which pin is used, ranges from 1 to 6 (AUX1-AUX6)
+ * Selects which pin is used, ranges from 1 to 6 (AUX1-AUX6 on px4fmu-v2 and the rail
+ * pins on px4fmu-v4). The PWM interface takes two pins per camera, while relay
+ * triggers on every pin individually. Example: Value 34 would trigger on pins 3 and 4.
  *
  * @min 1
  * @max 123456
diff --git a/src/drivers/camera_trigger/interfaces/src/camera_interface.h b/src/drivers/camera_trigger/interfaces/src/camera_interface.h
index e6b848884c..6ce9e5011d 100644
--- a/src/drivers/camera_trigger/interfaces/src/camera_interface.h
+++ b/src/drivers/camera_trigger/interfaces/src/camera_interface.h
@@ -24,6 +24,11 @@ public:
 	 */
 	virtual void trigger(bool enable) {};
 
+	/**
+	 * prevent the camera from sleeping
+	 * @param keep alive signal:
+	 */
+	virtual void keep_alive(bool signal_on) {};
 
 	/**
 	 * Display info.
diff --git a/src/drivers/camera_trigger/interfaces/src/pwm.cpp b/src/drivers/camera_trigger/interfaces/src/pwm.cpp
index 5dafcc7775..1ce5d91e94 100644
--- a/src/drivers/camera_trigger/interfaces/src/pwm.cpp
+++ b/src/drivers/camera_trigger/interfaces/src/pwm.cpp
@@ -7,12 +7,14 @@
 
 // PWM levels of the interface to seagull MAP converter to
 // Multiport (http://www.seagulluav.com/manuals/Seagull_MAP2-Manual.pdf)
-#define PWM_CAMERA_DISARMED			90 // TODO(birchera): check here value
+#define PWM_CAMERA_DISARMED			900
 #define PWM_CAMERA_ON				1100
 #define PWM_CAMERA_AUTOFOCUS_SHOOT	1300
 #define PWM_CAMERA_NEUTRAL			1500
 #define PWM_CAMERA_INSTANT_SHOOT	1700
 #define PWM_CAMERA_OFF				1900
+#define PWM_2_CAMERA_KEEP_ALIVE		1700
+#define PWM_2_CAMERA_ON_OFF			1900
 
 CameraInterfacePWM::CameraInterfacePWM():
 	CameraInterface(),
@@ -48,13 +50,18 @@ CameraInterfacePWM::CameraInterfacePWM():
 
 CameraInterfacePWM::~CameraInterfacePWM()
 {
+	// Deinitialise pwm channels
+	// Can currently not be used, because up_pwm_servo_deinit() will deinitialise all pwm channels
+	// up_pwm_servo_deinit();
 }
 
 void CameraInterfacePWM::setup()
 {
-	for (unsigned i = 0; i < sizeof(_pins) / sizeof(_pins[0]); i++) {
-		if (_pins[i] >= 0) {
-			// TODO(birchera): use return value to make sure everything is fine
+	for (unsigned i = 0; i < sizeof(_pins) / sizeof(_pins[0]); i = i + 2) {
+		if (_pins[i] >= 0 && _pins[i + 1] >= 0) {
+			uint8_t pin_bitmask = (1 << _pins[i + 1]) | (1 << _pins[i]);
+			up_pwm_servo_init(pin_bitmask);
+			up_pwm_servo_set(_pins[i + 1], math::constrain(PWM_CAMERA_DISARMED, PWM_CAMERA_DISARMED, 2000));
 			up_pwm_servo_set(_pins[i], math::constrain(PWM_CAMERA_DISARMED, PWM_CAMERA_DISARMED, 2000));
 		}
 	}
@@ -65,23 +72,52 @@ void CameraInterfacePWM::trigger(bool enable)
 	// This only starts working upon prearming
 
 	if (!_camera_is_on) {
+		// (TODO: powerOn does not work yet)
 		// Turn camera on and give time to start up
-		powerOn();
+		// powerOn();
 		return;
 	}
 
 	if (enable) {
 		// Set all valid pins to shoot level
-		for (unsigned i = 0; i < sizeof(_pins) / sizeof(_pins[0]); i++) {
-			if (_pins[i] >= 0) {
-				up_pwm_servo_set(_pins[i], math::constrain(PWM_CAMERA_INSTANT_SHOOT, 1000, 2000));
+		for (unsigned i = 0; i < sizeof(_pins) / sizeof(_pins[0]); i = i + 2) {
+			if (_pins[i] >= 0 && _pins[i + 1] >= 0) {
+				up_pwm_servo_set(_pins[i + 1], math::constrain(PWM_CAMERA_INSTANT_SHOOT, 1000, 2000));
 			}
 		}
 
 	} else {
 		// Set all valid pins back to neutral level
-		for (unsigned i = 0; i < sizeof(_pins) / sizeof(_pins[0]); i++) {
-			if (_pins[i] >= 0) {
+		for (unsigned i = 0; i < sizeof(_pins) / sizeof(_pins[0]); i = i + 2) {
+			if (_pins[i] >= 0 && _pins[i + 1] >= 0) {
+				up_pwm_servo_set(_pins[i + 1], math::constrain(PWM_CAMERA_NEUTRAL, 1000, 2000));
+			}
+		}
+	}
+}
+
+void CameraInterfacePWM::keep_alive(bool signal_on)
+{
+	// This should alternate between signal_on and !signal_on to keep the camera alive
+
+	if (!_camera_is_on) {
+		// (TODO: powerOn does not work yet)
+		// Turn camera on and give time to start up
+		powerOn();
+	}
+
+	if (signal_on) {
+		// Set channel 2 pin to keep_alive signal
+		for (unsigned i = 0; i < sizeof(_pins) / sizeof(_pins[0]); i = i + 2) {
+			if (_pins[i] >= 0 && _pins[i + 1] >= 0) {
+				up_pwm_servo_set(_pins[i], math::constrain(PWM_2_CAMERA_KEEP_ALIVE, 1000, 2000));
+			}
+		}
+
+	} else {
+		// Set channel 2 pin to neutral signal
+		for (unsigned i = 0; i < sizeof(_pins) / sizeof(_pins[0]); i = i + 2) {
+			if (_pins[i] >= 0 && _pins[i + 1] >= 0) {
 				up_pwm_servo_set(_pins[i], math::constrain(PWM_CAMERA_NEUTRAL, 1000, 2000));
 			}
 		}
@@ -93,9 +129,17 @@ int CameraInterfacePWM::powerOn()
 	// This only starts working upon prearming
 
 	// Set all valid pins to turn on level
-	for (unsigned i = 0; i < sizeof(_pins) / sizeof(_pins[0]); i++) {
-		if (_pins[i] >= 0) {
-			up_pwm_servo_set(_pins[i], math::constrain(PWM_CAMERA_ON, 1000, 2000));
+	// for (unsigned i = 0; i < sizeof(_pins) / sizeof(_pins[0]); i = i + 2) {
+	// 	if (_pins[i] >= 0 && _pins[i + 1] >= 0) {
+	// 		up_pwm_servo_set(_pins[i + 1], math::constrain(PWM_CAMERA_ON, 1000, 2000));
+	// 		up_pwm_servo_set(_pins[i], math::constrain(PWM_2_CAMERA_ON_OFF, 1000, 2000));
+	// 	}
+	// }
+
+	// For now, set channel one on neutral upon startup.
+	for (unsigned i = 0; i < sizeof(_pins) / sizeof(_pins[0]); i = i + 2) {
+		if (_pins[i] >= 0 && _pins[i + 1] >= 0) {
+			up_pwm_servo_set(_pins[i + 1], math::constrain(PWM_CAMERA_NEUTRAL, 1000, 2000));
 		}
 	}
 
@@ -109,9 +153,10 @@ int CameraInterfacePWM::powerOff()
 	// This only starts working upon prearming
 
 	// Set all valid pins to turn off level
-	for (unsigned i = 0; i < sizeof(_pins) / sizeof(_pins[0]); i++) {
-		if (_pins[i] >= 0) {
-			up_pwm_servo_set(_pins[i], math::constrain(PWM_CAMERA_OFF, 1000, 2000));
+	for (unsigned i = 0; i < sizeof(_pins) / sizeof(_pins[0]); i = i + 2) {
+		if (_pins[i] >= 0 && _pins[i + 1] >= 0) {
+			up_pwm_servo_set(_pins[i + 1], math::constrain(PWM_CAMERA_OFF, 1000, 2000));
+			up_pwm_servo_set(_pins[i], math::constrain(PWM_2_CAMERA_ON_OFF, 1000, 2000));
 		}
 	}
 
diff --git a/src/drivers/camera_trigger/interfaces/src/pwm.h b/src/drivers/camera_trigger/interfaces/src/pwm.h
index 7310be8b9e..33392d6047 100644
--- a/src/drivers/camera_trigger/interfaces/src/pwm.h
+++ b/src/drivers/camera_trigger/interfaces/src/pwm.h
@@ -18,6 +18,7 @@ public:
 	virtual ~CameraInterfacePWM();
 
 	void trigger(bool enable);
+	void keep_alive(bool signal_on);
 
 	int powerOn();
 	int powerOff();

From 4252511b8eb54fc44bc4c09d53f8f61fbe2c3d0c Mon Sep 17 00:00:00 2001
From: Daniel Agar <daniel@agar.ca>
Date: Sun, 3 Jul 2016 19:34:45 -0400
Subject: [PATCH 0939/1230] add bitmask param metadata

---
 Tools/px4params/srcparser.py   | 48 ++++++++++++++++++++++++++++--
 Tools/px4params/xmlout.py      |  8 +++++
 src/modules/ekf2/ekf2_params.c | 53 ++++++++++++++++++++++------------
 3 files changed, 89 insertions(+), 20 deletions(-)

diff --git a/Tools/px4params/srcparser.py b/Tools/px4params/srcparser.py
index b4ad401a9d..183c0cb3c9 100644
--- a/Tools/px4params/srcparser.py
+++ b/Tools/px4params/srcparser.py
@@ -1,5 +1,7 @@
 import sys
 import re
+import math
+
 global default_var
 default_var = {}
 
@@ -39,7 +41,7 @@ class Parameter(object):
 
     # Define sorting order of the fields
     priority = {
-		"board": 9,
+        "board": 9,
         "short_desc": 8,
         "long_desc": 7,
         "min": 5,
@@ -52,6 +54,7 @@ class Parameter(object):
     def __init__(self, name, type, default = ""):
         self.fields = {}
         self.values = {}
+        self.bitmask = {}
         self.name = name
         self.type = type
         self.default = default
@@ -77,6 +80,12 @@ class Parameter(object):
         """
         self.values[code] = value
 
+    def SetBitmaskBit(self, index, bit):
+        """
+        Set named enum value
+        """
+        self.bitmask[index] = bit
+
     def GetFieldCodes(self):
         """
         Return list of existing field codes in convenient order
@@ -115,6 +124,24 @@ class Parameter(object):
                 return ""
         return fv
 
+    def GetBitmaskList(self):
+        """
+        Return list of existing bitmask codes in convenient order
+        """
+        keys = self.bitmask.keys()
+        keys.sort(key=float)
+        return keys
+
+    def GetBitmaskBit(self, index):
+        """
+        Return value of the given bitmask code or None if not found.
+        """
+        fv =  self.bitmask.get(index)
+        if not fv:
+                # required because python 3 sorted does not accept None
+                return ""
+        return fv
+
 class SourceParser(object):
     """
     Parses provided data and stores all found parameters internally.
@@ -133,7 +160,7 @@ class SourceParser(object):
     re_remove_dots = re.compile(r'\.+$')
     re_remove_carriage_return = re.compile('\n+')
 
-    valid_tags = set(["group", "board", "min", "max", "unit", "decimal", "increment", "reboot_required", "value", "boolean"])
+    valid_tags = set(["group", "board", "min", "max", "unit", "decimal", "increment", "reboot_required", "value", "boolean", "bit"])
 
     # Order of parameter groups
     priority = {
@@ -164,6 +191,7 @@ class SourceParser(object):
                 long_desc = None
                 tags = {}
                 def_values = {}
+                def_bitmask = {}
             elif state is not None and state != "comment-processed":
                 m = self.re_comment_end.search(line)
                 if m:
@@ -187,6 +215,10 @@ class SourceParser(object):
                                 # Take the meta info string and split the code and description
                                 metainfo = desc.split(" ",  1)
                                 def_values[metainfo[0]] = metainfo[1]
+                            elif (tag == "bit"):
+                                # Take the meta info string and split the code and description
+                                metainfo = desc.split(" ",  1)
+                                def_bitmask[metainfo[0]] = metainfo[1]
                             else:
                                 tags[tag] = desc
                             current_tag = tag
@@ -262,6 +294,8 @@ class SourceParser(object):
                                 param.SetField(tag, tags[tag])
                         for def_value in def_values:
                             param.SetEnumValue(def_value, def_values[def_value])
+                        for def_bit in def_bitmask:
+                            param.SetBitmaskBit(def_bit, def_bitmask[def_bit])
                     # Store the parameter
                     if group not in self.param_groups:
                         self.param_groups[group] = ParameterGroup(group)
@@ -324,6 +358,16 @@ class SourceParser(object):
                         if param.GetEnumValue(code) == "":
                             sys.stderr.write("Description for enum value is empty: {0} {1}\n".format(name, code))
                             return False
+                for index in param.GetBitmaskList():
+                        if not self.IsNumber(index):
+                            sys.stderr.write("bit value not number: {0} {1}\n".format(name, index))
+                            return False
+                        if not int(min) <= math.pow(2, int(index)) <= int(max):
+                            sys.stderr.write("Bitmask bit must be between {0} and {1}: {2} {3}\n".format(min, max, name, math.pow(2, int(index))))
+                            return False
+                        if param.GetBitmaskBit(index) == "":
+                            sys.stderr.write("Description for bitmask bit is empty: {0} {1}\n".format(name, index))
+                            return False
         return True
 
     def GetParamGroups(self):
diff --git a/Tools/px4params/xmlout.py b/Tools/px4params/xmlout.py
index 0c041a6fc8..fbee44f405 100644
--- a/Tools/px4params/xmlout.py
+++ b/Tools/px4params/xmlout.py
@@ -63,6 +63,14 @@ class XMLOutput():
                         xml_value = ET.SubElement(xml_values, "value")
                         xml_value.attrib["code"] = code;
                         xml_value.text = param.GetEnumValue(code)
+
+                if len(param.GetBitmaskList()) > 0:
+                    xml_values = ET.SubElement(xml_param, "bitmask")
+                    for index in param.GetBitmaskList():
+                        xml_value = ET.SubElement(xml_values, "bit")
+                        xml_value.attrib["index"] = index;
+                        xml_value.text = param.GetBitmaskBit(index)
+
         indent(xml_parameters)
         self.xml_document = ET.ElementTree(xml_parameters)
 
diff --git a/src/modules/ekf2/ekf2_params.c b/src/modules/ekf2/ekf2_params.c
index 6a9eab4770..25c48086ea 100644
--- a/src/modules/ekf2/ekf2_params.c
+++ b/src/modules/ekf2/ekf2_params.c
@@ -121,21 +121,30 @@ PARAM_DEFINE_FLOAT(EKF2_EV_DELAY, 175);
 
 /**
  * Integer bitmask controlling GPS checks.
- * 
+ *
  * Set bits to 1 to enable checks. Checks enabled by the following bit positions
  * 0 : Minimum required sat count set by EKF2_REQ_NSATS
  * 1 : Minimum required GDoP set by EKF2_REQ_GDOP
  * 2 : Maximum allowed horizontal position error set by EKF2_REQ_EPH
  * 3 : Maximum allowed vertical position error set by EKF2_REQ_EPV
  * 4 : Maximum allowed speed error set by EKF2_REQ_SACC
- * 5 : Maximum allowed horizontal position rate set by EKF2_REQ_HDRIFT. This check can only be used if the vehciel is stationary during alignment.
- * 6 : Maximum allowed vertical position rate set by EKF2_REQ_VDRIFT. This check can only be used if the vehciel is stationary during alignment.
- * 7 : Maximum allowed horizontal speed set by EKF2_REQ_HDRIFT. This check can only be used if the vehciel is stationary during alignment.
+ * 5 : Maximum allowed horizontal position rate set by EKF2_REQ_HDRIFT. This check can only be used if the vehicle is stationary during alignment.
+ * 6 : Maximum allowed vertical position rate set by EKF2_REQ_VDRIFT. This check can only be used if the vehicle is stationary during alignment.
+ * 7 : Maximum allowed horizontal speed set by EKF2_REQ_HDRIFT. This check can only be used if the vehicle is stationary during alignment.
  * 8 : Maximum allowed vertical velocity discrepancy set by EKF2_REQ_VDRIFT
  *
  * @group EKF2
  * @min 0
  * @max 511
+ * @bit 0 Min sat count (EKF2_REQ_NSATS)
+ * @bit 1 Min GDoP (EKF2_REQ_GDOP)
+ * @bit 2 Max horizontal position error (EKF2_REQ_EPH)
+ * @bit 3 Max vertical position error (EKF2_REQ_EPV)
+ * @bit 4 Max speed error (EKF2_REQ_SACC)
+ * @bit 5 Max horizontal position rate (EKF2_REQ_HDRIFT)
+ * @bit 6 Max vertical position rate (EKF2_REQ_VDRIFT)
+ * @bit 7 Max horizontal speed (EKF2_REQ_HDRIFT)
+ * @bit 8 Max vertical velocity discrepancy (EKF2_REQ_VDRIFT)
  */
 PARAM_DEFINE_INT32(EKF2_GPS_CHECK, 21);
 
@@ -365,7 +374,7 @@ PARAM_DEFINE_FLOAT(EKF2_MAG_NOISE, 5.0e-2f);
  * @unit m/s
  * @decimal 1
  */
- PARAM_DEFINE_FLOAT(EKF2_EAS_NOISE, 1.4f);
+PARAM_DEFINE_FLOAT(EKF2_EAS_NOISE, 1.4f);
 
 /**
  * Magnetic declination
@@ -402,11 +411,14 @@ PARAM_DEFINE_FLOAT(EKF2_MAG_GATE, 3.0f);
  * Set bits in the following positions to enable functions.
  * 0 : Set to true to use the declination from the geo_lookup library when the GPS position becomes available, set to false to always use the EKF2_MAG_DECL value.
  * 1 : Set to true to save the EKF2_MAG_DECL parameter to the value returned by the EKF when the vehicle disarms.
- * 2 : Set to true to always use the declination as an observaton when 3-axis magnetometer fusion is being used.
+ * 2 : Set to true to always use the declination as an observation when 3-axis magnetometer fusion is being used.
  *
  * @group EKF2
  * @min 0
  * @max 7
+ * @bit 0 use geo_lookup declination
+ * @bit 1 save EKF2_MAG_DECL on disarm
+ * @bit 2 use declination as an observation
  */
 PARAM_DEFINE_INT32(EKF2_DECL_TYPE, 7);
 
@@ -415,7 +427,7 @@ PARAM_DEFINE_INT32(EKF2_DECL_TYPE, 7);
  *
  * Integer controlling the type of magnetometer fusion used - magnetic heading or 3-axis magnetometer.
  * If set to automatic: heading fusion on-ground and 3-axis fusion in-flight
- * 
+ *
  * @group EKF2
  * @value 0 Automatic
  * @value 1 Magnetic heading
@@ -484,10 +496,15 @@ PARAM_DEFINE_INT32(EKF2_REC_RPL, 0);
  * 2 : Set to true to inhibit IMU bias estimation
  * 3 : Set to true to enable vision position fusion
  * 4 : Set to true to enable vision yaw fusion
- * *
+ *
  * @group EKF2
  * @min 0
  * @max 28
+ * @bit 0 use GPS
+ * @bit 1 use optical flow
+ * @bit 2 inhibit IMU bias estimation
+ * @bit 3 vision position fusion
+ * @bit 4 vision yaw fusion
  */
 PARAM_DEFINE_INT32(EKF2_AID_MASK, 1);
 
@@ -782,16 +799,16 @@ PARAM_DEFINE_FLOAT(EKF2_EV_POS_Y, 0.0f);
  */
 PARAM_DEFINE_FLOAT(EKF2_EV_POS_Z, 0.0f);
 
- /**
- * Airspeed fusion threshold. A value of zero will deactivate airspeed fusion. Any other positive
- * value will determine the minimum airspeed which will still be fused.
- *
- * @group EKF2
- * @min 0.0
- * @unit m/s
- * @decimal 1
- */
- PARAM_DEFINE_FLOAT(EKF2_ARSP_THR, 0.0f);
+/**
+* Airspeed fusion threshold. A value of zero will deactivate airspeed fusion. Any other positive
+* value will determine the minimum airspeed which will still be fused.
+*
+* @group EKF2
+* @min 0.0
+* @unit m/s
+* @decimal 1
+*/
+PARAM_DEFINE_FLOAT(EKF2_ARSP_THR, 0.0f);
 
 /**
 

From ffcefd9047488a4ab27f4c69994266fdd3059c2f Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Beat=20K=C3=BCng?= <beat-kueng@gmx.net>
Date: Fri, 10 Jun 2016 12:36:41 +0200
Subject: [PATCH 0940/1230] orb: read & apply publisher rules from file
 (currently disabled via #ifdef)

If enabled, orb reads a rules file (./rootfs/orb_publisher.rules) on
startup. This can contain rules about which module is allowed to publish
which topic. It is completely transparent, so a publisher does not know
if he's not allowed to publish, and publications will look as if they
succeeded.

To test, add
#define ORB_USE_PUBLISHER_RULES
to uORBManager.hpp
---
 src/modules/uORB/uORBManager.cpp | 196 +++++++++++++++++++++++++++++++
 src/modules/uORB/uORBManager.hpp |  40 +++++++
 2 files changed, 236 insertions(+)

diff --git a/src/modules/uORB/uORBManager.cpp b/src/modules/uORB/uORBManager.cpp
index ce9ae40f8c..05f88dd6f2 100644
--- a/src/modules/uORB/uORBManager.cpp
+++ b/src/modules/uORB/uORBManager.cpp
@@ -38,6 +38,7 @@
 #include <errno.h>
 #include <px4_config.h>
 #include <px4_posix.h>
+#include <px4_tasks.h>
 #include "uORBUtils.hpp"
 #include "uORBManager.hpp"
 #include "px4_config.h"
@@ -63,6 +64,21 @@ bool uORB::Manager::initialize()
 uORB::Manager::Manager()
 	: _comm_channel(nullptr)
 {
+
+#ifdef ORB_USE_PUBLISHER_RULES
+	const char *file_name = "./rootfs/orb_publisher.rules";
+	int ret = readPublisherRulesFromFile(file_name, _publisher_rule);
+
+	if (ret == PX4_OK) {
+		_has_publisher_rules = true;
+		PX4_INFO("Using orb rules from %s", file_name);
+
+	} else {
+		PX4_ERR("Failed to read publisher rules file %s (%s)", file_name, strerror(-ret));
+	}
+
+#endif /* ORB_USE_PUBLISHER_RULES */
+
 }
 
 int uORB::Manager::orb_exists(const struct orb_metadata *meta, int instance)
@@ -96,6 +112,30 @@ orb_advert_t uORB::Manager::orb_advertise(const struct orb_metadata *meta, const
 orb_advert_t uORB::Manager::orb_advertise_multi(const struct orb_metadata *meta, const void *data, int *instance,
 		int priority, unsigned int queue_size)
 {
+#ifdef ORB_USE_PUBLISHER_RULES
+
+	// check publisher rule
+	if (_has_publisher_rules) {
+		const char *prog_name = px4_get_taskname();
+
+		if (strcmp(_publisher_rule.module_name, prog_name) == 0) {
+			if (_publisher_rule.ignore_other_topics) {
+				if (!findTopic(_publisher_rule, meta->o_name)) {
+					PX4_DEBUG("not allowing %s to publish topic %s", prog_name, meta->o_name);
+					return (orb_advert_t)_Instance;
+				}
+			}
+
+		} else {
+			if (findTopic(_publisher_rule, meta->o_name)) {
+				PX4_DEBUG("not allowing %s to publish topic %s", prog_name, meta->o_name);
+				return (orb_advert_t)_Instance;
+			}
+		}
+	}
+
+#endif /* ORB_USE_PUBLISHER_RULES */
+
 	int result, fd;
 	orb_advert_t advertiser;
 
@@ -140,6 +180,14 @@ orb_advert_t uORB::Manager::orb_advertise_multi(const struct orb_metadata *meta,
 
 int uORB::Manager::orb_unadvertise(orb_advert_t handle)
 {
+#ifdef ORB_USE_PUBLISHER_RULES
+
+	if (handle == _Instance) {
+		return PX4_OK; //pretend success
+	}
+
+#endif /* ORB_USE_PUBLISHER_RULES */
+
 	return uORB::DeviceNode::unadvertise(handle);
 }
 
@@ -161,6 +209,14 @@ int uORB::Manager::orb_unsubscribe(int fd)
 
 int uORB::Manager::orb_publish(const struct orb_metadata *meta, orb_advert_t handle, const void *data)
 {
+#ifdef ORB_USE_PUBLISHER_RULES
+
+	if (handle == _Instance) {
+		return PX4_OK; //pretend success
+	}
+
+#endif /* ORB_USE_PUBLISHER_RULES */
+
 	return uORB::DeviceNode::publish(meta, handle, data);
 }
 
@@ -446,3 +502,143 @@ bool uORB::Manager::is_remote_subscriber_present(const char *messageName)
 	return (_remote_subscriber_topics.find(messageName) != _remote_subscriber_topics.end());
 #endif
 }
+
+
+#ifdef ORB_USE_PUBLISHER_RULES
+
+bool uORB::Manager::startsWith(const char *pre, const char *str)
+{
+	size_t lenpre = strlen(pre),
+	       lenstr = strlen(str);
+	return lenstr < lenpre ? false : strncmp(pre, str, lenpre) == 0;
+}
+
+bool uORB::Manager::findTopic(const PublisherRule &rule, const char *topic_name)
+{
+	const char **topics_ptr = rule.topics;
+
+	while (*topics_ptr) {
+		if (strcmp(*topics_ptr, topic_name) == 0) {
+			return true;
+		}
+
+		++topics_ptr;
+	}
+
+	return false;
+}
+
+void uORB::Manager::strTrim(const char **str)
+{
+	while (**str == ' ' || **str == '\t') { ++(*str); }
+}
+
+int uORB::Manager::readPublisherRulesFromFile(const char *file_name, PublisherRule &rule)
+{
+	FILE *fp;
+	static const int line_len = 1024;
+	int ret = PX4_OK;
+	char *line = new char[line_len];
+
+	if (!line) {
+		return -ENOMEM;
+	}
+
+	fp = fopen(file_name, "r");
+
+	if (fp == NULL) {
+		delete[](line);
+		return -errno;
+	}
+
+	const char *restrict_topics_str = "restrict_topics:";
+	const char *module_str = "module:";
+	const char *ignore_others = "ignore_others:";
+
+	rule.ignore_other_topics = false;
+	rule.module_name = nullptr;
+	rule.topics = nullptr;
+
+	while (fgets(line, line_len, fp) && ret == PX4_OK) {
+
+		if (strlen(line) < 2 || line[0] == '#') {
+			continue;
+		}
+
+		if (startsWith(restrict_topics_str, line)) {
+			//read topics list
+			char *start = line + strlen(restrict_topics_str);
+			strTrim((const char **)&start);
+			char *topics = strdup(start);
+			int topic_len = 0, num_topics = 0;
+
+			for (int i = 0; topics[i]; ++i) {
+				if (topics[i] == ',' || topics[i] == '\n') {
+					if (topic_len > 0) {
+						topics[i] = 0;
+						++num_topics;
+					}
+
+					topic_len = 0;
+
+				} else {
+					++topic_len;
+				}
+			}
+
+			if (num_topics > 0) {
+				rule.topics = new const char *[num_topics + 1];
+				int topic = 0;
+				strTrim((const char **)&topics);
+				rule.topics[topic++] = topics;
+
+				while (topic < num_topics) {
+					if (*topics == 0) {
+						++topics;
+						strTrim((const char **)&topics);
+						rule.topics[topic++] = topics;
+
+					} else {
+						++topics;
+					}
+				}
+
+				rule.topics[num_topics] = nullptr;
+			}
+
+		} else if (startsWith(module_str, line)) {
+			//read module name
+			char *start = line + strlen(module_str);
+			strTrim((const char **)&start);
+			int len = strlen(start);
+
+			if (len > 0 && start[len - 1] == '\n') {
+				start[len - 1] = 0;
+			}
+
+			rule.module_name = strdup(start);
+
+		} else if (startsWith(ignore_others, line)) {
+			const char *start = line + strlen(ignore_others);
+			strTrim(&start);
+
+			if (startsWith("true", start)) {
+				rule.ignore_other_topics = true;
+			}
+
+		} else {
+			PX4_ERR("orb rules file: wrong format: %s", line);
+			ret = -EINVAL;
+		}
+	}
+
+	if (ret == PX4_OK && (!rule.module_name || !rule.topics)) {
+		PX4_ERR("Wrong format in orb publisher rules file");
+		ret = -EINVAL;
+	}
+
+	delete[](line);
+	fclose(fp);
+	return ret;
+}
+#endif /* ORB_USE_PUBLISHER_RULES */
diff --git a/src/modules/uORB/uORBManager.hpp b/src/modules/uORB/uORBManager.hpp
index e43e57e887..d0e82caa76 100644
--- a/src/modules/uORB/uORBManager.hpp
+++ b/src/modules/uORB/uORBManager.hpp
@@ -446,6 +446,46 @@ private: //class methods
 	virtual int16_t process_received_message(const char *messageName,
 			int32_t length, uint8_t *data);
 
+
+#ifdef ORB_USE_PUBLISHER_RULES
+
+	struct PublisherRule {
+		const char **topics; //null-terminated list of topic names
+		const char *module_name; //only this module is allowed to publish one of the topics
+		bool ignore_other_topics;
+	};
+
+	/**
+	 * test if str starts with pre
+	 */
+	bool startsWith(const char *pre, const char *str);
+
+	/**
+	 * find a topic in a rule
+	 */
+	bool findTopic(const PublisherRule &rule, const char *topic_name);
+
+	/**
+	 * trim whitespace from the beginning of a string
+	 */
+	void strTrim(const char **str);
+
+	/**
+	 * Read publisher rules from a file. It has the format:
+	 *
+	 * restrict_topics: <topic1>, <topic2>, <topic3>
+	 * module: <module_name>
+	 * [ignore_others:true]
+	 *
+	 * @return 0 on success, <0 otherwise
+	 */
+	int readPublisherRulesFromFile(const char *file_name, PublisherRule &rule);
+
+	PublisherRule _publisher_rule;
+	bool _has_publisher_rules = false;
+
+#endif /* ORB_USE_PUBLISHER_RULES */
+
 };
 
 #endif /* _uORBManager_hpp_ */

From 28ad6066aa6f241ff42efe9646f2c3ddd7917f38 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Beat=20K=C3=BCng?= <beat-kueng@gmx.net>
Date: Fri, 10 Jun 2016 12:43:07 +0200
Subject: [PATCH 0941/1230] replay: add replay module, build for sitl_default,
 but do not load on startup

This adds a new module that does:
- read an parse an ULog file, given via ENV variable 'replay'
- apply all parameters from the log file
- read and apply user-defined override parameters from a file
- publish all messages in 'real-time' from the log file and add a constant
  offset to the timestamp to match the system time.
- apply changed parameters in the log (which are not overridden)
---
 Tools/check_code_style_all.sh          |   1 +
 cmake/configs/posix_sitl_default.cmake |   1 +
 src/modules/logger/messages.h          |   8 +-
 src/modules/replay/CMakeLists.txt      |  44 ++
 src/modules/replay/definitions.hpp     |  45 ++
 src/modules/replay/replay.hpp          | 152 +++++
 src/modules/replay/replay_main.cpp     | 861 +++++++++++++++++++++++++
 7 files changed, 1110 insertions(+), 2 deletions(-)
 create mode 100644 src/modules/replay/CMakeLists.txt
 create mode 100644 src/modules/replay/definitions.hpp
 create mode 100644 src/modules/replay/replay.hpp
 create mode 100644 src/modules/replay/replay_main.cpp

diff --git a/Tools/check_code_style_all.sh b/Tools/check_code_style_all.sh
index f66a039e15..c8224c0a7b 100755
--- a/Tools/check_code_style_all.sh
+++ b/Tools/check_code_style_all.sh
@@ -31,6 +31,7 @@ find \
     src/modules/muorb \
     src/modules/param \
     src/modules/px4iofirmware \
+    src/modules/replay \
     src/modules/segway \
     src/modules/sensors \
     src/modules/simulator \
diff --git a/cmake/configs/posix_sitl_default.cmake b/cmake/configs/posix_sitl_default.cmake
index 6457ea5a2b..5721e30d45 100644
--- a/cmake/configs/posix_sitl_default.cmake
+++ b/cmake/configs/posix_sitl_default.cmake
@@ -52,6 +52,7 @@ set(config_module_list
 	modules/param
 	modules/position_estimator_inav
 	modules/local_position_estimator
+	modules/replay
 	modules/sdlog2
 	modules/sensors
 	modules/simulator
diff --git a/src/modules/logger/messages.h b/src/modules/logger/messages.h
index 10e5d01838..bcbe015730 100644
--- a/src/modules/logger/messages.h
+++ b/src/modules/logger/messages.h
@@ -49,14 +49,18 @@ enum class ULogMessageType : uint8_t {
 /* declare message data structs with byte alignment (no padding) */
 #pragma pack(push, 1)
 
-#define ULOG_MSG_HEADER_LEN 3 //accounts for msg_size and msg_type
-
 /** first bytes of the file */
 struct ulog_file_header_s {
 	uint8_t magic[8];
 	uint64_t timestamp;
 };
 
+#define ULOG_MSG_HEADER_LEN 3 //accounts for msg_size and msg_type
+struct ulog_message_header_s {
+	uint16_t msg_size;
+	uint8_t msg_type;
+};
+
 struct ulog_message_format_s {
 	uint16_t msg_size; //size of message - ULOG_MSG_HEADER_LEN
 	uint8_t msg_type = static_cast<uint8_t>(ULogMessageType::FORMAT);
diff --git a/src/modules/replay/CMakeLists.txt b/src/modules/replay/CMakeLists.txt
new file mode 100644
index 0000000000..557cb070f5
--- /dev/null
+++ b/src/modules/replay/CMakeLists.txt
@@ -0,0 +1,44 @@
+############################################################################
+#
+#   Copyright (c) 2016 PX4 Development Team. All rights reserved.
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions
+# are met:
+#
+# 1. Redistributions of source code must retain the above copyright
+#    notice, this list of conditions and the following disclaimer.
+# 2. Redistributions in binary form must reproduce the above copyright
+#    notice, this list of conditions and the following disclaimer in
+#    the documentation and/or other materials provided with the
+#    distribution.
+# 3. Neither the name PX4 nor the names of its contributors may be
+#    used to endorse or promote products derived from this software
+#    without specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.
+#
+#############################################################################
+px4_add_module(
+	MODULE modules__replay
+	MAIN replay
+	COMPILE_FLAGS
+	STACK_MAIN 1000
+	STACK_MAX 4000
+	SRCS
+		replay_main.cpp
+	DEPENDS
+		platforms__common
+	)
+# vim: set noet ft=cmake fenc=utf-8 ff=unix : 
diff --git a/src/modules/replay/definitions.hpp b/src/modules/replay/definitions.hpp
new file mode 100644
index 0000000000..1b728aa994
--- /dev/null
+++ b/src/modules/replay/definitions.hpp
@@ -0,0 +1,45 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2016 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+#pragma once
+
+namespace px4
+{
+namespace replay
+{
+
+static const char *ENV_FILENAME = "replay"; ///< name for getenv()
+
+
+} //namespace replay
+} //namespace px4
diff --git a/src/modules/replay/replay.hpp b/src/modules/replay/replay.hpp
new file mode 100644
index 0000000000..78315dde6a
--- /dev/null
+++ b/src/modules/replay/replay.hpp
@@ -0,0 +1,152 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2016 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+#pragma once
+
+#include <fstream>
+#include <map>
+#include <vector>
+#include <set>
+#include <string>
+
+#include "definitions.hpp"
+
+#include <uORB/uORBTopics.h>
+
+namespace px4
+{
+
+/**
+ * @class Replay
+ * Parses an ULog file and replays it in 'real-time'. The timestamp of each replayed message is offset
+ * to match the starting time of replay. It keeps a stream for each subscription to find the next message
+ * to replay. This is necessary because data messages from different subscriptions don't need to be in
+ * monotonic increasing order.
+ */
+class Replay
+{
+public:
+	Replay();
+
+	/// Destructor, also waits for task exit
+	~Replay();
+
+	/// Start task.
+	/// @param quiet silently fail if no log file found
+	/// @return		OK on success.
+	static int		start(bool quiet);
+
+	static void	task_main_trampoline(int argc, char *argv[]);
+
+	void		task_main();
+
+	/**
+	 * Tell the replay module that we want to use replay mode.
+	 * After that, only 'replay start' must be executed (typically the last step after startup).
+	 * @param file_name file name of the used log replay file. Will be copied.
+	 */
+	static void setupReplayFile(const char *file_name);
+
+	static bool isSetup() { return _replay_file; }
+private:
+	bool _task_should_exit = false;
+	std::set<std::string> _overridden_params;
+	std::map<std::string, std::string> _file_formats; ///< all formats we read from the file
+
+	uint64_t _file_start_time;
+	uint64_t _replay_start_time;
+	std::streampos _data_section_start; ///< first ADD_LOGGED_MSG message
+	std::vector<uint8_t> _read_buffer;
+
+	struct Subscription {
+
+		const orb_metadata *orb_meta = nullptr; ///< if nullptr, this subscription is invalid
+		orb_advert_t orb_advert = nullptr;
+		uint8_t multi_id;
+		int timestamp_offset; ///< marks the field of the timestamp
+
+		std::streampos next_read_pos;
+		uint64_t next_timestamp; ///< timestamp of the file
+	};
+	std::vector<Subscription> _subscriptions;
+
+	/** keep track of file position to avoid adding a subscription multiple times. */
+	std::streampos _subscription_file_pos = 0;
+
+	bool readFileHeader(std::ifstream &file);
+
+	/**
+	 * Read definitions section: check formats, apply parameters and store
+	 * the start of the data section.
+	 * @return true on success
+	 */
+	bool readFileDefinitions(std::ifstream &file);
+
+	///file parsing methods. They return false, when further parsing should be aborted.
+	bool readFormat(std::ifstream &file, uint16_t msg_size);
+	bool readAndAddSubscription(std::ifstream &file, uint16_t msg_size);
+
+	/**
+	 * Read and handle additional messages starting at current file position, while position < end_position.
+	 * This handles dropout and parameter update messages.
+	 * We need to handle these separately, because they have no timestamp. We look at the file position instead.
+	 * @return false on file error
+	 */
+	bool readAndHandleAdditionalMessages(std::ifstream &file, std::streampos end_position);
+	bool readDropout(std::ifstream &file, uint16_t msg_size);
+	bool readAndApplyParameter(std::ifstream &file, uint16_t msg_size);
+
+	/**
+	 * Find next data message for this subscription, starting with the stored file offset.
+	 * Skip the first message, and if found, read the timestamp and store the new file offset.
+	 * This also takes care of new subscriptions and parameter updates. When reaching EOF,
+	 * the subscription is set to invalid.
+	 * File seek position is arbitrary after this call.
+	 * @return false on file error
+	 */
+	bool nextDataMessage(std::ifstream &file, Subscription &subscription, int msg_id);
+
+	static const orb_metadata *findTopic(const std::string &name);
+	/** get the array size from a type. eg. float[3] -> return float */
+	static std::string extractArraySize(const std::string &type_name_full, int &array_size);
+	/** get the size of a type that is not an array */
+	static size_t sizeOfType(const std::string &type_name);
+	/** get the size of a type that can be an array */
+	static size_t sizeOfFullType(const std::string &type_name_full);
+
+	void setUserParams(const char *filename);
+
+	static char *_replay_file;
+};
+
+} //namespace px4
diff --git a/src/modules/replay/replay_main.cpp b/src/modules/replay/replay_main.cpp
new file mode 100644
index 0000000000..cf91f5a3aa
--- /dev/null
+++ b/src/modules/replay/replay_main.cpp
@@ -0,0 +1,861 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2016 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file replay_main.cpp
+ * This module reads messages from an ULog file and publishes them.
+ * It sets the parameters from the log file and handles user-defined
+ * parameter overrides.
+ *
+ * @author Beat Kueng
+*/
+
+#include <px4_defines.h>
+#include <px4_posix.h>
+#include <px4_tasks.h>
+#include <px4_time.h>
+
+#include <cstring>
+#include <float.h>
+#include <fstream>
+#include <iostream>
+#include <math.h>
+#include <time.h>
+#include <sstream>
+#include <stdio.h>
+#include <stdlib.h>
+#include <string>
+
+#include <logger/logger.h>
+#include <logger/messages.h>
+
+#include "replay.hpp"
+
+#define PARAMS_OVERRIDE_FILE PX4_ROOTFSDIR "/replay_params.txt"
+
+
+extern "C" __EXPORT int replay_main(int argc, char *argv[]);
+
+using namespace std;
+
+namespace px4
+{
+class Replay;
+
+namespace replay
+{
+Replay *instance = nullptr;
+static int	control_task = -1;			//task handle for task
+
+} //namespace replay
+
+
+char *Replay::_replay_file = nullptr;
+
+Replay::Replay()
+{
+}
+
+Replay::~Replay()
+{
+	if (replay::control_task != -1) {
+		_task_should_exit = true;
+
+		/* wait for a second for the task to quit at our request */
+		unsigned int i = 0;
+
+		do {
+			usleep(20000);
+
+			/* if we have given up, kill it */
+			if (++i > 200) {
+				px4_task_delete(replay::control_task);
+				replay::control_task = -1;
+				break;
+			}
+		} while (replay::control_task != -1);
+	}
+}
+
+void Replay::setupReplayFile(const char *file_name)
+{
+	if (_replay_file) {
+		free(_replay_file);
+	}
+
+	_replay_file = strdup(file_name);
+}
+
+
+
+void Replay::setUserParams(const char *filename)
+{
+	string line, param_name, value_string;
+	ifstream myfile(filename);
+
+	if (!myfile.is_open()) {
+		return;
+	}
+
+	PX4_INFO("Applying override params from %s...", filename);
+
+	while (!myfile.eof()) {
+		getline(myfile, line);
+
+		if (line.empty() || line[0] == '#') {
+			continue;
+		}
+
+		istringstream mystrstream(line);
+		mystrstream >> param_name;
+		mystrstream >> value_string;
+
+		double param_value_double = stod(value_string);
+
+		param_t handle = param_find(param_name.c_str());
+		param_type_t param_format = param_type(handle);
+		_overridden_params.insert(param_name);
+
+		if (param_format == PARAM_TYPE_INT32) {
+			int32_t value = 0;
+			value = (int32_t)param_value_double;
+			param_set(handle, (const void *)&value);
+
+		} else if (param_format == PARAM_TYPE_FLOAT) {
+			float value = 0;
+			value = (float)param_value_double;
+			param_set(handle, (const void *)&value);
+		}
+	}
+}
+
+bool Replay::readFileHeader(std::ifstream &file)
+{
+	file.seekg(0);
+	ulog_file_header_s msg_header;
+	file.read((char *)&msg_header, sizeof(msg_header));
+
+	if (!file) {
+		return false;
+	}
+
+	_file_start_time = msg_header.timestamp;
+	//verify it's an ULog file
+	char magic[8];
+	magic[0] = 'U';
+	magic[1] = 'L';
+	magic[2] = 'o';
+	magic[3] = 'g';
+	magic[4] = 0x01;
+	magic[5] = 0x12;
+	magic[6] = 0x35;
+	return memcmp(magic, msg_header.magic, 7) == 0;
+}
+
+bool Replay::readFileDefinitions(std::ifstream &file)
+{
+	PX4_INFO("Applying params from ULog file...");
+
+	ulog_message_header_s message_header;
+	file.seekg(sizeof(ulog_file_header_s));
+
+	while (true) {
+		file.read((char *)&message_header, ULOG_MSG_HEADER_LEN);
+
+		if (!file) {
+			return false;
+		}
+
+		switch (message_header.msg_type) {
+		case (int)ULogMessageType::FORMAT:
+			if (!readFormat(file, message_header.msg_size)) {
+				return false;
+			}
+
+			break;
+
+		case (int)ULogMessageType::PARAMETER:
+			if (!readAndApplyParameter(file, message_header.msg_size)) {
+				return false;
+			}
+
+			break;
+
+		case (int)ULogMessageType::ADD_LOGGED_MSG:
+			_data_section_start = file.tellg() - (streamoff)ULOG_MSG_HEADER_LEN;
+			return true;
+
+		case (int)ULogMessageType::INFO: //skip
+			file.seekg(message_header.msg_size, ios::cur);
+			break;
+
+		default:
+			PX4_ERR("unknown log definition type %i, size %i (offset %i)",
+				(int)message_header.msg_type, (int)message_header.msg_size, (int)file.tellg());
+			file.seekg(message_header.msg_size, ios::cur);
+			break;
+		}
+	}
+
+	return true;
+}
+
+bool Replay::readFormat(std::ifstream &file, uint16_t msg_size)
+{
+	_read_buffer.reserve(msg_size + 1);
+	char *format = (char *)_read_buffer.data();
+	file.read(format, msg_size);
+	format[msg_size] = 0;
+
+	if (!file) {
+		return false;
+	}
+
+	string str_format(format);
+	size_t pos = str_format.find(':');
+
+	if (pos == string::npos) {
+		return false;
+	}
+
+	string name = str_format.substr(0, pos);
+	string fields = str_format.substr(pos + 1);
+	_file_formats[name] = fields;
+
+	return true;
+}
+
+bool Replay::readAndAddSubscription(std::ifstream &file, uint16_t msg_size)
+{
+	_read_buffer.reserve(msg_size + 1);
+	char *message = (char *)_read_buffer.data();
+	streampos this_message_pos = file.tellg() - (streamoff)ULOG_MSG_HEADER_LEN;
+	file.read(message, msg_size);
+	message[msg_size] = 0;
+
+	if (!file) {
+		return false;
+	}
+
+	if (file.tellg() <= _subscription_file_pos) { //already read this subscription
+		return true;
+	}
+
+	_subscription_file_pos = file.tellg();
+
+	uint8_t multi_id = *(uint8_t *)message;
+	uint16_t msg_id = ((uint16_t) message[1]) | (((uint16_t) message[2]) << 8);
+	string topic_name(message + 3);
+	const orb_metadata *orb_meta = findTopic(topic_name);
+
+	if (!orb_meta) {
+		PX4_WARN("Topic %s not found internally. Will ignore it", topic_name.c_str());
+		return true;
+	}
+
+	//check the format: the field definitions must match
+	//FIXME: this should check recursively, all used nested types
+	string file_format = _file_formats[topic_name];
+
+	if (file_format != orb_meta->o_fields) {
+		PX4_WARN("Formats for %s don't match. Will ignore it.", topic_name.c_str());
+		PX4_WARN(" Internal format: %s", orb_meta->o_fields);
+		PX4_WARN(" File format    : %s", file_format.c_str());
+	}
+
+	Subscription subscription;
+	subscription.orb_meta = orb_meta;
+	subscription.multi_id = multi_id;
+
+
+	//find the timestamp offset (not necessarily the first field)
+	string fields = orb_meta->o_fields;
+	size_t prev_field_end = 0;
+	size_t field_end = fields.find(';');
+	bool timestamp_found = false;
+	subscription.timestamp_offset = 0;
+
+	while (field_end != string::npos && !timestamp_found) {
+		size_t space_pos = fields.find(' ', prev_field_end);
+
+		if (space_pos != string::npos) {
+			string type_name_full = fields.substr(prev_field_end, space_pos - prev_field_end);
+			string field_name = fields.substr(space_pos + 1, field_end - space_pos - 1);
+
+			if (field_name == "timestamp") {
+				timestamp_found = true;
+
+				if (type_name_full != "uint64_t") {
+					PX4_ERR("Unsupported timestamp type %s, ignoring the topic %s", type_name_full,
+						orb_meta->o_name);
+					return true;
+				}
+
+			} else {
+				subscription.timestamp_offset += sizeOfFullType(type_name_full);
+			}
+		}
+
+		prev_field_end = field_end + 1;
+		field_end = fields.find(';', prev_field_end);
+	}
+
+	if (!timestamp_found) {
+		return true;
+	}
+
+
+	//find first data message (and the timestamp)
+	streampos cur_pos = file.tellg();
+	subscription.next_read_pos = this_message_pos; //this will be skipped
+
+	if (!nextDataMessage(file, subscription, msg_id)) {
+		return false;
+	}
+
+	file.seekg(cur_pos);
+
+	if (!subscription.orb_meta) {
+		//no message found. This is not a fatal error
+		return true;
+	}
+
+	PX4_DEBUG("adding subscription for %s (msg_id %i)", subscription.orb_meta->o_name, msg_id);
+
+	//add subscription
+	if (_subscriptions.size() <= msg_id) {
+		_subscriptions.resize(msg_id + 1);
+	}
+
+	_subscriptions[msg_id] = subscription;
+
+	return true;
+}
+
+bool Replay::readAndHandleAdditionalMessages(std::ifstream &file, std::streampos end_position)
+{
+	ulog_message_header_s message_header;
+
+	while (file.tellg() < end_position) {
+		file.read((char *)&message_header, ULOG_MSG_HEADER_LEN);
+
+		if (!file) {
+			return false;
+		}
+
+		switch (message_header.msg_type) {
+		case (int)ULogMessageType::PARAMETER:
+			if (!readAndApplyParameter(file, message_header.msg_size)) {
+				return false;
+			}
+
+			break;
+
+		case (int)ULogMessageType::DROPOUT:
+			readDropout(file, message_header.msg_size);
+			break;
+
+		default: //skip all others
+			file.seekg(message_header.msg_size, ios::cur);
+			break;
+		}
+	}
+
+	return true;
+}
+
+bool Replay::readAndApplyParameter(std::ifstream &file, uint16_t msg_size)
+{
+	_read_buffer.reserve(msg_size);
+	uint8_t *message = (uint8_t *)_read_buffer.data();
+	file.read((char *)message, msg_size);
+
+	if (!file) {
+		return false;
+	}
+
+	uint8_t key_len = message[0];
+	string key((char *)message + 1, key_len);
+
+	size_t pos = key.find(' ');
+
+	if (pos == string::npos) {
+		return false;
+	}
+
+	string type = key.substr(0, pos);
+	string param_name = key.substr(pos + 1);
+
+	if (_overridden_params.find(param_name) != _overridden_params.end()) {
+		//this parameter is overridden, so don't apply it
+		return true;
+	}
+
+	if (type != "int32_t" && type != "float") {
+		PX4_WARN("unknown parameter type %s, name %s (ignoring it)", type.c_str(), param_name.c_str());
+		return true;
+	}
+
+	param_t handle = param_find(param_name.c_str());
+
+	if (handle != PARAM_INVALID) {
+		param_set(handle, (const void *)(message + 1 + key_len));
+	}
+
+	return true;
+}
+
+bool Replay::readDropout(std::ifstream &file, uint16_t msg_size)
+{
+	uint16_t duration;
+	file.read((char *)&duration, sizeof(duration));
+
+	PX4_INFO("Dropout in replayed log, %i ms", (int)duration);
+	return file.good();
+}
+
+bool Replay::nextDataMessage(std::ifstream &file, Subscription &subscription, int msg_id)
+{
+	ulog_message_header_s message_header;
+	file.seekg(subscription.next_read_pos);
+	//ignore the first message (it's data we already read)
+	file.read((char *)&message_header, ULOG_MSG_HEADER_LEN);
+
+	if (file) {
+		file.seekg(message_header.msg_size, ios::cur);
+	}
+
+	uint16_t file_msg_id;
+	bool done = false;
+
+	while (file && !done) {
+		streampos cur_pos = file.tellg();
+		file.read((char *)&message_header, ULOG_MSG_HEADER_LEN);
+
+		if (!file) {
+			break;
+		}
+
+		switch (message_header.msg_type) {
+		case (int)ULogMessageType::ADD_LOGGED_MSG:
+			readAndAddSubscription(file, message_header.msg_size);
+			break;
+
+		case (int)ULogMessageType::DATA:
+			file.read((char *)&file_msg_id, sizeof(file_msg_id));
+
+			if (file) {
+				if (msg_id == file_msg_id) {
+					if (message_header.msg_size == subscription.orb_meta->o_size_no_padding + 2) {
+						subscription.next_read_pos = cur_pos;
+						file.seekg(subscription.timestamp_offset, ios::cur);
+						file.read((char *)&subscription.next_timestamp, sizeof(subscription.next_timestamp));
+						done = true;
+
+					} else { //sanity check failed!
+						PX4_ERR("data message %s has wrong size %i (expected %i). Skipping",
+							subscription.orb_meta->o_name, message_header.msg_size,
+							subscription.orb_meta->o_size_no_padding + 2);
+						file.seekg(message_header.msg_size - sizeof(file_msg_id), ios::cur);
+					}
+
+				} else { //not the one we are looking for
+					file.seekg(message_header.msg_size - sizeof(file_msg_id), ios::cur);
+				}
+			}
+
+			break;
+
+		case (int)ULogMessageType::REMOVE_LOGGED_MSG: //skip these
+		case (int)ULogMessageType::PARAMETER:
+		case (int)ULogMessageType::DROPOUT:
+		case (int)ULogMessageType::SYNC:
+		case (int)ULogMessageType::LOGGING:
+			file.seekg(message_header.msg_size, ios::cur);
+			break;
+
+		default:
+			//this really should not happen
+			PX4_ERR("unknown log message type %i, size %i (offset %i)",
+				(int)message_header.msg_type, (int)message_header.msg_size, (int)file.tellg());
+			file.seekg(message_header.msg_size, ios::cur);
+			break;
+		}
+	}
+
+	if (file.eof()) { //no more data messages for this subscription
+		subscription.orb_meta = nullptr;
+		file.clear();
+	}
+
+	return file.good();
+}
+
+const orb_metadata *Replay::findTopic(const std::string &name)
+{
+	const orb_metadata **topics = orb_get_topics();
+
+	for (size_t i = 0; i < orb_topics_count(); i++) {
+		if (name == topics[i]->o_name) {
+			return topics[i];
+		}
+	}
+
+	return nullptr;
+}
+
+std::string Replay::extractArraySize(const std::string &type_name_full, int &array_size)
+{
+	size_t start_pos = type_name_full.find('[');
+	size_t end_pos = type_name_full.find(']');
+
+	if (start_pos == string::npos || end_pos == string::npos) {
+		array_size = 1;
+		return type_name_full;
+	}
+
+	array_size = atoi(type_name_full.substr(start_pos + 1, end_pos - start_pos - 1).c_str());
+	return type_name_full.substr(0, start_pos);
+}
+
+size_t Replay::sizeOfType(const std::string &type_name)
+{
+	if (type_name == "int8_t" || type_name == "uint8_t") {
+		return 1;
+
+	} else if (type_name == "int16_t" || type_name == "uint16_t") {
+		return 2;
+
+	} else if (type_name == "int32_t" || type_name == "uint32_t") {
+		return 4;
+
+	} else if (type_name == "int64_t" || type_name == "uint64_t") {
+		return 8;
+
+	} else if (type_name == "float") {
+		return 4;
+
+	} else if (type_name == "double") {
+		return 8;
+
+	} else if (type_name == "char" || type_name == "bool") {
+		return 1;
+	}
+
+	const orb_metadata *orb_meta = findTopic(type_name);
+
+	if (orb_meta) {
+		return orb_meta->o_size;
+	}
+
+	PX4_ERR("unknown type: %s", type_name.c_str());
+	return 0;
+}
+size_t Replay::sizeOfFullType(const std::string &type_name_full)
+{
+	int array_size;
+	string type_name = extractArraySize(type_name_full, array_size);
+	return sizeOfType(type_name) * array_size;
+}
+
+void Replay::task_main()
+{
+	// log reader currently assumes little endian
+	int num = 1;
+
+	if (*(char *)&num != 1) {
+		PX4_ERR("Replay only works on little endian!");
+		return;
+	}
+
+	ifstream replay_file(_replay_file, ios::in | ios::binary);
+
+	if (!replay_file.is_open()) {
+		PX4_ERR("Failed to open replay file");
+		return;
+	}
+
+	if (!readFileHeader(replay_file)) {
+		PX4_ERR("Failed to read file header. Not a valid ULog file");
+		return;
+	}
+
+	//initialize the formats and apply the parameters from the log file
+	if (!readFileDefinitions(replay_file)) {
+		PX4_ERR("Failed to read ULog definitions section. Broken file?");
+		return;
+	}
+
+	setUserParams(PARAMS_OVERRIDE_FILE);
+
+	_replay_start_time = hrt_absolute_time();
+
+	PX4_INFO("Replay in progress...");
+
+	ulog_message_header_s message_header;
+	replay_file.seekg(_data_section_start);
+
+	//we know the next message must be an ADD_LOGGED_MSG
+	replay_file.read((char *)&message_header, ULOG_MSG_HEADER_LEN);
+
+	if (!readAndAddSubscription(replay_file, message_header.msg_size)) {
+		PX4_ERR("Failed to read subscription");
+		return;
+	}
+
+
+	//we update the timestamps from the file by a constant offset to match
+	//the current replay time
+	const uint64_t timestamp_offset = _replay_start_time - _file_start_time;
+	uint32_t nr_published_messages = 0;
+	streampos last_additional_message_pos = _data_section_start;
+
+	while (!_task_should_exit && replay_file) {
+
+		//Find the next message to publish. Messages from different subscriptions don't need
+		//to be in chronological order, so we need to check all subscriptions
+		uint64_t next_file_time = 0;
+		int next_msg_id = -1;
+
+		for (size_t i = 0; i < _subscriptions.size(); ++i) {
+			const Subscription &subscription = _subscriptions[i];
+
+			if (subscription.orb_meta) {
+				if (next_file_time == 0 || subscription.next_timestamp < next_file_time) {
+					next_msg_id = (int)i;
+					next_file_time = subscription.next_timestamp;
+				}
+			}
+		}
+
+		if (next_msg_id == -1) {
+			break; //no active subscription anymore. We're done.
+		}
+
+		Subscription &sub = _subscriptions[next_msg_id];
+
+		if (next_file_time == 0) {
+			//someone didn't set the timestamp properly. Consider the message invalid
+			nextDataMessage(replay_file, sub, next_msg_id);
+			continue;
+		}
+
+
+		//handle additional messages between last and next published data
+		replay_file.seekg(last_additional_message_pos);
+		streampos next_additional_message_pos = sub.next_read_pos;
+		readAndHandleAdditionalMessages(replay_file, next_additional_message_pos);
+		last_additional_message_pos = next_additional_message_pos;
+
+
+		//wait if necessary
+		const uint64_t publish_timestamp = next_file_time + timestamp_offset;
+		uint64_t cur_time = hrt_absolute_time();
+
+		if (cur_time < publish_timestamp) {
+			usleep(publish_timestamp - cur_time);
+		}
+
+		//It's time to publish
+		const size_t msg_read_size = sub.orb_meta->o_size_no_padding;
+		const size_t msg_write_size = sub.orb_meta->o_size;
+		_read_buffer.reserve(msg_write_size);
+		replay_file.seekg(sub.next_read_pos + (streamoff)(ULOG_MSG_HEADER_LEN + 2)); //skip header & msg id
+		replay_file.read((char *)_read_buffer.data(), msg_read_size);
+		*(uint64_t *)(_read_buffer.data() + sub.timestamp_offset) = publish_timestamp;
+
+		if (sub.orb_advert) {
+			orb_publish(sub.orb_meta, sub.orb_advert, _read_buffer.data());
+			++nr_published_messages;
+
+		} else {
+			if (sub.multi_id == 0) {
+				sub.orb_advert = orb_advertise(sub.orb_meta, _read_buffer.data());
+				++nr_published_messages;
+
+			} else {
+				// make sure the other instances are advertised already so that we get the correct instance
+				bool advertised = false;
+
+				for (const auto &subscription : _subscriptions) {
+					if (subscription.orb_meta) {
+						if (strcmp(sub.orb_meta->o_name, subscription.orb_meta->o_name) == 0 &&
+						    subscription.orb_advert && subscription.multi_id == sub.multi_id - 1) {
+							advertised = true;
+						}
+					}
+				}
+
+				if (advertised) {
+					int instance;
+					sub.orb_advert = orb_advertise_multi(sub.orb_meta, _read_buffer.data(),
+									     &instance, ORB_PRIO_DEFAULT);
+					++nr_published_messages;
+				}
+			}
+		}
+
+
+		nextDataMessage(replay_file, _subscriptions[next_msg_id], next_msg_id);
+
+		//TODO: output status (eg. every sec), including total duration...
+	}
+
+	for (auto &subscription : _subscriptions) {
+		if (subscription.orb_advert) {
+			orb_unadvertise(subscription.orb_advert);
+			subscription.orb_advert = nullptr;
+		}
+	}
+
+	if (!_task_should_exit) {
+		PX4_INFO("Replay done (published %u msgs, %.3lf s)", nr_published_messages,
+			 (double)hrt_elapsed_time(&_replay_start_time) / 1.e6);
+
+		//TODO: should we close the log file & exit (optionally, by adding a parameter -q) ?
+	}
+}
+
+void Replay::task_main_trampoline(int argc, char *argv[])
+{
+	replay::instance = new Replay();
+
+	if (replay::instance == nullptr) {
+		PX4_ERR("alloc failed");
+		return;
+	}
+
+	replay::instance->task_main();
+	replay::control_task = -1;
+}
+
+int Replay::start(bool quiet)
+{
+	ASSERT(replay::control_task == -1);
+
+	//check for logfile env variable
+	const char *logfile = getenv(replay::ENV_FILENAME);
+
+	if (logfile) {
+		PX4_INFO("using replay log file: %s", logfile);
+		setupReplayFile(logfile);
+
+	} else {
+		if (quiet) {
+			return OK;
+
+		} else {
+			PX4_ERR("no log file given (via env variable %s)", replay::ENV_FILENAME);
+			return -1;
+		}
+	}
+
+	/* start the task */
+	replay::control_task = px4_task_spawn_cmd("replay",
+			       SCHED_DEFAULT,
+			       SCHED_PRIORITY_MAX - 5,
+			       4000,
+			       (px4_main_t)&Replay::task_main_trampoline,
+			       nullptr);
+
+	if (replay::control_task < 0) {
+		replay::control_task = -1;
+		PX4_ERR("task start failed");
+		return -errno;
+	}
+
+	return OK;
+}
+
+} //namespace px4
+
+using namespace px4;
+
+int replay_main(int argc, char *argv[])
+{
+	if (argc < 1) {
+		PX4_WARN("usage: replay {trystart|start|stop|status}");
+		return 1;
+	}
+
+	bool do_start = false;
+	bool quiet = false;
+
+	if (!strcmp(argv[1], "start")) {
+		do_start = true;
+
+	} else if (!strcmp(argv[1], "trystart")) {
+		do_start = true;
+		quiet = true;
+	}
+
+	if (do_start) {
+		if (replay::instance != nullptr) {
+			PX4_WARN("already running");
+			return 1;
+		}
+
+		if (OK != replay::instance->start(quiet)) {
+			PX4_ERR("start failed");
+			return 1;
+		}
+
+		return 0;
+	}
+
+	if (!strcmp(argv[1], "stop")) {
+		if (replay::instance == nullptr) {
+			PX4_WARN("not running");
+			return 1;
+		}
+
+		delete replay::instance;
+		replay::instance = nullptr;
+
+		return 0;
+	}
+
+	if (!strcmp(argv[1], "status")) {
+		if (replay::instance) {
+			PX4_WARN("running");
+			return 0;
+
+		} else {
+			PX4_WARN("not running");
+			return 1;
+		}
+	}
+
+	PX4_ERR("unrecognized command");
+	return 1;
+}

From 84a1a100062980b0613db6628a161dcb0a3d7872 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Beat=20K=C3=BCng?= <beat-kueng@gmx.net>
Date: Fri, 10 Jun 2016 12:45:22 +0200
Subject: [PATCH 0942/1230] logger: check if we are in replay mode via ENV
 variable 'replay'

---
 src/modules/logger/logger.cpp | 14 +++++++++++++-
 src/modules/logger/logger.h   |  5 ++---
 2 files changed, 15 insertions(+), 4 deletions(-)

diff --git a/src/modules/logger/logger.cpp b/src/modules/logger/logger.cpp
index 4412809f79..c736d34c1f 100644
--- a/src/modules/logger/logger.cpp
+++ b/src/modules/logger/logger.cpp
@@ -37,6 +37,7 @@
 #include <sys/stat.h>
 #include <errno.h>
 #include <string.h>
+#include <stdlib.h>
 #include <time.h>
 
 #include <uORB/uORB.h>
@@ -51,6 +52,7 @@
 #include <px4_getopt.h>
 #include <px4_log.h>
 #include <systemlib/mavlink_log.h>
+#include <replay/definitions.hpp>
 
 #ifdef __PX4_DARWIN
 #include <sys/param.h>
@@ -82,7 +84,6 @@ static int logger_task = -1;
 static pthread_t writer_thread;
 
 
-char *Logger::_replay_file_name = nullptr;
 
 int logger_main(int argc, char *argv[])
 {
@@ -312,6 +313,13 @@ void Logger::run_trampoline(int argc, char *argv[])
 		PX4_ERR("alloc failed");
 
 	} else {
+		//check for replay mode
+		const char *logfile = getenv(px4::replay::ENV_FILENAME);
+
+		if (logfile) {
+			logger_ptr->setReplayFile(logfile);
+		}
+
 		logger_ptr->run();
 	}
 
@@ -353,6 +361,10 @@ Logger::~Logger()
 		} while (logger_task != -1);
 	}
 
+	if (_replay_file_name) {
+		free(_replay_file_name);
+	}
+
 	if (_msg_buffer) {
 		delete[](_msg_buffer);
 	}
diff --git a/src/modules/logger/logger.h b/src/modules/logger/logger.h
index d1e96658fe..6270837ec6 100644
--- a/src/modules/logger/logger.h
+++ b/src/modules/logger/logger.h
@@ -85,7 +85,7 @@ public:
 	 * before starting the logger.
 	 * @param file_name file name of the used log replay file. Will be copied.
 	 */
-	static void setReplayFile(const char *file_name);
+	void setReplayFile(const char *file_name);
 
 	/**
 	 * Add a topic to be logged. This must be called before start_log()
@@ -233,8 +233,7 @@ private:
 	param_t						_log_utc_offset;
 	orb_advert_t					_mavlink_log_pub = nullptr;
 	uint16_t					_next_topic_id; ///< id of next subscribed topic
-
-	static char		*_replay_file_name;
+	char						*_replay_file_name = nullptr;
 };
 
 } //namespace logger

From 6e44760819f082369f367a5836e21fb8e756ae80 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Beat=20K=C3=BCng?= <beat-kueng@gmx.net>
Date: Mon, 13 Jun 2016 16:05:01 +0200
Subject: [PATCH 0943/1230] replay: add 'tryapplyparams' command

This only applies parameters from the log file and user-supplied overrides.
It is intended to be called as one of the first startup commands (after
param load), so that during startup, all applications find the parameters
from the replayed system.

Note that this is an optional command and 'replay start' will again load
and apply the parameters in any case.
---
 src/modules/replay/replay.hpp      | 11 +++-
 src/modules/replay/replay_main.cpp | 92 +++++++++++++++++++++---------
 2 files changed, 74 insertions(+), 29 deletions(-)

diff --git a/src/modules/replay/replay.hpp b/src/modules/replay/replay.hpp
index 78315dde6a..f4fbcbff2c 100644
--- a/src/modules/replay/replay.hpp
+++ b/src/modules/replay/replay.hpp
@@ -63,8 +63,10 @@ public:
 
 	/// Start task.
 	/// @param quiet silently fail if no log file found
+	/// @param apply_params_only if true, only apply parameters from definitions section of the file
+	///                          and user-overridden parameters, then exit w/o replaying.
 	/// @return		OK on success.
-	static int		start(bool quiet);
+	static int		start(bool quiet, bool apply_params_only);
 
 	static void	task_main_trampoline(int argc, char *argv[]);
 
@@ -116,6 +118,13 @@ private:
 	bool readFormat(std::ifstream &file, uint16_t msg_size);
 	bool readAndAddSubscription(std::ifstream &file, uint16_t msg_size);
 
+	/**
+	 * Read the file header and definitions sections. Apply the parameters from this section
+	 * and apply user-defined overridden parameters.
+	 * @return true on success
+	 */
+	bool readDefinitionsAndApplyParams(std::ifstream &file);
+
 	/**
 	 * Read and handle additional messages starting at current file position, while position < end_position.
 	 * This handles dropout and parameter update messages.
diff --git a/src/modules/replay/replay_main.cpp b/src/modules/replay/replay_main.cpp
index cf91f5a3aa..4db9d36cc6 100644
--- a/src/modules/replay/replay_main.cpp
+++ b/src/modules/replay/replay_main.cpp
@@ -588,35 +588,43 @@ size_t Replay::sizeOfFullType(const std::string &type_name_full)
 	return sizeOfType(type_name) * array_size;
 }
 
-void Replay::task_main()
+bool Replay::readDefinitionsAndApplyParams(std::ifstream &file)
 {
 	// log reader currently assumes little endian
 	int num = 1;
 
 	if (*(char *)&num != 1) {
 		PX4_ERR("Replay only works on little endian!");
-		return;
+		return false;
 	}
 
-	ifstream replay_file(_replay_file, ios::in | ios::binary);
-
-	if (!replay_file.is_open()) {
+	if (!file.is_open()) {
 		PX4_ERR("Failed to open replay file");
-		return;
+		return false;
 	}
 
-	if (!readFileHeader(replay_file)) {
+	if (!readFileHeader(file)) {
 		PX4_ERR("Failed to read file header. Not a valid ULog file");
-		return;
+		return false;
 	}
 
 	//initialize the formats and apply the parameters from the log file
-	if (!readFileDefinitions(replay_file)) {
+	if (!readFileDefinitions(file)) {
 		PX4_ERR("Failed to read ULog definitions section. Broken file?");
-		return;
+		return false;
 	}
 
 	setUserParams(PARAMS_OVERRIDE_FILE);
+	return true;
+}
+
+void Replay::task_main()
+{
+	ifstream replay_file(_replay_file, ios::in | ios::binary);
+
+	if (!readDefinitionsAndApplyParams(replay_file)) {
+		return;
+	}
 
 	_replay_start_time = hrt_absolute_time();
 
@@ -759,20 +767,23 @@ void Replay::task_main_trampoline(int argc, char *argv[])
 	replay::control_task = -1;
 }
 
-int Replay::start(bool quiet)
+int Replay::start(bool quiet, bool apply_params_only)
 {
 	ASSERT(replay::control_task == -1);
+	int ret = PX4_OK;
 
 	//check for logfile env variable
 	const char *logfile = getenv(replay::ENV_FILENAME);
 
 	if (logfile) {
-		PX4_INFO("using replay log file: %s", logfile);
-		setupReplayFile(logfile);
+		if (!isSetup()) {
+			PX4_INFO("using replay log file: %s", logfile);
+			setupReplayFile(logfile);
+		}
 
 	} else {
 		if (quiet) {
-			return OK;
+			return PX4_OK;
 
 		} else {
 			PX4_ERR("no log file given (via env variable %s)", replay::ENV_FILENAME);
@@ -780,21 +791,40 @@ int Replay::start(bool quiet)
 		}
 	}
 
-	/* start the task */
-	replay::control_task = px4_task_spawn_cmd("replay",
-			       SCHED_DEFAULT,
-			       SCHED_PRIORITY_MAX - 5,
-			       4000,
-			       (px4_main_t)&Replay::task_main_trampoline,
-			       nullptr);
+	if (apply_params_only) {
+		Replay *r = new Replay();
 
-	if (replay::control_task < 0) {
-		replay::control_task = -1;
-		PX4_ERR("task start failed");
-		return -errno;
+		if (r == nullptr) {
+			PX4_ERR("alloc failed");
+			return -ENOMEM;
+		}
+
+		ifstream replay_file(_replay_file, ios::in | ios::binary);
+
+		if (!r->readDefinitionsAndApplyParams(replay_file)) {
+			ret = -1;
+		}
+
+		delete(r);
+
+	} else {
+
+		/* start the task */
+		replay::control_task = px4_task_spawn_cmd("replay",
+				       SCHED_DEFAULT,
+				       SCHED_PRIORITY_MAX - 5,
+				       4000,
+				       (px4_main_t)&Replay::task_main_trampoline,
+				       nullptr);
+
+		if (replay::control_task < 0) {
+			replay::control_task = -1;
+			PX4_ERR("task start failed");
+			return -errno;
+		}
 	}
 
-	return OK;
+	return ret;
 }
 
 } //namespace px4
@@ -804,12 +834,13 @@ using namespace px4;
 int replay_main(int argc, char *argv[])
 {
 	if (argc < 1) {
-		PX4_WARN("usage: replay {trystart|start|stop|status}");
+		PX4_WARN("usage: replay {tryapplyparams|trystart|start|stop|status}");
 		return 1;
 	}
 
 	bool do_start = false;
 	bool quiet = false;
+	bool apply_params_only = false;
 
 	if (!strcmp(argv[1], "start")) {
 		do_start = true;
@@ -817,6 +848,11 @@ int replay_main(int argc, char *argv[])
 	} else if (!strcmp(argv[1], "trystart")) {
 		do_start = true;
 		quiet = true;
+
+	} else if (!strcmp(argv[1], "tryapplyparams")) {
+		do_start = true;
+		quiet = true;
+		apply_params_only = true;
 	}
 
 	if (do_start) {
@@ -825,7 +861,7 @@ int replay_main(int argc, char *argv[])
 			return 1;
 		}
 
-		if (OK != replay::instance->start(quiet)) {
+		if (PX4_OK != replay::instance->start(quiet, apply_params_only)) {
 			PX4_ERR("start failed");
 			return 1;
 		}

From 168c7442323f7c8adb47ddb7617c63f5d05fad3f Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Beat=20K=C3=BCng?= <beat-kueng@gmx.net>
Date: Tue, 14 Jun 2016 14:41:40 +0200
Subject: [PATCH 0944/1230] replay: fix string printf output: add .c_str()

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

diff --git a/src/modules/replay/replay_main.cpp b/src/modules/replay/replay_main.cpp
index 4db9d36cc6..795d358446 100644
--- a/src/modules/replay/replay_main.cpp
+++ b/src/modules/replay/replay_main.cpp
@@ -316,7 +316,7 @@ bool Replay::readAndAddSubscription(std::ifstream &file, uint16_t msg_size)
 				timestamp_found = true;
 
 				if (type_name_full != "uint64_t") {
-					PX4_ERR("Unsupported timestamp type %s, ignoring the topic %s", type_name_full,
+					PX4_ERR("Unsupported timestamp type %s, ignoring the topic %s", type_name_full.c_str(),
 						orb_meta->o_name);
 					return true;
 				}

From f7bb43b20b6bef3c6b5b2e41aa82ae25fa12014c Mon Sep 17 00:00:00 2001
From: Julian Oes <julian@oes.ch>
Date: Thu, 7 Jul 2016 16:24:13 +0200
Subject: [PATCH 0945/1230] DriverFramework: update submodule (#5004)

This brings various PRs and fixes.
---
 src/lib/DriverFramework | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/src/lib/DriverFramework b/src/lib/DriverFramework
index d1ab15894a..7c468a60ae 160000
--- a/src/lib/DriverFramework
+++ b/src/lib/DriverFramework
@@ -1 +1 @@
-Subproject commit d1ab15894a147ecd9fce021f17dd591b352e126c
+Subproject commit 7c468a60ae64726fb42989fe552feb21b7978289

From 1631cfdc83a37fb52b2022e74bbc4c96d7157ddc Mon Sep 17 00:00:00 2001
From: Julian Oes <julian@oes.ch>
Date: Thu, 7 Jul 2016 10:07:34 +0200
Subject: [PATCH 0946/1230] Eagle: SDLOG_PRIO_BOOST to raise sdlog2 priority

This should lead to less drops on Snapdragon although it's hard to
verify this.
---
 posix-configs/eagle/flight/mainapp.config | 1 +
 1 file changed, 1 insertion(+)

diff --git a/posix-configs/eagle/flight/mainapp.config b/posix-configs/eagle/flight/mainapp.config
index b7441d911a..b60725fb03 100644
--- a/posix-configs/eagle/flight/mainapp.config
+++ b/posix-configs/eagle/flight/mainapp.config
@@ -2,6 +2,7 @@ uorb start
 muorb start
 sdlog2 start -r 100 -e -t -a -b 200
 param set MAV_BROADCAST 1
+param set SDLOG_PRIO_BOOST 3
 dataman start
 navigator start
 mavlink start -u 14556 -r 1000000

From 5f18f9bbba75025eaff128e7f8778e2b896061dd Mon Sep 17 00:00:00 2001
From: Julian Oes <julian@oes.ch>
Date: Thu, 7 Jul 2016 10:09:06 +0200
Subject: [PATCH 0947/1230] sdlog2: select MIN < MAX bytes to write

Previously, the MAX and MIN were both 512 meaning that usually it would
start writing at > 512 bytes but only write 512 bytes which results in
a 512 bytes write shortly followed by a e.g. 30 bytes write.

Also, performance (measured in missed poll updates) seems slightly
better on Snapdragon with bigger chunks.
---
 src/modules/sdlog2/sdlog2.c | 8 +++++++-
 1 file changed, 7 insertions(+), 1 deletion(-)

diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c
index 1f282d4787..af4cacceed 100644
--- a/src/modules/sdlog2/sdlog2.c
+++ b/src/modules/sdlog2/sdlog2.c
@@ -144,8 +144,14 @@ static bool logwriter_should_exit = false;	/**< Logwriter thread exit flag */
 static const unsigned MAX_NO_LOGFOLDER = 999;	/**< Maximum number of log dirs */
 static const unsigned MAX_NO_LOGFILE = 999;		/**< Maximum number of log files */
 static const int LOG_BUFFER_SIZE_DEFAULT = 8192;
+
+#if defined __PX4_POSIX
+static const int MAX_WRITE_CHUNK = 2048;
+static const int MIN_BYTES_TO_WRITE = 256;
+#else
 static const int MAX_WRITE_CHUNK = 512;
-static const int MIN_BYTES_TO_WRITE = 512;
+static const int MIN_BYTES_TO_WRITE = 256;
+#endif
 
 static bool _extended_logging = false;
 static bool _gpstime_only = false;

From fe91527604ef9cbe473076d50004c7991bedb96e Mon Sep 17 00:00:00 2001
From: Julian Oes <julian@oes.ch>
Date: Thu, 7 Jul 2016 10:11:51 +0200
Subject: [PATCH 0948/1230] sdlog2: poll for sensor and replay on Snappy

This brings better performance, so less missed updates on Snappy, as
well as a bit of a cleanup of the poll and orb_copy logic.
---
 src/modules/sdlog2/sdlog2.c | 141 +++++++++++++++++-------------------
 1 file changed, 68 insertions(+), 73 deletions(-)

diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c
index af4cacceed..6a8891e342 100644
--- a/src/modules/sdlog2/sdlog2.c
+++ b/src/modules/sdlog2/sdlog2.c
@@ -1400,7 +1400,8 @@ int sdlog2_thread_main(int argc, char *argv[])
 	thread_running = true;
 
 	// wakeup source
-	px4_pollfd_struct_t fds[1];
+	px4_pollfd_struct_t fds[2];
+	unsigned px4_pollfd_len = 0;
 
 	int poll_counter = 0;
 
@@ -1412,6 +1413,12 @@ int sdlog2_thread_main(int argc, char *argv[])
 			fds[0].fd = subs.sensor_sub;
 			fds[0].events = POLLIN;
 
+			subs.replay_sub = orb_subscribe(ORB_ID(ekf2_replay));
+			fds[1].fd = subs.replay_sub;
+			fds[1].events = POLLIN;
+
+			px4_pollfd_len = 2;
+
 			poll_to_logging_factor = 1;
 
 			break;
@@ -1422,6 +1429,8 @@ int sdlog2_thread_main(int argc, char *argv[])
 			fds[0].fd = subs.sensor_sub;
 			fds[0].events = POLLIN;
 
+			px4_pollfd_len = 1;
+
 			// TODO Remove hardcoded rate!
 			poll_to_logging_factor = 250 / (log_rate < 1 ? 1 : log_rate);
 
@@ -1433,6 +1442,8 @@ int sdlog2_thread_main(int argc, char *argv[])
 			fds[0].fd = subs.replay_sub;
 			fds[0].events = POLLIN;
 
+			px4_pollfd_len = 1;
+
 			poll_to_logging_factor = 1;
 
 			break;
@@ -1446,7 +1457,7 @@ int sdlog2_thread_main(int argc, char *argv[])
 	while (!main_thread_should_exit) {
 
 		// wait for up to 100ms for data
-		int pret = px4_poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 100);
+		int pret = px4_poll(&fds[0], px4_pollfd_len, 100);
 
 		// timed out - periodic check for _task_should_exit
 		if (pret == 0) {
@@ -1461,24 +1472,6 @@ int sdlog2_thread_main(int argc, char *argv[])
 			continue;
 		}
 
-		if (!fds[0].revents & POLLIN) {
-			continue;
-		}
-
-		switch (log_type) {
-			case LOG_TYPE_ALL:
-				orb_copy(ORB_ID(sensor_combined), subs.sensor_sub, &buf.sensor);
-				break;
-
-			case LOG_TYPE_NORMAL:
-				orb_copy(ORB_ID(sensor_combined), subs.sensor_sub, &buf.sensor);
-				break;
-
-			case LOG_TYPE_REPLAY_ONLY:
-				orb_copy(ORB_ID(ekf2_replay), subs.replay_sub, &buf.replay);
-				break;
-		}
-
 		if ((poll_counter + 1) % poll_to_logging_factor == 0) {
 			poll_counter = 0;
 		} else {
@@ -1539,15 +1532,17 @@ int sdlog2_thread_main(int argc, char *argv[])
 			bool replay_updated = false;
 
 			if (log_type == LOG_TYPE_ALL) {
-				// When logging everything we are polling for sensor_combined, so
-				// we need to use the usual copy_if_updated which includes orb_subscribe.
-				replay_updated = copy_if_updated(ORB_ID(ekf2_replay), &subs.replay_sub, &buf.replay);
 
-			} else {
-				// We poll on the replay topic so we know that it was updated
-				// but we need to copy it again since we are re-using the buffer.
-				orb_copy(ORB_ID(ekf2_replay), subs.replay_sub, &buf.replay);
-				replay_updated = true;
+				if (fds[1].revents & POLLIN) {
+					orb_copy(ORB_ID(ekf2_replay), subs.replay_sub, &buf.replay);
+					replay_updated = true;
+				}
+
+			} else if (log_type == LOG_TYPE_REPLAY_ONLY) {
+				if (fds[0].revents & POLLIN) {
+					orb_copy(ORB_ID(ekf2_replay), subs.replay_sub, &buf.replay);
+					replay_updated = true;
+				}
 			}
 
 			if (replay_updated) {
@@ -1636,60 +1631,60 @@ int sdlog2_thread_main(int argc, char *argv[])
 
 		if (log_type == LOG_TYPE_ALL || log_type == LOG_TYPE_NORMAL) {
 
-			// We poll on sensor combined, so we know it has updated just now
-			// but we need to copy it again because we are re-using the buffer.
-			orb_copy(ORB_ID(sensor_combined), subs.sensor_sub, &buf.sensor);
+			if (fds[0].revents & POLLIN) {
+				orb_copy(ORB_ID(sensor_combined), subs.sensor_sub, &buf.sensor);
 
-			bool write_IMU = false;
-			bool write_SENS = false;
+				bool write_IMU = false;
+				bool write_SENS = false;
 
-			if (buf.sensor.timestamp != gyro_timestamp) {
-				gyro_timestamp = buf.sensor.timestamp;
-				write_IMU = true;
-			}
+				if (buf.sensor.timestamp != gyro_timestamp) {
+					gyro_timestamp = buf.sensor.timestamp;
+					write_IMU = true;
+				}
 
-			if (buf.sensor.timestamp + buf.sensor.accelerometer_timestamp_relative != accelerometer_timestamp) {
-				accelerometer_timestamp = buf.sensor.timestamp + buf.sensor.accelerometer_timestamp_relative;
-				write_IMU = true;
-			}
+				if (buf.sensor.timestamp + buf.sensor.accelerometer_timestamp_relative != accelerometer_timestamp) {
+					accelerometer_timestamp = buf.sensor.timestamp + buf.sensor.accelerometer_timestamp_relative;
+					write_IMU = true;
+				}
 
-			if (buf.sensor.timestamp + buf.sensor.magnetometer_timestamp_relative != magnetometer_timestamp) {
-				magnetometer_timestamp = buf.sensor.timestamp + buf.sensor.magnetometer_timestamp_relative;
-				write_IMU = true;
-			}
+				if (buf.sensor.timestamp + buf.sensor.magnetometer_timestamp_relative != magnetometer_timestamp) {
+					magnetometer_timestamp = buf.sensor.timestamp + buf.sensor.magnetometer_timestamp_relative;
+					write_IMU = true;
+				}
 
-			if (buf.sensor.timestamp + buf.sensor.baro_timestamp_relative != barometer_timestamp) {
-				barometer_timestamp = buf.sensor.timestamp + buf.sensor.baro_timestamp_relative;
-				write_SENS = true;
-			}
+				if (buf.sensor.timestamp + buf.sensor.baro_timestamp_relative != barometer_timestamp) {
+					barometer_timestamp = buf.sensor.timestamp + buf.sensor.baro_timestamp_relative;
+					write_SENS = true;
+				}
 
-			if (write_IMU) {
-				log_msg.msg_type = LOG_IMU_MSG;
+				if (write_IMU) {
+					log_msg.msg_type = LOG_IMU_MSG;
 
-				log_msg.body.log_IMU.gyro_x = buf.sensor.gyro_rad[0];
-				log_msg.body.log_IMU.gyro_y = buf.sensor.gyro_rad[1];
-				log_msg.body.log_IMU.gyro_z = buf.sensor.gyro_rad[2];
-				log_msg.body.log_IMU.acc_x = buf.sensor.accelerometer_m_s2[0];
-				log_msg.body.log_IMU.acc_y = buf.sensor.accelerometer_m_s2[1];
-				log_msg.body.log_IMU.acc_z = buf.sensor.accelerometer_m_s2[2];
-				log_msg.body.log_IMU.mag_x = buf.sensor.magnetometer_ga[0];
-				log_msg.body.log_IMU.mag_y = buf.sensor.magnetometer_ga[1];
-				log_msg.body.log_IMU.mag_z = buf.sensor.magnetometer_ga[2];
-				log_msg.body.log_IMU.temp_gyro = 0;
-				log_msg.body.log_IMU.temp_acc = 0;
-				log_msg.body.log_IMU.temp_mag = 0;
-				LOGBUFFER_WRITE_AND_COUNT(IMU);
-			}
+					log_msg.body.log_IMU.gyro_x = buf.sensor.gyro_rad[0];
+					log_msg.body.log_IMU.gyro_y = buf.sensor.gyro_rad[1];
+					log_msg.body.log_IMU.gyro_z = buf.sensor.gyro_rad[2];
+					log_msg.body.log_IMU.acc_x = buf.sensor.accelerometer_m_s2[0];
+					log_msg.body.log_IMU.acc_y = buf.sensor.accelerometer_m_s2[1];
+					log_msg.body.log_IMU.acc_z = buf.sensor.accelerometer_m_s2[2];
+					log_msg.body.log_IMU.mag_x = buf.sensor.magnetometer_ga[0];
+					log_msg.body.log_IMU.mag_y = buf.sensor.magnetometer_ga[1];
+					log_msg.body.log_IMU.mag_z = buf.sensor.magnetometer_ga[2];
+					log_msg.body.log_IMU.temp_gyro = 0;
+					log_msg.body.log_IMU.temp_acc = 0;
+					log_msg.body.log_IMU.temp_mag = 0;
+					LOGBUFFER_WRITE_AND_COUNT(IMU);
+				}
 
-			if (write_SENS) {
-				log_msg.msg_type = LOG_SENS_MSG;
+				if (write_SENS) {
+					log_msg.msg_type = LOG_SENS_MSG;
 
-				log_msg.body.log_SENS.baro_pres = 0;
-				log_msg.body.log_SENS.baro_alt = buf.sensor.baro_alt_meter;
-				log_msg.body.log_SENS.baro_temp = buf.sensor.baro_temp_celcius;
-				log_msg.body.log_SENS.diff_pres = 0;
-				log_msg.body.log_SENS.diff_pres_filtered = 0;
-				LOGBUFFER_WRITE_AND_COUNT(SENS);
+					log_msg.body.log_SENS.baro_pres = 0;
+					log_msg.body.log_SENS.baro_alt = buf.sensor.baro_alt_meter;
+					log_msg.body.log_SENS.baro_temp = buf.sensor.baro_temp_celcius;
+					log_msg.body.log_SENS.diff_pres = 0;
+					log_msg.body.log_SENS.diff_pres_filtered = 0;
+					LOGBUFFER_WRITE_AND_COUNT(SENS);
+				}
 			}
 
 			/* --- VTOL VEHICLE STATUS --- */

From 772dc302b6c5036b9de5374e4f4e611b1c900724 Mon Sep 17 00:00:00 2001
From: Julian Oes <julian@oes.ch>
Date: Thu, 7 Jul 2016 11:47:08 +0200
Subject: [PATCH 0949/1230] sdlog2: raise min write size back

Turns out in practice there was not really a difference, so there was
no reason to change it.
---
 src/modules/sdlog2/sdlog2.c | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c
index 6a8891e342..be2b3a810f 100644
--- a/src/modules/sdlog2/sdlog2.c
+++ b/src/modules/sdlog2/sdlog2.c
@@ -147,10 +147,10 @@ static const int LOG_BUFFER_SIZE_DEFAULT = 8192;
 
 #if defined __PX4_POSIX
 static const int MAX_WRITE_CHUNK = 2048;
-static const int MIN_BYTES_TO_WRITE = 256;
+static const int MIN_BYTES_TO_WRITE = 512;
 #else
 static const int MAX_WRITE_CHUNK = 512;
-static const int MIN_BYTES_TO_WRITE = 256;
+static const int MIN_BYTES_TO_WRITE = 512;
 #endif
 
 static bool _extended_logging = false;

From 03dbcf54642144e76fe265432e979b7848474de1 Mon Sep 17 00:00:00 2001
From: Julian Oes <julian@oes.ch>
Date: Thu, 7 Jul 2016 12:28:05 +0200
Subject: [PATCH 0950/1230] sdlog2: don't forget to copy after poll

The previous changes broke sdlog2 on NuttX because no orb_copy was
called after polling in the case when sdlog2 was not actually logging.
---
 src/modules/sdlog2/sdlog2.c | 74 +++++++++++++++++++++++++------------
 1 file changed, 51 insertions(+), 23 deletions(-)

diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c
index be2b3a810f..4d9ede54c7 100644
--- a/src/modules/sdlog2/sdlog2.c
+++ b/src/modules/sdlog2/sdlog2.c
@@ -1456,29 +1456,9 @@ int sdlog2_thread_main(int argc, char *argv[])
 
 	while (!main_thread_should_exit) {
 
-		// wait for up to 100ms for data
-		int pret = px4_poll(&fds[0], px4_pollfd_len, 100);
-
-		// timed out - periodic check for _task_should_exit
-		if (pret == 0) {
-			continue;
-		}
-
-		// this is undesirable but not much we can do - might want to flag unhappy status
-		if (pret < 0) {
-			PX4_WARN("poll error %d, %d", pret, errno);
-			// sleep a bit before next try
-			usleep(100000);
-			continue;
-		}
-
-		if ((poll_counter + 1) % poll_to_logging_factor == 0) {
-			poll_counter = 0;
-		} else {
-			// copy topic
-			poll_counter++;
-			continue;
-		}
+		/* Check below's topics first even if logging is not enabled.
+		 * We need to do this because should only poll further below if we're
+		 * actually going to orb_copy the data after the poll. */
 
 		/* --- VEHICLE COMMAND - LOG MANAGEMENT --- */
 		if (copy_if_updated(ORB_ID(vehicle_command), &subs.cmd_sub, &buf_cmd)) {
@@ -1503,6 +1483,54 @@ int sdlog2_thread_main(int argc, char *argv[])
 		}
 
 		if (!logging_enabled) {
+			usleep(50000);
+			continue;
+		}
+
+		// wait for up to 100ms for data
+		int pret = px4_poll(&fds[0], px4_pollfd_len, 100);
+
+		// timed out - periodic check for _task_should_exit
+		if (pret == 0) {
+			continue;
+		}
+
+		// this is undesirable but not much we can do - might want to flag unhappy status
+		if (pret < 0) {
+			PX4_WARN("poll error %d, %d", pret, errno);
+			// sleep a bit before next try
+			usleep(100000);
+			continue;
+		}
+
+		if (poll_counter++ >= poll_to_logging_factor) {
+			poll_counter = 0;
+		} else {
+
+			/* In this case, we still need to do orb_copy, otherwise we'll stall. */
+			switch (log_type) {
+				case LOG_TYPE_ALL:
+					if (fds[0].revents & POLLIN) {
+						orb_copy(ORB_ID(sensor_combined), subs.sensor_sub, &buf.sensor);
+					}
+
+					if (fds[1].revents & POLLIN) {
+						orb_copy(ORB_ID(ekf2_replay), subs.replay_sub, &buf.replay);
+					}
+					break;
+
+				case LOG_TYPE_NORMAL:
+					if (fds[0].revents & POLLIN) {
+						orb_copy(ORB_ID(sensor_combined), subs.sensor_sub, &buf.sensor);
+					}
+					break;
+
+				case LOG_TYPE_REPLAY_ONLY:
+					if (fds[0].revents & POLLIN) {
+						orb_copy(ORB_ID(ekf2_replay), subs.replay_sub, &buf.replay);
+					}
+					break;
+			}
 			continue;
 		}
 

From 544ea72d4c789eac77198b7d54b35db88ff54eeb Mon Sep 17 00:00:00 2001
From: Julian Oes <julian@oes.ch>
Date: Thu, 7 Jul 2016 13:46:12 +0200
Subject: [PATCH 0951/1230] Snapdragon: set CPUs scaling to performance mode

Sdlog2 misses least updates when the CPU scaling governor is set at
maximum performance. This is not optimal to save power but the best
effort until there is a RT patched kernel on Snapdragon.
---
 src/platforms/posix/main.cpp | 17 +++++++++++++++++
 1 file changed, 17 insertions(+)

diff --git a/src/platforms/posix/main.cpp b/src/platforms/posix/main.cpp
index ccb9aaf494..34c879419f 100644
--- a/src/platforms/posix/main.cpp
+++ b/src/platforms/posix/main.cpp
@@ -162,6 +162,21 @@ bool px4_exit_requested(void)
 	return _ExitFlag;
 }
 
+static void set_cpu_scaling()
+{
+#ifdef __PX4_POSIX_EAGLE
+	// On Snapdragon we miss updates in sdlog2 unless all 4 CPUs are run
+	// at the maximum frequency all the time.
+	// Interestingely, cpu0 and cpu3 set the scaling for all 4 CPUs on Snapdragon.
+	system("echo performance > /sys/devices/system/cpu/cpu0/cpufreq/scaling_governor");
+	system("echo performance > /sys/devices/system/cpu/cpu3/cpufreq/scaling_governor");
+
+	// Alternatively we could also raise the minimum frequency to save some power,
+	// unfortunately this still lead to some drops.
+	//system("echo 1190400 > /sys/devices/system/cpu/cpu0/cpufreq/scaling_min_freq");
+#endif
+}
+
 int main(int argc, char **argv)
 {
 	bool daemon_mode = false;
@@ -184,6 +199,8 @@ int main(int argc, char **argv)
 	//sigaction(SIGTERM, &sig_int, NULL);
 	sigaction(SIGFPE, &sig_fpe, NULL);
 
+	set_cpu_scaling();
+
 	int index = 1;
 	char *commands_file = nullptr;
 

From ddba274496c687beba05c101d5cfcbe97450d0fc Mon Sep 17 00:00:00 2001
From: Julian Oes <julian@oes.ch>
Date: Thu, 7 Jul 2016 13:49:35 +0200
Subject: [PATCH 0952/1230] sdlog2: fix poll_counter that I broke earlier

---
 src/modules/sdlog2/sdlog2.c | 3 ++-
 1 file changed, 2 insertions(+), 1 deletion(-)

diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c
index 4d9ede54c7..7310db1341 100644
--- a/src/modules/sdlog2/sdlog2.c
+++ b/src/modules/sdlog2/sdlog2.c
@@ -1503,7 +1503,7 @@ int sdlog2_thread_main(int argc, char *argv[])
 			continue;
 		}
 
-		if (poll_counter++ >= poll_to_logging_factor) {
+		if ((poll_counter+1) >= poll_to_logging_factor) {
 			poll_counter = 0;
 		} else {
 
@@ -1531,6 +1531,7 @@ int sdlog2_thread_main(int argc, char *argv[])
 					}
 					break;
 			}
+			poll_counter++;
 			continue;
 		}
 

From e86c1851a7c4a4dedafbb54550634cc42f02fde1 Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Thu, 7 Jul 2016 16:49:28 +0200
Subject: [PATCH 0953/1230] Mag fusion update to ECL

---
 src/lib/ecl | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/src/lib/ecl b/src/lib/ecl
index 5e0a229ee8..a2d866520a 160000
--- a/src/lib/ecl
+++ b/src/lib/ecl
@@ -1 +1 @@
-Subproject commit 5e0a229ee8bf09430051090f58f71c95fa4755f0
+Subproject commit a2d866520a0477574d0ade68b0f5e7becb57431b

From 23175899505ae1065b08bfb4e33579c95545d922 Mon Sep 17 00:00:00 2001
From: Andreas Bircher <bircher@gmx.ch>
Date: Thu, 7 Jul 2016 16:50:46 +0200
Subject: [PATCH 0954/1230] fixing the publishing of the triggering stamps
 (#5005)

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

diff --git a/src/drivers/camera_trigger/camera_trigger.cpp b/src/drivers/camera_trigger/camera_trigger.cpp
index ecd7282a28..37299cb312 100644
--- a/src/drivers/camera_trigger/camera_trigger.cpp
+++ b/src/drivers/camera_trigger/camera_trigger.cpp
@@ -493,7 +493,7 @@ CameraTrigger::engage(void *arg)
 
 	report.seq = trig->_trigger_seq++;
 
-	orb_publish(ORB_ID(camera_trigger), &(trig->_trigger_pub), &report);
+	orb_publish(ORB_ID(camera_trigger), trig->_trigger_pub, &report);
 }
 
 void

From 3c11c0d8d86fd4c61faf079a16710222aeed32af Mon Sep 17 00:00:00 2001
From: Miguel Arroyo <mayanez@users.noreply.github.com>
Date: Thu, 7 Jul 2016 16:38:17 -0400
Subject: [PATCH 0955/1230] Adds Calibration Support for RPi2 and Navio2
 (#4999)

---
 .../commander/accelerometer_calibration.cpp        |  8 ++++----
 src/modules/commander/gyro_calibration.cpp         |  6 +++---
 src/modules/commander/mag_calibration.cpp          | 14 +++++++-------
 3 files changed, 14 insertions(+), 14 deletions(-)

diff --git a/src/modules/commander/accelerometer_calibration.cpp b/src/modules/commander/accelerometer_calibration.cpp
index 6384d415aa..bdb18eaab9 100644
--- a/src/modules/commander/accelerometer_calibration.cpp
+++ b/src/modules/commander/accelerometer_calibration.cpp
@@ -175,7 +175,7 @@ typedef struct  {
 
 int do_accel_calibration(orb_advert_t *mavlink_log_pub)
 {
-#if !defined(__PX4_QURT) && !defined(__PX4_POSIX_EAGLE)
+#if !defined(__PX4_QURT) && !defined(__PX4_POSIX_EAGLE) && !defined(__PX4_POSIX_RPI2)
 	int fd;
 #endif
 
@@ -195,7 +195,7 @@ int do_accel_calibration(orb_advert_t *mavlink_log_pub)
 
 	/* reset all sensors */
 	for (unsigned s = 0; s < max_accel_sens; s++) {
-#if !defined(__PX4_QURT) && !defined(__PX4_POSIX_EAGLE)
+#if !defined(__PX4_QURT) && !defined(__PX4_POSIX_EAGLE) && !defined(__PX4_POSIX_RPI2)
 		sprintf(str, "%s%u", ACCEL_BASE_DEVICE_PATH, s);
 		/* reset all offsets to zero and all scales to one */
 		fd = px4_open(str, 0);
@@ -329,7 +329,7 @@ int do_accel_calibration(orb_advert_t *mavlink_log_pub)
 			return ERROR;
 		}
 
-#if !defined(__PX4_QURT) && !defined(__PX4_POSIX_EAGLE)
+#if !defined(__PX4_QURT) && !defined(__PX4_POSIX_EAGLE) && !defined(__PX4_POSIX_RPI2)
 		sprintf(str, "%s%u", ACCEL_BASE_DEVICE_PATH, i);
 		fd = px4_open(str, 0);
 
@@ -419,7 +419,7 @@ calibrate_return do_accel_calibration_measurements(orb_advert_t *mavlink_log_pub
 			break;
 		}
 
-#if defined(__PX4_QURT) || defined(__PX4_POSIX_EAGLE)
+#if defined(__PX4_QURT) || defined(__PX4_POSIX_EAGLE) || defined(__PX4_POSIX_RPI2)
 		// For QURT respectively the driver framework, we need to get the device ID by copying one report.
 		struct accel_report	accel_report;
 		orb_copy(ORB_ID(sensor_accel), worker_data.subs[i], &accel_report);
diff --git a/src/modules/commander/gyro_calibration.cpp b/src/modules/commander/gyro_calibration.cpp
index a9b005c0f7..3b0e6815df 100644
--- a/src/modules/commander/gyro_calibration.cpp
+++ b/src/modules/commander/gyro_calibration.cpp
@@ -186,7 +186,7 @@ int do_gyro_calibration(orb_advert_t *mavlink_log_pub)
 
 		// Reset all offsets to 0 and scales to 1
 		(void)memcpy(&worker_data.gyro_scale[s], &gyro_scale_zero, sizeof(gyro_scale_zero));
-#if !defined(__PX4_QURT) && !defined(__PX4_POSIX_EAGLE)
+#if !defined(__PX4_QURT) && !defined(__PX4_POSIX_EAGLE) && !defined(__PX4_POSIX_RPI2)
 		sprintf(str, "%s%u", GYRO_BASE_DEVICE_PATH, s);
 		int fd = px4_open(str, 0);
 		if (fd >= 0) {
@@ -239,7 +239,7 @@ int do_gyro_calibration(orb_advert_t *mavlink_log_pub)
 	for (unsigned s = 0; s < gyro_count; s++) {
 
 		worker_data.gyro_sensor_sub[s] = orb_subscribe_multi(ORB_ID(sensor_gyro), s);
-#if defined(__PX4_QURT) || defined(__PX4_POSIX_EAGLE)
+#if defined(__PX4_QURT) || defined(__PX4_POSIX_EAGLE) || defined(__PX4_POSIX_RPI2)
 		// For QURT respectively the driver framework, we need to get the device ID by copying one report.
 		struct gyro_report gyro_report;
 		orb_copy(ORB_ID(sensor_gyro), worker_data.gyro_sensor_sub[s], &gyro_report);
@@ -337,7 +337,7 @@ int do_gyro_calibration(orb_advert_t *mavlink_log_pub)
 				(void)sprintf(str, "CAL_GYRO%u_ID", s);
 				failed |= (OK != param_set_no_notification(param_find(str), &(worker_data.device_id[s])));
 
-#if !defined(__PX4_QURT) && !defined(__PX4_POSIX_EAGLE)
+#if !defined(__PX4_QURT) && !defined(__PX4_POSIX_EAGLE) && !defined(__PX4_POSIX_RPI2)
 				/* apply new scaling and offsets */
 				(void)sprintf(str, "%s%u", GYRO_BASE_DEVICE_PATH, s);
 				int fd = px4_open(str, 0);
diff --git a/src/modules/commander/mag_calibration.cpp b/src/modules/commander/mag_calibration.cpp
index 8f589fb2c7..4d02260b40 100644
--- a/src/modules/commander/mag_calibration.cpp
+++ b/src/modules/commander/mag_calibration.cpp
@@ -124,7 +124,7 @@ int do_mag_calibration(orb_advert_t *mavlink_log_pub)
 	_last_mag_progress = 0;
 
 	for (unsigned cur_mag = 0; cur_mag < max_mags; cur_mag++) {
-#ifndef __PX4_QURT
+#if !defined(__PX4_QURT) && !defined(__PX4_POSIX_RPI2)
 		// Reset mag id to mag not available
 		(void)sprintf(str, "CAL_MAG%u_ID", cur_mag);
 		result = param_set_no_notification(param_find(str), &(device_ids[cur_mag]));
@@ -166,7 +166,7 @@ int do_mag_calibration(orb_advert_t *mavlink_log_pub)
 #endif
 
 /* for calibration, commander will run on apps, so orb messages are used to get info from dsp */
-#ifndef __PX4_QURT
+#if !defined(__PX4_QURT) && !defined(__PX4_POSIX_RPI2)
 		// Attempt to open mag
 		(void)sprintf(str, "%s%u", MAG_BASE_DEVICE_PATH, cur_mag);
 		int fd = px4_open(str, O_RDONLY);
@@ -508,7 +508,7 @@ calibrate_return mag_calibrate_all(orb_advert_t *mavlink_log_pub)
 			// Mag in this slot is available
 			worker_data.sub_mag[cur_mag] = orb_subscribe_multi(ORB_ID(sensor_mag), cur_mag);
 
-#ifdef __PX4_QURT
+#if defined(__PX4_QURT) || defined(__PX4_POSIX_RPI2)
 			// For QURT respectively the driver framework, we need to get the device ID by copying one report.
 			struct mag_report	mag_report;
 			orb_copy(ORB_ID(sensor_mag), worker_data.sub_mag[cur_mag], &mag_report);
@@ -664,7 +664,7 @@ calibrate_return mag_calibrate_all(orb_advert_t *mavlink_log_pub)
 		for (unsigned cur_mag=0; cur_mag<max_mags; cur_mag++) {
 			if (device_ids[cur_mag] != 0) {
 				struct mag_calibration_s mscale;
-#ifndef __PX4_QURT
+#if !defined(__PX4_QURT) && !defined(__PX4_POSIX_RPI2)
 				int fd_mag = -1;
 
 				// Set new scale
@@ -688,7 +688,7 @@ calibrate_return mag_calibrate_all(orb_advert_t *mavlink_log_pub)
 					mscale.y_offset = sphere_y[cur_mag];
 					mscale.z_offset = sphere_z[cur_mag];
 
-#ifndef __PX4_QURT
+#if !defined(__PX4_QURT) && !defined(__PX4_POSIX_RPI2)
 					if (px4_ioctl(fd_mag, MAGIOCSSCALE, (long unsigned int)&mscale) != OK) {
 						calibration_log_critical(mavlink_log_pub, CAL_ERROR_APPLY_CAL_MSG, cur_mag);
 						result = calibrate_return_error;
@@ -696,7 +696,7 @@ calibrate_return mag_calibrate_all(orb_advert_t *mavlink_log_pub)
 #endif
 				}
 
-#ifndef __PX4_QURT
+#if !defined(__PX4_QURT) && !defined(__PX4_POSIX_RPI2)
 				// Mag device no longer needed
 				if (fd_mag >= 0) {
 					px4_close(fd_mag);
@@ -716,10 +716,10 @@ calibrate_return mag_calibrate_all(orb_advert_t *mavlink_log_pub)
 					failed |= (OK != param_set_no_notification(param_find(str), &(mscale.y_offset)));
 					(void)sprintf(str, "CAL_MAG%u_ZOFF", cur_mag);
 					failed |= (OK != param_set_no_notification(param_find(str), &(mscale.z_offset)));
-					(void)sprintf(str, "CAL_MAG%u_XSCALE", cur_mag);
 
 					// FIXME: scaling is not used right now on QURT
 #ifndef __PX4_QURT
+					(void)sprintf(str, "CAL_MAG%u_XSCALE", cur_mag);
 					failed |= (OK != param_set_no_notification(param_find(str), &(mscale.x_scale)));
 					(void)sprintf(str, "CAL_MAG%u_YSCALE", cur_mag);
 					failed |= (OK != param_set_no_notification(param_find(str), &(mscale.y_scale)));

From 38652ec9405b6b634780ad21787c97d022e2a386 Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Thu, 7 Jul 2016 23:28:21 +0200
Subject: [PATCH 0956/1230] SITL: Re-tune multicopter gains

---
 posix-configs/SITL/init/rcS_gazebo_iris          | 12 +++++-------
 posix-configs/SITL/init/rcS_gazebo_solo          | 14 +++++---------
 posix-configs/SITL/init/rcS_gazebo_tailsitter    | 10 ++++------
 posix-configs/SITL/init/rcS_gazebo_typhoon_h480  | 12 +++++-------
 posix-configs/SITL/init/rcS_jmavsim_iris         | 10 ++++------
 posix-configs/SITL/init/rcS_lpe_jmavsim_iris     | 10 ++++------
 posix-configs/SITL/init/rcS_multiple_gazebo_iris | 10 ++++------
 7 files changed, 31 insertions(+), 47 deletions(-)

diff --git a/posix-configs/SITL/init/rcS_gazebo_iris b/posix-configs/SITL/init/rcS_gazebo_iris
index b84a93ee4a..76c19a17cd 100644
--- a/posix-configs/SITL/init/rcS_gazebo_iris
+++ b/posix-configs/SITL/init/rcS_gazebo_iris
@@ -31,15 +31,13 @@ param set RTL_RETURN_ALT 30.0
 param set RTL_DESCEND_ALT 5.0
 param set RTL_LAND_DELAY 5
 param set MIS_TAKEOFF_ALT 2.5
-param set MC_ROLLRATE_P 0.25
-param set MC_PITCHRATE_P 0.25
-param set MC_PITCH_P 5.5
-param set MC_ROLL_P 5.5
-param set MC_ROLLRATE_I 0.1
-param set MC_PITCHRATE_I 0.1
+param set MC_ROLLRATE_P 0.2
+param set MC_PITCHRATE_P 0.2
+param set MC_PITCH_P 6
+param set MC_ROLL_P 6
 param set MPC_HOLD_MAX_Z 2.0
 param set MPC_HOLD_XY_DZ 0.1
-param set MPC_Z_VEL_P 0.8
+param set MPC_Z_VEL_P 0.6
 param set MPC_Z_VEL_I 0.15
 param set MPC_XY_VEL_P 0.15
 param set MPC_XY_VEL_I 0.2
diff --git a/posix-configs/SITL/init/rcS_gazebo_solo b/posix-configs/SITL/init/rcS_gazebo_solo
index 8d9d09bf99..62fc557bfa 100644
--- a/posix-configs/SITL/init/rcS_gazebo_solo
+++ b/posix-configs/SITL/init/rcS_gazebo_solo
@@ -28,18 +28,14 @@ param set RTL_RETURN_ALT 30.0
 param set RTL_DESCEND_ALT 10.0
 param set RTL_LAND_DELAY 0
 param set MIS_TAKEOFF_ALT 2.5
-param set MC_ROLLRATE_P 0.25
-param set MC_PITCHRATE_P 0.25
-param set MC_PITCH_P 5.5
-param set MC_ROLL_P 5.5
-param set MC_ROLLRATE_I 0.1
-param set MC_PITCHRATE_I 0.1
+param set MC_ROLLRATE_P 0.2
+param set MC_PITCHRATE_P 0.2
+param set MC_PITCH_P 6
+param set MC_ROLL_P 6
 param set MPC_HOLD_MAX_Z 2.0
 param set MPC_HOLD_XY_DZ 0.1
-param set MPC_Z_VEL_P 0.8
+param set MPC_Z_VEL_P 0.6
 param set MPC_Z_VEL_I 0.15
-param set MPC_XY_VEL_P 0.15
-param set MPC_XY_VEL_I 0.2
 param set EKF2_GBIAS_INIT 0.01
 param set EKF2_ANGERR_INIT 0.01
 simulator start -s
diff --git a/posix-configs/SITL/init/rcS_gazebo_tailsitter b/posix-configs/SITL/init/rcS_gazebo_tailsitter
index 0882d895a0..663be85607 100644
--- a/posix-configs/SITL/init/rcS_gazebo_tailsitter
+++ b/posix-configs/SITL/init/rcS_gazebo_tailsitter
@@ -19,12 +19,10 @@ param set CAL_ACC0_YSCALE 1.01
 param set CAL_ACC0_ZSCALE 1.01
 param set CAL_ACC1_XOFF 0.01
 param set CAL_MAG0_XOFF 0.01
-param set MC_ROLLRATE_P 0.25
-param set MC_PITCHRATE_P 0.25
-param set MC_PITCH_P 5.5
-param set MC_ROLL_P 5.5
-param set MC_ROLLRATE_I 0.1
-param set MC_PITCHRATE_I 0.1
+param set MC_ROLLRATE_P 0.2
+param set MC_PITCHRATE_P 0.2
+param set MC_PITCH_P 6
+param set MC_ROLL_P 6
 param set MPC_XY_P 0.15
 param set MPC_XY_VEL_P 0.05
 param set MPC_XY_VEL_D 0.005
diff --git a/posix-configs/SITL/init/rcS_gazebo_typhoon_h480 b/posix-configs/SITL/init/rcS_gazebo_typhoon_h480
index a122924f6c..5fd7c5f5ac 100644
--- a/posix-configs/SITL/init/rcS_gazebo_typhoon_h480
+++ b/posix-configs/SITL/init/rcS_gazebo_typhoon_h480
@@ -28,15 +28,13 @@ param set RTL_RETURN_ALT 30.0
 param set RTL_DESCEND_ALT 10.0
 param set RTL_LAND_DELAY 0
 param set MIS_TAKEOFF_ALT 2.5
-param set MC_ROLLRATE_P 0.25
-param set MC_PITCHRATE_P 0.25
-param set MC_PITCH_P 5.5
-param set MC_ROLL_P 5.5
-param set MC_ROLLRATE_I 0.1
-param set MC_PITCHRATE_I 0.1
+param set MC_ROLLRATE_P 0.2
+param set MC_PITCHRATE_P 0.2
+param set MC_PITCH_P 6
+param set MC_ROLL_P 6
 param set MPC_HOLD_MAX_Z 2.0
 param set MPC_HOLD_XY_DZ 0.1
-param set MPC_Z_VEL_P 0.8
+param set MPC_Z_VEL_P 0.6
 param set MPC_Z_VEL_I 0.15
 param set MPC_XY_VEL_P 0.15
 param set MPC_XY_VEL_I 0.2
diff --git a/posix-configs/SITL/init/rcS_jmavsim_iris b/posix-configs/SITL/init/rcS_jmavsim_iris
index d48ecbf394..2e90404619 100644
--- a/posix-configs/SITL/init/rcS_jmavsim_iris
+++ b/posix-configs/SITL/init/rcS_jmavsim_iris
@@ -31,12 +31,10 @@ param set RTL_RETURN_ALT 30.0
 param set RTL_DESCEND_ALT 10.0
 param set RTL_LAND_DELAY 0
 param set MIS_TAKEOFF_ALT 2.5
-param set MC_ROLLRATE_P 0.3
-param set MC_PITCHRATE_P 0.3
-param set MC_PITCH_P 7
-param set MC_ROLL_P 7
-param set MC_ROLLRATE_I 0.1
-param set MC_PITCHRATE_I 0.1
+param set MC_ROLLRATE_P 0.2
+param set MC_PITCHRATE_P 0.2
+param set MC_PITCH_P 6
+param set MC_ROLL_P 6
 param set MPC_HOLD_MAX_Z 2.0
 param set MPC_HOLD_XY_DZ 0.1
 param set MPC_Z_VEL_MAX 2.0
diff --git a/posix-configs/SITL/init/rcS_lpe_jmavsim_iris b/posix-configs/SITL/init/rcS_lpe_jmavsim_iris
index fa8d9bca2a..6192b2e434 100644
--- a/posix-configs/SITL/init/rcS_lpe_jmavsim_iris
+++ b/posix-configs/SITL/init/rcS_lpe_jmavsim_iris
@@ -18,12 +18,10 @@ param set CAL_ACC0_YSCALE 1.01
 param set CAL_ACC0_ZSCALE 1.01
 param set CAL_ACC1_XOFF 0.01
 param set CAL_MAG0_XOFF 0.01
-param set MC_ROLLRATE_P 0.3
-param set MC_PITCHRATE_P 0.3
-param set MC_PITCH_P 5.5
-param set MC_ROLL_P 5.5
-param set MC_ROLLRATE_I 0.1
-param set MC_PITCHRATE_I 0.1
+param set MC_ROLLRATE_P 0.2
+param set MC_PITCHRATE_P 0.2
+param set MC_PITCH_P 6
+param set MC_ROLL_P 6
 param set MPC_XY_P 0.4
 param set MPC_XY_VEL_P 0.2
 param set MPC_XY_VEL_D 0.005
diff --git a/posix-configs/SITL/init/rcS_multiple_gazebo_iris b/posix-configs/SITL/init/rcS_multiple_gazebo_iris
index fb4d23d9b2..e6de00cff9 100644
--- a/posix-configs/SITL/init/rcS_multiple_gazebo_iris
+++ b/posix-configs/SITL/init/rcS_multiple_gazebo_iris
@@ -19,12 +19,10 @@ param set CAL_ACC0_YSCALE 1.01
 param set CAL_ACC0_ZSCALE 1.01
 param set CAL_ACC1_XOFF 0.01
 param set CAL_MAG0_XOFF 0.01
-param set MC_ROLLRATE_P 0.3
-param set MC_PITCHRATE_P 0.3
-param set MC_PITCH_P 5.5
-param set MC_ROLL_P 5.5
-param set MC_ROLLRATE_I 0.1
-param set MC_PITCHRATE_I 0.1
+param set MC_ROLLRATE_P 0.2
+param set MC_PITCHRATE_P 0.2
+param set MC_PITCH_P 6
+param set MC_ROLL_P 6
 param set MPC_XY_P 0.15
 param set MPC_XY_VEL_P 0.05
 param set MPC_XY_VEL_D 0.005

From eb6af9afb9b697e49e048265646a88ed2dff3087 Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Thu, 7 Jul 2016 23:30:57 +0200
Subject: [PATCH 0957/1230] VTOL updates

---
 posix-configs/SITL/init/rcS_gazebo_standard_vtol | 12 +++++-------
 1 file changed, 5 insertions(+), 7 deletions(-)

diff --git a/posix-configs/SITL/init/rcS_gazebo_standard_vtol b/posix-configs/SITL/init/rcS_gazebo_standard_vtol
index 454071fc4d..3473324e71 100644
--- a/posix-configs/SITL/init/rcS_gazebo_standard_vtol
+++ b/posix-configs/SITL/init/rcS_gazebo_standard_vtol
@@ -19,18 +19,16 @@ param set CAL_ACC0_YSCALE 1.01
 param set CAL_ACC0_ZSCALE 1.01
 param set CAL_ACC1_XOFF 0.01
 param set CAL_MAG0_XOFF 0.01
-param set MC_ROLLRATE_P 0.25
-param set MC_PITCHRATE_P 0.25
-param set MC_PITCH_P 5.5
-param set MC_ROLL_P 5.5
-param set MC_ROLLRATE_I 0.1
-param set MC_PITCHRATE_I 0.1
+param set MC_ROLLRATE_P 0.2
+param set MC_PITCHRATE_P 0.2
+param set MC_PITCH_P 6
+param set MC_ROLL_P 6
 param set MPC_XY_P 0.15
 param set MPC_XY_VEL_P 0.05
 param set MPC_XY_VEL_D 0.005
 param set MPC_XY_FF 0.1
 param set MPC_Z_VEL_MAX 1.5
-param set MPC_Z_VEL_P 0.8
+param set MPC_Z_VEL_P 0.6
 param set MPC_Z_VEL_I 0.15
 param set SENS_BOARD_ROT 8
 param set SENS_DPRES_OFF 0.001

From 7afe2e8dd84c2dba8c70619829351e179d145f22 Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Fri, 8 Jul 2016 00:02:48 +0200
Subject: [PATCH 0958/1230] Iris SITL: Default gains

---
 posix-configs/SITL/init/rcS_gazebo_iris | 2 --
 1 file changed, 2 deletions(-)

diff --git a/posix-configs/SITL/init/rcS_gazebo_iris b/posix-configs/SITL/init/rcS_gazebo_iris
index 76c19a17cd..e0be74519c 100644
--- a/posix-configs/SITL/init/rcS_gazebo_iris
+++ b/posix-configs/SITL/init/rcS_gazebo_iris
@@ -39,8 +39,6 @@ param set MPC_HOLD_MAX_Z 2.0
 param set MPC_HOLD_XY_DZ 0.1
 param set MPC_Z_VEL_P 0.6
 param set MPC_Z_VEL_I 0.15
-param set MPC_XY_VEL_P 0.15
-param set MPC_XY_VEL_I 0.2
 param set EKF2_GBIAS_INIT 0.01
 param set EKF2_ANGERR_INIT 0.01
 simulator start -s

From 09227526b62535ed913438c4d04f881717311640 Mon Sep 17 00:00:00 2001
From: Mark Whitehorn <kd0aij@gmail.com>
Date: Sun, 10 Jul 2016 03:15:27 -0600
Subject: [PATCH 0959/1230] enable use of GPS time for new logger folder/file
 naming (#5010)

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

diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS
index 30fbf5856f..4f101e89f7 100644
--- a/ROMFS/px4fmu_common/init.d/rcS
+++ b/ROMFS/px4fmu_common/init.d/rcS
@@ -648,7 +648,7 @@ then
 				then
 				fi
 			else
-				if logger start -b 12
+				if logger start -b 12 -t
 				then
 				fi
 			fi

From 5de19500c2e0cb0acb20ab36ce26372862f10de9 Mon Sep 17 00:00:00 2001
From: David Sidrane <david_s5@nscdg.com>
Date: Fri, 8 Jul 2016 12:15:20 -1000
Subject: [PATCH 0960/1230] TAP-v1 set Console Baud Rate to 57600

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

diff --git a/nuttx-configs/tap-v1/nsh/defconfig b/nuttx-configs/tap-v1/nsh/defconfig
index c55ddf53cd..5e095d7b4c 100644
--- a/nuttx-configs/tap-v1/nsh/defconfig
+++ b/nuttx-configs/tap-v1/nsh/defconfig
@@ -513,7 +513,7 @@ CONFIG_USART2_2STOP=0
 #
 CONFIG_USART3_RXBUFSIZE=256
 CONFIG_USART3_TXBUFSIZE=256
-CONFIG_USART3_BAUD=115200
+CONFIG_USART3_BAUD=57600
 CONFIG_USART3_BITS=8
 CONFIG_USART3_PARITY=0
 CONFIG_USART3_2STOP=0

From da96144e80acd57266188464c0591dd19390eddc Mon Sep 17 00:00:00 2001
From: David Sidrane <david_s5@nscdg.com>
Date: Fri, 8 Jul 2016 12:17:34 -1000
Subject: [PATCH 0961/1230] Added Power button and cleanup sdio

---
 nuttx-configs/tap-v1/include/board.h     |  32 ++---
 src/drivers/boards/tap-v1/CMakeLists.txt |   2 +
 src/drivers/boards/tap-v1/board_config.h |  53 ++++++-
 src/drivers/boards/tap-v1/tap_init.c     |  24 +---
 src/drivers/boards/tap-v1/tap_led.c      |  14 +-
 src/drivers/boards/tap-v1/tap_pwr.c      | 167 +++++++++++++++++++++++
 src/drivers/boards/tap-v1/tap_sdio.c     | 122 +++++++++++++++++
 src/drivers/boards/tap-v1/tap_spi.c      |   2 +-
 8 files changed, 359 insertions(+), 57 deletions(-)
 create mode 100644 src/drivers/boards/tap-v1/tap_pwr.c
 create mode 100644 src/drivers/boards/tap-v1/tap_sdio.c

diff --git a/nuttx-configs/tap-v1/include/board.h b/nuttx-configs/tap-v1/include/board.h
index 8df4099124..5c7467f9d0 100644
--- a/nuttx-configs/tap-v1/include/board.h
+++ b/nuttx-configs/tap-v1/include/board.h
@@ -167,7 +167,7 @@
  * PC4     BLUE_LED                  D4 Blue LED cathode
  * PC5     RED_LED                   D5 Red LED cathode
 */
-#define BOARD_RED         0
+#define BOARD_LED1        0
 #define BOARD_LED2        1
 #define BOARD_NLEDS       2
 
@@ -336,13 +336,13 @@
 
 /*
  * GPIO
- * todo:Revisit - cannnot tell from schematic - some could be ADC
+ *
  * Port     Signal Name               CONN
- * PA4     POWER                     JP1-23,24
- * PB4     TEMP_CONT                 J2-2,11,14,23 MPU6050 TEMP
- * PC0     VOLTAGE                   JP2-13,14
- * PC1     KEY_AD                    JP1-31,32
- * PC2     SD_SW                     SD-9 SW
+ * PA4     POWER                     JP1-23,            - Must be held High to run w/o USB
+ * PB4     TEMP_CONT                 J2-2,11,14,23      - Gyro Heater
+ * PC0     VOLTAGE                   JP2-13,14          - 1.84 @16.66  1.67 @15.12 Scale 0.1105
+ * PC1     KEY_AD                    JP1-31,32          - Low when Power button is depressed
+ * PC2     SD_SW                     SD-9 SW            - Card Present
  * PC3     PCON_RADIO                JP1-29,30
  * PC13    S2                        U8-9 74HCT151
  * PC14    S1                        U8-10 74HCT151
@@ -416,29 +416,13 @@ extern "C" {
  *
  * Description:
  *   All STM32 architectures must provide the following entry point.  This entry point
- *   is called early in the intitialization -- after all memory has been configured
+ *   is called early in the initialization -- after all memory has been configured
  *   and mapped but before any devices have been initialized.
  *
  ************************************************************************************/
 
 EXTERN void stm32_boardinitialize(void);
 
-/************************************************************************************
- * Name:  stm32_ledinit, stm32_setled, and stm32_setleds
- *
- * Description:
- *   If CONFIG_ARCH_LEDS is defined, then NuttX will control the on-board LEDs.  If
- *   CONFIG_ARCH_LEDS is not defined, then the following interfacesare available to
- *   control the LEDs from user applications.
- *
- ************************************************************************************/
-
-#ifndef CONFIG_ARCH_LEDS
-EXTERN void stm32_ledinit(void);
-EXTERN void stm32_setled(int led, bool ledon);
-EXTERN void stm32_setleds(uint8_t ledset);
-#endif
-
 #undef EXTERN
 #if defined(__cplusplus)
 }
diff --git a/src/drivers/boards/tap-v1/CMakeLists.txt b/src/drivers/boards/tap-v1/CMakeLists.txt
index ca3baa3e31..b44014b0d7 100644
--- a/src/drivers/boards/tap-v1/CMakeLists.txt
+++ b/src/drivers/boards/tap-v1/CMakeLists.txt
@@ -38,6 +38,8 @@ px4_add_module(
 	SRCS
 		../common/board_name.c
 		tap_init.c
+		tap_pwr.c
+		tap_sdio.c
 		tap_timer_config.c
 		tap_spi.c
 		tap_usb.c
diff --git a/src/drivers/boards/tap-v1/board_config.h b/src/drivers/boards/tap-v1/board_config.h
index 0111e20114..26395d4908 100644
--- a/src/drivers/boards/tap-v1/board_config.h
+++ b/src/drivers/boards/tap-v1/board_config.h
@@ -71,7 +71,7 @@ __BEGIN_DECLS
 #define GPIO_LED1		(GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTC|GPIO_PIN4)
 #define GPIO_LED2		(GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTC|GPIO_PIN5)
 #define GPIO_BLUE_LED GPIO_LED1
-#define GPIO_RED_LED  GPIO_LED1
+#define GPIO_RED_LED  GPIO_LED2
 /*
  * SPI
  *
@@ -128,6 +128,8 @@ __BEGIN_DECLS
  * ADC channels
  *
  * These are the channel numbers of the ADCs of the microcontroller that can be used by the Px4 Firmware in the adc driver
+ * PC0     VOLTAGE                   JP2-13,14          - 1.84 @16.66  1.67 @15.12 Scale 0.1105
+ *
  */
 #define ADC_CHANNELS (1 << 10)
 /* todo:Revisit - cannnot tell from schematic - some could be ADC */
@@ -223,6 +225,11 @@ __BEGIN_DECLS
 		{GPIO_GPIO4_INPUT,       GPIO_GPIO4_OUTPUT,       0}, \
 		{GPIO_GPIO5_INPUT,       GPIO_GPIO5_OUTPUT,       0}, }
 
+
+#define MS_PWR_BUTTON_DOWN 750
+#define KEY_AD_GPIO    (GPIO_INPUT|GPIO_PULLDOWN|GPIO_EXTI|GPIO_PORTC|GPIO_PIN1)
+#define POWER_ON_GPIO  (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTA|GPIO_PIN4)
+#define POWER_OFF_GPIO (GPIO_INPUT|GPIO_PULLDOWN|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTA|GPIO_PIN4)
 /****************************************************************************************************
  * Public Types
  ****************************************************************************************************/
@@ -252,6 +259,16 @@ extern void stm32_spiinitialize(void);
 
 extern void stm32_usbinitialize(void);
 
+/************************************************************************************
+ * Name: board_sdio_initialize
+ *
+ * Description:
+ *   Called to configure SDIO.
+ *
+ ************************************************************************************/
+
+extern int board_sdio_initialize(void);
+
 /****************************************************************************
  * Name: nsh_archinitialize
  *
@@ -271,6 +288,38 @@ extern void stm32_usbinitialize(void);
 int nsh_archinitialize(void);
 #endif
 
-#endif /* __ASSEMBLY__ */
 
+/************************************************************************************
+ * Name: board_pwr_init()
+ *
+ * Description:
+ *   Called to configure power control for the tap-v1 board.
+ *
+ * Input Parameters:
+ *   stage- 0 for boot, 1 for board init
+ *
+ ************************************************************************************/
+
+void board_pwr_init(int stage);
+
+/****************************************************************************
+ * Name: board_pwr_button_down
+ *
+ * Description:
+ *   Called to Read the logical state of the power button
+ ****************************************************************************/
+
+bool board_pwr_button_down(void);
+
+/****************************************************************************
+ * Name: board_pwr
+ *
+ * Description:
+ *   Called to turn on or off the TAP
+ *
+ ****************************************************************************/
+
+void board_pwr(bool on_not_off);
+
+#endif /* __ASSEMBLY__ */
 __END_DECLS
diff --git a/src/drivers/boards/tap-v1/tap_init.c b/src/drivers/boards/tap-v1/tap_init.c
index 9bcfcdabc2..648e465beb 100644
--- a/src/drivers/boards/tap-v1/tap_init.c
+++ b/src/drivers/boards/tap-v1/tap_init.c
@@ -54,9 +54,7 @@
 #include <errno.h>
 
 #include <nuttx/arch.h>
-#include <nuttx/spi.h>
 #include <nuttx/i2c.h>
-#include <nuttx/mmcsd.h>
 #include <nuttx/analog/adc.h>
 
 #include "stm32.h"
@@ -128,6 +126,8 @@ __EXPORT void stm32_boardinitialize(void)
 	/* configure always-on ADC pins */
 	stm32_configgpio(GPIO_ADC1_IN10);
 
+	board_pwr_init(0);
+
 	/* configure SPI interfaces */
 	stm32_spiinitialize();
 
@@ -143,10 +143,6 @@ __EXPORT void stm32_boardinitialize(void)
  *
  ****************************************************************************/
 
-static struct spi_dev_s *spi;
-
-#include <math.h>
-
 __EXPORT int nsh_archinitialize(void)
 {
 	int result;
@@ -176,26 +172,18 @@ __EXPORT int nsh_archinitialize(void)
 		       (hrt_callout)stm32_serial_dma_poll,
 		       NULL);
 
+	board_pwr_init(1);
+
 	/* initial LED state */
 	drv_led_start();
 	led_off(LED_AMBER);
 	led_off(LED_BLUE);
 
-	/* Get the SPI port for the microSD slot */
+	/* Init the microSD slot */
 
-	spi = up_spiinitialize(CONFIG_NSH_MMCSDSPIPORTNO);
-
-	if (!spi) {
-		message("[boot] FAILED to initialize SPI port %d\n", CONFIG_NSH_MMCSDSPIPORTNO);
-		up_ledon(LED_AMBER);
-		return -ENODEV;
-	}
-
-	/* Now bind the SPI interface to the MMCSD driver */
-	result = mmcsd_spislotinitialize(CONFIG_NSH_MMCSDMINOR, CONFIG_NSH_MMCSDSLOTNO, spi);
+	result = board_sdio_initialize();
 
 	if (result != OK) {
-		message("[boot] FAILED to bind SPI port 2 to the MMCSD driver\n");
 		up_ledon(LED_AMBER);
 		return -ENODEV;
 	}
diff --git a/src/drivers/boards/tap-v1/tap_led.c b/src/drivers/boards/tap-v1/tap_led.c
index d39e8f4a84..b09d04216c 100644
--- a/src/drivers/boards/tap-v1/tap_led.c
+++ b/src/drivers/boards/tap-v1/tap_led.c
@@ -98,20 +98,10 @@ __EXPORT void led_off(int led)
 __EXPORT void led_toggle(int led)
 {
 	if (led == 0) {
-		if (stm32_gpioread(GPIO_BLUE_LED)) {
-			stm32_gpiowrite(GPIO_BLUE_LED, false);
-
-		} else {
-			stm32_gpiowrite(GPIO_BLUE_LED, true);
-		}
+		stm32_gpiowrite(GPIO_BLUE_LED, !stm32_gpioread(GPIO_BLUE_LED));
 	}
 
 	if (led == 1) {
-		if (stm32_gpioread(GPIO_RED_LED)) {
-			stm32_gpiowrite(GPIO_RED_LED, false);
-
-		} else {
-			stm32_gpiowrite(GPIO_RED_LED, true);
-		}
+		stm32_gpiowrite(GPIO_RED_LED, !stm32_gpioread(GPIO_RED_LED));
 	}
 }
diff --git a/src/drivers/boards/tap-v1/tap_pwr.c b/src/drivers/boards/tap-v1/tap_pwr.c
new file mode 100644
index 0000000000..04b232a223
--- /dev/null
+++ b/src/drivers/boards/tap-v1/tap_pwr.c
@@ -0,0 +1,167 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2016 PX4 Development Team. All rights reserved.
+ *         Author: David Sidrane <david_s5@nscdg.com>
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file tap-v1_pwr.c
+ *
+ * Board-specific SPI functions.
+ */
+
+/************************************************************************************
+ * Included Files
+ ************************************************************************************/
+
+#include <px4_config.h>
+
+#include <stdint.h>
+#include <stdbool.h>
+#include <time.h>
+#include <debug.h>
+
+#include <arch/board/board.h>
+#include <nuttx/arch.h>
+
+#include "up_arch.h"
+#include "board_config.h"
+#include <stm32_pwr.h>
+
+extern void led_on(int led);
+extern void led_off(int led);
+
+/************************************************************************************
+ * Private Data
+ ************************************************************************************/
+
+/************************************************************************************
+ * Private Functions
+ ************************************************************************************/
+
+static int board_button_irq(int irq, FAR void *context)
+{
+	static struct timespec time_down;
+
+	if (board_pwr_button_down()) {
+
+		led_on(BOARD_LED_RED);
+
+		clock_gettime(CLOCK_REALTIME, &time_down);
+
+	} else {
+
+		led_off(BOARD_LED_RED);
+
+		struct timespec now;
+
+		clock_gettime(CLOCK_REALTIME, &now);
+
+		uint64_t tdown_ms = time_down.tv_sec * 1000 + time_down.tv_nsec / 1000000;
+
+		uint64_t tnow_ms  = now.tv_sec * 1000 + now.tv_nsec / 1000000;
+
+		if (tdown_ms != 0 && (tnow_ms - tdown_ms) >= MS_PWR_BUTTON_DOWN) {
+
+			led_on(BOARD_LED_BLUE);
+
+			up_mdelay(750);
+			stm32_pwr_enablebkp();
+			/* XXX wow, this is evil - write a magic number into backup register zero */
+			*(uint32_t *)0x40002850 = 0xdeaddead;
+			up_mdelay(750);
+			up_systemreset();
+
+			while (1);
+		}
+	}
+
+	return OK;
+}
+
+/************************************************************************************
+ * Public Functions
+ ************************************************************************************/
+
+/************************************************************************************
+ * Name: board_pwr_init()
+ *
+ * Description:
+ *   Called to configure power control for the tap-v1 board.
+ *
+ * Input Parameters:
+ *   stage- 0 for boot, 1 for board init
+ *
+ ************************************************************************************/
+
+void board_pwr_init(int stage)
+{
+	if (stage == 0) {
+		stm32_configgpio(POWER_ON_GPIO);
+		stm32_configgpio(KEY_AD_GPIO);
+	}
+
+	if (stage == 1) {
+		stm32_gpiosetevent(KEY_AD_GPIO, true, true, true, board_button_irq);
+	}
+}
+
+/****************************************************************************
+ * Name: board_pwr_button_down
+ *
+ * Description:
+ *   Called to Read the logical state of the active low power button.
+ *
+ ****************************************************************************/
+
+bool board_pwr_button_down(void)
+{
+	return 0 == stm32_gpioread(KEY_AD_GPIO);
+}
+
+/****************************************************************************
+ * Name: board_pwr
+ *
+ * Description:
+ *   Called to turn on or off the TAP
+ *
+ ****************************************************************************/
+
+void board_pwr(bool on_not_off)
+{
+	if (on_not_off) {
+		stm32_configgpio(POWER_ON_GPIO);
+
+	} else {
+
+		stm32_configgpio(POWER_OFF_GPIO);
+	}
+}
diff --git a/src/drivers/boards/tap-v1/tap_sdio.c b/src/drivers/boards/tap-v1/tap_sdio.c
new file mode 100644
index 0000000000..4367134c63
--- /dev/null
+++ b/src/drivers/boards/tap-v1/tap_sdio.c
@@ -0,0 +1,122 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2016 PX4 Development Team. All rights reserved.
+ *         Author: David Sidrane <david_s5@nscdg.com>
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file tap-v1_sdio.c
+ *
+ * Board-specific SDIOfunctions.
+ */
+
+/************************************************************************************
+ * Included Files
+ ************************************************************************************/
+
+#include <px4_config.h>
+
+#include <stdint.h>
+#include <stdio.h>
+#include <stdbool.h>
+#include <debug.h>
+#include <errno.h>
+
+#include <nuttx/spi.h>
+#include <nuttx/mmcsd.h>
+#include <arch/board/board.h>
+
+#include "up_arch.h"
+#include "chip.h"
+#include "stm32.h"
+#include "board_config.h"
+
+/****************************************************************************
+ * Pre-Processor Definitions
+ ****************************************************************************/
+
+/* Configuration ************************************************************/
+
+/* Debug ********************************************************************/
+
+#ifdef CONFIG_CPP_HAVE_VARARGS
+#  ifdef CONFIG_DEBUG
+#    define message(...) lowsyslog(__VA_ARGS__)
+#  else
+#    define message(...) printf(__VA_ARGS__)
+#  endif
+#else
+#  ifdef CONFIG_DEBUG
+#    define message lowsyslog
+#  else
+#    define message printf
+#  endif
+#endif
+
+/************************************************************************************
+ * Private Data
+ ************************************************************************************/
+static struct spi_dev_s *spi;
+
+/************************************************************************************
+ * Public Functions
+ ************************************************************************************/
+
+/************************************************************************************
+ * Name: board_sdio_initialize
+ *
+ * Description:
+ *   Called to configure SDIO.
+ *
+ ************************************************************************************/
+
+__EXPORT int board_sdio_initialize(void)
+{
+	/* Get the SPI port for the microSD slot */
+
+	spi = up_spiinitialize(CONFIG_NSH_MMCSDSPIPORTNO);
+
+	if (!spi) {
+		message("[boot] FAILED to initialize SPI port %d\n", CONFIG_NSH_MMCSDSPIPORTNO);
+		return -ENODEV;
+	}
+
+	/* Now bind the SPI interface to the MMCSD driver */
+	int result = mmcsd_spislotinitialize(CONFIG_NSH_MMCSDMINOR, CONFIG_NSH_MMCSDSLOTNO, spi);
+
+	if (result != OK) {
+		message("[boot] FAILED to bind SPI port 2 to the MMCSD driver\n");
+		return -ENODEV;
+	}
+
+	return OK;
+}
+
diff --git a/src/drivers/boards/tap-v1/tap_spi.c b/src/drivers/boards/tap-v1/tap_spi.c
index 87f4df5110..b7a6c2c59e 100644
--- a/src/drivers/boards/tap-v1/tap_spi.c
+++ b/src/drivers/boards/tap-v1/tap_spi.c
@@ -83,6 +83,6 @@ __EXPORT void stm32_spi2select(FAR struct spi_dev_s *dev, enum spi_dev_e devid,
 
 __EXPORT uint8_t stm32_spi2status(FAR struct spi_dev_s *dev, enum spi_dev_e devid)
 {
-	return stm32_gpioread(GPIO_SPI_SD_SW);
+	return !stm32_gpioread(GPIO_SPI_SD_SW);
 }
 

From a36f392b580be251062dc270af7f73cdeeee41cc Mon Sep 17 00:00:00 2001
From: David Sidrane <david_s5@nscdg.com>
Date: Fri, 8 Jul 2016 16:48:52 -1000
Subject: [PATCH 0962/1230] Rework PWM LED Driver

---
 src/drivers/rgbled_pwm/CMakeLists.txt         |   1 -
 src/drivers/stm32/CMakeLists.txt              |   1 +
 src/drivers/stm32/drv_io_timer.h              |   8 +
 .../{rgbled_pwm => stm32}/drv_led_pwm.cpp     | 146 +++++-------------
 4 files changed, 50 insertions(+), 106 deletions(-)
 rename src/drivers/{rgbled_pwm => stm32}/drv_led_pwm.cpp (67%)

diff --git a/src/drivers/rgbled_pwm/CMakeLists.txt b/src/drivers/rgbled_pwm/CMakeLists.txt
index 7bc768d91b..0805c8aea9 100644
--- a/src/drivers/rgbled_pwm/CMakeLists.txt
+++ b/src/drivers/rgbled_pwm/CMakeLists.txt
@@ -36,7 +36,6 @@ px4_add_module(
 	COMPILE_FLAGS
 		-Os
 	SRCS
-		drv_led_pwm.cpp
 		rgbled_pwm.cpp
 	DEPENDS
 		platforms__common
diff --git a/src/drivers/stm32/CMakeLists.txt b/src/drivers/stm32/CMakeLists.txt
index b7b3825aa8..40bbef8ad7 100644
--- a/src/drivers/stm32/CMakeLists.txt
+++ b/src/drivers/stm32/CMakeLists.txt
@@ -39,6 +39,7 @@ px4_add_module(
 		drv_io_timer.c
 		drv_pwm_servo.c
 		drv_input_capture.c
+		drv_led_pwm.cpp
 	DEPENDS
 		platforms__common
 	)
diff --git a/src/drivers/stm32/drv_io_timer.h b/src/drivers/stm32/drv_io_timer.h
index 0cd59a4e1e..250f7ce3eb 100644
--- a/src/drivers/stm32/drv_io_timer.h
+++ b/src/drivers/stm32/drv_io_timer.h
@@ -47,6 +47,10 @@ __BEGIN_DECLS
 /* configuration limits */
 #define MAX_IO_TIMERS			4
 #define MAX_TIMER_IO_CHANNELS	8
+
+#define MAX_LED_TIMERS			1
+#define MAX_TIMER_LED_CHANNELS	3
+
 #define IO_TIMER_ALL_MODES_CHANNELS 0
 
 typedef enum io_timer_channel_mode_t {
@@ -90,6 +94,10 @@ typedef void (*channel_handler_t)(void *context, const io_timers_t *timer, uint3
 /* supplied by board-specific code */
 __EXPORT extern const io_timers_t io_timers[MAX_IO_TIMERS];
 __EXPORT extern const timer_io_channels_t timer_io_channels[MAX_TIMER_IO_CHANNELS];
+
+__EXPORT extern const io_timers_t led_pwm_timers[MAX_LED_TIMERS];
+__EXPORT extern const timer_io_channels_t led_pwm_channels[MAX_TIMER_LED_CHANNELS];
+
 __EXPORT extern io_timer_channel_allocation_t allocations[IOTimerChanModeSize];
 __EXPORT int io_timer_handler0(int irq, void *context);
 __EXPORT int io_timer_handler1(int irq, void *context);
diff --git a/src/drivers/rgbled_pwm/drv_led_pwm.cpp b/src/drivers/stm32/drv_led_pwm.cpp
similarity index 67%
rename from src/drivers/rgbled_pwm/drv_led_pwm.cpp
rename to src/drivers/stm32/drv_led_pwm.cpp
index 17590004ec..263ca6645c 100644
--- a/src/drivers/rgbled_pwm/drv_led_pwm.cpp
+++ b/src/drivers/stm32/drv_led_pwm.cpp
@@ -54,6 +54,7 @@
 #include <systemlib/perf_counter.h>
 #include <systemlib/err.h>
 #include <systemlib/systemlib.h>
+#include <systemlib/px4_macros.h>
 
 #include <drivers/drv_pwm_output.h>
 #include <drivers/stm32/drv_io_timer.h>
@@ -61,7 +62,7 @@
 
 #include <board_config.h>
 
-
+#if defined(BOARD_HAS_LED_PWM)
 #define REG(_tmr, _reg) (*(volatile uint32_t *)(led_pwm_timers[_tmr].base + _reg))
 
 #define rCR1(_tmr)      REG(_tmr, STM32_GTIM_CR1_OFFSET)
@@ -96,103 +97,41 @@ void led_pwm_servo_arm(bool armed);
 unsigned led_pwm_timer_get_period(unsigned timer);
 
 
-const struct io_timers_t led_pwm_timers[3] = {
-	{
-		.base = STM32_TIM3_BASE,
-		.clock_register = STM32_RCC_APB1ENR,
-		.clock_bit = RCC_APB1ENR_TIM3EN,
-		.clock_freq = STM32_APB1_TIM3_CLKIN,
-		.vectorno =  STM32_IRQ_TIM3,
-		.first_channel_index = 4,
-		.last_channel_index = 7,
-		.handler = io_timer_handler0,
-	},
-	{
-		.base = STM32_TIM3_BASE,
-		.clock_register = STM32_RCC_APB1ENR,
-		.clock_bit = RCC_APB1ENR_TIM3EN,
-		.clock_freq = STM32_APB1_TIM3_CLKIN,
-		.vectorno =  STM32_IRQ_TIM3,
-		.first_channel_index = 4,
-		.last_channel_index = 7,
-		.handler = io_timer_handler1,
-	},
-	{
-		.base = STM32_TIM3_BASE,
-		.clock_register = STM32_RCC_APB1ENR,
-		.clock_bit = RCC_APB1ENR_TIM3EN,
-		.clock_freq = STM32_APB1_TIM3_CLKIN,
-		.vectorno =  STM32_IRQ_TIM3,
-		.first_channel_index = 4,
-		.last_channel_index = 7,
-		.handler = io_timer_handler2,
-	}
-};
-
-#define LED_TIM3_CH3OUT  (GPIO_ALT|GPIO_AF2|GPIO_SPEED_50MHz|GPIO_OPENDRAIN|GPIO_PORTB|GPIO_PIN0)
-#define LED_TIM3_CH2OUT  (GPIO_ALT|GPIO_AF2|GPIO_SPEED_50MHz|GPIO_OPENDRAIN|GPIO_PORTC|GPIO_PIN7)
-#define LED_TIM3_CH4OUT  (GPIO_ALT|GPIO_AF2|GPIO_SPEED_50MHz|GPIO_OPENDRAIN|GPIO_PORTB|GPIO_PIN1)
-#define LED_TIM3_CH3IN  (GPIO_INPUT|GPIO_AF2|GPIO_SPEED_50MHz|GPIO_OPENDRAIN|GPIO_PORTB|GPIO_PIN0)
-#define LED_TIM3_CH2IN  (GPIO_INPUT|GPIO_AF2|GPIO_SPEED_50MHz|GPIO_OPENDRAIN|GPIO_PORTC|GPIO_PIN7)
-#define LED_TIM3_CH4IN  (GPIO_INPUT|GPIO_AF2|GPIO_SPEED_50MHz|GPIO_OPENDRAIN|GPIO_PORTB|GPIO_PIN1)
-
-const struct timer_io_channels_t led_pwm_channels[3] = {
-	{
-		.gpio_out = LED_TIM3_CH3OUT,
-		.gpio_in  = LED_TIM3_CH3IN,
-		.timer_index = 0,
-		.timer_channel = 3,
-		.masks	= GTIM_SR_CC3IF | GTIM_SR_CC3OF,
-		.ccr_offset = STM32_GTIM_CCR3_OFFSET,
-	},
-	{
-		.gpio_out = LED_TIM3_CH2OUT,
-		.gpio_in  = LED_TIM3_CH2IN,
-		.timer_index = 1,
-		.timer_channel = 2,
-		.masks	= GTIM_SR_CC2IF | GTIM_SR_CC2OF,
-		.ccr_offset = STM32_GTIM_CCR2_OFFSET,
-	},
-	{
-		.gpio_out = LED_TIM3_CH4OUT,
-		.gpio_in  = LED_TIM3_CH4IN,
-		.timer_index = 2,
-		.timer_channel = 4,
-		.masks	= GTIM_SR_CC4IF | GTIM_SR_CC4OF,
-		.ccr_offset = STM32_GTIM_CCR4_OFFSET,
-	}
-};
-
-
 static void
 led_pwm_timer_init(unsigned timer)
 {
-	/* enable the timer clock before we try to talk to it */
-	modifyreg32(led_pwm_timers[timer].clock_register, 0, led_pwm_timers[timer].clock_bit);
+	/* valid Timer */
 
-	/* disable and configure the timer */
-	rCR1(timer) = 0;
-	rCR2(timer) = 0;
-	rSMCR(timer) = 0;
-	rDIER(timer) = 0;
-	rCCER(timer) = 0;
-	rCCMR1(timer) = 0;
-	rCCMR2(timer) = 0;
-	rCCER(timer) = 0;
-	rDCR(timer) = 0;
+	if (led_pwm_timers[timer].base != 0) {
 
-	if ((led_pwm_timers[timer].base == STM32_TIM1_BASE) || (led_pwm_timers[timer].base == STM32_TIM8_BASE)) {
-		/* master output enable = on */
-		rBDTR(timer) = ATIM_BDTR_MOE;
+		/* enable the timer clock before we try to talk to it */
+
+		modifyreg32(led_pwm_timers[timer].clock_register, 0, led_pwm_timers[timer].clock_bit);
+
+		/* disable and configure the timer */
+		rCR1(timer) = 0;
+		rCR2(timer) = 0;
+		rSMCR(timer) = 0;
+		rDIER(timer) = 0;
+		rCCER(timer) = 0;
+		rCCMR1(timer) = 0;
+		rCCMR2(timer) = 0;
+		rCCER(timer) = 0;
+		rDCR(timer) = 0;
+
+		if ((led_pwm_timers[timer].base == STM32_TIM1_BASE) || (led_pwm_timers[timer].base == STM32_TIM8_BASE)) {
+			/* master output enable = on */
+			rBDTR(timer) = ATIM_BDTR_MOE;
+		}
+
+		/* configure the timer to free-run at 1MHz */
+		rPSC(timer) = (led_pwm_timers[timer].clock_freq / 1000000) - 1;
+
+		/* default to updating at 50Hz */
+		led_pwm_timer_set_rate(timer, 50);
+
+		/* note that the timer is left disabled - arming is performed separately */
 	}
-
-	/* configure the timer to free-run at 1MHz */
-	rPSC(timer) = (led_pwm_timers[timer].clock_freq / 1000000) - 1;
-
-	/* default to updating at 50Hz */
-	led_pwm_timer_set_rate(timer, 50);
-
-	/* note that the timer is left disabled - arming is performed separately */
 }
 unsigned
 led_pwm_timer_get_period(unsigned timer)
@@ -216,31 +155,28 @@ led_pwm_channel_init(unsigned channel)
 	unsigned timer = led_pwm_channels[channel].timer_index;
 
 	/* configure the GPIO first */
+
 	px4_arch_configgpio(led_pwm_channels[channel].gpio_out);
 
 	/* configure the channel */
 	switch (led_pwm_channels[channel].timer_channel) {
 	case 1:
 		rCCMR1(timer) |= (GTIM_CCMR_MODE_PWM1 << GTIM_CCMR1_OC1M_SHIFT) | GTIM_CCMR1_OC1PE;
-		//rCCR1(timer) = led_pwm_channels[channel].default_value;
 		rCCER(timer) |= GTIM_CCER_CC1E;
 		break;
 
 	case 2:
 		rCCMR1(timer) |= (GTIM_CCMR_MODE_PWM1 << GTIM_CCMR1_OC2M_SHIFT) | GTIM_CCMR1_OC2PE;
-		//rCCR2(timer) = led_pwm_channels[channel].default_value;
 		rCCER(timer) |= GTIM_CCER_CC2E;
 		break;
 
 	case 3:
 		rCCMR2(timer) |= (GTIM_CCMR_MODE_PWM1 << GTIM_CCMR2_OC3M_SHIFT) | GTIM_CCMR2_OC3PE;
-		//rCCR3(timer) = led_pwm_channels[channel].default_value;
 		rCCER(timer) |= GTIM_CCER_CC3E;
 		break;
 
 	case 4:
 		rCCMR2(timer) |= (GTIM_CCMR_MODE_PWM1 << GTIM_CCMR2_OC4M_SHIFT) | GTIM_CCMR2_OC4PE;
-		//rCCR4(timer) = led_pwm_channels[channel].default_value;
 		rCCER(timer) |= GTIM_CCER_CC4E;
 		break;
 	}
@@ -249,7 +185,7 @@ led_pwm_channel_init(unsigned channel)
 int
 led_pwm_servo_set(unsigned channel, uint8_t  cvalue)
 {
-	if (channel >= 3) {
+	if (channel >= arraySize(led_pwm_channels)) {
 		return -1;
 	}
 
@@ -263,7 +199,11 @@ led_pwm_servo_set(unsigned channel, uint8_t  cvalue)
 
 	unsigned period = led_pwm_timer_get_period(timer);
 
+#if defined(BOARD_LED_PWM_DRIVE_ACTIVE_LOW)
 	unsigned value = period - (unsigned)cvalue * period / 255;
+#else
+	unsigned value = (unsigned)cvalue * period / 255;
+#endif
 
 	/* configure the channel */
 	if (value > 0) {
@@ -336,12 +276,12 @@ int
 led_pwm_servo_init(void)
 {
 	/* do basic timer initialisation first */
-	for (unsigned i = 0; i < 3; i++) {
+	for (unsigned i = 0; i < arraySize(led_pwm_timers); i++) {
 		led_pwm_timer_init(i);
 	}
 
 	/* now init channels */
-	for (unsigned i = 0; i < 3; i++) {
+	for (unsigned i = 0; i < arraySize(led_pwm_channels); i++) {
 		led_pwm_channel_init(i);
 	}
 
@@ -359,7 +299,7 @@ void
 led_pwm_servo_arm(bool armed)
 {
 	/* iterate timers and arm/disarm appropriately */
-	for (unsigned i = 0; i < 3; i++) {
+	for (unsigned i = 0; i < arraySize(led_pwm_timers); i++) {
 		if (led_pwm_timers[i].base != 0) {
 			if (armed) {
 				/* force an update to preload all registers */
@@ -369,14 +309,10 @@ led_pwm_servo_arm(bool armed)
 				rCR1(i) |= GTIM_CR1_CEN | GTIM_CR1_ARPE;
 
 			} else {
-				// XXX This leads to FMU PWM being still active
-				// but uncontrollable. Just disable the timer
-				// and risk a runt.
-				///* on disarm, just stop auto-reload so we don't generate runts */
-				//rCR1(i) &= ~GTIM_CR1_ARPE;
 				rCR1(i) = 0;
 			}
 		}
 	}
 }
 
+#endif // BOARD_HAS_LED_PWM

From ea4d9a34fd7f739cbb93f61f279b4dd8bc966f0d Mon Sep 17 00:00:00 2001
From: David Sidrane <david_s5@nscdg.com>
Date: Fri, 8 Jul 2016 16:50:38 -1000
Subject: [PATCH 0963/1230] mindpx-v2 Uses New PWM LED driver structure

---
 src/drivers/boards/mindpx-v2/board_config.h   |  5 ++++
 .../boards/mindpx-v2/mindpx_timer_config.c    | 30 +++++++++++++++++++
 2 files changed, 35 insertions(+)

diff --git a/src/drivers/boards/mindpx-v2/board_config.h b/src/drivers/boards/mindpx-v2/board_config.h
index 207bf577f2..9176246e66 100644
--- a/src/drivers/boards/mindpx-v2/board_config.h
+++ b/src/drivers/boards/mindpx-v2/board_config.h
@@ -226,6 +226,11 @@ __BEGIN_DECLS
 #define GPIO_GPIO12_OUTPUT       (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTE|GPIO_PIN15)
 #define GPIO_GPIO13_OUTPUT       (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTC|GPIO_PIN6)
 
+#define BOARD_HAS_LED_PWM
+#define BOARD_LED_PWM_DRIVE_ACTIVE_LOW
+#define LED_TIM3_CH3OUT  (GPIO_ALT|GPIO_AF2|GPIO_SPEED_50MHz|GPIO_OPENDRAIN|GPIO_PORTB|GPIO_PIN0)
+#define LED_TIM3_CH2OUT  (GPIO_ALT|GPIO_AF2|GPIO_SPEED_50MHz|GPIO_OPENDRAIN|GPIO_PORTC|GPIO_PIN7)
+#define LED_TIM3_CH4OUT  (GPIO_ALT|GPIO_AF2|GPIO_SPEED_50MHz|GPIO_OPENDRAIN|GPIO_PORTB|GPIO_PIN1)
 
 /* Power supply control and monitoring GPIOs */
 //#define GPIO_VDD_5V_PERIPH_EN	(GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTA|GPIO_PIN8)
diff --git a/src/drivers/boards/mindpx-v2/mindpx_timer_config.c b/src/drivers/boards/mindpx-v2/mindpx_timer_config.c
index 9cd2205f1e..20d332c50d 100644
--- a/src/drivers/boards/mindpx-v2/mindpx_timer_config.c
+++ b/src/drivers/boards/mindpx-v2/mindpx_timer_config.c
@@ -129,3 +129,33 @@ __EXPORT const timer_io_channels_t timer_io_channels[MAX_TIMER_IO_CHANNELS] = {
 //		.default_value = 1000,
 //	}
 };
+
+__EXPORT const struct io_timers_t led_pwm_timers[1] = {
+	{
+		.base = STM32_TIM3_BASE,
+		.clock_register = STM32_RCC_APB1ENR,
+		.clock_bit = RCC_APB1ENR_TIM3EN,
+		.clock_freq = STM32_APB1_TIM3_CLKIN,
+	}
+};
+
+__EXPORT const struct timer_io_channels_t led_pwm_channels[3] = {
+	{
+		.gpio_out = LED_TIM3_CH3OUT,
+		.gpio_in  = 0,
+		.timer_index = 0,
+		.timer_channel = 3,
+	},
+	{
+		.gpio_out = LED_TIM3_CH2OUT,
+		.gpio_in  = 0,
+		.timer_index = 0,
+		.timer_channel = 2,
+	},
+	{
+		.gpio_out = LED_TIM3_CH4OUT,
+		.gpio_in  = 0,
+		.timer_index = 0,
+		.timer_channel = 4,
+	}
+};

From a23785842c29e43848abdb3b630943a5855933db Mon Sep 17 00:00:00 2001
From: David Sidrane <david_s5@nscdg.com>
Date: Fri, 8 Jul 2016 16:51:15 -1000
Subject: [PATCH 0964/1230] tap-v1 Uses New PWM LED driver structure

---
 src/drivers/boards/tap-v1/board_config.h     | 14 ++++++---
 src/drivers/boards/tap-v1/tap_timer_config.c | 33 ++++++++++++++++++++
 2 files changed, 43 insertions(+), 4 deletions(-)

diff --git a/src/drivers/boards/tap-v1/board_config.h b/src/drivers/boards/tap-v1/board_config.h
index 26395d4908..654b70a45d 100644
--- a/src/drivers/boards/tap-v1/board_config.h
+++ b/src/drivers/boards/tap-v1/board_config.h
@@ -168,10 +168,10 @@ __BEGIN_DECLS
  * Tone alarm output
  */
 /* todo:Revisit - cannnot tell from schematic - one could be tone alarm*/
-#define TONE_ALARM_TIMER	3	/* timer 3 */
-#define TONE_ALARM_CHANNEL	3	/* channel 3 */
-#define GPIO_TONE_ALARM_IDLE	(GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTC|GPIO_PIN8)
-#define GPIO_TONE_ALARM		(GPIO_ALT|GPIO_AF2|GPIO_SPEED_2MHz|GPIO_FLOAT|GPIO_PUSHPULL|GPIO_PORTC|GPIO_PIN8)
+#define TONE_ALARM_TIMER        8   /* timer 8 */
+#define TONE_ALARM_CHANNEL      3   /* channel 3 */
+#define GPIO_TONE_ALARM_IDLE    (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTC|GPIO_PIN8)
+#define GPIO_TONE_ALARM         (GPIO_ALT|GPIO_AF2|GPIO_SPEED_2MHz|GPIO_FLOAT|GPIO_PUSHPULL|GPIO_PORTC|GPIO_PIN8)
 
 /*
  * PWM
@@ -192,6 +192,12 @@ __BEGIN_DECLS
 #define GPIO_TIM3_CH4OUT	GPIO_TIM3_CH4OUT_1
 #define DIRECT_PWM_OUTPUT_CHANNELS	4
 
+#define BOARD_HAS_LED_PWM
+#define LED_TIM3_CH1OUT  GPIO_TIM3_CH1OUT
+#define LED_TIM3_CH2OUT  GPIO_TIM3_CH2OUT
+#define LED_TIM3_CH3OUT  GPIO_TIM3_CH3OUT
+
+
 /* USB OTG FS
  *
  * PA9  OTG_FS_VBUS VBUS sensing (also connected to the green LED)
diff --git a/src/drivers/boards/tap-v1/tap_timer_config.c b/src/drivers/boards/tap-v1/tap_timer_config.c
index e59fdfe641..0d43e7931c 100644
--- a/src/drivers/boards/tap-v1/tap_timer_config.c
+++ b/src/drivers/boards/tap-v1/tap_timer_config.c
@@ -98,3 +98,36 @@ __EXPORT const timer_io_channels_t timer_io_channels[MAX_TIMER_IO_CHANNELS] = {
 		.masks  = GTIM_SR_CC4IF | GTIM_SR_CC4OF
 	}
 };
+
+__EXPORT const struct io_timers_t led_pwm_timers[1] = {
+	{
+		.base 				= STM32_TIM3_BASE,
+		.clock_register 	= STM32_RCC_APB1ENR,
+		.clock_bit 			= RCC_APB1ENR_TIM3EN,
+		.clock_freq 		= STM32_APB1_TIM3_CLKIN,
+		.vectorno 			=  STM32_IRQ_TIM3,
+		.first_channel_index = 0,
+		.last_channel_index = 2,
+	}
+};
+
+__EXPORT const struct timer_io_channels_t led_pwm_channels[3] = {
+	{
+		.gpio_out = LED_TIM3_CH1OUT,
+		.gpio_in  = 0,
+		.timer_index = 0,
+		.timer_channel = 1,
+	},
+	{
+		.gpio_out = LED_TIM3_CH2OUT,
+		.gpio_in  = 0,
+		.timer_index = 0,
+		.timer_channel = 2,
+	},
+	{
+		.gpio_out = LED_TIM3_CH3OUT,
+		.gpio_in  = 0,
+		.timer_index = 0,
+		.timer_channel = 3,
+	}
+};

From f69e9a3d37ee71a830ca5bbc0698ce4e5a2ae7f1 Mon Sep 17 00:00:00 2001
From: tommises <tommises@gmail.com>
Date: Sat, 9 Jul 2016 12:59:32 -0600
Subject: [PATCH 0965/1230] Fixed VISION_POSITION_ESTIMATE.usec

Fixed units to be microseconds and not milliseconds.
---
 src/modules/mavlink/mavlink_messages.cpp | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp
index 038796de31..e649c8738b 100644
--- a/src/modules/mavlink/mavlink_messages.cpp
+++ b/src/modules/mavlink/mavlink_messages.cpp
@@ -1459,7 +1459,7 @@ protected:
 
         if (_pos_sub->update(&_pos_time, &vpos)) {
             mavlink_vision_position_estimate_t vmsg;
-            vmsg.usec = vpos.timestamp_boot / 1000;
+            vmsg.usec = vpos.timestamp_boot;
             vmsg.x = vpos.x;
             vmsg.y = vpos.y;
             vmsg.z = vpos.z;

From 5bd574dd05ffd34d8050ebaa81554c4d098f417b Mon Sep 17 00:00:00 2001
From: tommises <tommises@gmail.com>
Date: Sat, 9 Jul 2016 11:09:19 -0600
Subject: [PATCH 0966/1230] Fixed timestamp synchronization

---
 src/modules/mavlink/mavlink_receiver.cpp | 5 +++--
 1 file changed, 3 insertions(+), 2 deletions(-)

diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp
index dc9b711385..acdc67993b 100644
--- a/src/modules/mavlink/mavlink_receiver.cpp
+++ b/src/modules/mavlink/mavlink_receiver.cpp
@@ -2177,8 +2177,9 @@ void MavlinkReceiver::print_status()
 
 uint64_t MavlinkReceiver::sync_stamp(uint64_t usec)
 {
-	if (_time_offset > 0) {
-		return usec - (_time_offset / 1000) ;
+
+	if (_time_offset != 0) {
+		return usec + (_time_offset / 1000) ;
 
 	} else {
 		return hrt_absolute_time();

From 19b56c1574bc75fbb16294ad7abf4f6352cf9173 Mon Sep 17 00:00:00 2001
From: tommises <tommises@gmail.com>
Date: Sat, 9 Jul 2016 16:34:58 -0600
Subject: [PATCH 0967/1230] Use timestamp received from companion computer as
 timestamp_boot

---
 src/modules/mavlink/mavlink_receiver.cpp | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp
index acdc67993b..f5bb444785 100644
--- a/src/modules/mavlink/mavlink_receiver.cpp
+++ b/src/modules/mavlink/mavlink_receiver.cpp
@@ -683,7 +683,7 @@ MavlinkReceiver::handle_message_att_pos_mocap(mavlink_message_t *msg)
 	// Use the component ID to identify the mocap system
 	att_pos_mocap.id = msg->compid;
 
-	att_pos_mocap.timestamp_boot = hrt_absolute_time(); // Monotonic time
+	att_pos_mocap.timestamp_boot = sync_stamp(mocap.time_usec);
 	att_pos_mocap.timestamp_computer = sync_stamp(mocap.time_usec); // Synced time
 
 	att_pos_mocap.q[0] = mocap.q[0];
@@ -963,7 +963,7 @@ MavlinkReceiver::handle_message_vision_position_estimate(mavlink_message_t *msg)
 	// Use the component ID to identify the vision sensor
 	vision_position.id = msg->compid;
 
-	vision_position.timestamp_boot = hrt_absolute_time(); // Monotonic time
+	vision_position.timestamp_boot = sync_stamp(pos.usec);
 	vision_position.timestamp_computer = sync_stamp(pos.usec); // Synced time
 	vision_position.x = pos.x;
 	vision_position.y = pos.y;

From 07384d6b5af38077299e798b64dee85833092378 Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Sun, 10 Jul 2016 12:42:56 +0200
Subject: [PATCH 0968/1230] Vision msg: Cleanup timestamp

---
 msg/vision_position_estimate.msg | 3 +--
 1 file changed, 1 insertion(+), 2 deletions(-)

diff --git a/msg/vision_position_estimate.msg b/msg/vision_position_estimate.msg
index 62c6ff6b52..7e3ee14766 100644
--- a/msg/vision_position_estimate.msg
+++ b/msg/vision_position_estimate.msg
@@ -1,7 +1,6 @@
 uint32 id			# ID of the estimator, commonly the component ID of the incoming message
 
-uint64 timestamp_boot		# time of this estimate, in microseconds since system start
-uint64 timestamp_computer	# timestamp provided by the companion computer, in us
+uint64 timestamp_received	# timestamp when the estimate was received
 
 float32 x			# X position in meters in NED earth-fixed frame
 float32 y			# Y position in meters in NED earth-fixed frame

From 4f875560b7d18647210570eeaad8aeb7504b92e5 Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Sun, 10 Jul 2016 12:43:10 +0200
Subject: [PATCH 0969/1230] Att Q Estimator: New vision timestamp

---
 .../attitude_estimator_q/attitude_estimator_q_main.cpp        | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp b/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp
index c093682044..33ff8ab605 100644
--- a/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp
+++ b/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp
@@ -401,10 +401,10 @@ void AttitudeEstimatorQ::task_main()
 
 		// Check for timeouts on data
 		if (_ext_hdg_mode == 1) {
-			_ext_hdg_good = _vision.timestamp_boot > 0 && (hrt_elapsed_time(&_vision.timestamp_boot) < 500000);
+			_ext_hdg_good = _vision.timestamp > 0 && (hrt_elapsed_time(&_vision.timestamp) < 500000);
 
 		} else if (_ext_hdg_mode == 2) {
-			_ext_hdg_good = _mocap.timestamp_boot > 0 && (hrt_elapsed_time(&_mocap.timestamp_boot) < 500000);
+			_ext_hdg_good = _mocap.timestamp > 0 && (hrt_elapsed_time(&_mocap.timestamp) < 500000);
 		}
 
 		bool gpos_updated;

From 826eaed2ee1d09fff6dbfc6d90e79617b83a68e3 Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Sun, 10 Jul 2016 12:43:21 +0200
Subject: [PATCH 0970/1230] EKF2: Clean up vision timestamps

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

diff --git a/src/modules/ekf2/ekf2_main.cpp b/src/modules/ekf2/ekf2_main.cpp
index b408227217..352de9430f 100644
--- a/src/modules/ekf2/ekf2_main.cpp
+++ b/src/modules/ekf2/ekf2_main.cpp
@@ -608,8 +608,8 @@ void Ekf2::task_main()
 				ev_data.angErr = _default_ev_ang_noise;
 			}
 
-			// use timestamp from external computer - requires clocks to be synchronised so may not be a good idea
-			_ekf.setExtVisionData(ev.timestamp_computer, &ev_data);
+			// use timestamp from external computer, clocks are synchronized when using MAVROS
+			_ekf.setExtVisionData(ev.timestamp, &ev_data);
 		}
 
 		orb_check(_vehicle_land_detected_sub, &vehicle_land_detected_updated);
@@ -979,7 +979,7 @@ void Ekf2::task_main()
 			}
 
 			if (vision_position_updated) {
-				replay.ev_timestamp = ev.timestamp_computer;
+				replay.ev_timestamp = ev.timestamp;
 				replay.pos_ev[0] = ev.x;
 				replay.pos_ev[1] = ev.y;
 				replay.pos_ev[2] = ev.z;

From c0a406b81f050ebf15f15c4a60967915e04bf4b7 Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Sun, 10 Jul 2016 12:43:34 +0200
Subject: [PATCH 0971/1230] LPE: Clean up vision timestamps

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

diff --git a/src/modules/local_position_estimator/sensors/vision.cpp b/src/modules/local_position_estimator/sensors/vision.cpp
index 4a4fa6a2d4..bb5f58e332 100644
--- a/src/modules/local_position_estimator/sensors/vision.cpp
+++ b/src/modules/local_position_estimator/sensors/vision.cpp
@@ -47,7 +47,7 @@ int BlockLocalPositionEstimator::visionMeasure(Vector<float, n_y_vision> &y)
 	y(Y_vision_y) = _sub_vision_pos.get().y;
 	y(Y_vision_z) = _sub_vision_pos.get().z;
 	_visionStats.update(y);
-	_time_last_vision_p = _sub_vision_pos.get().timestamp_boot;
+	_time_last_vision_p = _sub_vision_pos.get().timestamp;
 	return OK;
 }
 

From 8b3045baa28eeefaca5195916a267aba3dce401f Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Sun, 10 Jul 2016 12:43:47 +0200
Subject: [PATCH 0972/1230] MAVLink: clean up vision timestamps

---
 src/modules/mavlink/mavlink_messages.cpp | 2 +-
 src/modules/mavlink/mavlink_receiver.cpp | 7 +++----
 2 files changed, 4 insertions(+), 5 deletions(-)

diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp
index e649c8738b..b12ea1fb9d 100644
--- a/src/modules/mavlink/mavlink_messages.cpp
+++ b/src/modules/mavlink/mavlink_messages.cpp
@@ -1459,7 +1459,7 @@ protected:
 
         if (_pos_sub->update(&_pos_time, &vpos)) {
             mavlink_vision_position_estimate_t vmsg;
-            vmsg.usec = vpos.timestamp_boot;
+            vmsg.usec = vpos.timestamp;
             vmsg.x = vpos.x;
             vmsg.y = vpos.y;
             vmsg.z = vpos.z;
diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp
index f5bb444785..b9c7690a7e 100644
--- a/src/modules/mavlink/mavlink_receiver.cpp
+++ b/src/modules/mavlink/mavlink_receiver.cpp
@@ -957,14 +957,13 @@ MavlinkReceiver::handle_message_vision_position_estimate(mavlink_message_t *msg)
 	mavlink_vision_position_estimate_t pos;
 	mavlink_msg_vision_position_estimate_decode(msg, &pos);
 
-	struct vision_position_estimate_s vision_position;
-	memset(&vision_position, 0, sizeof(vision_position));
+	struct vision_position_estimate_s vision_position = {};
 
 	// Use the component ID to identify the vision sensor
 	vision_position.id = msg->compid;
 
-	vision_position.timestamp_boot = sync_stamp(pos.usec);
-	vision_position.timestamp_computer = sync_stamp(pos.usec); // Synced time
+	vision_position.timestamp = sync_stamp(pos.usec);
+	vision_position.timestamp_received = hrt_absolute_time();
 	vision_position.x = pos.x;
 	vision_position.y = pos.y;
 	vision_position.z = pos.z;

From 7601788c432eab14040c58e0938f9d3798bf921a Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Sun, 10 Jul 2016 12:44:00 +0200
Subject: [PATCH 0973/1230] INAV: clean up vision timestamps

---
 .../position_estimator_inav_main.cpp                      | 8 ++++----
 1 file changed, 4 insertions(+), 4 deletions(-)

diff --git a/src/modules/position_estimator_inav/position_estimator_inav_main.cpp b/src/modules/position_estimator_inav/position_estimator_inav_main.cpp
index 66c75c8fcf..6c2f670762 100644
--- a/src/modules/position_estimator_inav/position_estimator_inav_main.cpp
+++ b/src/modules/position_estimator_inav/position_estimator_inav_main.cpp
@@ -769,8 +769,8 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
 
 					static hrt_abstime last_vision_time = 0;
 
-					float vision_dt = (vision.timestamp_boot - last_vision_time) / 1e6f;
-					last_vision_time = vision.timestamp_boot;
+					float vision_dt = (vision.timestamp - last_vision_time) / 1e6f;
+					last_vision_time = vision.timestamp;
 
 					if (vision_dt > 0.000001f && vision_dt < 0.2f) {
 						vision.vx = (vision.x - last_vision_x) / vision_dt;
@@ -951,14 +951,14 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
 		}
 
 		/* check for timeout on vision topic */
-		if (vision_valid && (t > (vision.timestamp_boot + vision_topic_timeout))) {
+		if (vision_valid && (t > (vision.timestamp + vision_topic_timeout))) {
 			vision_valid = false;
 			warnx("VISION timeout");
 			mavlink_log_info(&mavlink_log_pub, "[inav] VISION timeout");
 		}
 
 		/* check for timeout on mocap topic */
-		if (mocap_valid && (t > (mocap.timestamp_boot + mocap_topic_timeout))) {
+		if (mocap_valid && (t > (mocap.timestamp + mocap_topic_timeout))) {
 			mocap_valid = false;
 			warnx("MOCAP timeout");
 			mavlink_log_info(&mavlink_log_pub, "[inav] MOCAP timeout");

From f39d28419378cb127c9256ccff982d3135b17f60 Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Sun, 10 Jul 2016 14:21:56 +0200
Subject: [PATCH 0974/1230] Update vision fields for attitude_estimator_ekf

---
 .../attitude_estimator_ekf/attitude_estimator_ekf_main.cpp    | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp
index be703728e2..480c4cdd21 100755
--- a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp
+++ b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp
@@ -499,7 +499,7 @@ int attitude_estimator_ekf_thread_main(int argc, char *argv[])
 						orb_copy(ORB_ID(att_pos_mocap), mocap_sub, &mocap);
 					}
 
-					if (mocap.timestamp_boot > 0 && (hrt_elapsed_time(&mocap.timestamp_boot) < 500000)) {
+					if (mocap.timestamp > 0 && (hrt_elapsed_time(&mocap.timestamp) < 500000)) {
 
 						math::Quaternion q(mocap.q);
 						math::Matrix<3, 3> Rmoc = q.to_dcm();
@@ -511,7 +511,7 @@ int attitude_estimator_ekf_thread_main(int argc, char *argv[])
 						z_k[6] = vn(0);
 						z_k[7] = vn(1);
 						z_k[8] = vn(2);
-					}else if (vision.timestamp_boot > 0 && (hrt_elapsed_time(&vision.timestamp_boot) < 500000)) {
+					} else if (vision.timestamp > 0 && (hrt_elapsed_time(&vision.timestamp) < 500000)) {
 
 						math::Quaternion q(vision.q);
 						math::Matrix<3, 3> Rvis = q.to_dcm();

From d2194d787f10110365bbaa777bb9b161d2d5c077 Mon Sep 17 00:00:00 2001
From: tommises <tommises@users.noreply.github.com>
Date: Sun, 10 Jul 2016 08:22:22 -0600
Subject: [PATCH 0975/1230] Refactored Mavlink stream configuration (#5015)

Streams ordered same way in all modes.
---
 src/modules/mavlink/mavlink_main.cpp | 110 +++++++++++++--------------
 1 file changed, 55 insertions(+), 55 deletions(-)

diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp
index b9ca1f2576..ee804996fd 100644
--- a/src/modules/mavlink/mavlink_main.cpp
+++ b/src/modules/mavlink/mavlink_main.cpp
@@ -1851,76 +1851,76 @@ Mavlink::task_main(int argc, char *argv[])
 	switch (_mode) {
 	case MAVLINK_MODE_NORMAL:
 		configure_stream("SYS_STATUS", 1.0f);
-		configure_stream("HOME_POSITION", 0.5f);
+		configure_stream("EXTENDED_SYS_STATE", 1.0f);
 		configure_stream("HIGHRES_IMU", 2.0f);
 		configure_stream("ATTITUDE", 20.0f);
-		configure_stream("VFR_HUD", 8.0f);
-		configure_stream("GPS_RAW_INT", 1.0f);
-		configure_stream("GLOBAL_POSITION_INT", 3.0f);
-		configure_stream("LOCAL_POSITION_NED", 3.0f);
 		configure_stream("RC_CHANNELS", 1.0f);
 		configure_stream("SERVO_OUTPUT_RAW_0", 1.0f);
-		configure_stream("POSITION_TARGET_GLOBAL_INT", 3.0f);
-		configure_stream("ATTITUDE_TARGET", 8.0f);
+		configure_stream("ALTITUDE", 1.0f);
+		configure_stream("GPS_RAW_INT", 1.0f);
+		configure_stream("ADSB_VEHICLE", 2.0f);
 		configure_stream("DISTANCE_SENSOR", 0.5f);
 		configure_stream("OPTICAL_FLOW_RAD", 5.0f);
-		configure_stream("EXTENDED_SYS_STATE", 1.0f);
-		configure_stream("ALTITUDE", 1.0f);
 		configure_stream("VISION_POSITION_NED", 10.0f);
-		configure_stream("NAMED_VALUE_FLOAT", 1.0f);
 		configure_stream("ESTIMATOR_STATUS", 0.5f);
-		configure_stream("ADSB_VEHICLE", 2.0f);
 		configure_stream("NAV_CONTROLLER_OUTPUT", 2.0f);
+		configure_stream("GLOBAL_POSITION_INT", 3.0f);
+		configure_stream("LOCAL_POSITION_NED", 3.0f);
+		configure_stream("POSITION_TARGET_GLOBAL_INT", 3.0f);
+		configure_stream("ATTITUDE_TARGET", 8.0f);
+		configure_stream("HOME_POSITION", 0.5f);
+		configure_stream("NAMED_VALUE_FLOAT", 1.0f);
+		configure_stream("VFR_HUD", 8.0f);
 		configure_stream("WIND", 2.0f);
 		break;
 
 	case MAVLINK_MODE_ONBOARD:
 		configure_stream("SYS_STATUS", 1.0f);
-		configure_stream("ATTITUDE", 250.0f);
+		configure_stream("EXTENDED_SYS_STATE", 2.0f);
 		configure_stream("HIGHRES_IMU", 50.0f);
-		configure_stream("GPS_RAW_INT", 5.0f);
-		configure_stream("GLOBAL_POSITION_INT", 50.0f);
-		configure_stream("LOCAL_POSITION_NED", 30.0f);
-		configure_stream("NAMED_VALUE_FLOAT", 10.0f);
-		configure_stream("CAMERA_CAPTURE", 2.0f);
-		configure_stream("HOME_POSITION", 0.5f);
-		configure_stream("ATTITUDE_TARGET", 10.0f);
-		configure_stream("POSITION_TARGET_GLOBAL_INT", 10.0f);
-		configure_stream("POSITION_TARGET_LOCAL_NED", 10.0f);
-		configure_stream("DISTANCE_SENSOR", 10.0f);
-		configure_stream("OPTICAL_FLOW_RAD", 10.0f);
+		configure_stream("ATTITUDE", 250.0f);
 		configure_stream("RC_CHANNELS", 20.0f);
 		configure_stream("SERVO_OUTPUT_RAW_0", 10.0f);
-		configure_stream("VFR_HUD", 10.0f);
-		configure_stream("SYSTEM_TIME", 1.0f);
-		configure_stream("TIMESYNC", 10.0f);
-		configure_stream("ACTUATOR_CONTROL_TARGET0", 10.0f);
-		//camera trigger is rate limited at the source, do not limit here
-		configure_stream("CAMERA_TRIGGER", 500.0f);
-		configure_stream("EXTENDED_SYS_STATE", 2.0f);
 		configure_stream("ALTITUDE", 10.0f);
+		configure_stream("GPS_RAW_INT", 5.0f);
+		configure_stream("ADSB_VEHICLE", 10.0f);
+		configure_stream("DISTANCE_SENSOR", 10.0f);
+		configure_stream("OPTICAL_FLOW_RAD", 10.0f);
 		configure_stream("VISION_POSITION_NED", 10.0f);
 		configure_stream("ESTIMATOR_STATUS", 1.0f);
-		configure_stream("ADSB_VEHICLE", 10.0f);
 		configure_stream("NAV_CONTROLLER_OUTPUT", 10.0f);
+		configure_stream("GLOBAL_POSITION_INT", 50.0f);
+		configure_stream("LOCAL_POSITION_NED", 30.0f);
+		configure_stream("POSITION_TARGET_GLOBAL_INT", 10.0f);
+		configure_stream("ATTITUDE_TARGET", 10.0f);
+		configure_stream("HOME_POSITION", 0.5f);
+		configure_stream("NAMED_VALUE_FLOAT", 10.0f);
+		configure_stream("VFR_HUD", 10.0f);
 		configure_stream("WIND", 10.0f);
+		configure_stream("POSITION_TARGET_LOCAL_NED", 10.0f);
+		configure_stream("SYSTEM_TIME", 1.0f);
+		configure_stream("TIMESYNC", 10.0f);
+		configure_stream("CAMERA_CAPTURE", 2.0f);
+		//camera trigger is rate limited at the source, do not limit here
+		configure_stream("CAMERA_TRIGGER", 500.0f);
+		configure_stream("ACTUATOR_CONTROL_TARGET0", 10.0f);
 		break;
 
 	case MAVLINK_MODE_OSD:
 		configure_stream("SYS_STATUS", 5.0f);
+		configure_stream("EXTENDED_SYS_STATE", 1.0f);
 		configure_stream("ATTITUDE", 25.0f);
-		configure_stream("VFR_HUD", 25.0f);
-		configure_stream("GPS_RAW_INT", 1.0f);
-		configure_stream("GLOBAL_POSITION_INT", 10.0f);
-		configure_stream("HOME_POSITION", 0.5f);
-		configure_stream("ATTITUDE_TARGET", 10.0f);
-		configure_stream("SYSTEM_TIME", 1.0f);
 		configure_stream("RC_CHANNELS", 5.0f);
 		configure_stream("SERVO_OUTPUT_RAW_0", 1.0f);
-		configure_stream("EXTENDED_SYS_STATE", 1.0f);
 		configure_stream("ALTITUDE", 1.0f);
+		configure_stream("GPS_RAW_INT", 1.0f);
 		configure_stream("ESTIMATOR_STATUS", 1.0f);
+		configure_stream("GLOBAL_POSITION_INT", 10.0f);
+		configure_stream("ATTITUDE_TARGET", 10.0f);
+		configure_stream("HOME_POSITION", 0.5f);
+		configure_stream("VFR_HUD", 25.0f);
 		configure_stream("WIND", 2.0f);
+		configure_stream("SYSTEM_TIME", 1.0f);
 		break;
 
 	case MAVLINK_MODE_MAGIC:
@@ -1930,32 +1930,32 @@ Mavlink::task_main(int argc, char *argv[])
 	case MAVLINK_MODE_CONFIG:
 		// Enable a number of interesting streams we want via USB
 		configure_stream("SYS_STATUS", 1.0f);
-		configure_stream("HOME_POSITION", 0.5f);
-		configure_stream("GLOBAL_POSITION_INT", 10.0f);
-		configure_stream("ATTITUDE_TARGET", 8.0f);
-		configure_stream("MISSION_ITEM", 50.0f);
-		configure_stream("NAMED_VALUE_FLOAT", 50.0f);
-		configure_stream("OPTICAL_FLOW_RAD", 10.0f);
-		configure_stream("DISTANCE_SENSOR", 10.0f);
-		configure_stream("VFR_HUD", 20.0f);
+		configure_stream("EXTENDED_SYS_STATE", 2.0f);
+		configure_stream("HIGHRES_IMU", 50.0f);
 		configure_stream("ATTITUDE", 100.0f);
-		configure_stream("ACTUATOR_CONTROL_TARGET0", 30.0f);
 		configure_stream("RC_CHANNELS", 10.0f);
 		configure_stream("SERVO_OUTPUT_RAW_0", 20.0f);
 		configure_stream("SERVO_OUTPUT_RAW_1", 20.0f);
-		configure_stream("POSITION_TARGET_GLOBAL_INT", 10.0f);
-		configure_stream("LOCAL_POSITION_NED", 30.0f);
-		configure_stream("MANUAL_CONTROL", 5.0f);
-		configure_stream("HIGHRES_IMU", 50.0f);
-		configure_stream("GPS_RAW_INT", 10.0f);
-		configure_stream("CAMERA_TRIGGER", 500.0f);
-		configure_stream("EXTENDED_SYS_STATE", 2.0f);
 		configure_stream("ALTITUDE", 10.0f);
+		configure_stream("GPS_RAW_INT", 10.0f);
+		configure_stream("ADSB_VEHICLE", 20.0f);
+		configure_stream("DISTANCE_SENSOR", 10.0f);
+		configure_stream("OPTICAL_FLOW_RAD", 10.0f);
 		configure_stream("VISION_POSITION_NED", 10.0f);
 		configure_stream("ESTIMATOR_STATUS", 5.0f);
-		configure_stream("ADSB_VEHICLE", 20.0f);
 		configure_stream("NAV_CONTROLLER_OUTPUT", 10.0f);
+		configure_stream("GLOBAL_POSITION_INT", 10.0f);
+		configure_stream("LOCAL_POSITION_NED", 30.0f);
+		configure_stream("POSITION_TARGET_GLOBAL_INT", 10.0f);
+		configure_stream("ATTITUDE_TARGET", 8.0f);
+		configure_stream("HOME_POSITION", 0.5f);
+		configure_stream("NAMED_VALUE_FLOAT", 50.0f);
+		configure_stream("VFR_HUD", 20.0f);
 		configure_stream("WIND", 10.0f);
+		configure_stream("CAMERA_TRIGGER", 500.0f);
+		configure_stream("MISSION_ITEM", 50.0f);
+		configure_stream("ACTUATOR_CONTROL_TARGET0", 30.0f);
+		configure_stream("MANUAL_CONTROL", 5.0f);
 	default:
 		break;
 	}

From 18330f7ab7425d6e8b0fc2a6c8fa11983058d3e1 Mon Sep 17 00:00:00 2001
From: Lucas De Marchi <lucas.demarchi@intel.com>
Date: Thu, 7 Jul 2016 18:06:43 -0300
Subject: [PATCH 0976/1230] Move __STDC_FORMAT_MACROS to build system

__STDC_FORMAT_MACROS changes the behavior of inttypes.h to allow
defining format macros for printf-like functions. It needs to be defined
before any include is done, otherwise due to include chains and header
guards it may not take effect.

Instead of having to define it everywhere it is used, move the define to the
build system. Also update ecl and DriverFramework submodules to deal with the
changed definitions.
---
 Tools/generate_listener.py                      | 1 -
 cmake/common/px4_base.cmake                     | 1 +
 src/drivers/drv_hrt.h                           | 1 -
 src/drivers/px4flow/i2c_frame.h                 | 1 -
 src/lib/DriverFramework                         | 2 +-
 src/lib/controllib/block/Block.hpp              | 1 -
 src/lib/ecl                                     | 2 +-
 src/modules/px4iofirmware/protocol.h            | 1 -
 src/modules/systemlib/err.c                     | 1 -
 src/modules/systemlib/uthash/uthash.h           | 1 -
 src/platforms/posix/drivers/barosim/baro.cpp    | 1 -
 src/platforms/posix/drivers/gpssim/gpssim.cpp   | 1 -
 src/platforms/posix/drivers/gyrosim/gyrosim.cpp | 1 -
 src/platforms/posix/px4_layer/drv_hrt.c         | 1 -
 src/platforms/px4_log.h                         | 1 -
 src/platforms/px4_nodehandle.h                  | 1 -
 src/systemcmds/tests/test_bson.c                | 1 -
 src/systemcmds/tests/test_int.c                 | 1 -
 18 files changed, 3 insertions(+), 17 deletions(-)

diff --git a/Tools/generate_listener.py b/Tools/generate_listener.py
index d3b4f27d0e..59cd79c7bd 100755
--- a/Tools/generate_listener.py
+++ b/Tools/generate_listener.py
@@ -115,7 +115,6 @@ print("""
 #include <stdint.h>
 #include <stdio.h>
 #include <stdlib.h>
-#define __STDC_FORMAT_MACROS
 #include <inttypes.h>
 
 #ifndef PRIu64
diff --git a/cmake/common/px4_base.cmake b/cmake/common/px4_base.cmake
index 70b2a7d854..7b65c35071 100644
--- a/cmake/common/px4_base.cmake
+++ b/cmake/common/px4_base.cmake
@@ -787,6 +787,7 @@ function(px4_add_common_flags)
 	string(REPLACE "-" "_" board_config ${board_upper})
 	set(added_definitions
 		-DCONFIG_ARCH_BOARD_${board_config}
+		-D__STDC_FORMAT_MACROS
 		)
 
 	if (NOT (APPLE AND (${CMAKE_C_COMPILER_ID} MATCHES ".*Clang.*")))
diff --git a/src/drivers/drv_hrt.h b/src/drivers/drv_hrt.h
index beaf9b1ad9..befbfe799d 100644
--- a/src/drivers/drv_hrt.h
+++ b/src/drivers/drv_hrt.h
@@ -41,7 +41,6 @@
 
 #include <sys/types.h>
 #include <stdbool.h>
-#define __STDC_FORMAT_MACROS
 #include <inttypes.h>
 
 #include <px4_time.h>
diff --git a/src/drivers/px4flow/i2c_frame.h b/src/drivers/px4flow/i2c_frame.h
index f73d4a92cc..ec8a5e1fda 100644
--- a/src/drivers/px4flow/i2c_frame.h
+++ b/src/drivers/px4flow/i2c_frame.h
@@ -41,7 +41,6 @@
 #ifndef I2C_FRAME_H_
 #define I2C_FRAME_H_
 
-#define __STDC_FORMAT_MACROS
 #include <inttypes.h>
 
 typedef  struct i2c_frame {
diff --git a/src/lib/DriverFramework b/src/lib/DriverFramework
index 7c468a60ae..ed8550b1ec 160000
--- a/src/lib/DriverFramework
+++ b/src/lib/DriverFramework
@@ -1 +1 @@
-Subproject commit 7c468a60ae64726fb42989fe552feb21b7978289
+Subproject commit ed8550b1ec4d8a71e3512bb6712e28e8f4d8a6cc
diff --git a/src/lib/controllib/block/Block.hpp b/src/lib/controllib/block/Block.hpp
index e74258b523..7c2fd4a524 100644
--- a/src/lib/controllib/block/Block.hpp
+++ b/src/lib/controllib/block/Block.hpp
@@ -39,7 +39,6 @@
 
 #pragma once
 
-#define __STDC_FORMAT_MACROS
 #include <stdint.h>
 #include <inttypes.h>
 
diff --git a/src/lib/ecl b/src/lib/ecl
index a2d866520a..63b95a1738 160000
--- a/src/lib/ecl
+++ b/src/lib/ecl
@@ -1 +1 @@
-Subproject commit a2d866520a0477574d0ade68b0f5e7becb57431b
+Subproject commit 63b95a17382bb4f930ac56ae2ddf6fdeb157ac73
diff --git a/src/modules/px4iofirmware/protocol.h b/src/modules/px4iofirmware/protocol.h
index f61e56d204..6c984ccac5 100644
--- a/src/modules/px4iofirmware/protocol.h
+++ b/src/modules/px4iofirmware/protocol.h
@@ -33,7 +33,6 @@
 
 #pragma once
 
-#define __STDC_FORMAT_MACROS
 #include <inttypes.h>
 
 /**
diff --git a/src/modules/systemlib/err.c b/src/modules/systemlib/err.c
index 78e3058068..469c06175b 100644
--- a/src/modules/systemlib/err.c
+++ b/src/modules/systemlib/err.c
@@ -40,7 +40,6 @@
 
 #include <px4_config.h>
 
-#define __STDC_FORMAT_MACROS
 #include <inttypes.h>
 
 #include <stdlib.h>
diff --git a/src/modules/systemlib/uthash/uthash.h b/src/modules/systemlib/uthash/uthash.h
index a8642833f6..9f83fc34f1 100644
--- a/src/modules/systemlib/uthash/uthash.h
+++ b/src/modules/systemlib/uthash/uthash.h
@@ -61,7 +61,6 @@ do {
 typedef unsigned int uint32_t;
 typedef unsigned char uint8_t;
 #else
-#define __STDC_FORMAT_MACROS
 #include <inttypes.h>   /* uint32_t */
 #endif
 
diff --git a/src/platforms/posix/drivers/barosim/baro.cpp b/src/platforms/posix/drivers/barosim/baro.cpp
index 0ab376ed3d..248a20fef8 100644
--- a/src/platforms/posix/drivers/barosim/baro.cpp
+++ b/src/platforms/posix/drivers/barosim/baro.cpp
@@ -36,7 +36,6 @@
  * Driver for the simulated barometric pressure sensor
  */
 
-#define __STDC_FORMAT_MACROS
 #include <inttypes.h>
 #include <px4_config.h>
 #include <px4_defines.h>
diff --git a/src/platforms/posix/drivers/gpssim/gpssim.cpp b/src/platforms/posix/drivers/gpssim/gpssim.cpp
index 0b3f4ba2b2..8eb462585a 100644
--- a/src/platforms/posix/drivers/gpssim/gpssim.cpp
+++ b/src/platforms/posix/drivers/gpssim/gpssim.cpp
@@ -37,7 +37,6 @@
  */
 
 #include <sys/types.h>
-#define __STDC_FORMAT_MACROS
 #include <inttypes.h>
 #include <stdio.h>
 #include <stdbool.h>
diff --git a/src/platforms/posix/drivers/gyrosim/gyrosim.cpp b/src/platforms/posix/drivers/gyrosim/gyrosim.cpp
index 6fb35a7dbe..349fe461c6 100644
--- a/src/platforms/posix/drivers/gyrosim/gyrosim.cpp
+++ b/src/platforms/posix/drivers/gyrosim/gyrosim.cpp
@@ -41,7 +41,6 @@
  * @author Mark Charlebois
  */
 
-#define __STDC_FORMAT_MACROS
 #include <inttypes.h>
 
 #include <px4_config.h>
diff --git a/src/platforms/posix/px4_layer/drv_hrt.c b/src/platforms/posix/px4_layer/drv_hrt.c
index bdd8dfd786..70a9c5f02d 100644
--- a/src/platforms/posix/px4_layer/drv_hrt.c
+++ b/src/platforms/posix/px4_layer/drv_hrt.c
@@ -45,7 +45,6 @@
 #include <semaphore.h>
 #include <time.h>
 #include <string.h>
-#define __STDC_FORMAT_MACROS
 #include <inttypes.h>
 #include <errno.h>
 #include "hrt_work.h"
diff --git a/src/platforms/px4_log.h b/src/platforms/px4_log.h
index 53af0bb2ed..1eb13a0e77 100644
--- a/src/platforms/px4_log.h
+++ b/src/platforms/px4_log.h
@@ -118,7 +118,6 @@ static inline void do_nothing(int level, ...)
 
 #else
 
-#define __STDC_FORMAT_MACROS
 #include <inttypes.h>
 #include <stdint.h>
 #include <sys/cdefs.h>
diff --git a/src/platforms/px4_nodehandle.h b/src/platforms/px4_nodehandle.h
index 31afbfd031..9ae11474de 100644
--- a/src/platforms/px4_nodehandle.h
+++ b/src/platforms/px4_nodehandle.h
@@ -48,7 +48,6 @@
 /* includes when building for ros */
 #include "ros/ros.h"
 #include <list>
-#define __STDC_FORMAT_MACROS
 #include <inttypes.h>
 #include <type_traits>
 #else
diff --git a/src/systemcmds/tests/test_bson.c b/src/systemcmds/tests/test_bson.c
index 945b00aa51..7a168cac5a 100644
--- a/src/systemcmds/tests/test_bson.c
+++ b/src/systemcmds/tests/test_bson.c
@@ -37,7 +37,6 @@
  * Tests for the bson en/decoder
  */
 
-#define __STDC_FORMAT_MACROS
 #include <inttypes.h>
 
 #include <px4_defines.h>
diff --git a/src/systemcmds/tests/test_int.c b/src/systemcmds/tests/test_int.c
index 01092aa2d4..7db4b48f9b 100644
--- a/src/systemcmds/tests/test_int.c
+++ b/src/systemcmds/tests/test_int.c
@@ -36,7 +36,6 @@
  * Included Files
  ****************************************************************************/
 
-#define __STDC_FORMAT_MACROS
 #include <inttypes.h>
 
 #include <px4_config.h>

From 02b3adc4a70b7e958218264f88c5ffebe0990c88 Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Sun, 10 Jul 2016 16:32:59 +0200
Subject: [PATCH 0977/1230] Update DriverFramework

---
 src/lib/DriverFramework | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/src/lib/DriverFramework b/src/lib/DriverFramework
index ed8550b1ec..db5a1f91aa 160000
--- a/src/lib/DriverFramework
+++ b/src/lib/DriverFramework
@@ -1 +1 @@
-Subproject commit ed8550b1ec4d8a71e3512bb6712e28e8f4d8a6cc
+Subproject commit db5a1f91aab361dbde1fbbc75b792080745832d4

From c7130081cc42a3eaa9dcd971085bf81ba8503fa4 Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Sun, 10 Jul 2016 16:33:07 +0200
Subject: [PATCH 0978/1230] Update ECL

---
 src/lib/ecl | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/src/lib/ecl b/src/lib/ecl
index 63b95a1738..2240eb6b4f 160000
--- a/src/lib/ecl
+++ b/src/lib/ecl
@@ -1 +1 @@
-Subproject commit 63b95a17382bb4f930ac56ae2ddf6fdeb157ac73
+Subproject commit 2240eb6b4fd0b1603b9d3275ac29a6b3ab3f66e0

From 71d150f6ec0789d36d4f7af85a2e44c225846d51 Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Sun, 10 Jul 2016 17:05:33 +0200
Subject: [PATCH 0979/1230] MAVLink app: Fix rate handling

---
 src/modules/mavlink/mavlink_main.cpp       | 41 ++++++++++++----------
 src/modules/mavlink/mavlink_messages.cpp   | 17 ++++-----
 src/modules/mavlink/mavlink_parameters.cpp |  6 ++++
 src/modules/mavlink/mavlink_parameters.h   |  2 ++
 src/modules/mavlink/mavlink_stream.h       | 10 ++++++
 5 files changed, 50 insertions(+), 26 deletions(-)

diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp
index ee804996fd..d160320dc0 100644
--- a/src/modules/mavlink/mavlink_main.cpp
+++ b/src/modules/mavlink/mavlink_main.cpp
@@ -1521,15 +1521,20 @@ Mavlink::update_rate_mult()
 	MavlinkStream *stream;
 	LL_FOREACH(_streams, stream) {
 		if (stream->const_rate()) {
-			const_rate += stream->get_size() * 1000000.0f / stream->get_interval();
+			const_rate += (stream->get_interval() > 0) ? stream->get_size_avg() * 1000000.0f / stream->get_interval() : 0;
 
 		} else {
-			rate += stream->get_size() * 1000000.0f / stream->get_interval();
+			rate += (stream->get_interval() > 0) ? stream->get_size_avg() * 1000000.0f / stream->get_interval() : 0;
 		}
 	}
 
-	/* don't scale up rates, only scale down if needed */
-	float bandwidth_mult = fminf(1.0f, ((float)_datarate - const_rate) / rate);
+	/* scale up and down as the link permits */
+	float bandwidth_mult = (float)(_datarate - const_rate) / rate;
+
+	/* if we do not have flow control, limit to the set data rate */
+	if (!get_flow_control_enabled()) {
+		bandwidth_mult = fminf(1.0f, bandwidth_mult);
+	}
 
 	/* check if we have radio feedback */
 	struct telemetry_status_s &tstatus = get_rx_status();
@@ -1852,26 +1857,26 @@ Mavlink::task_main(int argc, char *argv[])
 	case MAVLINK_MODE_NORMAL:
 		configure_stream("SYS_STATUS", 1.0f);
 		configure_stream("EXTENDED_SYS_STATE", 1.0f);
-		configure_stream("HIGHRES_IMU", 2.0f);
+		configure_stream("HIGHRES_IMU", 1.5f);
 		configure_stream("ATTITUDE", 20.0f);
-		configure_stream("RC_CHANNELS", 1.0f);
+		configure_stream("RC_CHANNELS", 5.0f);
 		configure_stream("SERVO_OUTPUT_RAW_0", 1.0f);
 		configure_stream("ALTITUDE", 1.0f);
 		configure_stream("GPS_RAW_INT", 1.0f);
 		configure_stream("ADSB_VEHICLE", 2.0f);
 		configure_stream("DISTANCE_SENSOR", 0.5f);
-		configure_stream("OPTICAL_FLOW_RAD", 5.0f);
-		configure_stream("VISION_POSITION_NED", 10.0f);
+		configure_stream("OPTICAL_FLOW_RAD", 1.0f);
+		configure_stream("VISION_POSITION_NED", 1.0f);
 		configure_stream("ESTIMATOR_STATUS", 0.5f);
-		configure_stream("NAV_CONTROLLER_OUTPUT", 2.0f);
-		configure_stream("GLOBAL_POSITION_INT", 3.0f);
-		configure_stream("LOCAL_POSITION_NED", 3.0f);
-		configure_stream("POSITION_TARGET_GLOBAL_INT", 3.0f);
-		configure_stream("ATTITUDE_TARGET", 8.0f);
+		configure_stream("NAV_CONTROLLER_OUTPUT", 1.5f);
+		configure_stream("GLOBAL_POSITION_INT", 5.0f);
+		configure_stream("LOCAL_POSITION_NED", 1.0f);
+		configure_stream("POSITION_TARGET_GLOBAL_INT", 1.5f);
+		configure_stream("ATTITUDE_TARGET", 2.0f);
 		configure_stream("HOME_POSITION", 0.5f);
 		configure_stream("NAMED_VALUE_FLOAT", 1.0f);
-		configure_stream("VFR_HUD", 8.0f);
-		configure_stream("WIND", 2.0f);
+		configure_stream("VFR_HUD", 4.0f);
+		configure_stream("WIND_COV", 1.0f);
 		break;
 
 	case MAVLINK_MODE_ONBOARD:
@@ -1896,7 +1901,7 @@ Mavlink::task_main(int argc, char *argv[])
 		configure_stream("HOME_POSITION", 0.5f);
 		configure_stream("NAMED_VALUE_FLOAT", 10.0f);
 		configure_stream("VFR_HUD", 10.0f);
-		configure_stream("WIND", 10.0f);
+		configure_stream("WIND_COV", 10.0f);
 		configure_stream("POSITION_TARGET_LOCAL_NED", 10.0f);
 		configure_stream("SYSTEM_TIME", 1.0f);
 		configure_stream("TIMESYNC", 10.0f);
@@ -1919,7 +1924,7 @@ Mavlink::task_main(int argc, char *argv[])
 		configure_stream("ATTITUDE_TARGET", 10.0f);
 		configure_stream("HOME_POSITION", 0.5f);
 		configure_stream("VFR_HUD", 25.0f);
-		configure_stream("WIND", 2.0f);
+		configure_stream("WIND_COV", 2.0f);
 		configure_stream("SYSTEM_TIME", 1.0f);
 		break;
 
@@ -1951,7 +1956,7 @@ Mavlink::task_main(int argc, char *argv[])
 		configure_stream("HOME_POSITION", 0.5f);
 		configure_stream("NAMED_VALUE_FLOAT", 50.0f);
 		configure_stream("VFR_HUD", 20.0f);
-		configure_stream("WIND", 10.0f);
+		configure_stream("WIND_COV", 10.0f);
 		configure_stream("CAMERA_TRIGGER", 500.0f);
 		configure_stream("MISSION_ITEM", 50.0f);
 		configure_stream("ACTUATOR_CONTROL_TARGET0", 30.0f);
diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp
index b12ea1fb9d..b366c9940e 100644
--- a/src/modules/mavlink/mavlink_messages.cpp
+++ b/src/modules/mavlink/mavlink_messages.cpp
@@ -1219,7 +1219,7 @@ public:
 
 	unsigned get_size()
 	{
-		return MAVLINK_MSG_ID_ADSB_VEHICLE_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES;
+		return (_pos_time > 0) ? MAVLINK_MSG_ID_ADSB_VEHICLE_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES : 0;
 	}
 
 private:
@@ -1292,7 +1292,7 @@ public:
 
 	unsigned get_size()
 	{
-		return MAVLINK_MSG_ID_CAMERA_TRIGGER_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES;
+		return (_trigger_time > 0) ? MAVLINK_MSG_ID_CAMERA_TRIGGER_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES : 0;
 	}
 
 private:
@@ -1435,7 +1435,7 @@ public:
 
     unsigned get_size()
     {
-        return MAVLINK_MSG_ID_VISION_POSITION_ESTIMATE_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES;
+        return (_pos_time > 0) ? MAVLINK_MSG_ID_VISION_POSITION_ESTIMATE_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES : 0;
     }
 private:
 
@@ -2674,7 +2674,7 @@ public:
 
 	unsigned get_size()
 	{
-		return MAVLINK_MSG_ID_NAMED_VALUE_FLOAT_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES;
+		return (_debug_time > 0) ? MAVLINK_MSG_ID_NAMED_VALUE_FLOAT_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES : 0;
 	}
 
 private:
@@ -2739,7 +2739,8 @@ public:
 
 	unsigned get_size()
 	{
-		return MAVLINK_MSG_ID_NAV_CONTROLLER_OUTPUT + MAVLINK_NUM_NON_PAYLOAD_BYTES;
+		return (_fw_pos_ctrl_status_sub->is_published()) ?
+				MAVLINK_MSG_ID_NAV_CONTROLLER_OUTPUT + MAVLINK_NUM_NON_PAYLOAD_BYTES : 0;
 	}
 
 private:
@@ -3068,7 +3069,7 @@ public:
 
 	unsigned get_size()
 	{
-		return MAVLINK_MSG_ID_ALTITUDE_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES;
+		return (_local_pos_time > 0) ? MAVLINK_MSG_ID_ALTITUDE_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES : 0;
 	}
 
 private:
@@ -3147,7 +3148,7 @@ public:
 
 	static const char *get_name_static()
 	{
-		return "WIND";
+		return "WIND_COV";
 	}
 
 	static uint8_t get_id_static()
@@ -3167,7 +3168,7 @@ public:
 
 	unsigned get_size()
 	{
-		return MAVLINK_MSG_ID_WIND_COV_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES;
+		return (_wind_estimate_time > 0) ? MAVLINK_MSG_ID_WIND_COV_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES : 0;
 	}
 
 private:
diff --git a/src/modules/mavlink/mavlink_parameters.cpp b/src/modules/mavlink/mavlink_parameters.cpp
index d6b7ef090c..b846daf1e0 100644
--- a/src/modules/mavlink/mavlink_parameters.cpp
+++ b/src/modules/mavlink/mavlink_parameters.cpp
@@ -73,6 +73,12 @@ MavlinkParametersManager::get_size()
 	return MAVLINK_MSG_ID_PARAM_VALUE_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES;
 }
 
+unsigned
+MavlinkParametersManager::get_size_avg()
+{
+	return 0;
+}
+
 void
 MavlinkParametersManager::handle_message(const mavlink_message_t *msg)
 {
diff --git a/src/modules/mavlink/mavlink_parameters.h b/src/modules/mavlink/mavlink_parameters.h
index d0a14d1cd8..1ef29411c7 100644
--- a/src/modules/mavlink/mavlink_parameters.h
+++ b/src/modules/mavlink/mavlink_parameters.h
@@ -73,6 +73,8 @@ public:
 
 	unsigned get_size();
 
+	unsigned get_size_avg();
+
 	void handle_message(const mavlink_message_t *msg);
 
 private:
diff --git a/src/modules/mavlink/mavlink_stream.h b/src/modules/mavlink/mavlink_stream.h
index c37b791b04..f77a8d94a1 100644
--- a/src/modules/mavlink/mavlink_stream.h
+++ b/src/modules/mavlink/mavlink_stream.h
@@ -86,6 +86,16 @@ public:
 	 */
 	virtual unsigned get_size() = 0;
 
+	/**
+	 * Get the average message size
+	 *
+	 * For a normal stream this equals the message size,
+	 * for something like a parameter or mission message
+	 * this equals usually zero, as no bandwidth
+	 * needs to be reserved
+	 */
+	virtual unsigned get_size_avg() { return get_size(); }
+
 protected:
 	Mavlink     *_mavlink;
 	unsigned int _interval;

From 80134d2b7d7369eed420d5bbd7202637cd240dfd Mon Sep 17 00:00:00 2001
From: Randy Mackay <rmackay9@yahoo.com>
Date: Mon, 11 Jul 2016 16:05:29 +0900
Subject: [PATCH 0980/1230] IR-LOCK: rework driver (from jschall) (#5024)

* irlock: change output format to tangent of angles

* irlock: put all targets in single struct

* irlock: eliminate tanf function in constant
---
 src/drivers/drv_irlock.h      | 15 ++++---
 src/drivers/irlock/irlock.cpp | 78 ++++++++++++++++++++---------------
 2 files changed, 54 insertions(+), 39 deletions(-)

diff --git a/src/drivers/drv_irlock.h b/src/drivers/drv_irlock.h
index fa6b3016cd..ea0eac8d75 100644
--- a/src/drivers/drv_irlock.h
+++ b/src/drivers/drv_irlock.h
@@ -49,12 +49,17 @@
 
 #define IRLOCK_OBJECTS_MAX	5	/** up to 5 objects can be detected/reported **/
 
+struct irlock_target_s {
+	uint16_t signature;	/** target signature **/
+	float pos_x;	/** x-axis distance from center of image to center of target in units of tan(theta) **/
+	float pos_y;	/** y-axis distance from center of image to center of target in units of tan(theta) **/
+	float size_x;	/** size of target along x-axis in units of tan(theta) **/
+	float size_y;	/** size of target along y-axis in units of tan(theta) **/
+};
+
 /** irlock_s structure returned from read calls **/
 struct irlock_s {
 	uint64_t timestamp; /** microseconds since system start **/
-	uint16_t target_num;	/** target number prioritised by size (largest is 0) **/
-	float angle_x;	/** x-axis angle in radians from center of image to center of target **/
-	float angle_y;	/** y-axis angle in radians from center of image to center of target **/
-	float size_x;	/** size in radians of target along x-axis **/
-	float size_y;	/** size in radians of target along y-axis **/
+	uint8_t num_targets;
+	struct irlock_target_s targets[IRLOCK_OBJECTS_MAX];
 };
diff --git a/src/drivers/irlock/irlock.cpp b/src/drivers/irlock/irlock.cpp
index 620cbf0824..e412bb57cc 100644
--- a/src/drivers/irlock/irlock.cpp
+++ b/src/drivers/irlock/irlock.cpp
@@ -67,10 +67,20 @@
 #define IRLOCK_RESYNC		0x5500
 #define IRLOCK_ADJUST		0xAA
 
-#define IRLOCK_CENTER_X				159			// the x-axis center pixel position
-#define IRLOCK_CENTER_Y				99			// the y-axis center pixel position
-#define IRLOCK_PIXELS_PER_RADIAN_X	307.9075f	// x-axis pixel to radian scaler assuming 60deg FOV on x-axis
-#define IRLOCK_PIXELS_PER_RADIAN_Y	326.4713f	// y-axis pixel to radian scaler assuming 35deg FOV on y-axis
+#define IRLOCK_RES_X 320
+#define IRLOCK_RES_Y 200
+
+#define IRLOCK_CENTER_X				(IRLOCK_RES_X/2)			// the x-axis center pixel position
+#define IRLOCK_CENTER_Y				(IRLOCK_RES_Y/2)			// the y-axis center pixel position
+
+#define IRLOCK_FOV_X (60.0f*M_PI_F/180.0f)
+#define IRLOCK_FOV_Y (35.0f*M_PI_F/180.0f)
+
+#define IRLOCK_TAN_HALF_FOV_X 0.57735026919f // tan(0.5 * 60 * pi/180)
+#define IRLOCK_TAN_HALF_FOV_Y 0.31529878887f // tan(0.5 * 35 * pi/180)
+
+#define IRLOCK_TAN_ANG_PER_PIXEL_X	(2*IRLOCK_TAN_HALF_FOV_X/IRLOCK_RES_X)
+#define IRLOCK_TAN_ANG_PER_PIXEL_Y	(2*IRLOCK_TAN_HALF_FOV_Y/IRLOCK_RES_Y)
 
 #ifndef CONFIG_SCHED_WORKQUEUE
 # error This requires CONFIG_SCHED_WORKQUEUE.
@@ -107,7 +117,7 @@ private:
 	int 		read_device();
 	bool 		sync_device();
 	int 		read_device_word(uint16_t *word);
-	int 		read_device_block(struct irlock_s *block);
+	int 		read_device_block(struct irlock_target_s *block);
 
 	/** internal variables **/
 	ringbuffer::RingBuffer *_reports;
@@ -158,7 +168,7 @@ int IRLOCK::init()
 	}
 
 	/** allocate buffer storing values read from sensor **/
-	_reports = new ringbuffer::RingBuffer(IRLOCK_OBJECTS_MAX, sizeof(struct irlock_s));
+	_reports = new ringbuffer::RingBuffer(2, sizeof(struct irlock_s));
 
 	if (_reports == nullptr) {
 		return ENOTTY;
@@ -224,20 +234,20 @@ int IRLOCK::test()
 	warnx("searching for object for 10 seconds");
 
 	/** read from sensor for 10 seconds **/
-	struct irlock_s obj_report;
+	struct irlock_s report;
 	uint64_t start_time = hrt_absolute_time();
 
 	while ((hrt_absolute_time() - start_time) < 10000000) {
-
-		/** output all objects found **/
-		while (_reports->count() > 0) {
-			_reports->get(&obj_report);
-			warnx("sig:%d x:%4.3f y:%4.3f width:%4.3f height:%4.3f",
-			      (int)obj_report.target_num,
-			      (double)obj_report.angle_x,
-			      (double)obj_report.angle_y,
-			      (double)obj_report.size_x,
-			      (double)obj_report.size_y);
+		if (_reports->get(&report)) {
+			/** output all objects found **/
+			for (uint8_t i = 0; i < report.num_targets; i++) {
+				warnx("sig:%d x:%4.3f y:%4.3f width:%4.3f height:%4.3f",
+				      (int)report.targets[i].signature,
+				      (double)report.targets[i].pos_x,
+				      (double)report.targets[i].pos_y,
+				      (double)report.targets[i].size_x,
+				      (double)report.targets[i].size_y);
+			}
 		}
 
 		/** sleep for 0.05 seconds **/
@@ -337,20 +347,22 @@ int IRLOCK::read_device()
 		return -ENOTTY;
 	}
 
-	/** now read blocks until sync stops, first flush stale queue data **/
-	_reports->flush();
-	int num_objects = 0;
+	struct irlock_s report;
 
-	while (sync_device() && (num_objects < IRLOCK_OBJECTS_MAX)) {
-		struct irlock_s block;
+	report.timestamp = hrt_absolute_time();
 
-		if (read_device_block(&block) != OK) {
+	report.num_targets = 0;
+
+	while (report.num_targets < IRLOCK_OBJECTS_MAX) {
+		if (!sync_device() || read_device_block(&report.targets[report.num_targets]) != OK) {
 			break;
 		}
 
-		_reports->force(&block);
+		report.num_targets++;
 	}
 
+	_reports->force(&report);
+
 	return OK;
 }
 
@@ -367,33 +379,31 @@ int IRLOCK::read_device_word(uint16_t *word)
 }
 
 /** read a single block (a full frame) from sensor **/
-int IRLOCK::read_device_block(struct irlock_s *block)
+int IRLOCK::read_device_block(struct irlock_target_s *block)
 {
 	uint8_t bytes[12];
 	memset(bytes, 0, sizeof bytes);
 
 	int status = transfer(nullptr, 0, &bytes[0], 12);
 	uint16_t checksum = bytes[1] << 8 | bytes[0];
-	uint16_t target_num = bytes[3] << 8 | bytes[2];
+	uint16_t signature = bytes[3] << 8 | bytes[2];
 	uint16_t pixel_x = bytes[5] << 8 | bytes[4];
 	uint16_t pixel_y = bytes[7] << 8 | bytes[6];
 	uint16_t pixel_size_x = bytes[9] << 8 | bytes[8];
 	uint16_t pixel_size_y = bytes[11] << 8 | bytes[10];
 
 	/** crc check **/
-	if (target_num + pixel_x + pixel_y + pixel_size_x + pixel_size_y != checksum) {
+	if (signature + pixel_x + pixel_y + pixel_size_x + pixel_size_y != checksum) {
 		_read_failures++;
 		return -EIO;
 	}
 
 	/** convert to angles **/
-	block->target_num = target_num;
-	block->angle_x = (((float)(pixel_x - IRLOCK_CENTER_X)) / IRLOCK_PIXELS_PER_RADIAN_X);
-	block->angle_y = (((float)(pixel_y - IRLOCK_CENTER_Y)) / IRLOCK_PIXELS_PER_RADIAN_Y);
-	block->size_x = pixel_size_x / IRLOCK_PIXELS_PER_RADIAN_X;
-	block->size_y = pixel_size_y / IRLOCK_PIXELS_PER_RADIAN_Y;
-
-	block->timestamp = hrt_absolute_time();
+	block->signature = signature;
+	block->pos_x = (pixel_x - IRLOCK_CENTER_X) * IRLOCK_TAN_ANG_PER_PIXEL_X;
+	block->pos_y = (pixel_y - IRLOCK_CENTER_Y) * IRLOCK_TAN_ANG_PER_PIXEL_Y;
+	block->size_x = pixel_size_x * IRLOCK_TAN_ANG_PER_PIXEL_X;
+	block->size_y = pixel_size_y * IRLOCK_TAN_ANG_PER_PIXEL_Y;
 	return status;
 }
 

From e9726af54cbc2eb1a849f0d42e2ea9e2bac30654 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Beat=20K=C3=BCng?= <beat-kueng@gmx.net>
Date: Thu, 30 Jun 2016 15:16:52 +0200
Subject: [PATCH 0981/1230] Makefile: check for 'replay' env variable and
 change build dir accordingly

For replay with the new logger, we want a separate build dir so that the
parameters file and orb rules don't get mixed up.
---
 Makefile | 13 ++++++++++---
 1 file changed, 10 insertions(+), 3 deletions(-)

diff --git a/Makefile b/Makefile
index 31978311b2..adfa2654fb 100644
--- a/Makefile
+++ b/Makefile
@@ -104,14 +104,21 @@ endif
     PX4_MAKE_ARGS = -j$(j) --no-print-directory
 endif
 
+# check if replay env variable is set & set build dir accordingly
+ifdef replay
+	BUILD_DIR_SUFFIX := _replay
+else
+	BUILD_DIR_SUFFIX :=
+endif
+
 # Functions
 # --------------------------------------------------------------------
 # describe how to build a cmake config
 define cmake-build
-+@if [ $(PX4_CMAKE_GENERATOR) = "Ninja" ] && [ -e $(PWD)/build_$@/Makefile ]; then rm -rf $(PWD)/build_$@; fi
-+@if [ ! -e $(PWD)/build_$@/CMakeCache.txt ]; then Tools/check_submodules.sh && mkdir -p $(PWD)/build_$@ && cd $(PWD)/build_$@ && cmake .. -G$(PX4_CMAKE_GENERATOR) -DCONFIG=$(1) || (cd .. && rm -rf $(PWD)/build_$@); fi
++@if [ $(PX4_CMAKE_GENERATOR) = "Ninja" ] && [ -e $(PWD)/build_$@$(BUILD_DIR_SUFFIX)/Makefile ]; then rm -rf $(PWD)/build_$@$(BUILD_DIR_SUFFIX); fi
++@if [ ! -e $(PWD)/build_$@$(BUILD_DIR_SUFFIX)/CMakeCache.txt ]; then Tools/check_submodules.sh && mkdir -p $(PWD)/build_$@$(BUILD_DIR_SUFFIX) && cd $(PWD)/build_$@$(BUILD_DIR_SUFFIX) && cmake .. -G$(PX4_CMAKE_GENERATOR) -DCONFIG=$(1) || (cd .. && rm -rf $(PWD)/build_$@$(BUILD_DIR_SUFFIX)); fi
 +@Tools/check_submodules.sh
-+@(echo "PX4 CONFIG: $@" && cd $(PWD)/build_$@ && $(PX4_MAKE) $(PX4_MAKE_ARGS) $(ARGS))
++@(echo "PX4 CONFIG: $@$(BUILD_DIR_SUFFIX)" && cd $(PWD)/build_$@$(BUILD_DIR_SUFFIX) && $(PX4_MAKE) $(PX4_MAKE_ARGS) $(ARGS))
 endef
 
 define cmake-build-other

From db13ac4c531a06b6a25fbe73d8ad30ec407e9a13 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Beat=20K=C3=BCng?= <beat-kueng@gmx.net>
Date: Thu, 30 Jun 2016 15:17:44 +0200
Subject: [PATCH 0982/1230] posix_sitl_default.cmake: add
 -DORB_USE_PUBLISHER_RULES if it's a replay build

---
 cmake/configs/posix_sitl_default.cmake | 9 +++++++++
 1 file changed, 9 insertions(+)

diff --git a/cmake/configs/posix_sitl_default.cmake b/cmake/configs/posix_sitl_default.cmake
index 5721e30d45..07b77c8478 100644
--- a/cmake/configs/posix_sitl_default.cmake
+++ b/cmake/configs/posix_sitl_default.cmake
@@ -112,3 +112,12 @@ set(config_sitl_debugger
 	)
 set_property(CACHE config_sitl_debugger
 	PROPERTY STRINGS "disable;gdb;lldb")
+
+# If the environment variable 'replay' is defined, we are building with replay
+# support. In this case, we enable the orb publisher rules.
+set(REPLAY_FILE "$ENV{replay}")
+if(REPLAY_FILE)
+	message("Building with uorb publisher rules support")
+	add_definitions(-DORB_USE_PUBLISHER_RULES)
+endif()
+

From 49614cfe38a46d98557c559cc7eed798462c6a28 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Beat=20K=C3=BCng?= <beat-kueng@gmx.net>
Date: Thu, 30 Jun 2016 15:41:17 +0200
Subject: [PATCH 0983/1230] SITL init scripts: add replay tryapplyparams &
 replay trystart

These will only start replay if there's an environment variable 'replay',
otherwise they do nothing.

We apply the parameters even before loading the modules, so that the
modules 'see' the same state as the recorded system had. We will have to
see how well this works in practice.
---
 posix-configs/SITL/init/rcS_gazebo_iris              | 2 ++
 posix-configs/SITL/init/rcS_gazebo_iris_opt_flow     | 2 ++
 posix-configs/SITL/init/rcS_gazebo_plane             | 2 ++
 posix-configs/SITL/init/rcS_gazebo_solo              | 2 ++
 posix-configs/SITL/init/rcS_gazebo_standard_vtol     | 2 ++
 posix-configs/SITL/init/rcS_gazebo_tailsitter        | 2 ++
 posix-configs/SITL/init/rcS_gazebo_typhoon_h480      | 2 ++
 posix-configs/SITL/init/rcS_jmavsim_iris             | 4 ++++
 posix-configs/SITL/init/rcS_lpe_gazebo_iris          | 2 ++
 posix-configs/SITL/init/rcS_lpe_gazebo_iris_opt_flow | 2 ++
 posix-configs/SITL/init/rcS_multiple_gazebo_iris     | 2 ++
 11 files changed, 24 insertions(+)

diff --git a/posix-configs/SITL/init/rcS_gazebo_iris b/posix-configs/SITL/init/rcS_gazebo_iris
index e0be74519c..02add40647 100644
--- a/posix-configs/SITL/init/rcS_gazebo_iris
+++ b/posix-configs/SITL/init/rcS_gazebo_iris
@@ -41,6 +41,7 @@ param set MPC_Z_VEL_P 0.6
 param set MPC_Z_VEL_I 0.15
 param set EKF2_GBIAS_INIT 0.01
 param set EKF2_ANGERR_INIT 0.01
+replay tryapplyparams
 simulator start -s
 rgbledsim start
 tone_alarm start
@@ -73,3 +74,4 @@ mavlink stream -r 250 -s HIGHRES_IMU -u 14556
 mavlink stream -r 10 -s OPTICAL_FLOW_RAD -u 14556
 sdlog2 start -r 100 -e -t -a
 mavlink boot_complete
+replay trystart
diff --git a/posix-configs/SITL/init/rcS_gazebo_iris_opt_flow b/posix-configs/SITL/init/rcS_gazebo_iris_opt_flow
index d6a10f8d0e..2be4900b81 100644
--- a/posix-configs/SITL/init/rcS_gazebo_iris_opt_flow
+++ b/posix-configs/SITL/init/rcS_gazebo_iris_opt_flow
@@ -40,6 +40,7 @@ param set MPC_Z_VEL_P 0.8
 param set MPC_Z_VEL_I 0.15
 param set MPC_XY_VEL_P 0.15
 param set MPC_XY_VEL_I 0.2
+replay tryapplyparams
 simulator start -s
 rgbledsim start
 tone_alarm start
@@ -72,3 +73,4 @@ mavlink stream -r 250 -s HIGHRES_IMU -u 14556
 mavlink stream -r 10 -s OPTICAL_FLOW_RAD -u 14556
 sdlog2 start -r 100 -e -t -a
 mavlink boot_complete
+replay trystart
diff --git a/posix-configs/SITL/init/rcS_gazebo_plane b/posix-configs/SITL/init/rcS_gazebo_plane
index 42bb9fe76f..19844e69bd 100644
--- a/posix-configs/SITL/init/rcS_gazebo_plane
+++ b/posix-configs/SITL/init/rcS_gazebo_plane
@@ -25,6 +25,7 @@ param set COM_RC_IN_MODE 1
 param set NAV_DLL_ACT 2
 param set NAV_ACC_RAD 15.0
 param set FW_THR_IDLE 0.8
+replay tryapplyparams
 simulator start -s
 rgbledsim start
 tone_alarm start
@@ -57,3 +58,4 @@ mavlink stream -r 250 -s HIGHRES_IMU -u 14556
 mavlink stream -r 10 -s OPTICAL_FLOW_RAD -u 14556
 sdlog2 start -r 200 -e -t -a
 mavlink boot_complete
+replay trystart
diff --git a/posix-configs/SITL/init/rcS_gazebo_solo b/posix-configs/SITL/init/rcS_gazebo_solo
index 62fc557bfa..a075fdff30 100644
--- a/posix-configs/SITL/init/rcS_gazebo_solo
+++ b/posix-configs/SITL/init/rcS_gazebo_solo
@@ -38,6 +38,7 @@ param set MPC_Z_VEL_P 0.6
 param set MPC_Z_VEL_I 0.15
 param set EKF2_GBIAS_INIT 0.01
 param set EKF2_ANGERR_INIT 0.01
+replay tryapplyparams
 simulator start -s
 rgbledsim start
 tone_alarm start
@@ -70,3 +71,4 @@ mavlink stream -r 250 -s HIGHRES_IMU -u 14556
 mavlink stream -r 10 -s OPTICAL_FLOW_RAD -u 14556
 sdlog2 start -r 100 -e -t -a
 mavlink boot_complete
+replay trystart
diff --git a/posix-configs/SITL/init/rcS_gazebo_standard_vtol b/posix-configs/SITL/init/rcS_gazebo_standard_vtol
index 3473324e71..7f6c8bab34 100644
--- a/posix-configs/SITL/init/rcS_gazebo_standard_vtol
+++ b/posix-configs/SITL/init/rcS_gazebo_standard_vtol
@@ -47,6 +47,7 @@ param set MPC_ACC_HOR_MAX 2
 param set GF_ACTION 3
 param set GF_MAX_HOR_DIST 500
 param set GF_MAX_VER_DIST 500
+replay tryapplyparams
 simulator start -s 
 rgbledsim start
 tone_alarm start
@@ -82,3 +83,4 @@ mavlink stream -r 250 -s HIGHRES_IMU -u 14556
 mavlink stream -r 10 -s OPTICAL_FLOW_RAD -u 14556
 sdlog2 start -r 200 -e -t -a
 mavlink boot_complete
+replay trystart
diff --git a/posix-configs/SITL/init/rcS_gazebo_tailsitter b/posix-configs/SITL/init/rcS_gazebo_tailsitter
index 663be85607..ea37d6f431 100644
--- a/posix-configs/SITL/init/rcS_gazebo_tailsitter
+++ b/posix-configs/SITL/init/rcS_gazebo_tailsitter
@@ -35,6 +35,7 @@ param set SENS_DPRES_OFF 0.001
 param set SENS_BOARD_X_OFF 0.000001
 param set COM_RC_IN_MODE 1
 param set NAV_DLL_ACT 2
+replay tryapplyparams
 simulator start -s
 rgbledsim start
 tone_alarm start
@@ -70,3 +71,4 @@ mavlink stream -r 250 -s HIGHRES_IMU -u 14556
 mavlink stream -r 10 -s OPTICAL_FLOW_RAD -u 14556
 sdlog2 start -r 200 -e -t -a
 mavlink boot_complete
+replay trystart
diff --git a/posix-configs/SITL/init/rcS_gazebo_typhoon_h480 b/posix-configs/SITL/init/rcS_gazebo_typhoon_h480
index 5fd7c5f5ac..392ff03c94 100644
--- a/posix-configs/SITL/init/rcS_gazebo_typhoon_h480
+++ b/posix-configs/SITL/init/rcS_gazebo_typhoon_h480
@@ -40,6 +40,7 @@ param set MPC_XY_VEL_P 0.15
 param set MPC_XY_VEL_I 0.2
 param set EKF2_GBIAS_INIT 0.01
 param set EKF2_ANGERR_INIT 0.01
+replay tryapplyparams
 simulator start -s
 rgbledsim start
 tone_alarm start
@@ -72,3 +73,4 @@ mavlink stream -r 250 -s HIGHRES_IMU -u 14556
 mavlink stream -r 10 -s OPTICAL_FLOW_RAD -u 14556
 sdlog2 start -r 100 -e -t -a
 mavlink boot_complete
+replay trystart
diff --git a/posix-configs/SITL/init/rcS_jmavsim_iris b/posix-configs/SITL/init/rcS_jmavsim_iris
index 2e90404619..1e44f2b13a 100644
--- a/posix-configs/SITL/init/rcS_jmavsim_iris
+++ b/posix-configs/SITL/init/rcS_jmavsim_iris
@@ -41,6 +41,7 @@ param set MPC_Z_VEL_MAX 2.0
 param set MPC_Z_VEL_P 0.4
 param set EKF2_GBIAS_INIT 0.01
 param set EKF2_ANGERR_INIT 0.01
+replay tryapplyparams
 simulator start -s
 rgbledsim start
 tone_alarm start
@@ -49,6 +50,7 @@ accelsim start
 barosim start
 adcsim start
 gpssim start
+#gps start -d /dev/ttyACM0 -s
 pwm_out_sim mode_pwm
 sleep 1
 sensors start
@@ -72,4 +74,6 @@ mavlink stream -r 250 -s HIGHRES_IMU -u 14556
 mavlink stream -r 10 -s OPTICAL_FLOW_RAD -u 14556
 mavlink stream -r 20 -s MANUAL_CONTROL -u 14556
 sdlog2 start -r 100 -e -t -a
+logger start -e -t
 mavlink boot_complete
+replay trystart
diff --git a/posix-configs/SITL/init/rcS_lpe_gazebo_iris b/posix-configs/SITL/init/rcS_lpe_gazebo_iris
index f3e5dd55f8..a72c2c432a 100644
--- a/posix-configs/SITL/init/rcS_lpe_gazebo_iris
+++ b/posix-configs/SITL/init/rcS_lpe_gazebo_iris
@@ -35,6 +35,7 @@ param set SENS_BOARD_X_OFF 0.000001
 param set COM_RC_IN_MODE 1
 param set NAV_DLL_ACT 2
 param set LPE_BETA_MAX 10000
+replay tryapplyparams
 simulator start -s
 rgbled start
 tone_alarm start
@@ -70,3 +71,4 @@ sdlog2 start -r 100 -e -t -a
 sleep 5
 local_position_estimator start
 mavlink boot_complete
+replay trystart
diff --git a/posix-configs/SITL/init/rcS_lpe_gazebo_iris_opt_flow b/posix-configs/SITL/init/rcS_lpe_gazebo_iris_opt_flow
index e7c0c33258..536f7790b8 100644
--- a/posix-configs/SITL/init/rcS_lpe_gazebo_iris_opt_flow
+++ b/posix-configs/SITL/init/rcS_lpe_gazebo_iris_opt_flow
@@ -39,6 +39,7 @@ param set RTL_DESCEND_ALT 10.0
 param set COM_RC_IN_MODE 1
 param set NAV_DLL_ACT 2
 param set LPE_BETA_MAX 10000
+replay tryapplyparams
 simulator start -s
 rgbledsim start
 tone_alarm start
@@ -74,3 +75,4 @@ sdlog2 start -r 100 -e -t -a
 sleep 5
 local_position_estimator start
 mavlink boot_complete
+replay trystart
diff --git a/posix-configs/SITL/init/rcS_multiple_gazebo_iris b/posix-configs/SITL/init/rcS_multiple_gazebo_iris
index e6de00cff9..fbd5b5a643 100644
--- a/posix-configs/SITL/init/rcS_multiple_gazebo_iris
+++ b/posix-configs/SITL/init/rcS_multiple_gazebo_iris
@@ -32,6 +32,7 @@ param set COM_RC_IN_MODE 1
 param set NAV_ACC_RAD 2.0
 param set RTL_RETURN_ALT 30.0
 param set RTL_DESCEND_ALT 10.0
+replay tryapplyparams
 rgbledsim start
 tone_alarm start
 gyrosim start
@@ -62,3 +63,4 @@ mavlink stream -r 250 -s HIGHRES_IMU -u _MAVPORT_
 mavlink stream -r 10 -s OPTICAL_FLOW_RAD -u _MAVPORT_
 sdlog2 start -r 100 -e -t -a
 mavlink boot_complete
+replay trystart

From 68e14ceb295d6d48e656dd775d3d7c9431d9bb3b Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Beat=20K=C3=BCng?= <beat-kueng@gmx.net>
Date: Mon, 11 Jul 2016 10:01:16 +0200
Subject: [PATCH 0984/1230] tap startup script: add -t param for logger

---
 ROMFS/tap_common/init.d/rcS | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/ROMFS/tap_common/init.d/rcS b/ROMFS/tap_common/init.d/rcS
index 643e421bae..111e695868 100644
--- a/ROMFS/tap_common/init.d/rcS
+++ b/ROMFS/tap_common/init.d/rcS
@@ -275,7 +275,7 @@ then
 		then
 		fi
 	else
-		if logger start -b 12
+		if logger start -b 12 -t
 		then
 		fi
 	fi

From 4ec9e53debe3b75859d3d45195799dc71242c6b7 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Beat=20K=C3=BCng?= <beat-kueng@gmx.net>
Date: Mon, 11 Jul 2016 10:03:02 +0200
Subject: [PATCH 0985/1230] param SYS_LOGGER: remove experimental for new
 logger

It's ready to be used!
---
 src/modules/systemlib/system_params.c | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/src/modules/systemlib/system_params.c b/src/modules/systemlib/system_params.c
index 692f919126..6072b7f2c5 100644
--- a/src/modules/systemlib/system_params.c
+++ b/src/modules/systemlib/system_params.c
@@ -144,7 +144,7 @@ PARAM_DEFINE_INT32(SYS_PARAM_VER, 1);
  * SD logger
  *
  * @value 0 sdlog2 (default)
- * @value 1 new logger (experimental)
+ * @value 1 new logger
  * @min 0
  * @max 1
  * @reboot_required true

From c9652fd42a16faff0c170b40d0d87f7725ad9620 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Beat=20K=C3=BCng?= <beat-kueng@gmx.net>
Date: Mon, 11 Jul 2016 10:04:44 +0200
Subject: [PATCH 0986/1230] logger: update set of default topics to match
 functionality of sdlog2

logging rate with these topics: ~50KB/s

The rates may need to be adjusted
---
 src/modules/logger/logger.cpp | 57 +++++++++++++++++++++++++----------
 1 file changed, 41 insertions(+), 16 deletions(-)

diff --git a/src/modules/logger/logger.cpp b/src/modules/logger/logger.cpp
index c736d34c1f..322a99021f 100644
--- a/src/modules/logger/logger.cpp
+++ b/src/modules/logger/logger.cpp
@@ -463,25 +463,50 @@ bool Logger::copy_if_updated_multi(LoggerSubscription &sub, int multi_instance,
 
 void Logger::add_default_topics()
 {
-	add_topic("sensor_gyro", 0);
-	add_topic("sensor_accel", 0);
-	add_topic("vehicle_rates_setpoint", 10);
-	add_topic("vehicle_attitude_setpoint", 10);
-	add_topic("vehicle_attitude", 0);
+	add_topic("vehicle_attitude", 10);
 	add_topic("actuator_outputs", 50);
-	add_topic("battery_status", 100);
-	add_topic("vehicle_command", 100);
-	add_topic("actuator_controls", 10);
-	add_topic("vehicle_local_position_setpoint", 200);
-	add_topic("rc_channels", 20);
-	add_topic("commander_state", 100);
-	add_topic("vehicle_local_position", 200);
-	add_topic("vehicle_global_position", 200);
-	add_topic("system_power", 100);
-	add_topic("servorail_status", 200);
-	add_topic("mc_att_ctrl_status", 50);
+	add_topic("telemetry_status", 50);
+	add_topic("vehicle_command");
 	add_topic("vehicle_status", 200);
+	add_topic("vtol_vehicle_status", 100);
+	add_topic("commander_state", 100);
+	add_topic("satellite_info", 1000);
+	add_topic("vehicle_attitude_setpoint", 20);
+	add_topic("vehicle_rates_setpoint", 10);
+	add_topic("actuator_controls", 20);
+	add_topic("actuator_controls_0", 20);
+	add_topic("actuator_controls_1", 20);
+	add_topic("vehicle_local_position", 100);
+	add_topic("vehicle_local_position_setpoint", 50);
+	add_topic("vehicle_global_position", 100);
+	add_topic("vehicle_global_velocity_setpoint", 100);
+	add_topic("battery_status", 300);
+	add_topic("system_power", 300);
+	add_topic("servorail_status", 300);
+	add_topic("position_setpoint_triplet", 10);
+	add_topic("att_pos_mocap", 50);
+	add_topic("vision_position_estimate", 50);
+	add_topic("optical_flow", 50);
+	add_topic("rc_channels");
+	add_topic("airspeed", 50);
+	add_topic("distance_sensor", 20);
+	add_topic("esc_status", 20);
+	add_topic("estimator_status", 50); //this one is large
+	add_topic("ekf2_innovations", 20);
+	add_topic("tecs_status", 20);
+	add_topic("wind_estimate", 100);
+	add_topic("encoders", 50);
+	add_topic("time_offset", 1000);
+	add_topic("mc_att_ctrl_status", 50);
+	add_topic("control_state", 20);
+	add_topic("camera_trigger");
+	add_topic("cpuload");
 	add_topic("gps_dump"); //this will only be published if GPS_DUMP_COMM is set
+
+	/* for estimator replay (need to be at full rate) */
+	add_topic("sensor_combined");
+	add_topic("vehicle_gps_position");
+	add_topic("vehicle_land_detected");
 }
 
 int Logger::add_topics_from_file(const char *fname)

From 4fa2c54485916fc1a0cfef3e9179b8486fcaf726 Mon Sep 17 00:00:00 2001
From: tommises <tommises@users.noreply.github.com>
Date: Mon, 11 Jul 2016 04:46:29 -0600
Subject: [PATCH 0987/1230] Mocap timestamp cleanup (#5021)

---
 msg/att_pos_mocap.msg                                  | 3 +--
 src/modules/local_position_estimator/sensors/mocap.cpp | 2 +-
 src/modules/mavlink/mavlink_messages.cpp               | 2 +-
 src/modules/mavlink/mavlink_receiver.cpp               | 7 +++----
 4 files changed, 6 insertions(+), 8 deletions(-)

diff --git a/msg/att_pos_mocap.msg b/msg/att_pos_mocap.msg
index 52bc04b5aa..6c007a6393 100644
--- a/msg/att_pos_mocap.msg
+++ b/msg/att_pos_mocap.msg
@@ -1,7 +1,6 @@
 uint32 id			# ID of the estimator, commonly the component ID of the incoming message
 
-uint64 timestamp_boot		# time of this estimate, in microseconds since system start
-uint64 timestamp_computer	# timestamp provided by the companion computer, in us
+uint64 timestamp_received	# timestamp when the estimate was received
 
 float32[4] q			# Estimated attitude as quaternion
 
diff --git a/src/modules/local_position_estimator/sensors/mocap.cpp b/src/modules/local_position_estimator/sensors/mocap.cpp
index 8319fc403f..c629b3f17b 100644
--- a/src/modules/local_position_estimator/sensors/mocap.cpp
+++ b/src/modules/local_position_estimator/sensors/mocap.cpp
@@ -47,7 +47,7 @@ int BlockLocalPositionEstimator::mocapMeasure(Vector<float, n_y_mocap> &y)
 	y(Y_mocap_y) = _sub_mocap.get().y;
 	y(Y_mocap_z) = _sub_mocap.get().z;
 	_mocapStats.update(y);
-	_time_last_mocap = _sub_mocap.get().timestamp_boot;
+	_time_last_mocap = _sub_mocap.get().timestamp;
 	return OK;
 }
 
diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp
index b366c9940e..aca7f71077 100644
--- a/src/modules/mavlink/mavlink_messages.cpp
+++ b/src/modules/mavlink/mavlink_messages.cpp
@@ -1741,7 +1741,7 @@ protected:
 		if (_mocap_sub->update(&_mocap_time, &mocap)) {
 			mavlink_att_pos_mocap_t msg;
 
-			msg.time_usec = mocap.timestamp_boot;
+			msg.time_usec = mocap.timestamp;
 			msg.q[0] = mocap.q[0];
 			msg.q[1] = mocap.q[1];
 			msg.q[2] = mocap.q[2];
diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp
index b9c7690a7e..81f123ef03 100644
--- a/src/modules/mavlink/mavlink_receiver.cpp
+++ b/src/modules/mavlink/mavlink_receiver.cpp
@@ -677,14 +677,13 @@ MavlinkReceiver::handle_message_att_pos_mocap(mavlink_message_t *msg)
 	mavlink_att_pos_mocap_t mocap;
 	mavlink_msg_att_pos_mocap_decode(msg, &mocap);
 
-	struct att_pos_mocap_s att_pos_mocap;
-	memset(&att_pos_mocap, 0, sizeof(att_pos_mocap));
+	struct att_pos_mocap_s att_pos_mocap = {};
 
 	// Use the component ID to identify the mocap system
 	att_pos_mocap.id = msg->compid;
 
-	att_pos_mocap.timestamp_boot = sync_stamp(mocap.time_usec);
-	att_pos_mocap.timestamp_computer = sync_stamp(mocap.time_usec); // Synced time
+	att_pos_mocap.timestamp = sync_stamp(mocap.time_usec);
+	att_pos_mocap.timestamp_received = hrt_absolute_time();
 
 	att_pos_mocap.q[0] = mocap.q[0];
 	att_pos_mocap.q[1] = mocap.q[1];

From 021f0840ae7f724bfdfd317dda3d57362017b835 Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Mon, 11 Jul 2016 15:03:07 +0200
Subject: [PATCH 0988/1230] Fix Hobbyking Pixracers

---
 ROMFS/px4fmu_common/init.d/rc.sensors | 11 +++++++++++
 1 file changed, 11 insertions(+)

diff --git a/ROMFS/px4fmu_common/init.d/rc.sensors b/ROMFS/px4fmu_common/init.d/rc.sensors
index 9a4d12192e..e356f939d7 100644
--- a/ROMFS/px4fmu_common/init.d/rc.sensors
+++ b/ROMFS/px4fmu_common/init.d/rc.sensors
@@ -14,6 +14,17 @@ else
 	fmu i2c 1 100000
 	fmu i2c 2 100000
 
+	if ver hwcmp PX4FMU_V4
+	then
+		# We know there are sketchy boards out there
+		# as chinese companies produce Pixracers without
+		# fully understanding the critical parts of the
+		# schematic and BOM, leading to sensor brownouts
+		# on boot. Original Pixracers following the
+		# open hardware design do not require this.
+		fmu sensor_reset 20
+	fi
+
 	if ms5611 -s start
 	then
 	fi

From a740d80a2092aa582bc9af95abeedad42c2c4a13 Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Mon, 11 Jul 2016 15:16:35 +0200
Subject: [PATCH 0989/1230] Mag voter: Be more forgiving on load changes

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

diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp
index db605504fd..af852abffe 100644
--- a/src/modules/sensors/sensors.cpp
+++ b/src/modules/sensors/sensors.cpp
@@ -593,7 +593,7 @@ Sensors::Sensors() :
 	_vibration_warning_timestamp(0),
 	_vibration_warning(false)
 {
-	_mag.voter.set_timeout(200000);
+	_mag.voter.set_timeout(300000);
 
 	memset(&_rc, 0, sizeof(_rc));
 	memset(&_diff_pres, 0, sizeof(_diff_pres));

From 817f69529784a5ad48aa366d1c8794c781e4f2a8 Mon Sep 17 00:00:00 2001
From: James Goppert <jgoppert@users.noreply.github.com>
Date: Mon, 11 Jul 2016 16:02:39 -0400
Subject: [PATCH 0990/1230] Change to LPE terrain model to account for velocity
 scaling. (#5027)

---
 .../BlockLocalPositionEstimator.cpp                       | 8 ++++----
 .../BlockLocalPositionEstimator.hpp                       | 2 +-
 src/modules/local_position_estimator/params.c             | 8 ++++----
 3 files changed, 9 insertions(+), 9 deletions(-)

diff --git a/src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp b/src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp
index 50f47a9621..89dae8cd99 100644
--- a/src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp
+++ b/src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp
@@ -85,7 +85,7 @@ BlockLocalPositionEstimator::BlockLocalPositionEstimator() :
 	_pn_p_noise_density(this, "PN_P"),
 	_pn_v_noise_density(this, "PN_V"),
 	_pn_b_noise_density(this, "PN_B"),
-	_pn_t_noise_density(this, "PN_T"),
+	_t_max_grade(this, "T_MAX_GRADE"),
 
 	// init home
 	_init_home_lat(this, "LAT"),
@@ -831,9 +831,9 @@ void BlockLocalPositionEstimator::updateSSParams()
 	_Q(X_by, X_by) = pn_b_sq;
 	_Q(X_bz, X_bz) = pn_b_sq;
 
-	// terrain random walk noise
-	float pn_t_sq = _pn_t_noise_density.get() * _pn_t_noise_density.get();
-	_Q(X_tz, X_tz) = pn_t_sq;
+	// terrain random walk noise ((m/s)/sqrt(hz)), scales with velocity
+	float pn_t_stddev = (_t_max_grade.get() / 100.0f) * sqrtf(_x(X_vx) * _x(X_vx) + _x(X_vy) * _x(X_vy));
+	_Q(X_tz, X_tz) = pn_t_stddev * pn_t_stddev;
 }
 
 void BlockLocalPositionEstimator::predict()
diff --git a/src/modules/local_position_estimator/BlockLocalPositionEstimator.hpp b/src/modules/local_position_estimator/BlockLocalPositionEstimator.hpp
index c541f53e03..abf858aa6a 100644
--- a/src/modules/local_position_estimator/BlockLocalPositionEstimator.hpp
+++ b/src/modules/local_position_estimator/BlockLocalPositionEstimator.hpp
@@ -290,7 +290,7 @@ private:
 	BlockParamFloat  _pn_p_noise_density;
 	BlockParamFloat  _pn_v_noise_density;
 	BlockParamFloat  _pn_b_noise_density;
-	BlockParamFloat  _pn_t_noise_density;
+	BlockParamFloat  _t_max_grade;
 
 	// init home
 	BlockParamFloat  _init_home_lat;
diff --git a/src/modules/local_position_estimator/params.c b/src/modules/local_position_estimator/params.c
index 43e6988b97..361147235e 100644
--- a/src/modules/local_position_estimator/params.c
+++ b/src/modules/local_position_estimator/params.c
@@ -286,15 +286,15 @@ PARAM_DEFINE_FLOAT(LPE_PN_V, 0.1f);
 PARAM_DEFINE_FLOAT(LPE_PN_B, 1e-3f);
 
 /**
- * Terrain random walk noise density, hilly/outdoor (1e-1), flat/Indoor (1e-3)
+ * Terrain maximum percent grade, hilly/outdoor (100 = 45 deg), flat/Indoor (0 = 0 deg)
  *
  * @group Local Position Estimator
- * @unit m/s/sqrt(Hz)
+ * @unit %
  * @min 0
- * @max 1
+ * @max 100
  * @decimal 3
  */
-PARAM_DEFINE_FLOAT(LPE_PN_T, 1e-1f);
+PARAM_DEFINE_FLOAT(LPE_T_MAX_GRADE, 1.0f);
 
 /**
  * Flow gyro high pass filter cut off frequency

From 5cc58fa0677e1f4a43b64b70bb6444aa26ee8c05 Mon Sep 17 00:00:00 2001
From: David Sidrane <david_s5@nscdg.com>
Date: Tue, 12 Jul 2016 03:57:31 -1000
Subject: [PATCH 0991/1230] Reserving 32Kib of FLASH for parameters (#5035)

---
 nuttx-configs/tap-v1/scripts/ld.script | 5 +++--
 1 file changed, 3 insertions(+), 2 deletions(-)

diff --git a/nuttx-configs/tap-v1/scripts/ld.script b/nuttx-configs/tap-v1/scripts/ld.script
index 4cf52e0a4b..c1ded8f3df 100644
--- a/nuttx-configs/tap-v1/scripts/ld.script
+++ b/nuttx-configs/tap-v1/scripts/ld.script
@@ -44,12 +44,13 @@
  * where the code expects to begin execution by jumping to the entry point in
  * the 0x0800:0000 address range.
  *
- * The first 0x4000 of flash is reserved for the bootloader.
+ * The first 16 KiB of flash is reserved for the bootloader.
+ * Paramater storage will use the next 2 16KiB Sectors.
  */
 
 MEMORY
 {
-    flash (rx)   : ORIGIN = 0x08004000, LENGTH = 1008K
+    flash (rx)   : ORIGIN = 0x0800C000, LENGTH = 976K
     sram (rwx)   : ORIGIN = 0x20000000, LENGTH = 128K
     ccsram (rwx) : ORIGIN = 0x10000000, LENGTH = 64K
 }

From f8382a27137b15da1ede6689ba5213abe48c809e Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Beat=20K=C3=BCng?= <beat-kueng@gmx.net>
Date: Tue, 12 Jul 2016 14:47:39 +0200
Subject: [PATCH 0992/1230] upload scripts: add TAP for the uploader script

---
 Tools/upload.sh             | 4 ++--
 cmake/common/px4_base.cmake | 1 +
 2 files changed, 3 insertions(+), 2 deletions(-)

diff --git a/Tools/upload.sh b/Tools/upload.sh
index d78abc3d23..4a2e463008 100755
--- a/Tools/upload.sh
+++ b/Tools/upload.sh
@@ -17,7 +17,7 @@ fi
 
 if [ $SYSTYPE = "Linux" ];
 then
-SERIAL_PORTS="/dev/serial/by-id/usb-3D_Robotics*"
+SERIAL_PORTS="/dev/serial/by-id/usb-3D_Robotics*,/dev/serial/by-id/usb-The_Autopilot*"
 fi
 
 if [ $SYSTYPE = "" ];
@@ -25,4 +25,4 @@ then
 SERIAL_PORTS="COM32,COM31,COM30,COM29,COM28,COM27,COM26,COM25,COM24,COM23,COM22,COM21,COM20,COM19,COM18,COM17,COM16,COM15,COM14,COM13,COM12,COM11,COM10,COM9,COM8,COM7,COM6,COM5,COM4,COM3,COM2,COM1,COM0"
 fi
 
-python $BASEDIR/px_uploader.py --port $SERIAL_PORTS $1
\ No newline at end of file
+python $BASEDIR/px_uploader.py --port $SERIAL_PORTS $1
diff --git a/cmake/common/px4_base.cmake b/cmake/common/px4_base.cmake
index 7b65c35071..ef05520375 100644
--- a/cmake/common/px4_base.cmake
+++ b/cmake/common/px4_base.cmake
@@ -488,6 +488,7 @@ function(px4_add_upload)
 	if(${CMAKE_HOST_SYSTEM_NAME} STREQUAL "Linux")
 		list(APPEND serial_ports
 			/dev/serial/by-id/usb-3D_Robotics*
+			/dev/serial/by-id/usb-The_Autopilot*
 			/dev/serial/by-id/pci-3D_Robotics*
 			)
 	elseif(${CMAKE_HOST_SYSTEM_NAME} STREQUAL "Darwin")

From 0576031a81e652a13a154685fd09f2c71cfccbd5 Mon Sep 17 00:00:00 2001
From: David Sidrane <david_s5@nscdg.com>
Date: Tue, 12 Jul 2016 05:07:03 -1000
Subject: [PATCH 0993/1230] TAP PID (#5036)

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

diff --git a/nuttx-configs/tap-v1/nsh/defconfig b/nuttx-configs/tap-v1/nsh/defconfig
index 5e095d7b4c..00144af2ad 100644
--- a/nuttx-configs/tap-v1/nsh/defconfig
+++ b/nuttx-configs/tap-v1/nsh/defconfig
@@ -593,7 +593,7 @@ CONFIG_CDCACM_BULKIN_REQLEN=96
 CONFIG_CDCACM_RXBUFSIZE=300
 CONFIG_CDCACM_TXBUFSIZE=1000
 CONFIG_CDCACM_VENDORID=0x26ac
-CONFIG_CDCACM_PRODUCTID=0x0010
+CONFIG_CDCACM_PRODUCTID=0x0040
 CONFIG_CDCACM_VENDORSTR="The Autopilot"
 CONFIG_CDCACM_PRODUCTSTR="PX4 TAP v1.x"
 # CONFIG_USBMSC is not set

From 7419151314e7667d76a82565d3fd1c40bf416d2a Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Tue, 12 Jul 2016 22:17:22 +0200
Subject: [PATCH 0994/1230] Update EKF2 replay timestamp handling

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

diff --git a/src/modules/ekf2_replay/ekf2_replay_main.cpp b/src/modules/ekf2_replay/ekf2_replay_main.cpp
index 4c30fe350d..e175662b04 100644
--- a/src/modules/ekf2_replay/ekf2_replay_main.cpp
+++ b/src/modules/ekf2_replay/ekf2_replay_main.cpp
@@ -443,7 +443,7 @@ void Ekf2Replay::setEstimatorInput(uint8_t *data, uint8_t type)
 		uint8_t *dest_ptr = (uint8_t *)&replay_part5.time_ev_usec;
 		parseMessage(data, dest_ptr, type);
 		_ev.timestamp = replay_part5.time_ev_usec;
-		_ev.timestamp_computer = replay_part5.time_ev_usec; // fake this timestamp
+		_ev.timestamp_received = replay_part5.time_ev_usec; // fake this timestamp
 		_ev.x = replay_part5.x;
 		_ev.y = replay_part5.y;
 		_ev.z = replay_part5.z;

From 59b4350aa010eeaf085f168b19b42724ad216b5f Mon Sep 17 00:00:00 2001
From: Daniel Agar <daniel@agar.ca>
Date: Mon, 4 Jul 2016 14:17:27 -0400
Subject: [PATCH 0995/1230] implement MAV_CMD_NAV_LOITER_TO_ALT and general
 mission cleanup

---
 ROMFS/px4fmu_common/init.d/rc.fw_defaults     |   3 +
 src/modules/commander/commander.cpp           |  12 +-
 src/modules/mavlink/mavlink_mission.cpp       | 225 +++++++++++++-----
 src/modules/navigator/mission.cpp             |  38 ++-
 src/modules/navigator/mission_block.cpp       | 122 ++++++++--
 src/modules/navigator/mission_block.h         |   1 +
 .../navigator/mission_feasibility_checker.cpp |  19 +-
 src/modules/navigator/navigation.h            |  13 +-
 8 files changed, 314 insertions(+), 119 deletions(-)

diff --git a/ROMFS/px4fmu_common/init.d/rc.fw_defaults b/ROMFS/px4fmu_common/init.d/rc.fw_defaults
index 1ae8f81227..a7206200c2 100644
--- a/ROMFS/px4fmu_common/init.d/rc.fw_defaults
+++ b/ROMFS/px4fmu_common/init.d/rc.fw_defaults
@@ -14,6 +14,9 @@ then
 	# FW uses L1 distance for acceptance radius 
 	#  set a smaller NAV_ACC_RAD for vertical acceptance distance
 	param set NAV_ACC_RAD 10
+	
+	param set MIS_LTRMIN_ALT 25
+	param set MIS_TAKEOFF_ALT 25
 
 	param set PE_VELNE_NOISE 0.3
 	param set PE_VELD_NOISE 0.35
diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp
index 9fd1534e15..a8ab8f69ae 100644
--- a/src/modules/commander/commander.cpp
+++ b/src/modules/commander/commander.cpp
@@ -1064,23 +1064,23 @@ bool handle_command(struct vehicle_status_s *status_local, const struct safety_s
 		}
 		break;
 
+	case vehicle_command_s::VEHICLE_CMD_CUSTOM_0:
+	case vehicle_command_s::VEHICLE_CMD_CUSTOM_1:
+	case vehicle_command_s::VEHICLE_CMD_CUSTOM_2:
 	case vehicle_command_s::VEHICLE_CMD_PREFLIGHT_REBOOT_SHUTDOWN:
 	case vehicle_command_s::VEHICLE_CMD_PREFLIGHT_CALIBRATION:
 	case vehicle_command_s::VEHICLE_CMD_PREFLIGHT_SET_SENSOR_OFFSETS:
 	case vehicle_command_s::VEHICLE_CMD_PREFLIGHT_STORAGE:
 	case vehicle_command_s::VEHICLE_CMD_PREFLIGHT_UAVCAN:
-	case vehicle_command_s::VEHICLE_CMD_CUSTOM_0:
-	case vehicle_command_s::VEHICLE_CMD_CUSTOM_1:
-	case vehicle_command_s::VEHICLE_CMD_CUSTOM_2:
 	case vehicle_command_s::VEHICLE_CMD_PAYLOAD_PREPARE_DEPLOY:
 	case vehicle_command_s::VEHICLE_CMD_PAYLOAD_CONTROL_DEPLOY:
-	case vehicle_command_s::VEHICLE_CMD_DO_MOUNT_CONTROL:
-	case vehicle_command_s::VEHICLE_CMD_DO_MOUNT_CONTROL_QUAT:
-	case vehicle_command_s::VEHICLE_CMD_DO_MOUNT_CONFIGURE:
 	case vehicle_command_s::VEHICLE_CMD_DO_TRIGGER_CONTROL:
 	case vehicle_command_s::VEHICLE_CMD_DO_VTOL_TRANSITION:
 	case vehicle_command_s::VEHICLE_CMD_DO_DIGICAM_CONTROL:
+	case vehicle_command_s::VEHICLE_CMD_DO_MOUNT_CONFIGURE:
+	case vehicle_command_s::VEHICLE_CMD_DO_MOUNT_CONTROL:
 	case vehicle_command_s::VEHICLE_CMD_DO_SET_CAM_TRIGG_DIST:
+	case vehicle_command_s::VEHICLE_CMD_DO_MOUNT_CONTROL_QUAT:
 	case vehicle_command_s::VEHICLE_CMD_DO_CHANGE_SPEED:
 	case vehicle_command_s::VEHICLE_CMD_START_RX_PAIR:
 		/* ignore commands that handled in low prio loop */
diff --git a/src/modules/mavlink/mavlink_mission.cpp b/src/modules/mavlink/mavlink_mission.cpp
index 088ba47ac0..832bb4dc52 100644
--- a/src/modules/mavlink/mavlink_mission.cpp
+++ b/src/modules/mavlink/mavlink_mission.cpp
@@ -767,24 +767,83 @@ MavlinkMissionManager::handle_mission_clear_all(const mavlink_message_t *msg)
 int
 MavlinkMissionManager::parse_mavlink_mission_item(const mavlink_mission_item_t *mavlink_mission_item, struct mission_item_s *mission_item)
 {
-	/* only support global waypoints for now */
-	switch (mavlink_mission_item->frame) {
-	case MAV_FRAME_GLOBAL:
+	if (mavlink_mission_item->frame == MAV_FRAME_GLOBAL ||
+		mavlink_mission_item->frame == MAV_FRAME_GLOBAL_RELATIVE_ALT) {
+
+		// only support global waypoints for now
+
 		mission_item->lat = (double)mavlink_mission_item->x;
 		mission_item->lon = (double)mavlink_mission_item->y;
 		mission_item->altitude = mavlink_mission_item->z;
-		mission_item->altitude_is_relative = false;
-		break;
 
-	case MAV_FRAME_GLOBAL_RELATIVE_ALT:
-		mission_item->lat = (double)mavlink_mission_item->x;
-		mission_item->lon = (double)mavlink_mission_item->y;
-		mission_item->altitude = mavlink_mission_item->z;
-		mission_item->altitude_is_relative = true;
-		break;
+		if (mavlink_mission_item->frame == MAV_FRAME_GLOBAL) {
+			mission_item->altitude_is_relative = false;
+		} else if (mavlink_mission_item->frame == MAV_FRAME_GLOBAL_RELATIVE_ALT) {
+			mission_item->altitude_is_relative = true;
+		}
+
+		mission_item->time_inside = 0.0f;
+
+		switch (mavlink_mission_item->command) {
+		case MAV_CMD_NAV_WAYPOINT:
+			mission_item->nav_cmd = NAV_CMD_WAYPOINT;
+			mission_item->time_inside = mavlink_mission_item->param1;
+			mission_item->acceptance_radius = mavlink_mission_item->param2;
+			mission_item->yaw = _wrap_pi(mavlink_mission_item->param4 * M_DEG_TO_RAD_F);
+			break;
+
+		case MAV_CMD_NAV_LOITER_UNLIM:
+			mission_item->nav_cmd = NAV_CMD_LOITER_UNLIMITED;
+			mission_item->loiter_radius = fabsf(mavlink_mission_item->param3);
+			mission_item->loiter_direction = (mavlink_mission_item->param3 > 0) ? 1 : -1; /* 1 if positive CW, -1 if negative CCW */
+			mission_item->yaw = _wrap_pi(mavlink_mission_item->param4 * M_DEG_TO_RAD_F);
+			break;
+
+		case MAV_CMD_NAV_LOITER_TIME:
+			mission_item->nav_cmd = NAV_CMD_LOITER_TIME_LIMIT;
+			mission_item->time_inside = mavlink_mission_item->param1;
+			mission_item->loiter_radius = fabsf(mavlink_mission_item->param3);
+			mission_item->loiter_direction = (mavlink_mission_item->param3 > 0) ? 1 : -1; /* 1 if positive CW, -1 if negative CCW */
+			mission_item->loiter_exit_xtrack = (mavlink_mission_item->param4 > 0) ? true : false;
+			break;
+
+		case MAV_CMD_NAV_LAND:
+			mission_item->nav_cmd = NAV_CMD_LAND;
+			// TODO: abort alt param1
+			mission_item->yaw = _wrap_pi(mavlink_mission_item->param4 * M_DEG_TO_RAD_F);
+			break;
+
+		case MAV_CMD_NAV_TAKEOFF:
+			mission_item->nav_cmd = NAV_CMD_TAKEOFF;
+			mission_item->pitch_min = mavlink_mission_item->param1;
+			mission_item->yaw = _wrap_pi(mavlink_mission_item->param4 * M_DEG_TO_RAD_F);
+			break;
+
+		case MAV_CMD_NAV_LOITER_TO_ALT:
+			mission_item->nav_cmd = NAV_CMD_LOITER_TO_ALT;
+			mission_item->force_heading = (mavlink_mission_item->param1 > 0) ? true : false;
+			mission_item->loiter_radius = fabsf(mavlink_mission_item->param2);
+			mission_item->loiter_direction = (mavlink_mission_item->param2 > 0) ? 1 : -1; /* 1 if positive CW, -1 if negative CCW */
+			mission_item->loiter_exit_xtrack = (mavlink_mission_item->param4 > 0) ? true : false;
+			break;
+
+		case MAV_CMD_NAV_VTOL_TAKEOFF:
+		case MAV_CMD_NAV_VTOL_LAND:
+			mission_item->nav_cmd = (NAV_CMD)mavlink_mission_item->command;
+			mission_item->yaw = _wrap_pi(mavlink_mission_item->param4 * M_DEG_TO_RAD_F);
+			break;
+
+		default:
+			mission_item->nav_cmd = NAV_CMD_INVALID;
+			return MAV_MISSION_UNSUPPORTED;
+		}
+
+		mission_item->frame = mavlink_mission_item->frame;
+
+	} else if (mavlink_mission_item->frame == MAV_FRAME_MISSION) {
+
+		// this is a mission item with no coordinates
 
-	case MAV_FRAME_MISSION:
-		// This is a mission item with no coordinate
 		mission_item->params[0] = mavlink_mission_item->param1;
 		mission_item->params[1] = mavlink_mission_item->param2;
 		mission_item->params[2] = mavlink_mission_item->param3;
@@ -792,50 +851,44 @@ MavlinkMissionManager::parse_mavlink_mission_item(const mavlink_mission_item_t *
 		mission_item->params[4] = mavlink_mission_item->x;
 		mission_item->params[5] = mavlink_mission_item->y;
 		mission_item->params[6] = mavlink_mission_item->z;
-		break;
 
-	default:
-		return MAV_MISSION_UNSUPPORTED_FRAME;
-	}
+		switch (mavlink_mission_item->command) {
+		case MAV_CMD_DO_JUMP:
+			mission_item->nav_cmd = NAV_CMD_DO_JUMP;
+			mission_item->do_jump_mission_index = mavlink_mission_item->param1;
+			mission_item->do_jump_current_count = 0;
+			mission_item->do_jump_repeat_count = mavlink_mission_item->param2;
+			break;
 
-	mission_item->frame = mavlink_mission_item->frame;
+		case MAV_CMD_DO_CHANGE_SPEED:
+		case MAV_CMD_DO_SET_SERVO:
+		case MAV_CMD_DO_DIGICAM_CONTROL:
+		case MAV_CMD_DO_MOUNT_CONFIGURE:
+		case MAV_CMD_DO_MOUNT_CONTROL:
+		case MAV_CMD_DO_SET_CAM_TRIGG_DIST:
+		case MAV_CMD_DO_VTOL_TRANSITION:
+			mission_item->nav_cmd = (NAV_CMD)mavlink_mission_item->command;
+			break;
 
-	switch (mavlink_mission_item->command) {
-	case MAV_CMD_NAV_TAKEOFF:
-		mission_item->pitch_min = mavlink_mission_item->param1;
-		break;
+		default:
+			mission_item->nav_cmd = NAV_CMD_INVALID;
+			return MAV_MISSION_UNSUPPORTED;
+		}
 
-	case MAV_CMD_DO_JUMP:
-		mission_item->do_jump_mission_index = mavlink_mission_item->param1;
-		mission_item->do_jump_current_count = 0;
-		mission_item->do_jump_repeat_count = mavlink_mission_item->param2;
-		break;
-
-	default:
-		mission_item->acceptance_radius = mavlink_mission_item->param2;
-		mission_item->time_inside = mavlink_mission_item->param1;
-		break;
-	}
-
-	mission_item->yaw = _wrap_pi(mavlink_mission_item->param4 * M_DEG_TO_RAD_F);
-	mission_item->loiter_radius = fabsf(mavlink_mission_item->param3);
-	mission_item->loiter_direction = (mavlink_mission_item->param3 > 0) ? 1 : -1; /* 1 if positive CW, -1 if negative CCW */
-
-	/* unsigned, so can't be negative, only check for overflow */
-	if (mavlink_mission_item->command >= NAV_CMD_INVALID) {
-		mission_item->nav_cmd = NAV_CMD_INVALID;
+		mission_item->frame = MAV_FRAME_MISSION;
 
 	} else {
-		mission_item->nav_cmd = (NAV_CMD)mavlink_mission_item->command;
+		return MAV_MISSION_UNSUPPORTED_FRAME;
 	}
 
 	mission_item->autocontinue = mavlink_mission_item->autocontinue;
 	// mission_item->index = mavlink_mission_item->seq;
-	mission_item->origin = ORIGIN_MAVLINK;
 
 	/* reset DO_JUMP count */
 	mission_item->do_jump_current_count = 0;
 
+	mission_item->origin = ORIGIN_MAVLINK;
+
 	return MAV_MISSION_ACCEPTED;
 }
 
@@ -857,32 +910,82 @@ MavlinkMissionManager::format_mavlink_mission_item(const struct mission_item_s *
 		mavlink_mission_item->y = mission_item->params[5];
 		mavlink_mission_item->z = mission_item->params[6];
 
-	// default mappings for regular waypoints
+		switch (mavlink_mission_item->command) {
+		case NAV_CMD_DO_JUMP:
+			mavlink_mission_item->param1 = mission_item->do_jump_mission_index;
+			mavlink_mission_item->param2 = mission_item->do_jump_repeat_count;
+			break;
+
+		case NAV_CMD_DO_CHANGE_SPEED:
+		case NAV_CMD_DO_SET_SERVO:
+		case NAV_CMD_DO_DIGICAM_CONTROL:
+		case NAV_CMD_DO_MOUNT_CONFIGURE:
+		case NAV_CMD_DO_MOUNT_CONTROL:
+		case NAV_CMD_DO_SET_CAM_TRIGG_DIST:
+		case NAV_CMD_DO_VTOL_TRANSITION:
+			break;
+
+		default:
+			return ERROR;
+		}
+
 	} else {
+		mavlink_mission_item->param1 = 0.0f;
+		mavlink_mission_item->param2 = 0.0f;
+		mavlink_mission_item->param3 = 0.0f;
+		mavlink_mission_item->param4 = 0.0f;
 		mavlink_mission_item->x = (float)mission_item->lat;
 		mavlink_mission_item->y = (float)mission_item->lon;
 		mavlink_mission_item->z = mission_item->altitude;
 
-		mavlink_mission_item->param4 = mission_item->yaw * M_RAD_TO_DEG_F;
-		mavlink_mission_item->param3 = mission_item->loiter_radius * (float)mission_item->loiter_direction;
-	}
+		if (mission_item->altitude_is_relative) {
+			mavlink_mission_item->frame = MAV_FRAME_GLOBAL_RELATIVE_ALT;
+		} else {
+			mavlink_mission_item->frame = MAV_FRAME_GLOBAL;
+		}
 
-	// specific item handling
-	switch (mission_item->nav_cmd) {
-	case NAV_CMD_TAKEOFF:
-	case NAV_CMD_VTOL_TAKEOFF:
-		mavlink_mission_item->param1 = mission_item->pitch_min;
-		break;
+		switch (mission_item->nav_cmd) {
+		case NAV_CMD_WAYPOINT:
+			mavlink_mission_item->param1 = mission_item->time_inside;
+			mavlink_mission_item->param2 = mission_item->acceptance_radius;
+			mavlink_mission_item->param4 = mission_item->yaw * M_RAD_TO_DEG_F;
+			break;
 
-	case NAV_CMD_DO_JUMP:
-		mavlink_mission_item->param1 = mission_item->do_jump_mission_index;
-		mavlink_mission_item->param2 = mission_item->do_jump_repeat_count - mission_item->do_jump_current_count;
-		break;
+		case NAV_CMD_LOITER_UNLIMITED:
+			mavlink_mission_item->param3 = mission_item->loiter_radius * (float)mission_item->loiter_direction;
+			mavlink_mission_item->param4 = mission_item->yaw * M_RAD_TO_DEG_F;
+			break;
 
-	default:
-		mavlink_mission_item->param2 = mission_item->acceptance_radius;
-		mavlink_mission_item->param1 = mission_item->time_inside;
-		break;
+		case NAV_CMD_LOITER_TIME_LIMIT:
+			mavlink_mission_item->param1 = mission_item->time_inside;
+			mavlink_mission_item->param3 = mission_item->loiter_radius * (float)mission_item->loiter_direction;
+			mavlink_mission_item->param4 = mission_item->loiter_exit_xtrack;
+			break;
+
+		case NAV_CMD_LAND:
+			// TODO: param1 abort alt
+			mavlink_mission_item->param4 = mission_item->yaw * M_RAD_TO_DEG_F;
+			break;
+
+		case NAV_CMD_TAKEOFF:
+			mavlink_mission_item->param1 = mission_item->pitch_min;
+			mavlink_mission_item->param4 = mission_item->yaw * M_RAD_TO_DEG_F;
+			break;
+
+		case NAV_CMD_LOITER_TO_ALT:
+			mavlink_mission_item->param1 = mission_item->force_heading;
+			mavlink_mission_item->param2 = mission_item->loiter_radius * (float)mission_item->loiter_direction;
+			mavlink_mission_item->param4 = mission_item->loiter_exit_xtrack;
+			break;
+
+		case MAV_CMD_NAV_VTOL_TAKEOFF:
+		case MAV_CMD_NAV_VTOL_LAND:
+			mavlink_mission_item->param4 = mission_item->yaw * M_RAD_TO_DEG_F;
+			break;
+
+		default:
+			return ERROR;
+		}
 	}
 
 	return OK;
diff --git a/src/modules/navigator/mission.cpp b/src/modules/navigator/mission.cpp
index 8640051177..ad350be3ee 100644
--- a/src/modules/navigator/mission.cpp
+++ b/src/modules/navigator/mission.cpp
@@ -348,7 +348,7 @@ Mission::set_mission_items()
 	/* make sure param is up to date */
 	updateParams();
 
-	/* reset the altitude foh logic, if altitude foh is enabled (param) a new foh element starts now */
+	/* reset the altitude foh (first order hold) logic, if altitude foh is enabled (param) a new foh element starts now */
 	altitude_sp_foh_reset();
 
 	struct position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet();
@@ -453,6 +453,7 @@ Mission::set_mission_items()
 
 		/* do takeoff before going to setpoint if needed and not already in takeoff */
 		if (do_need_takeoff() && _work_item_type != WORK_ITEM_TYPE_TAKEOFF) {
+
 			new_work_item_type = WORK_ITEM_TYPE_TAKEOFF;
 
 			/* use current mission item as next position item */
@@ -482,8 +483,9 @@ Mission::set_mission_items()
 					&& _navigator->get_vstatus()->is_rotary_wing
 					&& !_navigator->get_land_detected()->landed
 					&& has_next_position_item) {
+
 				/* check if the vtol_takeoff command is on top of us */
-				if(do_need_move_to_takeoff()){
+				if (do_need_move_to_takeoff()){
 					new_work_item_type = WORK_ITEM_TYPE_TRANSITON_AFTER_TAKEOFF;
 				} else {
 					new_work_item_type = WORK_ITEM_TYPE_DEFAULT;
@@ -516,6 +518,7 @@ Mission::set_mission_items()
 		if (_mission_item.nav_cmd == NAV_CMD_VTOL_LAND
 				&& _work_item_type == WORK_ITEM_TYPE_DEFAULT
 				&& !_navigator->get_land_detected()->landed) {
+
 			new_work_item_type = WORK_ITEM_TYPE_MOVE_TO_LAND;
 			/* use current mission item as next position item */
 			memcpy(&mission_item_next_position, &_mission_item, sizeof(struct mission_item_s));
@@ -537,6 +540,7 @@ Mission::set_mission_items()
 				&& _work_item_type == WORK_ITEM_TYPE_MOVE_TO_LAND
 				&& !_navigator->get_vstatus()->is_rotary_wing
 				&& !_navigator->get_land_detected()->landed) {
+
 			_mission_item.nav_cmd = NAV_CMD_DO_VTOL_TRANSITION;
 			_mission_item.params[0] = vtol_vehicle_status_s::VEHICLE_VTOL_STATE_MC;
 			_mission_item.autocontinue = true;
@@ -547,6 +551,7 @@ Mission::set_mission_items()
 		if (do_need_move_to_land() &&
 				(_work_item_type == WORK_ITEM_TYPE_DEFAULT ||
 				 _work_item_type == WORK_ITEM_TYPE_MOVE_TO_LAND_AFTER_TRANSITION)) {
+
 			new_work_item_type = WORK_ITEM_TYPE_MOVE_TO_LAND;
 
 			/* use current mission item as next position item */
@@ -572,6 +577,7 @@ Mission::set_mission_items()
 		/* we just moved to the landing waypoint, now descend */
 		if (_work_item_type == WORK_ITEM_TYPE_MOVE_TO_LAND
 				&& _navigator->get_vstatus()->is_rotary_wing) {
+
 			new_work_item_type = WORK_ITEM_TYPE_DEFAULT;
 		}
 
@@ -607,7 +613,7 @@ Mission::set_mission_items()
 
 	/*********************************** set setpoints and check next *********************************************/
 
-	/* set current position setpoint from mission item (is protected agains non-position items) */
+	/* set current position setpoint from mission item (is protected against non-position items) */
 	mission_item_to_position_setpoint(&_mission_item, &pos_sp_triplet->current);
 
 	/* issue command if ready (will do nothing for position mission items) */
@@ -617,7 +623,9 @@ Mission::set_mission_items()
 	_work_item_type = new_work_item_type;
 
 	/* require takeoff after landing or idle */
-	if (pos_sp_triplet->current.type == position_setpoint_s::SETPOINT_TYPE_LAND || pos_sp_triplet->current.type == position_setpoint_s::SETPOINT_TYPE_IDLE) {
+	if (pos_sp_triplet->current.type == position_setpoint_s::SETPOINT_TYPE_LAND
+			|| pos_sp_triplet->current.type == position_setpoint_s::SETPOINT_TYPE_IDLE) {
+
 		_need_takeoff = true;
 	}
 
@@ -647,6 +655,7 @@ Mission::set_mission_items()
 
 	/* Save the distance between the current sp and the previous one */
 	if (pos_sp_triplet->current.valid && pos_sp_triplet->previous.valid) {
+
 		_distance_current_previous = get_distance_to_next_waypoint(
 				pos_sp_triplet->current.lat,
 				pos_sp_triplet->current.lon,
@@ -661,6 +670,7 @@ bool
 Mission::do_need_takeoff()
 {
 	if (_navigator->get_vstatus()->is_rotary_wing) {
+
 		float takeoff_alt = calculate_takeoff_altitude(&_mission_item);
 
 		/* force takeoff if landed (additional protection) */
@@ -674,13 +684,12 @@ Mission::do_need_takeoff()
 
 		/* check if current mission item is one that requires takeoff before */
 		if (_need_takeoff && (
-				_mission_item.nav_cmd == NAV_CMD_TAKEOFF ||
 				_mission_item.nav_cmd == NAV_CMD_WAYPOINT ||
-				_mission_item.nav_cmd == NAV_CMD_VTOL_TAKEOFF ||
-				_mission_item.nav_cmd == NAV_CMD_LOITER_TIME_LIMIT ||
-				_mission_item.nav_cmd == NAV_CMD_LOITER_TURN_COUNT ||
 				_mission_item.nav_cmd == NAV_CMD_LOITER_UNLIMITED ||
-				_mission_item.nav_cmd == NAV_CMD_RETURN_TO_LAUNCH)) {
+				_mission_item.nav_cmd == NAV_CMD_LOITER_TIME_LIMIT ||
+				_mission_item.nav_cmd == NAV_CMD_TAKEOFF ||
+				_mission_item.nav_cmd == NAV_CMD_LOITER_TO_ALT ||
+				_mission_item.nav_cmd == NAV_CMD_VTOL_TAKEOFF)) {
 
 			_need_takeoff = false;
 			return true;
@@ -693,7 +702,8 @@ Mission::do_need_takeoff()
 bool
 Mission::do_need_move_to_land()
 {
-	if (_navigator->get_vstatus()->is_rotary_wing && (_mission_item.nav_cmd == NAV_CMD_LAND || _mission_item.nav_cmd == NAV_CMD_VTOL_LAND)) {
+	if (_navigator->get_vstatus()->is_rotary_wing
+		&& (_mission_item.nav_cmd == NAV_CMD_LAND || _mission_item.nav_cmd == NAV_CMD_VTOL_LAND)) {
 
 		float d_current = get_distance_to_next_waypoint(_mission_item.lat, _mission_item.lon,
 			_navigator->get_global_position()->lat, _navigator->get_global_position()->lon);
@@ -757,7 +767,7 @@ Mission::calculate_takeoff_altitude(struct mission_item_s *mission_item)
 	/* calculate takeoff altitude */
 	float takeoff_alt = get_absolute_altitude_for_item(*mission_item);
 
-	/* takeoff to at least NAV_TAKEOFF_ALT above home/ground, even if first waypoint is lower */
+	/* takeoff to at least MIS_TAKEOFF_ALT above home/ground, even if first waypoint is lower */
 	if (_navigator->get_land_detected()->landed) {
 		takeoff_alt = fmaxf(takeoff_alt, _navigator->get_global_position()->alt + _param_takeoff_alt.get());
 
@@ -866,10 +876,14 @@ Mission::altitude_sp_foh_update()
 	}
 
 	/* Don't do FOH for non-missions, landing and takeoff waypoints, the ground may be near
-	 * and the FW controller has a custom landing logic */
+	 * and the FW controller has a custom landing logic
+	 *
+	 * NAV_CMD_LOITER_TO_ALT doesn't change altitude until reaching desired lat/lon
+	 * */
 	if (_mission_item.nav_cmd == NAV_CMD_LAND
 			|| _mission_item.nav_cmd == NAV_CMD_VTOL_LAND
 			|| _mission_item.nav_cmd == NAV_CMD_TAKEOFF
+			|| _mission_item.nav_cmd == NAV_CMD_LOITER_TO_ALT
 			|| !_navigator->is_planned_mission()) {
 		return;
 	}
diff --git a/src/modules/navigator/mission_block.cpp b/src/modules/navigator/mission_block.cpp
index cd838e3fa6..6af37faa29 100644
--- a/src/modules/navigator/mission_block.cpp
+++ b/src/modules/navigator/mission_block.cpp
@@ -75,6 +75,7 @@ MissionBlock::MissionBlock(Navigator *navigator, const char *name) :
 	_actuators{},
 	_actuator_pub(nullptr),
 	_cmd_pub(nullptr),
+	_param_loiter_min_alt(this, "MIS_LTRMIN_ALT", false),
 	_param_yaw_timeout(this, "MIS_YAW_TMT", false),
 	_param_yaw_err(this, "MIS_YAW_ERR", false),
 	_param_vtol_wv_land(this, "VT_WV_LND_EN", false),
@@ -98,13 +99,13 @@ MissionBlock::is_mission_item_reached()
 		case NAV_CMD_VTOL_LAND:
 			return _navigator->get_land_detected()->landed;
 
-		/* TODO: count turns */
-		/*_mission_item.nav_cmd == NAV_CMD_LOITER_TURN_COUNT ||*/
 		case NAV_CMD_IDLE: /* fall through */
 		case NAV_CMD_LOITER_UNLIMITED:
 			return false;
 
 		case NAV_CMD_DO_DIGICAM_CONTROL:
+		case NAV_CMD_DO_MOUNT_CONFIGURE:
+		case NAV_CMD_DO_MOUNT_CONTROL:
 		case NAV_CMD_DO_SET_CAM_TRIGG_DIST:
 			return true;
 
@@ -115,13 +116,14 @@ MissionBlock::is_mission_item_reached()
 			 */
 			if (hrt_absolute_time() - _action_start > 500000 &&
 					!_navigator->get_vstatus()->in_transition_mode) {
+
 				_action_start = 0;
 				return true;
 			} else {
 				return false;
 			}
 
-		case vehicle_command_s::VEHICLE_CMD_DO_CHANGE_SPEED:
+		case NAV_CMD_DO_CHANGE_SPEED:
 			// XXX not differentiating ground and airspeed yet
 			if (_mission_item.params[1] > 0.0f) {
 				_navigator->set_cruising_speed(_mission_item.params[1]);
@@ -152,14 +154,14 @@ MissionBlock::is_mission_item_reached()
 		float dist_z = -1.0f;
 
 		float altitude_amsl = _mission_item.altitude_is_relative
-				      ? _mission_item.altitude + _navigator->get_home_position()->alt
-			              : _mission_item.altitude;
+					? _mission_item.altitude + _navigator->get_home_position()->alt
+					: _mission_item.altitude;
 
 		dist = get_distance_to_point_global_wgs84(_mission_item.lat, _mission_item.lon, altitude_amsl,
-				                          _navigator->get_global_position()->lat,
-							  _navigator->get_global_position()->lon,
-							  _navigator->get_global_position()->alt,
-				&dist_xy, &dist_z);
+					_navigator->get_global_position()->lat,
+					_navigator->get_global_position()->lon,
+					_navigator->get_global_position()->alt,
+					&dist_xy, &dist_z);
 
 		if ((_mission_item.nav_cmd == NAV_CMD_TAKEOFF || _mission_item.nav_cmd == NAV_CMD_VTOL_TAKEOFF)
 			&& _navigator->get_vstatus()->is_rotary_wing) {
@@ -176,8 +178,7 @@ MissionBlock::is_mission_item_reached()
 			}
 		} else if (!_navigator->get_vstatus()->is_rotary_wing &&
 			(_mission_item.nav_cmd == NAV_CMD_LOITER_UNLIMITED ||
-			_mission_item.nav_cmd == NAV_CMD_LOITER_TIME_LIMIT ||
-			_mission_item.nav_cmd == NAV_CMD_LOITER_TURN_COUNT)) {
+			_mission_item.nav_cmd == NAV_CMD_LOITER_TIME_LIMIT)) {
 			/* Loiter mission item on a non rotary wing: the aircraft is going to circle the
 			 * coordinates with a radius equal to the loiter_radius field. It is not flying
 			 * through the waypoint center.
@@ -186,7 +187,61 @@ MissionBlock::is_mission_item_reached()
 			 */
 			if (dist >= 0.0f && dist <= _navigator->get_acceptance_radius(_mission_item.loiter_radius * 1.2f)
 				&& dist_z <= _navigator->get_default_acceptance_radius()) {
+
 				_waypoint_position_reached = true;
+			} else {
+				_time_first_inside_orbit = 0;
+			}
+
+		} else if (!_navigator->get_vstatus()->is_rotary_wing &&
+			(_mission_item.nav_cmd == NAV_CMD_LOITER_TO_ALT)) {
+
+
+			// NAV_CMD_LOITER_TO_ALT only uses mission item altitude once it's in the loiter
+			//  first check if the altitude setpoint is the mission setpoint
+			struct position_setpoint_s *curr_sp = &_navigator->get_position_setpoint_triplet()->current;
+
+			if (fabsf(curr_sp->alt - altitude_amsl) >= FLT_EPSILON) {
+				// check if the initial loiter has been accepted
+				dist = -1.0f;
+				dist_xy = -1.0f;
+				dist_z = -1.0f;
+
+				dist = get_distance_to_point_global_wgs84(_mission_item.lat, _mission_item.lon, curr_sp->alt,
+								_navigator->get_global_position()->lat,
+								_navigator->get_global_position()->lon,
+								_navigator->get_global_position()->alt,
+								&dist_xy, &dist_z);
+
+				if (dist >= 0.0f && dist <= _navigator->get_acceptance_radius(_mission_item.loiter_radius * 1.2f)
+					&& dist_z <= _navigator->get_default_acceptance_radius()) {
+
+					// now set the loiter to the final altitude in the NAV_CMD_LOITER_TO_ALT mission item
+					curr_sp->alt = altitude_amsl;
+					_navigator->set_position_setpoint_triplet_updated();
+				}
+
+			} else {
+				if (dist >= 0.0f && dist <= _navigator->get_acceptance_radius(_mission_item.loiter_radius * 1.2f)
+					&& dist_z <= _navigator->get_default_acceptance_radius()) {
+
+					_waypoint_position_reached = true;
+
+					// set required yaw from bearing to the next mission item
+					if (_mission_item.force_heading) {
+						struct position_setpoint_s next_sp = _navigator->get_position_setpoint_triplet()->next;
+						
+						if (next_sp.valid) {
+							_mission_item.yaw = get_bearing_to_next_waypoint(_navigator->get_global_position()->lat,
+											_navigator->get_global_position()->lon,
+											next_sp.lat, next_sp.lon);
+
+							_waypoint_yaw_reached = false;
+						} else {
+							_waypoint_yaw_reached = true;
+						}
+					}
+				}
 			}
 		} else {
 			/* for normal mission items used their acceptance radius */
@@ -213,8 +268,9 @@ MissionBlock::is_mission_item_reached()
 
 	if (_waypoint_position_reached && !_waypoint_yaw_reached) {
 
-		/* TODO: removed takeoff, why? */
-		if (_navigator->get_vstatus()->is_rotary_wing && PX4_ISFINITE(_mission_item.yaw)) {
+		if ((_navigator->get_vstatus()->is_rotary_wing
+			|| (_mission_item.nav_cmd == NAV_CMD_LOITER_TO_ALT && _mission_item.force_heading))
+			&& PX4_ISFINITE(_mission_item.yaw)) {
 
 			/* check yaw if defined only for rotary wing except takeoff */
 			float yaw_err = _wrap_pi(_mission_item.yaw - _navigator->get_global_position()->yaw);
@@ -242,18 +298,29 @@ MissionBlock::is_mission_item_reached()
 
 		if (_time_first_inside_orbit == 0) {
 			_time_first_inside_orbit = now;
-
-			// if (_mission_item.time_inside > 0.01f) {
-			// 	mavlink_log_critical(_mavlink_log_pub, "waypoint reached, wait for %.1fs",
-			// 		(double)_mission_item.time_inside);
-			// }
 		}
 
 		/* check if the MAV was long enough inside the waypoint orbit */
 		if (now - _time_first_inside_orbit >= (hrt_abstime)_mission_item.time_inside * 1e6f) {
+
+			// exit xtrack location
+			if (_mission_item.loiter_exit_xtrack &&
+				(_mission_item.nav_cmd == NAV_CMD_LOITER_TIME_LIMIT ||
+				 _mission_item.nav_cmd == NAV_CMD_LOITER_TO_ALT)) {
+
+				// reset lat/lon of loiter waypoint so vehicle exits on a tangent
+				struct position_setpoint_s *curr_sp = &_navigator->get_position_setpoint_triplet()->current;
+				curr_sp->lat = _navigator->get_global_position()->lat;
+				curr_sp->lon = _navigator->get_global_position()->lon;
+			}
+
 			return true;
 		}
 	}
+
+	// all acceptance criteria must be met in the same iteration
+	_waypoint_position_reached = false;
+	_waypoint_yaw_reached = false;
 	return false;
 }
 
@@ -298,7 +365,7 @@ MissionBlock::issue_command(const struct mission_item_s *item)
 		PX4_WARN("do_set_servo command");
 		// XXX: we should issue a vehicle command and handle this somewhere else
 		memset(&actuators, 0, sizeof(actuators));
-		// params[0] actuator number to be set 0..5 ( corresponds to AUX outputs 1..6
+		// params[0] actuator number to be set 0..5 (corresponds to AUX outputs 1..6)
 		// params[1] new value for selected actuator in ms 900...2000
 		actuators.control[(int)item->params[0]] = 1.0f / 2000 * -item->params[1];
 		actuators.timestamp = hrt_absolute_time();
@@ -332,8 +399,9 @@ MissionBlock::item_contains_position(const struct mission_item_s *item)
 	if (item->nav_cmd == NAV_CMD_DO_JUMP ||
 		item->nav_cmd == NAV_CMD_DO_CHANGE_SPEED ||
 		item->nav_cmd == NAV_CMD_DO_SET_SERVO ||
-		item->nav_cmd == NAV_CMD_DO_REPEAT_SERVO ||
 		item->nav_cmd == NAV_CMD_DO_DIGICAM_CONTROL ||
+		item->nav_cmd == NAV_CMD_DO_MOUNT_CONFIGURE ||
+		item->nav_cmd == NAV_CMD_DO_MOUNT_CONTROL ||
 		item->nav_cmd == NAV_CMD_DO_SET_CAM_TRIGG_DIST ||
 		item->nav_cmd == NAV_CMD_DO_VTOL_TRANSITION) {
 		return false;
@@ -349,6 +417,7 @@ MissionBlock::mission_item_to_position_setpoint(const struct mission_item_s *ite
 
 	if (item->nav_cmd == NAV_CMD_DO_VTOL_TRANSITION && PX4_ISFINITE(item->yaw)
 			&& item->params[0] >= vtol_vehicle_status_s::VEHICLE_VTOL_STATE_FW - 0.5f) {
+
 		sp->type = position_setpoint_s::SETPOINT_TYPE_POSITION;
 		waypoint_from_heading_and_distance(_navigator->get_global_position()->lat,
 										   _navigator->get_global_position()->lon,
@@ -365,7 +434,6 @@ MissionBlock::mission_item_to_position_setpoint(const struct mission_item_s *ite
 		return;
 	}
 
-	sp->valid = true;
 	sp->lat = item->lat;
 	sp->lon = item->lon;
 	sp->alt = item->altitude_is_relative ? item->altitude + _navigator->get_home_position()->alt : item->altitude;
@@ -397,8 +465,12 @@ MissionBlock::mission_item_to_position_setpoint(const struct mission_item_s *ite
 		}
 		break;
 
+	case NAV_CMD_LOITER_TO_ALT:
+		// initially use current altitude, and switch to mission item altitude once in loiter position
+		sp->alt = math::max(_navigator->get_global_position()->alt, _navigator->get_home_position()->alt + _param_loiter_min_alt.get());
+
+		// no break
 	case NAV_CMD_LOITER_TIME_LIMIT:
-	case NAV_CMD_LOITER_TURN_COUNT:
 	case NAV_CMD_LOITER_UNLIMITED:
 		sp->type = position_setpoint_s::SETPOINT_TYPE_LOITER;
 		if (_navigator->get_vstatus()->is_vtol && _param_vtol_wv_loiter.get()) {
@@ -410,6 +482,8 @@ MissionBlock::mission_item_to_position_setpoint(const struct mission_item_s *ite
 		sp->type = position_setpoint_s::SETPOINT_TYPE_POSITION;
 		break;
 	}
+
+	sp->valid = true;
 }
 
 void
@@ -472,7 +546,7 @@ MissionBlock::set_follow_target_item(struct mission_item_s *item, float min_clea
 
 	} else {
 
-		item->nav_cmd = NAV_CMD_FOLLOW_TARGET;
+		item->nav_cmd = NAV_CMD_DO_FOLLOW_REPOSITION;
 
 		/* use current target position */
 
@@ -605,5 +679,3 @@ MissionBlock::set_idle_item(struct mission_item_s *item)
 	item->autocontinue = true;
 	item->origin = ORIGIN_ONBOARD;
 }
-
-
diff --git a/src/modules/navigator/mission_block.h b/src/modules/navigator/mission_block.h
index 5f6e00a764..3b95c7a4ee 100644
--- a/src/modules/navigator/mission_block.h
+++ b/src/modules/navigator/mission_block.h
@@ -141,6 +141,7 @@ protected:
 	orb_advert_t    _actuator_pub;
 	orb_advert_t	_cmd_pub;
 
+	control::BlockParamFloat _param_loiter_min_alt;
 	control::BlockParamFloat _param_yaw_timeout;
 	control::BlockParamFloat _param_yaw_err;
 	control::BlockParamInt _param_vtol_wv_land;
diff --git a/src/modules/navigator/mission_feasibility_checker.cpp b/src/modules/navigator/mission_feasibility_checker.cpp
index bb68624437..720778770b 100644
--- a/src/modules/navigator/mission_feasibility_checker.cpp
+++ b/src/modules/navigator/mission_feasibility_checker.cpp
@@ -238,17 +238,18 @@ bool MissionFeasibilityChecker::checkMissionItemValidity(dm_item_t dm_current, s
 		if (missionitem.nav_cmd != NAV_CMD_IDLE &&
 			missionitem.nav_cmd != NAV_CMD_WAYPOINT &&
 			missionitem.nav_cmd != NAV_CMD_LOITER_UNLIMITED &&
-			/* not yet supported: missionitem.nav_cmd != NAV_CMD_LOITER_TURN_COUNT && */
 			missionitem.nav_cmd != NAV_CMD_LOITER_TIME_LIMIT &&
 			missionitem.nav_cmd != NAV_CMD_LAND &&
 			missionitem.nav_cmd != NAV_CMD_TAKEOFF &&
-			missionitem.nav_cmd != NAV_CMD_VTOL_LAND &&
+			missionitem.nav_cmd != NAV_CMD_LOITER_TO_ALT &&
 			missionitem.nav_cmd != NAV_CMD_VTOL_TAKEOFF &&
-			missionitem.nav_cmd != NAV_CMD_PATHPLANNING &&
+			missionitem.nav_cmd != NAV_CMD_VTOL_LAND &&
 			missionitem.nav_cmd != NAV_CMD_DO_JUMP &&
 			missionitem.nav_cmd != NAV_CMD_DO_SET_SERVO &&
 			missionitem.nav_cmd != NAV_CMD_DO_CHANGE_SPEED &&
 			missionitem.nav_cmd != NAV_CMD_DO_DIGICAM_CONTROL &&
+			missionitem.nav_cmd != NAV_CMD_DO_MOUNT_CONFIGURE &&
+			missionitem.nav_cmd != NAV_CMD_DO_MOUNT_CONTROL &&
 			missionitem.nav_cmd != NAV_CMD_DO_SET_CAM_TRIGG_DIST &&
 			missionitem.nav_cmd != NAV_CMD_DO_VTOL_TRANSITION) {
 
@@ -407,13 +408,15 @@ MissionFeasibilityChecker::check_dist_1wp(dm_item_t dm_current, size_t nMissionI
 
 bool
 MissionFeasibilityChecker::isPositionCommand(unsigned cmd){
-	if( cmd == NAV_CMD_WAYPOINT ||
-		cmd == NAV_CMD_LOITER_TIME_LIMIT ||
-		cmd == NAV_CMD_LOITER_TURN_COUNT ||
+	if (cmd == NAV_CMD_WAYPOINT ||
 		cmd == NAV_CMD_LOITER_UNLIMITED ||
-		cmd == NAV_CMD_TAKEOFF ||
+		cmd == NAV_CMD_LOITER_TIME_LIMIT ||
 		cmd == NAV_CMD_LAND ||
-		cmd == NAV_CMD_PATHPLANNING) {
+		cmd == NAV_CMD_TAKEOFF ||
+		cmd == NAV_CMD_LOITER_TO_ALT ||
+		cmd == NAV_CMD_VTOL_TAKEOFF ||
+		cmd == NAV_CMD_VTOL_LAND) {
+
 		return true;
 	} else {
 		return false;
diff --git a/src/modules/navigator/navigation.h b/src/modules/navigator/navigation.h
index 3497b8563e..85a4dd8c22 100644
--- a/src/modules/navigator/navigation.h
+++ b/src/modules/navigator/navigation.h
@@ -54,21 +54,19 @@ enum NAV_CMD {
 	NAV_CMD_IDLE = 0,
 	NAV_CMD_WAYPOINT = 16,
 	NAV_CMD_LOITER_UNLIMITED = 17,
-	NAV_CMD_LOITER_TURN_COUNT = 18,
 	NAV_CMD_LOITER_TIME_LIMIT = 19,
-	NAV_CMD_RETURN_TO_LAUNCH = 20,
 	NAV_CMD_LAND = 21,
 	NAV_CMD_TAKEOFF = 22,
-	NAV_CMD_ROI = 80,
-	NAV_CMD_PATHPLANNING = 81,
+	NAV_CMD_LOITER_TO_ALT = 31,
+	NAV_CMD_DO_FOLLOW_REPOSITION = 33,
 	NAV_CMD_VTOL_TAKEOFF = 84,
 	NAV_CMD_VTOL_LAND = 85,
 	NAV_CMD_DO_JUMP = 177,
 	NAV_CMD_DO_CHANGE_SPEED = 178,
 	NAV_CMD_DO_SET_SERVO=183,
-	NAV_CMD_DO_REPEAT_SERVO=184,
-	NAV_CMD_FOLLOW_TARGET = 194, // temporary placeholder
 	NAV_CMD_DO_DIGICAM_CONTROL=203,
+	NAV_CMD_DO_MOUNT_CONFIGURE=204,
+	NAV_CMD_DO_MOUNT_CONTROL=205,
 	NAV_CMD_DO_SET_CAM_TRIGG_DIST=206,
 	NAV_CMD_DO_VTOL_TRANSITION=3000,
 	NAV_CMD_INVALID=UINT16_MAX /* ensure that casting a large number results in a specific error */
@@ -99,7 +97,8 @@ struct mission_item_s {
 	float yaw;			/**< in radians NED -PI..+PI, NAN means don't change yaw		*/
 	float loiter_radius;		/**< loiter radius in meters, 0 for a VTOL to hover     */
 	int8_t loiter_direction;	/**< 1: positive / clockwise, -1, negative.		*/
-	unsigned nav_cmd;		/**< navigation command					*/
+	bool loiter_exit_xtrack;	/**< exit xtrack location: 0 for center of loiter wp, 1 for exit location */
+	enum NAV_CMD nav_cmd;		/**< navigation command					*/
 	float acceptance_radius;	/**< default radius in which the mission is accepted as reached in meters */
 	float time_inside;		/**< time that the MAV should stay inside the radius before advancing in seconds */
 	float pitch_min;		/**< minimal pitch angle for fixed wing takeoff waypoints */

From ebce72572097f1941a753638bfa15a3d16de8fe1 Mon Sep 17 00:00:00 2001
From: Daniel Agar <daniel@agar.ca>
Date: Tue, 5 Jul 2016 17:38:24 -0400
Subject: [PATCH 0996/1230] px4fmu-v2 disable motor_ramp

---
 cmake/configs/nuttx_px4fmu-v2_default.cmake | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/cmake/configs/nuttx_px4fmu-v2_default.cmake b/cmake/configs/nuttx_px4fmu-v2_default.cmake
index 53c4f8c792..8907cf82dd 100644
--- a/cmake/configs/nuttx_px4fmu-v2_default.cmake
+++ b/cmake/configs/nuttx_px4fmu-v2_default.cmake
@@ -68,7 +68,7 @@ set(config_module_list
 	systemcmds/ver
 	#systemcmds/sd_bench
 	#systemcmds/tests
-	systemcmds/motor_ramp
+	#systemcmds/motor_ramp
 
 	#
 	# General system control

From de14418e93ff9c8758cfd228ae2d2e4cc618db3d Mon Sep 17 00:00:00 2001
From: Daniel Agar <daniel@agar.ca>
Date: Tue, 12 Jul 2016 13:45:32 -0400
Subject: [PATCH 0997/1230] fw_pos_ctrl_l1 var naming consistency and effc++

---
 Tools/check_code_style_all.sh                 |   1 +
 src/lib/controllib/block/BlockParam.hpp       |   3 +
 src/lib/external_lgpl/tecs/tecs.h             |   2 +
 src/lib/launchdetection/LaunchDetector.h      |   6 +-
 src/lib/launchdetection/LaunchMethod.h        |   2 +
 src/modules/fw_pos_control_l1/CMakeLists.txt  |   2 +-
 .../fw_pos_control_l1_main.cpp                | 272 ++++++++++--------
 .../fw_pos_control_l1/landingslope.cpp        |  15 +-
 src/modules/fw_pos_control_l1/landingslope.h  |   2 +-
 .../fw_pos_control_l1/mtecs/limitoverride.cpp |   8 +-
 .../fw_pos_control_l1/mtecs/limitoverride.h   |  30 +-
 src/modules/fw_pos_control_l1/mtecs/mTecs.cpp |  50 ++--
 .../fw_pos_control_l1/mtecs/mTecs_blocks.h    |  32 ++-
 src/modules/vtol_att_control/vtol_type.h      |   2 +
 14 files changed, 256 insertions(+), 171 deletions(-)

diff --git a/Tools/check_code_style_all.sh b/Tools/check_code_style_all.sh
index c8224c0a7b..d39e8ea0b9 100755
--- a/Tools/check_code_style_all.sh
+++ b/Tools/check_code_style_all.sh
@@ -23,6 +23,7 @@ find \
     src/modules/controllib_test \
     src/modules/dataman \
     src/modules/fw_att_control \
+    src/modules/fw_pos_control_l1 \
     src/modules/gpio_led \
     src/modules/land_detector \
     src/modules/local_position_estimator \
diff --git a/src/lib/controllib/block/BlockParam.hpp b/src/lib/controllib/block/BlockParam.hpp
index 693a8ec3b2..f88a80e6ba 100644
--- a/src/lib/controllib/block/BlockParam.hpp
+++ b/src/lib/controllib/block/BlockParam.hpp
@@ -78,6 +78,9 @@ class BlockParam : public BlockParamBase
 public:
 	BlockParam(Block *block, const char *name,
 		   bool parent_prefix = true, T *extern_address = NULL);
+	BlockParam(const BlockParam &) = delete;
+	BlockParam &operator=(const BlockParam &) = delete;
+
 	T get();
 	void commit();
 	void set(T val);
diff --git a/src/lib/external_lgpl/tecs/tecs.h b/src/lib/external_lgpl/tecs/tecs.h
index e892a72f7a..cbcc6ed9ae 100644
--- a/src/lib/external_lgpl/tecs/tecs.h
+++ b/src/lib/external_lgpl/tecs/tecs.h
@@ -66,6 +66,8 @@ public:
 		_TASmin(3.0f),
 		_TAS_dem(0.0f),
 		_TAS_dem_last(0.0f),
+		_EAS_dem(0.0f),
+		_hgt_dem(0.0f),
 		_hgt_dem_in_old(0.0f),
 		_hgt_dem_adj(0.0f),
 		_hgt_dem_adj_last(0.0f),
diff --git a/src/lib/launchdetection/LaunchDetector.h b/src/lib/launchdetection/LaunchDetector.h
index 3b276c5563..cc64f8b431 100644
--- a/src/lib/launchdetection/LaunchDetector.h
+++ b/src/lib/launchdetection/LaunchDetector.h
@@ -55,7 +55,10 @@ class __EXPORT LaunchDetector : public control::SuperBlock
 {
 public:
 	LaunchDetector();
-	~LaunchDetector();
+	LaunchDetector(const LaunchDetector &) = delete;
+	LaunchDetector operator=(const LaunchDetector &) = delete;
+	virtual ~LaunchDetector();
+
 	void reset();
 
 	void update(float accel_x);
@@ -80,7 +83,6 @@ private:
 	control::BlockParamInt launchdetection_on;
 	control::BlockParamFloat throttlePreTakeoff;
 
-
 };
 
 }
diff --git a/src/lib/launchdetection/LaunchMethod.h b/src/lib/launchdetection/LaunchMethod.h
index 66ee3f2c6e..065811aa9c 100644
--- a/src/lib/launchdetection/LaunchMethod.h
+++ b/src/lib/launchdetection/LaunchMethod.h
@@ -58,6 +58,8 @@ enum LaunchDetectionResult {
 class LaunchMethod
 {
 public:
+	virtual ~LaunchMethod() {};
+
 	virtual void update(float accel_x) = 0;
 	virtual LaunchDetectionResult getLaunchDetected() const = 0;
 	virtual void reset() = 0;
diff --git a/src/modules/fw_pos_control_l1/CMakeLists.txt b/src/modules/fw_pos_control_l1/CMakeLists.txt
index 53d12b0401..5a4e7fa6f1 100644
--- a/src/modules/fw_pos_control_l1/CMakeLists.txt
+++ b/src/modules/fw_pos_control_l1/CMakeLists.txt
@@ -35,7 +35,7 @@ px4_add_module(
 	MAIN fw_pos_control_l1
 	STACK_MAIN 1200
 	COMPILE_FLAGS
-		-Os
+		-Os -Weffc++
 	SRCS
 		fw_pos_control_l1_main.cpp
 		landingslope.cpp
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 ae38b77095..8871ccf551 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
@@ -103,18 +103,16 @@
 #include <vtol_att_control/vtol_type.h>
 
 static int	_control_task = -1;			/**< task handle for sensor task */
+
 #define HDG_HOLD_DIST_NEXT 			3000.0f 	// initial distance of waypoint in front of plane in heading hold mode
 #define HDG_HOLD_REACHED_DIST 		1000.0f 	// distance (plane to waypoint in front) at which waypoints are reset in heading hold mode
 #define HDG_HOLD_SET_BACK_DIST 		100.0f 		// distance by which previous waypoint is set behind the plane
 #define HDG_HOLD_YAWRATE_THRESH 	0.15f 		// max yawrate at which plane locks yaw for heading hold mode
 #define HDG_HOLD_MAN_INPUT_THRESH 	0.01f 		// max manual roll input from user which does not change the locked heading
 #define T_ALT_TIMEOUT 				1 			// time after which we abort landing if terrain estimate is not valid
-
-static constexpr float THROTTLE_THRESH =
-	0.05f; 	///< max throttle from user which will not lead to motors spinning up in altitude controlled modes
-static constexpr float MANUAL_THROTTLE_CLIMBOUT_THRESH =
-	0.85f;	///< a throttle / pitch input above this value leads to the system switching to climbout mode
-static constexpr float ALTHOLD_EPV_RESET_THRESH = 5.0f;
+#define THROTTLE_THRESH 0.05f 	///< max throttle from user which will not lead to motors spinning up in altitude controlled modes
+#define MANUAL_THROTTLE_CLIMBOUT_THRESH 0.85f	///< a throttle / pitch input above this value leads to the system switching to climbout mode
+#define ALTHOLD_EPV_RESET_THRESH 5.0f
 
 /**
  * L1 control app start / stop handling function
@@ -138,6 +136,10 @@ public:
 	 */
 	~FixedwingPositionControl();
 
+	// prevent copying
+	FixedwingPositionControl(const FixedwingPositionControl &) = delete;
+	FixedwingPositionControl operator=(const FixedwingPositionControl &other) = delete;
+
 	/**
 	 * Start the sensors task.
 	 *
@@ -164,8 +166,8 @@ private:
 	int		_control_mode_sub;		/**< control mode subscription */
 	int		_vehicle_status_sub;		/**< vehicle status subscription */
 	int		_vehicle_land_detected_sub;	/**< vehicle land detected subscription */
-	int 		_params_sub;			/**< notification of parameter updates */
-	int 		_manual_control_sub;		/**< notification of manual control updates */
+	int		_params_sub;			/**< notification of parameter updates */
+	int		_manual_control_sub;		/**< notification of manual control updates */
 	int		_sensor_combined_sub;		/**< for body frame accelerations */
 
 	orb_advert_t	_attitude_sp_pub;		/**< attitude setpoint */
@@ -198,39 +200,35 @@ private:
 	struct position_setpoint_s _hdg_hold_curr_wp;	/**< position to which heading hold flies */
 	hrt_abstime _control_position_last_called; /**<last call of control_position  */
 
-	/* land states */
-	bool land_noreturn_horizontal;
-	bool land_noreturn_vertical;
-	bool land_stayonground;
-	bool land_motor_lim;
-	bool land_onslope;
-	bool land_useterrain;
+	/* Landing */
+	bool _land_noreturn_horizontal;
+	bool _land_noreturn_vertical;
+	bool _land_stayonground;
+	bool _land_motor_lim;
+	bool _land_onslope;
+	bool _land_useterrain;
+
+	Landingslope _landingslope;
+
+	hrt_abstime _time_started_landing;	//*< time at which landing started */
 
-	// landing relevant states
 	float _t_alt_prev_valid;	//**< last terrain estimate which was valid */
 	hrt_abstime _time_last_t_alt; //*< time at which we had last valid terrain alt */
-	hrt_abstime _time_started_landing;	//*< time at which landing started */
-	float height_flare;					//*< estimated height to ground at which flare started */
+
+	float _flare_height;					//*< estimated height to ground at which flare started */
+	float _flare_curve_alt_rel_last;
+	float _target_bearing;				//*< estimated height to ground at which flare started */
 
 	bool _was_in_air;	/**< indicated wether the plane was in the air in the previous interation*/
 	hrt_abstime _time_went_in_air;	/**< time at which the plane went in the air */
 
+	/* Takeoff launch detection and runway */
+	launchdetection::LaunchDetector _launchDetector;
+	LaunchDetectionResult _launch_detection_state;
+
 	runwaytakeoff::RunwayTakeoff _runway_takeoff;
 
-	/* takeoff/launch states */
-	LaunchDetectionResult launch_detection_state;
-
-	bool last_manual;				///< true if the last iteration was in manual mode (used to determine when a reset is needed)
-
-	/* Landingslope object */
-	Landingslope landingslope;
-	float flare_curve_alt_rel_last;
-
-	/* heading hold */
-	float target_bearing;
-
-	/* Launch detection */
-	launchdetection::LaunchDetector launchDetector;
+	bool _last_manual;				///< true if the last iteration was in manual mode (used to determine when a reset is needed)
 
 	/* throttle and airspeed states */
 	float _airspeed_error;				///< airspeed error to setpoint in m/s
@@ -305,9 +303,10 @@ private:
 		float land_flare_pitch_max_deg;
 		int land_use_terrain_estimate;
 		float land_airspeed_scale;
+
 		int vtol_type;
 
-	}		_parameters;			/**< local copies of interesting parameters */
+	} _parameters;			/**< local copies of interesting parameters */
 
 	struct {
 
@@ -358,9 +357,10 @@ private:
 		param_t land_flare_pitch_max_deg;
 		param_t land_use_terrain_estimate;
 		param_t land_airspeed_scale;
+
 		param_t vtol_type;
 
-	}		_parameter_handles;		/**< handles for interesting parameters */
+	} _parameter_handles;		/**< handles for interesting parameters */
 
 
 	/**
@@ -438,7 +438,7 @@ private:
 	/**
 	 * Check if we are in a takeoff situation
 	 */
-	bool in_takeoff_situation();
+	bool 		in_takeoff_situation();
 
 	/**
 	 * Do takeoff help when in altitude controlled modes
@@ -564,38 +564,44 @@ FixedwingPositionControl::FixedwingPositionControl() :
 	_hdg_hold_prev_wp{},
 	_hdg_hold_curr_wp{},
 	_control_position_last_called(0),
-
-	land_noreturn_horizontal(false),
-	land_noreturn_vertical(false),
-	land_stayonground(false),
-	land_motor_lim(false),
-	land_onslope(false),
-	land_useterrain(false),
+	_land_noreturn_horizontal(false),
+	_land_noreturn_vertical(false),
+	_land_stayonground(false),
+	_land_motor_lim(false),
+	_land_onslope(false),
+	_land_useterrain(false),
+	_landingslope(),
+	_time_started_landing(0),
 	_t_alt_prev_valid(0),
 	_time_last_t_alt(0),
-	_time_started_landing(0),
-	height_flare(0.0f),
+	_flare_height(0.0f),
+	_flare_curve_alt_rel_last(0.0f),
+	_target_bearing(0.0f),
 	_was_in_air(false),
 	_time_went_in_air(0),
+	_launchDetector(),
+	_launch_detection_state(LAUNCHDETECTION_RES_NONE),
 	_runway_takeoff(),
-	launch_detection_state(LAUNCHDETECTION_RES_NONE),
-	last_manual(false),
-	landingslope(),
-	flare_curve_alt_rel_last(0.0f),
-	target_bearing(0.0f),
-	launchDetector(),
+	_last_manual(false),
 	_airspeed_error(0.0f),
 	_airspeed_valid(false),
 	_airspeed_last_received(0),
 	_groundspeed_undershoot(0.0f),
 	_global_pos_valid(false),
+	_R_nb(),
+	_roll(0.0f),
+	_pitch(0.0f),
+	_yaw(0.0f),
 	_reinitialize_tecs(true),
 	_is_tecs_running(false),
 	_last_tecs_update(0.0f),
 	_asp_after_transition(0.0f),
 	_was_in_transition(false),
 	_l1_control(),
-	_control_mode_current(FW_POSCTRL_MODE_OTHER)
+	_tecs(),
+	_control_mode_current(FW_POSCTRL_MODE_OTHER),
+	_parameters(),
+	_parameter_handles()
 {
 	_fw_pos_ctrl_status = {};
 
@@ -777,17 +783,17 @@ FixedwingPositionControl::parameters_update()
 	}
 
 	/* Update the landing slope */
-	landingslope.update(math::radians(_parameters.land_slope_angle), _parameters.land_flare_alt_relative,
-			    _parameters.land_thrust_lim_alt_relative, _parameters.land_H1_virt);
+	_landingslope.update(math::radians(_parameters.land_slope_angle), _parameters.land_flare_alt_relative,
+			     _parameters.land_thrust_lim_alt_relative, _parameters.land_H1_virt);
 
 	/* Update and publish the navigation capabilities */
-	_fw_pos_ctrl_status.landing_slope_angle_rad = landingslope.landing_slope_angle_rad();
-	_fw_pos_ctrl_status.landing_horizontal_slope_displacement = landingslope.horizontal_slope_displacement();
-	_fw_pos_ctrl_status.landing_flare_length = landingslope.flare_length();
+	_fw_pos_ctrl_status.landing_slope_angle_rad = _landingslope.landing_slope_angle_rad();
+	_fw_pos_ctrl_status.landing_horizontal_slope_displacement = _landingslope.horizontal_slope_displacement();
+	_fw_pos_ctrl_status.landing_flare_length = _landingslope.flare_length();
 	fw_pos_ctrl_status_publish();
 
 	/* Update Launch Detector Parameters */
-	launchDetector.updateParams();
+	_launchDetector.updateParams();
 
 	_runway_takeoff.updateParams();
 
@@ -1093,9 +1099,9 @@ float FixedwingPositionControl::get_terrain_altitude_landing(float land_setpoint
 	/* Decide if the terrain estimation can be used, once we switched to using the terrain we stick with it
 	 * for the whole landing */
 	if (_parameters.land_use_terrain_estimate && global_pos.terrain_alt_valid) {
-		if (!land_useterrain) {
+		if (!_land_useterrain) {
 			mavlink_log_info(&_mavlink_log_pub, "Landing, using terrain estimate");
-			land_useterrain = true;
+			_land_useterrain = true;
 		}
 
 		return global_pos.terrain_alt;
@@ -1183,10 +1189,12 @@ bool FixedwingPositionControl::update_desired_altitude(float dt)
 
 bool FixedwingPositionControl::in_takeoff_situation()
 {
+	// in air for < 10s
 	const hrt_abstime delta_takeoff = 10000000;
 
 	if (hrt_elapsed_time(&_time_went_in_air) < delta_takeoff
 	    && _global_pos.alt <= _takeoff_ground_alt + _parameters.climbout_diff) {
+
 		return true;
 	}
 
@@ -1325,7 +1333,8 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
 		float mission_throttle = _parameters.throttle_cruise;
 
 		if (PX4_ISFINITE(_pos_sp_triplet.current.cruising_throttle) &&
-			_pos_sp_triplet.current.cruising_throttle > 0.01f) {
+		    _pos_sp_triplet.current.cruising_throttle > 0.01f) {
+
 			mission_throttle = _pos_sp_triplet.current.cruising_throttle;
 		}
 
@@ -1373,8 +1382,8 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
 			    ((_global_pos.alt < pos_sp_triplet.current.alt + _parameters.climbout_diff)
 			     && _fw_pos_ctrl_status.abort_landing == true)) {
 				/* limit roll motion to ensure enough lift */
-				_att_sp.roll_body = math::constrain(_att_sp.roll_body, math::radians(-15.0f),
-								    math::radians(15.0f));
+				_att_sp.roll_body = math::constrain(_att_sp.roll_body, math::radians(-10.0f),
+								    math::radians(10.0f));
 			}
 
 			tecs_update_pitch_throttle(alt_sp, calculate_target_airspeed(mission_airspeed), eas2tas,
@@ -1400,6 +1409,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
 			/* Horizontal landing control */
 			/* switch to heading hold for the last meters, continue heading hold after */
 			float wp_distance = get_distance_to_next_waypoint(current_position(0), current_position(1), curr_wp(0), curr_wp(1));
+
 			/* calculate a waypoint distance value which is 0 when the aircraft is behind the waypoint */
 			float wp_distance_save = wp_distance;
 
@@ -1414,24 +1424,25 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
 			math::Vector<2> curr_wp_shifted;
 			double lat;
 			double lon;
-			create_waypoint_from_line_and_dist(pos_sp_triplet.current.lat, pos_sp_triplet.current.lon, pos_sp_triplet.previous.lat,
-							   pos_sp_triplet.previous.lon, -1000.0f, &lat, &lon);
+			create_waypoint_from_line_and_dist(pos_sp_triplet.current.lat, pos_sp_triplet.current.lon,
+							   pos_sp_triplet.previous.lat, pos_sp_triplet.previous.lon, -1000.0f, &lat, &lon);
 			curr_wp_shifted(0) = (float)lat;
 			curr_wp_shifted(1) = (float)lon;
 
 			// we want the plane to keep tracking the desired flight path until we start flaring
 			// if we go into heading hold mode earlier then we risk to be pushed away from the runway by cross winds
 			//if (land_noreturn_vertical) {
-			if (wp_distance < _parameters.land_heading_hold_horizontal_distance || land_noreturn_horizontal) {
+			if (wp_distance < _parameters.land_heading_hold_horizontal_distance || _land_noreturn_horizontal) {
 
 				/* heading hold, along the line connecting this and the last waypoint */
 
-				if (!land_noreturn_horizontal) {//set target_bearing in first occurrence
+				if (!_land_noreturn_horizontal) {
+					// set target_bearing in first occurrence
 					if (pos_sp_triplet.previous.valid) {
-						target_bearing = bearing_lastwp_currwp;
+						_target_bearing = bearing_lastwp_currwp;
 
 					} else {
-						target_bearing = _yaw;
+						_target_bearing = _yaw;
 					}
 
 					mavlink_log_info(&_mavlink_log_pub, "#Landing, heading hold");
@@ -1439,9 +1450,9 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
 
 //					warnx("NORET: %d, target_bearing: %d, yaw: %d", (int)land_noreturn_horizontal, (int)math::degrees(target_bearing), (int)math::degrees(_yaw));
 
-				_l1_control.navigate_heading(target_bearing, _yaw, ground_speed_2d);
+				_l1_control.navigate_heading(_target_bearing, _yaw, ground_speed_2d);
 
-				land_noreturn_horizontal = true;
+				_land_noreturn_horizontal = true;
 
 			} else {
 
@@ -1452,7 +1463,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
 			_att_sp.roll_body = _l1_control.nav_roll();
 			_att_sp.yaw_body = _l1_control.nav_bearing();
 
-			if (land_noreturn_horizontal) {
+			if (_land_noreturn_horizontal) {
 				/* limit roll motion to prevent wings from touching the ground first */
 				_att_sp.roll_body = math::constrain(_att_sp.roll_body, math::radians(-10.0f), math::radians(10.0f));
 			}
@@ -1491,7 +1502,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
 					}
 
 				} else if ((!_global_pos.terrain_alt_valid && hrt_elapsed_time(&_time_last_t_alt) < T_ALT_TIMEOUT * 1000 * 1000)
-					   || land_noreturn_vertical) {
+					   || _land_noreturn_vertical) {
 					// use previous terrain estimate for some time and hope to recover
 					// if we are already flaring (land_noreturn_vertical) then just
 					//  go with the old estimate
@@ -1513,75 +1524,81 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
 			float L_altitude_rel = pos_sp_triplet.previous.valid ?
 					       pos_sp_triplet.previous.alt - terrain_alt : 0.0f;
 
-			float landing_slope_alt_rel_desired = landingslope.getLandingSlopeRelativeAltitudeSave(wp_distance,
+			float landing_slope_alt_rel_desired = _landingslope.getLandingSlopeRelativeAltitudeSave(wp_distance,
 							      bearing_lastwp_currwp, bearing_airplane_currwp);
 
 			/* Check if we should start flaring with a vertical and a
 			 * horizontal limit (with some tolerance)
 			 * The horizontal limit is only applied when we are in front of the wp
 			 */
-			if (((_global_pos.alt < terrain_alt + landingslope.flare_relative_alt()) &&
-			     (wp_distance_save < landingslope.flare_length() + 5.0f)) ||
-			    land_noreturn_vertical) {  //checking for land_noreturn to avoid unwanted climb out
+			if (((_global_pos.alt < terrain_alt + _landingslope.flare_relative_alt()) &&
+			     (wp_distance_save < _landingslope.flare_length() + 5.0f)) ||
+			    _land_noreturn_vertical) {  //checking for land_noreturn to avoid unwanted climb out
+
 				/* land with minimal speed */
 
-//					/* force TECS to only control speed with pitch, altitude is only implicitely controlled now */
-//					_tecs.set_speed_weight(2.0f);
+				/* force TECS to only control speed with pitch, altitude is only implicitly controlled now */
+				// _tecs.set_speed_weight(2.0f);
 
 				/* kill the throttle if param requests it */
 				throttle_max = _parameters.throttle_max;
 
 				/* enable direct yaw control using rudder/wheel */
-				_att_sp.yaw_body = target_bearing;
+				_att_sp.yaw_body = _target_bearing;
 				_att_sp.fw_control_yaw = true;
 
-				if (_global_pos.alt < terrain_alt + landingslope.motor_lim_relative_alt() || land_motor_lim) {
+				if (_global_pos.alt < terrain_alt + _landingslope.motor_lim_relative_alt() || _land_motor_lim) {
 					throttle_max = math::min(throttle_max, _parameters.throttle_land_max);
 
-					if (!land_motor_lim) {
-						land_motor_lim  = true;
+					if (!_land_motor_lim) {
+						_land_motor_lim  = true;
 						mavlink_log_info(&_mavlink_log_pub, "#Landing, limiting throttle");
 					}
 
 				}
 
-				float flare_curve_alt_rel = landingslope.getFlareCurveRelativeAltitudeSave(wp_distance, bearing_lastwp_currwp,
+				float flare_curve_alt_rel = _landingslope.getFlareCurveRelativeAltitudeSave(wp_distance, bearing_lastwp_currwp,
 							    bearing_airplane_currwp);
 
 				/* avoid climbout */
-				if ((flare_curve_alt_rel_last < flare_curve_alt_rel && land_noreturn_vertical) || land_stayonground) {
+				if ((_flare_curve_alt_rel_last < flare_curve_alt_rel && _land_noreturn_vertical) || _land_stayonground) {
 					flare_curve_alt_rel = 0.0f; // stay on ground
-					land_stayonground = true;
+					_land_stayonground = true;
 				}
 
 				tecs_update_pitch_throttle(terrain_alt + flare_curve_alt_rel,
-							   calculate_target_airspeed(airspeed_land), eas2tas,
+							   calculate_target_airspeed(airspeed_land),
+							   eas2tas,
 							   math::radians(_parameters.land_flare_pitch_min_deg),
 							   math::radians(_parameters.land_flare_pitch_max_deg),
-							   0.0f, throttle_max, throttle_land,
-							   false,  land_motor_lim ? math::radians(_parameters.land_flare_pitch_min_deg) : math::radians(
-								   _parameters.pitch_limit_min),
-							   _global_pos.alt, ground_speed,
-							   land_motor_lim ? tecs_status_s::TECS_MODE_LAND_THROTTLELIM : tecs_status_s::TECS_MODE_LAND);
+							   0.0f,
+							   throttle_max,
+							   throttle_land,
+							   false,
+							   _land_motor_lim ? math::radians(_parameters.land_flare_pitch_min_deg)
+							   : math::radians(_parameters.pitch_limit_min),
+							   _global_pos.alt,
+							   ground_speed,
+							   _land_motor_lim ? tecs_status_s::TECS_MODE_LAND_THROTTLELIM : tecs_status_s::TECS_MODE_LAND);
 
-				if (!land_noreturn_vertical) {
+				if (!_land_noreturn_vertical) {
 					// just started with the flaring phase
 					_att_sp.pitch_body = 0.0f;
-					height_flare = _global_pos.alt - terrain_alt;
+					_flare_height = _global_pos.alt - terrain_alt;
 					mavlink_log_info(&_mavlink_log_pub, "#Landing, flaring");
-					land_noreturn_vertical = true;
+					_land_noreturn_vertical = true;
 
 				} else {
 					if (_global_pos.vel_d > 0.1f) {
 						_att_sp.pitch_body = math::radians(_parameters.land_flare_pitch_min_deg) *
-								     math::constrain((height_flare - (_global_pos.alt - terrain_alt)) / height_flare, 0.0f, 1.0f);
+								     math::constrain((_flare_height - (_global_pos.alt - terrain_alt)) / _flare_height, 0.0f, 1.0f);
 
 					} else {
 						_att_sp.pitch_body = _att_sp.pitch_body;
 					}
 				}
 
-				flare_curve_alt_rel_last = flare_curve_alt_rel;
+				_flare_curve_alt_rel_last = flare_curve_alt_rel;
 
 			} else {
 
@@ -1596,13 +1613,13 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
 				 * */
 				float altitude_desired_rel;
 
-				if (_global_pos.alt > terrain_alt + landing_slope_alt_rel_desired || land_onslope) {
+				if (_global_pos.alt > terrain_alt + landing_slope_alt_rel_desired || _land_onslope) {
 					/* stay on slope */
 					altitude_desired_rel = landing_slope_alt_rel_desired;
 
-					if (!land_onslope) {
+					if (!_land_onslope) {
 						mavlink_log_info(&_mavlink_log_pub, "#Landing, on slope");
-						land_onslope = true;
+						_land_onslope = true;
 					}
 
 				} else {
@@ -1692,29 +1709,29 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
 
 			} else {
 				/* Perform launch detection */
-				if (launchDetector.launchDetectionEnabled() &&
-				    launch_detection_state != LAUNCHDETECTION_RES_DETECTED_ENABLEMOTORS) {
+				if (_launchDetector.launchDetectionEnabled() &&
+				    _launch_detection_state != LAUNCHDETECTION_RES_DETECTED_ENABLEMOTORS) {
 					/* Inform user that launchdetection is running */
 					static hrt_abstime last_sent = 0;
 
 					if (hrt_absolute_time() - last_sent > 4e6) {
-						mavlink_log_critical(&_mavlink_log_pub, "#Launchdetection running");
+						mavlink_log_critical(&_mavlink_log_pub, "#Launch detection running");
 						last_sent = hrt_absolute_time();
 					}
 
 					/* Detect launch */
-					launchDetector.update(accel_body(0));
+					_launchDetector.update(accel_body(0));
 
 					/* update our copy of the launch detection state */
-					launch_detection_state = launchDetector.getLaunchDetected();
+					_launch_detection_state = _launchDetector.getLaunchDetected();
 
 				} else	{
 					/* no takeoff detection --> fly */
-					launch_detection_state = LAUNCHDETECTION_RES_DETECTED_ENABLEMOTORS;
+					_launch_detection_state = LAUNCHDETECTION_RES_DETECTED_ENABLEMOTORS;
 				}
 
 				/* Set control values depending on the detection state */
-				if (launch_detection_state != LAUNCHDETECTION_RES_NONE) {
+				if (_launch_detection_state != LAUNCHDETECTION_RES_NONE) {
 					/* Launch has been detected, hence we have to control the plane. */
 
 					_l1_control.navigate_waypoints(prev_wp, curr_wp, current_position, ground_speed_2d);
@@ -1723,13 +1740,13 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
 
 					/* Select throttle: only in LAUNCHDETECTION_RES_DETECTED_ENABLEMOTORS we want to use
 					 * full throttle, otherwise we use the preTakeOff Throttle */
-					float takeoff_throttle = launch_detection_state !=
+					float takeoff_throttle = _launch_detection_state !=
 								 LAUNCHDETECTION_RES_DETECTED_ENABLEMOTORS ?
-								 launchDetector.getThrottlePreTakeoff() : _parameters.throttle_max;
+								 _launchDetector.getThrottlePreTakeoff() : _parameters.throttle_max;
 
 					/* select maximum pitch: the launchdetector may impose another limit for the pitch
 					 * depending on the state of the launch */
-					float takeoff_pitch_max_deg = launchDetector.getPitchMax(_parameters.pitch_limit_max);
+					float takeoff_pitch_max_deg = _launchDetector.getPitchMax(_parameters.pitch_limit_max);
 					float takeoff_pitch_max_rad = math::radians(takeoff_pitch_max_deg);
 
 					/* apply minimum pitch and limit roll if target altitude is not within climbout_diff
@@ -1970,7 +1987,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
 
 		} else {
 			_att_sp.roll_body = (_manual.y * _parameters.man_roll_max_rad)
-						+ _parameters.rollsp_offset_rad;
+					    + _parameters.rollsp_offset_rad;
 		}
 
 	} else {
@@ -1992,7 +2009,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
 		setpoint = false;
 
 		/* reset landing and takeoff state */
-		if (!last_manual) {
+		if (!_last_manual) {
 			reset_landing_state();
 			reset_takeoff_state();
 		}
@@ -2005,12 +2022,12 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
 
 	} else if (_control_mode_current ==  FW_POSCTRL_MODE_AUTO && // launchdetector only available in auto
 		   pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_TAKEOFF &&
-		   launch_detection_state != LAUNCHDETECTION_RES_DETECTED_ENABLEMOTORS &&
+		   _launch_detection_state != LAUNCHDETECTION_RES_DETECTED_ENABLEMOTORS &&
 		   !_runway_takeoff.runwayTakeoffEnabled()) {
 		/* making sure again that the correct thrust is used,
 		 * without depending on library calls for safety reasons.
 		   the pre-takeoff throttle and the idle throttle normally map to the same parameter. */
-		_att_sp.thrust = (launchDetector.launchDetectionEnabled()) ? launchDetector.getThrottlePreTakeoff() :
+		_att_sp.thrust = (_launchDetector.launchDetectionEnabled()) ? _launchDetector.getThrottlePreTakeoff() :
 				 _parameters.throttle_idle;
 
 	} else if (_control_mode_current ==  FW_POSCTRL_MODE_AUTO &&
@@ -2039,19 +2056,19 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
 	 * already (not by tecs) */
 	if (!(_control_mode_current ==  FW_POSCTRL_MODE_AUTO && (
 		      (pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_TAKEOFF &&
-		       (launch_detection_state == LAUNCHDETECTION_RES_NONE ||
+		       (_launch_detection_state == LAUNCHDETECTION_RES_NONE ||
 			_runway_takeoff.runwayTakeoffEnabled())) ||
 		      (pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_LAND &&
-		       land_noreturn_vertical))
+		       _land_noreturn_vertical))
 	     )) {
 		_att_sp.pitch_body = get_tecs_pitch();
 	}
 
 	if (_control_mode.flag_control_position_enabled) {
-		last_manual = false;
+		_last_manual = false;
 
 	} else {
-		last_manual = true;
+		_last_manual = true;
 	}
 
 
@@ -2216,7 +2233,8 @@ FixedwingPositionControl::task_main()
 					_fw_pos_ctrl_status.xtrack_error = _l1_control.crosstrack_error();
 
 					math::Vector<2> curr_wp((float)_pos_sp_triplet.current.lat, (float)_pos_sp_triplet.current.lon);
-					_fw_pos_ctrl_status.wp_dist = get_distance_to_next_waypoint(current_position(0), current_position(1), curr_wp(0), curr_wp(1));
+					_fw_pos_ctrl_status.wp_dist = get_distance_to_next_waypoint(current_position(0), current_position(1), curr_wp(0),
+								      curr_wp(1));
 
 					fw_pos_ctrl_status_publish();
 				}
@@ -2237,19 +2255,21 @@ FixedwingPositionControl::task_main()
 
 void FixedwingPositionControl::reset_takeoff_state()
 {
-	launch_detection_state = LAUNCHDETECTION_RES_NONE;
-	launchDetector.reset();
+	_launch_detection_state = LAUNCHDETECTION_RES_NONE;
+	_launchDetector.reset();
 	_runway_takeoff.reset();
 }
 
 void FixedwingPositionControl::reset_landing_state()
 {
-	land_noreturn_horizontal = false;
-	land_noreturn_vertical = false;
-	land_stayonground = false;
-	land_motor_lim = false;
-	land_onslope = false;
-	land_useterrain = false;
+	_time_started_landing = 0;
+
+	_land_noreturn_horizontal = false;
+	_land_noreturn_vertical = false;
+	_land_stayonground = false;
+	_land_motor_lim = false;
+	_land_onslope = false;
+	_land_useterrain = false;
 }
 
 void FixedwingPositionControl::tecs_update_pitch_throttle(float alt_sp, float v_sp, float eas2tas,
diff --git a/src/modules/fw_pos_control_l1/landingslope.cpp b/src/modules/fw_pos_control_l1/landingslope.cpp
index 02f6ab47f7..56d7cfb434 100644
--- a/src/modules/fw_pos_control_l1/landingslope.cpp
+++ b/src/modules/fw_pos_control_l1/landingslope.cpp
@@ -46,6 +46,19 @@
 #include <unistd.h>
 #include <mathlib/mathlib.h>
 
+Landingslope::Landingslope() :
+	_landing_slope_angle_rad(0.0f),
+	_flare_relative_alt(0.0f),
+	_motor_lim_relative_alt(0.0f),
+	_H1_virt(0.0f),
+	_H0(0.0f),
+	_d1(0.0f),
+	_flare_constant(0.0f),
+	_flare_length(0.0f),
+	_horizontal_slope_displacement(0.0f)
+{
+}
+
 void Landingslope::update(float landing_slope_angle_rad_new,
 			  float flare_relative_alt_new,
 			  float motor_lim_relative_alt_new,
@@ -69,7 +82,7 @@ void Landingslope::calculateSlopeValues()
 	_horizontal_slope_displacement = (_flare_length - _d1);
 }
 
-float  Landingslope::getLandingSlopeRelativeAltitude(float wp_landing_distance)
+float Landingslope::getLandingSlopeRelativeAltitude(float wp_landing_distance)
 {
 	return Landingslope::getLandingSlopeRelativeAltitude(wp_landing_distance, _horizontal_slope_displacement,
 			_landing_slope_angle_rad);
diff --git a/src/modules/fw_pos_control_l1/landingslope.h b/src/modules/fw_pos_control_l1/landingslope.h
index f7b7d7d960..b801b40f8b 100644
--- a/src/modules/fw_pos_control_l1/landingslope.h
+++ b/src/modules/fw_pos_control_l1/landingslope.h
@@ -60,7 +60,7 @@ private:
 	void calculateSlopeValues();
 
 public:
-	Landingslope() {}
+	Landingslope();
 	~Landingslope() {}
 
 
diff --git a/src/modules/fw_pos_control_l1/mtecs/limitoverride.cpp b/src/modules/fw_pos_control_l1/mtecs/limitoverride.cpp
index 58795edb65..66dbf81f70 100644
--- a/src/modules/fw_pos_control_l1/mtecs/limitoverride.cpp
+++ b/src/modules/fw_pos_control_l1/mtecs/limitoverride.cpp
@@ -41,10 +41,11 @@
 
 #include "limitoverride.h"
 
-namespace fwPosctrl {
+namespace fwPosctrl
+{
 
 bool LimitOverride::applyOverride(BlockOutputLimiter &outputLimiterThrottle,
-		BlockOutputLimiter &outputLimiterPitch)
+				  BlockOutputLimiter &outputLimiterPitch)
 {
 	bool ret = false;
 
@@ -52,14 +53,17 @@ bool LimitOverride::applyOverride(BlockOutputLimiter &outputLimiterThrottle,
 		outputLimiterThrottle.setMin(overrideThrottleMin);
 		ret = true;
 	}
+
 	if (overrideThrottleMaxEnabled)	{
 		outputLimiterThrottle.setMax(overrideThrottleMax);
 		ret = true;
 	}
+
 	if (overridePitchMinEnabled)	{
 		outputLimiterPitch.setMin(overridePitchMin);
 		ret = true;
 	}
+
 	if (overridePitchMaxEnabled)	{
 		outputLimiterPitch.setMax(overridePitchMax);
 		ret = true;
diff --git a/src/modules/fw_pos_control_l1/mtecs/limitoverride.h b/src/modules/fw_pos_control_l1/mtecs/limitoverride.h
index 64c2e7bbde..5b2ea9c53a 100644
--- a/src/modules/fw_pos_control_l1/mtecs/limitoverride.h
+++ b/src/modules/fw_pos_control_l1/mtecs/limitoverride.h
@@ -69,20 +69,32 @@ public:
 	* @return true if the limit was applied
 	*/
 	bool applyOverride(BlockOutputLimiter &outputLimiterThrottle,
-			BlockOutputLimiter &outputLimiterPitch);
+			   BlockOutputLimiter &outputLimiterPitch);
 
 	/* Functions to enable or disable the override */
-	void enableThrottleMinOverride(float value) { enable(&overrideThrottleMinEnabled,
-			&overrideThrottleMin, value); }
+	void enableThrottleMinOverride(float value)
+	{
+		enable(&overrideThrottleMinEnabled,
+		       &overrideThrottleMin, value);
+	}
 	void disableThrottleMinOverride() { disable(&overrideThrottleMinEnabled); }
-	void enableThrottleMaxOverride(float value) { enable(&overrideThrottleMaxEnabled,
-			&overrideThrottleMax, value); }
+	void enableThrottleMaxOverride(float value)
+	{
+		enable(&overrideThrottleMaxEnabled,
+		       &overrideThrottleMax, value);
+	}
 	void disableThrottleMaxOverride() { disable(&overrideThrottleMaxEnabled); }
-	void enablePitchMinOverride(float value) { enable(&overridePitchMinEnabled,
-			&overridePitchMin, value); }
+	void enablePitchMinOverride(float value)
+	{
+		enable(&overridePitchMinEnabled,
+		       &overridePitchMin, value);
+	}
 	void disablePitchMinOverride() { disable(&overridePitchMinEnabled); }
-	void enablePitchMaxOverride(float value) { enable(&overridePitchMaxEnabled,
-			&overridePitchMax, value); }
+	void enablePitchMaxOverride(float value)
+	{
+		enable(&overridePitchMaxEnabled,
+		       &overridePitchMax, value);
+	}
 	void disablePitchMaxOverride() { disable(&overridePitchMaxEnabled); }
 
 protected:
diff --git a/src/modules/fw_pos_control_l1/mtecs/mTecs.cpp b/src/modules/fw_pos_control_l1/mtecs/mTecs.cpp
index 7a045fb1bb..333d8a5b8f 100644
--- a/src/modules/fw_pos_control_l1/mtecs/mTecs.cpp
+++ b/src/modules/fw_pos_control_l1/mtecs/mTecs.cpp
@@ -44,7 +44,8 @@
 #include <lib/geo/geo.h>
 #include <stdio.h>
 
-namespace fwPosctrl {
+namespace fwPosctrl
+{
 
 mTecs::mTecs() :
 	SuperBlock(NULL, "MT"),
@@ -85,11 +86,11 @@ mTecs::~mTecs()
 }
 
 int mTecs::updateAltitudeSpeed(float flightPathAngle, float altitude, float altitudeSp, float airspeed,
-		float airspeedSp, unsigned mode, LimitOverride limitOverride)
+			       float airspeedSp, unsigned mode, LimitOverride limitOverride)
 {
 	/* check if all input arguments are numbers and abort if not so */
 	if (!PX4_ISFINITE(flightPathAngle) || !PX4_ISFINITE(altitude) ||
-			!PX4_ISFINITE(altitudeSp) || !PX4_ISFINITE(airspeed) || !PX4_ISFINITE(airspeedSp) || !PX4_ISFINITE(mode)) {
+	    !PX4_ISFINITE(altitudeSp) || !PX4_ISFINITE(airspeed) || !PX4_ISFINITE(airspeedSp) || !PX4_ISFINITE(mode)) {
 		return -1;
 	}
 
@@ -106,7 +107,8 @@ int mTecs::updateAltitudeSpeed(float flightPathAngle, float altitude, float alti
 	/* Debug output */
 	if (_counter % 10 == 0) {
 		debug("***");
-		debug("updateAltitudeSpeed: altitudeSp %.4f, altitude %.4f, altitude filtered %.4f, flightPathAngleSp %.4f", (double)altitudeSp, (double)altitude, (double)altitudeFiltered, (double)flightPathAngleSp);
+		debug("updateAltitudeSpeed: altitudeSp %.4f, altitude %.4f, altitude filtered %.4f, flightPathAngleSp %.4f",
+		      (double)altitudeSp, (double)altitude, (double)altitudeFiltered, (double)flightPathAngleSp);
 	}
 
 #if defined(__PX4_NUTTX)
@@ -118,15 +120,15 @@ int mTecs::updateAltitudeSpeed(float flightPathAngle, float altitude, float alti
 
 	/* use flightpath angle setpoint for total energy control */
 	return updateFlightPathAngleSpeed(flightPathAngle, flightPathAngleSp, airspeed,
-			airspeedSp, mode, limitOverride);
+					  airspeedSp, mode, limitOverride);
 }
 
 int mTecs::updateFlightPathAngleSpeed(float flightPathAngle, float flightPathAngleSp, float airspeed,
-		float airspeedSp, unsigned mode, LimitOverride limitOverride)
+				      float airspeedSp, unsigned mode, LimitOverride limitOverride)
 {
 	/* check if all input arguments are numbers and abort if not so */
 	if (!PX4_ISFINITE(flightPathAngle) || !PX4_ISFINITE(flightPathAngleSp) ||
-			!PX4_ISFINITE(airspeed) || !PX4_ISFINITE(airspeedSp) || !PX4_ISFINITE(mode)) {
+	    !PX4_ISFINITE(airspeed) || !PX4_ISFINITE(airspeedSp) || !PX4_ISFINITE(mode)) {
 		return -1;
 	}
 
@@ -142,9 +144,9 @@ int mTecs::updateFlightPathAngleSpeed(float flightPathAngle, float flightPathAng
 	/* Debug output */
 	if (_counter % 10 == 0) {
 		debug("updateFlightPathAngleSpeed airspeedSp %.4f, airspeed %.4f airspeedFiltered %.4f,"
-				"accelerationLongitudinalSp%.4f",
-				(double)airspeedSp, (double)airspeed,
-				(double)airspeedFiltered, (double)accelerationLongitudinalSp);
+		      "accelerationLongitudinalSp%.4f",
+		      (double)airspeedSp, (double)airspeed,
+		      (double)airspeedFiltered, (double)accelerationLongitudinalSp);
 	}
 
 #if defined(__PX4_NUTTX)
@@ -164,9 +166,10 @@ int mTecs::updateFlightPathAngleAcceleration(float flightPathAngle, float flight
 {
 	/* check if all input arguments are numbers and abort if not so */
 	if (!PX4_ISFINITE(flightPathAngle) || !PX4_ISFINITE(flightPathAngleSp) ||
-			!PX4_ISFINITE(airspeedFiltered) || !PX4_ISFINITE(accelerationLongitudinalSp) || !PX4_ISFINITE(mode)) {
+	    !PX4_ISFINITE(airspeedFiltered) || !PX4_ISFINITE(accelerationLongitudinalSp) || !PX4_ISFINITE(mode)) {
 		return -1;
 	}
+
 	/* time measurement */
 	updateTimeMeasurement();
 
@@ -179,9 +182,11 @@ int mTecs::updateFlightPathAngleAcceleration(float flightPathAngle, float flight
 	/* calculate values (energies) */
 	float flightPathAngleError = flightPathAngleSp - flightPathAngleFiltered;
 	float airspeedDerivative = 0.0f;
-	if(_airspeedDerivative.getDt() > 0.0f) {
+
+	if (_airspeedDerivative.getDt() > 0.0f) {
 		airspeedDerivative = _airspeedDerivative.update(airspeedFiltered);
 	}
+
 	float airspeedDerivativeNorm = airspeedDerivative / CONSTANTS_ONE_G;
 	float airspeedDerivativeSp = accelerationLongitudinalSp;
 	float airspeedDerivativeNormSp = airspeedDerivativeSp / CONSTANTS_ONE_G;
@@ -200,9 +205,11 @@ int mTecs::updateFlightPathAngleAcceleration(float flightPathAngle, float flight
 	/* Debug output */
 	if (_counter % 10 == 0) {
 		debug("totalEnergyRateSp %.2f, totalEnergyRate %.2f, totalEnergyRateError %.2f totalEnergyRateError2 %.2f airspeedDerivativeNorm %.4f",
-				(double)totalEnergyRateSp, (double)totalEnergyRate, (double)totalEnergyRateError, (double)totalEnergyRateError2, (double)airspeedDerivativeNorm);
+		      (double)totalEnergyRateSp, (double)totalEnergyRate, (double)totalEnergyRateError, (double)totalEnergyRateError2,
+		      (double)airspeedDerivativeNorm);
 		debug("energyDistributionRateSp %.2f, energyDistributionRate %.2f, energyDistributionRateError %.2f energyDistributionRateError2 %.2f",
-				(double)energyDistributionRateSp, (double)energyDistributionRate, (double)energyDistributionRateError, (double)energyDistributionRateError2);
+		      (double)energyDistributionRateSp, (double)energyDistributionRate, (double)energyDistributionRateError,
+		      (double)energyDistributionRateError2);
 	}
 
 	/* Check airspeed: if below safe value switch to underspeed mode (if not in land or takeoff mode) */
@@ -213,15 +220,19 @@ int mTecs::updateFlightPathAngleAcceleration(float flightPathAngle, float flight
 	/* Set special output limiters if we are not in MTECS_MODE_NORMAL */
 	BlockOutputLimiter *outputLimiterThrottle = &_controlTotalEnergy.getOutputLimiter();
 	BlockOutputLimiter *outputLimiterPitch = &_controlEnergyDistribution.getOutputLimiter();
+
 	if (mode == MTECS_MODE_TAKEOFF) {
 		outputLimiterThrottle = &_BlockOutputLimiterTakeoffThrottle;
 		outputLimiterPitch = &_BlockOutputLimiterTakeoffPitch;
+
 	} else if (mode == MTECS_MODE_LAND) {
 		// only limit pitch but do not limit throttle
 		outputLimiterPitch = &_BlockOutputLimiterLandPitch;
+
 	} else if (mode == MTECS_MODE_LAND_THROTTLELIM) {
 		outputLimiterThrottle = &_BlockOutputLimiterLandThrottle;
 		outputLimiterPitch = &_BlockOutputLimiterLandPitch;
+
 	} else if (mode == MTECS_MODE_UNDERSPEED) {
 		outputLimiterThrottle = &_BlockOutputLimiterUnderspeedThrottle;
 		outputLimiterPitch = &_BlockOutputLimiterUnderspeedPitch;
@@ -256,9 +267,9 @@ int mTecs::updateFlightPathAngleAcceleration(float flightPathAngle, float flight
 
 	if (_counter % 10 == 0) {
 		debug("_throttleSp %.1f, _pitchSp %.1f, flightPathAngleSp %.1f, flightPathAngle %.1f accelerationLongitudinalSp %.1f, airspeedDerivative %.1f",
-				(double)_throttleSp, (double)_pitchSp,
-				(double)flightPathAngleSp, (double)flightPathAngle,
-				(double)accelerationLongitudinalSp, (double)airspeedDerivative);
+		      (double)_throttleSp, (double)_pitchSp,
+		      (double)flightPathAngleSp, (double)flightPathAngle,
+		      (double)accelerationLongitudinalSp, (double)airspeedDerivative);
 	}
 
 #if defined(__PX4_NUTTX)
@@ -293,11 +304,13 @@ void mTecs::updateTimeMeasurement()
 {
 	if (!_dtCalculated) {
 		float deltaTSeconds = 0.0f;
+
 		if (!_firstIterationAfterReset) {
 			hrt_abstime timestampNow = hrt_absolute_time();
 			deltaTSeconds = (float)(timestampNow - timestampLastIteration) * 1e-6f;
 			timestampLastIteration = timestampNow;
 		}
+
 		setDt(deltaTSeconds);
 
 		_dtCalculated = true;
@@ -312,7 +325,8 @@ void mTecs::debug_print(const char *fmt, va_list args)
 	fprintf(stderr, "\n");
 }
 
-void mTecs::debug(const char *fmt, ...) {
+void mTecs::debug(const char *fmt, ...)
+{
 
 	if (!_debug) {
 		return;
diff --git a/src/modules/fw_pos_control_l1/mtecs/mTecs_blocks.h b/src/modules/fw_pos_control_l1/mtecs/mTecs_blocks.h
index bb20654728..1363d121f7 100644
--- a/src/modules/fw_pos_control_l1/mtecs/mTecs_blocks.h
+++ b/src/modules/fw_pos_control_l1/mtecs/mTecs_blocks.h
@@ -68,21 +68,25 @@ public:
 	 *
 	 * @param value is changed to be on the interval _min to _max
 	 * @param difference if the value is changed this corresponds to the change of value * (-1)
-     * otherwise unchanged
+	* otherwise unchanged
 	 * @return: true if the limit is applied, false otherwise
 	 */
-	bool limit(float& value, float& difference) {
+	bool limit(float &value, float &difference)
+	{
 		float minimum = getIsAngularLimit() ? getMin() * M_DEG_TO_RAD_F : getMin();
 		float maximum = getIsAngularLimit() ? getMax() * M_DEG_TO_RAD_F : getMax();
+
 		if (value < minimum) {
 			difference = value - minimum;
 			value = minimum;
 			return true;
+
 		} else if (value > maximum) {
 			difference = value - maximum;
 			value = maximum;
 			return true;
 		}
+
 		return false;
 	}
 //accessor:
@@ -126,15 +130,18 @@ protected:
 	BlockOutputLimiter _outputLimiter;
 
 	float calcUnlimitedOutput(float inputValue, float inputError) {return getOffset() + getKFF() * inputValue + getKP() * inputError + getKI() * getIntegral().update(inputError);}
-	float calcLimitedOutput(float inputValue, float inputError, BlockOutputLimiter &outputLimiter) {
+	float calcLimitedOutput(float inputValue, float inputError, BlockOutputLimiter &outputLimiter)
+	{
 		float difference = 0.0f;
 		float integralYPrevious = _integral.getY();
 		float output = calcUnlimitedOutput(inputValue, inputError);
-		if(outputLimiter.limit(output, difference) &&
-			(((difference < 0) && (getKI() * getIntegral().getY() < 0)) ||
-			((difference > 0) && (getKI() * getIntegral().getY() > 0)))) {
-				getIntegral().setY(integralYPrevious);
+
+		if (outputLimiter.limit(output, difference) &&
+		    (((difference < 0) && (getKI() * getIntegral().getY() < 0)) ||
+		     ((difference > 0) && (getKI() * getIntegral().getY() > 0)))) {
+			getIntegral().setY(integralYPrevious);
 		}
+
 		return output;
 	}
 private:
@@ -152,9 +159,10 @@ public:
 // methods
 	BlockFFPILimitedCustom(SuperBlock *parent, const char *name, bool isAngularLimit = false) :
 		BlockFFPILimited(parent, name, isAngularLimit)
-		{};
+	{};
 	virtual ~BlockFFPILimitedCustom() {};
-	float update(float inputValue, float inputError, BlockOutputLimiter *outputLimiter = NULL) {
+	float update(float inputValue, float inputError, BlockOutputLimiter *outputLimiter = NULL)
+	{
 		return calcLimitedOutput(inputValue, inputError, outputLimiter == NULL ? _outputLimiter : *outputLimiter);
 	}
 };
@@ -170,7 +178,8 @@ public:
 		_outputLimiter(this, "", isAngularLimit)
 	{};
 	virtual ~BlockPLimited() {};
-	float update(float input) {
+	float update(float input)
+	{
 		float difference = 0.0f;
 		float output = getKP() * input;
 		getOutputLimiter().limit(output, difference);
@@ -197,7 +206,8 @@ public:
 		_outputLimiter(this, "", isAngularLimit)
 	{};
 	virtual ~BlockPDLimited() {};
-	float update(float input) {
+	float update(float input)
+	{
 		float difference = 0.0f;
 		float output = getKP() * input + (getDerivative().getDt() > 0.0f ? getKD() * getDerivative().update(input) : 0.0f);
 		getOutputLimiter().limit(output, difference);
diff --git a/src/modules/vtol_att_control/vtol_type.h b/src/modules/vtol_att_control/vtol_type.h
index d755fb43e4..4570e39d8e 100644
--- a/src/modules/vtol_att_control/vtol_type.h
+++ b/src/modules/vtol_att_control/vtol_type.h
@@ -82,6 +82,8 @@ class VtolType
 public:
 
 	VtolType(VtolAttitudeControl *att_controller);
+	VtolType(const VtolType &) = delete;
+	VtolType &operator=(const VtolType &) = delete;
 
 	virtual ~VtolType();
 

From 7718343b27653e00ba2f59f89ff871da81f81db1 Mon Sep 17 00:00:00 2001
From: xiaoyuli <lixiaoyuzaici@163.com>
Date: Wed, 13 Jul 2016 05:43:17 +0800
Subject: [PATCH 0998/1230] fix the function of disabling safety switch (#5031)

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

diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp
index 7937bcfd40..03ea3601a1 100644
--- a/src/drivers/px4io/px4io.cpp
+++ b/src/drivers/px4io/px4io.cpp
@@ -1126,7 +1126,7 @@ PX4IO::task_main()
 				}
 
 				int32_t safety_param_val;
-				param_t safety_param = param_find("RC_FAILS_THR");
+				param_t safety_param = param_find("CBRK_IO_SAFETY");
 
 				if (safety_param != PARAM_INVALID) {
 

From 784883af2249dabbea526b1d0f6c2dc5bacef86f Mon Sep 17 00:00:00 2001
From: tumbili <roman@px4.io>
Date: Thu, 9 Jun 2016 17:23:02 +0200
Subject: [PATCH 0999/1230] added parameters to allow scaling controls in full
 manual mode for fixed wings

---
 .../fw_att_control/fw_att_control_params.c    | 43 +++++++++++++++++++
 1 file changed, 43 insertions(+)

diff --git a/src/modules/fw_att_control/fw_att_control_params.c b/src/modules/fw_att_control/fw_att_control_params.c
index 80fe1c6228..ebc6547c96 100644
--- a/src/modules/fw_att_control/fw_att_control_params.c
+++ b/src/modules/fw_att_control/fw_att_control_params.c
@@ -518,3 +518,46 @@ PARAM_DEFINE_FLOAT(FW_FLAPERON_SCL, 0.0f);
  * @group FW Attitude Control
  */
 PARAM_DEFINE_INT32(FW_ARSP_MODE, 0);
+
+/**
+ * Manual roll scale
+ *
+ * Scale factor applied to the desired roll actuator command in full manual mode. This parameter allows
+ * to adjust the throws of the control surfaces.
+ *
+ * @unit norm
+ * @min 0.0
+ * @max 1.0
+ * @decimal 2
+ * @increment 0.01
+ * @group FW Attitude Control
+ */
+PARAM_DEFINE_FLOAT(FW_MAN_R_SC, 1.0f);
+
+/**
+ * Manual pitch scale
+ *
+ * Scale factor applied to the desired pitch actuator command in full manual mode. This parameter allows
+ * to adjust the throws of the control surfaces.
+ *
+ * @unit norm
+ * @min 0.0
+ * @decimal 2
+ * @increment 0.01
+ * @group FW Attitude Control
+ */
+PARAM_DEFINE_FLOAT(FW_MAN_P_SC, 1.0f);
+
+/**
+ * Manual yaw scale
+ *
+ * Scale factor applied to the desired yaw actuator command in full manual mode. This parameter allows
+ * to adjust the throws of the control surfaces.
+ *
+ * @unit norm
+ * @min 0.0
+ * @decimal 2
+ * @increment 0.01
+ * @group FW Attitude Control
+ */
+PARAM_DEFINE_FLOAT(FW_MAN_Y_SC, 1.0f);

From f0dd5a103c2841de09932a07577786da5d735e84 Mon Sep 17 00:00:00 2001
From: tumbili <roman@px4.io>
Date: Thu, 9 Jun 2016 17:23:54 +0200
Subject: [PATCH 1000/1230] allow scaling controls in full manual mode for
 fixed wings

---
 .../fw_att_control/fw_att_control_main.cpp    | 19 ++++++++++++++++---
 1 file changed, 16 insertions(+), 3 deletions(-)

diff --git a/src/modules/fw_att_control/fw_att_control_main.cpp b/src/modules/fw_att_control/fw_att_control_main.cpp
index b0f3adf506..06b126895a 100644
--- a/src/modules/fw_att_control/fw_att_control_main.cpp
+++ b/src/modules/fw_att_control/fw_att_control_main.cpp
@@ -209,6 +209,9 @@ private:
 		float pitchsp_offset_rad;		/**< Pitch Setpoint Offset in rad */
 		float man_roll_max;				/**< Max Roll in rad */
 		float man_pitch_max;			/**< Max Pitch in rad */
+		float man_roll_scale;			/**< scale factor applied to roll actuator control in pure manual mode */
+		float man_pitch_scale;			/**< scale factor applied to pitch actuator control in pure manual mode */
+		float man_yaw_scale; 			/**< scale factor applied to yaw actuator control in pure manual mode */
 
 		float flaps_scale;				/**< Scale factor for flaps */
 		float flaperon_scale;			/**< Scale factor for flaperons */
@@ -257,6 +260,9 @@ private:
 		param_t pitchsp_offset_deg;
 		param_t man_roll_max;
 		param_t man_pitch_max;
+		param_t man_roll_scale;
+		param_t man_pitch_scale;
+		param_t man_yaw_scale;
 
 		param_t flaps_scale;
 		param_t flaperon_scale;
@@ -444,6 +450,9 @@ FixedwingAttitudeControl::FixedwingAttitudeControl() :
 
 	_parameter_handles.man_roll_max = param_find("FW_MAN_R_MAX");
 	_parameter_handles.man_pitch_max = param_find("FW_MAN_P_MAX");
+	_parameter_handles.man_roll_scale = param_find("FW_MAN_R_SC");
+	_parameter_handles.man_pitch_scale = param_find("FW_MAN_P_SC");
+	_parameter_handles.man_yaw_scale = param_find("FW_MAN_Y_SC");
 
 	_parameter_handles.flaps_scale = param_find("FW_FLAPS_SCL");
 	_parameter_handles.flaperon_scale = param_find("FW_FLAPERON_SCL");
@@ -532,6 +541,9 @@ FixedwingAttitudeControl::parameters_update()
 	param_get(_parameter_handles.man_pitch_max, &(_parameters.man_pitch_max));
 	_parameters.man_roll_max = math::radians(_parameters.man_roll_max);
 	_parameters.man_pitch_max = math::radians(_parameters.man_pitch_max);
+	param_get(_parameter_handles.man_roll_scale, &(_parameters.man_roll_scale));
+	param_get(_parameter_handles.man_pitch_scale, &(_parameters.man_pitch_scale));
+	param_get(_parameter_handles.man_yaw_scale, &(_parameters.man_yaw_scale));
 
 	param_get(_parameter_handles.flaps_scale, &_parameters.flaps_scale);
 	param_get(_parameter_handles.flaperon_scale, &_parameters.flaperon_scale);
@@ -1243,9 +1255,10 @@ FixedwingAttitudeControl::task_main()
 
 			} else {
 				/* manual/direct control */
-				_actuators.control[actuator_controls_s::INDEX_ROLL] = _manual.y + _parameters.trim_roll;
-				_actuators.control[actuator_controls_s::INDEX_PITCH] = -_manual.x + _parameters.trim_pitch;
-				_actuators.control[actuator_controls_s::INDEX_YAW] = _manual.r + _parameters.trim_yaw;
+				_actuators.control[actuator_controls_s::INDEX_ROLL] = _manual.y * _parameters.man_roll_scale + _parameters.trim_roll;
+				_actuators.control[actuator_controls_s::INDEX_PITCH] = -_manual.x * _parameters.man_pitch_scale +
+						_parameters.trim_pitch;
+				_actuators.control[actuator_controls_s::INDEX_YAW] = _manual.r * _parameters.man_yaw_scale + _parameters.trim_yaw;
 				_actuators.control[actuator_controls_s::INDEX_THROTTLE] = _manual.z;
 			}
 

From 71e2a43790f1baca06e9fa2de0956bd37115d3d0 Mon Sep 17 00:00:00 2001
From: tumbili <roman@px4.io>
Date: Thu, 9 Jun 2016 17:26:59 +0200
Subject: [PATCH 1001/1230] consider scale parameters in rc calibration code

---
 src/modules/commander/rc_calibration.cpp | 14 +++++++++++---
 1 file changed, 11 insertions(+), 3 deletions(-)

diff --git a/src/modules/commander/rc_calibration.cpp b/src/modules/commander/rc_calibration.cpp
index 5423c03a82..fa99b4da27 100644
--- a/src/modules/commander/rc_calibration.cpp
+++ b/src/modules/commander/rc_calibration.cpp
@@ -79,18 +79,26 @@ int do_trim_calibration(orb_advert_t *mavlink_log_pub)
 	float yaw_trim_active;
 	param_get(param_find("TRIM_YAW"), &yaw_trim_active);
 
+	/* get manual control scale values */
+	float roll_scale;
+	param_get(param_find("FW_MAN_R_SC"), &roll_scale);
+	float pitch_scale;
+	param_get(param_find("FW_MAN_P_SC"), &pitch_scale);
+	float yaw_scale;
+	param_get(param_find("FW_MAN_Y_SC"), &yaw_scale);
+
 	/* set parameters: the new trim values are the combination of active trim values
 	   and the values coming from the remote control of the user
 	*/
-	float p = sp.y + roll_trim_active;
+	float p = sp.y * roll_scale + roll_trim_active;
 	int p1r = param_set(param_find("TRIM_ROLL"), &p);
 	/*
 	 we explicitly swap sign here because the trim is added to the actuator controls
 	 which are moving in an inverse sense to manual pitch inputs
 	*/
-	p = -sp.x + pitch_trim_active;
+	p = -sp.x * pitch_scale + pitch_trim_active;
 	int p2r = param_set(param_find("TRIM_PITCH"), &p);
-	p = sp.r + yaw_trim_active;
+	p = sp.r * yaw_scale + yaw_trim_active;
 	int p3r = param_set(param_find("TRIM_YAW"), &p);
 
 	/* store to permanent storage */

From 08bbd6dbfaff58524928437a56fd7976d69fa2f5 Mon Sep 17 00:00:00 2001
From: Roman <bapstr@ethz.ch>
Date: Tue, 12 Jul 2016 13:48:57 +0200
Subject: [PATCH 1002/1230] px4iofirmware: added scale parameters for roll,
 pitch and yaw

since pure manual control for fixed wings in handled on the io side
the scale parameters for roll, pitch and yaw had to be introduced there
as well.

Signed-off-by: Roman <bapstr@ethz.ch>
---
 src/modules/px4iofirmware/mixer.cpp   | 6 +++---
 src/modules/px4iofirmware/protocol.h  | 5 ++++-
 src/modules/px4iofirmware/px4io.h     | 3 +++
 src/modules/px4iofirmware/registers.c | 8 +++++++-
 4 files changed, 17 insertions(+), 5 deletions(-)

diff --git a/src/modules/px4iofirmware/mixer.cpp b/src/modules/px4iofirmware/mixer.cpp
index 0ff0eb4892..f41a80ef66 100644
--- a/src/modules/px4iofirmware/mixer.cpp
+++ b/src/modules/px4iofirmware/mixer.cpp
@@ -373,15 +373,15 @@ mixer_callback(uintptr_t handle,
 	if (source == MIX_OVERRIDE || source == MIX_OVERRIDE_FMU_OK) {
 		if (control_group == actuator_controls_s::GROUP_INDEX_ATTITUDE &&
 		    control_index == actuator_controls_s::INDEX_ROLL) {
-			control += REG_TO_FLOAT(r_setup_trim_roll);
+			control *= REG_TO_FLOAT(r_setup_scale_roll) + REG_TO_FLOAT(r_setup_trim_roll);
 
 		} else if (control_group == actuator_controls_s::GROUP_INDEX_ATTITUDE &&
 			   control_index == actuator_controls_s::INDEX_PITCH) {
-			control += REG_TO_FLOAT(r_setup_trim_pitch);
+			control *= REG_TO_FLOAT(r_setup_scale_pitch) + REG_TO_FLOAT(r_setup_trim_pitch);
 
 		} else if (control_group == actuator_controls_s::GROUP_INDEX_ATTITUDE &&
 			   control_index == actuator_controls_s::INDEX_YAW) {
-			control += REG_TO_FLOAT(r_setup_trim_yaw);
+			control *= REG_TO_FLOAT(r_setup_scale_yaw) + REG_TO_FLOAT(r_setup_trim_yaw);
 		}
 	}
 
diff --git a/src/modules/px4iofirmware/protocol.h b/src/modules/px4iofirmware/protocol.h
index 6c984ccac5..a4c9ce235a 100644
--- a/src/modules/px4iofirmware/protocol.h
+++ b/src/modules/px4iofirmware/protocol.h
@@ -236,8 +236,11 @@ enum {							/* DSM bind states */
 #define PX4IO_P_SETUP_TRIM_ROLL			16	/**< Roll trim, in actuator units */
 #define PX4IO_P_SETUP_TRIM_PITCH		17	/**< Pitch trim, in actuator units */
 #define PX4IO_P_SETUP_TRIM_YAW			18	/**< Yaw trim, in actuator units */
+#define PX4IO_P_SETUP_SCALE_ROLL		19	/**< Roll scale, in actuator units */
+#define PX4IO_P_SETUP_SCALE_PITCH		20	/**< Pitch scale, in actuator units */
+#define PX4IO_P_SETUP_SCALE_YAW			21	/**< Yaw scale, in actuator units */
 
-#define PX4IO_P_SETUP_SBUS_RATE			19	/* frame rate of SBUS1 output in Hz */
+#define PX4IO_P_SETUP_SBUS_RATE			22	/* frame rate of SBUS1 output in Hz */
 
 /* autopilot control values, -10000..10000 */
 #define PX4IO_PAGE_CONTROLS			51	/**< actuator control groups, one after the other, 8 wide */
diff --git a/src/modules/px4iofirmware/px4io.h b/src/modules/px4iofirmware/px4io.h
index ed38b7769e..89dae97710 100644
--- a/src/modules/px4iofirmware/px4io.h
+++ b/src/modules/px4iofirmware/px4io.h
@@ -122,6 +122,9 @@ extern uint16_t			r_page_servo_disarmed[];	/* PX4IO_PAGE_DISARMED_PWM */
 #define r_setup_trim_roll	r_page_setup[PX4IO_P_SETUP_TRIM_ROLL]
 #define r_setup_trim_pitch	r_page_setup[PX4IO_P_SETUP_TRIM_PITCH]
 #define r_setup_trim_yaw	r_page_setup[PX4IO_P_SETUP_TRIM_YAW]
+#define r_setup_scale_roll 	r_page_setup[PX4IO_P_SETUP_SCALE_ROLL]
+#define r_setup_scale_pitch	r_page_setup[PX4IO_P_SETUP_SCALE_PITCH]
+#define r_setup_scale_yaw	r_page_setup[PX4IO_P_SETUP_SCALE_YAW]
 #define r_setup_sbus_rate	r_page_setup[PX4IO_P_SETUP_SBUS_RATE]
 
 #define r_control_values	(&r_page_controls[0])
diff --git a/src/modules/px4iofirmware/registers.c b/src/modules/px4iofirmware/registers.c
index 3a7d90aecf..901dc061da 100644
--- a/src/modules/px4iofirmware/registers.c
+++ b/src/modules/px4iofirmware/registers.c
@@ -177,7 +177,10 @@ volatile uint16_t	r_page_setup[] = {
 	[PX4IO_P_SETUP_PWM_REVERSE] = 0,
 	[PX4IO_P_SETUP_TRIM_ROLL] = 0,
 	[PX4IO_P_SETUP_TRIM_PITCH] = 0,
-	[PX4IO_P_SETUP_TRIM_YAW] = 0
+	[PX4IO_P_SETUP_TRIM_YAW] = 0,
+	[PX4IO_P_SETUP_SCALE_ROLL] = 10000,
+	[PX4IO_P_SETUP_SCALE_PITCH] = 10000,
+	[PX4IO_P_SETUP_SCALE_YAW] = 10000
 };
 
 #ifdef CONFIG_ARCH_BOARD_PX4IO_V2
@@ -681,6 +684,9 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value)
 		case PX4IO_P_SETUP_TRIM_ROLL:
 		case PX4IO_P_SETUP_TRIM_PITCH:
 		case PX4IO_P_SETUP_TRIM_YAW:
+		case PX4IO_P_SETUP_SCALE_ROLL:
+		case PX4IO_P_SETUP_SCALE_PITCH:
+		case PX4IO_P_SETUP_SCALE_YAW:
 			r_page_setup[offset] = value;
 			break;
 

From 13905c2480c63ab3b42d96e5b55606514221c817 Mon Sep 17 00:00:00 2001
From: Roman <bapstr@ethz.ch>
Date: Tue, 12 Jul 2016 13:49:32 +0200
Subject: [PATCH 1003/1230] px4io driver: send roll, pitch and yaw scale
 parameter values to io

Signed-off-by: Roman <bapstr@ethz.ch>
---
 src/drivers/px4io/px4io.cpp | 35 ++++++++++++++++++++++++++++-------
 1 file changed, 28 insertions(+), 7 deletions(-)

diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp
index 03ea3601a1..e97b8db898 100644
--- a/src/drivers/px4io/px4io.cpp
+++ b/src/drivers/px4io/px4io.cpp
@@ -1166,28 +1166,49 @@ PX4IO::task_main()
 
 				(void)io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_PWM_REVERSE, pwm_invert_mask);
 
-				float trim_val;
+				float param_val;
 				param_t parm_handle;
 
 				parm_handle = param_find("TRIM_ROLL");
 
 				if (parm_handle != PARAM_INVALID) {
-					param_get(parm_handle, &trim_val);
-					(void)io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_TRIM_ROLL, FLOAT_TO_REG(trim_val));
+					param_get(parm_handle, &param_val);
+					(void)io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_TRIM_ROLL, FLOAT_TO_REG(param_val));
 				}
 
 				parm_handle = param_find("TRIM_PITCH");
 
 				if (parm_handle != PARAM_INVALID) {
-					param_get(parm_handle, &trim_val);
-					(void)io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_TRIM_PITCH, FLOAT_TO_REG(trim_val));
+					param_get(parm_handle, &param_val);
+					(void)io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_TRIM_PITCH, FLOAT_TO_REG(param_val));
 				}
 
 				parm_handle = param_find("TRIM_YAW");
 
 				if (parm_handle != PARAM_INVALID) {
-					param_get(parm_handle, &trim_val);
-					(void)io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_TRIM_YAW, FLOAT_TO_REG(trim_val));
+					param_get(parm_handle, &param_val);
+					(void)io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_TRIM_YAW, FLOAT_TO_REG(param_val));
+				}
+
+				parm_handle = param_find("FW_MAN_R_SC");
+
+				if (parm_handle != PARAM_INVALID) {
+					param_get(parm_handle, &param_val);
+					(void)io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_SCALE_ROLL, FLOAT_TO_REG(param_val));
+				}
+
+				parm_handle = param_find("FW_MAN_P_SC");
+
+				if (parm_handle != PARAM_INVALID) {
+					param_get(parm_handle, &param_val);
+					(void)io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_SCALE_PITCH, FLOAT_TO_REG(param_val));
+				}
+
+				parm_handle = param_find("FW_MAN_Y_SC");
+
+				if (parm_handle != PARAM_INVALID) {
+					param_get(parm_handle, &param_val);
+					(void)io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_SCALE_YAW, FLOAT_TO_REG(param_val));
 				}
 
 				/* S.BUS output */

From af8cd3f880a14864e57284a5f1242796a71a103d Mon Sep 17 00:00:00 2001
From: Andreas Antener <antener_a@gmx.ch>
Date: Tue, 12 Jul 2016 14:33:44 +0200
Subject: [PATCH 1004/1230] correctly scale and trim outputs in IO

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

diff --git a/src/modules/px4iofirmware/mixer.cpp b/src/modules/px4iofirmware/mixer.cpp
index f41a80ef66..9c2dfdc5aa 100644
--- a/src/modules/px4iofirmware/mixer.cpp
+++ b/src/modules/px4iofirmware/mixer.cpp
@@ -373,15 +373,18 @@ mixer_callback(uintptr_t handle,
 	if (source == MIX_OVERRIDE || source == MIX_OVERRIDE_FMU_OK) {
 		if (control_group == actuator_controls_s::GROUP_INDEX_ATTITUDE &&
 		    control_index == actuator_controls_s::INDEX_ROLL) {
-			control *= REG_TO_FLOAT(r_setup_scale_roll) + REG_TO_FLOAT(r_setup_trim_roll);
+			control *= REG_TO_FLOAT(r_setup_scale_roll);
+			control += REG_TO_FLOAT(r_setup_trim_roll);
 
 		} else if (control_group == actuator_controls_s::GROUP_INDEX_ATTITUDE &&
 			   control_index == actuator_controls_s::INDEX_PITCH) {
-			control *= REG_TO_FLOAT(r_setup_scale_pitch) + REG_TO_FLOAT(r_setup_trim_pitch);
+			control *= REG_TO_FLOAT(r_setup_scale_pitch);
+			control += REG_TO_FLOAT(r_setup_trim_pitch);
 
 		} else if (control_group == actuator_controls_s::GROUP_INDEX_ATTITUDE &&
 			   control_index == actuator_controls_s::INDEX_YAW) {
-			control *= REG_TO_FLOAT(r_setup_scale_yaw) + REG_TO_FLOAT(r_setup_trim_yaw);
+			control *= REG_TO_FLOAT(r_setup_scale_yaw);
+			control += REG_TO_FLOAT(r_setup_trim_yaw);
 		}
 	}
 

From 631ce1fc55d72847279953a4f6181e8d9df1c214 Mon Sep 17 00:00:00 2001
From: Julian Oes <julian@oes.ch>
Date: Tue, 12 Jul 2016 12:12:59 +0200
Subject: [PATCH 1005/1230] commander: proper arguments for preflight check

---
 src/modules/commander/commander.cpp | 8 ++++----
 1 file changed, 4 insertions(+), 4 deletions(-)

diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp
index a8ab8f69ae..de2293e34c 100644
--- a/src/modules/commander/commander.cpp
+++ b/src/modules/commander/commander.cpp
@@ -1531,7 +1531,7 @@ int commander_thread_main(int argc, char *argv[])
 			// sensor diagnostics done continuously, not just at boot so don't warn about any issues just yet
 			status_flags.condition_system_sensors_initialized = Commander::preflightCheck(&mavlink_log_pub, true, true, true, true,
 				checkAirspeed, (status.rc_input_mode == vehicle_status_s::RC_IN_MODE_DEFAULT),
-				!can_arm_without_gps, false);
+				!can_arm_without_gps, /*checkDynamic */ false, /* reportFailures */ false);
 			set_tune_override(TONE_STARTUP_TUNE); //normal boot tune
 	}
 
@@ -1759,11 +1759,11 @@ int commander_thread_main(int argc, char *argv[])
 					if (is_hil_setup(autostart_id)) {
 						/* HIL configuration: check only RC input */
 						(void)Commander::preflightCheck(&mavlink_log_pub, false, false, false, false, false,
-								(status.rc_input_mode == vehicle_status_s::RC_IN_MODE_DEFAULT), false, true);
+								(status.rc_input_mode == vehicle_status_s::RC_IN_MODE_DEFAULT), /* checkGNSS */ false, /* checkDynamic */ true, /* reportFailures */ false);
 					} else {
 						/* check sensors also */
 						(void)Commander::preflightCheck(&mavlink_log_pub, true, true, true, true, chAirspeed,
-								(status.rc_input_mode == vehicle_status_s::RC_IN_MODE_DEFAULT), !can_arm_without_gps, hotplug_timeout);
+								(status.rc_input_mode == vehicle_status_s::RC_IN_MODE_DEFAULT), !can_arm_without_gps, /* checkDynamic */ true, hotplug_timeout);
 					}
 				}
 
@@ -3811,7 +3811,7 @@ void *commander_low_prio_loop(void *arg)
 						}
 
 						status_flags.condition_system_sensors_initialized = Commander::preflightCheck(&mavlink_log_pub, true, true, true, true, checkAirspeed,
-							!(status.rc_input_mode >= vehicle_status_s::RC_IN_MODE_OFF), !can_arm_without_gps, hotplug_timeout);
+							!(status.rc_input_mode >= vehicle_status_s::RC_IN_MODE_OFF), !can_arm_without_gps, /* checkDynamic */ true, hotplug_timeout);
 
 						arming_state_transition(&status,
 									&battery,

From 314ee6b7e009da7ca518168808e6c2ac7b8ef877 Mon Sep 17 00:00:00 2001
From: Julian Oes <julian@oes.ch>
Date: Tue, 12 Jul 2016 13:41:09 +0200
Subject: [PATCH 1006/1230] commander: remove some if confusion

This is a try to simplify the if statements a bit. Also, a check of
new_arming_state which was impossible, is removed.
---
 .../commander/state_machine_helper.cpp        | 21 +++++++++++--------
 1 file changed, 12 insertions(+), 9 deletions(-)

diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp
index c334e2b381..3d56545b4e 100644
--- a/src/modules/commander/state_machine_helper.cpp
+++ b/src/modules/commander/state_machine_helper.cpp
@@ -278,17 +278,20 @@ transition_result_t arming_state_transition(struct vehicle_status_s *status,
 		// Sensors need to be initialized for STANDBY state, except for HIL
 		} else if ((status->hil_state != vehicle_status_s::HIL_STATE_ON) &&
 			(new_arming_state == vehicle_status_s::ARMING_STATE_STANDBY) &&
-			(status->arming_state != vehicle_status_s::ARMING_STATE_STANDBY_ERROR) &&
-			(!status_flags->condition_system_sensors_initialized)) {
-			if ((!status_flags->condition_system_prearm_error_reported &&
-			      status_flags->condition_system_hotplug_timeout) ||
-			     (new_arming_state == vehicle_status_s::ARMING_STATE_ARMED)) {
+			(status->arming_state != vehicle_status_s::ARMING_STATE_STANDBY_ERROR)) {
 
-				mavlink_and_console_log_critical(mavlink_log_pub, "Not ready to fly: Sensors not set up correctly");
-				status_flags->condition_system_prearm_error_reported = true;
+			if (!status_flags->condition_system_sensors_initialized) {
+
+				if (status_flags->condition_system_hotplug_timeout) {
+					if (!status_flags->condition_system_prearm_error_reported) {
+						mavlink_and_console_log_critical(mavlink_log_pub,
+							"Not ready to fly: Sensors not set up correctly");
+						status_flags->condition_system_prearm_error_reported = true;
+					}
+				}
+				feedback_provided = true;
+				valid_transition = false;
 			}
-			feedback_provided = true;
-			valid_transition = false;
 		}
 
 		// Finish up the state transition

From 8ded6a58abbf46cfc52018e30f764df6e58aa8ac Mon Sep 17 00:00:00 2001
From: Julian Oes <julian@oes.ch>
Date: Tue, 12 Jul 2016 13:50:54 +0200
Subject: [PATCH 1007/1230] mavlink_log: enable queueing

We don't want to drop messages if possible for mavlink log messages, so
let's use the orb queueing.
---
 src/modules/systemlib/mavlink_log.c | 5 ++++-
 1 file changed, 4 insertions(+), 1 deletion(-)

diff --git a/src/modules/systemlib/mavlink_log.c b/src/modules/systemlib/mavlink_log.c
index 311e7d8c91..33d463f502 100644
--- a/src/modules/systemlib/mavlink_log.c
+++ b/src/modules/systemlib/mavlink_log.c
@@ -47,6 +47,7 @@
 #include <uORB/topics/mavlink_log.h>
 #include "mavlink_log.h"
 
+#define MAVLINK_LOG_QUEUE_SIZE 5
 
 
 __EXPORT void mavlink_vasprintf(int severity, orb_advert_t *mavlink_log_pub, const char *fmt, ...)
@@ -79,7 +80,9 @@ __EXPORT void mavlink_vasprintf(int severity, orb_advert_t *mavlink_log_pub, con
 		orb_publish(ORB_ID(mavlink_log), *mavlink_log_pub, &log_msg);
 
 	} else {
-		*mavlink_log_pub = orb_advertise(ORB_ID(mavlink_log), &log_msg);
+		*mavlink_log_pub = orb_advertise_queue(ORB_ID(mavlink_log),
+						       &log_msg,
+						       MAVLINK_LOG_QUEUE_SIZE);
 	}
 }
 

From 8345a0368b443de72101fe2e67ee1cf106a1ef04 Mon Sep 17 00:00:00 2001
From: Julian Oes <julian@oes.ch>
Date: Tue, 12 Jul 2016 13:52:12 +0200
Subject: [PATCH 1008/1230] mavlink: add function to copy only if updated

The MavlinkOrbSubscription only had an interface to either always copy
or copy based on a timestamp. This commit adds a copy interface if the
topic has been updated.
---
 .../mavlink/mavlink_orb_subscription.cpp      | 27 +++++++++++++++++--
 .../mavlink/mavlink_orb_subscription.h        | 23 +++++++++++-----
 2 files changed, 42 insertions(+), 8 deletions(-)

diff --git a/src/modules/mavlink/mavlink_orb_subscription.cpp b/src/modules/mavlink/mavlink_orb_subscription.cpp
index 66d309a156..87cf8624d3 100644
--- a/src/modules/mavlink/mavlink_orb_subscription.cpp
+++ b/src/modules/mavlink/mavlink_orb_subscription.cpp
@@ -74,7 +74,7 @@ MavlinkOrbSubscription::get_instance() const
 }
 
 bool
-MavlinkOrbSubscription::update(uint64_t *time, void* data)
+MavlinkOrbSubscription::update(uint64_t *time, void *data)
 {
 	// TODO this is NOT atomic operation, we can get data newer than time
 	// if topic was published between orb_stat and orb_copy calls.
@@ -106,11 +106,34 @@ MavlinkOrbSubscription::update(uint64_t *time, void* data)
 }
 
 bool
-MavlinkOrbSubscription::update(void* data)
+MavlinkOrbSubscription::update(void *data)
 {
 	return !orb_copy(_topic, _fd, data);
 }
 
+bool
+MavlinkOrbSubscription::update_if_changed(void *data)
+{
+	bool updated;
+	if (orb_check(_fd, &updated)) {
+		return false;
+	}
+
+	if (!updated) {
+		return false;
+	}
+
+	if (orb_copy(_topic, _fd, data)) {
+		if (data != nullptr) {
+			/* error copying topic data */
+			memset(data, 0, _topic->o_size);
+		}
+		return false;
+	}
+
+	return true;
+}
+
 bool
 MavlinkOrbSubscription::is_published()
 {
diff --git a/src/modules/mavlink/mavlink_orb_subscription.h b/src/modules/mavlink/mavlink_orb_subscription.h
index 5394e5097b..913f19ec97 100644
--- a/src/modules/mavlink/mavlink_orb_subscription.h
+++ b/src/modules/mavlink/mavlink_orb_subscription.h
@@ -54,26 +54,37 @@ public:
 	~MavlinkOrbSubscription();
 
 	/**
-	 * Check if subscription updated and get data.
+	 * Check if subscription updated based on timestamp.
 	 *
-	 * @return true only if topic was updated and data copied to buffer successfully.
-	 * If topic was not updated since last check it will return false but still copy the data.
-	 * If no data available data buffer will be filled with zeroes.
+	 * @return true only if topic was updated based on a timestamp and
+	 * copied to buffer successfully.
+	 * If topic was not updated since last check it will return false but
+	 * still copy the data.
+	 * If no data available data buffer will be filled with zeros.
 	 */
-	bool update(uint64_t *time, void* data);
+	bool update(uint64_t *time, void *data);
 
 	/**
 	 * Copy topic data to given buffer.
 	 *
 	 * @return true only if topic data copied successfully.
 	 */
-	bool update(void* data);
+	bool update(void *data);
+
+	/**
+	 * Check if the subscription has been updated.
+	 *
+	 * @return true if there has been an update which has been
+	 * copied successfully.
+	 */
+	bool update_if_changed(void *data);
 
 	/**
 	 * Check if the topic has been published.
 	 *
 	 * This call will return true if the topic was ever published.
 	 * @return true if the topic has been published at least once.
+	 * If no data is available the buffer will be filled with zeros.
 	 */
 	bool is_published();
 	orb_id_t get_topic() const;

From d9343fa925131c30b66140c1be77bfaa92799ccd Mon Sep 17 00:00:00 2001
From: Julian Oes <julian@oes.ch>
Date: Tue, 12 Jul 2016 13:56:00 +0200
Subject: [PATCH 1009/1230] mavlink: use new copy_if_updated interface

---
 src/modules/mavlink/mavlink_main.cpp | 3 +--
 1 file changed, 1 insertion(+), 2 deletions(-)

diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp
index d160320dc0..3282af4f10 100644
--- a/src/modules/mavlink/mavlink_main.cpp
+++ b/src/modules/mavlink/mavlink_main.cpp
@@ -1812,7 +1812,6 @@ Mavlink::task_main(int argc, char *argv[])
 	MavlinkOrbSubscription *ack_sub = add_orb_subscription(ORB_ID(vehicle_command_ack));
 	uint64_t ack_time = 0;
 	MavlinkOrbSubscription *mavlink_log_sub = add_orb_subscription(ORB_ID(mavlink_log));
-	uint64_t mavlink_log_time = 0;
 
 	struct vehicle_status_s status;
 	status_sub->update(&status_time, &status);
@@ -2069,7 +2068,7 @@ Mavlink::task_main(int argc, char *argv[])
 		}
 
 		struct mavlink_log_s mavlink_log;
-		if (mavlink_log_sub->update(&mavlink_log_time, &mavlink_log)) {
+		if (mavlink_log_sub->update_if_changed(&mavlink_log)) {
 			_logbuffer.put(&mavlink_log);
 		}
 

From eda2915f0b4706ddfbd6df9770ce237baa5c5ad9 Mon Sep 17 00:00:00 2001
From: Julian Oes <julian@oes.ch>
Date: Tue, 12 Jul 2016 13:56:38 +0200
Subject: [PATCH 1010/1230] mavlink: whitespace fix

---
 src/modules/mavlink/mavlink_messages.cpp | 8 ++++----
 1 file changed, 4 insertions(+), 4 deletions(-)

diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp
index aca7f71077..f4938afc96 100644
--- a/src/modules/mavlink/mavlink_messages.cpp
+++ b/src/modules/mavlink/mavlink_messages.cpp
@@ -3057,10 +3057,10 @@ public:
 		return MAVLINK_MSG_ID_ALTITUDE;
 	}
 
-    uint8_t get_id()
-    {
-        return get_id_static();
-    }
+	uint8_t get_id()
+	{
+		return get_id_static();
+	}
 
 	static MavlinkStream *new_instance(Mavlink *mavlink)
 	{

From 399d4ef833cd72ac059f794963e4e8a6d51ea65a Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Beat=20K=C3=BCng?= <beat-kueng@gmx.net>
Date: Wed, 13 Jul 2016 14:35:03 +0200
Subject: [PATCH 1011/1230] sensors: only poll on first gyro for now

This fixes a bug with following setup:
- two (or N > 1) connected gyros
- ekf2 enabled
In this case, sensors would publish with the combined rate of the gyros,
but with N following messages having the same gyro data & timestamp.
Apparently ekf2 cannot handle this, the other estimators can.

We may want to rethink what the proper solution is here.
---
 src/modules/sensors/sensors.cpp | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp
index af852abffe..1d69e03cfd 100644
--- a/src/modules/sensors/sensors.cpp
+++ b/src/modules/sensors/sensors.cpp
@@ -2342,7 +2342,7 @@ Sensors::task_main()
 		}
 
 		/* wait for up to 50ms for data */
-		int pret = px4_poll(fds, num_poll_fds, 50);
+		int pret = px4_poll(fds, 1, 50);
 
 		/* if pret == 0 it timed out - periodic check for _task_should_exit, etc. */
 

From 3ed8b735c22a787761d2b92dfd212e8881f52e72 Mon Sep 17 00:00:00 2001
From: Julian Oes <julian@oes.ch>
Date: Wed, 13 Jul 2016 09:55:58 +0200
Subject: [PATCH 1012/1230] mavlink: only warn once if broadcast fails

This fixes the issue where the console was spammed if a broadcast failed
after a connection had previously been established.
---
 src/modules/mavlink/mavlink_main.cpp | 8 +++++++-
 src/modules/mavlink/mavlink_main.h   | 1 +
 2 files changed, 8 insertions(+), 1 deletion(-)

diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp
index 3282af4f10..ed0713421b 100644
--- a/src/modules/mavlink/mavlink_main.cpp
+++ b/src/modules/mavlink/mavlink_main.cpp
@@ -224,6 +224,7 @@ Mavlink::Mavlink() :
 	_src_addr_initialized(false),
 	_broadcast_address_found(false),
 	_broadcast_address_not_found_warned(false),
+	_broadcast_failed_warned(false),
 	_network_buf{},
 	_network_buf_len(0),
 #endif
@@ -920,7 +921,12 @@ Mavlink::send_packet()
 						  (struct sockaddr *)&_bcast_addr, sizeof(_bcast_addr));
 
 				if (bret <= 0) {
-					PX4_ERR("sending broadcast failed, errno: %d: %s", errno, strerror(errno));
+					if (!_broadcast_failed_warned) {
+						PX4_ERR("sending broadcast failed, errno: %d: %s", errno, strerror(errno));
+						_broadcast_failed_warned = true;
+					}
+				} else {
+					_broadcast_failed_warned = false;
 				}
 			}
 		}
diff --git a/src/modules/mavlink/mavlink_main.h b/src/modules/mavlink/mavlink_main.h
index a07385c77d..b2f616a98a 100644
--- a/src/modules/mavlink/mavlink_main.h
+++ b/src/modules/mavlink/mavlink_main.h
@@ -508,6 +508,7 @@ private:
 	bool _src_addr_initialized;
 	bool _broadcast_address_found;
 	bool _broadcast_address_not_found_warned;
+	bool _broadcast_failed_warned;
 	uint8_t _network_buf[MAVLINK_MAX_PACKET_LEN];
 	unsigned _network_buf_len;
 #endif

From 78bc7d850b78552cab9299521f11a38cc1556b82 Mon Sep 17 00:00:00 2001
From: David Sidrane <david_s5@nscdg.com>
Date: Wed, 13 Jul 2016 05:53:47 -1000
Subject: [PATCH 1013/1230] Fix Syntax error - but I suspect the USE_IO logic
 can be simplified

---
 ROMFS/tap_common/init.d/rcS | 2 ++
 1 file changed, 2 insertions(+)

diff --git a/ROMFS/tap_common/init.d/rcS b/ROMFS/tap_common/init.d/rcS
index 111e695868..373ac6d090 100644
--- a/ROMFS/tap_common/init.d/rcS
+++ b/ROMFS/tap_common/init.d/rcS
@@ -117,6 +117,8 @@ then
 	set EXIT_ON_END no
 	set MAV_TYPE none
 	set FAILSAFE none
+	set USE_IO no
+	
 
 	#
 	# Set parameters and env variables for selected AUTOSTART

From 39ce201efe10418512b0791a91288fcf4acd31f1 Mon Sep 17 00:00:00 2001
From: David Sidrane <david_s5@nscdg.com>
Date: Tue, 12 Jul 2016 14:47:55 -1000
Subject: [PATCH 1014/1230] TAP:Fix build to use correct ROMFS

---
 src/firmware/nuttx/CMakeLists.txt | 4 ++++
 1 file changed, 4 insertions(+)

diff --git a/src/firmware/nuttx/CMakeLists.txt b/src/firmware/nuttx/CMakeLists.txt
index 73215f797f..53988b43cf 100644
--- a/src/firmware/nuttx/CMakeLists.txt
+++ b/src/firmware/nuttx/CMakeLists.txt
@@ -47,10 +47,14 @@ if(NOT ${BOARD} STREQUAL "sim")
 	endif()
 	
 	set(romfs_dir "ROMFS/px4fmu_common")
+	if (${BOARD} STREQUAL "tap-v1")
+	set(romfs_dir "ROMFS/tap_common")
+	endif()
 	if (${BOARD} STREQUAL "px4fmu-v2" AND ${LABEL} STREQUAL "test")
 	    set(romfs_dir "ROMFS/px4fmu_test")
 	endif()
 
+
 	px4_nuttx_add_romfs(OUT romfs
 		ROOT ${romfs_dir}
 		EXTRAS ${extras}

From 5e8d6375c97719531b5098eb8d07a883468f3ab8 Mon Sep 17 00:00:00 2001
From: David Sidrane <david_s5@nscdg.com>
Date: Mon, 11 Jul 2016 10:51:42 -1000
Subject: [PATCH 1015/1230] Back Port nuttx_v3 FLASH based parameter hooks

---
 src/modules/systemlib/CMakeLists.txt          |    2 +
 src/modules/systemlib/bson/tinybson.c         |    4 +-
 src/modules/systemlib/flashparams/flashfs.c   | 1122 +++++++++++++++++
 src/modules/systemlib/flashparams/flashfs.h   |  220 ++++
 .../systemlib/flashparams/flashparams.c       |  363 ++++++
 .../systemlib/flashparams/flashparams.h       |   73 ++
 src/modules/systemlib/param/param.c           |   50 +-
 src/platforms/px4_defines.h                   |    4 +
 src/systemcmds/param/param.c                  |   42 +-
 9 files changed, 1870 insertions(+), 10 deletions(-)
 create mode 100644 src/modules/systemlib/flashparams/flashfs.c
 create mode 100644 src/modules/systemlib/flashparams/flashfs.h
 create mode 100644 src/modules/systemlib/flashparams/flashparams.c
 create mode 100644 src/modules/systemlib/flashparams/flashparams.h

diff --git a/src/modules/systemlib/CMakeLists.txt b/src/modules/systemlib/CMakeLists.txt
index 4507974e85..04fccf3566 100644
--- a/src/modules/systemlib/CMakeLists.txt
+++ b/src/modules/systemlib/CMakeLists.txt
@@ -56,6 +56,8 @@ if(${OS} STREQUAL "nuttx")
 		err.c
 		printload.c
 		param/param.c
+		flashparams/flashparams.c
+		flashparams/flashfs.c
 		up_cxxinitialize.c
 		)
 elseif ("${CONFIG_SHMEM}" STREQUAL "1")
diff --git a/src/modules/systemlib/bson/tinybson.c b/src/modules/systemlib/bson/tinybson.c
index 41ffbcf822..099dffe4bc 100644
--- a/src/modules/systemlib/bson/tinybson.c
+++ b/src/modules/systemlib/bson/tinybson.c
@@ -456,7 +456,9 @@ bson_encoder_fini(bson_encoder_t encoder)
 	}
 
 	/* sync file */
-	BSON_FSYNC(encoder->fd);
+	if (encoder->fd > -1) {
+		BSON_FSYNC(encoder->fd);
+	}
 
 	return 0;
 }
diff --git a/src/modules/systemlib/flashparams/flashfs.c b/src/modules/systemlib/flashparams/flashfs.c
new file mode 100644
index 0000000000..c1c05ad321
--- /dev/null
+++ b/src/modules/systemlib/flashparams/flashfs.c
@@ -0,0 +1,1122 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2015 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file flashparam.c
+ *
+ * Global flash based parameter store.
+ *
+ * This provides the mechanisms to interface to the PX4
+ * parameter system but replace the IO with non file based flash
+ * i/o routines. So that the code my be implemented on a SMALL memory
+ * foot print device.
+ */
+
+#include <px4_defines.h>
+#include <px4_posix.h>
+#include <crc32.h>
+#include <stddef.h>
+#include <string.h>
+#include <stdbool.h>
+#include <stdint.h>
+#include <stdlib.h>
+#include <errno.h>
+#include "flashfs.h"
+#include <nuttx/compiler.h>
+#include <nuttx/progmem.h>
+
+
+/****************************************************************************
+ * Pre-processor Definitions
+ ****************************************************************************/
+
+/****************************************************************************
+ * Private Types
+ ****************************************************************************/
+
+typedef uint32_t h_magic_t;
+typedef uint16_t h_size_t;
+typedef uint16_t h_flag_t;
+typedef uint16_t data_size_t;
+
+typedef enum  flash_config_t {
+	LargestBlock = 2 * 1024, // This represents the size need for backing store
+	MagicSig     = 0xaa553cc3,
+	BlankSig     = 0xffffffff
+} flash_config_t;
+
+typedef enum  flash_flags_t {
+	SizeMask      = 0x0003,
+	MaskEntry     = ~SizeMask,
+	BlankEntry    = (h_flag_t)BlankSig,
+	ValidEntry    = (0xa5ac & ~SizeMask),
+	ErasedEntry   = 0x0000,
+} flash_flags_t;
+
+
+/* File flash_entry_header_t will be sizeof(h_magic_t) aligned
+ * The Size will be the actual length of the header plus the data
+ * and any padding needed to have the size be an even multiple of
+ * sizeof(h_magic_t)
+ *  The
+ */
+#pragma GCC diagnostic push
+#pragma GCC diagnostic ignored "-Wattributes"
+typedef struct packed_struct flash_entry_header_t {
+	h_magic_t            magic;           /* Used to ID files*/
+	h_flag_t             flag;            /* Used to erase this entry */
+	uint32_t             crc;             /* Calculated over the size - end of data */
+	h_size_t             size;            /* When added to a byte pointer to flash_entry_header_t
+                                               * Will result the offset of the next active file or
+                                               * free space. */
+	flash_file_token_t   file_token;      /* file token type - essentially the name/type */
+} flash_entry_header_t;
+#pragma GCC diagnostic pop
+
+/****************************************************************************
+ * Private Function Prototypes
+ ****************************************************************************/
+
+/****************************************************************************
+ * Private Data
+ ****************************************************************************/
+static uint8_t *working_buffer;
+static uint16_t working_buffer_size;
+static bool working_buffer_static;
+static sector_descriptor_t *sector_map;
+static int last_erased;
+
+/****************************************************************************
+ * Public Data
+ ****************************************************************************/
+
+const flash_file_token_t parameters_token = {
+	.n = {'p', 'a', 'r', 'm'},
+};
+
+/****************************************************************************
+ * Private Functions
+ ****************************************************************************/
+
+/****************************************************************************
+ * Name: parameter_flashfs_free
+ *
+ * Description:
+ *   Frees  dynamicaly allocated memory
+ *
+ *
+ ****************************************************************************/
+
+static void parameter_flashfs_free(void)
+{
+	if (!working_buffer_static && working_buffer != NULL) {
+		free(working_buffer);
+		working_buffer = NULL;
+		working_buffer_size = 0;
+	}
+}
+
+/****************************************************************************
+ * Name: blank_flash
+ *
+ * Description:
+ *   This helper function returns true if the pointer points to a blank_flash
+ *   location
+ *
+ * Input Parameters:
+ *   pf   - A pointer to memory aligned on sizeof(uint32_t) boundaries
+ *
+ * Returned value:
+ *   true if blank
+ *
+ *
+ ****************************************************************************/
+
+static inline int blank_flash(uint32_t *pf)
+{
+	return *pf == BlankSig;
+}
+
+/****************************************************************************
+ * Name: blank_check
+ *
+ * Description:
+ *   Given a pointer to a flash entry header and a new size
+ *
+ * Input Parameters:
+ *   pf       - A pointer to the current flash entry header
+ *   new_size - The total number of bytes to be written
+ *
+ * Returned value:
+ *  true if space is blank, If it is not blank it returns false
+ *
+ ****************************************************************************/
+
+static bool blank_check(flash_entry_header_t *pf,
+			size_t new_size)
+{
+	bool rv = true;
+	uint32_t *pm = (uint32_t *) pf;
+	new_size /= sizeof(uint32_t);
+
+	while (new_size-- && rv) {
+		if (!blank_flash(pm++)) {
+			rv = false;
+		}
+	}
+
+	return rv;
+}
+
+/****************************************************************************
+ * Name: valid_magic
+ *
+ * Description:
+ *   This helper function returns true if the pointer points to a valid
+ *   magic signature
+ *
+ * Input Parameters:
+ *   pm   - A pointer to memory aligned on sizeof(h_magic_t) boundaries
+ *
+ * Returned value:
+ *   true if magic is valid
+ *
+ *
+ ****************************************************************************/
+
+static inline int valid_magic(h_magic_t *pm)
+{
+	return *pm == MagicSig;
+}
+
+/****************************************************************************
+ * Name: blank_magic
+ *
+ * Description:
+ *   This helper function returns true if the pointer points to a valid
+ *   blank magic signature
+ *
+ * Input Parameters:
+ *   pm   - A pointer to memory aligned on sizeof(h_magic_t) boundaries
+ *
+ * Returned value:
+ *   true if magic is valid
+ *
+ *
+ ****************************************************************************/
+
+static inline int blank_magic(h_magic_t *pm)
+{
+	return *pm == BlankSig;
+}
+/****************************************************************************
+ * Name: erased_entry
+ *
+ * Description:
+ *   This helper function returns true if the entry is Erased
+ *
+ * Input Parameters:
+ *   fi            - A pointer to the current flash entry header
+ *
+ * Returned value:
+ *   true if  erased
+ *
+ *
+ ****************************************************************************/
+
+static inline int erased_entry(flash_entry_header_t *fi)
+{
+	return (fi->flag & MaskEntry) == ErasedEntry;
+}
+
+/****************************************************************************
+ * Name: blank_entry
+ *
+ * Description:
+ *   This helper function returns true if the entry is Blank
+ *
+ * Input Parameters:
+ *   fi            - A pointer to the current flash entry header
+ *
+ * Returned value:
+ *   true if Blank
+ *
+ *
+ ****************************************************************************/
+
+static inline int blank_entry(flash_entry_header_t *fi)
+{
+	return fi->magic == BlankSig  &&  fi->flag == BlankEntry;
+}
+
+/****************************************************************************
+ * Name: valid_entry
+ *
+ * Description:
+ *   This helper function returns true if the entry is Blank
+ *
+ * Input Parameters:
+ *   fi            - A pointer to the current flash entry header
+ *
+ * Returned value:
+ *   true if valid_entry
+ *
+ *
+ ****************************************************************************/
+
+static inline int valid_entry(flash_entry_header_t *fi)
+{
+	return (fi->flag & MaskEntry) == ValidEntry;
+}
+
+/****************************************************************************
+ * Name: entry_size_adjust
+ *
+ * Description:
+ *   This helper function returns the size adjust
+ *
+ * Input Parameters:
+ *   fi            - A pointer to the current flash entry header
+ *
+ * Returned value:
+ *   true if valid_entry
+ *
+ *
+ ****************************************************************************/
+
+static inline int entry_size_adjust(flash_entry_header_t *fi)
+{
+	return fi->flag & SizeMask;
+}
+
+/****************************************************************************
+ * Name: next_entry
+ *
+ * Description:
+ *   This helper function advances the flash entry header pointer to the
+ *   locations of the next entry.
+ *
+ * Input Parameters:
+ *   fh            - A pointer to the current file header
+ *
+ * Returned value:
+ *                - A pointer to the next file header location
+ *
+ *
+ ****************************************************************************/
+
+static inline flash_entry_header_t *next_entry(flash_entry_header_t *fi)
+{
+	uint8_t *pb = (uint8_t *)fi;
+	return (flash_entry_header_t *) &pb[fi->size];
+}
+
+/****************************************************************************
+ * Name: entry_data
+ *
+ * Description:
+ *   This helper function returns a pointer the the data in the entry
+ *
+ * Input Parameters:
+ *   fh            - A pointer to the current file header
+ *
+ * Returned value:
+ *                - A pointer to the next file header location
+ *
+ *
+ ****************************************************************************/
+
+static inline uint8_t *entry_data(flash_entry_header_t *fi)
+{
+	return ((uint8_t *)fi) + sizeof(flash_entry_header_t);
+}
+
+/****************************************************************************
+ * Name: entry_data_length
+ *
+ * Description:
+ *   This helper function returns the size of the user data
+ *
+ * Input Parameters:
+ *   fh            - A pointer to the current file header
+ *
+ * Returned value:
+ *                - The length of the data in the entry
+ *
+ *
+ ****************************************************************************/
+
+static inline data_size_t entry_data_length(flash_entry_header_t *fi)
+{
+	return fi->size - (sizeof(flash_entry_header_t) + entry_size_adjust(fi));
+}
+
+/****************************************************************************
+ * Name: entry_crc_start
+ *
+ * Description:
+ *   This helper function returns a const byte pointer to the location
+ *   where the CRC is calculated over
+ *
+ * Input Parameters:
+ *   fi  - A pointer to the current file header
+ *
+ * Returned value:
+ * A pointer to the point at which the crc is calculated from.
+ *
+ *
+ ****************************************************************************/
+
+static inline const uint8_t *entry_crc_start(flash_entry_header_t *fi)
+{
+	return (const uint8_t *)&fi->size;
+}
+
+/****************************************************************************
+ * Name: entry_crc_length
+ *
+ * Description:
+ *   This helper function returns the length of the regone where the CRC is
+ *   calculated over
+ *
+ * Input Parameters:
+ *   fi  - A pointer to the current file header
+ *
+ * Returned value:
+ * Number of bytes to to crc
+ *
+ *
+ ****************************************************************************/
+
+static inline data_size_t entry_crc_length(flash_entry_header_t *fi)
+{
+	return fi->size - offsetof(flash_entry_header_t, size);
+}
+
+/****************************************************************************
+ * Name: find_entry
+ *
+ * Description:
+ *   This helper function locates an "file" from the the file token
+ *
+ * Input Parameters:
+ *   token        - A flash file token, the pseudo file name
+ *
+ * Returned value:
+ *  On Success a pointer to flash entry header or NULL on failure
+ *
+ *
+ ****************************************************************************/
+
+static flash_entry_header_t *find_entry(flash_file_token_t token)
+{
+	for (int s = 0; sector_map[s].address; s++) {
+
+		h_magic_t *pmagic = (h_magic_t *) sector_map[s].address;
+		h_magic_t *pe = pmagic + (sector_map[s].size / sizeof(h_magic_t)) - 1;
+
+		/* Hunt for Magic Signature */
+cont:
+
+		while (pmagic != pe && !valid_magic(pmagic)) {
+			pmagic++;
+		}
+
+		/* Did we reach the end
+		 * if so try the next sector */
+
+		if (pmagic == pe) { continue; }
+
+		/* Found a magic So assume it is a file header */
+
+		flash_entry_header_t *pf = (flash_entry_header_t *) pmagic;
+
+		/* Test the CRC */
+
+		if (pf->crc == crc32(entry_crc_start(pf), entry_crc_length(pf))) {
+
+			/* Good CRC is it the one we are looking for ?*/
+
+			if (valid_entry(pf) && pf->file_token.t == token.t) {
+
+				return pf;
+
+			} else {
+
+				/* Not the one we wanted but we can trust the size */
+
+				pf = next_entry(pf);
+				pmagic = (h_magic_t *) pf;
+
+				/* If the next one is erased */
+
+				if (blank_entry(pf)) {
+					continue;
+				}
+			}
+
+			goto cont;
+
+		} else {
+
+			/* in valid CRC so keep looking */
+
+			pmagic++;
+		}
+	}
+
+	return NULL;
+}
+
+/****************************************************************************
+ * Name: find_free
+ *
+ * Description:
+ *   This helper function locates free space for the number of bytes required
+ *
+ * Input Parameters:
+ *   required  - Number of bytes required
+ *
+ * Returned value:
+ *  On Success a pointer to flash entry header or NULL on failure
+ *
+ *
+ ****************************************************************************/
+
+static flash_entry_header_t *find_free(data_size_t required)
+{
+	for (int s = 0; sector_map[s].address; s++) {
+
+		h_magic_t *pmagic = (h_magic_t *) sector_map[s].address;
+		h_magic_t *pe = pmagic + (sector_map[s].size / sizeof(h_magic_t)) - 1;
+
+		/* Hunt for Magic Signature */
+
+		do {
+
+			if (valid_magic(pmagic)) {
+
+				flash_entry_header_t *pf = (flash_entry_header_t *) pmagic;
+
+				/* Test the CRC */
+
+				if (pf->crc == crc32(entry_crc_start(pf), entry_crc_length(pf))) {
+
+					/* Valid Magic and CRC look for the next record*/
+
+					pmagic = ((uint32_t *) next_entry(pf));
+
+				} else {
+
+					pmagic++;
+				}
+			}
+
+			if (blank_magic(pmagic)) {
+
+				flash_entry_header_t *pf = (flash_entry_header_t *) pmagic;
+
+				if (blank_entry(pf) && blank_check(pf, required)) {
+					return pf;
+				}
+
+			}
+		}  while (++pmagic != pe);
+	}
+
+	return NULL;
+}
+
+/****************************************************************************
+ * Name: get_next_sector_descriptor
+ *
+ * Description:
+ *   Given a pointer to sector_descriptor_t, this helper function
+ *   returns a pointer to the next sector_descriptor_t
+ *
+ * Input Parameters:
+ *   current      - A pointer to the current sector_descriptor_t
+ *
+ * Returned value:
+ *  On Success A pointer to the next sector_descriptor_t,
+ *  otherwise NULL
+ *
+ *
+ ****************************************************************************/
+
+static sector_descriptor_t *get_next_sector_descriptor(sector_descriptor_t *
+		current)
+{
+	for (int s = 0; sector_map[s].address; s++) {
+		if (current == &sector_map[s]) {
+			if (sector_map[s + 1].address) {
+				s++;
+
+			} else {
+				s = 0;
+			}
+
+			return &sector_map[s];
+		}
+	}
+
+	return NULL;
+}
+
+/****************************************************************************
+ * Name: get_next_sector
+ *
+ * Description:
+ *   Given a pointer to a flash entry header returns the sector descriptor
+ *   for the file is located in
+ *
+ * Input Parameters:
+ *   current      - A pointer to the current flash entry header
+ *
+ * Returned value:
+ *  On Success A pointer to the next sector_descriptor_t,
+ *  otherwise NULL
+ *
+ *
+ ****************************************************************************/
+
+static sector_descriptor_t *get_sector_info(flash_entry_header_t *current)
+{
+	for (int s = 0; sector_map[s].address != 0; s++) {
+		uint8_t *pb = (uint8_t *) sector_map[s].address;
+		uint8_t *pe = pb + sector_map[s].size - 1;
+		uint8_t *pc = (uint8_t *) current;
+
+		if (pc >= pb && pc <= pe) {
+			return &sector_map[s];
+		}
+	}
+
+	return 0;
+}
+
+/****************************************************************************
+ * Name: erase_sector
+ *
+ * Description:
+ *   Given a pointer to sector_descriptor_t, this function
+ *   erases the sector and updates the last_erased using
+ *   the pointer to the flash_entry_header_t as a sanity check
+ *
+ * Input Parameters:
+ *   sm      - A pointer to the current sector_descriptor_t
+ *   pf      - A pointer to the current flash entry header
+ *
+ * Returned value:
+ *  O On Success or a negative errno
+ *
+ *
+ ****************************************************************************/
+
+static int erase_sector(sector_descriptor_t *sm, flash_entry_header_t *pf)
+{
+	int rv = 0;
+	ssize_t page = up_progmem_getpage((size_t)pf);
+
+	if (page > 0 && page == sm->page) {
+		last_erased = sm->page;
+		ssize_t size = up_progmem_erasepage(page);
+
+		if (size < 0 || size != sm->size) {
+			rv = size;
+		}
+	}
+
+	return rv;
+}
+
+/****************************************************************************
+ * Name: erase_entry
+ *
+ * Description:
+ *   Given a pointer to a flash entry header erases the entry
+ *
+ * Input Parameters:
+ *   pf  - A pointer to the current flash entry header
+ *
+ *
+ * Returned value:
+ *  >0 On Success or a negative errno
+ *
+ *
+ ****************************************************************************/
+
+static int erase_entry(flash_entry_header_t *pf)
+{
+	h_flag_t data = ErasedEntry;
+	size_t size = sizeof(h_flag_t);
+	int rv = up_progmem_write((size_t) &pf->flag, &data, size);
+	return rv;
+}
+
+/****************************************************************************
+ * Name: check_free_space_in_sector
+ *
+ * Description:
+ *   Given a pointer to a flash entry header and a new size
+ *
+ * Input Parameters:
+*   pf       - A pointer to the current flash entry header
+ *   new_size - The total number of bytes to be written
+  *
+ * Returned value:
+ *  0 if there is enough space left to write new size
+ *  If not it returns the flash_file_sector_t * that needs to be erased.
+ *
+ ****************************************************************************/
+
+static sector_descriptor_t *check_free_space_in_sector(flash_entry_header_t
+		*pf, size_t new_size)
+{
+	sector_descriptor_t *sm = get_sector_info(pf);
+	uint8_t *psector_first = (uint8_t *) sm->address;
+	uint8_t *psector_last = psector_first + sm->size - 1;
+	uint8_t *pnext_end = (uint8_t *)(valid_magic((h_magic_t *)pf) ? next_entry(pf) : pf) + new_size;
+
+	if (pnext_end >= psector_first && pnext_end <= psector_last) {
+		sm = 0;
+	}
+
+	return sm;
+}
+
+/****************************************************************************
+ * Public Functions
+ ****************************************************************************/
+/****************************************************************************
+ * Name: parameter_flashfs_read
+ *
+ * Description:
+ *   This function returns a pointer to the locations of the data associated
+ *   with the file token. On successful return *buffer will be set to Flash
+ *   location and *buf_size the length of the user data.
+ *
+ * Input Parameters:
+ *   token       - File Token File to read
+ *   buffer      - A pointer to a pointer that will receive the address
+ *                 in flash of the data of this "files" data
+ *   buf_size    - A pointer to receive the number of bytes in the "file"
+ *
+ * Returned value:
+ *   On success number of bytes read or a negative errno value,
+ *
+ *
+ ****************************************************************************/
+
+int parameter_flashfs_read(flash_file_token_t token, uint8_t **buffer, size_t
+			   *buf_size)
+{
+	int rv = -ENXIO;
+
+	if (sector_map) {
+
+		rv = -ENOENT;
+		flash_entry_header_t *pf = find_entry(token);
+
+		if (pf) {
+			(*buffer) = entry_data(pf);
+			rv = entry_data_length(pf);
+			*buf_size = rv;
+		}
+	}
+
+	return rv;
+}
+
+/****************************************************************************
+ * Name: parameter_flashfs_write
+ *
+ * Description:
+ *   This function writes user data from the buffer allocated with a previous call
+ *   to parameter_flashfs_alloc. flash starting at the given address
+ *
+ * Input Parameters:
+ *   token      - File Token File to read
+ *   buffer      - A pointer to a buffer with buf_size bytes to be written
+ *                 to the flash. This buffer must be allocated
+ *                 with a previous call to flash_alloc_buffer
+ *   buf_size    - Number of bytes to write
+ *
+ * Returned value:
+ *   On success the number of bytes written On Error a negative value of errno
+ *
+ ****************************************************************************/
+
+int
+parameter_flashfs_write(flash_file_token_t token, uint8_t *buffer, size_t buf_size)
+{
+	int rv = -ENXIO;
+
+	if (sector_map) {
+
+		rv = 0;
+
+		/* Calculate the total space needed */
+
+		size_t total_size = buf_size + sizeof(flash_entry_header_t);
+		size_t alignment = sizeof(h_magic_t) - 1;
+		size_t  size_adjust = ((total_size + alignment) & ~alignment) - total_size;
+		total_size += size_adjust;
+
+		/* Is this and existing entry */
+
+		flash_entry_header_t *pf = find_entry(token);
+
+		if (!pf) {
+
+			/* No Entry exists for this token so find a place for it */
+
+			pf = find_free(total_size);
+
+			/* No Space */
+
+			if (pf == 0) {
+				parameter_flashfs_free();
+				return -ENOSPC;
+			}
+
+		} else {
+
+			/* Do we have space after the entry in the sector for the update */
+
+			sector_descriptor_t *current_sector = check_free_space_in_sector(pf,
+							      total_size);
+
+
+			if (current_sector == 0) {
+
+				/* Mark the last entry erased */
+
+				/* todo:consider a 2 stage erase or write before erase and do a fs check
+				 * at start up
+				 */
+
+				rv = erase_entry(pf);
+
+				if (rv < 0) {
+					parameter_flashfs_free();
+					return rv;
+				}
+
+				/* We had space and marked the last entry erased so use the  Next Free */
+
+				pf = next_entry(pf);
+
+			} else {
+
+				/*
+				 * We did not have space in the current sector so select the next sector
+				 */
+
+				current_sector = get_next_sector_descriptor(current_sector);
+
+				/* Will the data fit */
+
+				if (current_sector->size < total_size) {
+					parameter_flashfs_free();
+					return -ENOSPC;
+				}
+
+				/* Mark the last entry erased */
+
+				/* todo:consider a 2 stage erase or write before erase and do a fs check
+				 * at start up
+				 */
+
+				rv = erase_entry(pf);
+
+				if (rv < 0) {
+					parameter_flashfs_free();
+					return rv;
+				}
+
+				pf = (flash_entry_header_t *) current_sector->address;
+			}
+
+			if (!blank_check(pf, total_size)) {
+				rv = erase_sector(current_sector, pf);
+			}
+		}
+
+		flash_entry_header_t *pn = (flash_entry_header_t *)(buffer - sizeof(flash_entry_header_t));
+		pn->magic = MagicSig;
+		pn->file_token.t = token.t;
+		pn->flag = ValidEntry + size_adjust;
+		pn->size = total_size;
+
+		for (size_t a = 0; a < size_adjust; a++) {
+			buffer[buf_size + a] = (uint8_t)BlankSig;
+		}
+
+		pn->crc = crc32(entry_crc_start(pn), entry_crc_length(pn));
+		rv = up_progmem_write((size_t) pf, pn, pn->size);
+		int system_bytes = (sizeof(flash_entry_header_t) + size_adjust);
+
+		if (rv >= system_bytes) {
+			rv -= system_bytes;
+		}
+	}
+
+	parameter_flashfs_free();
+	return rv;
+}
+
+/****************************************************************************
+ * Name: parameter_flashfs_alloc
+ *
+ * Description:
+ *   This function is called to get a buffer to use in a subsequent call
+ *   to parameter_flashfs_write. The address returned is advanced into the
+ *   buffer to reserve space for the flash entry header.
+ *
+ * Input Parameters:
+ *   token      - File Token File to read (not used)
+ *   buffer     - A pointer to return a pointer to Memory of buf_size length
+ *   			  suitable for calling parameter_flashfs_write
+ *   buf_size   - In: the size needed for the write operation.
+ *   			  Out: The maximum number of bytes that can be written to
+ *                the buffer
+ *
+ * Returned value:
+ *   On success the number of bytes written On Error a negative value of errno
+ *
+ ****************************************************************************/
+
+int parameter_flashfs_alloc(flash_file_token_t token, uint8_t **buffer, size_t *buf_size)
+{
+	int rv = -ENXIO;
+
+	if (sector_map) {
+
+		rv = -ENOMEM;
+
+		if (!working_buffer_static) {
+
+			working_buffer_size = *buf_size + sizeof(flash_entry_header_t);
+			working_buffer = malloc(working_buffer_size);
+
+		}
+
+		/* Allocation failed or not provided */
+
+		if (working_buffer == NULL) {
+
+			working_buffer_size = 0;
+
+		} else {
+
+			/* We have a buffer reserve space and init it */
+			*buffer = &working_buffer[sizeof(flash_entry_header_t)];
+			*buf_size = working_buffer_size - sizeof(flash_entry_header_t);
+			memset(working_buffer, 0xff , working_buffer_size);
+			rv = 0;
+
+		}
+
+	}
+
+	return rv;
+}
+
+/****************************************************************************
+ * Name: parameter_flashfs_erase
+ *
+ * Description:
+ *   This function erases the sectors that were passed to parameter_flashfs_init
+ *
+ * Input Parameters:
+ *
+ * Returned value:
+ *   On success the number of bytes erased
+ *   On Error a negative value of errno
+ *
+ ****************************************************************************/
+
+int parameter_flashfs_erase(void)
+{
+	int rv = -ENXIO;
+
+	if (sector_map) {
+		rv = 0;
+
+		for (int s = 0; sector_map[s].address; s++) {
+			int sz = erase_sector(&sector_map[s], (flash_entry_header_t *)sector_map[s].address);
+
+			if (sz != 0) {
+				return sz;
+			}
+
+			rv += sector_map[s].size;
+		}
+	}
+
+	return rv;
+}
+
+/****************************************************************************
+ * Name: parameter_flashfs_init
+ *
+ * Description:
+ *   This helper function advances the flash entry header pointer to the
+ *   locations of the next entry.
+ *
+ * Input Parameters:
+ *   fconfig      - A pointer to an null entry terminated array of
+ *                  flash_file_sector_t
+ *    buffer      - A pointer to a memory to make available to callers
+ *                  for write operations. When allocated to the caller
+ *                  space is reserved in the front for the
+ *                  flash_entry_header_t.
+ *                  If this is passes as NULL. The buffer will be
+ *                  allocated from the heap on callse to
+ *                  parameter_flashfs_alloc and fread on calls calls
+ *                  to parameter_flashfs_write
+ *
+ *   size         - The size of the buffer in bytes. Should be be 0 if buffer
+ *                  is NULL
+ *
+ * Returned value:
+ *                - A pointer to the next file header location
+ *
+ *
+ ****************************************************************************/
+
+int parameter_flashfs_init(sector_descriptor_t *fconfig, uint8_t *buffer, uint16_t size)
+{
+	int rv = 0;
+	sector_map = fconfig;
+	working_buffer_static = buffer != NULL;
+
+	if (!working_buffer_static) {
+		size = 0;
+	}
+
+	working_buffer = buffer;
+	working_buffer_size = size;
+	last_erased = -1;
+
+	/* Sanity check */
+
+	flash_entry_header_t *pf = find_entry(parameters_token);
+
+	/*  No paramaters */
+
+	if (pf == NULL) {
+		size_t total_size = size + sizeof(flash_entry_header_t);
+		size_t alignment = sizeof(h_magic_t) - 1;
+		size_t  size_adjust = ((total_size + alignment) & ~alignment) - total_size;
+		total_size += size_adjust;
+
+		/* Do we have free space ?*/
+
+		if (find_free(total_size) == NULL) {
+
+			/* No paramates and no free space => neeed erase */
+
+			rv  = parameter_flashfs_erase();
+		}
+	}
+
+	return rv;
+}
+
+#if defined(FLASH_UNIT_TEST)
+
+static sector_descriptor_t  test_sector_map[] = {
+	{1, 16 * 1024, 0x08004000},
+	{2, 16 * 1024, 0x08008000},
+	{0, 0, 0},
+};
+
+__EXPORT void test(void);
+
+uint8_t test_buf[32 * 1024];
+
+__EXPORT void test(void)
+{
+	uint16_t largest_block = (32 * 1024) + 32;
+	uint8_t *buffer = malloc(largest_block);
+
+	parameter_flashfs_init(test_sector_map, buffer, largest_block);
+
+	for (int t = 0; t < sizeof(test_buf); t++) {
+		test_buf[t] = (uint8_t) t;
+	}
+
+	int er = parameter_flashfs_erase();
+	uint8_t *fbuffer;
+	size_t buf_size;
+	int written = 0;
+	int read = 0;
+	int rv = 0;
+
+	for (int a = 0; a <= 4; a++) {
+		rv =  parameter_flashfs_alloc(parameters_token, &fbuffer, &buf_size);
+		memcpy(fbuffer, test_buf, a);
+		buf_size = a;
+		written = parameter_flashfs_write(parameters_token, fbuffer, buf_size);
+		read = parameter_flashfs_read(parameters_token, &fbuffer, &buf_size);
+
+		if (read != written) {
+			static volatile int j;
+			j++;
+		}
+	}
+
+	int block = 2048;
+
+	for (int a = 0; a <= 8; a++) {
+		rv =  parameter_flashfs_alloc(parameters_token, &fbuffer, &buf_size);
+		memcpy(fbuffer, test_buf, block);
+		buf_size = block;
+		written = parameter_flashfs_write(parameters_token, fbuffer, buf_size);
+		read = parameter_flashfs_read(parameters_token, &fbuffer, &buf_size);
+
+		if (read != written) {
+			static volatile int j;
+			j++;
+		}
+
+		block += 2048;
+	}
+
+	rv++;
+	er++;
+	free(buffer);
+}
+#endif /* FLASH_UNIT_TEST */
diff --git a/src/modules/systemlib/flashparams/flashfs.h b/src/modules/systemlib/flashparams/flashfs.h
new file mode 100644
index 0000000000..65bcb48d00
--- /dev/null
+++ b/src/modules/systemlib/flashparams/flashfs.h
@@ -0,0 +1,220 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2015 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file param.h
+ *
+ * Global flash based parameter store.
+ *
+ * This provides the mechanisms to interface to the PX4
+ * parameter system but replace the IO with non file based flash
+ * i/o routines. So that the code my be implemented on a SMALL memory
+ * foot print device.
+ *
+ */
+
+#ifndef _SYSTEMLIB_FLASHPARAMS_NUTTX_PARAM_H
+#define _SYSTEMLIB_FLASHPARAMS_NUTTX_PARAM_H
+
+#include <stdint.h>
+#include <stdbool.h>
+
+
+
+/****************************************************************************
+ * Pre-processor Definitions
+ ****************************************************************************/
+/*
+ *  PARAMETER_BUFFER_SIZE must be defined larger then the maximum parameter
+ * memory needed to commit the recored + ~20 bytes. For the syslib's parameter
+ * this would be the size of the bson representations of the data
+ */
+#if !defined(PARAMETER_BUFFER_SIZE)
+#define PARAMETER_BUFFER_SIZE 512
+#endif
+
+__BEGIN_DECLS
+
+/*
+ *  Define the interface data a flash_file_token_t
+ *  is like a file name
+ *
+ */
+typedef uint32_t flash_file_tokens_t;
+
+typedef struct flash_file_token_t {
+	union {
+		flash_file_tokens_t t;
+		uint8_t n[sizeof(flash_file_tokens_t)];
+	};
+} flash_file_token_t;
+
+/*
+ * Define the parameter "file name" Currently there is only
+ * and it is hard coded. If more are added the
+ * parameter_flashfs_write would need to support a backing buffer
+ * for when a sector is erased.
+ */
+__EXPORT extern const flash_file_token_t parameters_token;
+
+/* Define the elements of the array passed to the
+ * parameter_flashfs_init function
+ *
+ * For example
+ * static sector_descriptor_t  sector_map[] = {
+ *      {1, 16 * 1024, 0x08004000},
+ *      {2, 16 * 1024, 0x08008000},
+ *       {0, 0, 0},
+ *
+ */
+typedef struct sector_descriptor_t {
+	uint8_t       page;
+	uint16_t      size;
+	uint32_t      address;
+} sector_descriptor_t;
+
+
+/****************************************************************************
+ * Name: parameter_flashfs_init
+ *
+ * Description:
+ *   This helper function advances the flash entry header pointer to the
+ *   locations of the next entry.
+ *
+ * Input Parameters:
+ *   fconfig      - A pointer to an null entry terminated array of
+ *                  flash_file_sector_t
+ *    buffer      - A pointer to a memory to make available to callers
+ *                  for write operations. When allocated to the caller
+ *                  space is reserved in the front for the
+ *                  flash_entry_header_t.
+ *                  If this is passes as NULL. The buffer will be
+ *                  allocated from the heap on callse to
+ *                  parameter_flashfs_alloc and fread on calls calls
+ *                  to parameter_flashfs_write
+ *
+ *   size         - The size of the buffer in bytes. Should be be 0 if buffer
+ *                  is NULL
+ *
+ * Returned value:
+ *                - A pointer to the next file header location
+ *
+ *
+ ****************************************************************************/
+
+__EXPORT int parameter_flashfs_init(sector_descriptor_t *fconfig, uint8_t *buffer, uint16_t size);
+
+/****************************************************************************
+ * Name: parameter_flashfs_read
+ *
+ * Description:
+ *   This function returns a pointer to the locations of the data associated
+ *   with the file token. On successful return *buffer will be set to Flash
+ *   location and *buf_size the length of the user data.
+ *
+ * Input Parameters:
+ *   token       - File Token File to read
+ *   buffer      - A pointer to a pointer that will receive the address
+ *                 in flash of the data of this "files" data
+ *   buf_size    - A pointer to receive the number of bytes in the "file"
+ *
+ * Returned value:
+ *   On success number of bytes read or a negative errno value,
+ *
+ *
+ ****************************************************************************/
+
+__EXPORT int parameter_flashfs_read(flash_file_token_t ft, uint8_t **buffer, size_t *buf_size);
+
+/****************************************************************************
+ * Name: parameter_flashfs_write
+ *
+ * Description:
+ *   This function writes user data from the buffer allocated with a previous call
+ *   to parameter_flashfs_alloc. flash starting at the given address
+ *
+ * Input Parameters:
+ *   token      - File Token File to read
+ *   buffer      - A pointer to a buffer with buf_size bytes to be written
+ *                 to the flash. This buffer must be allocated
+ *                 with a previous call to flash_alloc_buffer
+ *   buf_size    - Number of bytes to write
+ *
+ * Returned value:
+ *   On success the number of bytes written On Error a negative value of errno
+ *   If static buffer was not provided to parameter_flashfs_init the
+ *   buffer will be freed.
+ *
+ ****************************************************************************/
+
+__EXPORT int parameter_flashfs_write(flash_file_token_t ft, uint8_t *buffer, size_t buf_size);
+
+/****************************************************************************
+ * Name: parameter_flashfs_erase
+ *
+ * Description:
+ *   This function erases the sectors that were passed to parameter_flashfs_init
+ *
+ * Input Parameters:
+ *
+ * Returned value:
+ *   On success the number of bytes erased
+ *   On Error a negative value of errno
+ *
+ ****************************************************************************/
+
+__EXPORT int parameter_flashfs_erase(void);
+
+/****************************************************************************
+ * Name: parameter_flashfs_alloc
+ *
+ * Description:
+ *   This function is called to get a buffer to use in a subsequent call
+ *   to parameter_flashfs_write. The address returned is advanced into the
+ *   buffer to reserve space for the flash entry header.
+ *
+ * Input Parameters:
+ *   token      - File Token File to read (not used)
+ *   buffer     - Memory of buf_size length suitable for calling
+ *                parameter_flashfs_write
+ *   buf_size   - The maximum number of bytes that can be written to
+ *                the buffer
+ *
+ * Returned value:
+ *   On success the number of bytes written On Error a negative value of errno
+ *
+ ****************************************************************************/
+
+__EXPORT int parameter_flashfs_alloc(flash_file_token_t ft, uint8_t **buffer, size_t *buf_size);
+__END_DECLS
+#endif /* _SYSTEMLIB_FLASHPARAMS_NUTTX_PARAM_H */
diff --git a/src/modules/systemlib/flashparams/flashparams.c b/src/modules/systemlib/flashparams/flashparams.c
new file mode 100644
index 0000000000..623715b635
--- /dev/null
+++ b/src/modules/systemlib/flashparams/flashparams.c
@@ -0,0 +1,363 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2015 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+/**
+ * @file flashparam.c
+ *
+ * Global flash based parameter store.
+ *
+ * This provides the mechanisms to interface to the PX4
+ * parameter system but replace the IO with non file based flash
+ * i/o routines. So that the code my be implemented on a SMALL memory
+ * foot print device.
+ */
+
+#include <px4_defines.h>
+#include <px4_posix.h>
+#include <string.h>
+#include <stdbool.h>
+#include <stdint.h>
+
+#include "systemlib/param/param.h"
+#include "systemlib/uthash/utarray.h"
+#include "systemlib/bson/tinybson.h"
+#include "flashparams.h"
+#include "flashfs.h"
+
+#if 0
+# define debug(fmt, args...)            do { warnx(fmt, ##args); } while(0)
+#else
+# define debug(fmt, args...)            do { } while(0)
+#endif
+
+
+/**
+ * Storage for modified parameters.
+ */
+struct param_wbuf_s {
+	param_t                 param;
+	union param_value_u     val;
+	bool                    unsaved;
+};
+
+
+/** lock the parameter store */
+static void
+param_lock(void)
+{
+	//do {} while (sem_wait(&param_sem) != 0);
+}
+
+/** unlock the parameter store */
+static void
+param_unlock(void)
+{
+	//sem_post(&param_sem);
+}
+
+
+static int
+param_export_internal(bool only_unsaved)
+{
+	struct param_wbuf_s *s = NULL;
+	struct bson_encoder_s encoder;
+	int     result = -1;
+
+	param_lock();
+
+	/* Use realloc */
+
+	bson_encoder_init_buf(&encoder, NULL, 0);
+
+	/* no modified parameters -> we are done */
+	if (param_values == NULL) {
+		result = 0;
+		goto out;
+	}
+
+	while ((s = (struct param_wbuf_s *)utarray_next(param_values, s)) != NULL) {
+
+		int32_t i;
+		float   f;
+
+		/*
+		 * If we are only saving values changed since last save, and this
+		 * one hasn't, then skip it
+		 */
+		if (only_unsaved && !s->unsaved) {
+			continue;
+		}
+
+		s->unsaved = false;
+
+		/* append the appropriate BSON type object */
+
+		switch (param_type(s->param)) {
+
+		case PARAM_TYPE_INT32:
+			param_get(s->param, &i);
+
+			if (bson_encoder_append_int(&encoder, param_name(s->param), i)) {
+				debug("BSON append failed for '%s'", param_name(s->param));
+				goto out;
+			}
+
+			break;
+
+		case PARAM_TYPE_FLOAT:
+			param_get(s->param, &f);
+
+			if (bson_encoder_append_double(&encoder, param_name(s->param), f)) {
+				debug("BSON append failed for '%s'", param_name(s->param));
+				goto out;
+			}
+
+			break;
+
+		case PARAM_TYPE_STRUCT ... PARAM_TYPE_STRUCT_MAX:
+			if (bson_encoder_append_binary(&encoder,
+						       param_name(s->param),
+						       BSON_BIN_BINARY,
+						       param_size(s->param),
+						       param_get_value_ptr_external(s->param))) {
+				debug("BSON append failed for '%s'", param_name(s->param));
+				goto out;
+			}
+
+			break;
+
+		default:
+			debug("unrecognized parameter type");
+			goto out;
+		}
+	}
+
+	result = 0;
+
+out:
+	param_unlock();
+
+	if (result == 0) {
+
+		/* Finalize the bison encoding*/
+
+		bson_encoder_fini(&encoder);
+
+		/* Get requiered space */
+
+		size_t buf_size = bson_encoder_buf_size(&encoder);
+
+		/* Get a buffer from the flash driver with enough space */
+
+		uint8_t *buffer;
+		result = parameter_flashfs_alloc(parameters_token, &buffer, &buf_size);
+
+		if (result == OK) {
+
+			void *enc_buff = bson_encoder_buf_data(&encoder);
+			memcpy(buffer, enc_buff, buf_size);
+			free(enc_buff);
+			result = parameter_flashfs_write(parameters_token, buffer, buf_size);
+			result = result == buf_size ? OK : -EFBIG;
+		}
+	}
+
+	return result;
+}
+
+struct param_import_state {
+	bool mark_saved;
+};
+
+static int
+param_import_callback(bson_decoder_t decoder, void *private, bson_node_t node)
+{
+	float f;
+	int32_t i;
+	void *v, *tmp = NULL;
+	int result = -1;
+	struct param_import_state *state = (struct param_import_state *)private;
+
+	/*
+	 * EOO means the end of the parameter object. (Currently not supporting
+	 * nested BSON objects).
+	 */
+	if (node->type == BSON_EOO) {
+		debug("end of parameters");
+		return 0;
+	}
+
+	/*
+	 * Find the parameter this node represents.  If we don't know it,
+	 * ignore the node.
+	 */
+	param_t param = param_find_no_notification(node->name);
+
+	if (param == PARAM_INVALID) {
+		debug("ignoring unrecognised parameter '%s'", node->name);
+		return 1;
+	}
+
+	/*
+	 * Handle setting the parameter from the node
+	 */
+
+	switch (node->type) {
+	case BSON_INT32:
+		if (param_type(param) != PARAM_TYPE_INT32) {
+			debug("unexpected type for '%s", node->name);
+			goto out;
+		}
+
+		i = node->i;
+		v = &i;
+		break;
+
+	case BSON_DOUBLE:
+		if (param_type(param) != PARAM_TYPE_FLOAT) {
+			debug("unexpected type for '%s", node->name);
+			goto out;
+		}
+
+		f = node->d;
+		v = &f;
+		break;
+
+	case BSON_BINDATA:
+		if (node->subtype != BSON_BIN_BINARY) {
+			debug("unexpected subtype for '%s", node->name);
+			goto out;
+		}
+
+		if (bson_decoder_data_pending(decoder) != param_size(param)) {
+			debug("bad size for '%s'", node->name);
+			goto out;
+		}
+
+		/* XXX check actual file data size? */
+		tmp = malloc(param_size(param));
+
+		if (tmp == NULL) {
+			debug("failed allocating for '%s'", node->name);
+			goto out;
+		}
+
+		if (bson_decoder_copy_data(decoder, tmp)) {
+			debug("failed copying data for '%s'", node->name);
+			goto out;
+		}
+
+		v = tmp;
+		break;
+
+	default:
+		debug("unrecognised node type");
+		goto out;
+	}
+
+	if (param_set_external(param, v, state->mark_saved, true, false)) {
+
+		debug("error setting value for '%s'", node->name);
+		goto out;
+	}
+
+	if (tmp != NULL) {
+		free(tmp);
+		tmp = NULL;
+	}
+
+	/* don't return zero, that means EOF */
+	result = 1;
+
+out:
+
+	if (tmp != NULL) {
+		free(tmp);
+	}
+
+	return result;
+}
+
+static int
+param_import_internal(bool mark_saved)
+{
+	struct bson_decoder_s decoder;
+	int result = -1;
+	struct param_import_state state;
+
+	uint8_t *buffer = 0;
+	size_t buf_size;
+	parameter_flashfs_read(parameters_token, &buffer, &buf_size);
+
+	if (bson_decoder_init_buf(&decoder, buffer, buf_size, param_import_callback, &state)) {
+		debug("decoder init failed");
+		goto out;
+	}
+
+	state.mark_saved = mark_saved;
+
+	do {
+		result = bson_decoder_next(&decoder);
+
+	} while (result > 0);
+
+out:
+
+	if (result < 0) {
+		debug("BSON error decoding parameters");
+	}
+
+	return result;
+}
+
+int flash_param_save(void)
+{
+	return param_export_internal(false);
+}
+
+
+int flash_param_save_default(void)
+{
+	return param_export_internal(false);
+}
+
+
+int flash_param_load(void)
+{
+	param_reset_all();
+	return param_import_internal(true);
+}
+
+int flash_param_import(void)
+{
+	return 0;
+}
diff --git a/src/modules/systemlib/flashparams/flashparams.h b/src/modules/systemlib/flashparams/flashparams.h
new file mode 100644
index 0000000000..8300669bd4
--- /dev/null
+++ b/src/modules/systemlib/flashparams/flashparams.h
@@ -0,0 +1,73 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2015 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file param.h
+ *
+ * Global flash based parameter store.
+ *
+ * This provides the mechanisms to interface to the PX4
+ * parameter system but replace the IO with non file based flash
+ * i/o routines. So that the code my be implemented on a SMALL memory
+ * foot print device.
+ *
+ */
+
+#ifndef _SYSTEMLIB_FLASHPARAMS_FLASHPARAMS_H
+#define _SYSTEMLIB_FLASHPARAMS_FLASHPARAMS_H
+
+#include <stdint.h>
+#include <stdbool.h>
+#include <sys/types.h>
+#include "systemlib/uthash/utarray.h"
+
+__BEGIN_DECLS
+
+/*
+ * When using the flash based parameter store we have to force
+ * the param_values and 2 functions to be global
+ */
+
+#define FLASH_PARAMS_EXPOSE __EXPORT
+
+__EXPORT extern UT_array        *param_values;
+__EXPORT int param_set_external(param_t param, const void *val, bool mark_saved, bool notify_changes, bool is_saved);
+__EXPORT const void *param_get_value_ptr_external(param_t param);
+
+/* The interface hooks to the Flash based storage */
+__EXPORT int flash_param_save(void);
+__EXPORT int flash_param_save_default(void);
+__EXPORT int flash_param_load(void);
+__EXPORT int flash_param_import(void);
+__END_DECLS
+#endif /* _SYSTEMLIB_FLASHPARAMS_FLASHPARAMS_H */
diff --git a/src/modules/systemlib/param/param.c b/src/modules/systemlib/param/param.c
index e758099124..265c5f7e14 100644
--- a/src/modules/systemlib/param/param.c
+++ b/src/modules/systemlib/param/param.c
@@ -63,12 +63,21 @@
 #include "systemlib/uthash/utarray.h"
 #include "systemlib/bson/tinybson.h"
 
-#include "uORB/uORB.h"
-#include "uORB/topics/parameter_update.h"
-#include "px4_parameters.h"
+#if !defined(PARAM_NO_ORB)
+# include "uORB/uORB.h"
+# include "uORB/topics/parameter_update.h"
+#endif
 
+#if !defined(FLASH_BASED_PARAMS)
+#  define FLASH_PARAMS_EXPOSE
+#else
+#  include "systemlib/flashparams/flashparams.h"
+#endif
+
+#include "px4_parameters.h"
 #include <crc32.h>
 
+
 #if 0
 # define debug(fmt, args...)		do { warnx(fmt, ##args); } while(0)
 #else
@@ -131,13 +140,16 @@ get_param_info_count(void)
 }
 
 /** flexible array holding modified parameter values */
-UT_array	*param_values;
+FLASH_PARAMS_EXPOSE UT_array        *param_values;
 
 /** array info for the modified parameters array */
-const UT_icd	param_icd = {sizeof(struct param_wbuf_s), NULL, NULL, NULL};
+FLASH_PARAMS_EXPOSE const UT_icd    param_icd = {sizeof(struct param_wbuf_s), NULL, NULL, NULL};
+
+#if !defined(PARAM_NO_ORB)
 
 /** parameter update topic handle */
 static orb_advert_t param_topic = NULL;
+#endif
 
 static void param_set_used_internal(param_t param);
 
@@ -235,9 +247,11 @@ param_find_changed(param_t param)
 static void
 param_notify_changes(bool is_saved)
 {
-	struct parameter_update_s pup;
-	pup.timestamp = hrt_absolute_time();
-	pup.saved = is_saved;
+#if !defined(PARAM_NO_ORB)
+	struct parameter_update_s pup = {
+		.timestamp = hrt_absolute_time(),
+		.saved = is_saved
+	};
 
 	/*
 	 * If we don't have a handle to our topic, create one now; otherwise
@@ -249,6 +263,8 @@ param_notify_changes(bool is_saved)
 	} else {
 		orb_publish(ORB_ID(parameter_update), param_topic, &pup);
 	}
+
+#endif
 }
 
 param_t
@@ -436,6 +452,7 @@ param_size(param_t param)
 	return 0;
 }
 
+
 /**
  * Obtain a pointer to the storage allocated for a parameter.
  *
@@ -583,6 +600,19 @@ out:
 	return result;
 }
 
+#if defined(FLASH_BASED_PARAMS)
+int param_set_external(param_t param, const void *val, bool mark_saved, bool notify_changes, bool is_saved)
+{
+	return param_set_internal(param, val, mark_saved, notify_changes, is_saved);
+}
+
+const void *param_get_value_ptr_external(param_t param)
+{
+	return param_get_value_ptr(param);
+}
+
+#endif
+
 int
 param_set(param_t param, const void *val)
 {
@@ -734,6 +764,7 @@ int
 param_save_default(void)
 {
 	int res;
+#if !defined(FLASH_BASED_PARAMS)
 	int fd;
 
 	const char *filename = param_get_default_file();
@@ -759,6 +790,9 @@ param_save_default(void)
 	}
 
 	PARAM_CLOSE(fd);
+#else
+	res = flash_param_save();
+#endif
 
 	return res;
 }
diff --git a/src/platforms/px4_defines.h b/src/platforms/px4_defines.h
index 1861d27ede..b41dc7b0eb 100644
--- a/src/platforms/px4_defines.h
+++ b/src/platforms/px4_defines.h
@@ -120,6 +120,10 @@ typedef param_t px4_param_t;
 #define PRId64 "lld"
 #endif
 
+#if !defined(offsetof)
+#  define offsetof(TYPE, MEMBER) __builtin_offsetof (TYPE, MEMBER)
+#endif
+
 /*
  * POSIX Specific defines
  */
diff --git a/src/systemcmds/param/param.c b/src/systemcmds/param/param.c
index ea698ae5a1..034c342597 100644
--- a/src/systemcmds/param/param.c
+++ b/src/systemcmds/param/param.c
@@ -55,6 +55,9 @@
 
 #include "systemlib/systemlib.h"
 #include "systemlib/param/param.h"
+#if defined(FLASH_BASED_PARAMS)
+# include "systemlib/flashparams/flashparams.h"
+#endif
 #include "systemlib/err.h"
 
 __EXPORT int param_main(int argc, char *argv[]);
@@ -65,6 +68,7 @@ enum COMPARE_OPERATOR {
 };
 
 static int 	do_save(const char *param_file_name);
+static int	do_save_default(void);
 static int 	do_load(const char *param_file_name);
 static int	do_import(const char *param_file_name);
 static int	do_show(const char *search_string);
@@ -84,7 +88,7 @@ param_main(int argc, char *argv[])
 				return do_save(argv[2]);
 
 			} else {
-				if (param_save_default()) {
+				if (do_save_default()) {
 					warnx("Param export failed.");
 					return 1;
 
@@ -213,6 +217,35 @@ param_main(int argc, char *argv[])
 	return 1;
 }
 
+#if defined(FLASH_BASED_PARAMS)
+/* If flash based parameters are uses we call out
+ * to the following set of flash routines
+ */
+static int
+
+do_save(const char *param_file_name)
+{
+	return flash_param_save();
+}
+static int
+do_save_default(void)
+{
+	return flash_param_save_default();
+}
+
+static int
+do_load(const char *param_file_name)
+{
+	return flash_param_load();
+}
+
+static int
+do_import(const char *param_file_name)
+{
+	return flash_param_import();
+}
+#else
+
 static int
 do_save(const char *param_file_name)
 {
@@ -238,6 +271,12 @@ do_save(const char *param_file_name)
 	return 0;
 }
 
+static int
+do_save_default(void)
+{
+	return param_save_default();
+}
+
 static int
 do_load(const char *param_file_name)
 {
@@ -279,6 +318,7 @@ do_import(const char *param_file_name)
 
 	return 0;
 }
+#endif
 
 static int
 do_show(const char *search_string)

From 8d13dba0ccad7740de7a6e555e459e3f04040e37 Mon Sep 17 00:00:00 2001
From: David Sidrane <david_s5@nscdg.com>
Date: Tue, 12 Jul 2016 14:39:28 -1000
Subject: [PATCH 1016/1230] Update NuttX with backport of stm32 FLASH driver to
 support F4 ==master_flash

---
 NuttX | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/NuttX b/NuttX
index f0f4bdc872..9526676011 160000
--- a/NuttX
+++ b/NuttX
@@ -1 +1 @@
-Subproject commit f0f4bdc872d324f64e9d93d6f8989d3c1dfa2633
+Subproject commit 9526676011e82b3762914063794dfd9ec9ec5b87

From 5d1bd6fb2c29923b93523e8a38642af361fb7f5f Mon Sep 17 00:00:00 2001
From: David Sidrane <david_s5@nscdg.com>
Date: Tue, 12 Jul 2016 14:43:31 -1000
Subject: [PATCH 1017/1230] TAP used FLASH Based parameter storage - needs
 timing eval

---
 src/drivers/boards/tap-v1/board_config.h |  3 +++
 src/drivers/boards/tap-v1/tap_init.c     | 25 ++++++++++++++++++++++++
 2 files changed, 28 insertions(+)

diff --git a/src/drivers/boards/tap-v1/board_config.h b/src/drivers/boards/tap-v1/board_config.h
index 654b70a45d..87291d3ad8 100644
--- a/src/drivers/boards/tap-v1/board_config.h
+++ b/src/drivers/boards/tap-v1/board_config.h
@@ -236,6 +236,9 @@ __BEGIN_DECLS
 #define KEY_AD_GPIO    (GPIO_INPUT|GPIO_PULLDOWN|GPIO_EXTI|GPIO_PORTC|GPIO_PIN1)
 #define POWER_ON_GPIO  (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTA|GPIO_PIN4)
 #define POWER_OFF_GPIO (GPIO_INPUT|GPIO_PULLDOWN|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTA|GPIO_PIN4)
+
+
+#define  FLASH_BASED_PARAMS
 /****************************************************************************************************
  * Public Types
  ****************************************************************************************************/
diff --git a/src/drivers/boards/tap-v1/tap_init.c b/src/drivers/boards/tap-v1/tap_init.c
index 648e465beb..86773fccb5 100644
--- a/src/drivers/boards/tap-v1/tap_init.c
+++ b/src/drivers/boards/tap-v1/tap_init.c
@@ -57,6 +57,8 @@
 #include <nuttx/i2c.h>
 #include <nuttx/analog/adc.h>
 
+
+
 #include "stm32.h"
 #include "board_config.h"
 #include "stm32_uart.h"
@@ -68,6 +70,10 @@
 
 #include <systemlib/cpuload.h>
 
+# if defined(FLASH_BASED_PARAMS)
+#  include <systemlib/flashparams/flashfs.h>
+#endif
+
 /****************************************************************************
  * Pre-Processor Definitions
  ****************************************************************************/
@@ -179,6 +185,25 @@ __EXPORT int nsh_archinitialize(void)
 	led_off(LED_AMBER);
 	led_off(LED_BLUE);
 
+#if defined(FLASH_BASED_PARAMS)
+	static sector_descriptor_t  sector_map[] = {
+		{1, 16 * 1024, 0x08004000},
+		{2, 16 * 1024, 0x08008000},
+		{0, 0, 0},
+	};
+
+	/* Initalizee the flashfs layer to use heap allocated memory */
+
+	result = parameter_flashfs_init(sector_map, NULL, 0);
+
+	if (result != OK) {
+		message("[boot] FAILED to init params in FLASH %d\n", result);
+		up_ledon(LED_AMBER);
+		return -ENODEV;
+	}
+
+#endif
+
 	/* Init the microSD slot */
 
 	result = board_sdio_initialize();

From 8a2df2a4580f2a0683ad4ee33511d7d79f323296 Mon Sep 17 00:00:00 2001
From: David Sidrane <david_s5@nscdg.com>
Date: Wed, 13 Jul 2016 04:11:08 -1000
Subject: [PATCH 1018/1230] Avoid unnecessary Write Cycle

---
 .../systemlib/flashparams/flashparams.c       | 20 ++++++++++++++++---
 1 file changed, 17 insertions(+), 3 deletions(-)

diff --git a/src/modules/systemlib/flashparams/flashparams.c b/src/modules/systemlib/flashparams/flashparams.c
index 623715b635..fa474dffdf 100644
--- a/src/modules/systemlib/flashparams/flashparams.c
+++ b/src/modules/systemlib/flashparams/flashparams.c
@@ -183,11 +183,25 @@ out:
 
 		if (result == OK) {
 
+			/* Check for a write that has no changes */
+
+			uint8_t *was_buffer;
+			size_t was_buf_size;
+			int was_result = parameter_flashfs_read(parameters_token, &was_buffer, &was_buf_size);
+
 			void *enc_buff = bson_encoder_buf_data(&encoder);
-			memcpy(buffer, enc_buff, buf_size);
+
+			bool commit = was_result < OK || was_buf_size != buf_size || 0 != memcmp(was_buffer, enc_buff, was_buf_size);
+
+			if (commit) {
+
+				memcpy(buffer, enc_buff, buf_size);
+				result = parameter_flashfs_write(parameters_token, buffer, buf_size);
+				result = result == buf_size ? OK : -EFBIG;
+
+			}
+
 			free(enc_buff);
-			result = parameter_flashfs_write(parameters_token, buffer, buf_size);
-			result = result == buf_size ? OK : -EFBIG;
 		}
 	}
 

From f1b8bed5dfefc5a1a8c4b55b4eedf055b1d20bfa Mon Sep 17 00:00:00 2001
From: Julian Oes <julian@oes.ch>
Date: Thu, 14 Jul 2016 13:13:56 +0200
Subject: [PATCH 1019/1230] gyro_calibration: set scale to 1 instead of 0

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

diff --git a/src/modules/commander/gyro_calibration.cpp b/src/modules/commander/gyro_calibration.cpp
index 3b0e6815df..33fe04d606 100644
--- a/src/modules/commander/gyro_calibration.cpp
+++ b/src/modules/commander/gyro_calibration.cpp
@@ -142,6 +142,9 @@ static calibrate_return gyro_calibration_worker(int cancel_sub, void* data)
 			return calibrate_return_error;
 		}
 
+		worker_data->gyro_scale[s].x_scale = 1.0f;
+		worker_data->gyro_scale[s].y_scale = 1.0f;
+		worker_data->gyro_scale[s].z_scale = 1.0f;
 		worker_data->gyro_scale[s].x_offset /= calibration_counter[s];
 		worker_data->gyro_scale[s].y_offset /= calibration_counter[s];
 		worker_data->gyro_scale[s].z_offset /= calibration_counter[s];

From 4a199c1360d9548871d686b8ac7959cb358eda64 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Beat=20K=C3=BCng?= <beat-kueng@gmx.net>
Date: Thu, 14 Jul 2016 14:17:38 +0200
Subject: [PATCH 1020/1230] gyro calibration: avoid double initialization of
 gyro_scale data

They're initialized in do_gyro_calibration already
---
 src/modules/commander/gyro_calibration.cpp | 4 ----
 1 file changed, 4 deletions(-)

diff --git a/src/modules/commander/gyro_calibration.cpp b/src/modules/commander/gyro_calibration.cpp
index 33fe04d606..9a48866ab5 100644
--- a/src/modules/commander/gyro_calibration.cpp
+++ b/src/modules/commander/gyro_calibration.cpp
@@ -92,7 +92,6 @@ static calibrate_return gyro_calibration_worker(int cancel_sub, void* data)
 	}
 
 	memset(&worker_data->gyro_report_0, 0, sizeof(worker_data->gyro_report_0));
-	memset(&worker_data->gyro_scale, 0, sizeof(worker_data->gyro_scale));
 
 	/* use first gyro to pace, but count correctly per-gyro for statistics */
 	while (calibration_counter[0] < calibration_count) {
@@ -142,9 +141,6 @@ static calibrate_return gyro_calibration_worker(int cancel_sub, void* data)
 			return calibrate_return_error;
 		}
 
-		worker_data->gyro_scale[s].x_scale = 1.0f;
-		worker_data->gyro_scale[s].y_scale = 1.0f;
-		worker_data->gyro_scale[s].z_scale = 1.0f;
 		worker_data->gyro_scale[s].x_offset /= calibration_counter[s];
 		worker_data->gyro_scale[s].y_offset /= calibration_counter[s];
 		worker_data->gyro_scale[s].z_offset /= calibration_counter[s];

From e2a6ae676f10164fb1cee1fc927ceee666afea8d Mon Sep 17 00:00:00 2001
From: David Sidrane <david_s5@nscdg.com>
Date: Thu, 14 Jul 2016 07:32:11 -1000
Subject: [PATCH 1021/1230] More complete IO init (#5065)

---
 cmake/configs/nuttx_tap-v1_default.cmake |  1 +
 src/drivers/boards/tap-v1/board_config.h |  9 ++++++++-
 src/drivers/boards/tap-v1/tap_init.c     | 20 ++++++++++++++++++--
 3 files changed, 27 insertions(+), 3 deletions(-)

diff --git a/cmake/configs/nuttx_tap-v1_default.cmake b/cmake/configs/nuttx_tap-v1_default.cmake
index d4753fdaca..6ce1ae215c 100644
--- a/cmake/configs/nuttx_tap-v1_default.cmake
+++ b/cmake/configs/nuttx_tap-v1_default.cmake
@@ -14,6 +14,7 @@ set(config_module_list
 	drivers/px4fmu
 	drivers/boards/tap-v1
 	drivers/rgbled_pwm
+	drivers/tap_esc
 	#drivers/mpu6500
 	drivers/ms5611
 	drivers/gps
diff --git a/src/drivers/boards/tap-v1/board_config.h b/src/drivers/boards/tap-v1/board_config.h
index 87291d3ad8..b31935e258 100644
--- a/src/drivers/boards/tap-v1/board_config.h
+++ b/src/drivers/boards/tap-v1/board_config.h
@@ -235,8 +235,15 @@ __BEGIN_DECLS
 #define MS_PWR_BUTTON_DOWN 750
 #define KEY_AD_GPIO    (GPIO_INPUT|GPIO_PULLDOWN|GPIO_EXTI|GPIO_PORTC|GPIO_PIN1)
 #define POWER_ON_GPIO  (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTA|GPIO_PIN4)
-#define POWER_OFF_GPIO (GPIO_INPUT|GPIO_PULLDOWN|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTA|GPIO_PIN4)
+#define POWER_OFF_GPIO (GPIO_INPUT|GPIO_PULLDOWN|GPIO_PORTA|GPIO_PIN4)
 
+#define GPIO_S0  (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTC|GPIO_PIN15)
+#define GPIO_S1  (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTC|GPIO_PIN14)
+#define GPIO_S2  (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTC|GPIO_PIN13)
+
+#define GPIO_PCON_RADIO (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTC|GPIO_PIN3)
+
+#define RF_RADIO_CONTOL(_on_true)	px4_arch_gpiowrite(GPIO_PCON_RADIO, !(_on_true))
 
 #define  FLASH_BASED_PARAMS
 /****************************************************************************************************
diff --git a/src/drivers/boards/tap-v1/tap_init.c b/src/drivers/boards/tap-v1/tap_init.c
index 86773fccb5..238963c941 100644
--- a/src/drivers/boards/tap-v1/tap_init.c
+++ b/src/drivers/boards/tap-v1/tap_init.c
@@ -129,15 +129,31 @@ __END_DECLS
 
 __EXPORT void stm32_boardinitialize(void)
 {
-	/* configure always-on ADC pins */
-	stm32_configgpio(GPIO_ADC1_IN10);
+	/* Hold power state */
 
 	board_pwr_init(0);
 
+	/* Select 0 */
+
+	stm32_configgpio(GPIO_S0);
+	stm32_configgpio(GPIO_S1);
+	stm32_configgpio(GPIO_S2);
+
+	/* Radio Off (active low, init is set) */
+
+	stm32_configgpio(GPIO_PCON_RADIO);
+
+	/* configure always-on ADC pins */
+
+	stm32_configgpio(GPIO_ADC1_IN10);
+
+
 	/* configure SPI interfaces */
+
 	stm32_spiinitialize();
 
 	/* configure LEDs (empty call to NuttX' ledinit) */
+
 	up_ledinit();
 }
 

From 33e4f38d82be48bee72e7b5390d345352788d095 Mon Sep 17 00:00:00 2001
From: David Sidrane <david_s5@nscdg.com>
Date: Thu, 14 Jul 2016 08:26:40 -1000
Subject: [PATCH 1022/1230] Add GPS and Compass (#5066)

---
 ROMFS/tap_common/init.d/rcS              | 2 ++
 cmake/configs/nuttx_tap-v1_default.cmake | 1 +
 src/drivers/boards/tap-v1/board_config.h | 2 ++
 3 files changed, 5 insertions(+)

diff --git a/ROMFS/tap_common/init.d/rcS b/ROMFS/tap_common/init.d/rcS
index 373ac6d090..a68c78e6fd 100644
--- a/ROMFS/tap_common/init.d/rcS
+++ b/ROMFS/tap_common/init.d/rcS
@@ -154,6 +154,8 @@ then
 		fi
 	fi
 
+	gps start -d /dev/ttyS0
+
 	# waypoint storage
 	# REBOOTWORK this needs to start in parallel
 	if dataman start
diff --git a/cmake/configs/nuttx_tap-v1_default.cmake b/cmake/configs/nuttx_tap-v1_default.cmake
index 6ce1ae215c..727ba5712f 100644
--- a/cmake/configs/nuttx_tap-v1_default.cmake
+++ b/cmake/configs/nuttx_tap-v1_default.cmake
@@ -17,6 +17,7 @@ set(config_module_list
 	drivers/tap_esc
 	#drivers/mpu6500
 	drivers/ms5611
+	drivers/hmc5883
 	drivers/gps
 	drivers/airspeed
 	drivers/meas_airspeed
diff --git a/src/drivers/boards/tap-v1/board_config.h b/src/drivers/boards/tap-v1/board_config.h
index b31935e258..a5f377ef91 100644
--- a/src/drivers/boards/tap-v1/board_config.h
+++ b/src/drivers/boards/tap-v1/board_config.h
@@ -106,6 +106,8 @@ __BEGIN_DECLS
 #define PX4_I2C_BUS_SONAR      2
 #define PX4_I2C_BUS_EXPANSION  3
 
+#define PX4_I2C_OBDEV_HMC5883	0x1e
+
 /*
  * Devices on the onboard bus.
  *

From 1cfa429efee002690a92b162fd1957587a08b621 Mon Sep 17 00:00:00 2001
From: Julian Oes <julian@oes.ch>
Date: Wed, 13 Jul 2016 17:19:59 +0200
Subject: [PATCH 1023/1230] DriverFramework: update submodule

Small additions for Intel Edison.
---
 src/lib/DriverFramework | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/src/lib/DriverFramework b/src/lib/DriverFramework
index db5a1f91aa..dab303d799 160000
--- a/src/lib/DriverFramework
+++ b/src/lib/DriverFramework
@@ -1 +1 @@
-Subproject commit db5a1f91aab361dbde1fbbc75b792080745832d4
+Subproject commit dab303d7992c9a3bbc39509b13eddf2b65d382b8

From f430c39f5b661ff869f052a3927ae55f5ef6dc35 Mon Sep 17 00:00:00 2001
From: Julian Oes <julian@oes.ch>
Date: Wed, 13 Jul 2016 20:09:33 +0200
Subject: [PATCH 1024/1230] commander: allow POSCTL with local position

We want to allow flying POSCTL with optical flow only without GPS.
---
 src/modules/commander/state_machine_helper.cpp | 3 ++-
 1 file changed, 2 insertions(+), 1 deletion(-)

diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp
index 3d56545b4e..a631a1a3e5 100644
--- a/src/modules/commander/state_machine_helper.cpp
+++ b/src/modules/commander/state_machine_helper.cpp
@@ -674,7 +674,8 @@ bool set_nav_state(struct vehicle_status_s *status, struct commander_state_s *in
 				}
 
 			/* As long as there is RC, we can fallback to ALTCTL, or STAB. */
-			} else if (status_flags->gps_failure && armed) {
+			/* A local position estimate is enough for POSCTL, this enables POSCTL using e.g. flow. */
+			} else if (!status_flags->condition_local_position_valid && armed) {
 				status->failsafe = true;
 
 				if (status_flags->condition_local_altitude_valid) {

From 87e964ec10e6d4cdb8c90b905944e7c158158074 Mon Sep 17 00:00:00 2001
From: Julian Oes <julian@oes.ch>
Date: Thu, 14 Jul 2016 09:53:41 +0200
Subject: [PATCH 1025/1230] commander: POSCTL with localpos for MC

Fixedwings need a global position estimate for POSCTL.
---
 src/modules/commander/state_machine_helper.cpp | 8 ++++++--
 1 file changed, 6 insertions(+), 2 deletions(-)

diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp
index a631a1a3e5..483f52cc08 100644
--- a/src/modules/commander/state_machine_helper.cpp
+++ b/src/modules/commander/state_machine_helper.cpp
@@ -674,8 +674,12 @@ bool set_nav_state(struct vehicle_status_s *status, struct commander_state_s *in
 				}
 
 			/* As long as there is RC, we can fallback to ALTCTL, or STAB. */
-			/* A local position estimate is enough for POSCTL, this enables POSCTL using e.g. flow. */
-			} else if (!status_flags->condition_local_position_valid && armed) {
+			/* A local position estimate is enough for POSCTL for multirotors,
+			 * this enables POSCTL using e.g. flow.
+			 * For fixedwing, a global position is needed. */
+			} else if (((status->is_rotary_wing && !status_flags->condition_local_position_valid) ||
+				   (!status->is_rotary_wing && !status_flags->condition_global_position_valid))
+				   && armed) {
 				status->failsafe = true;
 
 				if (status_flags->condition_local_altitude_valid) {

From b48c081e5cddfa79659aeb13f062b92e54350abb Mon Sep 17 00:00:00 2001
From: lovettchris <clovett@microsoft.com>
Date: Fri, 15 Jul 2016 14:59:30 -0700
Subject: [PATCH 1026/1230] Reset _transfer_in_progress if mavlink transfer
 times out. (#5077)

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

diff --git a/src/modules/mavlink/mavlink_mission.cpp b/src/modules/mavlink/mavlink_mission.cpp
index 832bb4dc52..976076e73e 100644
--- a/src/modules/mavlink/mavlink_mission.cpp
+++ b/src/modules/mavlink/mavlink_mission.cpp
@@ -346,6 +346,9 @@ MavlinkMissionManager::send(const hrt_abstime now)
 
 		_state = MAVLINK_WPM_STATE_IDLE;
 
+		// since we are giving up, reset this state also, so another request can be started.
+		_transfer_in_progress = false;
+
 	} else if (_state == MAVLINK_WPM_STATE_GETLIST && hrt_elapsed_time(&_time_last_sent) > _retry_timeout) {
 		/* try to request item again after timeout */
 		send_mission_request(_transfer_partner_sysid, _transfer_partner_compid, _transfer_seq);

From ee58f0d11dd96624a2c3d4ffedda9eb1a1754a71 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Beat=20K=C3=BCng?= <beat-kueng@gmx.net>
Date: Fri, 15 Jul 2016 23:59:52 +0200
Subject: [PATCH 1027/1230] encoders.msg: remove this topic, it's never
 published (#5074)

---
 msg/CMakeLists.txt                |  1 -
 msg/encoders.msg                  |  4 ----
 src/modules/sdlog2/sdlog2.c       | 14 --------------
 src/modules/uORB/Publication.cpp  |  2 --
 src/modules/uORB/Subscription.cpp |  2 --
 5 files changed, 23 deletions(-)
 delete mode 100644 msg/encoders.msg

diff --git a/msg/CMakeLists.txt b/msg/CMakeLists.txt
index 5a94a5ba54..278874c089 100644
--- a/msg/CMakeLists.txt
+++ b/msg/CMakeLists.txt
@@ -49,7 +49,6 @@ set(msg_file_names
 	distance_sensor.msg
 	ekf2_innovations.msg
 	ekf2_replay.msg
-	encoders.msg
 	esc_report.msg
 	esc_status.msg
 	estimator_status.msg
diff --git a/msg/encoders.msg b/msg/encoders.msg
deleted file mode 100644
index ef863a9cba..0000000000
--- a/msg/encoders.msg
+++ /dev/null
@@ -1,4 +0,0 @@
-uint8 NUM_ENCODERS = 4
-
-int64[4] counts			# counts of encoder
-float32[4] velocity		# counts of encoder/ second
diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c
index 7310db1341..f1fc820d81 100644
--- a/src/modules/sdlog2/sdlog2.c
+++ b/src/modules/sdlog2/sdlog2.c
@@ -102,7 +102,6 @@
 #include <uORB/topics/system_power.h>
 #include <uORB/topics/servorail_status.h>
 #include <uORB/topics/wind_estimate.h>
-#include <uORB/topics/encoders.h>
 #include <uORB/topics/vtol_vehicle_status.h>
 #include <uORB/topics/time_offset.h>
 #include <uORB/topics/mc_att_ctrl_status.h>
@@ -1196,7 +1195,6 @@ int sdlog2_thread_main(int argc, char *argv[])
 		struct servorail_status_s servorail_status;
 		struct satellite_info_s sat_info;
 		struct wind_estimate_s wind_estimate;
-		struct encoders_s encoders;
 		struct vtol_vehicle_status_s vtol_status;
 		struct time_offset_s time_offset;
 		struct mc_att_ctrl_status_s mc_att_ctrl_status;
@@ -1308,7 +1306,6 @@ int sdlog2_thread_main(int argc, char *argv[])
 		int system_power_sub;
 		int servorail_status_sub;
 		int wind_sub;
-		int encoders_sub;
 		int tsync_sub;
 		int mc_att_ctrl_status_sub;
 		int ctrl_state_sub;
@@ -1354,7 +1351,6 @@ int sdlog2_thread_main(int argc, char *argv[])
 	subs.tsync_sub = -1;
 	subs.mc_att_ctrl_status_sub = -1;
 	subs.ctrl_state_sub = -1;
-	subs.encoders_sub = -1;
 	subs.innov_sub = -1;
 	subs.cam_trig_sub = -1;
 	subs.replay_sub = -1;
@@ -2215,16 +2211,6 @@ int sdlog2_thread_main(int argc, char *argv[])
 				LOGBUFFER_WRITE_AND_COUNT(WIND);
 			}
 
-			/* --- ENCODERS --- */
-			if (copy_if_updated(ORB_ID(encoders), &subs.encoders_sub, &buf.encoders)) {
-				log_msg.msg_type = LOG_ENCD_MSG;
-				log_msg.body.log_ENCD.cnt0 = buf.encoders.counts[0];
-				log_msg.body.log_ENCD.vel0 = buf.encoders.velocity[0];
-				log_msg.body.log_ENCD.cnt1 = buf.encoders.counts[1];
-				log_msg.body.log_ENCD.vel1 = buf.encoders.velocity[1];
-				LOGBUFFER_WRITE_AND_COUNT(ENCD);
-			}
-
 			/* --- TIMESYNC OFFSET --- */
 			if (copy_if_updated(ORB_ID(time_offset), &subs.tsync_sub, &buf.time_offset)) {
 				log_msg.msg_type = LOG_TSYN_MSG;
diff --git a/src/modules/uORB/Publication.cpp b/src/modules/uORB/Publication.cpp
index 9990d8d543..3282f5c749 100644
--- a/src/modules/uORB/Publication.cpp
+++ b/src/modules/uORB/Publication.cpp
@@ -47,7 +47,6 @@
 #include "topics/vehicle_rates_setpoint.h"
 #include "topics/actuator_outputs.h"
 #include "topics/actuator_direct.h"
-#include "topics/encoders.h"
 #include "topics/tecs_status.h"
 #include "topics/rc_channels.h"
 #include "topics/filtered_bottom_flow.h"
@@ -118,7 +117,6 @@ template class __EXPORT Publication<vehicle_attitude_setpoint_s>;
 template class __EXPORT Publication<vehicle_rates_setpoint_s>;
 template class __EXPORT Publication<actuator_outputs_s>;
 template class __EXPORT Publication<actuator_direct_s>;
-template class __EXPORT Publication<encoders_s>;
 template class __EXPORT Publication<tecs_status_s>;
 template class __EXPORT Publication<rc_channels_s>;
 template class __EXPORT Publication<filtered_bottom_flow_s>;
diff --git a/src/modules/uORB/Subscription.cpp b/src/modules/uORB/Subscription.cpp
index 7ddf234848..b01efef7f1 100644
--- a/src/modules/uORB/Subscription.cpp
+++ b/src/modules/uORB/Subscription.cpp
@@ -45,7 +45,6 @@
 #include "topics/hil_sensor.h"
 #include "topics/vehicle_attitude.h"
 #include "topics/vehicle_global_position.h"
-#include "topics/encoders.h"
 #include "topics/position_setpoint_triplet.h"
 #include "topics/vehicle_status.h"
 #include "topics/manual_control_setpoint.h"
@@ -162,7 +161,6 @@ template class __EXPORT Subscription<sensor_combined_s>;
 template class __EXPORT Subscription<hil_sensor_s>;
 template class __EXPORT Subscription<vehicle_attitude_s>;
 template class __EXPORT Subscription<vehicle_global_position_s>;
-template class __EXPORT Subscription<encoders_s>;
 template class __EXPORT Subscription<position_setpoint_triplet_s>;
 template class __EXPORT Subscription<vehicle_status_s>;
 template class __EXPORT Subscription<manual_control_setpoint_s>;

From a1b710025b9c686b4ce4ccb6e96889239fe8f122 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Beat=20K=C3=BCng?= <beat-kueng@gmx.net>
Date: Sat, 16 Jul 2016 01:55:32 +0200
Subject: [PATCH 1028/1230] Improve new logger update rate (#5073)

* logger: disable some default topics, which are most likely not used

This is also to safe CPU and lower the amount of file descriptors used.

* logger: use the hrt timer for more accurate scheduling

Under NuttX with the default rate of 285Hz, the actual measured rate was
only 200Hz while on Linux it was ~280Hz. The reason is that NuttX only
uses a usleep() granularity of 1ms, so that the typical sleep time is
longer than what we set.

Now the logger waits on a semaphore, which gets activated periodically
with a hrt timer. With this the measured rate is exactly the expected one,
285Hz.
---
 src/modules/logger/logger.cpp | 36 +++++++++++++++++++++++++++--------
 src/modules/logger/logger.h   |  2 +-
 2 files changed, 29 insertions(+), 9 deletions(-)

diff --git a/src/modules/logger/logger.cpp b/src/modules/logger/logger.cpp
index 322a99021f..034580d99b 100644
--- a/src/modules/logger/logger.cpp
+++ b/src/modules/logger/logger.cpp
@@ -48,9 +48,11 @@
 #include <uORB/topics/vehicle_status.h>
 #include <uORB/topics/vehicle_gps_position.h>
 
+#include <drivers/drv_hrt.h>
 #include <px4_includes.h>
 #include <px4_getopt.h>
 #include <px4_log.h>
+#include <px4_sem.h>
 #include <systemlib/mavlink_log.h>
 #include <replay/definitions.hpp>
 
@@ -83,6 +85,12 @@ static Logger *logger_ptr = nullptr;
 static int logger_task = -1;
 static pthread_t writer_thread;
 
+/* This is used to schedule work for the logger (periodic scan for updated topics) */
+static void timer_callback(void *arg)
+{
+	px4_sem_t *semaphore = (px4_sem_t *)arg;
+	px4_sem_post(semaphore);
+}
 
 
 int logger_main(int argc, char *argv[])
@@ -236,7 +244,7 @@ void Logger::print_statistics()
 
 void Logger::run_trampoline(int argc, char *argv[])
 {
-	unsigned log_interval = 3500;
+	uint32_t log_interval = 3500;
 	int log_buffer_size = 12 * 1024;
 	bool log_on_start = false;
 	bool log_until_shutdown = false;
@@ -327,7 +335,7 @@ void Logger::run_trampoline(int argc, char *argv[])
 }
 
 
-Logger::Logger(size_t buffer_size, unsigned log_interval, bool log_on_start,
+Logger::Logger(size_t buffer_size, uint32_t log_interval, bool log_on_start,
 	       bool log_until_shutdown, bool log_name_timestamp) :
 	_arm_override(false),
 	_log_on_start(log_on_start),
@@ -482,7 +490,6 @@ void Logger::add_default_topics()
 	add_topic("vehicle_global_velocity_setpoint", 100);
 	add_topic("battery_status", 300);
 	add_topic("system_power", 300);
-	add_topic("servorail_status", 300);
 	add_topic("position_setpoint_triplet", 10);
 	add_topic("att_pos_mocap", 50);
 	add_topic("vision_position_estimate", 50);
@@ -495,9 +502,6 @@ void Logger::add_default_topics()
 	add_topic("ekf2_innovations", 20);
 	add_topic("tecs_status", 20);
 	add_topic("wind_estimate", 100);
-	add_topic("encoders", 50);
-	add_topic("time_offset", 1000);
-	add_topic("mc_att_ctrl_status", 50);
 	add_topic("control_state", 20);
 	add_topic("camera_trigger");
 	add_topic("cpuload");
@@ -646,7 +650,13 @@ void Logger::run()
 		start_log();
 	}
 
-	/* every log_interval usec, check for orb updates */
+	/* init the update timer */
+	struct hrt_call timer_call;
+	px4_sem_t timer_semaphore;
+	px4_sem_init(&timer_semaphore, 0, 0);
+	hrt_call_every(&timer_call, _log_interval, _log_interval, timer_callback, &timer_semaphore);
+
+
 	while (!_task_should_exit) {
 
 		// Start/stop logging when system arm/disarm
@@ -776,9 +786,19 @@ void Logger::run()
 
 		}
 
-		usleep(_log_interval);
+		/*
+		 * We wait on the semaphore, which periodically gets updated by a high-resolution timer.
+		 * The simpler alternative would be:
+		 *   usleep(max(300, _log_interval - elapsed_time_since_loop_start));
+		 * And on linux this is quite accurate as well, but under NuttX it is not accurate,
+		 * because usleep() has only a granularity of CONFIG_MSEC_PER_TICK (=1ms).
+		 */
+		while (px4_sem_wait(&timer_semaphore) != 0);
 	}
 
+	hrt_cancel(&timer_call);
+	px4_sem_destroy(&timer_semaphore);
+
 	// stop the writer thread
 	_writer.thread_stop();
 
diff --git a/src/modules/logger/logger.h b/src/modules/logger/logger.h
index 6270837ec6..eb11ed56bd 100644
--- a/src/modules/logger/logger.h
+++ b/src/modules/logger/logger.h
@@ -75,7 +75,7 @@ struct LoggerSubscription {
 class Logger
 {
 public:
-	Logger(size_t buffer_size, unsigned log_interval, bool log_on_start,
+	Logger(size_t buffer_size, uint32_t log_interval, bool log_on_start,
 	       bool log_until_shutdown, bool log_name_timestamp);
 
 	~Logger();

From d733fc9139ba575e59e054878cabb96ee6950d7a Mon Sep 17 00:00:00 2001
From: Daniel Agar <daniel@agar.ca>
Date: Sat, 16 Jul 2016 08:41:19 -0400
Subject: [PATCH 1029/1230] travis-ci use tagged docker image (#5068)

---
 .travis.yml | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/.travis.yml b/.travis.yml
index 50e56d71a8..f0abcb58b1 100644
--- a/.travis.yml
+++ b/.travis.yml
@@ -8,7 +8,7 @@ matrix:
   include:
     - os: linux
       sudo: required
-      env: GCC_VER=4.8 DOCKER_REPO="px4io/px4-dev-base"
+      env: GCC_VER=4.8 DOCKER_REPO="px4io/px4-dev-base:2016-07-14"
       services:
         - docker
     - os: osx

From f241518d0e00dc0ad162810dc3b8c7cdb3caebd5 Mon Sep 17 00:00:00 2001
From: Julian Oes <julian@oes.ch>
Date: Thu, 14 Jul 2016 13:46:15 +0200
Subject: [PATCH 1030/1230] RPi2: bring default and release cmake in sync

The two cmake files for default and release where out of
sync. This change bases them on a common cmake file.
---
 cmake/configs/posix_rpi2_common.cmake  | 112 +++++++++++++++++++++++++
 cmake/configs/posix_rpi2_default.cmake |  49 +----------
 cmake/configs/posix_rpi2_release.cmake | 105 +----------------------
 3 files changed, 114 insertions(+), 152 deletions(-)
 create mode 100644 cmake/configs/posix_rpi2_common.cmake

diff --git a/cmake/configs/posix_rpi2_common.cmake b/cmake/configs/posix_rpi2_common.cmake
new file mode 100644
index 0000000000..95477cd8c0
--- /dev/null
+++ b/cmake/configs/posix_rpi2_common.cmake
@@ -0,0 +1,112 @@
+# This file is shared between posix_rpi2_default.cmake
+# and posix_rpi2_release.cmake.
+
+include(posix/px4_impl_posix)
+
+# This definition allows to differentiate if this just the usual POSIX build
+# or if it is for the RPi.
+add_definitions(
+	-D__PX4_POSIX_RPI2
+	-D__LINUX
+)
+
+
+set(config_module_list
+	#
+	# Board support modules
+	#
+	drivers/device
+	modules/sensors
+	platforms/posix/drivers/df_mpu9250_wrapper
+	platforms/posix/drivers/df_lsm9ds1_wrapper
+	platforms/posix/drivers/df_ms5611_wrapper
+	platforms/posix/drivers/df_hmc5883_wrapper
+	platforms/posix/drivers/df_trone_wrapper
+	platforms/posix/drivers/df_isl29501_wrapper
+
+	#
+	# System commands
+	#
+	systemcmds/param
+	systemcmds/mixer
+	systemcmds/ver
+	systemcmds/esc_calib
+	systemcmds/reboot
+	systemcmds/topic_listener
+	systemcmds/perf
+
+	#
+	# Estimation modules (EKF/ SO3 / other filters)
+	#
+	#modules/attitude_estimator_ekf
+	modules/ekf_att_pos_estimator
+	modules/attitude_estimator_q
+	modules/position_estimator_inav
+	modules/local_position_estimator
+	modules/ekf2
+
+	#
+	# Vehicle Control
+	#
+	modules/mc_att_control
+	modules/mc_pos_control
+	modules/fw_att_control
+	modules/fw_pos_control_l1
+	modules/vtol_att_control
+
+	#
+	# Library modules
+	#
+	modules/sdlog2
+	modules/logger
+	modules/commander
+	modules/load_mon
+	modules/param
+	modules/systemlib
+	modules/systemlib/mixer
+	modules/uORB
+	modules/dataman
+	modules/land_detector
+	modules/navigator
+	modules/mavlink
+
+	#
+	# PX4 drivers
+	#
+
+	#
+	# Libraries
+	#
+	lib/controllib
+	lib/mathlib
+	lib/mathlib/math/filter
+	lib/geo
+	lib/ecl
+	lib/geo_lookup
+	lib/launchdetection
+	lib/external_lgpl
+	lib/conversion
+	lib/terrain_estimation
+	lib/runway_takeoff
+	lib/tailsitter_recovery
+	lib/DriverFramework/framework
+
+	#
+	# POSIX
+	#
+	platforms/common
+	platforms/posix/px4_layer
+	platforms/posix/work_queue
+)
+
+#
+# DriverFramework driver
+#
+set(config_df_driver_list
+	mpu9250
+	lsm9ds1
+	ms5611
+	hmc5883
+	trone
+	isl29501
+)
diff --git a/cmake/configs/posix_rpi2_default.cmake b/cmake/configs/posix_rpi2_default.cmake
index ca677261be..9b0f45c718 100644
--- a/cmake/configs/posix_rpi2_default.cmake
+++ b/cmake/configs/posix_rpi2_default.cmake
@@ -1,50 +1,3 @@
-include(posix/px4_impl_posix)
+include(configs/posix_rpi2_common)
 
 set(CMAKE_TOOLCHAIN_FILE ${CMAKE_SOURCE_DIR}/cmake/toolchains/Toolchain-native.cmake)
-
-set(config_module_list
-	drivers/device
-	platforms/common
-	platforms/posix/px4_layer
-	platforms/posix/work_queue
-	systemcmds/param
-	systemcmds/mixer
-	systemcmds/ver
-	systemcmds/esc_calib
-	systemcmds/reboot
-	systemcmds/topic_listener
-	systemcmds/perf
-	modules/uORB
-	modules/param
-	modules/systemlib
-	modules/systemlib/mixer
-	modules/sensors
-	modules/mavlink
-	modules/attitude_estimator_q
-	modules/position_estimator_inav
-	modules/navigator
-	modules/vtol_att_control
-	modules/mc_pos_control
-	modules/mc_att_control
-	modules/land_detector
-	modules/fw_att_control
-	modules/fw_pos_control_l1
-	modules/dataman
-	modules/sdlog2
-	modules/logger
-	modules/commander
-	modules/load_mon
-	lib/controllib
-	lib/mathlib
-	lib/mathlib/math/filter
-	lib/conversion
-	lib/ecl
-	lib/external_lgpl
-	lib/geo
-	lib/geo_lookup
-	lib/launchdetection
-	lib/terrain_estimation
-	lib/runway_takeoff
-	lib/tailsitter_recovery
-	lib/DriverFramework/framework
-)
diff --git a/cmake/configs/posix_rpi2_release.cmake b/cmake/configs/posix_rpi2_release.cmake
index 0778efa1ff..d17ef5cec3 100644
--- a/cmake/configs/posix_rpi2_release.cmake
+++ b/cmake/configs/posix_rpi2_release.cmake
@@ -1,4 +1,4 @@
-include(posix/px4_impl_posix)
+include(configs/posix_rpi2_common)
 
 set(CMAKE_TOOLCHAIN_FILE ${CMAKE_SOURCE_DIR}/cmake/toolchains/Toolchain-arm-linux-gnueabihf-raspbian.cmake)
 
@@ -6,106 +6,3 @@ set(CMAKE_PROGRAM_PATH
 	"${RPI_TOOLCHAIN_DIR}/gcc-linaro-arm-linux-gnueabihf-raspbian/bin"
 	${CMAKE_PROGRAM_PATH}
 )
-
-# This definition allows to differentiate if this just the usual POSIX build
-# or if it is for the RPi.
-add_definitions(
-	-D__PX4_POSIX_RPI2
-	-D__LINUX
-)
-
-set(config_module_list
-	#
-	# Board support modules
-	#
-	drivers/device
-	modules/sensors
-	platforms/posix/drivers/df_mpu9250_wrapper
-	platforms/posix/drivers/df_lsm9ds1_wrapper
-	platforms/posix/drivers/df_ms5611_wrapper
-	platforms/posix/drivers/df_hmc5883_wrapper
-	platforms/posix/drivers/df_trone_wrapper
-	platforms/posix/drivers/df_isl29501_wrapper
-
-	#
-	# System commands
-	#
-	systemcmds/param
-	systemcmds/mixer
-	systemcmds/ver
-	systemcmds/esc_calib
-	systemcmds/topic_listener
-	systemcmds/perf
-
-	#
-	# Estimation modules (EKF/ SO3 / other filters)
-	#
-	#modules/attitude_estimator_ekf
-	modules/ekf_att_pos_estimator
-	modules/attitude_estimator_q
-	modules/position_estimator_inav
-	modules/local_position_estimator
-	modules/ekf2
-
-	#
-	# Vehicle Control
-	#
-	modules/mc_att_control
-	modules/mc_pos_control
-	modules/fw_att_control
-	modules/fw_pos_control_l1	
-	modules/vtol_att_control
-
-	#
-	# Library modules
-	#
-	modules/sdlog2
-	modules/logger
-	modules/commander
-	modules/load_mon
-	modules/param
-	modules/systemlib
-	modules/systemlib/mixer
-	modules/uORB
-	modules/dataman
-	modules/land_detector
-	modules/navigator
-	modules/mavlink
-
-	#
-	# PX4 drivers
-	#
-
-	#
-	# Libraries
-	#
-	lib/controllib
-	lib/mathlib
-	lib/mathlib/math/filter
-	lib/geo
-	lib/ecl
-	lib/geo_lookup
-	lib/launchdetection
-	lib/external_lgpl
-	lib/conversion
-	lib/terrain_estimation
-	lib/runway_takeoff
-	lib/tailsitter_recovery
-	lib/DriverFramework/framework
-
-	#
-	# POSIX
-	#
-	platforms/common
-	platforms/posix/px4_layer
-	platforms/posix/work_queue
-)
-
-set(config_df_driver_list
-	mpu9250
-	lsm9ds1
-	ms5611
-	hmc5883
-	trone
-	isl29501
-)

From 1939b88a33f91f22be34cd31987185721bb955c2 Mon Sep 17 00:00:00 2001
From: Julian Oes <julian@oes.ch>
Date: Thu, 14 Jul 2016 13:49:44 +0200
Subject: [PATCH 1031/1230] RPi2: use cross/native instead of release/default

---
 Makefile                                           | 14 +++++++-------
 cmake/configs/posix_rpi2_common.cmake              |  4 ++--
 ...x_rpi2_release.cmake => posix_rpi2_cross.cmake} |  0
 ..._rpi2_default.cmake => posix_rpi2_native.cmake} |  0
 4 files changed, 9 insertions(+), 9 deletions(-)
 rename cmake/configs/{posix_rpi2_release.cmake => posix_rpi2_cross.cmake} (100%)
 rename cmake/configs/{posix_rpi2_default.cmake => posix_rpi2_native.cmake} (100%)

diff --git a/Makefile b/Makefile
index adfa2654fb..8271d7c3c8 100644
--- a/Makefile
+++ b/Makefile
@@ -152,7 +152,7 @@ px4fmu-v1_default:
 
 px4fmu-v2_default:
 	$(call cmake-build,nuttx_px4fmu-v2_default)
-	
+
 px4fmu-v2_test:
 	$(call cmake-build,nuttx_px4fmu-v2_test)
 
@@ -170,7 +170,7 @@ mindpx-v2_default:
 
 posix_sitl_default:
 	$(call cmake-build,$@)
-	
+
 posix_sitl_lpe:
 	$(call cmake-build,$@)
 
@@ -199,10 +199,10 @@ eagle_default: posix_eagle_default qurt_eagle_default
 eagle_legacy_default: posix_eagle_legacy_driver_default qurt_eagle_legacy_driver_default
 
 qurt_eagle_legacy_driver_default:
-	$(call cmake-build,$@)	
-	
+	$(call cmake-build,$@)
+
 posix_eagle_legacy_driver_default:
-	$(call cmake-build,$@) 
+	$(call cmake-build,$@)
 
 qurt_excelsior_default:
 	$(call cmake-build,$@)
@@ -212,10 +212,10 @@ posix_excelsior_default:
 
 excelsior_default: posix_excelsior_default qurt_excelsior_default
 
-posix_rpi2_default:
+posix_rpi2_native:
 	$(call cmake-build,$@)
 
-posix_rpi2_release:
+posix_rpi2_cross:
 	$(call cmake-build,$@)
 
 posix_bebop_default:
diff --git a/cmake/configs/posix_rpi2_common.cmake b/cmake/configs/posix_rpi2_common.cmake
index 95477cd8c0..68479eaf9a 100644
--- a/cmake/configs/posix_rpi2_common.cmake
+++ b/cmake/configs/posix_rpi2_common.cmake
@@ -1,5 +1,5 @@
-# This file is shared between posix_rpi2_default.cmake
-# and posix_rpi2_release.cmake.
+# This file is shared between posix_rpi2_native.cmake
+# and posix_rpi2_cross.cmake.
 
 include(posix/px4_impl_posix)
 
diff --git a/cmake/configs/posix_rpi2_release.cmake b/cmake/configs/posix_rpi2_cross.cmake
similarity index 100%
rename from cmake/configs/posix_rpi2_release.cmake
rename to cmake/configs/posix_rpi2_cross.cmake
diff --git a/cmake/configs/posix_rpi2_default.cmake b/cmake/configs/posix_rpi2_native.cmake
similarity index 100%
rename from cmake/configs/posix_rpi2_default.cmake
rename to cmake/configs/posix_rpi2_native.cmake

From 17be06cf1ab65f8352878901b971359be24e8b83 Mon Sep 17 00:00:00 2001
From: Miguel Arroyo <miguel@arroyo.me>
Date: Wed, 13 Jul 2016 09:53:11 -0400
Subject: [PATCH 1032/1230] Fixes Navio2 Config Typo

---
 posix-configs/rpi2/navio2.config | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/posix-configs/rpi2/navio2.config b/posix-configs/rpi2/navio2.config
index 3e14dbd5f1..74de109c57 100644
--- a/posix-configs/rpi2/navio2.config
+++ b/posix-configs/rpi2/navio2.config
@@ -20,5 +20,5 @@ sleep 1
 mavlink stream -d /dev/ttyUSB0 -s HIGHRES_IMU -r 50
 mavlink stream -d /dev/ttyUSB0 -s ATTITUDE -r 50
 mavlink boot_complete
-navio_sysfs_rcinput start
+navio_sysfs_rc_in start
 navio_sysfs_pwm_out start

From 2bf40efe8bb86f18ca68033d3658fb9f144c5014 Mon Sep 17 00:00:00 2001
From: Julian Oes <julian@oes.ch>
Date: Thu, 14 Jul 2016 13:55:21 +0200
Subject: [PATCH 1033/1230] RPi2: fuse mainapp/common and navio2

We currently only support Navio2, so let's fuse the two configurations.
---
 Makefile                                 |   3 -
 cmake/configs/posix_navio2_release.cmake | 114 -----------------------
 cmake/configs/posix_rpi2_common.cmake    |   3 +
 posix-configs/rpi2/mainapp.config        |   8 +-
 posix-configs/rpi2/navio2.config         |  24 -----
 5 files changed, 10 insertions(+), 142 deletions(-)
 delete mode 100644 cmake/configs/posix_navio2_release.cmake
 delete mode 100644 posix-configs/rpi2/navio2.config

diff --git a/Makefile b/Makefile
index 8271d7c3c8..4d7529b61c 100644
--- a/Makefile
+++ b/Makefile
@@ -221,9 +221,6 @@ posix_rpi2_cross:
 posix_bebop_default:
 	$(call cmake-build,$@)
 
-posix_navio2_release:
-	$(call cmake-build,$@)
-
 posix: posix_sitl_default
 
 broadcast: posix_sitl_broadcast
diff --git a/cmake/configs/posix_navio2_release.cmake b/cmake/configs/posix_navio2_release.cmake
deleted file mode 100644
index 402b74dba8..0000000000
--- a/cmake/configs/posix_navio2_release.cmake
+++ /dev/null
@@ -1,114 +0,0 @@
-include(posix/px4_impl_posix)
-
-set(CMAKE_TOOLCHAIN_FILE ${CMAKE_SOURCE_DIR}/cmake/toolchains/Toolchain-arm-linux-gnueabihf-raspbian.cmake)
-
-set(CMAKE_PROGRAM_PATH
-	"${RPI_TOOLCHAIN_DIR}/gcc-linaro-arm-linux-gnueabihf-raspbian/bin"
-	${CMAKE_PROGRAM_PATH}
-)
-
-# This definition allows to differentiate if this just the usual POSIX build
-# or if it is for the RPi.
-add_definitions(
-	-D__PX4_POSIX_RPI2
-	-D__LINUX
-)
-
-set(config_module_list
-	#
-	# Board support modules
-	#
-	drivers/device
-	modules/sensors
-	platforms/posix/drivers/df_mpu9250_wrapper
-	platforms/posix/drivers/df_lsm9ds1_wrapper
-	platforms/posix/drivers/df_ms5611_wrapper
-	platforms/posix/drivers/df_hmc5883_wrapper
-	platforms/posix/drivers/df_trone_wrapper
-	platforms/posix/drivers/df_isl29501_wrapper
-
-	#
-	# System commands
-	#
-	systemcmds/param
-	systemcmds/mixer
-	systemcmds/ver
-	systemcmds/esc_calib
-	systemcmds/topic_listener
-	systemcmds/perf
-
-	#
-	# Estimation modules (EKF/ SO3 / other filters)
-	#
-	#modules/attitude_estimator_ekf
-	modules/ekf_att_pos_estimator
-	modules/attitude_estimator_q
-	modules/position_estimator_inav
-	modules/local_position_estimator
-	modules/ekf2
-
-	#
-	# Vehicle Control
-	#
-	modules/mc_att_control
-	modules/mc_pos_control
-	modules/fw_att_control
-	modules/fw_pos_control_l1	
-	modules/vtol_att_control
-
-	#
-	# Library modules
-	#
-	modules/sdlog2
-	modules/logger
-	modules/commander
-	modules/load_mon
-	modules/param
-	modules/systemlib
-	modules/systemlib/mixer
-	modules/uORB
-	modules/dataman
-	modules/land_detector
-	modules/navigator
-	modules/mavlink
-
-	#
-	# PX4 drivers
-	#
-	drivers/gps
-	drivers/navio_sysfs_rc_in
-	drivers/navio_sysfs_pwm_out
-
-	#
-	# Libraries
-	#
-	lib/controllib
-	lib/mathlib
-	lib/mathlib/math/filter
-	lib/geo
-	lib/ecl
-	lib/geo_lookup
-	lib/launchdetection
-	lib/external_lgpl
-	lib/conversion
-	lib/terrain_estimation
-	lib/runway_takeoff
-	lib/tailsitter_recovery
-	lib/DriverFramework/framework
-
-	#
-	# POSIX
-	#
-	platforms/common
-	platforms/posix/px4_layer
-	platforms/posix/work_queue
-)
-
-set(config_df_driver_list
-	mpu9250
-	lsm9ds1
-	ms5611
-	hmc5883
-	trone
-	isl29501
-)
diff --git a/cmake/configs/posix_rpi2_common.cmake b/cmake/configs/posix_rpi2_common.cmake
index 68479eaf9a..b6437879a8 100644
--- a/cmake/configs/posix_rpi2_common.cmake
+++ b/cmake/configs/posix_rpi2_common.cmake
@@ -73,6 +73,9 @@ set(config_module_list
 	#
 	# PX4 drivers
 	#
+	drivers/gps
+	drivers/navio_sysfs_rc_in
+	drivers/navio_sysfs_pwm_out
 
 	#
 	# Libraries
diff --git a/posix-configs/rpi2/mainapp.config b/posix-configs/rpi2/mainapp.config
index 752ea8f66b..003fb52c44 100644
--- a/posix-configs/rpi2/mainapp.config
+++ b/posix-configs/rpi2/mainapp.config
@@ -7,6 +7,7 @@ df_lsm9ds1_wrapper start -R 4
 #df_mpu9250_wrapper start -R 10
 #df_hmc5883_wrapper start
 df_ms5611_wrapper start
+gps start -d /dev/pts/0
 sensors start
 commander start
 attitude_estimator_q start
@@ -18,4 +19,9 @@ mavlink start -u 14556 -r 1000000
 sleep 1
 mavlink stream -u 14556 -s HIGHRES_IMU -r 50
 mavlink stream -u 14556 -s ATTITUDE -r 50
-mavlink boot_complete
+mavlink start -d /dev/ttyUSB0
+mavlink stream -d /dev/ttyUSB0 -s HIGHRES_IMU -r 50
+mavlink stream -d /dev/ttyUSB0 -s ATTITUDE -r 50
+navio_sysfs_rc_in start
+navio_sysfs_pwm_out start
+mavlink boot_complete
\ No newline at end of file
diff --git a/posix-configs/rpi2/navio2.config b/posix-configs/rpi2/navio2.config
deleted file mode 100644
index 74de109c57..0000000000
--- a/posix-configs/rpi2/navio2.config
+++ /dev/null
@@ -1,24 +0,0 @@
-uorb start
-param set SYS_AUTOSTART 4001
-param set MAV_BROADCAST 1
-sleep 1
-param set MAV_TYPE 2
-df_lsm9ds1_wrapper start -R 4
-#df_mpu9250_wrapper start -R 10
-#df_hmc5883_wrapper start
-df_ms5611_wrapper start
-gps start -d /dev/pts/0
-sensors start
-commander start
-attitude_estimator_q start
-position_estimator_inav start
-land_detector start multicopter
-mc_pos_control start
-mc_att_control start
-mavlink start -d /dev/ttyUSB0
-sleep 1
-mavlink stream -d /dev/ttyUSB0 -s HIGHRES_IMU -r 50
-mavlink stream -d /dev/ttyUSB0 -s ATTITUDE -r 50
-mavlink boot_complete
-navio_sysfs_rc_in start
-navio_sysfs_pwm_out start

From fa614a3cc1534c9c5f46de9d2f118ea9b4f4e8e4 Mon Sep 17 00:00:00 2001
From: Julian Oes <julian@oes.ch>
Date: Thu, 14 Jul 2016 17:34:41 +0200
Subject: [PATCH 1034/1230] RPi: just use RPI instead of RPI2.

The reason for this change is that RPi2 and RPi3 are compatible, and
hopefully all differences coming up can be resolved without ifdefs but
at runtime.
---
 Makefile                                      |  4 +--
 ...i2_common.cmake => posix_rpi_common.cmake} |  6 ++---
 ...rpi2_cross.cmake => posix_rpi_cross.cmake} |  2 +-
 ...i2_native.cmake => posix_rpi_native.cmake} |  2 +-
 ...olchain-arm-linux-gnueabihf-raspbian.cmake |  2 +-
 posix-configs/rpi2/mainapp.config             | 27 -------------------
 src/firmware/posix/CMakeLists.txt             |  4 +--
 src/lib/DriverFramework                       |  2 +-
 src/lib/version/version.h                     |  2 +-
 .../commander/accelerometer_calibration.cpp   |  8 +++---
 src/modules/commander/gyro_calibration.cpp    |  6 ++---
 src/modules/commander/mag_calibration.cpp     | 12 ++++-----
 src/modules/sensors/sensors.cpp               | 10 +++----
 src/modules/sensors/sensors_init.cpp          |  2 +-
 14 files changed, 31 insertions(+), 58 deletions(-)
 rename cmake/configs/{posix_rpi2_common.cmake => posix_rpi_common.cmake} (94%)
 rename cmake/configs/{posix_rpi2_cross.cmake => posix_rpi_cross.cmake} (86%)
 rename cmake/configs/{posix_rpi2_native.cmake => posix_rpi_native.cmake} (71%)
 delete mode 100644 posix-configs/rpi2/mainapp.config

diff --git a/Makefile b/Makefile
index 4d7529b61c..5435ef7c70 100644
--- a/Makefile
+++ b/Makefile
@@ -212,10 +212,10 @@ posix_excelsior_default:
 
 excelsior_default: posix_excelsior_default qurt_excelsior_default
 
-posix_rpi2_native:
+posix_rpi_native:
 	$(call cmake-build,$@)
 
-posix_rpi2_cross:
+posix_rpi_cross:
 	$(call cmake-build,$@)
 
 posix_bebop_default:
diff --git a/cmake/configs/posix_rpi2_common.cmake b/cmake/configs/posix_rpi_common.cmake
similarity index 94%
rename from cmake/configs/posix_rpi2_common.cmake
rename to cmake/configs/posix_rpi_common.cmake
index b6437879a8..a22565640b 100644
--- a/cmake/configs/posix_rpi2_common.cmake
+++ b/cmake/configs/posix_rpi_common.cmake
@@ -1,12 +1,12 @@
-# This file is shared between posix_rpi2_native.cmake
-# and posix_rpi2_cross.cmake.
+# This file is shared between posix_rpi_native.cmake
+# and posix_rpi_cross.cmake.
 
 include(posix/px4_impl_posix)
 
 # This definition allows to differentiate if this just the usual POSIX build
 # or if it is for the RPi.
 add_definitions(
-	-D__PX4_POSIX_RPI2
+	-D__PX4_POSIX_RPI
 	-D__LINUX
 )
 
diff --git a/cmake/configs/posix_rpi2_cross.cmake b/cmake/configs/posix_rpi_cross.cmake
similarity index 86%
rename from cmake/configs/posix_rpi2_cross.cmake
rename to cmake/configs/posix_rpi_cross.cmake
index d17ef5cec3..46243fc361 100644
--- a/cmake/configs/posix_rpi2_cross.cmake
+++ b/cmake/configs/posix_rpi_cross.cmake
@@ -1,4 +1,4 @@
-include(configs/posix_rpi2_common)
+include(configs/posix_rpi_common)
 
 set(CMAKE_TOOLCHAIN_FILE ${CMAKE_SOURCE_DIR}/cmake/toolchains/Toolchain-arm-linux-gnueabihf-raspbian.cmake)
 
diff --git a/cmake/configs/posix_rpi2_native.cmake b/cmake/configs/posix_rpi_native.cmake
similarity index 71%
rename from cmake/configs/posix_rpi2_native.cmake
rename to cmake/configs/posix_rpi_native.cmake
index 9b0f45c718..e58f370282 100644
--- a/cmake/configs/posix_rpi2_native.cmake
+++ b/cmake/configs/posix_rpi_native.cmake
@@ -1,3 +1,3 @@
-include(configs/posix_rpi2_common)
+include(configs/posix_rpi_common)
 
 set(CMAKE_TOOLCHAIN_FILE ${CMAKE_SOURCE_DIR}/cmake/toolchains/Toolchain-native.cmake)
diff --git a/cmake/toolchains/Toolchain-arm-linux-gnueabihf-raspbian.cmake b/cmake/toolchains/Toolchain-arm-linux-gnueabihf-raspbian.cmake
index 0a9d2b4f3b..4806222740 100644
--- a/cmake/toolchains/Toolchain-arm-linux-gnueabihf-raspbian.cmake
+++ b/cmake/toolchains/Toolchain-arm-linux-gnueabihf-raspbian.cmake
@@ -71,7 +71,7 @@ foreach(tool echo grep rm mkdir nm cp touch make unzip)
 endforeach()
 
 add_definitions(
-	-D __RPI2
+	-D __RPI
 	)
 
 set(LINKER_FLAGS "-Wl,-gc-sections")
diff --git a/posix-configs/rpi2/mainapp.config b/posix-configs/rpi2/mainapp.config
deleted file mode 100644
index 003fb52c44..0000000000
--- a/posix-configs/rpi2/mainapp.config
+++ /dev/null
@@ -1,27 +0,0 @@
-uorb start
-param set SYS_AUTOSTART 4001
-param set MAV_BROADCAST 1
-sleep 1
-param set MAV_TYPE 2
-df_lsm9ds1_wrapper start -R 4
-#df_mpu9250_wrapper start -R 10
-#df_hmc5883_wrapper start
-df_ms5611_wrapper start
-gps start -d /dev/pts/0
-sensors start
-commander start
-attitude_estimator_q start
-position_estimator_inav start
-land_detector start multicopter
-mc_pos_control start
-mc_att_control start
-mavlink start -u 14556 -r 1000000
-sleep 1
-mavlink stream -u 14556 -s HIGHRES_IMU -r 50
-mavlink stream -u 14556 -s ATTITUDE -r 50
-mavlink start -d /dev/ttyUSB0
-mavlink stream -d /dev/ttyUSB0 -s HIGHRES_IMU -r 50
-mavlink stream -d /dev/ttyUSB0 -s ATTITUDE -r 50
-navio_sysfs_rc_in start
-navio_sysfs_pwm_out start
-mavlink boot_complete
\ No newline at end of file
diff --git a/src/firmware/posix/CMakeLists.txt b/src/firmware/posix/CMakeLists.txt
index 74d4025a7a..ff348c9b45 100644
--- a/src/firmware/posix/CMakeLists.txt
+++ b/src/firmware/posix/CMakeLists.txt
@@ -37,7 +37,7 @@ if ("${BOARD}" STREQUAL "eagle" OR ("${BOARD}" STREQUAL "excelsior"))
 			 DEPENDS mainapp
 			 DEST /home/linaro)
 
-elseif ("${BOARD}" STREQUAL "rpi2")
+elseif ("${BOARD}" STREQUAL "rpi")
 
 	add_executable(mainapp
 		${CMAKE_SOURCE_DIR}/src/platforms/posix/main.cpp
@@ -57,7 +57,7 @@ elseif ("${BOARD}" STREQUAL "rpi2")
 			 OS ${OS}
 			 BOARD ${BOARD}
 			 FILES ${CMAKE_CURRENT_BINARY_DIR}/mainapp
-				 ${CMAKE_SOURCE_DIR}/posix-configs/rpi2/mainapp.config
+				 ${CMAKE_SOURCE_DIR}/posix-configs/rpi/mainapp.config
 			 DEPENDS mainapp
 			 DEST /home/pi)
 
diff --git a/src/lib/DriverFramework b/src/lib/DriverFramework
index dab303d799..458b726027 160000
--- a/src/lib/DriverFramework
+++ b/src/lib/DriverFramework
@@ -1 +1 @@
-Subproject commit dab303d7992c9a3bbc39509b13eddf2b65d382b8
+Subproject commit 458b726027ec9d67099e744dd8e0af7208ea64a5
diff --git a/src/lib/version/version.h b/src/lib/version/version.h
index a78be4fda7..b524cb2878 100644
--- a/src/lib/version/version.h
+++ b/src/lib/version/version.h
@@ -59,7 +59,7 @@ __END_DECLS
 #  define	HW_ARCH "EAGLE"
 #elif defined(CONFIG_ARCH_BOARD_EXCELSIOR)
 #  define HW_ARCH "EXCELSIOR"
-#elif defined(CONFIG_ARCH_BOARD_RPI2) || defined(CONFIG_ARCH_BOARD_NAVIO2)
+#elif defined(CONFIG_ARCH_BOARD_RPI) || defined(CONFIG_ARCH_BOARD_NAVIO2)
 #  define	HW_ARCH "RPI"
 #elif defined(CONFIG_ARCH_BOARD_BEBOP)
 #  define	HW_ARCH "BEBOP"
diff --git a/src/modules/commander/accelerometer_calibration.cpp b/src/modules/commander/accelerometer_calibration.cpp
index bdb18eaab9..7b3cae8d3f 100644
--- a/src/modules/commander/accelerometer_calibration.cpp
+++ b/src/modules/commander/accelerometer_calibration.cpp
@@ -175,7 +175,7 @@ typedef struct  {
 
 int do_accel_calibration(orb_advert_t *mavlink_log_pub)
 {
-#if !defined(__PX4_QURT) && !defined(__PX4_POSIX_EAGLE) && !defined(__PX4_POSIX_RPI2)
+#if !defined(__PX4_QURT) && !defined(__PX4_POSIX_EAGLE) && !defined(__PX4_POSIX_RPI)
 	int fd;
 #endif
 
@@ -195,7 +195,7 @@ int do_accel_calibration(orb_advert_t *mavlink_log_pub)
 
 	/* reset all sensors */
 	for (unsigned s = 0; s < max_accel_sens; s++) {
-#if !defined(__PX4_QURT) && !defined(__PX4_POSIX_EAGLE) && !defined(__PX4_POSIX_RPI2)
+#if !defined(__PX4_QURT) && !defined(__PX4_POSIX_EAGLE) && !defined(__PX4_POSIX_RPI)
 		sprintf(str, "%s%u", ACCEL_BASE_DEVICE_PATH, s);
 		/* reset all offsets to zero and all scales to one */
 		fd = px4_open(str, 0);
@@ -329,7 +329,7 @@ int do_accel_calibration(orb_advert_t *mavlink_log_pub)
 			return ERROR;
 		}
 
-#if !defined(__PX4_QURT) && !defined(__PX4_POSIX_EAGLE) && !defined(__PX4_POSIX_RPI2)
+#if !defined(__PX4_QURT) && !defined(__PX4_POSIX_EAGLE) && !defined(__PX4_POSIX_RPI)
 		sprintf(str, "%s%u", ACCEL_BASE_DEVICE_PATH, i);
 		fd = px4_open(str, 0);
 
@@ -419,7 +419,7 @@ calibrate_return do_accel_calibration_measurements(orb_advert_t *mavlink_log_pub
 			break;
 		}
 
-#if defined(__PX4_QURT) || defined(__PX4_POSIX_EAGLE) || defined(__PX4_POSIX_RPI2)
+#if defined(__PX4_QURT) || defined(__PX4_POSIX_EAGLE) || defined(__PX4_POSIX_RPI)
 		// For QURT respectively the driver framework, we need to get the device ID by copying one report.
 		struct accel_report	accel_report;
 		orb_copy(ORB_ID(sensor_accel), worker_data.subs[i], &accel_report);
diff --git a/src/modules/commander/gyro_calibration.cpp b/src/modules/commander/gyro_calibration.cpp
index 9a48866ab5..945935e38d 100644
--- a/src/modules/commander/gyro_calibration.cpp
+++ b/src/modules/commander/gyro_calibration.cpp
@@ -185,7 +185,7 @@ int do_gyro_calibration(orb_advert_t *mavlink_log_pub)
 
 		// Reset all offsets to 0 and scales to 1
 		(void)memcpy(&worker_data.gyro_scale[s], &gyro_scale_zero, sizeof(gyro_scale_zero));
-#if !defined(__PX4_QURT) && !defined(__PX4_POSIX_EAGLE) && !defined(__PX4_POSIX_RPI2)
+#if !defined(__PX4_QURT) && !defined(__PX4_POSIX_EAGLE) && !defined(__PX4_POSIX_RPI)
 		sprintf(str, "%s%u", GYRO_BASE_DEVICE_PATH, s);
 		int fd = px4_open(str, 0);
 		if (fd >= 0) {
@@ -238,7 +238,7 @@ int do_gyro_calibration(orb_advert_t *mavlink_log_pub)
 	for (unsigned s = 0; s < gyro_count; s++) {
 
 		worker_data.gyro_sensor_sub[s] = orb_subscribe_multi(ORB_ID(sensor_gyro), s);
-#if defined(__PX4_QURT) || defined(__PX4_POSIX_EAGLE) || defined(__PX4_POSIX_RPI2)
+#if defined(__PX4_QURT) || defined(__PX4_POSIX_EAGLE) || defined(__PX4_POSIX_RPI)
 		// For QURT respectively the driver framework, we need to get the device ID by copying one report.
 		struct gyro_report gyro_report;
 		orb_copy(ORB_ID(sensor_gyro), worker_data.gyro_sensor_sub[s], &gyro_report);
@@ -336,7 +336,7 @@ int do_gyro_calibration(orb_advert_t *mavlink_log_pub)
 				(void)sprintf(str, "CAL_GYRO%u_ID", s);
 				failed |= (OK != param_set_no_notification(param_find(str), &(worker_data.device_id[s])));
 
-#if !defined(__PX4_QURT) && !defined(__PX4_POSIX_EAGLE) && !defined(__PX4_POSIX_RPI2)
+#if !defined(__PX4_QURT) && !defined(__PX4_POSIX_EAGLE) && !defined(__PX4_POSIX_RPI)
 				/* apply new scaling and offsets */
 				(void)sprintf(str, "%s%u", GYRO_BASE_DEVICE_PATH, s);
 				int fd = px4_open(str, 0);
diff --git a/src/modules/commander/mag_calibration.cpp b/src/modules/commander/mag_calibration.cpp
index 4d02260b40..10f1c5510d 100644
--- a/src/modules/commander/mag_calibration.cpp
+++ b/src/modules/commander/mag_calibration.cpp
@@ -124,7 +124,7 @@ int do_mag_calibration(orb_advert_t *mavlink_log_pub)
 	_last_mag_progress = 0;
 
 	for (unsigned cur_mag = 0; cur_mag < max_mags; cur_mag++) {
-#if !defined(__PX4_QURT) && !defined(__PX4_POSIX_RPI2)
+#if !defined(__PX4_QURT) && !defined(__PX4_POSIX_RPI)
 		// Reset mag id to mag not available
 		(void)sprintf(str, "CAL_MAG%u_ID", cur_mag);
 		result = param_set_no_notification(param_find(str), &(device_ids[cur_mag]));
@@ -166,7 +166,7 @@ int do_mag_calibration(orb_advert_t *mavlink_log_pub)
 #endif
 
 /* for calibration, commander will run on apps, so orb messages are used to get info from dsp */
-#if !defined(__PX4_QURT) && !defined(__PX4_POSIX_RPI2)
+#if !defined(__PX4_QURT) && !defined(__PX4_POSIX_RPI)
 		// Attempt to open mag
 		(void)sprintf(str, "%s%u", MAG_BASE_DEVICE_PATH, cur_mag);
 		int fd = px4_open(str, O_RDONLY);
@@ -508,7 +508,7 @@ calibrate_return mag_calibrate_all(orb_advert_t *mavlink_log_pub)
 			// Mag in this slot is available
 			worker_data.sub_mag[cur_mag] = orb_subscribe_multi(ORB_ID(sensor_mag), cur_mag);
 
-#if defined(__PX4_QURT) || defined(__PX4_POSIX_RPI2)
+#if defined(__PX4_QURT) || defined(__PX4_POSIX_RPI)
 			// For QURT respectively the driver framework, we need to get the device ID by copying one report.
 			struct mag_report	mag_report;
 			orb_copy(ORB_ID(sensor_mag), worker_data.sub_mag[cur_mag], &mag_report);
@@ -664,7 +664,7 @@ calibrate_return mag_calibrate_all(orb_advert_t *mavlink_log_pub)
 		for (unsigned cur_mag=0; cur_mag<max_mags; cur_mag++) {
 			if (device_ids[cur_mag] != 0) {
 				struct mag_calibration_s mscale;
-#if !defined(__PX4_QURT) && !defined(__PX4_POSIX_RPI2)
+#if !defined(__PX4_QURT) && !defined(__PX4_POSIX_RPI)
 				int fd_mag = -1;
 
 				// Set new scale
@@ -688,7 +688,7 @@ calibrate_return mag_calibrate_all(orb_advert_t *mavlink_log_pub)
 					mscale.y_offset = sphere_y[cur_mag];
 					mscale.z_offset = sphere_z[cur_mag];
 
-#if !defined(__PX4_QURT) && !defined(__PX4_POSIX_RPI2)
+#if !defined(__PX4_QURT) && !defined(__PX4_POSIX_RPI)
 					if (px4_ioctl(fd_mag, MAGIOCSSCALE, (long unsigned int)&mscale) != OK) {
 						calibration_log_critical(mavlink_log_pub, CAL_ERROR_APPLY_CAL_MSG, cur_mag);
 						result = calibrate_return_error;
@@ -696,7 +696,7 @@ calibrate_return mag_calibrate_all(orb_advert_t *mavlink_log_pub)
 #endif
 				}
 
-#if !defined(__PX4_QURT) && !defined(__PX4_POSIX_RPI2)
+#if !defined(__PX4_QURT) && !defined(__PX4_POSIX_RPI)
 				// Mag device no longer needed
 				if (fd_mag >= 0) {
 					px4_close(fd_mag);
diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp
index 1d69e03cfd..b9dc1ca382 100644
--- a/src/modules/sensors/sensors.cpp
+++ b/src/modules/sensors/sensors.cpp
@@ -1017,7 +1017,7 @@ Sensors::parameters_update()
 	DevHandle h_baro;
 	DevMgr::getHandle(BARO0_DEVICE_PATH, h_baro);
 
-#if !defined(__PX4_QURT) && !defined(__PX4_POSIX_RPI2)
+#if !defined(__PX4_QURT) && !defined(__PX4_POSIX_RPI)
 
 	// TODO: this needs fixing for QURT and Raspberry Pi
 	if (!h_baro.isValid()) {
@@ -1671,7 +1671,7 @@ Sensors::parameter_update_poll(bool forced)
 bool
 Sensors::apply_gyro_calibration(DevHandle &h, const struct gyro_calibration_s *gcal, const int device_id)
 {
-#if !defined(__PX4_QURT) && !defined(__PX4_POSIX_RPI2)
+#if !defined(__PX4_QURT) && !defined(__PX4_POSIX_RPI)
 
 	/* On most systems, we can just use the IOCTL call to set the calibration params. */
 	return !h.ioctl(GYROIOCSSCALE, (long unsigned int)gcal);
@@ -1685,7 +1685,7 @@ Sensors::apply_gyro_calibration(DevHandle &h, const struct gyro_calibration_s *g
 bool
 Sensors::apply_accel_calibration(DevHandle &h, const struct accel_calibration_s *acal, const int device_id)
 {
-#if !defined(__PX4_QURT) && !defined(__PX4_POSIX_RPI2)
+#if !defined(__PX4_QURT) && !defined(__PX4_POSIX_RPI)
 
 	/* On most systems, we can just use the IOCTL call to set the calibration params. */
 	return !h.ioctl(ACCELIOCSSCALE, (long unsigned int)acal);
@@ -1699,7 +1699,7 @@ Sensors::apply_accel_calibration(DevHandle &h, const struct accel_calibration_s
 bool
 Sensors::apply_mag_calibration(DevHandle &h, const struct mag_calibration_s *mcal, const int device_id)
 {
-#if !defined(__PX4_QURT) && !defined(__PX4_POSIX_RPI2)
+#if !defined(__PX4_QURT) && !defined(__PX4_POSIX_RPI)
 
 	/* On most systems, we can just use the IOCTL call to set the calibration params. */
 	return !h.ioctl(MAGIOCSSCALE, (long unsigned int)mcal);
@@ -2253,7 +2253,7 @@ Sensors::task_main()
 	/* This calls a sensors_init which can have different implementations on NuttX, POSIX, QURT. */
 	ret = sensors_init();
 
-#if !defined(__PX4_QURT) && !defined(__PX4_POSIX_RPI2)
+#if !defined(__PX4_QURT) && !defined(__PX4_POSIX_RPI)
 	// TODO: move adc_init into the sensors_init call.
 	ret = ret || adc_init();
 #endif
diff --git a/src/modules/sensors/sensors_init.cpp b/src/modules/sensors/sensors_init.cpp
index 93742333bd..561ee06e27 100644
--- a/src/modules/sensors/sensors_init.cpp
+++ b/src/modules/sensors/sensors_init.cpp
@@ -52,7 +52,7 @@
 
 using namespace DriverFramework;
 
-#if defined(__PX4_QURT) || defined(__PX4_POSIX_RPI2)
+#if defined(__PX4_QURT) || defined(__PX4_POSIX_RPI)
 
 // Sensor initialization is performed automatically when the QuRT sensor drivers
 // are loaded.

From 981353f439ad2f9a9d8e3af5ef1922a2fe2df645 Mon Sep 17 00:00:00 2001
From: Mark Whitehorn <kd0aij@gmail.com>
Date: Thu, 14 Jul 2016 21:19:07 -0600
Subject: [PATCH 1035/1230] fix bug in RC scan for SUMD input

---
 src/drivers/px4fmu/fmu.cpp | 3 +--
 1 file changed, 1 insertion(+), 2 deletions(-)

diff --git a/src/drivers/px4fmu/fmu.cpp b/src/drivers/px4fmu/fmu.cpp
index 81712d1860..a9c69d8401 100644
--- a/src/drivers/px4fmu/fmu.cpp
+++ b/src/drivers/px4fmu/fmu.cpp
@@ -1319,10 +1319,9 @@ PX4FMU::cycle()
 
 		} else {
 			// Scan the next protocol
-			set_rc_scan_state(RC_SCAN_SUMD);
+			set_rc_scan_state(RC_SCAN_PPM);
 		}
 
-		set_rc_scan_state(RC_SCAN_PPM);
 		break;
 
 	case RC_SCAN_PPM:

From 9fcf121380f364e32c8d5b03e6b491cd0ba59afc Mon Sep 17 00:00:00 2001
From: James Goppert <jgoppert@users.noreply.github.com>
Date: Sun, 17 Jul 2016 10:16:17 -0400
Subject: [PATCH 1036/1230] LPE: Don't use home as local origin (#5067)

---
 .../BlockLocalPositionEstimator.cpp           | 72 ++++++-------------
 .../BlockLocalPositionEstimator.hpp           | 21 +++---
 src/modules/local_position_estimator/params.c |  4 +-
 .../local_position_estimator/sensors/baro.cpp | 14 ++--
 .../local_position_estimator/sensors/gps.cpp  | 22 +++---
 .../sensors/mocap.cpp                         | 12 ++--
 .../sensors/vision.cpp                        | 12 ++--
 src/modules/uORB/Subscription.cpp             |  2 +
 8 files changed, 65 insertions(+), 94 deletions(-)

diff --git a/src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp b/src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp
index 89dae8cd99..566f0cfbde 100644
--- a/src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp
+++ b/src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp
@@ -32,7 +32,6 @@ BlockLocalPositionEstimator::BlockLocalPositionEstimator() :
 	// status updates 2 hz
 	_sub_param_update(ORB_ID(parameter_update), 1000 / 2, 0, &getSubscriptions()),
 	_sub_manual(ORB_ID(manual_control_setpoint), 1000 / 2, 0, &getSubscriptions()),
-	_sub_home(ORB_ID(home_position), 1000 / 2, 0, &getSubscriptions()),
 	// gps 10 hz
 	_sub_gps(ORB_ID(vehicle_gps_position), 1000 / 10, 0, &getSubscriptions()),
 	// vision 5 hz
@@ -87,9 +86,9 @@ BlockLocalPositionEstimator::BlockLocalPositionEstimator() :
 	_pn_b_noise_density(this, "PN_B"),
 	_t_max_grade(this, "T_MAX_GRADE"),
 
-	// init home
-	_init_home_lat(this, "LAT"),
-	_init_home_lon(this, "LON"),
+	// init origin
+	_init_origin_lat(this, "LAT"),
+	_init_origin_lon(this, "LON"),
 
 	// flow gyro
 	_flow_gyro_x_high_pass(this, "FGYRO_HP"),
@@ -140,12 +139,12 @@ BlockLocalPositionEstimator::BlockLocalPositionEstimator() :
 	_mocapInitialized(false),
 
 	// reference altitudes
-	_altHome(0),
-	_altHomeInitialized(false),
-	_baroAltHome(0),
-	_gpsAltHome(0),
-	_visionHome(),
-	_mocapHome(),
+	_altOrigin(0),
+	_altOriginInitialized(false),
+	_baroAltOrigin(0),
+	_gpsAltOrigin(0),
+	_visionOrigin(),
+	_mocapOrigin(),
 
 	// flow integration
 	_flowX(0),
@@ -256,12 +255,10 @@ void BlockLocalPositionEstimator::update()
 	// reset pos, vel, and terrain on arming
 	if (!_lastArmedState && armedState) {
 
-		// we just armed, we are at home position on the ground
+		// we just armed, we are at origin on the ground
 		_x(X_x) = 0;
 		_x(X_y) = 0;
-
-		// the pressure altitude of home may have drifted, so we don't
-		// reset z to zero
+		_x(X_z) = 0;
 
 		// reset flow integral
 		_flowX = 0;
@@ -287,7 +284,6 @@ void BlockLocalPositionEstimator::update()
 	bool paramsUpdated = _sub_param_update.updated();
 	bool baroUpdated = _sub_sensor.updated();
 	bool gpsUpdated = _gps_on.get() && _sub_gps.updated();
-	bool homeUpdated = _sub_home.updated();
 	bool visionUpdated = _vision_on.get() && _sub_vision_pos.updated();
 	bool mocapUpdated = _sub_mocap.updated();
 	bool lidarUpdated = (_sub_lidar != NULL) && _sub_lidar->updated();
@@ -302,11 +298,6 @@ void BlockLocalPositionEstimator::update()
 		updateSSParams();
 	}
 
-	// update home position projection
-	if (homeUpdated) {
-		updateHome();
-	}
-
 	// is xy valid?
 	bool xy_stddev_ok = sqrtf(math::max(_P(X_x, X_x), _P(X_y, X_y))) < _xy_pub_thresh.get();
 
@@ -370,8 +361,8 @@ void BlockLocalPositionEstimator::update()
 	// if we have no lat, lon initialize projection at 0,0
 	if (_validXY && !_map_ref.init_done) {
 		map_projection_init(&_map_ref,
-				    _init_home_lat.get(),
-				    _init_home_lon.get());
+				    _init_origin_lat.get(),
+				    _init_origin_lon.get());
 	}
 
 	// reinitialize x if necessary
@@ -484,7 +475,7 @@ void BlockLocalPositionEstimator::update()
 		}
 	}
 
-	if (_altHomeInitialized) {
+	if (_altOriginInitialized) {
 		// update all publications if possible
 		publishLocalPos();
 		publishEstimatorStatus();
@@ -570,6 +561,7 @@ void BlockLocalPositionEstimator::correctionLogic(Vector<float, n_x> &dx)
 		dx(X_bz) = 0;
 	}
 
+	// if xy not valid, stop estimating
 	if (!_validXY) {
 		dx(X_x) = 0;
 		dx(X_y) = 0;
@@ -579,12 +571,14 @@ void BlockLocalPositionEstimator::correctionLogic(Vector<float, n_x> &dx)
 		dx(X_by) = 0;
 	}
 
+	// if z not valid, stop estimating
 	if (!_validZ) {
 		dx(X_z) = 0;
 		dx(X_vz) = 0;
 		dx(X_bz) = 0;
 	}
 
+	// if terrain not valid, stop estimating
 	if (!_validTZ) {
 		dx(X_tz) = 0;
 	}
@@ -629,28 +623,6 @@ void BlockLocalPositionEstimator::detectDistanceSensors()
 	}
 }
 
-void BlockLocalPositionEstimator::updateHome()
-{
-	double lat = _sub_home.get().lat;
-	double lon = _sub_home.get().lon;
-	float alt = _sub_home.get().alt;
-
-	// updating home causes absolute measurements
-	// like gps and baro to be off, need to allow it
-	// to reset by resetting covariance
-	initP();
-	mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] home "
-				     "lat %6.2f lon %6.2f alt %5.1f m",
-				     lat, lon, double(alt));
-	map_projection_init(&_map_ref, lat, lon);
-	float delta_alt = alt - _altHome;
-	_altHomeInitialized = true;
-	_altHome = alt;
-	_gpsAltHome += delta_alt;
-	_baroAltHome +=  delta_alt;
-	_visionHome(2) += delta_alt;
-	_mocapHome(2) += delta_alt;
-}
 
 void BlockLocalPositionEstimator::publishLocalPos()
 {
@@ -672,12 +644,12 @@ void BlockLocalPositionEstimator::publishLocalPos()
 		_pub_lpos.get().vy = xLP(X_vy); // east
 		_pub_lpos.get().vz = xLP(X_vz); // down
 		_pub_lpos.get().yaw = _sub_att.get().yaw;
-		_pub_lpos.get().xy_global = _sub_home.get().timestamp != 0; // need home for reference
+		_pub_lpos.get().xy_global = _validXY;
 		_pub_lpos.get().z_global = _baroInitialized;
-		_pub_lpos.get().ref_timestamp = _sub_home.get().timestamp;
+		_pub_lpos.get().ref_timestamp = _timeStamp;
 		_pub_lpos.get().ref_lat = _map_ref.lat_rad * 180 / M_PI;
 		_pub_lpos.get().ref_lon = _map_ref.lon_rad * 180 / M_PI;
-		_pub_lpos.get().ref_alt = _sub_home.get().alt;
+		_pub_lpos.get().ref_alt = _altOrigin;
 		_pub_lpos.get().dist_bottom = _aglLowPass.getState();
 		_pub_lpos.get().dist_bottom_rate = - xLP(X_vz);
 		_pub_lpos.get().surface_bottom_timestamp = _timeStamp;
@@ -725,7 +697,7 @@ void BlockLocalPositionEstimator::publishGlobalPos()
 	double lon = 0;
 	const Vector<float, n_x> &xLP = _xLowPass.getState();
 	map_projection_reproject(&_map_ref, xLP(X_x), xLP(X_y), &lat, &lon);
-	float alt = -xLP(X_z) + _altHome;
+	float alt = -xLP(X_z) + _altOrigin;
 
 	if (PX4_ISFINITE(lat) && PX4_ISFINITE(lon) && PX4_ISFINITE(alt) &&
 	    PX4_ISFINITE(xLP(X_vx)) && PX4_ISFINITE(xLP(X_vy)) &&
@@ -741,7 +713,7 @@ void BlockLocalPositionEstimator::publishGlobalPos()
 		_pub_gpos.get().yaw = _sub_att.get().yaw;
 		_pub_gpos.get().eph = sqrtf(_P(X_x, X_x) + _P(X_y, X_y));
 		_pub_gpos.get().epv = sqrtf(_P(X_z, X_z));
-		_pub_gpos.get().terrain_alt = _altHome - xLP(X_tz);
+		_pub_gpos.get().terrain_alt = _altOrigin - xLP(X_tz);
 		_pub_gpos.get().terrain_alt_valid = _validTZ;
 		_pub_gpos.get().dead_reckoning = !_validXY && !_xyTimeout;
 		_pub_gpos.get().pressure_alt = _sub_sensor.get().baro_alt_meter;
diff --git a/src/modules/local_position_estimator/BlockLocalPositionEstimator.hpp b/src/modules/local_position_estimator/BlockLocalPositionEstimator.hpp
index abf858aa6a..4a612182b1 100644
--- a/src/modules/local_position_estimator/BlockLocalPositionEstimator.hpp
+++ b/src/modules/local_position_estimator/BlockLocalPositionEstimator.hpp
@@ -19,7 +19,6 @@
 #include <uORB/topics/distance_sensor.h>
 #include <uORB/topics/parameter_update.h>
 #include <uORB/topics/manual_control_setpoint.h>
-#include <uORB/topics/home_position.h>
 #include <uORB/topics/vehicle_gps_position.h>
 #include <uORB/topics/vision_position_estimate.h>
 #include <uORB/topics/att_pos_mocap.h>
@@ -205,7 +204,6 @@ private:
 	float agl();
 	void correctionLogic(Vector<float, n_x> &dx);
 	void detectDistanceSensors();
-	void updateHome();
 
 	// publications
 	void publishLocalPos();
@@ -222,7 +220,6 @@ private:
 	uORB::Subscription<sensor_combined_s> _sub_sensor;
 	uORB::Subscription<parameter_update_s> _sub_param_update;
 	uORB::Subscription<manual_control_setpoint_s> _sub_manual;
-	uORB::Subscription<home_position_s> _sub_home;
 	uORB::Subscription<vehicle_gps_position_s> _sub_gps;
 	uORB::Subscription<vision_position_estimate_s> _sub_vision_pos;
 	uORB::Subscription<att_pos_mocap_s> _sub_mocap;
@@ -292,9 +289,9 @@ private:
 	BlockParamFloat  _pn_b_noise_density;
 	BlockParamFloat  _t_max_grade;
 
-	// init home
-	BlockParamFloat  _init_home_lat;
-	BlockParamFloat  _init_home_lon;
+	// init origin
+	BlockParamFloat  _init_origin_lat;
+	BlockParamFloat  _init_origin_lon;
 
 	// flow gyro filter
 	BlockHighPass _flow_gyro_x_high_pass;
@@ -344,12 +341,12 @@ private:
 	bool _mocapInitialized;
 
 	// reference altitudes
-	float _altHome;
-	bool _altHomeInitialized;
-	float _baroAltHome;
-	float _gpsAltHome;
-	Vector3f _visionHome;
-	Vector3f _mocapHome;
+	float _altOrigin;
+	bool _altOriginInitialized;
+	float _baroAltOrigin;
+	float _gpsAltOrigin;
+	Vector3f _visionOrigin;
+	Vector3f _mocapOrigin;
 
 	// flow integration
 	float _flowX;
diff --git a/src/modules/local_position_estimator/params.c b/src/modules/local_position_estimator/params.c
index 361147235e..3543377a10 100644
--- a/src/modules/local_position_estimator/params.c
+++ b/src/modules/local_position_estimator/params.c
@@ -308,7 +308,7 @@ PARAM_DEFINE_FLOAT(LPE_T_MAX_GRADE, 1.0f);
 PARAM_DEFINE_FLOAT(LPE_FGYRO_HP, 0.1f);
 
 /**
- * Home latitude for nav w/o GPS
+ * Local origin latitude for nav w/o GPS
  *
  * @group Local Position Estimator
  * @unit deg
@@ -319,7 +319,7 @@ PARAM_DEFINE_FLOAT(LPE_FGYRO_HP, 0.1f);
 PARAM_DEFINE_FLOAT(LPE_LAT, 40.430f);
 
 /**
- * Home longitude for nav w/o GPS
+ * Local origin longitude for nav w/o GPS
  *
  * @group Local Position Estimator
  * @unit deg
diff --git a/src/modules/local_position_estimator/sensors/baro.cpp b/src/modules/local_position_estimator/sensors/baro.cpp
index ffe8e51611..1d0624a8c8 100644
--- a/src/modules/local_position_estimator/sensors/baro.cpp
+++ b/src/modules/local_position_estimator/sensors/baro.cpp
@@ -21,7 +21,7 @@ void BlockLocalPositionEstimator::baroInit()
 
 	// if finished
 	if (_baroStats.getCount() > REQ_BARO_INIT_COUNT) {
-		_baroAltHome = _baroStats.getMean()(0);
+		_baroAltOrigin = _baroStats.getMean()(0);
 		mavlink_and_console_log_info(&mavlink_log_pub,
 					     "[lpe] baro init %d m std %d cm",
 					     (int)_baroStats.getMean()(0),
@@ -29,10 +29,10 @@ void BlockLocalPositionEstimator::baroInit()
 		_baroInitialized = true;
 		_baroFault = FAULT_NONE;
 
-		// only initialize alt home with baro if gps is disabled
-		if (!_altHomeInitialized && !_gps_on.get()) {
-			_altHomeInitialized = true;
-			_altHome = _baroAltHome;
+		// only initialize alt origin with baro and no gps
+		if (!_altOriginInitialized && !_gps_on.get()) {
+			_altOriginInitialized = true;
+			_altOrigin = _baroAltOrigin;
 		}
 	}
 }
@@ -54,8 +54,8 @@ void BlockLocalPositionEstimator::baroCorrect()
 
 	if (baroMeasure(y) != OK) { return; }
 
-	// subtract baro home alt
-	y -= _baroAltHome;
+	// subtract baro origin alt
+	y -= _baroAltOrigin;
 
 	// baro measurement matrix
 	Matrix<float, n_y_baro, n_x> C;
diff --git a/src/modules/local_position_estimator/sensors/gps.cpp b/src/modules/local_position_estimator/sensors/gps.cpp
index e08119dddb..c003faf7ca 100644
--- a/src/modules/local_position_estimator/sensors/gps.cpp
+++ b/src/modules/local_position_estimator/sensors/gps.cpp
@@ -37,27 +37,27 @@ void BlockLocalPositionEstimator::gpsInit()
 
 	// if finished
 	if (_gpsStats.getCount() > REQ_GPS_INIT_COUNT) {
-		double gpsLatHome = _gpsStats.getMean()(0);
-		double gpsLonHome = _gpsStats.getMean()(1);
+		double gpsLatOrigin = _gpsStats.getMean()(0);
+		double gpsLonOrigin = _gpsStats.getMean()(1);
 
 		if (!_receivedGps) {
 			_receivedGps = true;
-			map_projection_init(&_map_ref, gpsLatHome, gpsLonHome);
+			map_projection_init(&_map_ref, gpsLatOrigin, gpsLonOrigin);
 		}
 
-		_gpsAltHome = _gpsStats.getMean()(2);
+		_gpsAltOrigin = _gpsStats.getMean()(2);
 		mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] gps init "
 					     "lat %6.2f lon %6.2f alt %5.1f m",
-					     gpsLatHome,
-					     gpsLonHome,
-					     double(_gpsAltHome));
+					     gpsLatOrigin,
+					     gpsLonOrigin,
+					     double(_gpsAltOrigin));
 		_gpsInitialized = true;
 		_gpsFault = FAULT_NONE;
 		_gpsStats.reset();
 
-		if (!_altHomeInitialized) {
-			_altHomeInitialized = true;
-			_altHome = _gpsAltHome;
+		if (!_altOriginInitialized) {
+			_altOriginInitialized = true;
+			_altOrigin = _gpsAltOrigin;
 		}
 	}
 }
@@ -92,7 +92,7 @@ void BlockLocalPositionEstimator::gpsCorrect()
 	float  alt = y_global(2);
 	float px = 0;
 	float py = 0;
-	float pz = -(alt - _gpsAltHome);
+	float pz = -(alt - _gpsAltOrigin);
 	map_projection_project(&_map_ref, lat, lon, &px, &py);
 	Vector<float, 6> y;
 	y.setZero();
diff --git a/src/modules/local_position_estimator/sensors/mocap.cpp b/src/modules/local_position_estimator/sensors/mocap.cpp
index c629b3f17b..d87f94593d 100644
--- a/src/modules/local_position_estimator/sensors/mocap.cpp
+++ b/src/modules/local_position_estimator/sensors/mocap.cpp
@@ -21,7 +21,7 @@ void BlockLocalPositionEstimator::mocapInit()
 
 	// if finished
 	if (_mocapStats.getCount() > REQ_MOCAP_INIT_COUNT) {
-		_mocapHome = _mocapStats.getMean();
+		_mocapOrigin = _mocapStats.getMean();
 		mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] mocap position init: "
 					     "%5.2f, %5.2f, %5.2f m std %5.2f, %5.2f, %5.2f m",
 					     double(_mocapStats.getMean()(0)),
@@ -33,9 +33,9 @@ void BlockLocalPositionEstimator::mocapInit()
 		_mocapInitialized = true;
 		_mocapFault = FAULT_NONE;
 
-		if (!_altHomeInitialized) {
-			_altHomeInitialized = true;
-			_altHome = _mocapHome(2);
+		if (!_altOriginInitialized) {
+			_altOriginInitialized = true;
+			_altOrigin = _mocapOrigin(2);
 		}
 	}
 }
@@ -58,8 +58,8 @@ void BlockLocalPositionEstimator::mocapCorrect()
 
 	if (mocapMeasure(y) != OK) { return; }
 
-	// make measurement relative to home
-	y -= _mocapHome;
+	// make measurement relative to origin
+	y -= _mocapOrigin;
 
 	// mocap measurement matrix, measures position
 	Matrix<float, n_y_mocap, n_x> C;
diff --git a/src/modules/local_position_estimator/sensors/vision.cpp b/src/modules/local_position_estimator/sensors/vision.cpp
index bb5f58e332..f0d9f8c0db 100644
--- a/src/modules/local_position_estimator/sensors/vision.cpp
+++ b/src/modules/local_position_estimator/sensors/vision.cpp
@@ -21,7 +21,7 @@ void BlockLocalPositionEstimator::visionInit()
 
 	// increament sums for mean
 	if (_visionStats.getCount() > REQ_VISION_INIT_COUNT) {
-		_visionHome = _visionStats.getMean();
+		_visionOrigin = _visionStats.getMean();
 		mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] vision position init: "
 					     "%5.2f %5.2f %5.2f m std %5.2f %5.2f %5.2f m",
 					     double(_visionStats.getMean()(0)),
@@ -33,9 +33,9 @@ void BlockLocalPositionEstimator::visionInit()
 		_visionInitialized = true;
 		_visionFault = FAULT_NONE;
 
-		if (!_altHomeInitialized) {
-			_altHomeInitialized = true;
-			_altHome = _visionHome(2);
+		if (!_altOriginInitialized) {
+			_altOriginInitialized = true;
+			_altOrigin = _visionOrigin(2);
 		}
 	}
 }
@@ -58,8 +58,8 @@ void BlockLocalPositionEstimator::visionCorrect()
 
 	if (visionMeasure(y) != OK) { return; }
 
-	// make measurement relative to home
-	y -= _visionHome;
+	// make measurement relative to origin
+	y -= _visionOrigin;
 
 	// vision measurement matrix, measures position
 	Matrix<float, n_y_vision, n_x> C;
diff --git a/src/modules/uORB/Subscription.cpp b/src/modules/uORB/Subscription.cpp
index b01efef7f1..071254c80e 100644
--- a/src/modules/uORB/Subscription.cpp
+++ b/src/modules/uORB/Subscription.cpp
@@ -63,6 +63,7 @@
 #include "topics/att_pos_mocap.h"
 #include "topics/vision_position_estimate.h"
 #include "topics/control_state.h"
+#include "topics/vehicle_land_detected.h"
 
 #include <px4_defines.h>
 
@@ -179,5 +180,6 @@ template class __EXPORT Subscription<distance_sensor_s>;
 template class __EXPORT Subscription<att_pos_mocap_s>;
 template class __EXPORT Subscription<vision_position_estimate_s>;
 template class __EXPORT Subscription<control_state_s>;
+template class __EXPORT Subscription<vehicle_land_detected_s>;
 
 } // namespace uORB

From 5b85fdb63604b5e24422a6b854f33cd434c42a49 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Beat=20K=C3=BCng?= <beat-kueng@gmx.net>
Date: Mon, 18 Jul 2016 15:25:49 +0200
Subject: [PATCH 1037/1230] fix sdlog2 replay: changed format string from
 9c73eae941c (#5090)

---
 src/modules/sdlog2/sdlog2_messages.h | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/src/modules/sdlog2/sdlog2_messages.h b/src/modules/sdlog2/sdlog2_messages.h
index 8cbcdba6e8..4d1f3f1f87 100644
--- a/src/modules/sdlog2/sdlog2_messages.h
+++ b/src/modules/sdlog2/sdlog2_messages.h
@@ -706,7 +706,7 @@ static const struct log_format_s log_formats[] = {
 	LOG_FORMAT(TSYN, "Q", 		"TimeOffset"),
 	LOG_FORMAT(MACS, "fff", "RRint,PRint,YRint"),
 	LOG_FORMAT(CAMT, "QI", "timestamp,seq"),
-	LOG_FORMAT(RPL1, "QQQQQffffffffff", "t,gIdt,aIdt,Tm,Tb,gIx,gIy,gIz,aIx,aIy,aIz,magX,magY,magZ,b_alt"),
+	LOG_FORMAT(RPL1, "QffQQffffffffff", "t,gIdt,aIdt,Tm,Tb,gx,gy,gz,ax,ay,az,magX,magY,magZ,b_alt"),
 	LOG_FORMAT(RPL2, "QQLLiMMfffffffM", "Tpos,Tvel,lat,lon,alt,fix,nsats,eph,epv,sacc,v,vN,vE,vD,v_val"),
 	LOG_FORMAT(RPL3, "QffffIB", "Tflow,fx,fy,gx,gy,delT,qual"),
 	LOG_FORMAT(RPL4, "Qf", "Trng,rng"),

From a420d4779797064e02c97ad9a44715188a4110af Mon Sep 17 00:00:00 2001
From: David Sidrane <david_s5@nscdg.com>
Date: Mon, 18 Jul 2016 15:01:55 -1000
Subject: [PATCH 1038/1230] Minor fix usage (#5094)

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

diff --git a/src/drivers/mpu6000/mpu6000.cpp b/src/drivers/mpu6000/mpu6000.cpp
index 7a479dc30f..fce303315e 100644
--- a/src/drivers/mpu6000/mpu6000.cpp
+++ b/src/drivers/mpu6000/mpu6000.cpp
@@ -2394,7 +2394,7 @@ usage()
 	warnx("missing command: try 'start', 'info', 'test', 'stop',\n'reset', 'regdump', 'factorytest', 'testerror'");
 	warnx("options:");
 	warnx("    -X    (external bus)");
-	warnx("    -M 6000|20608 (default 6000)");
+	warnx("    -T 6000|20608 (default 6000)");
 	warnx("    -R rotation");
 	warnx("    -a accel range (in g)");
 }

From 7b52eced6615fedcc45ce6d2ee28bc7ed1fca7ba Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Beat=20K=C3=BCng?= <beat-kueng@gmx.net>
Date: Tue, 19 Jul 2016 09:21:35 +0200
Subject: [PATCH 1039/1230] commander: write timestamp of commander_state topic
 on init and state change

---
 src/modules/commander/commander.cpp            | 1 +
 src/modules/commander/state_machine_helper.cpp | 1 +
 2 files changed, 2 insertions(+)

diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp
index de2293e34c..804b03172c 100644
--- a/src/modules/commander/commander.cpp
+++ b/src/modules/commander/commander.cpp
@@ -1267,6 +1267,7 @@ int commander_thread_main(int argc, char *argv[])
 	status_flags.rc_input_blocked = false;
 	status.rc_input_mode = vehicle_status_s::RC_IN_MODE_DEFAULT;
 	internal_state.main_state = commander_state_s::MAIN_STATE_MANUAL;
+	internal_state.timestamp = hrt_absolute_time();
 	main_state_prev = commander_state_s::MAIN_STATE_MAX;
 	status.nav_state = vehicle_status_s::NAVIGATION_STATE_MANUAL;
 	status.arming_state = vehicle_status_s::ARMING_STATE_INIT;
diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp
index 483f52cc08..dadc652009 100644
--- a/src/modules/commander/state_machine_helper.cpp
+++ b/src/modules/commander/state_machine_helper.cpp
@@ -407,6 +407,7 @@ main_state_transition(struct vehicle_status_s *status, main_state_t new_main_sta
 		if (internal_state->main_state != new_main_state) {
 			main_state_prev = internal_state->main_state;
 			internal_state->main_state = new_main_state;
+			internal_state->timestamp = hrt_absolute_time();
 		} else {
 			ret = TRANSITION_NOT_CHANGED;
 		}

From 5ded579bf38a507894769f62bd7993a6d6228fe2 Mon Sep 17 00:00:00 2001
From: David Sidrane <david_s5@nscdg.com>
Date: Tue, 19 Jul 2016 11:58:44 -1000
Subject: [PATCH 1040/1230] TAP:More IO Init (#5096)

---
 src/drivers/boards/tap-v1/board_config.h | 4 +++-
 src/drivers/boards/tap-v1/tap_init.c     | 6 ++++++
 2 files changed, 9 insertions(+), 1 deletion(-)

diff --git a/src/drivers/boards/tap-v1/board_config.h b/src/drivers/boards/tap-v1/board_config.h
index a5f377ef91..f1f5ac31d8 100644
--- a/src/drivers/boards/tap-v1/board_config.h
+++ b/src/drivers/boards/tap-v1/board_config.h
@@ -244,9 +244,11 @@ __BEGIN_DECLS
 #define GPIO_S2  (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTC|GPIO_PIN13)
 
 #define GPIO_PCON_RADIO (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTC|GPIO_PIN3)
-
 #define RF_RADIO_CONTOL(_on_true)	px4_arch_gpiowrite(GPIO_PCON_RADIO, !(_on_true))
 
+#define GPIO_TEMP_CONT (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN4)
+#define TEMP_CONTROL(_on_true)	px4_arch_gpiowrite(GPIO_TEMP_CONT, (_on_true))
+
 #define  FLASH_BASED_PARAMS
 /****************************************************************************************************
  * Public Types
diff --git a/src/drivers/boards/tap-v1/tap_init.c b/src/drivers/boards/tap-v1/tap_init.c
index 238963c941..d221456f00 100644
--- a/src/drivers/boards/tap-v1/tap_init.c
+++ b/src/drivers/boards/tap-v1/tap_init.c
@@ -133,6 +133,11 @@ __EXPORT void stm32_boardinitialize(void)
 
 	board_pwr_init(0);
 
+	/* TEMP ctrl Off (active high, init is clear) */
+
+	stm32_configgpio(GPIO_TEMP_CONT);
+
+
 	/* Select 0 */
 
 	stm32_configgpio(GPIO_S0);
@@ -143,6 +148,7 @@ __EXPORT void stm32_boardinitialize(void)
 
 	stm32_configgpio(GPIO_PCON_RADIO);
 
+
 	/* configure always-on ADC pins */
 
 	stm32_configgpio(GPIO_ADC1_IN10);

From 9a8c092116d2db8ffd421a3202ddfbffbcae944a Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Beat=20K=C3=BCng?= <beat-kueng@gmx.net>
Date: Tue, 19 Jul 2016 09:25:36 +0200
Subject: [PATCH 1041/1230] mavlink_messages.cpp: fix coding style: prepend _
 to class members

---
 src/modules/mavlink/mavlink_messages.cpp | 49 +++++++++++-------------
 1 file changed, 23 insertions(+), 26 deletions(-)

diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp
index f4938afc96..32054dd7f3 100644
--- a/src/modules/mavlink/mavlink_messages.cpp
+++ b/src/modules/mavlink/mavlink_messages.cpp
@@ -371,23 +371,20 @@ private:
 	MavlinkStreamStatustext(MavlinkStreamStatustext &);
 	MavlinkStreamStatustext& operator = (const MavlinkStreamStatustext &);
 
-	unsigned write_err_count = 0;
+	unsigned _write_err_count = 0;
 	static const unsigned write_err_threshold = 5;
 #ifndef __PX4_POSIX_EAGLE
-	FILE *fp;
+	FILE *_fp = nullptr;
 #endif
 
 protected:
 	explicit MavlinkStreamStatustext(Mavlink *mavlink) : MavlinkStream(mavlink)
-#ifndef __PX4_POSIX_EAGLE
-	, fp(nullptr)
-#endif
 	{}
 
 	~MavlinkStreamStatustext() {
 #ifndef __PX4_POSIX_EAGLE
-		if (fp != nullptr) {
-			fclose(fp);
+		if (_fp != nullptr) {
+			fclose(_fp);
 		}
 #endif
 	}
@@ -421,27 +418,27 @@ protected:
 				strftime(tstamp, sizeof(tstamp) - 1, "%Y_%m_%d_%H_%M_%S", &tt);
 
 				if (_mavlink->get_instance_id() == 0/* && _mavlink->get_logging_enabled()*/) {
-					if (fp != nullptr) {
-						fputs(tstamp, fp);
-						fputs(": ", fp);
-						if (EOF == fputs(msg.text, fp)) {
-							write_err_count++;
+					if (_fp != nullptr) {
+						fputs(tstamp, _fp);
+						fputs(": ", _fp);
+						if (EOF == fputs(msg.text, _fp)) {
+							_write_err_count++;
 						} else {
-							write_err_count = 0;
+							_write_err_count = 0;
 						}
 
-						if (write_err_count >= write_err_threshold) {
-							(void)fclose(fp);
-							fp = nullptr;
+						if (_write_err_count >= write_err_threshold) {
+							(void)fclose(_fp);
+							_fp = nullptr;
 							PX4_WARN("mavlink logging disabled");
 						} else {
-							(void)fputs("\n", fp);
+							(void)fputs("\n", _fp);
 #ifdef __PX4_NUTTX
-							fsync(fp->fs_filedes);
+							fsync(_fp->fs_filedes);
 #endif
 						}
 
-					} else if (write_err_count < write_err_threshold) {
+					} else if (_write_err_count < write_err_threshold) {
 						/* string to hold the path to the log */
 						char log_file_path[128];
 
@@ -449,16 +446,16 @@ protected:
 
 						/* store the log file in the root directory */
 						snprintf(log_file_path, sizeof(log_file_path) - 1, PX4_ROOTFSDIR"/fs/microsd/msgs_%s.txt", tstamp);
-						fp = fopen(log_file_path, "ab");
+						_fp = fopen(log_file_path, "ab");
 
-						if (fp != nullptr) {
+						if (_fp != nullptr) {
 							/* write first message */
-							fputs(tstamp, fp);
-							fputs(": ", fp);
-							fputs(msg.text, fp);
-							fputs("\n", fp);
+							fputs(tstamp, _fp);
+							fputs(": ", _fp);
+							fputs(msg.text, _fp);
+							fputs("\n", _fp);
 #ifdef __PX4_NUTTX
-							fsync(fp->fs_filedes);
+							fsync(_fp->fs_filedes);
 #endif
 						} else {
 							PX4_WARN("Failed to open MAVLink log: %s", log_file_path);

From 0f6a6f7150c1a1978c0abdfd4a649f9a9aae9eb8 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Beat=20K=C3=BCng?= <beat-kueng@gmx.net>
Date: Tue, 19 Jul 2016 09:27:26 +0200
Subject: [PATCH 1042/1230] mavlink_messages.cpp: only warn once, when MAVLink
 log opening fails

This happens for example if no SD card is present.
---
 src/modules/mavlink/mavlink_messages.cpp | 1 +
 1 file changed, 1 insertion(+)

diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp
index 32054dd7f3..86d7ba395f 100644
--- a/src/modules/mavlink/mavlink_messages.cpp
+++ b/src/modules/mavlink/mavlink_messages.cpp
@@ -459,6 +459,7 @@ protected:
 #endif
 						} else {
 							PX4_WARN("Failed to open MAVLink log: %s", log_file_path);
+							_write_err_count = write_err_threshold; //only try to open the file once
 						}
 					}
 				}

From 38e4882b5ff0c8387bac9dbc8d2d69c9f9105fa8 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Beat=20K=C3=BCng?= <beat-kueng@gmx.net>
Date: Tue, 19 Jul 2016 12:49:52 +0200
Subject: [PATCH 1043/1230] tap_esc: subscribe to test_motor topic and set
 outputs accordingly in non-armed state

This makes it possible to use the 'motor_test' command
---
 src/drivers/tap_esc/tap_esc.cpp | 55 +++++++++++++++++++++------------
 1 file changed, 36 insertions(+), 19 deletions(-)

diff --git a/src/drivers/tap_esc/tap_esc.cpp b/src/drivers/tap_esc/tap_esc.cpp
index 397dcdd1e5..65bb927790 100644
--- a/src/drivers/tap_esc/tap_esc.cpp
+++ b/src/drivers/tap_esc/tap_esc.cpp
@@ -44,6 +44,7 @@
 #include <uORB/topics/actuator_controls.h>
 #include <uORB/topics/actuator_outputs.h>
 #include <uORB/topics/actuator_armed.h>
+#include <uORB/topics/test_motor.h>
 #include <uORB/topics/input_rc.h>
 #include <uORB/topics/esc_status.h>
 
@@ -98,6 +99,7 @@ private:
 	// subscriptions
 	unsigned	_num_outputs;
 	int	_armed_sub;
+	int _test_motor_sub;
 	orb_advert_t        	_outputs_pub = nullptr;
 	actuator_outputs_s      _outputs;
 	static actuator_armed_s	_armed;
@@ -152,6 +154,7 @@ TAP_ESC::TAP_ESC():
 	_mode(MODE_4PWM),
 	_num_outputs(0),
 	_armed_sub(-1),
+	_test_motor_sub(-1),
 	_outputs_pub(nullptr),
 	_control_subs{ -1},
 	_esc_feedback_pub(nullptr),
@@ -437,6 +440,7 @@ TAP_ESC::cycle()
 		_outputs_pub = orb_advertise(ORB_ID(actuator_outputs), &_outputs);
 		_esc_feedback_pub = orb_advertise(ORB_ID(esc_report), &_esc_feedback);
 		_armed_sub = orb_subscribe(ORB_ID(actuator_armed));
+		_test_motor_sub = orb_subscribe(ORB_ID(test_motor));
 		_initialized = true;
 	}
 
@@ -477,19 +481,14 @@ TAP_ESC::cycle()
 
 
 	/* check if anything updated */
-	int ret = ::poll(_poll_fds, _poll_fds_num, 0);
+	int ret = ::poll(_poll_fds, _poll_fds_num, 5);
 
 
 	/* this would be bad... */
 	if (ret < 0) {
 		DEVICE_LOG("poll error %d", errno);
 
-	} else if (ret == 0) {
-		/* timeout: no control data, switch to failsafe values */
-//		warnx("no PWM: failsafe");
-		//printf("timeout\n");
-
-	} else {
+	} else { /* update even in the case of a timeout, to check for test_motor commands */
 
 		/* get controls for required topics */
 		unsigned poll_id = 0;
@@ -539,10 +538,8 @@ TAP_ESC::cycle()
 			_outputs.timestamp = hrt_absolute_time();
 
 			/* disable unused ports by setting their output to NaN */
-			for (size_t i = 0; i < sizeof(_outputs.output) / sizeof(_outputs.output[0]); i++) {
-				if (i >= num_outputs) {
-					_outputs.output[i] = NAN;
-				}
+			for (size_t i = num_outputs; i < sizeof(_outputs.output) / sizeof(_outputs.output[0]); i++) {
+				_outputs.output[i] = NAN;
 			}
 
 			/* iterate actuators */
@@ -570,18 +567,30 @@ TAP_ESC::cycle()
 			_outputs.noutputs = num_outputs;
 			_outputs.timestamp = hrt_absolute_time();
 
-			for (unsigned i = 0; i < num_outputs; i++) {
-
-				_outputs.output[i] = 900;
+			/* check for motor test commands */
+			bool test_motor_updated;
+			orb_check(_test_motor_sub, &test_motor_updated);
 
+			if (test_motor_updated) {
+				struct test_motor_s test_motor;
+				orb_copy(ORB_ID(test_motor), _test_motor_sub, &test_motor);
+				_outputs.output[test_motor.motor_number] = 900.f + (1100.f * test_motor.value); //scale to [900, 2000]
+				PX4_INFO("setting motor %i to %.1lf", test_motor.motor_number,
+					 (double)_outputs.output[test_motor.motor_number]);
 			}
 
-			for (size_t i = 0; i < sizeof(_outputs.output) / sizeof(_outputs.output[0]); i++) {
-				if (i >= num_outputs) {
-					_outputs.output[i] = NAN;
+			/* set the invalid values to the minimum */
+			for (unsigned i = 0; i < num_outputs; i++) {
+				if (!PX4_ISFINITE(_outputs.output[i])) {
+					_outputs.output[i] = 900;
 				}
 			}
 
+			/* disable unused ports by setting their output to NaN */
+			for (size_t i = num_outputs; i < sizeof(_outputs.output) / sizeof(_outputs.output[0]); i++) {
+				_outputs.output[i] = NAN;
+			}
+
 		}
 
 		const unsigned esc_count = 4;
@@ -630,6 +639,11 @@ TAP_ESC::cycle()
 		orb_copy(ORB_ID(actuator_armed), _armed_sub, &_armed);
 		/* do not obey the lockdown value, as lockdown is for PWMSim */
 		IS_armed = _armed.armed;
+
+		/* reset all outputs */
+		for (size_t i = 0; i < sizeof(_outputs.output) / sizeof(_outputs.output[0]); i++) {
+			_outputs.output[i] = NAN;
+		}
 	}
 
 
@@ -639,12 +653,15 @@ void TAP_ESC::work_stop()
 {
 	for (unsigned i = 0; i < actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS; i++) {
 		if (_control_subs[i] > 0) {
-			::close(_control_subs[i]);
+			orb_unsubscribe(_control_subs[i]);
 			_control_subs[i] = -1;
 		}
 	}
 
-	::close(_armed_sub);
+	orb_unsubscribe(_armed_sub);
+	_armed_sub = -1;
+	orb_unsubscribe(_test_motor_sub);
+	_test_motor_sub = -1;
 
 	DEVICE_LOG("stopping");
 	_initialized = false;

From 2c0c30eadfc72ece3e61fe7e28b02634111d489f Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Beat=20K=C3=BCng?= <beat-kueng@gmx.net>
Date: Tue, 19 Jul 2016 12:50:26 +0200
Subject: [PATCH 1044/1230] drv_tap_esc.h: fix file permissions

---
 src/drivers/tap_esc/drv_tap_esc.h | 0
 1 file changed, 0 insertions(+), 0 deletions(-)
 mode change 100755 => 100644 src/drivers/tap_esc/drv_tap_esc.h

diff --git a/src/drivers/tap_esc/drv_tap_esc.h b/src/drivers/tap_esc/drv_tap_esc.h
old mode 100755
new mode 100644

From 03b3bfa98d10f8647c240a59523ff3bf641a76c9 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Beat=20K=C3=BCng?= <beat-kueng@gmx.net>
Date: Tue, 19 Jul 2016 12:51:18 +0200
Subject: [PATCH 1045/1230] tap: add the motor_test command to the build config

---
 cmake/configs/nuttx_tap-v1_default.cmake | 1 +
 1 file changed, 1 insertion(+)

diff --git a/cmake/configs/nuttx_tap-v1_default.cmake b/cmake/configs/nuttx_tap-v1_default.cmake
index 727ba5712f..0ea32abc9a 100644
--- a/cmake/configs/nuttx_tap-v1_default.cmake
+++ b/cmake/configs/nuttx_tap-v1_default.cmake
@@ -32,6 +32,7 @@ set(config_module_list
 	systemcmds/param
 	systemcmds/perf
 	systemcmds/pwm
+	systemcmds/motor_test
 	systemcmds/reboot
 	systemcmds/top
 	systemcmds/config

From 8ab841e0467d8509db9f97019c640ea8da2cdaff Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Beat=20K=C3=BCng?= <beat-kueng@gmx.net>
Date: Thu, 21 Jul 2016 23:52:32 +0200
Subject: [PATCH 1046/1230] sensors: poll on best-voted gyro (#5106)

This is a follow-up to 399d4ef833cd72ac0
---
 src/modules/sensors/sensors.cpp | 27 +++++++++------------------
 1 file changed, 9 insertions(+), 18 deletions(-)

diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp
index b9dc1ca382..f9f937a7ff 100644
--- a/src/modules/sensors/sensors.cpp
+++ b/src/modules/sensors/sensors.cpp
@@ -2166,7 +2166,7 @@ Sensors::check_failover(SensorData &sensor, const char *sensor_name)
 			PX4_INFO("%s sensor switch from #%i", sensor_name, sensor.voter.failover_index());
 
 		} else {
-			mavlink_and_console_log_emergency(&_mavlink_log_pub, "%s #%i failure :%s%s%s%s%s!",
+			mavlink_and_console_log_emergency(&_mavlink_log_pub, "%s #%i failover :%s%s%s%s%s!",
 							  sensor_name,
 							  sensor.voter.failover_index(),
 							  ((flags & DataValidator::ERROR_FLAG_NO_DATA) ? " No data" : ""),
@@ -2230,9 +2230,6 @@ Sensors::init_sensor_class(const struct orb_metadata *meta, SensorData &sensor_d
 	for (unsigned i = 0; i < group_count; i++) {
 		if (sensor_data.subscription[i] < 0) {
 			sensor_data.subscription[i] = orb_subscribe_multi(meta, i);
-			int32_t priority;
-			orb_priority(sensor_data.subscription[i], &priority);
-			sensor_data.priority[i] = (uint8_t)priority;
 		}
 
 		int32_t priority;
@@ -2320,10 +2317,10 @@ Sensors::task_main()
 	/* advertise the sensor_combined topic and make the initial publication */
 	_sensor_pub = orb_advertise(ORB_ID(sensor_combined), &raw);
 
-	/* wakeup source(s) */
-	px4_pollfd_struct_t fds[SENSOR_COUNT_MAX] = {};
+	/* wakeup source */
+	px4_pollfd_struct_t poll_fds = {};
 
-	int num_poll_fds = 0;
+	poll_fds.events = POLLIN;
 
 	_task_should_exit = false;
 
@@ -2331,18 +2328,12 @@ Sensors::task_main()
 
 	while (!_task_should_exit) {
 
-		/* use the gyro(s) to pace output */
-		if (num_poll_fds != _gyro.subscription_count) { //happens the first time we enter, or when new gyro added
-			num_poll_fds = _gyro.subscription_count;
+		/* use the best-voted gyro to pace output */
+		poll_fds.fd = _gyro.subscription[_gyro.last_best_vote];
 
-			for (int i = 0; i < _gyro.subscription_count; ++i) {
-				fds[i].fd = _gyro.subscription[i];
-				fds[i].events = POLLIN;
-			}
-		}
-
-		/* wait for up to 50ms for data */
-		int pret = px4_poll(fds, 1, 50);
+		/* wait for up to 50ms for data (Note that this implies, we can have a fail-over time of 50ms,
+		 * if a gyro fails) */
+		int pret = px4_poll(&poll_fds, 1, 50);
 
 		/* if pret == 0 it timed out - periodic check for _task_should_exit, etc. */
 

From e73d8d73e1ed61deed70b29f7807b88bc16f292d Mon Sep 17 00:00:00 2001
From: Michael Schaeuble <schaeuble.michael@gmail.com>
Date: Wed, 29 Jun 2016 16:55:21 +0200
Subject: [PATCH 1047/1230] Add DF wrapper for MPU6050

---
 .../drivers/df_mpu6050_wrapper/CMakeLists.txt |  46 ++
 .../df_mpu6050_wrapper/df_mpu6050_wrapper.cpp | 712 ++++++++++++++++++
 2 files changed, 758 insertions(+)
 create mode 100644 src/platforms/posix/drivers/df_mpu6050_wrapper/CMakeLists.txt
 create mode 100644 src/platforms/posix/drivers/df_mpu6050_wrapper/df_mpu6050_wrapper.cpp

diff --git a/src/platforms/posix/drivers/df_mpu6050_wrapper/CMakeLists.txt b/src/platforms/posix/drivers/df_mpu6050_wrapper/CMakeLists.txt
new file mode 100644
index 0000000000..3c5514e600
--- /dev/null
+++ b/src/platforms/posix/drivers/df_mpu6050_wrapper/CMakeLists.txt
@@ -0,0 +1,46 @@
+############################################################################
+#
+#   Copyright (c) 2016 PX4 Development Team. All rights reserved.
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions
+# are met:
+#
+# 1. Redistributions of source code must retain the above copyright
+#    notice, this list of conditions and the following disclaimer.
+# 2. Redistributions in binary form must reproduce the above copyright
+#    notice, this list of conditions and the following disclaimer in
+#    the documentation and/or other materials provided with the
+#    distribution.
+# 3. Neither the name PX4 nor the names of its contributors may be
+#    used to endorse or promote products derived from this software
+#    without specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.
+#
+############################################################################
+
+include_directories(../../../../lib/DriverFramework/drivers)
+
+px4_add_module(
+	MODULE platforms__posix__drivers__df_mpu6050_wrapper
+	MAIN df_mpu6050_wrapper
+	SRCS
+		df_mpu6050_wrapper.cpp
+	DEPENDS
+		platforms__common
+		df_driver_framework
+		df_mpu6050
+	)
+# vim: set noet ft=cmake fenc=utf-8 ff=unix :
diff --git a/src/platforms/posix/drivers/df_mpu6050_wrapper/df_mpu6050_wrapper.cpp b/src/platforms/posix/drivers/df_mpu6050_wrapper/df_mpu6050_wrapper.cpp
new file mode 100644
index 0000000000..00ffc37b30
--- /dev/null
+++ b/src/platforms/posix/drivers/df_mpu6050_wrapper/df_mpu6050_wrapper.cpp
@@ -0,0 +1,712 @@
+/****************************************************************************
+ *
+ * Copyright (c) 2016 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file df_mpu6050_wrapper.cpp
+ * Lightweight driver to access the MPU6050 of the DriverFramework.
+ */
+
+#include <px4_config.h>
+
+#include <sys/types.h>
+#include <sys/stat.h>
+#include <stdint.h>
+#include <stddef.h>
+#include <stdlib.h>
+#include <string.h>
+#include <math.h>
+#include <unistd.h>
+#include <px4_getopt.h>
+#include <errno.h>
+
+#include <systemlib/err.h>
+#include <systemlib/perf_counter.h>
+#include <systemlib/mavlink_log.h>
+
+#include <drivers/drv_hrt.h>
+#include <drivers/drv_accel.h>
+#include <drivers/drv_gyro.h>
+#include <drivers/device/integrator.h>
+
+#include <lib/conversion/rotation.h>
+
+#include <uORB/topics/parameter_update.h>
+
+#include <mpu6050/MPU6050.hpp>
+#include <DevMgr.hpp>
+
+// We don't want to auto publish, therefore set this to 0.
+#define MPU6050_NEVER_AUTOPUBLISH_US 0
+
+
+extern "C" { __EXPORT int df_mpu6050_wrapper_main(int argc, char *argv[]); }
+
+using namespace DriverFramework;
+
+
+class DfMPU6050Wrapper : public MPU6050
+{
+public:
+	DfMPU6050Wrapper(enum Rotation rotation);
+	~DfMPU6050Wrapper();
+
+
+	/**
+	 * Start automatic measurement.
+	 *
+	 * @return 0 on success
+	 */
+	int		start();
+
+	/**
+	 * Stop automatic measurement.
+	 *
+	 * @return 0 on success
+	 */
+	int		stop();
+
+	/**
+	 * Print some debug info.
+	 */
+	void		info();
+
+private:
+	int _publish(struct imu_sensor_data &data);
+
+	void _update_accel_calibration();
+	void _update_gyro_calibration();
+
+	orb_advert_t		    _accel_topic;
+	orb_advert_t		    _gyro_topic;
+
+	orb_advert_t		    _mavlink_log_pub;
+
+	int			    _param_update_sub;
+
+	struct accel_calibration_s {
+		float x_offset;
+		float x_scale;
+		float y_offset;
+		float y_scale;
+		float z_offset;
+		float z_scale;
+	} _accel_calibration;
+
+	struct gyro_calibration_s {
+		float x_offset;
+		float x_scale;
+		float y_offset;
+		float y_scale;
+		float z_offset;
+		float z_scale;
+	} _gyro_calibration;
+
+	math::Matrix<3, 3>	    _rotation_matrix;
+
+	int			    _accel_orb_class_instance;
+	int			    _gyro_orb_class_instance;
+
+	Integrator		    _accel_int;
+	Integrator		    _gyro_int;
+
+	unsigned		    _publish_count;
+
+	perf_counter_t		    _read_counter;
+	perf_counter_t		    _error_counter;
+	perf_counter_t		    _fifo_overflow_counter;
+	perf_counter_t		    _fifo_corruption_counter;
+	perf_counter_t		    _gyro_range_hit_counter;
+	perf_counter_t		    _accel_range_hit_counter;
+	perf_counter_t		    _publish_perf;
+
+	hrt_abstime		    _last_accel_range_hit_time;
+	uint64_t		    _last_accel_range_hit_count;
+
+};
+
+DfMPU6050Wrapper::DfMPU6050Wrapper(enum Rotation rotation) :
+	MPU6050(IMU_DEVICE_PATH),
+	_accel_topic(nullptr),
+	_gyro_topic(nullptr),
+	_mavlink_log_pub(nullptr),
+	_param_update_sub(-1),
+	_accel_calibration{},
+	_gyro_calibration{},
+	_accel_orb_class_instance(-1),
+	_gyro_orb_class_instance(-1),
+	_accel_int(MPU6050_NEVER_AUTOPUBLISH_US, false),
+	_gyro_int(MPU6050_NEVER_AUTOPUBLISH_US, true),
+	_publish_count(0),
+	_read_counter(perf_alloc(PC_COUNT, "mpu6050_reads")),
+	_error_counter(perf_alloc(PC_COUNT, "mpu6050_errors")),
+	_fifo_overflow_counter(perf_alloc(PC_COUNT, "mpu6050_fifo_overflows")),
+	_fifo_corruption_counter(perf_alloc(PC_COUNT, "mpu6050_fifo_corruptions")),
+	_gyro_range_hit_counter(perf_alloc(PC_COUNT, "mpu6050_gyro_range_hits")),
+	_accel_range_hit_counter(perf_alloc(PC_COUNT, "mpu6050_accel_range_hits")),
+	_publish_perf(perf_alloc(PC_ELAPSED, "mpu6050_publish")),
+	_last_accel_range_hit_time(0),
+	_last_accel_range_hit_count(0)
+{
+	// Set sane default calibration values
+	_accel_calibration.x_scale = 1.0f;
+	_accel_calibration.y_scale = 1.0f;
+	_accel_calibration.z_scale = 1.0f;
+	_accel_calibration.x_offset = 0.0f;
+	_accel_calibration.y_offset = 0.0f;
+	_accel_calibration.z_offset = 0.0f;
+
+	_gyro_calibration.x_scale = 1.0f;
+	_gyro_calibration.y_scale = 1.0f;
+	_gyro_calibration.z_scale = 1.0f;
+	_gyro_calibration.x_offset = 0.0f;
+	_gyro_calibration.y_offset = 0.0f;
+	_gyro_calibration.z_offset = 0.0f;
+
+	// Get sensor rotation matrix
+	get_rot_matrix(rotation, &_rotation_matrix);
+}
+
+DfMPU6050Wrapper::~DfMPU6050Wrapper()
+{
+	perf_free(_read_counter);
+	perf_free(_error_counter);
+	perf_free(_fifo_overflow_counter);
+	perf_free(_fifo_corruption_counter);
+	perf_free(_gyro_range_hit_counter);
+	perf_free(_accel_range_hit_counter);
+
+	perf_free(_publish_perf);
+}
+
+int DfMPU6050Wrapper::start()
+{
+	/* Subscribe to param update topic. */
+	if (_param_update_sub < 0) {
+		_param_update_sub = orb_subscribe(ORB_ID(parameter_update));
+	}
+
+	/* Init device and start sensor. */
+	int ret = init();
+
+	if (ret != 0) {
+		PX4_ERR("MPU6050 init fail: %d", ret);
+		return ret;
+	}
+
+	ret = MPU6050::start();
+
+	if (ret != 0) {
+		PX4_ERR("MPU6050 start fail: %d", ret);
+		return ret;
+	}
+
+	PX4_DEBUG("MPU6050 device id is: %d", m_id.dev_id);
+
+	/* Force getting the calibration values. */
+	_update_accel_calibration();
+	_update_gyro_calibration();
+
+	return 0;
+}
+
+int DfMPU6050Wrapper::stop()
+{
+	/* Stop sensor. */
+	int ret = MPU6050::stop();
+
+	if (ret != 0) {
+		PX4_ERR("MPU6050 stop fail: %d", ret);
+		return ret;
+	}
+
+	return 0;
+}
+
+void DfMPU6050Wrapper::info()
+{
+	perf_print_counter(_read_counter);
+	perf_print_counter(_error_counter);
+	perf_print_counter(_fifo_overflow_counter);
+	perf_print_counter(_fifo_corruption_counter);
+	perf_print_counter(_gyro_range_hit_counter);
+	perf_print_counter(_accel_range_hit_counter);
+
+	perf_print_counter(_publish_perf);
+}
+
+void DfMPU6050Wrapper::_update_gyro_calibration()
+{
+	// TODO: replace magic number
+	for (unsigned i = 0; i < 3; ++i) {
+
+		// TODO: remove printfs and add error counter
+
+		char str[30];
+		(void)sprintf(str, "CAL_GYRO%u_ID", i);
+		int32_t device_id;
+		int res = param_get(param_find(str), &device_id);
+
+		if (res != OK) {
+			PX4_ERR("Could not access param %s", str);
+			continue;
+		}
+
+		if ((uint32_t)device_id != m_id.dev_id) {
+			continue;
+		}
+
+		(void)sprintf(str, "CAL_GYRO%u_XSCALE", i);
+		res = param_get(param_find(str), &_gyro_calibration.x_scale);
+
+		if (res != OK) {
+			PX4_ERR("Could not access param %s", str);
+		}
+
+		(void)sprintf(str, "CAL_GYRO%u_YSCALE", i);
+		res = param_get(param_find(str), &_gyro_calibration.y_scale);
+
+		if (res != OK) {
+			PX4_ERR("Could not access param %s", str);
+		}
+
+		(void)sprintf(str, "CAL_GYRO%u_ZSCALE", i);
+		res = param_get(param_find(str), &_gyro_calibration.z_scale);
+
+		if (res != OK) {
+			PX4_ERR("Could not access param %s", str);
+		}
+
+		(void)sprintf(str, "CAL_GYRO%u_XOFF", i);
+		res = param_get(param_find(str), &_gyro_calibration.x_offset);
+
+		if (res != OK) {
+			PX4_ERR("Could not access param %s", str);
+		}
+
+		(void)sprintf(str, "CAL_GYRO%u_YOFF", i);
+		res = param_get(param_find(str), &_gyro_calibration.y_offset);
+
+		if (res != OK) {
+			PX4_ERR("Could not access param %s", str);
+		}
+
+		(void)sprintf(str, "CAL_GYRO%u_ZOFF", i);
+		res = param_get(param_find(str), &_gyro_calibration.z_offset);
+
+		if (res != OK) {
+			PX4_ERR("Could not access param %s", str);
+		}
+
+		// We got calibration values, let's exit.
+		return;
+	}
+
+	_gyro_calibration.x_scale = 1.0f;
+	_gyro_calibration.y_scale = 1.0f;
+	_gyro_calibration.z_scale = 1.0f;
+	_gyro_calibration.x_offset = 0.0f;
+	_gyro_calibration.y_offset = 0.0f;
+	_gyro_calibration.z_offset = 0.0f;
+}
+
+void DfMPU6050Wrapper::_update_accel_calibration()
+{
+	// TODO: replace magic number
+	for (unsigned i = 0; i < 3; ++i) {
+
+		// TODO: remove printfs and add error counter
+
+		char str[30];
+		(void)sprintf(str, "CAL_ACC%u_ID", i);
+		int32_t device_id;
+		int res = param_get(param_find(str), &device_id);
+
+		if (res != OK) {
+			PX4_ERR("Could not access param %s", str);
+			continue;
+		}
+
+		if ((uint32_t)device_id != m_id.dev_id) {
+			continue;
+		}
+
+		(void)sprintf(str, "CAL_ACC%u_XSCALE", i);
+		res = param_get(param_find(str), &_accel_calibration.x_scale);
+
+		if (res != OK) {
+			PX4_ERR("Could not access param %s", str);
+		}
+
+		(void)sprintf(str, "CAL_ACC%u_YSCALE", i);
+		res = param_get(param_find(str), &_accel_calibration.y_scale);
+
+		if (res != OK) {
+			PX4_ERR("Could not access param %s", str);
+		}
+
+		(void)sprintf(str, "CAL_ACC%u_ZSCALE", i);
+		res = param_get(param_find(str), &_accel_calibration.z_scale);
+
+		if (res != OK) {
+			PX4_ERR("Could not access param %s", str);
+		}
+
+		(void)sprintf(str, "CAL_ACC%u_XOFF", i);
+		res = param_get(param_find(str), &_accel_calibration.x_offset);
+
+		if (res != OK) {
+			PX4_ERR("Could not access param %s", str);
+		}
+
+		(void)sprintf(str, "CAL_ACC%u_YOFF", i);
+		res = param_get(param_find(str), &_accel_calibration.y_offset);
+
+		if (res != OK) {
+			PX4_ERR("Could not access param %s", str);
+		}
+
+		(void)sprintf(str, "CAL_ACC%u_ZOFF", i);
+		res = param_get(param_find(str), &_accel_calibration.z_offset);
+
+		if (res != OK) {
+			PX4_ERR("Could not access param %s", str);
+		}
+
+		// We got calibration values, let's exit.
+		return;
+	}
+
+	// Set sane default calibration values
+	_accel_calibration.x_scale = 1.0f;
+	_accel_calibration.y_scale = 1.0f;
+	_accel_calibration.z_scale = 1.0f;
+	_accel_calibration.x_offset = 0.0f;
+	_accel_calibration.y_offset = 0.0f;
+	_accel_calibration.z_offset = 0.0f;
+}
+
+int DfMPU6050Wrapper::_publish(struct imu_sensor_data &data)
+{
+	/* Check if calibration values are still up-to-date. */
+	bool updated;
+	orb_check(_param_update_sub, &updated);
+
+	if (updated) {
+		parameter_update_s parameter_update;
+		orb_copy(ORB_ID(parameter_update), _param_update_sub, &parameter_update);
+
+		_update_accel_calibration();
+		_update_gyro_calibration();
+	}
+
+	math::Vector<3> vec_integrated_unused;
+	uint64_t integral_dt_unused;
+
+	math::Vector<3> accel_val((data.accel_m_s2_x - _accel_calibration.x_offset) * _accel_calibration.x_scale,
+				  (data.accel_m_s2_y - _accel_calibration.y_offset) * _accel_calibration.y_scale,
+				  (data.accel_m_s2_z - _accel_calibration.z_offset) * _accel_calibration.z_scale);
+
+	// apply sensor rotation on the accel measurement
+	accel_val = _rotation_matrix * accel_val;
+
+	_accel_int.put_with_interval(data.fifo_sample_interval_us,
+				     accel_val,
+				     vec_integrated_unused,
+				     integral_dt_unused);
+
+	math::Vector<3> gyro_val((data.gyro_rad_s_x - _gyro_calibration.x_offset) * _gyro_calibration.x_scale,
+				 (data.gyro_rad_s_y - _gyro_calibration.y_offset) * _gyro_calibration.y_scale,
+				 (data.gyro_rad_s_z - _gyro_calibration.z_offset) * _gyro_calibration.z_scale);
+
+	// apply sensor rotation on the gyro measurement
+	gyro_val = _rotation_matrix * gyro_val;
+
+	_gyro_int.put_with_interval(data.fifo_sample_interval_us,
+				    gyro_val,
+				    vec_integrated_unused,
+				    integral_dt_unused);
+
+	// If we are not receiving the last sample from the FIFO buffer yet, let's stop here
+	// and wait for more packets.
+	if (!data.is_last_fifo_sample) {
+		return 0;
+	}
+
+	// The driver empties the FIFO buffer at 1kHz, however we only need to publish at 250Hz.
+	// Therefore, only publish every forth time.
+	++_publish_count;
+
+	if (_publish_count < 4) {
+		return 0;
+	}
+
+	_publish_count = 0;
+
+	// Update all the counters.
+	perf_set_count(_read_counter, data.read_counter);
+	perf_set_count(_error_counter, data.error_counter);
+	perf_set_count(_fifo_overflow_counter, data.fifo_overflow_counter);
+	perf_set_count(_fifo_corruption_counter, data.fifo_overflow_counter);
+	perf_set_count(_gyro_range_hit_counter, data.gyro_range_hit_counter);
+	perf_set_count(_accel_range_hit_counter, data.accel_range_hit_counter);
+
+	perf_begin(_publish_perf);
+
+	accel_report accel_report = {};
+	gyro_report gyro_report = {};
+
+	accel_report.timestamp = gyro_report.timestamp = hrt_absolute_time();
+
+	// TODO: get these right
+	gyro_report.scaling = -1.0f;
+	gyro_report.range_rad_s = -1.0f;
+	gyro_report.device_id = m_id.dev_id;
+
+	accel_report.scaling = -1.0f;
+	accel_report.range_m_s2 = -1.0f;
+	accel_report.device_id = m_id.dev_id;
+
+	// TODO: remove these (or get the values)
+	gyro_report.x_raw = NAN;
+	gyro_report.y_raw = NAN;
+	gyro_report.z_raw = NAN;
+
+	accel_report.x_raw = NAN;
+	accel_report.y_raw = NAN;
+	accel_report.z_raw = NAN;
+
+	math::Vector<3> gyro_val_filt;
+	math::Vector<3> accel_val_filt;
+
+	// Read and reset.
+	math::Vector<3> gyro_val_integ = _gyro_int.get_and_filtered(true, gyro_report.integral_dt, gyro_val_filt);
+	math::Vector<3> accel_val_integ = _accel_int.get_and_filtered(true, accel_report.integral_dt, accel_val_filt);
+
+	// Use the filtered (by integration) values to get smoother / less noisy data.
+	gyro_report.x = gyro_val_filt(0);
+	gyro_report.y = gyro_val_filt(1);
+	gyro_report.z = gyro_val_filt(2);
+
+	accel_report.x = accel_val_filt(0);
+	accel_report.y = accel_val_filt(1);
+	accel_report.z = accel_val_filt(2);
+
+	gyro_report.x_integral = gyro_val_integ(0);
+	gyro_report.y_integral = gyro_val_integ(1);
+	gyro_report.z_integral = gyro_val_integ(2);
+
+	accel_report.x_integral = accel_val_integ(0);
+	accel_report.y_integral = accel_val_integ(1);
+	accel_report.z_integral = accel_val_integ(2);
+
+	// TODO: when is this ever blocked?
+	if (!(m_pub_blocked)) {
+
+		if (_gyro_topic == nullptr) {
+      _gyro_topic = orb_advertise_multi(ORB_ID(sensor_gyro), &gyro_report,
+                &_gyro_orb_class_instance, ORB_PRIO_DEFAULT);
+    }
+    else {
+			orb_publish(ORB_ID(sensor_gyro), _gyro_topic, &gyro_report);
+		}
+
+		if (_accel_topic == nullptr) {
+      _accel_topic = orb_advertise_multi(ORB_ID(sensor_accel), &accel_report,
+                &_accel_orb_class_instance, ORB_PRIO_DEFAULT);
+    }
+    else {
+			orb_publish(ORB_ID(sensor_accel), _accel_topic, &accel_report);
+		}
+
+		/* Notify anyone waiting for data. */
+		DevMgr::updateNotify(*this);
+
+		// Report if there are high vibrations, every 10 times it happens.
+		const bool threshold_reached = (data.accel_range_hit_counter - _last_accel_range_hit_count > 10);
+
+		// Report every 5s.
+		const bool due_to_report = (hrt_elapsed_time(&_last_accel_range_hit_time) > 5000000);
+
+		if (threshold_reached && due_to_report) {
+			mavlink_log_critical(&_mavlink_log_pub,
+					     "High accelerations, range exceeded %llu times",
+					     data.accel_range_hit_counter);
+
+			_last_accel_range_hit_time = hrt_absolute_time();
+			_last_accel_range_hit_count = data.accel_range_hit_counter;
+		}
+	}
+
+	perf_end(_publish_perf);
+
+	// TODO: check the return codes of this function
+	return 0;
+};
+
+
+namespace df_mpu6050_wrapper
+{
+
+DfMPU6050Wrapper *g_dev = nullptr;
+
+int start(enum Rotation rotation);
+int stop();
+int info();
+void usage();
+
+int start(enum Rotation rotation)
+{
+	g_dev = new DfMPU6050Wrapper(rotation);
+
+	if (g_dev == nullptr) {
+		PX4_ERR("failed instantiating DfMPU6050Wrapper object");
+		return -1;
+	}
+
+	int ret = g_dev->start();
+
+	if (ret != 0) {
+		PX4_ERR("DfMPU6050Wrapper start failed");
+		return ret;
+	}
+
+	// Open the IMU sensor
+	DevHandle h;
+	DevMgr::getHandle(IMU_DEVICE_PATH, h);
+
+	if (!h.isValid()) {
+		DF_LOG_INFO("Error: unable to obtain a valid handle for the receiver at: %s (%d)",
+			    IMU_DEVICE_PATH, h.getError());
+		return -1;
+	}
+
+	DevMgr::releaseHandle(h);
+
+	return 0;
+}
+
+int stop()
+{
+	if (g_dev == nullptr) {
+		PX4_ERR("driver not running");
+		return 1;
+	}
+
+	int ret = g_dev->stop();
+
+	if (ret != 0) {
+		PX4_ERR("driver could not be stopped");
+		return ret;
+	}
+
+	delete g_dev;
+	g_dev = nullptr;
+	return 0;
+}
+
+/**
+ * Print a little info about the driver.
+ */
+int
+info()
+{
+	if (g_dev == nullptr) {
+		PX4_ERR("driver not running");
+		return 1;
+	}
+
+	PX4_INFO("state @ %p", g_dev);
+	g_dev->info();
+
+	return 0;
+}
+
+void
+usage()
+{
+	PX4_INFO("Usage: df_mpu6050_wrapper 'start', 'info', 'stop'");
+	PX4_INFO("options:");
+	PX4_INFO("    -R rotation");
+}
+
+} // namespace df_mpu6050_wrapper
+
+
+int
+df_mpu6050_wrapper_main(int argc, char *argv[])
+{
+	int ch;
+	enum Rotation rotation = ROTATION_NONE;
+	int ret = 0;
+	int myoptind = 1;
+	const char *myoptarg = NULL;
+
+	/* jump over start/off/etc and look at options first */
+	while ((ch = px4_getopt(argc, argv, "R:", &myoptind, &myoptarg)) != EOF) {
+		switch (ch) {
+		case 'R':
+			rotation = (enum Rotation)atoi(myoptarg);
+			break;
+
+		default:
+			df_mpu6050_wrapper::usage();
+			return 0;
+		}
+	}
+
+	if (argc <= 1) {
+		df_mpu6050_wrapper::usage();
+		return 1;
+	}
+
+	const char *verb = argv[myoptind];
+
+	if (!strcmp(verb, "start")) {
+		ret = df_mpu6050_wrapper::start(rotation);
+	}
+
+	else if (!strcmp(verb, "stop")) {
+		ret = df_mpu6050_wrapper::stop();
+	}
+
+	else if (!strcmp(verb, "info")) {
+		ret = df_mpu6050_wrapper::info();
+	}
+
+	else {
+		df_mpu6050_wrapper::usage();
+		return 1;
+	}
+
+	return ret;
+}

From d94bdb08293d4a6ec1799e2376c3baa8eded7392 Mon Sep 17 00:00:00 2001
From: Michael Schaeuble <schaeuble.michael@gmail.com>
Date: Wed, 29 Jun 2016 16:56:22 +0200
Subject: [PATCH 1048/1230] Add missing defines for Bebop sensor drivers

---
 cmake/configs/posix_bebop_default.cmake |  3 +++
 src/modules/sensors/sensors.cpp         | 10 +++++-----
 src/modules/sensors/sensors_init.cpp    |  2 +-
 3 files changed, 9 insertions(+), 6 deletions(-)

diff --git a/cmake/configs/posix_bebop_default.cmake b/cmake/configs/posix_bebop_default.cmake
index a2296222ad..77175863c0 100644
--- a/cmake/configs/posix_bebop_default.cmake
+++ b/cmake/configs/posix_bebop_default.cmake
@@ -5,6 +5,7 @@ set(CMAKE_TOOLCHAIN_FILE ${CMAKE_SOURCE_DIR}/cmake/toolchains/Toolchain-arm-linu
 add_definitions(
   -D__PX4_POSIX_BEBOP
   -D__LINUX
+  -D__BEBOP
 	)
 
 set(CMAKE_PROGRAM_PATH
@@ -22,6 +23,7 @@ set(config_module_list
 	drivers/device
 	modules/sensors
 	platforms/posix/drivers/df_ms5607_wrapper
+	platforms/posix/drivers/df_mpu6050_wrapper
 
 	#
 	# System commands
@@ -99,4 +101,5 @@ set(config_module_list
 
 set(config_df_driver_list
   ms5607
+  mpu6050
 )
diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp
index f9f937a7ff..52f250396f 100644
--- a/src/modules/sensors/sensors.cpp
+++ b/src/modules/sensors/sensors.cpp
@@ -1017,7 +1017,7 @@ Sensors::parameters_update()
 	DevHandle h_baro;
 	DevMgr::getHandle(BARO0_DEVICE_PATH, h_baro);
 
-#if !defined(__PX4_QURT) && !defined(__PX4_POSIX_RPI)
+#if !defined(__PX4_QURT) && !defined(__PX4_POSIX_RPI) && !defined(__PX4_POSIX_BEBOP)
 
 	// TODO: this needs fixing for QURT and Raspberry Pi
 	if (!h_baro.isValid()) {
@@ -1671,7 +1671,7 @@ Sensors::parameter_update_poll(bool forced)
 bool
 Sensors::apply_gyro_calibration(DevHandle &h, const struct gyro_calibration_s *gcal, const int device_id)
 {
-#if !defined(__PX4_QURT) && !defined(__PX4_POSIX_RPI)
+#if !defined(__PX4_QURT) && !defined(__PX4_POSIX_RPI) && !defined(__PX4_POSIX_BEBOP)
 
 	/* On most systems, we can just use the IOCTL call to set the calibration params. */
 	return !h.ioctl(GYROIOCSSCALE, (long unsigned int)gcal);
@@ -1685,7 +1685,7 @@ Sensors::apply_gyro_calibration(DevHandle &h, const struct gyro_calibration_s *g
 bool
 Sensors::apply_accel_calibration(DevHandle &h, const struct accel_calibration_s *acal, const int device_id)
 {
-#if !defined(__PX4_QURT) && !defined(__PX4_POSIX_RPI)
+#if !defined(__PX4_QURT) && !defined(__PX4_POSIX_RPI) && !defined(__PX4_POSIX_BEBOP)
 
 	/* On most systems, we can just use the IOCTL call to set the calibration params. */
 	return !h.ioctl(ACCELIOCSSCALE, (long unsigned int)acal);
@@ -1699,7 +1699,7 @@ Sensors::apply_accel_calibration(DevHandle &h, const struct accel_calibration_s
 bool
 Sensors::apply_mag_calibration(DevHandle &h, const struct mag_calibration_s *mcal, const int device_id)
 {
-#if !defined(__PX4_QURT) && !defined(__PX4_POSIX_RPI)
+#if !defined(__PX4_QURT) && !defined(__PX4_POSIX_RPI) && !defined(__PX4_POSIX_BEBOP)
 
 	/* On most systems, we can just use the IOCTL call to set the calibration params. */
 	return !h.ioctl(MAGIOCSSCALE, (long unsigned int)mcal);
@@ -2250,7 +2250,7 @@ Sensors::task_main()
 	/* This calls a sensors_init which can have different implementations on NuttX, POSIX, QURT. */
 	ret = sensors_init();
 
-#if !defined(__PX4_QURT) && !defined(__PX4_POSIX_RPI)
+#if !defined(__PX4_QURT) && !defined(__PX4_POSIX_RPI) && !defined(__PX4_POSIX_BEBOP)
 	// TODO: move adc_init into the sensors_init call.
 	ret = ret || adc_init();
 #endif
diff --git a/src/modules/sensors/sensors_init.cpp b/src/modules/sensors/sensors_init.cpp
index 561ee06e27..ae9cf4db86 100644
--- a/src/modules/sensors/sensors_init.cpp
+++ b/src/modules/sensors/sensors_init.cpp
@@ -52,7 +52,7 @@
 
 using namespace DriverFramework;
 
-#if defined(__PX4_QURT) || defined(__PX4_POSIX_RPI)
+#if defined(__PX4_QURT) || defined(__PX4_POSIX_RPI) || defined(__PX4_POSIX_BEBOP)
 
 // Sensor initialization is performed automatically when the QuRT sensor drivers
 // are loaded.

From 28de6d2cbda92661ce90e13df7bf1a05294693f6 Mon Sep 17 00:00:00 2001
From: Michael Schaeuble <schaeuble.michael@gmail.com>
Date: Wed, 29 Jun 2016 16:57:05 +0200
Subject: [PATCH 1049/1230] Update mainapp.config and add MPU6050

---
 posix-configs/bebop/mainapp.config | 5 +++--
 1 file changed, 3 insertions(+), 2 deletions(-)

diff --git a/posix-configs/bebop/mainapp.config b/posix-configs/bebop/mainapp.config
index 3c6bffdc80..caee245e63 100644
--- a/posix-configs/bebop/mainapp.config
+++ b/posix-configs/bebop/mainapp.config
@@ -4,9 +4,10 @@ param load
 param set SYS_AUTOSTART 4001
 param set MAV_BROADCAST 1
 df_ms5607_wrapper start
+df_mpu6050_wrapper start
+sensors start
 sleep 1
 mavlink start -u 14556 -r 1000000
 sleep 1
-mavlink stream -u 14556 -s ATTITUDE -r 50
-mavlink stream -u 14556 -s ALTITUDE -r 1
+mavlink stream -u 14556 -s HIGHRES_IMU -r 50
 mavlink boot_complete

From 2197bf518d6801661b8e526f27eb40c3b5e3377c Mon Sep 17 00:00:00 2001
From: Michael Schaeuble <schaeuble.michael@gmail.com>
Date: Sun, 10 Jul 2016 23:14:32 +0200
Subject: [PATCH 1050/1230] Fix code style

---
 .../df_mpu6050_wrapper/df_mpu6050_wrapper.cpp    | 16 ++++++++--------
 1 file changed, 8 insertions(+), 8 deletions(-)

diff --git a/src/platforms/posix/drivers/df_mpu6050_wrapper/df_mpu6050_wrapper.cpp b/src/platforms/posix/drivers/df_mpu6050_wrapper/df_mpu6050_wrapper.cpp
index 00ffc37b30..4f9f3d4780 100644
--- a/src/platforms/posix/drivers/df_mpu6050_wrapper/df_mpu6050_wrapper.cpp
+++ b/src/platforms/posix/drivers/df_mpu6050_wrapper/df_mpu6050_wrapper.cpp
@@ -533,18 +533,18 @@ int DfMPU6050Wrapper::_publish(struct imu_sensor_data &data)
 	if (!(m_pub_blocked)) {
 
 		if (_gyro_topic == nullptr) {
-      _gyro_topic = orb_advertise_multi(ORB_ID(sensor_gyro), &gyro_report,
-                &_gyro_orb_class_instance, ORB_PRIO_DEFAULT);
-    }
-    else {
+			_gyro_topic = orb_advertise_multi(ORB_ID(sensor_gyro), &gyro_report,
+							  &_gyro_orb_class_instance, ORB_PRIO_DEFAULT);
+
+		} else {
 			orb_publish(ORB_ID(sensor_gyro), _gyro_topic, &gyro_report);
 		}
 
 		if (_accel_topic == nullptr) {
-      _accel_topic = orb_advertise_multi(ORB_ID(sensor_accel), &accel_report,
-                &_accel_orb_class_instance, ORB_PRIO_DEFAULT);
-    }
-    else {
+			_accel_topic = orb_advertise_multi(ORB_ID(sensor_accel), &accel_report,
+							   &_accel_orb_class_instance, ORB_PRIO_DEFAULT);
+
+		} else {
 			orb_publish(ORB_ID(sensor_accel), _accel_topic, &accel_report);
 		}
 

From 7d11b3298157177d29b5de042483240451a60c21 Mon Sep 17 00:00:00 2001
From: Michael Schaeuble <schaeuble.michael@gmail.com>
Date: Wed, 20 Jul 2016 12:15:58 +0200
Subject: [PATCH 1051/1230] Add correct rotation for MPU6050 on Bebop

---
 posix-configs/bebop/mainapp.config | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/posix-configs/bebop/mainapp.config b/posix-configs/bebop/mainapp.config
index caee245e63..25370f234c 100644
--- a/posix-configs/bebop/mainapp.config
+++ b/posix-configs/bebop/mainapp.config
@@ -4,7 +4,7 @@ param load
 param set SYS_AUTOSTART 4001
 param set MAV_BROADCAST 1
 df_ms5607_wrapper start
-df_mpu6050_wrapper start
+df_mpu6050_wrapper start -R 8
 sensors start
 sleep 1
 mavlink start -u 14556 -r 1000000

From 1fc8e3815701835e2294166af796b524938919d2 Mon Sep 17 00:00:00 2001
From: Michael Schaeuble <schaeuble.michael@gmail.com>
Date: Wed, 20 Jul 2016 12:19:00 +0200
Subject: [PATCH 1052/1230] Fix incorrect indentation

---
 cmake/configs/posix_bebop_default.cmake | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/cmake/configs/posix_bebop_default.cmake b/cmake/configs/posix_bebop_default.cmake
index 77175863c0..25a29d4df3 100644
--- a/cmake/configs/posix_bebop_default.cmake
+++ b/cmake/configs/posix_bebop_default.cmake
@@ -100,6 +100,6 @@ set(config_module_list
 )
 
 set(config_df_driver_list
-  ms5607
-  mpu6050
+	ms5607
+	mpu6050
 )

From ae66085f89ea6197b2c469c63f9c6d5d70c62f3c Mon Sep 17 00:00:00 2001
From: Michael Schaeuble <schaeuble.michael@gmail.com>
Date: Thu, 21 Jul 2016 18:48:28 +0200
Subject: [PATCH 1053/1230] MPU6050: Apply calibration after coordinate frame
 rotation

---
 .../df_mpu6050_wrapper/df_mpu6050_wrapper.cpp    | 16 ++++++++++------
 1 file changed, 10 insertions(+), 6 deletions(-)

diff --git a/src/platforms/posix/drivers/df_mpu6050_wrapper/df_mpu6050_wrapper.cpp b/src/platforms/posix/drivers/df_mpu6050_wrapper/df_mpu6050_wrapper.cpp
index 4f9f3d4780..08fcb50217 100644
--- a/src/platforms/posix/drivers/df_mpu6050_wrapper/df_mpu6050_wrapper.cpp
+++ b/src/platforms/posix/drivers/df_mpu6050_wrapper/df_mpu6050_wrapper.cpp
@@ -432,25 +432,29 @@ int DfMPU6050Wrapper::_publish(struct imu_sensor_data &data)
 	math::Vector<3> vec_integrated_unused;
 	uint64_t integral_dt_unused;
 
-	math::Vector<3> accel_val((data.accel_m_s2_x - _accel_calibration.x_offset) * _accel_calibration.x_scale,
-				  (data.accel_m_s2_y - _accel_calibration.y_offset) * _accel_calibration.y_scale,
-				  (data.accel_m_s2_z - _accel_calibration.z_offset) * _accel_calibration.z_scale);
+	math::Vector<3> accel_val(data.accel_m_s2_x, data.accel_m_s2_y, data.accel_m_s2_z);
 
 	// apply sensor rotation on the accel measurement
 	accel_val = _rotation_matrix * accel_val;
 
+	accel_val(0) = (accel_val(0) - _accel_calibration.x_offset) * _accel_calibration.x_scale;
+	accel_val(1) = (accel_val(1) - _accel_calibration.y_offset) * _accel_calibration.y_scale;
+	accel_val(2) = (accel_val(2) - _accel_calibration.z_offset) * _accel_calibration.z_scale;
+
 	_accel_int.put_with_interval(data.fifo_sample_interval_us,
 				     accel_val,
 				     vec_integrated_unused,
 				     integral_dt_unused);
 
-	math::Vector<3> gyro_val((data.gyro_rad_s_x - _gyro_calibration.x_offset) * _gyro_calibration.x_scale,
-				 (data.gyro_rad_s_y - _gyro_calibration.y_offset) * _gyro_calibration.y_scale,
-				 (data.gyro_rad_s_z - _gyro_calibration.z_offset) * _gyro_calibration.z_scale);
+	math::Vector<3> gyro_val(data.gyro_rad_s_x, data.gyro_rad_s_y, data.gyro_rad_s_z);
 
 	// apply sensor rotation on the gyro measurement
 	gyro_val = _rotation_matrix * gyro_val;
 
+	gyro_val(0) = (gyro_val(0) - _gyro_calibration.x_offset) * _gyro_calibration.x_scale;
+	gyro_val(1) = (gyro_val(1) - _gyro_calibration.y_offset) * _gyro_calibration.y_scale;
+	gyro_val(2) = (gyro_val(2) - _gyro_calibration.z_offset) * _gyro_calibration.z_scale;
+
 	_gyro_int.put_with_interval(data.fifo_sample_interval_us,
 				    gyro_val,
 				    vec_integrated_unused,

From b99f51a03536f966a917eab123fcdb046fdcba42 Mon Sep 17 00:00:00 2001
From: Julian Oes <julian@oes.ch>
Date: Mon, 18 Jul 2016 16:01:20 +0100
Subject: [PATCH 1054/1230] gitignore: don't ignore all .config files

---
 .gitignore | 1 -
 1 file changed, 1 deletion(-)

diff --git a/.gitignore b/.gitignore
index 4a12b5f3bc..8b1b934563 100644
--- a/.gitignore
+++ b/.gitignore
@@ -64,7 +64,6 @@ CMakeLists.txt.user
 GPATH
 GRTAGS
 GTAGS
-*.config
 *.creator
 *.creator.user
 *.files

From 317fd270d19fe569b91acb5a8d732b13118a27cd Mon Sep 17 00:00:00 2001
From: Julian Oes <julian@oes.ch>
Date: Mon, 18 Jul 2016 16:01:39 +0100
Subject: [PATCH 1055/1230] RPi: add mainapp.config again

This file got deleted accidentally because all .config files were in
gitignore.
---
 posix-configs/rpi/mainapp.config | 27 +++++++++++++++++++++++++++
 1 file changed, 27 insertions(+)
 create mode 100644 posix-configs/rpi/mainapp.config

diff --git a/posix-configs/rpi/mainapp.config b/posix-configs/rpi/mainapp.config
new file mode 100644
index 0000000000..003fb52c44
--- /dev/null
+++ b/posix-configs/rpi/mainapp.config
@@ -0,0 +1,27 @@
+uorb start
+param set SYS_AUTOSTART 4001
+param set MAV_BROADCAST 1
+sleep 1
+param set MAV_TYPE 2
+df_lsm9ds1_wrapper start -R 4
+#df_mpu9250_wrapper start -R 10
+#df_hmc5883_wrapper start
+df_ms5611_wrapper start
+gps start -d /dev/pts/0
+sensors start
+commander start
+attitude_estimator_q start
+position_estimator_inav start
+land_detector start multicopter
+mc_pos_control start
+mc_att_control start
+mavlink start -u 14556 -r 1000000
+sleep 1
+mavlink stream -u 14556 -s HIGHRES_IMU -r 50
+mavlink stream -u 14556 -s ATTITUDE -r 50
+mavlink start -d /dev/ttyUSB0
+mavlink stream -d /dev/ttyUSB0 -s HIGHRES_IMU -r 50
+mavlink stream -d /dev/ttyUSB0 -s ATTITUDE -r 50
+navio_sysfs_rc_in start
+navio_sysfs_pwm_out start
+mavlink boot_complete
\ No newline at end of file

From 2b93f16118605b48c1c521ad52524309b0792a53 Mon Sep 17 00:00:00 2001
From: Henry Zhang <zhanghui629@gmail.com>
Date: Fri, 22 Jul 2016 14:55:13 +0800
Subject: [PATCH 1056/1230] mindpx-v2: build topic_listener, sd_bench, tests,
 motor_ramp.

---
 cmake/configs/nuttx_mindpx-v2_default.cmake | 6 ++++--
 1 file changed, 4 insertions(+), 2 deletions(-)

diff --git a/cmake/configs/nuttx_mindpx-v2_default.cmake b/cmake/configs/nuttx_mindpx-v2_default.cmake
index 18b496fa02..f24a614b92 100644
--- a/cmake/configs/nuttx_mindpx-v2_default.cmake
+++ b/cmake/configs/nuttx_mindpx-v2_default.cmake
@@ -64,13 +64,16 @@ set(config_module_list
 	systemcmds/pwm
 	systemcmds/esc_calib
 	systemcmds/reboot
-	#systemcmds/topic_listener
+	systemcmds/topic_listener
 	systemcmds/top
 	systemcmds/config
 	systemcmds/nshterm
 	systemcmds/mtd
 	systemcmds/dumpfile
 	systemcmds/ver
+	systemcmds/sd_bench
+	systemcmds/tests
+	systemcmds/motor_ramp
 
 	#
 	# General system control
@@ -95,7 +98,6 @@ set(config_module_list
 	#
 	# Vehicle Control
 	#
-	# modules/segway # XXX Needs GCC 4.7 fix
 	modules/fw_pos_control_l1
 	modules/fw_att_control
 	modules/mc_att_control

From 0730e681bce1af46101e8896e5320228275f8cb0 Mon Sep 17 00:00:00 2001
From: Henry Zhang <zhanghui629@gmail.com>
Date: Fri, 22 Jul 2016 14:57:05 +0800
Subject: [PATCH 1057/1230] Nuttx config: MAVLink app needs more FDs.

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

diff --git a/nuttx-configs/mindpx-v2/nsh/defconfig b/nuttx-configs/mindpx-v2/nsh/defconfig
index 54862aa09d..ae23ec7bde 100644
--- a/nuttx-configs/mindpx-v2/nsh/defconfig
+++ b/nuttx-configs/mindpx-v2/nsh/defconfig
@@ -440,7 +440,7 @@ CONFIG_SIG_SIGWORK=4
 CONFIG_MAX_TASKS=32
 CONFIG_MAX_TASK_ARGS=10
 CONFIG_NPTHREAD_KEYS=4
-CONFIG_NFILE_DESCRIPTORS=42
+CONFIG_NFILE_DESCRIPTORS=47
 CONFIG_NFILE_STREAMS=8
 CONFIG_NAME_MAX=32
 CONFIG_PREALLOC_MQ_MSGS=4

From a4ef364f800f917a15cfe8120614c211497e30e4 Mon Sep 17 00:00:00 2001
From: James Goppert <jgoppert@users.noreply.github.com>
Date: Fri, 22 Jul 2016 19:19:30 -0400
Subject: [PATCH 1058/1230] Added innovation logging to LPE. (#5124)

---
 .../BlockLocalPositionEstimator.cpp              |  2 ++
 .../BlockLocalPositionEstimator.hpp              |  2 ++
 .../local_position_estimator/sensors/flow.cpp    |  6 +++++-
 .../local_position_estimator/sensors/gps.cpp     | 16 +++++++++++-----
 .../local_position_estimator/sensors/lidar.cpp   |  4 +++-
 .../local_position_estimator/sensors/sonar.cpp   |  4 +++-
 src/modules/uORB/Publication.cpp                 |  2 ++
 7 files changed, 28 insertions(+), 8 deletions(-)

diff --git a/src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp b/src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp
index 566f0cfbde..584efc77e1 100644
--- a/src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp
+++ b/src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp
@@ -50,6 +50,7 @@ BlockLocalPositionEstimator::BlockLocalPositionEstimator() :
 	_pub_lpos(ORB_ID(vehicle_local_position), -1, &getPublications()),
 	_pub_gpos(ORB_ID(vehicle_global_position), -1, &getPublications()),
 	_pub_est_status(ORB_ID(estimator_status), -1, &getPublications()),
+	_pub_innov(ORB_ID(ekf2_innovations), -1, &getPublications()),
 
 	// map projection
 	_map_ref(),
@@ -479,6 +480,7 @@ void BlockLocalPositionEstimator::update()
 		// update all publications if possible
 		publishLocalPos();
 		publishEstimatorStatus();
+		_pub_innov.update();
 
 		if (_validXY) {
 			publishGlobalPos();
diff --git a/src/modules/local_position_estimator/BlockLocalPositionEstimator.hpp b/src/modules/local_position_estimator/BlockLocalPositionEstimator.hpp
index 4a612182b1..761251b0de 100644
--- a/src/modules/local_position_estimator/BlockLocalPositionEstimator.hpp
+++ b/src/modules/local_position_estimator/BlockLocalPositionEstimator.hpp
@@ -28,6 +28,7 @@
 #include <uORB/topics/vehicle_local_position.h>
 #include <uORB/topics/vehicle_global_position.h>
 #include <uORB/topics/estimator_status.h>
+#include <uORB/topics/ekf2_innovations.h>
 
 using namespace matrix;
 using namespace control;
@@ -235,6 +236,7 @@ private:
 	uORB::Publication<vehicle_local_position_s> _pub_lpos;
 	uORB::Publication<vehicle_global_position_s> _pub_gpos;
 	uORB::Publication<estimator_status_s> _pub_est_status;
+	uORB::Publication<ekf2_innovations_s> _pub_innov;
 
 	// map projection
 	struct map_projection_reference_s _map_ref;
diff --git a/src/modules/local_position_estimator/sensors/flow.cpp b/src/modules/local_position_estimator/sensors/flow.cpp
index 879da54b0a..2e45333f21 100644
--- a/src/modules/local_position_estimator/sensors/flow.cpp
+++ b/src/modules/local_position_estimator/sensors/flow.cpp
@@ -122,7 +122,7 @@ void BlockLocalPositionEstimator::flowCorrect()
 	C(Y_flow_x, X_x) = 1;
 	C(Y_flow_y, X_y) = 1;
 
-	Matrix<float, n_y_flow, n_y_flow> R;
+	SquareMatrix<float, n_y_flow> R;
 	R.setZero();
 	R(Y_flow_x, Y_flow_x) =
 		_flow_xy_stddev.get() * _flow_xy_stddev.get();
@@ -131,6 +131,10 @@ void BlockLocalPositionEstimator::flowCorrect()
 
 	// residual
 	Vector<float, 2> r = y - C * _x;
+	_pub_innov.get().flow_innov[0] = r(0);
+	_pub_innov.get().flow_innov[1] = r(1);
+	_pub_innov.get().flow_innov_var[0] = R(0, 0);
+	_pub_innov.get().flow_innov_var[1] = R(1, 1);
 
 	// residual covariance, (inverse)
 	Matrix<float, n_y_flow, n_y_flow> S_I =
diff --git a/src/modules/local_position_estimator/sensors/gps.cpp b/src/modules/local_position_estimator/sensors/gps.cpp
index c003faf7ca..ffe3db2aab 100644
--- a/src/modules/local_position_estimator/sensors/gps.cpp
+++ b/src/modules/local_position_estimator/sensors/gps.cpp
@@ -114,7 +114,7 @@ void BlockLocalPositionEstimator::gpsCorrect()
 	C(Y_gps_vz, X_vz) = 1;
 
 	// gps covariance matrix
-	Matrix<float, n_y_gps, n_y_gps> R;
+	SquareMatrix<float, n_y_gps> R;
 	R.setZero();
 
 	// default to parameter, use gps cov if provided
@@ -141,10 +141,10 @@ void BlockLocalPositionEstimator::gpsCorrect()
 
 	// get delayed x and P
 	float t_delay = 0;
-	int i = 0;
+	int i_hist = 0;
 
-	for (i = 1; i < HIST_LEN; i++) {
-		t_delay = 1.0e-6f * (_timeStamp - _tDelay.get(i)(0, 0));
+	for (i_hist = 1; i_hist < HIST_LEN; i_hist++) {
+		t_delay = 1.0e-6f * (_timeStamp - _tDelay.get(i_hist)(0, 0));
 
 		if (t_delay > _gps_delay.get()) {
 			break;
@@ -158,10 +158,16 @@ void BlockLocalPositionEstimator::gpsCorrect()
 		return;
 	}
 
-	Vector<float, n_x> x0 = _xDelay.get(i);
+	Vector<float, n_x> x0 = _xDelay.get(i_hist);
 
 	// residual
 	Vector<float, n_y_gps> r = y - C * x0;
+
+	for (int i = 0; i < 6; i ++) {
+		_pub_innov.get().vel_pos_innov[i] = r(i);
+		_pub_innov.get().vel_pos_innov_var[i] = R(i, i);
+	}
+
 	Matrix<float, n_y_gps, n_y_gps> S_I = inv<float, 6>(C * _P * C.transpose() + R);
 
 	// fault detection
diff --git a/src/modules/local_position_estimator/sensors/lidar.cpp b/src/modules/local_position_estimator/sensors/lidar.cpp
index 1efb0a234e..407a49ace6 100644
--- a/src/modules/local_position_estimator/sensors/lidar.cpp
+++ b/src/modules/local_position_estimator/sensors/lidar.cpp
@@ -77,7 +77,7 @@ void BlockLocalPositionEstimator::lidarCorrect()
 	C(Y_lidar_z, X_tz) = 1; // measured altitude, negative down dir.
 
 	// use parameter covariance unless sensor provides reasonable value
-	Matrix<float, n_y_lidar, n_y_lidar> R;
+	SquareMatrix<float, n_y_lidar> R;
 	R.setZero();
 	float cov = _sub_lidar->get().covariance;
 
@@ -91,6 +91,8 @@ void BlockLocalPositionEstimator::lidarCorrect()
 	// residual
 	Matrix<float, n_y_lidar, n_y_lidar> S_I = inv<float, n_y_lidar>((C * _P * C.transpose()) + R);
 	Vector<float, n_y_lidar> r = y - C * _x;
+	_pub_innov.get().hagl_innov = r(0);
+	_pub_innov.get().hagl_innov_var = R(0, 0);
 
 	// fault detection
 	float beta = (r.transpose() * (S_I * r))(0, 0);
diff --git a/src/modules/local_position_estimator/sensors/sonar.cpp b/src/modules/local_position_estimator/sensors/sonar.cpp
index c263cb0791..d2ea5be9f5 100644
--- a/src/modules/local_position_estimator/sensors/sonar.cpp
+++ b/src/modules/local_position_estimator/sensors/sonar.cpp
@@ -99,12 +99,14 @@ void BlockLocalPositionEstimator::sonarCorrect()
 	C(Y_sonar_z, X_tz) = 1; // measured altitude, negative down dir.
 
 	// covariance matrix
-	Matrix<float, n_y_sonar, n_y_sonar> R;
+	SquareMatrix<float, n_y_sonar> R;
 	R.setZero();
 	R(0, 0) = cov;
 
 	// residual
 	Vector<float, n_y_sonar> r = y - C * _x;
+	_pub_innov.get().hagl_innov = r(0);
+	_pub_innov.get().hagl_innov_var = R(0, 0);
 
 	// residual covariance, (inverse)
 	Matrix<float, n_y_sonar, n_y_sonar> S_I =
diff --git a/src/modules/uORB/Publication.cpp b/src/modules/uORB/Publication.cpp
index 3282f5c749..fdfb22300c 100644
--- a/src/modules/uORB/Publication.cpp
+++ b/src/modules/uORB/Publication.cpp
@@ -50,6 +50,7 @@
 #include "topics/tecs_status.h"
 #include "topics/rc_channels.h"
 #include "topics/filtered_bottom_flow.h"
+#include "topics/ekf2_innovations.h"
 
 #include <px4_defines.h>
 
@@ -120,5 +121,6 @@ template class __EXPORT Publication<actuator_direct_s>;
 template class __EXPORT Publication<tecs_status_s>;
 template class __EXPORT Publication<rc_channels_s>;
 template class __EXPORT Publication<filtered_bottom_flow_s>;
+template class __EXPORT Publication<ekf2_innovations_s>;
 
 }

From 7399e60e96c835ff26e62c1e176d0839e33191ec Mon Sep 17 00:00:00 2001
From: Julian Oes <julian@oes.ch>
Date: Fri, 22 Jul 2016 21:44:05 +0200
Subject: [PATCH 1059/1230] ecl: update submodule

This includes just some minor fixes.
---
 src/lib/ecl | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/src/lib/ecl b/src/lib/ecl
index 2240eb6b4f..168d9add76 160000
--- a/src/lib/ecl
+++ b/src/lib/ecl
@@ -1 +1 @@
-Subproject commit 2240eb6b4fd0b1603b9d3275ac29a6b3ab3f66e0
+Subproject commit 168d9add76f94c910d1930b58abb455bf2739c8a

From 76d74640ae402c1078d039671275654f46fd1bb0 Mon Sep 17 00:00:00 2001
From: Eric Ye <eye@intrinsyc.com>
Date: Wed, 20 Jul 2016 16:07:04 -0700
Subject: [PATCH 1060/1230] Add "help" to qshell for qurt

Fixes issue #5111
---
 src/drivers/qshell/qurt/qshell.cpp | 5 +++++
 1 file changed, 5 insertions(+)

diff --git a/src/drivers/qshell/qurt/qshell.cpp b/src/drivers/qshell/qurt/qshell.cpp
index 5be2495450..22247145db 100644
--- a/src/drivers/qshell/qurt/qshell.cpp
+++ b/src/drivers/qshell/qurt/qshell.cpp
@@ -62,6 +62,7 @@
 #include "DriverFramework.hpp"
 
 extern void init_app_map(std::map<std::string, px4_main_t> &apps);
+extern void list_builtins(std::map<std::string, px4_main_t> &apps);
 
 using std::map;
 using std::string;
@@ -146,6 +147,10 @@ int QShell::run_cmd(const std::vector<std::string> &appargs)
 	// command is appargs[0]
 	std::string command = appargs[0];
 
+	if (command.compare("help") == 0) {
+		list_builtins(apps);
+	}
+
 	//replaces app.find with iterator code to avoid null pointer exception
 	for (map<string, px4_main_t>::iterator it = apps.begin(); it != apps.end(); ++it) {
 		if (it->first == command) {

From 5cee3fa0e183ee0b63baa905b5b1ba5715088c77 Mon Sep 17 00:00:00 2001
From: Eric Ye <eye@intrinsyc.com>
Date: Wed, 20 Jul 2016 16:22:43 -0700
Subject: [PATCH 1061/1230] Proper return value on qshell help

---
 src/drivers/qshell/qurt/qshell.cpp | 1 +
 1 file changed, 1 insertion(+)

diff --git a/src/drivers/qshell/qurt/qshell.cpp b/src/drivers/qshell/qurt/qshell.cpp
index 22247145db..56ace44e03 100644
--- a/src/drivers/qshell/qurt/qshell.cpp
+++ b/src/drivers/qshell/qurt/qshell.cpp
@@ -149,6 +149,7 @@ int QShell::run_cmd(const std::vector<std::string> &appargs)
 
 	if (command.compare("help") == 0) {
 		list_builtins(apps);
+		return 0;
 	}
 
 	//replaces app.find with iterator code to avoid null pointer exception

From 05315abc89d735ef0d81fffe9d4e75f0fd00de88 Mon Sep 17 00:00:00 2001
From: James Goppert <jgoppert@users.noreply.github.com>
Date: Sun, 24 Jul 2016 13:07:14 -0400
Subject: [PATCH 1062/1230] Optical flow simulator and install cleanup. (#5132)

sitl CI is having some issue cloning, but I have verified it locally for various configs
---
 CMakeLists.txt                                | 34 +++++++++-
 Makefile                                      | 16 ++---
 Tools/sitl_gazebo                             |  2 +-
 posix-configs/SITL/init/rcS_lpe_gazebo_iris   | 64 ++++++++++---------
 .../SITL/init/rcS_lpe_gazebo_iris_opt_flow    | 48 ++++++++------
 posix-configs/SITL/init/rcS_lpe_jmavsim_iris  | 34 +++++++---
 src/firmware/posix/CMakeLists.txt             |  8 +++
 .../BlockLocalPositionEstimator.cpp           |  4 +-
 .../local_position_estimator/sensors/flow.cpp |  3 +
 src/modules/simulator/simulator.h             |  4 ++
 src/modules/simulator/simulator_mavlink.cpp   | 30 ++++++++-
 11 files changed, 172 insertions(+), 75 deletions(-)

diff --git a/CMakeLists.txt b/CMakeLists.txt
index 9bc92c91d1..b3b7efe008 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -152,6 +152,17 @@ set(target_name "${OS}-${BOARD}-${LABEL}")
 
 message(STATUS "${target_name}")
 
+# Define GNU standard installation directories
+include(GNUInstallDirs)
+
+# Setup install paths
+if(NOT CMAKE_INSTALL_PREFIX)
+	if (${OS} STREQUAL "posix")
+		set(CMAKE_INSTALL_PREFIX "/usr" CACHE PATH "Install path prefix" FORCE)
+	endif()
+endif()
+message(STATUS "CMAKE_INSTALL_PREFIX: ${CMAKE_INSTALL_PREFIX}")
+
 # switch to ros CMake file if building ros
 if (${OS} STREQUAL "ros")
 	include("cmake/ros-CMakeLists.txt")
@@ -202,7 +213,7 @@ if (NOT ${CMAKE_VERSION} VERSION_LESS 3.1.0)
 endif()
 
 set(version_major 1)
-set(version_minor 0)
+set(version_minor 4)
 set(version_patch 1)
 set(version "${version_major}.${version_minor}.${version_patch}")
 set(package-contact "px4users@googlegroups.com")
@@ -371,10 +382,27 @@ px4_create_git_hash_header(HEADER ${CMAKE_BINARY_DIR}/build_git_version.h)
 #
 # Important to having packaging at end of cmake file.
 #
+set(CPACK_PACKAGE_NAME ${PROJECT_NAME}-${CONFIG})
 set(CPACK_PACKAGE_VERSION ${version})
-set(CPACK_PACKAGE_CONTACT ${package_contact})
+set(CPACK_PACKAGE_CONTACT ${package-contact})
+set(CPACK_DEBIAN_PACKAGE_SHLIBDEPS ON)
+set(CPACK_DEBIAN_PACKAGE_SECTION "devel")
+set(CPACK_DEBIAN_PACKAGE_PRIORITY "optional")
+set(short-description "The px4 autopilot.")
+set(CPACK_DEBIAN_PACKAGE_DESCRIPTION ${short-description})
 set(CPACK_GENERATOR "ZIP")
-set(CPACK_SOURCE_GENERATOR "ZIP")
+set(CPACK_PACKAGE_FILE_NAME "${PROJECT_NAME}-${CONFIG}-${version}")
+set(CPACK_SOURCE_PACKAGE_FILE_NAME "${PROJECT_NAME}-${version}")
+set(CPACK_SOURCE_GENERATOR "ZIP;TBZ2")
+if ("${CMAKE_SYSTEM}" MATCHES "Linux")
+  find_program(DPKG_PROGRAM dpkg)
+  if (EXISTS ${DPKG_PROGRAM})
+    list (APPEND CPACK_GENERATOR "DEB")
+  endif()
+endif()
+if (${OS} STREQUAL "posix")
+	set(CPACK_SET_DESTDIR "ON")
+endif()
 include(CPack)
 
 endif() # ros alternative endif
diff --git a/Makefile b/Makefile
index 5435ef7c70..f65ac29c1a 100644
--- a/Makefile
+++ b/Makefile
@@ -115,16 +115,16 @@ endif
 # --------------------------------------------------------------------
 # describe how to build a cmake config
 define cmake-build
-+@if [ $(PX4_CMAKE_GENERATOR) = "Ninja" ] && [ -e $(PWD)/build_$@$(BUILD_DIR_SUFFIX)/Makefile ]; then rm -rf $(PWD)/build_$@$(BUILD_DIR_SUFFIX); fi
-+@if [ ! -e $(PWD)/build_$@$(BUILD_DIR_SUFFIX)/CMakeCache.txt ]; then Tools/check_submodules.sh && mkdir -p $(PWD)/build_$@$(BUILD_DIR_SUFFIX) && cd $(PWD)/build_$@$(BUILD_DIR_SUFFIX) && cmake .. -G$(PX4_CMAKE_GENERATOR) -DCONFIG=$(1) || (cd .. && rm -rf $(PWD)/build_$@$(BUILD_DIR_SUFFIX)); fi
++@if [ $(PX4_CMAKE_GENERATOR) = "Ninja" ] && [ -e ./build_$@$(BUILD_DIR_SUFFIX)/Makefile ]; then rm -rf ./build_$@$(BUILD_DIR_SUFFIX); fi
++@if [ ! -e ./build_$@$(BUILD_DIR_SUFFIX)/CMakeCache.txt ]; then Tools/check_submodules.sh && mkdir -p ./build_$@$(BUILD_DIR_SUFFIX) && cd ./build_$@$(BUILD_DIR_SUFFIX) && cmake .. -G$(PX4_CMAKE_GENERATOR) -DCONFIG=$(1) || (cd .. && rm -rf ./build_$@$(BUILD_DIR_SUFFIX)); fi
 +@Tools/check_submodules.sh
-+@(echo "PX4 CONFIG: $@$(BUILD_DIR_SUFFIX)" && cd $(PWD)/build_$@$(BUILD_DIR_SUFFIX) && $(PX4_MAKE) $(PX4_MAKE_ARGS) $(ARGS))
++@(echo "PX4 CONFIG: $@$(BUILD_DIR_SUFFIX)" && cd ./build_$@$(BUILD_DIR_SUFFIX) && $(PX4_MAKE) $(PX4_MAKE_ARGS) $(ARGS))
 endef
 
 define cmake-build-other
-+@if [ $(PX4_CMAKE_GENERATOR) = "Ninja" ] && [ -e $(PWD)/build_$@/Makefile ]; then rm -rf $(PWD)/build_$@; fi
-+@if [ ! -e $(PWD)/build_$@/CMakeCache.txt ]; then Tools/check_submodules.sh && mkdir -p $(PWD)/build_$@ && cd $(PWD)/build_$@ && cmake $(2) -G$(PX4_CMAKE_GENERATOR) || (cd .. && rm -rf $(PWD)/build_$@); fi
-+@(cd $(PWD)/build_$@ && $(PX4_MAKE) $(PX4_MAKE_ARGS) $(ARGS))
++@if [ $(PX4_CMAKE_GENERATOR) = "Ninja" ] && [ -e ./build_$@/Makefile ]; then rm -rf ./build_$@; fi
++@if [ ! -e ./build_$@/CMakeCache.txt ]; then Tools/check_submodules.sh && mkdir -p ./build_$@ && cd ./build_$@ && cmake $(2) -G$(PX4_CMAKE_GENERATOR) || (cd .. && rm -rf ./build_$@); fi
++@(cd ./build_$@ && $(PX4_MAKE) $(PX4_MAKE_ARGS) $(ARGS))
 endef
 
 # create empty targets to avoid msgs for targets passed to cmake
@@ -241,7 +241,7 @@ run_sitl_ros: sitl_deprecation
 
 gazebo_build:
 	@mkdir -p build_gazebo
-	@if [ ! -e $(PWD)/build_gazebo/CMakeCache.txt ];then cd build_gazebo && cmake -Wno-dev -G$(PX4_CMAKE_GENERATOR) $(PWD)/Tools/sitl_gazebo; fi
+	@if [ ! -e ./build_gazebo/CMakeCache.txt ];then cd build_gazebo && cmake -Wno-dev -G$(PX4_CMAKE_GENERATOR) ../Tools/sitl_gazebo; fi
 	@cd build_gazebo && $(PX4_MAKE) $(PX4_MAKE_ARGS)
 	@cd build_gazebo && $(PX4_MAKE) $(PX4_MAKE_ARGS) sdf
 
@@ -339,7 +339,7 @@ distclean: submodulesclean
 	@git clean -ff -x -d -e ".project" -e ".cproject"
 
 # targets handled by cmake
-cmake_targets = test upload package package_source debug debug_tui debug_ddd debug_io debug_io_tui debug_io_ddd check_weak \
+cmake_targets = install test upload package package_source debug debug_tui debug_ddd debug_io debug_io_tui debug_io_ddd check_weak \
 	run_cmake_config config gazebo gazebo_gdb gazebo_lldb jmavsim replay \
 	jmavsim_gdb jmavsim_lldb gazebo_gdb_iris gazebo_lldb_tailsitter gazebo_iris gazebo_iris_opt_flow gazebo_tailsitter \
 	gazebo_gdb_standard_vtol gazebo_lldb_standard_vtol gazebo_standard_vtol gazebo_plane gazebo_solo gazebo_typhoon_h480
diff --git a/Tools/sitl_gazebo b/Tools/sitl_gazebo
index 82b3548750..0e3e246f9e 160000
--- a/Tools/sitl_gazebo
+++ b/Tools/sitl_gazebo
@@ -1 +1 @@
-Subproject commit 82b354875040d97a3f598f0dc260ddc9ca4a95b3
+Subproject commit 0e3e246f9e00ab2281721b72b0ab934a066148f3
diff --git a/posix-configs/SITL/init/rcS_lpe_gazebo_iris b/posix-configs/SITL/init/rcS_lpe_gazebo_iris
index a72c2c432a..9c84018e98 100644
--- a/posix-configs/SITL/init/rcS_lpe_gazebo_iris
+++ b/posix-configs/SITL/init/rcS_lpe_gazebo_iris
@@ -10,34 +10,40 @@ param set CAL_ACC0_ID 1376264
 param set CAL_ACC1_ID 1310728
 param set CAL_MAG0_ID 196616
 param set CAL_GYRO0_XOFF 0.01
-param set CAL_ACC0_XOFF 1
-param set CAL_ACC0_YOFF 2
-param set CAL_ACC0_ZOFF 3
+param set CAL_ACC0_XOFF 0.01
+param set CAL_ACC0_YOFF -0.01
+param set CAL_ACC0_ZOFF 0.01
 param set CAL_ACC0_XSCALE 1.01
 param set CAL_ACC0_YSCALE 1.01
 param set CAL_ACC0_ZSCALE 1.01
 param set CAL_ACC1_XOFF 0.01
 param set CAL_MAG0_XOFF 0.01
-param set MC_ROLLRATE_P 0.3
-param set MC_PITCHRATE_P 0.3
-param set MC_PITCH_P 5.5
-param set MC_ROLL_P 5.5
-param set MC_ROLLRATE_I 0.1
-param set MC_PITCHRATE_I 0.1
-param set MPC_XY_P 0.15
-param set MPC_XY_VEL_P 0.05
-param set MPC_XY_VEL_D 0.005
-param set MPC_XY_FF 0.1
 param set SENS_BOARD_ROT 8
 param set SENS_BOARD_X_OFF 0.000001
-
-# changes for LPE
 param set COM_RC_IN_MODE 1
 param set NAV_DLL_ACT 2
-param set LPE_BETA_MAX 10000
+param set COM_DISARM_LAND 3
+param set NAV_ACC_RAD 2.0
+param set COM_OF_LOSS_T 5
+param set COM_OBL_ACT 2
+param set COM_OBL_RC_ACT 0
+param set RTL_RETURN_ALT 30.0
+param set RTL_DESCEND_ALT 5.0
+param set RTL_LAND_DELAY 5
+param set MIS_TAKEOFF_ALT 2.5
+param set MC_ROLLRATE_P 0.2
+param set MC_PITCHRATE_P 0.2
+param set MC_PITCH_P 6
+param set MC_ROLL_P 6
+param set MPC_HOLD_MAX_Z 2.0
+param set MPC_HOLD_XY_DZ 0.1
+param set MPC_Z_VEL_P 0.6
+param set MPC_Z_VEL_I 0.15
+param set EKF2_GBIAS_INIT 0.01
+param set EKF2_ANGERR_INIT 0.01
 replay tryapplyparams
 simulator start -s
-rgbled start
+rgbledsim start
 tone_alarm start
 gyrosim start
 accelsim start
@@ -51,24 +57,22 @@ commander start
 land_detector start multicopter
 navigator start
 attitude_estimator_q start
+local_position_estimator start
 mc_pos_control start
 mc_att_control start
-mixer load /dev/pwm_output0 ../../../../ROMFS/px4fmu_common/mixers/quad_w.main.mix
-mavlink start -u 14556 -r 2000000
-mavlink start -u 14557 -r 2000000 -m onboard -o 14540
-mavlink stream -r 80 -s POSITION_TARGET_LOCAL_NED -u 14556
-mavlink stream -r 80 -s LOCAL_POSITION_NED_COV -u 14556
-mavlink stream -r 80 -s GLOBAL_POSITION_INT -u 14556
-mavlink stream -r 80 -s ATTITUDE -u 14556
-mavlink stream -r 80 -s ATTITUDE_QUATERNION -u 14556
-mavlink stream -r 80 -s ATTITUDE_TARGET -u 14556
+mixer load /dev/pwm_output0 /usr/share/px4/ROMFS/px4fmu_common/mixers/quad_w.main.mix
+mavlink start -u 14556 -r 4000000
+mavlink start -u 14557 -r 4000000 -m onboard -o 14540
+mavlink stream -r 50 -s POSITION_TARGET_LOCAL_NED -u 14556
+mavlink stream -r 50 -s LOCAL_POSITION_NED -u 14556
+mavlink stream -r 50 -s GLOBAL_POSITION_INT -u 14556
+mavlink stream -r 50 -s ATTITUDE -u 14556
+mavlink stream -r 50 -s ATTITUDE_QUATERNION -u 14556
+mavlink stream -r 50 -s ATTITUDE_TARGET -u 14556
+mavlink stream -r 50 -s SERVO_OUTPUT_RAW_0 -u 14556
 mavlink stream -r 20 -s RC_CHANNELS -u 14556
 mavlink stream -r 250 -s HIGHRES_IMU -u 14556
 mavlink stream -r 10 -s OPTICAL_FLOW_RAD -u 14556
 sdlog2 start -r 100 -e -t -a
-
-# start LPE at end, when we know it is ok to init sensors
-sleep 5
-local_position_estimator start
 mavlink boot_complete
 replay trystart
diff --git a/posix-configs/SITL/init/rcS_lpe_gazebo_iris_opt_flow b/posix-configs/SITL/init/rcS_lpe_gazebo_iris_opt_flow
index 536f7790b8..c1a6a2eae5 100644
--- a/posix-configs/SITL/init/rcS_lpe_gazebo_iris_opt_flow
+++ b/posix-configs/SITL/init/rcS_lpe_gazebo_iris_opt_flow
@@ -18,27 +18,38 @@ param set CAL_ACC0_YSCALE 1.01
 param set CAL_ACC0_ZSCALE 1.01
 param set CAL_ACC1_XOFF 0.01
 param set CAL_MAG0_XOFF 0.01
+param set SENS_BOARD_ROT 8
+param set SENS_BOARD_X_OFF 0.000001
+param set COM_RC_IN_MODE 1
+param set NAV_DLL_ACT 2
+param set COM_DISARM_LAND 3
+param set NAV_ACC_RAD 12.0
+param set RTL_RETURN_ALT 30.0
+param set RTL_DESCEND_ALT 10.0
+param set RTL_LAND_DELAY 0
+param set MIS_TAKEOFF_ALT 2.5
 param set MC_ROLLRATE_P 0.3
 param set MC_PITCHRATE_P 0.3
 param set MC_PITCH_P 5.5
 param set MC_ROLL_P 5.5
 param set MC_ROLLRATE_I 0.1
 param set MC_PITCHRATE_I 0.1
-param set MPC_XY_P 0.15
-param set MPC_XY_VEL_P 0.05
-param set MPC_XY_VEL_D 0.005
-param set MPC_XY_FF 0.1
-param set SENS_BOARD_ROT 8
-param set SENS_BOARD_X_OFF 0.000001
-param set COM_RC_IN_MODE 1
-param set NAV_ACC_RAD 2.0
-param set RTL_RETURN_ALT 30.0
-param set RTL_DESCEND_ALT 10.0
+param set MPC_HOLD_MAX_Z 2.0
+param set MPC_HOLD_XY_DZ 0.1
+param set MPC_Z_VEL_P 0.8
+param set MPC_Z_VEL_I 0.15
+param set MPC_XY_VEL_P 0.15
+param set MPC_XY_VEL_I 0.2
+
+# changes for LPE indoor flight
+param set LPE_GPS_ON 0
+param set MPC_ALT_MODE 1
+param set LPE_T_MAX_GRADE 0
+param set MPC_XY_VEL_MAX 2
+param set MPC_Z_VEL_MAX 2
+param set MPC_XY_P 0.5
+param set MIS_TAKEOFF_ALT 2
 
-# changes for LPE
-param set COM_RC_IN_MODE 1
-param set NAV_DLL_ACT 2
-param set LPE_BETA_MAX 10000
 replay tryapplyparams
 simulator start -s
 rgbledsim start
@@ -55,13 +66,14 @@ commander start
 land_detector start multicopter
 navigator start
 attitude_estimator_q start
+local_position_estimator start
 mc_pos_control start
 mc_att_control start
-mixer load /dev/pwm_output0 ../../../../ROMFS/px4fmu_common/mixers/quad_w.main.mix
+mixer load /dev/pwm_output0 /usr/share/px4/ROMFS/px4fmu_common/mixers/quad_w.main.mix
 mavlink start -u 14556 -r 2000000
 mavlink start -u 14557 -r 2000000 -m onboard -o 14540
 mavlink stream -r 80 -s POSITION_TARGET_LOCAL_NED -u 14556
-mavlink stream -r 80 -s LOCAL_POSITION_NED_COV -u 14556
+mavlink stream -r 80 -s LOCAL_POSITION_NED -u 14556
 mavlink stream -r 80 -s GLOBAL_POSITION_INT -u 14556
 mavlink stream -r 80 -s ATTITUDE -u 14556
 mavlink stream -r 80 -s ATTITUDE_QUATERNION -u 14556
@@ -70,9 +82,5 @@ mavlink stream -r 20 -s RC_CHANNELS -u 14556
 mavlink stream -r 250 -s HIGHRES_IMU -u 14556
 mavlink stream -r 10 -s OPTICAL_FLOW_RAD -u 14556
 sdlog2 start -r 100 -e -t -a
-
-# start LPE at end, when we know it is ok to init sensors
-sleep 5
-local_position_estimator start
 mavlink boot_complete
 replay trystart
diff --git a/posix-configs/SITL/init/rcS_lpe_jmavsim_iris b/posix-configs/SITL/init/rcS_lpe_jmavsim_iris
index 6192b2e434..106d032275 100644
--- a/posix-configs/SITL/init/rcS_lpe_jmavsim_iris
+++ b/posix-configs/SITL/init/rcS_lpe_jmavsim_iris
@@ -18,21 +18,30 @@ param set CAL_ACC0_YSCALE 1.01
 param set CAL_ACC0_ZSCALE 1.01
 param set CAL_ACC1_XOFF 0.01
 param set CAL_MAG0_XOFF 0.01
-param set MC_ROLLRATE_P 0.2
-param set MC_PITCHRATE_P 0.2
-param set MC_PITCH_P 6
-param set MC_ROLL_P 6
 param set MPC_XY_P 0.4
 param set MPC_XY_VEL_P 0.2
 param set MPC_XY_VEL_D 0.005
 param set SENS_BOARD_ROT 0
 param set SENS_BOARD_X_OFF 0.000001
 param set COM_RC_IN_MODE 1
+param set NAV_DLL_ACT 2
+param set COM_DISARM_LAND 3
 param set NAV_ACC_RAD 2.0
 param set RTL_RETURN_ALT 30.0
 param set RTL_DESCEND_ALT 10.0
-param set COM_RC_IN_MODE 1
-param set NAV_DLL_ACT 2
+param set RTL_LAND_DELAY 0
+param set MIS_TAKEOFF_ALT 2.5
+param set MC_ROLLRATE_P 0.2
+param set MC_PITCHRATE_P 0.2
+param set MC_PITCH_P 6
+param set MC_ROLL_P 6
+param set MPC_HOLD_MAX_Z 2.0
+param set MPC_HOLD_XY_DZ 0.1
+param set MPC_Z_VEL_MAX 2.0
+param set MPC_Z_VEL_P 0.4
+param set EKF2_GBIAS_INIT 0.01
+param set EKF2_ANGERR_INIT 0.01
+replay tryapplyparams
 simulator start -s
 rgbledsim start
 tone_alarm start
@@ -41,18 +50,22 @@ accelsim start
 barosim start
 adcsim start
 gpssim start
+#gps start -d /dev/ttyACM0 -s
 pwm_out_sim mode_pwm
 sleep 1
 sensors start
 commander start
 land_detector start multicopter
 navigator start
+attitude_estimator_q start
+local_position_estimator start
 mc_pos_control start
 mc_att_control start
-mixer load /dev/pwm_output0 ../../../../ROMFS/px4fmu_common/mixers/quad_x.main.mix
+mixer load /dev/pwm_output0 /usr/share/px4/ROMFS/px4fmu_common/mixers/quad_x.main.mix
 mavlink start -u 14556 -r 2000000
+mavlink start -u 14557 -r 2000000 -m onboard -o 14540
 mavlink stream -r 80 -s POSITION_TARGET_LOCAL_NED -u 14556
-mavlink stream -r 80 -s LOCAL_POSITION_NED_COV -u 14556
+mavlink stream -r 80 -s LOCAL_POSITION_NED -u 14556
 mavlink stream -r 80 -s GLOBAL_POSITION_INT -u 14556
 mavlink stream -r 80 -s ATTITUDE -u 14556
 mavlink stream -r 80 -s ATTITUDE_QUATERNION -u 14556
@@ -60,7 +73,8 @@ mavlink stream -r 80 -s ATTITUDE_TARGET -u 14556
 mavlink stream -r 20 -s RC_CHANNELS -u 14556
 mavlink stream -r 250 -s HIGHRES_IMU -u 14556
 mavlink stream -r 10 -s OPTICAL_FLOW_RAD -u 14556
+mavlink stream -r 20 -s MANUAL_CONTROL -u 14556
 sdlog2 start -r 100 -e -t -a
-attitude_estimator_q start
-local_position_estimator start
+logger start -e -t
 mavlink boot_complete
+replay trystart
diff --git a/src/firmware/posix/CMakeLists.txt b/src/firmware/posix/CMakeLists.txt
index ff348c9b45..6a43db5c11 100644
--- a/src/firmware/posix/CMakeLists.txt
+++ b/src/firmware/posix/CMakeLists.txt
@@ -154,4 +154,12 @@ foreach(viewer none jmavsim gazebo replay)
 	endforeach()
 endforeach()
 
+#=============================================================================
+# install
+#
+
+install(TARGETS mainapp DESTINATION bin)
+install(DIRECTORY ${PROJECT_SOURCE_DIR}/ROMFS DESTINATION ${CMAKE_INSTALL_DATADIR}/${PROJECT_NAME})
+install(DIRECTORY ${PROJECT_SOURCE_DIR}/posix-configs DESTINATION ${CMAKE_INSTALL_DATADIR}/${PROJECT_NAME})
+
 # vim: set noet ft=cmake fenc=utf-8 ff=unix :
diff --git a/src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp b/src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp
index 584efc77e1..c226560482 100644
--- a/src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp
+++ b/src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp
@@ -25,8 +25,8 @@ BlockLocalPositionEstimator::BlockLocalPositionEstimator() :
 	// subscriptions, set rate, add to list
 	_sub_armed(ORB_ID(actuator_armed), 1000 / 2, 0, &getSubscriptions()),
 	_sub_att(ORB_ID(vehicle_attitude), 1000 / 100, 0, &getSubscriptions()),
-	// flow 10 hz
-	_sub_flow(ORB_ID(optical_flow), 1000 / 10, 0, &getSubscriptions()),
+	// set flow max update rate higher than expected to we don't lose packets
+	_sub_flow(ORB_ID(optical_flow), 1000 / 100, 0, &getSubscriptions()),
 	// main prediction loop, 100 hz
 	_sub_sensor(ORB_ID(sensor_combined), 1000 / 100, 0, &getSubscriptions()),
 	// status updates 2 hz
diff --git a/src/modules/local_position_estimator/sensors/flow.cpp b/src/modules/local_position_estimator/sensors/flow.cpp
index 2e45333f21..13b646d9e0 100644
--- a/src/modules/local_position_estimator/sensors/flow.cpp
+++ b/src/modules/local_position_estimator/sensors/flow.cpp
@@ -82,6 +82,9 @@ int BlockLocalPositionEstimator::flowMeasure(Vector<float, n_y_flow> &y)
 	float gyro_y_rad = _flow_gyro_y_high_pass.update(
 				   _sub_flow.get().gyro_y_rate_integral);
 
+	//warnx("flow x: %10.4f y: %10.4f gyro_x: %10.4f gyro_y: %10.4f d: %10.4f",
+	//double(flow_x_rad), double(flow_y_rad), double(gyro_x_rad), double(gyro_y_rad), double(d));
+
 	// compute velocities in camera frame using ground distance
 	// assume camera frame is body frame
 	Vector3f delta_b(
diff --git a/src/modules/simulator/simulator.h b/src/modules/simulator/simulator.h
index 104101060f..fd6fc267c6 100644
--- a/src/modules/simulator/simulator.h
+++ b/src/modules/simulator/simulator.h
@@ -55,6 +55,7 @@
 #include <systemlib/battery.h>
 #include <uORB/uORB.h>
 #include <uORB/topics/optical_flow.h>
+#include <uORB/topics/distance_sensor.h>
 #include <v1.0/mavlink_types.h>
 #include <v1.0/common/mavlink.h>
 namespace simulator
@@ -238,6 +239,7 @@ private:
 		_gyro_pub(nullptr),
 		_mag_pub(nullptr),
 		_flow_pub(nullptr),
+		_dist_pub(nullptr),
 		_battery_pub(nullptr),
 		_initialized(false)
 #ifndef __PX4_QURT
@@ -288,6 +290,7 @@ private:
 	orb_advert_t _gyro_pub;
 	orb_advert_t _mag_pub;
 	orb_advert_t _flow_pub;
+	orb_advert_t _dist_pub;
 	orb_advert_t _battery_pub;
 
 	bool _initialized;
@@ -298,6 +301,7 @@ private:
 	// class methods
 	int publish_sensor_topics(mavlink_hil_sensor_t *imu);
 	int publish_flow_topic(mavlink_hil_optical_flow_t *flow);
+	int publish_distance_topic(mavlink_distance_sensor_t *dist);
 
 #ifndef __PX4_QURT
 	// uORB publisher handlers
diff --git a/src/modules/simulator/simulator_mavlink.cpp b/src/modules/simulator/simulator_mavlink.cpp
index 6b01a22909..feccfeca45 100644
--- a/src/modules/simulator/simulator_mavlink.cpp
+++ b/src/modules/simulator/simulator_mavlink.cpp
@@ -283,6 +283,12 @@ void Simulator::handle_message(mavlink_message_t *msg, bool publish)
 		publish_flow_topic(&flow);
 		break;
 
+	case MAVLINK_MSG_ID_DISTANCE_SENSOR:
+		mavlink_distance_sensor_t dist;
+		mavlink_msg_distance_sensor_decode(msg, &dist);
+		publish_distance_topic(&dist);
+		break;
+
 	case MAVLINK_MSG_ID_HIL_GPS:
 		mavlink_hil_gps_t gps_sim;
 		mavlink_msg_hil_gps_decode(msg, &gps_sim);
@@ -863,7 +869,7 @@ int Simulator::publish_flow_topic(mavlink_hil_optical_flow_t *flow_mavlink)
 	flow.gyro_y_rate_integral = flow_mavlink->integrated_ygyro;
 	flow.gyro_z_rate_integral = flow_mavlink->integrated_zgyro;
 	flow.pixel_flow_x_integral = flow_mavlink->integrated_x;
-	flow.pixel_flow_x_integral = flow_mavlink->integrated_y;
+	flow.pixel_flow_y_integral = flow_mavlink->integrated_y;
 	flow.quality = flow_mavlink->quality;
 
 	int flow_multi;
@@ -871,3 +877,25 @@ int Simulator::publish_flow_topic(mavlink_hil_optical_flow_t *flow_mavlink)
 
 	return OK;
 }
+
+int Simulator::publish_distance_topic(mavlink_distance_sensor_t *dist_mavlink)
+{
+	uint64_t timestamp = hrt_absolute_time();
+
+	struct distance_sensor_s dist;
+	memset(&dist, 0, sizeof(dist));
+
+	dist.timestamp = timestamp;
+	dist.min_distance = dist_mavlink->min_distance / 100.0f;
+	dist.max_distance = dist_mavlink->max_distance / 100.0f;
+	dist.current_distance = dist_mavlink->current_distance / 100.0f;
+	dist.type = dist_mavlink->type;
+	dist.id = dist_mavlink->id;
+	dist.orientation = dist_mavlink->orientation;
+	dist.covariance = dist_mavlink->covariance / 100.0f;
+
+	int dist_multi;
+	orb_publish_auto(ORB_ID(distance_sensor), &_dist_pub, &dist, &dist_multi, ORB_PRIO_HIGH);
+
+	return OK;
+}

From 4f62a35993d5c10c63a40acbdeaae50f96b825af Mon Sep 17 00:00:00 2001
From: James Goppert <james.goppert@gmail.com>
Date: Sun, 24 Jul 2016 16:27:12 -0400
Subject: [PATCH 1063/1230] Fixed sitl_gazebo.

---
 Tools/sitl_gazebo | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/Tools/sitl_gazebo b/Tools/sitl_gazebo
index 0e3e246f9e..a2430ba2f4 160000
--- a/Tools/sitl_gazebo
+++ b/Tools/sitl_gazebo
@@ -1 +1 @@
-Subproject commit 0e3e246f9e00ab2281721b72b0ab934a066148f3
+Subproject commit a2430ba2f4e44a162077278b321a5859e85c2686

From 563122f5d3ef264659a745e4342ea557c372b8c7 Mon Sep 17 00:00:00 2001
From: James Goppert <james.goppert@gmail.com>
Date: Sun, 24 Jul 2016 16:30:01 -0400
Subject: [PATCH 1064/1230] sitl_gazebo update.

---
 Tools/sitl_gazebo | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/Tools/sitl_gazebo b/Tools/sitl_gazebo
index a2430ba2f4..817ecad1de 160000
--- a/Tools/sitl_gazebo
+++ b/Tools/sitl_gazebo
@@ -1 +1 @@
-Subproject commit a2430ba2f4e44a162077278b321a5859e85c2686
+Subproject commit 817ecad1de5a9ed416281e63970e27b3a1e31f51

From 7458f1e07de84a6f77a2e9c993ce18ed6e36442a Mon Sep 17 00:00:00 2001
From: James Goppert <jgoppert@users.noreply.github.com>
Date: Sun, 24 Jul 2016 17:07:14 -0400
Subject: [PATCH 1065/1230] Update sitl_gazebo. (#5136)

* Update sitl_gazebo.

* Update sitl_gazebo.
---
 Tools/sitl_gazebo | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/Tools/sitl_gazebo b/Tools/sitl_gazebo
index 817ecad1de..ff20e62ddf 160000
--- a/Tools/sitl_gazebo
+++ b/Tools/sitl_gazebo
@@ -1 +1 @@
-Subproject commit 817ecad1de5a9ed416281e63970e27b3a1e31f51
+Subproject commit ff20e62ddf3b3e8c68f9b395ce3a782c36f10258

From 880fa47ba27438c81c4722244f5bb188f3b0af45 Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Sun, 24 Jul 2016 23:09:05 +0200
Subject: [PATCH 1066/1230] Fix install in Mac OS X Gazebo

---
 Tools/sitl_gazebo | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/Tools/sitl_gazebo b/Tools/sitl_gazebo
index ff20e62ddf..1a2c61b759 160000
--- a/Tools/sitl_gazebo
+++ b/Tools/sitl_gazebo
@@ -1 +1 @@
-Subproject commit ff20e62ddf3b3e8c68f9b395ce3a782c36f10258
+Subproject commit 1a2c61b7598920b3396fb4d8ed544befe3d7cc2f

From e3f3233ee438b8d38e581f5cc629d92c23c58e13 Mon Sep 17 00:00:00 2001
From: Roman <bapstr@ethz.ch>
Date: Mon, 27 Jun 2016 08:11:22 +0200
Subject: [PATCH 1067/1230] fw position controller: fixup attitude setpoint
 generation

- generate complete attitude setpoint for position and altitude
control mode
- fix generation of roll setpoint for position control which lead to
wing rock
- add roll and pitch setpoint offsets so that they are logged as well

Signed-off-by: Roman <bapstr@ethz.ch>
---
 .../fw_pos_control_l1_main.cpp                | 20 ++++++++++++-------
 1 file changed, 13 insertions(+), 7 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 8871ccf551..96584a788d 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
@@ -291,6 +291,7 @@ private:
 		float throttle_slew_max;
 		float man_roll_max_rad;
 		float rollsp_offset_rad;
+		float pitchsp_offset_rad;
 
 		float throttle_land_max;
 
@@ -345,6 +346,7 @@ private:
 		param_t throttle_slew_max;
 		param_t man_roll_max_deg;
 		param_t rollsp_offset_deg;
+		param_t pitchsp_offset_deg;
 
 		param_t throttle_land_max;
 
@@ -623,6 +625,7 @@ FixedwingPositionControl::FixedwingPositionControl() :
 	_parameter_handles.throttle_land_max = param_find("FW_THR_LND_MAX");
 	_parameter_handles.man_roll_max_deg = param_find("FW_MAN_R_MAX");
 	_parameter_handles.rollsp_offset_deg = param_find("FW_RSP_OFF");
+	_parameter_handles.pitchsp_offset_deg = param_find("FW_PSP_OFF");
 
 	_parameter_handles.land_slope_angle = param_find("FW_LND_ANG");
 	_parameter_handles.land_H1_virt = param_find("FW_LND_HVIRT");
@@ -709,6 +712,8 @@ FixedwingPositionControl::parameters_update()
 	_parameters.man_roll_max_rad = math::radians(_parameters.man_roll_max_rad);
 	param_get(_parameter_handles.rollsp_offset_deg, &_parameters.rollsp_offset_rad);
 	_parameters.rollsp_offset_rad = math::radians(_parameters.rollsp_offset_rad);
+	param_get(_parameter_handles.pitchsp_offset_deg, &_parameters.pitchsp_offset_rad);
+	_parameters.pitchsp_offset_rad = math::radians(_parameters.pitchsp_offset_rad);
 
 
 	param_get(_parameter_handles.time_const, &(_parameters.time_const));
@@ -1929,6 +1934,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
 		} else {
 			_hdg_hold_enabled = false;
 			_yaw_lock_engaged = false;
+			_att_sp.roll_body = _manual.y * _parameters.man_roll_max_rad;
 		}
 
 	} else if (_control_mode.flag_control_altitude_enabled) {
@@ -1980,14 +1986,10 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
 					   ground_speed,
 					   tecs_status_s::TECS_MODE_NORMAL);
 
-		// calculate roll setpoint from user input
-		// this is already calculated in fw_att_control_main.cpp but we do it here for the sake of logging
-		if (fabsf(_manual.y) < 0.01f && fabsf(_roll) < 0.2f) {
-			_att_sp.roll_body = _parameters.rollsp_offset_rad;
-
+		if (fabsf(_manual.y) > 0.01f) {
+			_att_sp.roll_body = _manual.y * _parameters.man_roll_max_rad;
 		} else {
-			_att_sp.roll_body = (_manual.y * _parameters.man_roll_max_rad)
-					    + _parameters.rollsp_offset_rad;
+			_att_sp.roll_body = 0.0f;
 		}
 
 	} else {
@@ -2204,6 +2206,10 @@ FixedwingPositionControl::task_main()
 			if (control_position(current_position, ground_speed, _pos_sp_triplet)) {
 				_att_sp.timestamp = hrt_absolute_time();
 
+				// add attitude setpoint offsets
+				_att_sp.roll_body += _parameters.rollsp_offset_rad;
+				_att_sp.pitch_body += _parameters.pitchsp_offset_rad;
+
 				/* lazily publish the setpoint only once available */
 				if (_attitude_sp_pub != nullptr) {
 					/* publish the attitude setpoint */

From fd51bf44d59a164ac8ae5c2d361571b4c8f5aaeb Mon Sep 17 00:00:00 2001
From: tumbili <roman@px4.io>
Date: Mon, 27 Jun 2016 10:53:39 +0200
Subject: [PATCH 1068/1230] fw position / fw attitude control: move attitude
 setpoint generation to position controller

- attitude setpoints for all modes are now computed in the fw position
controller

Signed-off-by: tumbili <roman@px4.io>
---
 .../fw_att_control/fw_att_control_main.cpp    | 134 +++---------------
 .../fw_pos_control_l1_main.cpp                |  19 ++-
 2 files changed, 31 insertions(+), 122 deletions(-)

diff --git a/src/modules/fw_att_control/fw_att_control_main.cpp b/src/modules/fw_att_control/fw_att_control_main.cpp
index 06b126895a..199b3189cb 100644
--- a/src/modules/fw_att_control/fw_att_control_main.cpp
+++ b/src/modules/fw_att_control/fw_att_control_main.cpp
@@ -975,129 +975,27 @@ FixedwingAttitudeControl::task_main()
 				float yaw_manual = 0.0f;
 				float throttle_sp = 0.0f;
 
-				/* Read attitude setpoint from uorb if
-				 * - velocity control or position control is enabled (pos controller is running)
-				 * - manual control is disabled (another app may send the setpoint, but it should
-				 *   for sure not be set from the remote control values)
-				 */
-				if (_vcontrol_mode.flag_control_auto_enabled ||
-				    !_vcontrol_mode.flag_control_manual_enabled) {
-					/* read in attitude setpoint from attitude setpoint uorb topic */
-					roll_sp = _att_sp.roll_body + _parameters.rollsp_offset_rad;
-					pitch_sp = _att_sp.pitch_body + _parameters.pitchsp_offset_rad;
-					yaw_sp = _att_sp.yaw_body;
-					throttle_sp = _att_sp.thrust;
+				roll_sp = _att_sp.roll_body;
+				pitch_sp = _att_sp.pitch_body;
+				yaw_sp = _att_sp.yaw_body;
+				throttle_sp = _att_sp.thrust;
 
-					/* reset integrals where needed */
-					if (_att_sp.roll_reset_integral) {
-						_roll_ctrl.reset_integrator();
-					}
-
-					if (_att_sp.pitch_reset_integral) {
-						_pitch_ctrl.reset_integrator();
-					}
-
-					if (_att_sp.yaw_reset_integral) {
-						_yaw_ctrl.reset_integrator();
-						_wheel_ctrl.reset_integrator();
-					}
-
-				} else if (_vcontrol_mode.flag_control_velocity_enabled) {
-
-					/* the pilot does not want to change direction,
-					 * take straight attitude setpoint from position controller
-					 */
-					if (fabsf(_manual.y) < 0.01f && fabsf(_roll) < 0.2f) {
-						roll_sp = _att_sp.roll_body + _parameters.rollsp_offset_rad;
-
-					} else {
-						roll_sp = (_manual.y * _parameters.man_roll_max)
-							  + _parameters.rollsp_offset_rad;
-					}
-
-					pitch_sp = _att_sp.pitch_body + _parameters.pitchsp_offset_rad;
-					throttle_sp = _att_sp.thrust;
-
-					/* reset integrals where needed */
-					if (_att_sp.roll_reset_integral) {
-						_roll_ctrl.reset_integrator();
-					}
-
-					if (_att_sp.pitch_reset_integral) {
-						_pitch_ctrl.reset_integrator();
-					}
-
-					if (_att_sp.yaw_reset_integral) {
-						_yaw_ctrl.reset_integrator();
-						_wheel_ctrl.reset_integrator();
-					}
-
-				} else if (_vcontrol_mode.flag_control_altitude_enabled) {
-					/*
-					 * Velocity should be controlled and manual is enabled.
-					*/
-					roll_sp = (_manual.y * _parameters.man_roll_max) + _parameters.rollsp_offset_rad;
-					pitch_sp = _att_sp.pitch_body + _parameters.pitchsp_offset_rad;
-					throttle_sp = _att_sp.thrust;
-
-					/* reset integrals where needed */
-					if (_att_sp.roll_reset_integral) {
-						_roll_ctrl.reset_integrator();
-					}
-
-					if (_att_sp.pitch_reset_integral) {
-						_pitch_ctrl.reset_integrator();
-					}
-
-					if (_att_sp.yaw_reset_integral) {
-						_yaw_ctrl.reset_integrator();
-						_wheel_ctrl.reset_integrator();
-					}
-
-				} else {
-					/*
-					 * Scale down roll and pitch as the setpoints are radians
-					 * and a typical remote can only do around 45 degrees, the mapping is
-					 * -1..+1 to -man_roll_max rad..+man_roll_max rad (equivalent for pitch)
-					 *
-					 * With this mapping the stick angle is a 1:1 representation of
-					 * the commanded attitude.
-					 *
-					 * The trim gets subtracted here from the manual setpoint to get
-					 * the intended attitude setpoint. Later, after the rate control step the
-					 * trim is added again to the control signal.
-					 */
-					roll_sp = (_manual.y * _parameters.man_roll_max) + _parameters.rollsp_offset_rad;
-					pitch_sp = -(_manual.x * _parameters.man_pitch_max) + _parameters.pitchsp_offset_rad;
-					/* allow manual control of rudder deflection */
+				if (!_vcontrol_mode.flag_control_velocity_enabled) {
 					yaw_manual = _manual.r;
-					throttle_sp = _manual.z;
+				}
 
-					/*
-					 * in manual mode no external source should / does emit attitude setpoints.
-					 * emit the manual setpoint here to allow attitude controller tuning
-					 * in attitude control mode.
-					 */
-					struct vehicle_attitude_setpoint_s att_sp = {};
-					att_sp.timestamp = hrt_absolute_time();
-					att_sp.roll_body = roll_sp;
-					att_sp.pitch_body = pitch_sp;
-					att_sp.yaw_body = 0.0f - _parameters.trim_yaw;
-					att_sp.thrust = throttle_sp;
+				/* reset integrals where needed */
+				if (_att_sp.roll_reset_integral) {
+					_roll_ctrl.reset_integrator();
+				}
 
-					att_sp.roll_reset_integral = false;
-					att_sp.pitch_reset_integral = false;
-					att_sp.yaw_reset_integral = false;
+				if (_att_sp.pitch_reset_integral) {
+					_pitch_ctrl.reset_integrator();
+				}
 
-					/* lazily publish the setpoint only once available */
-					if (_attitude_sp_pub != nullptr) {
-						/* publish the attitude setpoint */
-						orb_publish(_attitude_setpoint_id, _attitude_sp_pub, &att_sp);
-
-					} else if (_attitude_setpoint_id) {
-						/* advertise and publish */
-						_attitude_sp_pub = orb_advertise(_attitude_setpoint_id, &att_sp);
-					}
+				if (_att_sp.yaw_reset_integral) {
+					_yaw_ctrl.reset_integrator();
+					_wheel_ctrl.reset_integrator();
 				}
 
 				/* If the aircraft is on ground reset the integrators */
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 96584a788d..8390266d9f 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
@@ -290,6 +290,7 @@ private:
 		float throttle_cruise;
 		float throttle_slew_max;
 		float man_roll_max_rad;
+		float man_pitch_max_rad;
 		float rollsp_offset_rad;
 		float pitchsp_offset_rad;
 
@@ -345,6 +346,7 @@ private:
 		param_t throttle_cruise;
 		param_t throttle_slew_max;
 		param_t man_roll_max_deg;
+		param_t man_pitch_max_deg;
 		param_t rollsp_offset_deg;
 		param_t pitchsp_offset_deg;
 
@@ -624,6 +626,7 @@ FixedwingPositionControl::FixedwingPositionControl() :
 	_parameter_handles.throttle_cruise = param_find("FW_THR_CRUISE");
 	_parameter_handles.throttle_land_max = param_find("FW_THR_LND_MAX");
 	_parameter_handles.man_roll_max_deg = param_find("FW_MAN_R_MAX");
+	_parameter_handles.man_pitch_max_deg = param_find("FW_MAN_P_MAX");
 	_parameter_handles.rollsp_offset_deg = param_find("FW_RSP_OFF");
 	_parameter_handles.pitchsp_offset_deg = param_find("FW_PSP_OFF");
 
@@ -710,6 +713,8 @@ FixedwingPositionControl::parameters_update()
 
 	param_get(_parameter_handles.man_roll_max_deg, &_parameters.man_roll_max_rad);
 	_parameters.man_roll_max_rad = math::radians(_parameters.man_roll_max_rad);
+	param_get(_parameter_handles.man_pitch_max_deg, &_parameters.man_pitch_max_rad);
+	_parameters.man_pitch_max_rad = math::radians(_parameters.man_pitch_max_rad);
 	param_get(_parameter_handles.rollsp_offset_deg, &_parameters.rollsp_offset_rad);
 	_parameters.rollsp_offset_rad = math::radians(_parameters.rollsp_offset_rad);
 	param_get(_parameter_handles.pitchsp_offset_deg, &_parameters.pitchsp_offset_rad);
@@ -1996,6 +2001,14 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
 		_control_mode_current = FW_POSCTRL_MODE_OTHER;
 
 		/** MANUAL FLIGHT **/
+		setpoint = false;
+		if (_control_mode.flag_control_attitude_enabled) {
+			_att_sp.roll_body = _manual.y * _parameters.man_roll_max_rad;
+			_att_sp.pitch_body = -_manual.x * _parameters.man_pitch_max_rad;
+			_att_sp.yaw_body = 0.0f;
+			_att_sp.thrust = _manual.z;
+			setpoint = true;
+		}
 
 		// reset hold altitude
 		_hold_alt = _global_pos.alt;
@@ -2007,9 +2020,6 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
 		// reset lading abort state
 		_fw_pos_ctrl_status.abort_landing = false;
 
-		/* no flight mode applies, do not publish an attitude setpoint */
-		setpoint = false;
-
 		/* reset landing and takeoff state */
 		if (!_last_manual) {
 			reset_landing_state();
@@ -2040,7 +2050,8 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
 	} else if (_control_mode_current ==  FW_POSCTRL_MODE_AUTO &&
 		   pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_IDLE) {
 		_att_sp.thrust = 0.0f;
-
+	} else if (_control_mode_current ==  FW_POSCTRL_MODE_OTHER) {
+		_att_sp.thrust = math::min(_att_sp.thrust, _parameters.throttle_max);
 	} else {
 		/* Copy thrust and pitch values from tecs */
 		if (_vehicle_land_detected.landed) {

From 8c8120e2fc9e66e3962fe123881d57ff6ef94d05 Mon Sep 17 00:00:00 2001
From: tumbili <roman@px4.io>
Date: Mon, 27 Jun 2016 10:54:30 +0200
Subject: [PATCH 1069/1230] fw position controller: logic cleanup

cleaned up logic when not to use pitch setpoint from TECS

Signed-off-by: tumbili <roman@px4.io>
---
 .../fw_pos_control_l1_main.cpp                | 27 ++++++++++++-------
 1 file changed, 18 insertions(+), 9 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 8390266d9f..90879d9811 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
@@ -2065,15 +2065,24 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
 
 	}
 
-	/* During a takeoff waypoint while waiting for launch the pitch sp is set
-	 * already (not by tecs) */
-	if (!(_control_mode_current ==  FW_POSCTRL_MODE_AUTO && (
-		      (pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_TAKEOFF &&
-		       (_launch_detection_state == LAUNCHDETECTION_RES_NONE ||
-			_runway_takeoff.runwayTakeoffEnabled())) ||
-		      (pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_LAND &&
-		       _land_noreturn_vertical))
-	     )) {
+	// decide when to use pitch setpoint from TECS because in some cases pitch
+	// setpoint is generated by other means
+	bool use_tecs_pitch = true;
+
+	// auto runway takeoff
+	use_tecs_pitch &= !(_control_mode_current ==  FW_POSCTRL_MODE_AUTO &&
+		      pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_TAKEOFF &&
+		       (_launch_detection_state == LAUNCHDETECTION_RES_NONE || _runway_takeoff.runwayTakeoffEnabled()));
+
+
+	// flaring during landing
+	use_tecs_pitch &= !(pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_LAND &&
+		       _land_noreturn_vertical);
+
+	// manual attitude control
+	use_tecs_pitch &=  !(_control_mode_current == FW_POSCTRL_MODE_OTHER);
+
+	if (use_tecs_pitch) {
 		_att_sp.pitch_body = get_tecs_pitch();
 	}
 

From ec334f7c9b2b25430f3d92e3a57381774c850327 Mon Sep 17 00:00:00 2001
From: Roman <bapstroman@gmail.com>
Date: Mon, 18 Jul 2016 15:22:09 +0200
Subject: [PATCH 1070/1230] fw_pos_control_l1: limit max roll and pitch
 setpoints for manual modes

Signed-off-by: Roman <bapstroman@gmail.com>
---
 src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp | 5 +++++
 1 file changed, 5 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 90879d9811..699f6e1be1 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
@@ -2230,6 +2230,11 @@ FixedwingPositionControl::task_main()
 				_att_sp.roll_body += _parameters.rollsp_offset_rad;
 				_att_sp.pitch_body += _parameters.pitchsp_offset_rad;
 
+				if (_control_mode.flag_control_manual_enabled) {
+					_att_sp.roll_body = math::constrain(_att_sp.roll_body, -_parameters.man_roll_max_rad, _parameters.man_roll_max_rad);
+					_att_sp.pitch_body = math::constrain(_att_sp.pitch_body, -_parameters.man_pitch_max_rad, _parameters.man_pitch_max_rad);
+				}
+
 				/* lazily publish the setpoint only once available */
 				if (_attitude_sp_pub != nullptr) {
 					/* publish the attitude setpoint */

From 6226a0c77df64a892077bf74a0015b501929c9c5 Mon Sep 17 00:00:00 2001
From: Roman <bapstroman@gmail.com>
Date: Mon, 18 Jul 2016 15:49:55 +0200
Subject: [PATCH 1071/1230] fw_pos_control_l1: fixed code style

Signed-off-by: Roman <bapstroman@gmail.com>
---
 .../fw_pos_control_l1/fw_pos_control_l1_main.cpp       | 10 +++++++---
 1 file changed, 7 insertions(+), 3 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 699f6e1be1..c6344b02c2 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
@@ -1993,6 +1993,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
 
 		if (fabsf(_manual.y) > 0.01f) {
 			_att_sp.roll_body = _manual.y * _parameters.man_roll_max_rad;
+
 		} else {
 			_att_sp.roll_body = 0.0f;
 		}
@@ -2002,6 +2003,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
 
 		/** MANUAL FLIGHT **/
 		setpoint = false;
+
 		if (_control_mode.flag_control_attitude_enabled) {
 			_att_sp.roll_body = _manual.y * _parameters.man_roll_max_rad;
 			_att_sp.pitch_body = -_manual.x * _parameters.man_pitch_max_rad;
@@ -2050,8 +2052,10 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
 	} else if (_control_mode_current ==  FW_POSCTRL_MODE_AUTO &&
 		   pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_IDLE) {
 		_att_sp.thrust = 0.0f;
+
 	} else if (_control_mode_current ==  FW_POSCTRL_MODE_OTHER) {
 		_att_sp.thrust = math::min(_att_sp.thrust, _parameters.throttle_max);
+
 	} else {
 		/* Copy thrust and pitch values from tecs */
 		if (_vehicle_land_detected.landed) {
@@ -2071,13 +2075,13 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
 
 	// auto runway takeoff
 	use_tecs_pitch &= !(_control_mode_current ==  FW_POSCTRL_MODE_AUTO &&
-		      pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_TAKEOFF &&
-		       (_launch_detection_state == LAUNCHDETECTION_RES_NONE || _runway_takeoff.runwayTakeoffEnabled()));
+			    pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_TAKEOFF &&
+			    (_launch_detection_state == LAUNCHDETECTION_RES_NONE || _runway_takeoff.runwayTakeoffEnabled()));
 
 
 	// flaring during landing
 	use_tecs_pitch &= !(pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_LAND &&
-		       _land_noreturn_vertical);
+			    _land_noreturn_vertical);
 
 	// manual attitude control
 	use_tecs_pitch &=  !(_control_mode_current == FW_POSCTRL_MODE_OTHER);

From 9f7f6e4d3d8c883dc808f868d7bf9334125a258b Mon Sep 17 00:00:00 2001
From: Andreas Antener <antener_a@gmx.ch>
Date: Mon, 18 Jul 2016 23:29:33 +0200
Subject: [PATCH 1072/1230] removed roll lock for altitude hold

---
 src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp | 7 +------
 1 file changed, 1 insertion(+), 6 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 c6344b02c2..3ca07bee83 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
@@ -1991,12 +1991,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
 					   ground_speed,
 					   tecs_status_s::TECS_MODE_NORMAL);
 
-		if (fabsf(_manual.y) > 0.01f) {
-			_att_sp.roll_body = _manual.y * _parameters.man_roll_max_rad;
-
-		} else {
-			_att_sp.roll_body = 0.0f;
-		}
+		_att_sp.roll_body = _manual.y * _parameters.man_roll_max_rad;
 
 	} else {
 		_control_mode_current = FW_POSCTRL_MODE_OTHER;

From 0a997577f5e843b0d12ab09327bb19b7ec0241e1 Mon Sep 17 00:00:00 2001
From: Andreas Antener <antener_a@gmx.ch>
Date: Mon, 18 Jul 2016 23:46:46 +0200
Subject: [PATCH 1073/1230] allow manual yaw in all manual modes and also use
 it as threshold to snap into heading lock

---
 src/modules/fw_att_control/fw_att_control_main.cpp       | 3 ++-
 src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp | 5 +++--
 2 files changed, 5 insertions(+), 3 deletions(-)

diff --git a/src/modules/fw_att_control/fw_att_control_main.cpp b/src/modules/fw_att_control/fw_att_control_main.cpp
index 199b3189cb..8c6e70f83b 100644
--- a/src/modules/fw_att_control/fw_att_control_main.cpp
+++ b/src/modules/fw_att_control/fw_att_control_main.cpp
@@ -980,7 +980,8 @@ FixedwingAttitudeControl::task_main()
 				yaw_sp = _att_sp.yaw_body;
 				throttle_sp = _att_sp.thrust;
 
-				if (!_vcontrol_mode.flag_control_velocity_enabled) {
+				/* allow manual yaw in manual modes */
+				if (_vcontrol_mode.flag_control_manual_enabled) {
 					yaw_manual = _manual.r;
 				}
 
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 3ca07bee83..6a2e1a4576 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
@@ -108,7 +108,7 @@ static int	_control_task = -1;			/**< task handle for sensor task */
 #define HDG_HOLD_REACHED_DIST 		1000.0f 	// distance (plane to waypoint in front) at which waypoints are reset in heading hold mode
 #define HDG_HOLD_SET_BACK_DIST 		100.0f 		// distance by which previous waypoint is set behind the plane
 #define HDG_HOLD_YAWRATE_THRESH 	0.15f 		// max yawrate at which plane locks yaw for heading hold mode
-#define HDG_HOLD_MAN_INPUT_THRESH 	0.01f 		// max manual roll input from user which does not change the locked heading
+#define HDG_HOLD_MAN_INPUT_THRESH 	0.01f 		// max manual roll/yaw input from user which does not change the locked heading
 #define T_ALT_TIMEOUT 				1 			// time after which we abort landing if terrain estimate is not valid
 #define THROTTLE_THRESH 0.05f 	///< max throttle from user which will not lead to motors spinning up in altitude controlled modes
 #define MANUAL_THROTTLE_CLIMBOUT_THRESH 0.85f	///< a throttle / pitch input above this value leads to the system switching to climbout mode
@@ -1882,8 +1882,9 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
 					   tecs_status_s::TECS_MODE_NORMAL);
 
 		/* heading control */
+		if (fabsf(_manual.y) < HDG_HOLD_MAN_INPUT_THRESH &&
+			fabs(_manual.r) < HDG_HOLD_MAN_INPUT_THRESH) {
 
-		if (fabsf(_manual.y) < HDG_HOLD_MAN_INPUT_THRESH) {
 			/* heading / roll is zero, lock onto current heading */
 			if (fabsf(_ctrl_state.yaw_rate) < HDG_HOLD_YAWRATE_THRESH && !_yaw_lock_engaged) {
 				// little yaw movement, lock to current heading

From 6ff65cd8b2ade1c45f76d4c519cc12ee6a3d70da Mon Sep 17 00:00:00 2001
From: Andreas Antener <antener_a@gmx.ch>
Date: Tue, 19 Jul 2016 00:08:01 +0200
Subject: [PATCH 1074/1230] reset attitude setpoint where necessary

---
 .../fw_pos_control_l1_main.cpp                | 22 +++++++++++++++----
 1 file changed, 18 insertions(+), 4 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 6a2e1a4576..181ec6383e 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
@@ -1838,6 +1838,11 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
 			_hdg_hold_yaw = _yaw;
 			_hdg_hold_enabled = false; // this makes sure the waypoints are reset below
 			_yaw_lock_engaged = false;
+
+			/* reset setpoints from other modes (auto) otherwise we won't
+			 * level out without new manual input */
+			_att_sp.roll_body = _manual.y * _parameters.man_roll_max_rad;
+			_att_sp.yaw_body = 0;
 		}
 
 		/* Reset integrators if switching to this mode from a other mode in which posctl was not active */
@@ -1941,6 +1946,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
 			_hdg_hold_enabled = false;
 			_yaw_lock_engaged = false;
 			_att_sp.roll_body = _manual.y * _parameters.man_roll_max_rad;
+			_att_sp.yaw_body = 0;
 		}
 
 	} else if (_control_mode.flag_control_altitude_enabled) {
@@ -1993,19 +1999,27 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
 					   tecs_status_s::TECS_MODE_NORMAL);
 
 		_att_sp.roll_body = _manual.y * _parameters.man_roll_max_rad;
+		_att_sp.yaw_body = 0;
 
 	} else {
 		_control_mode_current = FW_POSCTRL_MODE_OTHER;
 
-		/** MANUAL FLIGHT **/
-		setpoint = false;
-
 		if (_control_mode.flag_control_attitude_enabled) {
+			/** STABILIZED FLIGHT **/
 			_att_sp.roll_body = _manual.y * _parameters.man_roll_max_rad;
 			_att_sp.pitch_body = -_manual.x * _parameters.man_pitch_max_rad;
 			_att_sp.yaw_body = 0.0f;
 			_att_sp.thrust = _manual.z;
-			setpoint = true;
+
+		} else {
+			/** MANUAL FLIGHT **/
+			_att_sp.roll_body = 0.0f;
+			_att_sp.pitch_body = 0.0f;
+			_att_sp.yaw_body = 0.0f;
+			_att_sp.thrust = 0.0f;
+
+			/* do not publish the setpoint */
+			setpoint = false;
 		}
 
 		// reset hold altitude

From 89a2f5057f32e6cc12fe39e5e0cacee0bae11778 Mon Sep 17 00:00:00 2001
From: Andreas Antener <antener_a@gmx.ch>
Date: Tue, 19 Jul 2016 00:15:30 +0200
Subject: [PATCH 1075/1230] fixed code style

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

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 181ec6383e..7bb9d4a3fc 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
@@ -1888,7 +1888,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
 
 		/* heading control */
 		if (fabsf(_manual.y) < HDG_HOLD_MAN_INPUT_THRESH &&
-			fabs(_manual.r) < HDG_HOLD_MAN_INPUT_THRESH) {
+		    fabs(_manual.r) < HDG_HOLD_MAN_INPUT_THRESH) {
 
 			/* heading / roll is zero, lock onto current heading */
 			if (fabsf(_ctrl_state.yaw_rate) < HDG_HOLD_YAWRATE_THRESH && !_yaw_lock_engaged) {

From 254358ef86f651ead3922a2ceb23c0f7fa65aac0 Mon Sep 17 00:00:00 2001
From: Andreas Antener <antener_a@gmx.ch>
Date: Tue, 19 Jul 2016 07:39:35 +0200
Subject: [PATCH 1076/1230] fixed implicit conversion

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

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 7bb9d4a3fc..98d67fd365 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
@@ -1888,7 +1888,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
 
 		/* heading control */
 		if (fabsf(_manual.y) < HDG_HOLD_MAN_INPUT_THRESH &&
-		    fabs(_manual.r) < HDG_HOLD_MAN_INPUT_THRESH) {
+		    fabsf(_manual.r) < HDG_HOLD_MAN_INPUT_THRESH) {
 
 			/* heading / roll is zero, lock onto current heading */
 			if (fabsf(_ctrl_state.yaw_rate) < HDG_HOLD_YAWRATE_THRESH && !_yaw_lock_engaged) {

From f0698805044a7cca7039056b4862500768c1cd3b Mon Sep 17 00:00:00 2001
From: Andreas Antener <antener_a@gmx.ch>
Date: Tue, 19 Jul 2016 22:37:05 +0200
Subject: [PATCH 1077/1230] VTOL: don't wait on TECS in FW state if TECS is not
 running

---
 src/modules/vtol_att_control/vtol_type.cpp | 6 ++++--
 1 file changed, 4 insertions(+), 2 deletions(-)

diff --git a/src/modules/vtol_att_control/vtol_type.cpp b/src/modules/vtol_att_control/vtol_type.cpp
index 83e331a581..ae3ed92ef5 100644
--- a/src/modules/vtol_att_control/vtol_type.cpp
+++ b/src/modules/vtol_att_control/vtol_type.cpp
@@ -173,8 +173,10 @@ void VtolType::update_fw_state()
 		_tecs_running_ts = hrt_absolute_time();
 	}
 
-	// tecs didn't publish yet or the position controller didn't publish yet AFTER tecs
-	if (!_tecs_running || (_tecs_running && _fw_virtual_att_sp->timestamp <= _tecs_running_ts)) {
+	// TECS didn't publish yet or the position controller didn't publish yet AFTER tecs
+	// only wait on TECS we're in a mode where it is actually running
+	if ((!_tecs_running || (_tecs_running && _fw_virtual_att_sp->timestamp <= _tecs_running_ts))
+	    && _v_control_mode->flag_control_altitude_enabled) {
 		waiting_on_tecs();
 	}
 

From aea7bd5b471cd64ddef843fe0c912f6b0e2eb679 Mon Sep 17 00:00:00 2001
From: Roman <bapstroman@gmail.com>
Date: Wed, 20 Jul 2016 13:21:18 +0200
Subject: [PATCH 1078/1230] fw_attitude_control: calculate attitude setpoint
 for STAB mode

- attitude setpoint generation for stabilized mode was shifted back
to the fw attitude controller. since the fw position controller is polling
on global position attitude setpoints were not generated when global
position was not published.

Signed-off-by: Roman <bapstroman@gmail.com>
---
 .../fw_att_control/fw_att_control_main.cpp    | 13 ++++++++++
 .../fw_pos_control_l1_main.cpp                | 24 +++++--------------
 2 files changed, 19 insertions(+), 18 deletions(-)

diff --git a/src/modules/fw_att_control/fw_att_control_main.cpp b/src/modules/fw_att_control/fw_att_control_main.cpp
index 8c6e70f83b..83f50050fb 100644
--- a/src/modules/fw_att_control/fw_att_control_main.cpp
+++ b/src/modules/fw_att_control/fw_att_control_main.cpp
@@ -975,6 +975,19 @@ FixedwingAttitudeControl::task_main()
 				float yaw_manual = 0.0f;
 				float throttle_sp = 0.0f;
 
+				// in STABILIZED mode we need to generate the attitude setpoint
+				// from manual user inputs
+				if (!_vcontrol_mode.flag_control_climb_rate_enabled) {
+					_att_sp.roll_body = _manual.y * _parameters.man_roll_max + _parameters.rollsp_offset_rad;
+					_att_sp.roll_body = math::constrain(_att_sp.roll_body, -_parameters.man_roll_max, _parameters.man_roll_max);
+					_att_sp.pitch_body = -_manual.x * _parameters.man_pitch_max + _parameters.pitchsp_offset_rad;
+					_att_sp.pitch_body = math::constrain(_att_sp.pitch_body, -_parameters.man_pitch_max, _parameters.man_pitch_max);
+					_att_sp.yaw_body = 0.0f;
+					_att_sp.thrust = _manual.z;
+					int instance;
+					orb_publish_auto(ORB_ID(vehicle_attitude_setpoint), &_attitude_sp_pub, &_att_sp, &instance, ORB_PRIO_DEFAULT);
+				}
+
 				roll_sp = _att_sp.roll_body;
 				pitch_sp = _att_sp.pitch_body;
 				yaw_sp = _att_sp.yaw_body;
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 98d67fd365..31c6a12877 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
@@ -1942,7 +1942,10 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
 				}
 			}
 
-		} else {
+		}
+
+		if (!_yaw_lock_engaged || fabsf(_manual.y) > HDG_HOLD_MAN_INPUT_THRESH ||
+		    fabsf(_manual.r) > HDG_HOLD_MAN_INPUT_THRESH) {
 			_hdg_hold_enabled = false;
 			_yaw_lock_engaged = false;
 			_att_sp.roll_body = _manual.y * _parameters.man_roll_max_rad;
@@ -2004,23 +2007,8 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
 	} else {
 		_control_mode_current = FW_POSCTRL_MODE_OTHER;
 
-		if (_control_mode.flag_control_attitude_enabled) {
-			/** STABILIZED FLIGHT **/
-			_att_sp.roll_body = _manual.y * _parameters.man_roll_max_rad;
-			_att_sp.pitch_body = -_manual.x * _parameters.man_pitch_max_rad;
-			_att_sp.yaw_body = 0.0f;
-			_att_sp.thrust = _manual.z;
-
-		} else {
-			/** MANUAL FLIGHT **/
-			_att_sp.roll_body = 0.0f;
-			_att_sp.pitch_body = 0.0f;
-			_att_sp.yaw_body = 0.0f;
-			_att_sp.thrust = 0.0f;
-
-			/* do not publish the setpoint */
-			setpoint = false;
-		}
+		/* do not publish the setpoint */
+		setpoint = false;
 
 		// reset hold altitude
 		_hold_alt = _global_pos.alt;

From 50175ca7eafd4b1fa46771cde96cb282dc9b7e32 Mon Sep 17 00:00:00 2001
From: Andreas Antener <antener_a@gmx.ch>
Date: Fri, 22 Jul 2016 12:32:03 +0200
Subject: [PATCH 1079/1230] manual condition includes threshold values

---
 src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp | 4 ++--
 1 file changed, 2 insertions(+), 2 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 31c6a12877..dc6e75f63f 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
@@ -1944,8 +1944,8 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
 
 		}
 
-		if (!_yaw_lock_engaged || fabsf(_manual.y) > HDG_HOLD_MAN_INPUT_THRESH ||
-		    fabsf(_manual.r) > HDG_HOLD_MAN_INPUT_THRESH) {
+		if (!_yaw_lock_engaged || fabsf(_manual.y) >= HDG_HOLD_MAN_INPUT_THRESH ||
+		    fabsf(_manual.r) >= HDG_HOLD_MAN_INPUT_THRESH) {
 			_hdg_hold_enabled = false;
 			_yaw_lock_engaged = false;
 			_att_sp.roll_body = _manual.y * _parameters.man_roll_max_rad;

From c3b58822a813e3fa1718707122604a0b406991e2 Mon Sep 17 00:00:00 2001
From: Gus Grubba <mavlink@grubba.com>
Date: Mon, 25 Jul 2016 08:15:23 -0400
Subject: [PATCH 1080/1230] Fixes to log download (#5133)

* Fixes to log download

* Fixing indentation
---
 src/modules/mavlink/mavlink_log_handler.cpp | 161 +++++++++++---------
 src/modules/mavlink/mavlink_log_handler.h   |  59 +++----
 2 files changed, 123 insertions(+), 97 deletions(-)

diff --git a/src/modules/mavlink/mavlink_log_handler.cpp b/src/modules/mavlink/mavlink_log_handler.cpp
index ab7c7c048e..721a22c757 100644
--- a/src/modules/mavlink/mavlink_log_handler.cpp
+++ b/src/modules/mavlink/mavlink_log_handler.cpp
@@ -40,17 +40,18 @@
 #include <time.h>
 
 #define MOUNTPOINT PX4_ROOTFSDIR "/fs/microsd"
- 
+
+static const char *kSDRoot     = MOUNTPOINT "/";
 static const char *kLogRoot    = MOUNTPOINT "/log";
 static const char *kLogData    = MOUNTPOINT "/logdata.txt";
 static const char *kTmpData    = MOUNTPOINT "/$log$.txt";
 
 #ifdef __PX4_NUTTX
-    #define PX4LOG_REGULAR_FILE DTYPE_FILE
-    #define PX4LOG_DIRECTORY 	DTYPE_DIRECTORY
+#define PX4LOG_REGULAR_FILE DTYPE_FILE
+#define PX4LOG_DIRECTORY    DTYPE_DIRECTORY
 #else
-    #define PX4LOG_REGULAR_FILE DT_REG
-    #define PX4LOG_DIRECTORY 	DT_DIR
+#define PX4LOG_REGULAR_FILE DT_REG
+#define PX4LOG_DIRECTORY    DT_DIR
 #endif
 
 //#define MAVLINK_LOG_HANDLER_VERBOSE
@@ -77,13 +78,13 @@ stat_file(const char* file, time_t* date = 0, uint32_t* size = 0) {
 MavlinkLogHandler *
 MavlinkLogHandler::new_instance(Mavlink *mavlink)
 {
-    return new MavlinkLogHandler(mavlink);
+	return new MavlinkLogHandler(mavlink);
 }
 
 //-------------------------------------------------------------------
 MavlinkLogHandler::MavlinkLogHandler(Mavlink *mavlink)
-    : MavlinkStream(mavlink)
-    , _pLogHandlerHelper(0)
+	: MavlinkStream(mavlink)
+	, _pLogHandlerHelper(0)
 {
 
 }
@@ -112,14 +113,14 @@ MavlinkLogHandler::handle_message(const mavlink_message_t *msg)
 const char*
 MavlinkLogHandler::get_name(void) const
 {
-    return "MAVLINK_LOG_HANDLER";
+	return "MAVLINK_LOG_HANDLER";
 }
 
 //-------------------------------------------------------------------
 uint8_t
 MavlinkLogHandler::get_id(void)
 {
-    return MAVLINK_MSG_ID_LOG_ENTRY;
+	return MAVLINK_MSG_ID_LOG_ENTRY;
 }
 
 //-------------------------------------------------------------------
@@ -142,15 +143,17 @@ MavlinkLogHandler::get_size(void)
 void
 MavlinkLogHandler::send(const hrt_abstime /*t*/)
 {
-	//-- Send log entry stream packets until buffer is full
-	while (_pLogHandlerHelper && _pLogHandlerHelper->current_status == LogListHelper::LOG_HANDLER_LISTING && _mavlink->get_free_tx_buf() > get_size()) {
-		_log_send_listing();
-	};
-	//-- An arbitrary count of max log data packets in one go
-	int count = 100;
-	while (_pLogHandlerHelper && _pLogHandlerHelper->current_status == LogListHelper::LOG_HANDLER_SENDING_DATA && _mavlink->get_free_tx_buf() > get_size() && --count) {
-		_log_send_data();
-	};
+	//-- An arbitrary count of max bytes in one go (one of the two below but never both)
+	#define MAX_BYTES_SEND 64 * 1024
+	size_t count = 0;
+	//-- Log Entries
+	while (_pLogHandlerHelper && _pLogHandlerHelper->current_status == LogListHelper::LOG_HANDLER_LISTING && _mavlink->get_free_tx_buf() > get_size() && count < MAX_BYTES_SEND) {
+		count += _log_send_listing();
+	}
+	//-- Log Data
+	while (_pLogHandlerHelper && _pLogHandlerHelper->current_status == LogListHelper::LOG_HANDLER_SENDING_DATA && _mavlink->get_free_tx_buf() > get_size() && count < MAX_BYTES_SEND) {
+		count += _log_send_data();
+	}
 }
 
 //-------------------------------------------------------------------
@@ -172,16 +175,16 @@ MavlinkLogHandler::_log_request_list(const mavlink_message_t *msg)
 		//-- Prepare new request
 		_pLogHandlerHelper = new LogListHelper;
 	}
-        if (_pLogHandlerHelper->log_count)
-        {
-        	//-- Define (and clamp) range
+	if (_pLogHandlerHelper->log_count)
+	{
+		//-- Define (and clamp) range
 		_pLogHandlerHelper->next_entry = request.start < _pLogHandlerHelper->log_count ? request.start : _pLogHandlerHelper->log_count - 1;
 		_pLogHandlerHelper->last_entry = request.end   < _pLogHandlerHelper->log_count ? request.end   : _pLogHandlerHelper->log_count - 1;
-        }
+	}
 	PX4LOG_WARN("\nMavlinkLogHandler::_log_request_list: start: %u last: %u count: %u\n",
-		_pLogHandlerHelper->next_entry,
-		_pLogHandlerHelper->last_entry,
-		_pLogHandlerHelper->log_count);
+				_pLogHandlerHelper->next_entry,
+				_pLogHandlerHelper->last_entry,
+				_pLogHandlerHelper->log_count);
 	//-- Enable streaming
 	_pLogHandlerHelper->current_status = LogListHelper::LOG_HANDLER_LISTING;
 }
@@ -203,23 +206,23 @@ MavlinkLogHandler::_log_request_data(const mavlink_message_t *msg)
 		return;
 	}
 	//-- If we were sending log entries, stop it
-        _pLogHandlerHelper->current_status = LogListHelper::LOG_HANDLER_IDLE;
-        //-- Init send log dataset
-        _pLogHandlerHelper->current_log_filename[0] = 0;
-        _pLogHandlerHelper->current_log_index = request.id;
+	_pLogHandlerHelper->current_status = LogListHelper::LOG_HANDLER_IDLE;
+	//-- Init send log dataset
+	_pLogHandlerHelper->current_log_filename[0] = 0;
+	_pLogHandlerHelper->current_log_index = request.id;
 	uint32_t time_utc = 0;
 	_pLogHandlerHelper->get_entry(_pLogHandlerHelper->current_log_index, _pLogHandlerHelper->current_log_size, time_utc, _pLogHandlerHelper->current_log_filename);
-        _pLogHandlerHelper->current_log_data_offset = request.ofs;
-        if (_pLogHandlerHelper->current_log_data_offset >= _pLogHandlerHelper->current_log_size) {
+	_pLogHandlerHelper->current_log_data_offset = request.ofs;
+	if (_pLogHandlerHelper->current_log_data_offset >= _pLogHandlerHelper->current_log_size) {
 		_pLogHandlerHelper->current_log_data_remaining = 0;
-        } else {
+	} else {
 		_pLogHandlerHelper->current_log_data_remaining = _pLogHandlerHelper->current_log_size - request.ofs;
-        }
-        if (_pLogHandlerHelper->current_log_data_remaining > request.count) {
+	}
+	if (_pLogHandlerHelper->current_log_data_remaining > request.count) {
 		_pLogHandlerHelper->current_log_data_remaining = request.count;
-        }
+	}
 	//-- Enable streaming
-        _pLogHandlerHelper->current_status = LogListHelper::LOG_HANDLER_SENDING_DATA;
+	_pLogHandlerHelper->current_status = LogListHelper::LOG_HANDLER_SENDING_DATA;
 }
 
 //-------------------------------------------------------------------
@@ -234,7 +237,29 @@ MavlinkLogHandler::_log_request_erase(const mavlink_message_t* /*msg*/)
 		delete _pLogHandlerHelper;
 		_pLogHandlerHelper = 0;
 	}
+	//-- Delete all logs
 	LogListHelper::delete_all(kLogRoot);
+	//-- Now delete all "msgs_*" from root
+	DIR* dp = opendir(kSDRoot);
+	if (dp) {
+		struct dirent entry, *result = nullptr;
+		while (readdir_r(dp, &entry, &result) == 0) {
+			// no more entries?
+			if (!result) {
+				break;
+			}
+			if (entry.d_type == PX4LOG_REGULAR_FILE) {
+				if(!memcmp(entry.d_name, "msgs_", 5)) {
+					char msg_path[128];
+					snprintf(msg_path, sizeof(msg_path), "%s%s", kSDRoot, entry.d_name);
+					if(unlink(msg_path)) {
+						PX4LOG_WARN("MavlinkLogHandler::_log_request_erase Error deleting %s\n", msg_path);
+					}
+				}
+			}
+		}
+		closedir(dp);
+	}
 }
 
 //-------------------------------------------------------------------
@@ -249,55 +274,55 @@ MavlinkLogHandler::_log_request_end(const mavlink_message_t* /*msg*/)
 }
 
 //-------------------------------------------------------------------
-void
+size_t
 MavlinkLogHandler::_log_send_listing()
 {
 	mavlink_log_entry_t response;
 	uint32_t size, date;
 	_pLogHandlerHelper->get_entry(_pLogHandlerHelper->next_entry, size, date);
-	response.size 		= size;
-	response.time_utc 	= date;
-	response.id 		= _pLogHandlerHelper->next_entry;
-	response.num_logs	= _pLogHandlerHelper->log_count;
-	response.last_log_num	= _pLogHandlerHelper->last_entry;
+	response.size         = size;
+	response.time_utc     = date;
+	response.id           = _pLogHandlerHelper->next_entry;
+	response.num_logs     = _pLogHandlerHelper->log_count;
+	response.last_log_num = _pLogHandlerHelper->last_entry;
 	mavlink_msg_log_entry_send_struct(_mavlink->get_channel(), &response);
-        //-- If we're done listing, flag it.
-        if (_pLogHandlerHelper->next_entry == _pLogHandlerHelper->last_entry) {
+	//-- If we're done listing, flag it.
+	if (_pLogHandlerHelper->next_entry == _pLogHandlerHelper->last_entry) {
 		_pLogHandlerHelper->current_status = LogListHelper::LOG_HANDLER_IDLE;
-        } else {
+	} else {
 		_pLogHandlerHelper->next_entry++;
-        }
+	}
 	PX4LOG_WARN("MavlinkLogHandler::_log_send_listing id: %u count: %u last: %u size: %u date: %u status: %d\n",
-		response.id,
-		response.num_logs,
-		response.last_log_num,
-		response.size,
-		response.time_utc,
-		_pLogHandlerHelper->current_status);
+				response.id,
+				response.num_logs,
+				response.last_log_num,
+				response.size,
+				response.time_utc,
+				_pLogHandlerHelper->current_status);
+	return sizeof(response);
 }
 
 //-------------------------------------------------------------------
-void
+size_t
 MavlinkLogHandler::_log_send_data()
 {
 	mavlink_log_data_t response;
+	memset(&response, 0, sizeof(response));
 	uint32_t len = _pLogHandlerHelper->current_log_data_remaining;
 	if (len > sizeof(response.data)) {
 		len = sizeof(response.data);
 	}
 	size_t read_size = _pLogHandlerHelper->get_log_data(len, response.data);
-	if (read_size < sizeof(response.data)) {
-		memset(&response.data[read_size], 0, sizeof(response.data) - read_size);
-	}
-	response.ofs   = _pLogHandlerHelper->current_log_data_offset;
-	response.id    = _pLogHandlerHelper->current_log_index;
-	response.count = read_size;
+	response.ofs     = _pLogHandlerHelper->current_log_data_offset;
+	response.id      = _pLogHandlerHelper->current_log_index;
+	response.count   = read_size;
 	mavlink_msg_log_data_send_struct(_mavlink->get_channel(), &response);
 	_pLogHandlerHelper->current_log_data_offset    += read_size;
 	_pLogHandlerHelper->current_log_data_remaining -= read_size;
 	if (read_size < sizeof(response.data) || _pLogHandlerHelper->current_log_data_remaining == 0) {
 		_pLogHandlerHelper->current_status = LogListHelper::LOG_HANDLER_IDLE;
 	}
+	return sizeof(response);
 }
 
 //-------------------------------------------------------------------
@@ -358,7 +383,7 @@ LogListHelper::get_entry(int idx, uint32_t& size, uint32_t& date, char* filename
 size_t
 LogListHelper::get_log_data(uint8_t len, uint8_t* buffer)
 {
-	if(!current_log_filename[0]) 
+	if(!current_log_filename[0])
 		return 0;
 	FILE* f = fopen(current_log_filename, "r");
 	if (!f) {
@@ -380,7 +405,7 @@ void
 LogListHelper::_init()
 {
 	/*
-	
+
 		When this helper is created, it scans the log directory
 		and collects all log files found into one file for easy,
 		subsequent access.
@@ -389,14 +414,14 @@ LogListHelper::_init()
 	current_log_filename[0] = 0;
 	// Remove old log data file (if any)
 	unlink(kLogData);
-    	// Open log directory
+	// Open log directory
 	DIR *dp = opendir(kLogRoot);
 	if (dp == nullptr) {
 		// No log directory. Nothing to do.
 		return;
 	}
-    	// Create work file
-    	FILE* f = ::fopen(kTmpData, "w");
+	// Create work file
+	FILE* f = ::fopen(kTmpData, "w");
 	if (!f) {
 		PX4LOG_WARN("MavlinkLogHandler::init Error creating %s\n", kTmpData);
 		closedir(dp);
@@ -414,7 +439,7 @@ LogListHelper::_init()
 			time_t tt;
 			char log_path[128];
 			snprintf(log_path, sizeof(log_path), "%s/%s", kLogRoot, entry.d_name);
-		        if (_get_session_date(log_path, entry.d_name, tt)) {
+			if (_get_session_date(log_path, entry.d_name, tt)) {
 				_scan_logs(f, log_path, tt);
 			}
 		}
@@ -519,8 +544,8 @@ LogListHelper::_get_log_time_size(const char* path, const char* file, time_t& da
 void
 LogListHelper::delete_all(const char* dir)
 {
-    	//-- Open log directory
-	DIR *dp = opendir(dir);
+	//-- Open log directory
+	DIR* dp = opendir(dir);
 	if (dp == nullptr) {
 		return;
 	}
diff --git a/src/modules/mavlink/mavlink_log_handler.h b/src/modules/mavlink/mavlink_log_handler.h
index 8742d8e2f4..8f42404672 100644
--- a/src/modules/mavlink/mavlink_log_handler.h
+++ b/src/modules/mavlink/mavlink_log_handler.h
@@ -54,11 +54,11 @@ public:
 
 public:
 	static void delete_all(const char* dir);
-	
+
 public:
 
-	bool 	get_entry		(int idx, uint32_t& size, uint32_t& date, char* filename = 0);
-	size_t 	get_log_data		(uint8_t len, uint8_t* buffer);
+	bool        get_entry           (int idx, uint32_t& size, uint32_t& date, char* filename = 0);
+	size_t      get_log_data        (uint8_t len, uint8_t* buffer);
 
 	enum {
 		LOG_HANDLER_IDLE,
@@ -66,22 +66,22 @@ public:
 		LOG_HANDLER_SENDING_DATA
 	};
 
-	int 		next_entry;
-	int 		last_entry;
-	int 		log_count;
+	int         next_entry;
+	int         last_entry;
+	int         log_count;
 
-	int		current_status;
-	uint16_t	current_log_index;
-	uint32_t 	current_log_size;
-	uint32_t	current_log_data_offset;
-	uint32_t	current_log_data_remaining;
-	char		current_log_filename[128];
+	int         current_status;
+	uint16_t    current_log_index;
+	uint32_t    current_log_size;
+	uint32_t    current_log_data_offset;
+	uint32_t    current_log_data_remaining;
+	char        current_log_filename[128];
 
 private:
-	void 	_init			();
-	bool 	_get_session_date	(const char* path, const char* dir, time_t& date);
-	void	_scan_logs		(FILE* f, const char* dir, time_t& date);
-	bool 	_get_log_time_size	(const char* path, const char* file, time_t& date, uint32_t& size);
+	void        _init                   ();
+	bool        _get_session_date       (const char* path, const char* dir, time_t& date);
+	void        _scan_logs              (FILE* f, const char* dir, time_t& date);
+	bool        _get_log_time_size      (const char* path, const char* file, time_t& date, uint32_t& size);
 };
 
 // MAVLink LOG_* Message Handler
@@ -93,24 +93,25 @@ public:
 	static MavlinkLogHandler *new_instance(Mavlink *mavlink);
 
 	// Handle possible LOG message
-	void handle_message		(const mavlink_message_t *msg);
+	void handle_message             (const mavlink_message_t *msg);
 
 	// Overrides from MavlinkStream
-	const char*	get_name	(void) const;
-	uint8_t 	get_id		(void);
-	unsigned	get_size	(void);
-	void 		send		(const hrt_abstime t);
+	const char*     get_name        (void) const;
+	uint8_t         get_id          (void);
+	unsigned        get_size        (void);
+	void            send            (const hrt_abstime t);
 
 private:
-	void _log_message	(const mavlink_message_t *msg);
-	void _log_request_list	(const mavlink_message_t *msg);
-	void _log_request_data	(const mavlink_message_t *msg);
-	void _log_request_erase	(const mavlink_message_t *msg);
-	void _log_request_end	(const mavlink_message_t *msg);
-	void _log_send_listing	();
-	void _log_send_data	();
+	void _log_message               (const mavlink_message_t *msg);
+	void _log_request_list          (const mavlink_message_t *msg);
+	void _log_request_data          (const mavlink_message_t *msg);
+	void _log_request_erase         (const mavlink_message_t *msg);
+	void _log_request_end           (const mavlink_message_t *msg);
+
+	size_t _log_send_listing();
+	size_t _log_send_data   ();
 
 private:
-	LogListHelper	*_pLogHandlerHelper;
+	LogListHelper    *_pLogHandlerHelper;
 
 };

From 83feb83fe2240c441b2cdbe8f39a0362705490c1 Mon Sep 17 00:00:00 2001
From: Julian Oes <julian@oes.ch>
Date: Fri, 22 Jul 2016 21:36:59 +0200
Subject: [PATCH 1081/1230] cmake: don't build load_mon for POSIX/QURT

---
 cmake/configs/posix_bebop_default.cmake               | 3 +--
 cmake/configs/posix_eagle_hil.cmake                   | 1 -
 cmake/configs/posix_eagle_legacy_driver_default.cmake | 1 -
 cmake/configs/posix_rpi_common.cmake                  | 1 -
 cmake/configs/posix_sdflight_default.cmake            | 1 -
 cmake/configs/posix_sitl_default.cmake                | 1 -
 cmake/configs/posix_sitl_test.cmake                   | 1 -
 cmake/configs/qurt_eagle_hil.cmake                    | 1 -
 cmake/configs/qurt_eagle_legacy_driver_default.cmake  | 1 -
 cmake/configs/qurt_eagle_travis.cmake                 | 1 -
 10 files changed, 1 insertion(+), 11 deletions(-)

diff --git a/cmake/configs/posix_bebop_default.cmake b/cmake/configs/posix_bebop_default.cmake
index 25a29d4df3..0393e2081d 100644
--- a/cmake/configs/posix_bebop_default.cmake
+++ b/cmake/configs/posix_bebop_default.cmake
@@ -51,7 +51,7 @@ set(config_module_list
 	modules/mc_att_control
 	modules/mc_pos_control
 	modules/fw_att_control
-	modules/fw_pos_control_l1	
+	modules/fw_pos_control_l1
 	modules/vtol_att_control
 
 	#
@@ -60,7 +60,6 @@ set(config_module_list
 	modules/sdlog2
 	modules/logger
 	modules/commander
-	modules/load_mon
 	modules/param
 	modules/systemlib
 	modules/systemlib/mixer
diff --git a/cmake/configs/posix_eagle_hil.cmake b/cmake/configs/posix_eagle_hil.cmake
index 63ca567be7..909bdeecb1 100644
--- a/cmake/configs/posix_eagle_hil.cmake
+++ b/cmake/configs/posix_eagle_hil.cmake
@@ -30,7 +30,6 @@ set(config_module_list
 	modules/logger
 	modules/simulator
 	modules/commander
-	modules/load_mon
 
 	lib/mathlib
 	lib/mathlib/math/filter
diff --git a/cmake/configs/posix_eagle_legacy_driver_default.cmake b/cmake/configs/posix_eagle_legacy_driver_default.cmake
index c3cbd6e475..ec172e7602 100644
--- a/cmake/configs/posix_eagle_legacy_driver_default.cmake
+++ b/cmake/configs/posix_eagle_legacy_driver_default.cmake
@@ -49,7 +49,6 @@ set(config_module_list
 	modules/simulator
 	modules/commander
 	modules/navigator
-	modules/load_mon
 
 	lib/controllib
 	lib/mathlib
diff --git a/cmake/configs/posix_rpi_common.cmake b/cmake/configs/posix_rpi_common.cmake
index a22565640b..b7f1dd0aa5 100644
--- a/cmake/configs/posix_rpi_common.cmake
+++ b/cmake/configs/posix_rpi_common.cmake
@@ -60,7 +60,6 @@ set(config_module_list
 	modules/sdlog2
 	modules/logger
 	modules/commander
-	modules/load_mon
 	modules/param
 	modules/systemlib
 	modules/systemlib/mixer
diff --git a/cmake/configs/posix_sdflight_default.cmake b/cmake/configs/posix_sdflight_default.cmake
index b6c686c9d4..c43e51974c 100644
--- a/cmake/configs/posix_sdflight_default.cmake
+++ b/cmake/configs/posix_sdflight_default.cmake
@@ -42,7 +42,6 @@ set(config_module_list
 	modules/simulator
 	modules/commander
 	modules/navigator
-	modules/load_mon
 
 	lib/controllib
 	lib/mathlib
diff --git a/cmake/configs/posix_sitl_default.cmake b/cmake/configs/posix_sitl_default.cmake
index 07b77c8478..2de3ecedf8 100644
--- a/cmake/configs/posix_sitl_default.cmake
+++ b/cmake/configs/posix_sitl_default.cmake
@@ -41,7 +41,6 @@ set(config_module_list
 	modules/fw_att_control
 	modules/fw_pos_control_l1
 	modules/land_detector
-	modules/load_mon
 	modules/logger
 	modules/mavlink
 	modules/mc_att_control
diff --git a/cmake/configs/posix_sitl_test.cmake b/cmake/configs/posix_sitl_test.cmake
index 3237c7b16e..c45b2fc39e 100644
--- a/cmake/configs/posix_sitl_test.cmake
+++ b/cmake/configs/posix_sitl_test.cmake
@@ -39,7 +39,6 @@ set(config_module_list
 	modules/fw_att_control
 	modules/fw_pos_control_l1
 	modules/land_detector
-	modules/load_mon
 	modules/logger
 	modules/mavlink
 	modules/mc_att_control
diff --git a/cmake/configs/qurt_eagle_hil.cmake b/cmake/configs/qurt_eagle_hil.cmake
index f2a77068e4..469432818f 100644
--- a/cmake/configs/qurt_eagle_hil.cmake
+++ b/cmake/configs/qurt_eagle_hil.cmake
@@ -48,7 +48,6 @@ set(config_module_list
 	modules/systemlib/mixer
 	modules/uORB
 	modules/commander
-	modules/load_mon
 
 	#
 	# Libraries
diff --git a/cmake/configs/qurt_eagle_legacy_driver_default.cmake b/cmake/configs/qurt_eagle_legacy_driver_default.cmake
index 2ccd5123d5..f0fdb2dee5 100644
--- a/cmake/configs/qurt_eagle_legacy_driver_default.cmake
+++ b/cmake/configs/qurt_eagle_legacy_driver_default.cmake
@@ -57,7 +57,6 @@ set(config_module_list
 	modules/uORB
 	modules/commander
 	modules/land_detector
-	modules/load_mon
 
 	#
 	# PX4 drivers
diff --git a/cmake/configs/qurt_eagle_travis.cmake b/cmake/configs/qurt_eagle_travis.cmake
index 609ae54283..16aa788edc 100644
--- a/cmake/configs/qurt_eagle_travis.cmake
+++ b/cmake/configs/qurt_eagle_travis.cmake
@@ -56,7 +56,6 @@ set(config_module_list
 	modules/systemlib/mixer
 	modules/uORB
 	modules/commander
-	modules/load_mon
 
 	#
 	# Libraries

From b6e57e1347bc5ebf2cca1ea59a7788ab9ee47e7d Mon Sep 17 00:00:00 2001
From: h3ct0r <h3ct0r.ml@gmail.com>
Date: Fri, 22 Jul 2016 14:19:23 -0300
Subject: [PATCH 1082/1230] Added geo tagger script to geo reference any set of
 arbitraty JPG images that does not have the CAM trigger, but have a valid
 creation date

---
 Tools/sdlog2/README.txt        |   6 +-
 Tools/sdlog2/geo_tag_images.py | 375 +++++++++++++++++++++++++++++++++
 2 files changed, 380 insertions(+), 1 deletion(-)
 create mode 100644 Tools/sdlog2/geo_tag_images.py

diff --git a/Tools/sdlog2/README.txt b/Tools/sdlog2/README.txt
index abeb9a4c71..84682fea6b 100644
--- a/Tools/sdlog2/README.txt
+++ b/Tools/sdlog2/README.txt
@@ -8,6 +8,10 @@ logconv.m: This is a MATLAB script which will automatically convert and display
 
 sdlog2_dump.py: This is a Python script (compatible with v2 and v3) which converts the self-describing binary log format to a CSV file. To export a CSV file from within a shell (Windows CMD or BASH on Linux / Mac OS), run:
 
-python sdlog2_dump.py log001.bin -f "export.csv" -t "TIME" -d "," -n ""
+	python sdlog2_dump.py log001.bin -f "export.csv" -t "TIME" -d "," -n ""
+
+geo_tag_images.py: Use this script to geotag a set of images. It uses GPS time and file creation date to synchronize the images, so it needs that the images have a valid creation date.
+
+	python geo_tag_images.py --logfile=mylog.bin --input=images/ --output=tagged/
 
 Python can be downloaded from http://python.org, but is available as default on Mac OS and Linux.
\ No newline at end of file
diff --git a/Tools/sdlog2/geo_tag_images.py b/Tools/sdlog2/geo_tag_images.py
new file mode 100644
index 0000000000..c934c26e8b
--- /dev/null
+++ b/Tools/sdlog2/geo_tag_images.py
@@ -0,0 +1,375 @@
+#!/usr/bin/env python
+#
+#  Tag the images recorded during a flight with geo location extracted from
+#  a PX4 binary log file.
+#
+#  This file accepts *.jpg format images and reads position information
+#  from a *.bin file
+#
+#  Example Syntax:
+#  python geotag.py --logfile=log001.bin --input=images/ --output=imagesWithTag/ --offset=-0.4 -v
+#
+#   Author: Hector Azpurua
+#   Based on the script of Andreas Bircher
+
+import os
+import re
+import sys
+import csv
+import bisect
+import pyexiv2
+import argparse
+from lxml import etree
+import datetime, calendar
+from shutil import copyfile
+from pykml.factory import KML_ElementMaker as KML
+from pykml.factory import GX_ElementMaker as GX
+
+
+class GpsPosition(object):
+    def __init__(self, timestamp, lat, lon, alt):
+        self.timestamp = timestamp
+        self.lat = float(lat)
+        self.lon = float(lon)
+        self.alt = float(alt)
+
+
+class Main:
+    def __init__(self):
+        """
+
+        :param logfile:
+        :param input:
+        :param output:
+        :param offset:
+        :param verbose:
+        :return:
+        """
+        args = self.get_arg()
+
+        self.logfile = args['logfile']
+        self.input = args['input']
+        self.output = args['output']
+        self.klm = args['klm']
+        self.verbose = args['verbose']
+        self.offset = args['offset']
+        self.time_tresh = args['treshold']
+
+        self.tdiff_list = []
+        self.non_processed_files = []
+        self.tagged_gps = []
+        self.gps_list = self.load_gps_from_log(self.logfile, self.offset)
+        self.img_list = self.load_image_list(self.input)
+
+        if len(self.img_list) <= 0:
+            print '[ERROR] Cannot load JPG images from input folder, please check filename extensions.'
+            sys.exit(1)
+
+        if not os.path.exists(self.output):
+            os.makedirs(self.output)
+
+        if not self.output.endswith(os.path.sep):
+            self.output += os.path.sep
+
+        self.tag_images()
+
+        if self.klm and len(self.tdiff_list) > 0:
+            self.gen_klm()
+
+        if len(self.non_processed_files) > 0:
+            print '[WARNING] Some images werent processed:'
+            for elem in self.non_processed_files:
+                print '\t', elem
+
+    @staticmethod
+    def to_degree(value, loc):
+        """
+        Convert a lat or lon value to degrees/minutes/seconds
+        :param value: the latitude or longitude value
+        :param loc: could be ["S", "N"] or ["W", "E"]
+        :return:
+        """
+        if value < 0:
+            loc_value = loc[0]
+        elif value > 0:
+            loc_value = loc[1]
+        else:
+            loc_value = ""
+
+        absolute_value = abs(value)
+        deg = int(absolute_value)
+        t1 = (absolute_value-deg) * 60
+        minute = int(t1)
+        sec = round((t1 - minute) * 60, 5)
+
+        return deg, minute, sec, loc_value
+
+    @staticmethod
+    def gps_week_seconds_to_datetime(gpsweek, gpsmillis, leapmillis=0):
+        """
+        Convert GPS week and seconds to datetime object, using leap milliseconds if necessary
+        :param gpsweek:
+        :param gpsmillis:
+        :param leapmillis:
+        :return:
+        """
+        datetimeformat = "%Y-%m-%d %H:%M:%S.%f"
+        epoch = datetime.datetime.strptime("1980-01-06 00:00:00.000", datetimeformat)
+        elapsed = datetime.timedelta(days=(gpsweek * 7), milliseconds=(gpsmillis + leapmillis))
+
+        return Main.utc_to_local(epoch + elapsed)
+
+    @staticmethod
+    def utc_to_local(utc_dt):
+        """
+        Convert UTC time in local time
+        :param utc_dt:
+        :return:
+        """
+        timestamp = calendar.timegm(utc_dt.timetuple())  # use integer timestamp to avoid precision lost
+        local_dt = datetime.datetime.fromtimestamp(timestamp)
+        assert utc_dt.resolution >= datetime.timedelta(microseconds=1)
+
+        return local_dt.replace(microsecond=utc_dt.microsecond)
+
+    def gen_klm(self):
+        """
+        Generate a KML file with keypoints on the locations of the pictures, including height
+        :return:
+        """
+        style_dot = "sn_shaded_dot"
+        style_path = "red_path"
+
+        doc = KML.kml(
+            KML.Document(
+                KML.Name("GPS of the images"),
+                KML.Style(
+                    KML.IconStyle(
+                        KML.scale(0.4),
+                        KML.Icon(
+                            KML.href("http://maps.google.com/mapfiles/kml/shapes/shaded_dot.png")
+                        ),
+                    ),
+                    id=style_dot,
+                ),
+                KML.Style(
+                    KML.LineStyle(
+                        KML.color('7f0000ff'),
+                        KML.width(6),
+                        GX.labelVisibility('1'),
+                    ),
+                    id=style_path
+                )   
+            )
+        )
+
+        # create points
+        for i, gps in enumerate(self.tagged_gps):
+                ii = i + 1
+                doc.Document.append(
+                    KML.Placemark(
+                        KML.styleUrl('#{0}'.format(style_dot)),
+                        KML.Point(
+                            KML.extrude(True),
+                            KML.altitudeMode('relativeToGround'),
+                            KML.coordinates("{},{},{}".format(gps.lon, gps.lat, gps.alt))
+                        ),
+                        KML.name(str(ii)) if ii % 5 == 0 or ii == 1 else KML.name()
+                    )
+                )
+
+        # create the path
+        doc.Document.append(
+            KML.Placemark(
+                KML.styleUrl('#{0}'.format(style_path)),
+                KML.LineString(
+                    KML.altitudeMode('relativeToGround'),
+                    KML.coordinates(
+                        ' '.join(["{},{},{}".format(gps.lon, gps.lat, gps.alt) for gps in self.tagged_gps])
+                    )
+                )
+            )
+        )
+
+        s = etree.tostring(doc)
+
+        file_path = self.output + 'GoogleEarth_points.kml'
+        f = open(file_path,'w')
+        f.write(s)
+        f.close()
+
+        print '[INFO] KML file generated on:', file_path
+
+    def get_closest_datetime_index(self, datetime_list, elem):
+        """
+        Get the closest element between a list of datetime objects and a date
+        :param datetime_list:
+        :param elem:
+        :return:
+        """
+        i = bisect.bisect_left(datetime_list, elem)
+        date = datetime_list[i]
+        diff = (date - elem).total_seconds()
+
+        if diff > self.time_tresh:
+            return -1, diff
+
+        return i, diff
+
+    def set_gps_location(self, file_name, lat, lng, alt):
+        """
+        Add the GPS tag and altitude to a image file
+        :param file_name:
+        :param lat:
+        :param lng:
+        :param alt:
+        :return:
+        """
+        lat_deg = self.to_degree(lat, ["S", "N"])
+        lng_deg = self.to_degree(lng, ["W", "E"])
+
+        exiv_lat = (pyexiv2.Rational(lat_deg[0] * 60 + lat_deg[1], 60),
+                    pyexiv2.Rational(lat_deg[2] * 100, 6000), pyexiv2.Rational(0, 1))
+        exiv_lng = (pyexiv2.Rational(lng_deg[0] * 60 + lng_deg[1], 60),
+                    pyexiv2.Rational(lng_deg[2] * 100, 6000), pyexiv2.Rational(0, 1))
+
+        try:
+            exiv_image = pyexiv2.ImageMetadata(file_name)
+            exiv_image.read()
+
+            exiv_image["Exif.GPSInfo.GPSLatitude"] = exiv_lat
+            exiv_image["Exif.GPSInfo.GPSLatitudeRef"] = lat_deg[3]
+            exiv_image["Exif.GPSInfo.GPSLongitude"] = exiv_lng
+            exiv_image["Exif.GPSInfo.GPSLongitudeRef"] = lng_deg[3]
+            exiv_image["Exif.GPSInfo.GPSAltitude"] = pyexiv2.Rational(alt, 1)
+            exiv_image["Exif.GPSInfo.GPSAltitudeRef"] = '0'
+            exiv_image["Exif.Image.GPSTag"] = 654
+            exiv_image["Exif.GPSInfo.GPSMapDatum"] = "WGS-84"
+            exiv_image["Exif.GPSInfo.GPSVersionID"] = '2 0 0 0'
+
+            exiv_image.write(True)
+        except Exception as e:
+            print '[ERROR]', e
+
+    def load_gps_from_log(self, log_file, offset):
+        """
+        Load gps list from PX4 binary log
+        :param log_file:
+        :param offset:
+        :return:
+        """
+        os.system('python sdlog2_dump.py ' + log_file + ' -f log.csv')
+        f = open('log.csv', 'rb')
+        reader = csv.reader(f)
+        headers = reader.next()
+        line = {}
+        for h in headers:
+            line[h] = []
+
+        for row in reader:
+            for h, v in zip(headers, row):
+                line[h].append(v)
+
+        gps_list = []
+        for seq in range(0, len(line['GPS_Lat']) - 1):
+            gps_time = int(line['GPS_TimeMS'][seq + 1])
+            gps_week = int(line['GPS_Week'][seq + 1])
+            gps_lat = float(line['GPS_Lat'][seq + 1])
+            gps_lon = float(line['GPS_Lng'][seq + 1])
+            gps_alt = float(line['GPS_RelAlt'][seq + 1])
+
+            date = self.gps_week_seconds_to_datetime(gps_week, gps_time, leapmillis=offset)
+            print date
+            gps_list.append(GpsPosition(date, gps_lat, gps_lon, gps_alt))
+
+        return gps_list
+
+    def load_image_list(self, input_folder, file_type='jpg'):
+        """
+        Load image list from a folder given a file type
+        :param input_folder:
+        :param file_type:
+        :return:
+        """
+        self.img_list = [input_folder + filename for filename in os.listdir(input_folder)
+                         if re.search(r'\.'+file_type+'$', filename, re.IGNORECASE)]
+        self.img_list = sorted(self.img_list)
+        return self.img_list
+
+    def tag_images(self):
+        """
+        Tag the image list using the GPS loaded from the LOG file
+        :return:
+        """
+        tagged_gps = []
+        img_size = len(self.img_list)
+        print '[INFO] Number of images:', img_size
+
+        dt_list = [x.timestamp for x in self.gps_list]
+
+        img_seq = 1
+
+        for i in xrange(img_size):
+            base_path, filename = os.path.split(self.img_list[i])
+            cdate = datetime.datetime.fromtimestamp(os.path.getmtime(self.img_list[i]))
+            gps_i, img_tdiff = self.get_closest_datetime_index(dt_list, cdate)
+
+            if gps_i == -1:
+                self.non_processed_files.append(filename)
+                continue
+
+            closest_gps = self.gps_list[gps_i]
+            self.tdiff_list.append(img_tdiff)
+
+            if self.verbose:
+                msg = "[DEBUG] %s/%s) %s\n\timg %s -> gps %s (%ss)\n\tlat:%s, lon:%s, alt:%s".ljust(60) %\
+                        (i+1, img_size, filename, cdate, closest_gps.timestamp, img_tdiff, closest_gps.lat, closest_gps.lon, closest_gps.alt)
+                print msg
+
+            copyfile(self.img_list[i], self.output + str(img_seq) + filename)
+            self.set_gps_location(self.output + str(img_seq) + filename, closest_gps.lat, closest_gps.lon, closest_gps.alt)
+            self.tagged_gps.append(closest_gps)
+            img_seq += 1
+
+        if len(self.tdiff_list) > 0:
+            print '[INFO] Mean diff in seconds:', sum(self.tdiff_list) / float(len(self.tdiff_list))
+
+    @staticmethod
+    def get_arg():
+        parser = argparse.ArgumentParser(
+            description='Geotag script to add GPS info to pictures from PX4 binary log files.'\
+                        'It uses synchronized time to allocate GPS positions.'
+        )
+
+        parser.add_argument(
+            '-l', '--logfile', help='PX4 log file containing recorded positions.', required=True
+        )
+        parser.add_argument(
+            '-i', '--input', help='Input folder containing untagged images.', required=True
+        )
+        parser.add_argument(
+            '-o', '--output', help='Output folder to contain tagged images.', required=True
+        )
+        parser.add_argument(
+            '-t', '--treshold', help='Time treshold between the GPS time and the local image time.',
+            default=1, required=False, type=float
+        )
+        parser.add_argument(
+            '-of', '--offset', help='Time offset in MILLISECONDS between the GPS time and the local time.',
+            default=-17000, required=False, type=float
+        )
+        parser.add_argument(
+            '-klm', '--klm', help='Save the in KML format the information of all tagged images.',
+            required=False, action='store_true'
+        )
+        parser.add_argument(
+            '-v', '--verbose', help='Prints lots of information.',
+            required=False, action='store_true'
+        )
+
+        args = vars(parser.parse_args())
+        return args
+
+
+if __name__ == "__main__":
+    m = Main()
\ No newline at end of file

From 4fa59c3cde0fc0ad5d015c91b186d564894fb321 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Beat=20K=C3=BCng?= <beat-kueng@gmx.net>
Date: Tue, 26 Jul 2016 10:20:22 +0200
Subject: [PATCH 1083/1230] sdlog2 README: describe geotagging.py script

---
 Tools/sdlog2/README.txt | 6 +++++-
 1 file changed, 5 insertions(+), 1 deletion(-)

diff --git a/Tools/sdlog2/README.txt b/Tools/sdlog2/README.txt
index 84682fea6b..e407c0936f 100644
--- a/Tools/sdlog2/README.txt
+++ b/Tools/sdlog2/README.txt
@@ -14,4 +14,8 @@ geo_tag_images.py: Use this script to geotag a set of images. It uses GPS time a
 
 	python geo_tag_images.py --logfile=mylog.bin --input=images/ --output=tagged/
 
-Python can be downloaded from http://python.org, but is available as default on Mac OS and Linux.
\ No newline at end of file
+geotagging.py: Use this script to geotag a set of images. It uses the CAM trigger data from the log file for image association.
+
+	python geotagging.py --logfile=mylog.bin --input=images/ --output=tagged/
+
+Python can be downloaded from http://python.org, but is available as default on Mac OS and Linux.

From 4656b342444fef96819e00922c90e80fe72521c6 Mon Sep 17 00:00:00 2001
From: Julian Oes <julian@oes.ch>
Date: Tue, 26 Jul 2016 17:11:29 +0200
Subject: [PATCH 1084/1230] DriverFramework: update submodule

This fixes a gyro/accel scaling bug on Snapdragon.
---
 src/lib/DriverFramework | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/src/lib/DriverFramework b/src/lib/DriverFramework
index 458b726027..12e3224367 160000
--- a/src/lib/DriverFramework
+++ b/src/lib/DriverFramework
@@ -1 +1 @@
-Subproject commit 458b726027ec9d67099e744dd8e0af7208ea64a5
+Subproject commit 12e32243675d0d5d9bce014b912d14f66fb4324e

From c4cb916afaebe457ee86806fb4cafbd942469419 Mon Sep 17 00:00:00 2001
From: Julian Oes <julian@oes.ch>
Date: Wed, 27 Jul 2016 09:55:29 +0200
Subject: [PATCH 1085/1230] Fix sdlog2/logger path/file name overflows. (#5138)

* logger: prevent logpath buffer overflows

The handling of the log path had the potential to cause buffer
overflows, especially on POSIX platforms where the paths are often much
longer than just 64 chars.

* sdlog2: prevent logpath buffer overflows

When the log folder path was created, this was done with the unsafe
sprintf function instead of snprintf. This caused buffer overflows in
SITL but the overflow was usually not detected until recent testing of
some work in progress.
---
 src/modules/logger/logger.cpp | 16 ++++++++++++++--
 src/modules/logger/logger.h   |  8 +++++++-
 src/modules/sdlog2/sdlog2.c   | 15 ++++++++++++++-
 3 files changed, 35 insertions(+), 4 deletions(-)

diff --git a/src/modules/logger/logger.cpp b/src/modules/logger/logger.cpp
index 034580d99b..0331ebf2ba 100644
--- a/src/modules/logger/logger.cpp
+++ b/src/modules/logger/logger.cpp
@@ -860,6 +860,12 @@ int Logger::create_log_dir(tm *tt)
 
 	if (tt) {
 		int n = snprintf(_log_dir, sizeof(_log_dir), "%s/", LOG_ROOT);
+
+		if (n >= sizeof(_log_dir)) {
+			PX4_ERR("log path too long");
+			return -1;
+		}
+
 		strftime(_log_dir + n, sizeof(_log_dir) - n, "%Y-%m-%d", tt);
 		mkdir_ret = mkdir(_log_dir, S_IRWXU | S_IRWXG | S_IRWXO);
 
@@ -874,7 +880,13 @@ int Logger::create_log_dir(tm *tt)
 		/* look for the next dir that does not exist */
 		while (!_has_log_dir && dir_number <= MAX_NO_LOGFOLDER) {
 			/* format log dir: e.g. /fs/microsd/sess001 */
-			sprintf(_log_dir, "%s/sess%03u", LOG_ROOT, dir_number);
+			int n = snprintf(_log_dir, sizeof(_log_dir), "%s/sess%03u", LOG_ROOT, dir_number);
+
+			if (n >= sizeof(_log_dir)) {
+				PX4_ERR("log path too long");
+				return -1;
+			}
+
 			mkdir_ret = mkdir(_log_dir, S_IRWXU | S_IRWXG | S_IRWXO);
 
 			if (mkdir_ret == 0) {
@@ -1030,7 +1042,7 @@ void Logger::start_log()
 
 	PX4_INFO("start log");
 
-	char file_name[64] = "";
+	char file_name[LOG_DIR_LEN] = "";
 
 	if (get_log_file_name(file_name, sizeof(file_name))) {
 		PX4_ERR("logger: failed to get log file name");
diff --git a/src/modules/logger/logger.h b/src/modules/logger/logger.h
index eb11ed56bd..44ad42ebbf 100644
--- a/src/modules/logger/logger.h
+++ b/src/modules/logger/logger.h
@@ -47,6 +47,12 @@ extern "C" __EXPORT int logger_main(int argc, char *argv[]);
 #define TRY_SUBSCRIBE_INTERVAL 1000*1000	// interval in microseconds at which we try to subscribe to a topic
 // if we haven't succeeded before
 
+#ifdef __PX4_NUTTX
+#define LOG_DIR_LEN 64
+#else
+#define LOG_DIR_LEN 256
+#endif
+
 namespace px4
 {
 namespace logger
@@ -210,7 +216,7 @@ private:
 	uint8_t						*_msg_buffer = nullptr;
 	int						_msg_buffer_len = 0;
 	bool						_task_should_exit = true;
-	char 						_log_dir[64];
+	char 						_log_dir[LOG_DIR_LEN];
 	bool						_has_log_dir = false;
 	bool						_enabled = false;
 	bool						_was_armed = false;
diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c
index f1fc820d81..a787a76e15 100644
--- a/src/modules/sdlog2/sdlog2.c
+++ b/src/modules/sdlog2/sdlog2.c
@@ -170,7 +170,11 @@ struct logbuffer_s lb;
 static pthread_mutex_t logbuffer_mutex;
 static pthread_cond_t logbuffer_cond;
 
+#ifdef __PX4_NUTTX
 #define LOG_BASE_PATH_LEN	64
+#else
+#define LOG_BASE_PATH_LEN	256
+#endif
 
 static char log_dir[LOG_BASE_PATH_LEN];
 
@@ -452,6 +456,10 @@ int create_log_dir()
 
 	if (log_name_timestamp && time_ok) {
 		int n = snprintf(log_dir, sizeof(log_dir), "%s/", log_root);
+		if (n >= sizeof(log_dir)) {
+			PX4_ERR("log path too long");
+			return -1;
+		}
 		strftime(log_dir + n, sizeof(log_dir) - n, "%Y-%m-%d", &tt);
 		mkdir_ret = mkdir(log_dir, S_IRWXU | S_IRWXG | S_IRWXO);
 
@@ -466,7 +474,12 @@ int create_log_dir()
 		 * let's re-use it. */
 		while (dir_number <= MAX_NO_LOGFOLDER && !sess_folder_created) {
 			/* format log dir: e.g. /fs/microsd/sess001 */
-			sprintf(log_dir, "%s/sess%03u", log_root, dir_number);
+			int n = snprintf(log_dir, sizeof(log_dir), "%s/sess%03u", log_root, dir_number);
+			if (n >= sizeof(log_dir)) {
+				PX4_ERR("log path too long");
+				return -1;
+			}
+
 			mkdir_ret = mkdir(log_dir, S_IRWXU | S_IRWXG | S_IRWXO);
 
 			if (mkdir_ret == 0) {

From a9cb2d2fbed5421e9297fbb2ab86c3fb12ef81e4 Mon Sep 17 00:00:00 2001
From: Daniel Agar <daniel@agar.ca>
Date: Sun, 17 Jul 2016 15:25:26 -0400
Subject: [PATCH 1086/1230] navigator mission don't report loiter if landed

---
 src/modules/navigator/mission.cpp | 17 +++++++++++------
 1 file changed, 11 insertions(+), 6 deletions(-)

diff --git a/src/modules/navigator/mission.cpp b/src/modules/navigator/mission.cpp
index ad350be3ee..3dd79ef419 100644
--- a/src/modules/navigator/mission.cpp
+++ b/src/modules/navigator/mission.cpp
@@ -388,13 +388,18 @@ Mission::set_mission_items()
 	} else {
 		/* no mission available or mission finished, switch to loiter */
 		if (_mission_type != MISSION_TYPE_NONE) {
-			/* https://en.wikipedia.org/wiki/Loiter_(aeronautics) */
-			mavlink_log_critical(_navigator->get_mavlink_log_pub(), "mission finished, loitering");
+
+			if (_navigator->get_land_detected()->landed) {
+				mavlink_log_critical(_navigator->get_mavlink_log_pub(), "mission finished, landed");
+			} else {
+				/* https://en.wikipedia.org/wiki/Loiter_(aeronautics) */
+				mavlink_log_critical(_navigator->get_mavlink_log_pub(), "mission finished, loitering");
+
+				/* use last setpoint for loiter */
+				_navigator->set_can_loiter_at_sp(true);
+			}
+
 			user_feedback_done = true;
-
-			/* use last setpoint for loiter */
-			_navigator->set_can_loiter_at_sp(true);
-
 		}
 
 		_mission_type = MISSION_TYPE_NONE;

From c4eb65862f307de4b679e24f44fab0c565d7c2a3 Mon Sep 17 00:00:00 2001
From: Sander Smeets <sander@droneslab.com>
Date: Wed, 27 Jul 2016 10:52:20 +0200
Subject: [PATCH 1087/1230] Reduce esc calibration pwm timeout (#5011)

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

diff --git a/src/modules/commander/esc_calibration.cpp b/src/modules/commander/esc_calibration.cpp
index 8bf042cde7..4e202d9f09 100644
--- a/src/modules/commander/esc_calibration.cpp
+++ b/src/modules/commander/esc_calibration.cpp
@@ -91,7 +91,7 @@ int do_esc_calibration(orb_advert_t *mavlink_log_pub, struct actuator_armed_s* a
 	bool	batt_connected = false;
 
 	hrt_abstime battery_connect_wait_timeout = 30000000;
-	hrt_abstime pwm_high_timeout = 4000000;
+	hrt_abstime pwm_high_timeout = 3000000;
 	hrt_abstime timeout_start;
 
 	calibration_log_info(mavlink_log_pub, CAL_QGC_STARTED_MSG, "esc");

From f4e35873a15a6312de7c863395996bf59c005b1d Mon Sep 17 00:00:00 2001
From: Julian Oes <julian@oes.ch>
Date: Thu, 28 Jul 2016 08:47:53 +0200
Subject: [PATCH 1088/1230] DriverFramework: updated submodule (#5148)

This brings fixes for Edison which should not have any influence on
Snapdragon or Raspberry Pi.
---
 src/lib/DriverFramework | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/src/lib/DriverFramework b/src/lib/DriverFramework
index 12e3224367..9f53ea2c2a 160000
--- a/src/lib/DriverFramework
+++ b/src/lib/DriverFramework
@@ -1 +1 @@
-Subproject commit 12e32243675d0d5d9bce014b912d14f66fb4324e
+Subproject commit 9f53ea2c2a4f3ea330153e6da02dbad8632f3cb1

From d63870ad716aae9a65da0b3b1d12fa7e14f16640 Mon Sep 17 00:00:00 2001
From: Julian Oes <julian@oes.ch>
Date: Mon, 25 Jul 2016 11:26:23 +0200
Subject: [PATCH 1089/1230] fmu: don' de-initialize

Once the PWM outputs are initialized, there is no point in
de-initializing them again.
---
 src/drivers/px4fmu/fmu.cpp | 3 ---
 1 file changed, 3 deletions(-)

diff --git a/src/drivers/px4fmu/fmu.cpp b/src/drivers/px4fmu/fmu.cpp
index a9c69d8401..f78962b01f 100644
--- a/src/drivers/px4fmu/fmu.cpp
+++ b/src/drivers/px4fmu/fmu.cpp
@@ -861,9 +861,6 @@ PX4FMU::update_pwm_out_state(bool on)
 		up_pwm_servo_init(_pwm_mask);
 		set_pwm_rate(_pwm_alt_rate_channels, _pwm_default_rate, _pwm_alt_rate);
 		_pwm_initialized = true;
-
-	} else {
-		_pwm_initialized = false;
 	}
 
 	up_pwm_servo_arm(on);

From 3289e0cee607f227d6e667034273bb230a73d9fb Mon Sep 17 00:00:00 2001
From: Julian Oes <julian@oes.ch>
Date: Mon, 25 Jul 2016 11:27:34 +0200
Subject: [PATCH 1090/1230] fmu: don't disarm with disarmed set

If disarmed PWM values have been set, we can't accept a disarm command.
If we did, the PWM output stop alltogether after the high pulse of the
ESC calibration.
---
 src/drivers/px4fmu/fmu.cpp | 7 ++++++-
 1 file changed, 6 insertions(+), 1 deletion(-)

diff --git a/src/drivers/px4fmu/fmu.cpp b/src/drivers/px4fmu/fmu.cpp
index f78962b01f..61d671c83c 100644
--- a/src/drivers/px4fmu/fmu.cpp
+++ b/src/drivers/px4fmu/fmu.cpp
@@ -1533,7 +1533,12 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg)
 		break;
 
 	case PWM_SERVO_DISARM:
-		update_pwm_out_state(false);
+
+		/* Ignore disarm if disarmed PWM is set already. */
+		if (_num_disarmed_set == 0) {
+			update_pwm_out_state(false);
+		}
+
 		break;
 
 	case PWM_SERVO_GET_DEFAULT_UPDATE_RATE:

From 664092b7d49006a4df2bf0ee5840b51c4c02981c Mon Sep 17 00:00:00 2001
From: Julian Oes <julian@oes.ch>
Date: Mon, 25 Jul 2016 11:29:02 +0200
Subject: [PATCH 1091/1230] fmu: support ESC calibration

The FMU was lacking support to do the ESC calibration. This is needed
for Pixracer.
---
 src/drivers/px4fmu/fmu.cpp | 37 ++++++++++++++++++++++++++++++++-----
 1 file changed, 32 insertions(+), 5 deletions(-)

diff --git a/src/drivers/px4fmu/fmu.cpp b/src/drivers/px4fmu/fmu.cpp
index 61d671c83c..268bd8124a 100644
--- a/src/drivers/px4fmu/fmu.cpp
+++ b/src/drivers/px4fmu/fmu.cpp
@@ -221,7 +221,10 @@ private:
 	bool		_safety_disabled;
 	orb_advert_t		_to_safety;
 
-	static bool	arm_nothrottle() { return (_armed.prearmed && !_armed.armed); }
+	static bool	arm_nothrottle()
+	{
+		return ((_armed.prearmed && !_armed.armed) || _armed.in_esc_calibration_mode);
+	}
 
 	static void	cycle_trampoline(void *arg);
 	void		cycle();
@@ -339,6 +342,14 @@ PX4FMU::PX4FMU() :
 	memset(_controls, 0, sizeof(_controls));
 	memset(_poll_fds, 0, sizeof(_poll_fds));
 
+	// Safely initialize armed flags.
+	_armed.armed = false;
+	_armed.prearmed = false;
+	_armed.ready_to_arm = false;
+	_armed.lockdown = false;
+	_armed.force_failsafe = false;
+	_armed.in_esc_calibration_mode = false;
+
 	// rc input, published to ORB
 	memset(&_rc_in, 0, sizeof(_rc_in));
 	_rc_in.input_source = input_rc_s::RC_INPUT_SOURCE_PX4FMU_PPM;
@@ -977,6 +988,19 @@ PX4FMU::cycle()
 
 				poll_id++;
 			}
+
+			/* During ESC calibration, we overwrite the throttle value. */
+			if (i == 0 && _armed.in_esc_calibration_mode) {
+
+				/* Set all controls to 0 */
+				memset(&_controls[i], 0, sizeof(_controls[i]));
+
+				/* except thrust to maximum. */
+				_controls[i].control[3] = 1.0f;
+
+				/* Switch off the PWM limit ramp for the calibration. */
+				_pwm_limit.state = PWM_LIMIT_STATE_ON;
+			}
 		}
 
 		/* can we mix? */
@@ -1095,11 +1119,14 @@ PX4FMU::cycle()
 	if (updated) {
 		orb_copy(ORB_ID(actuator_armed), _armed_sub, &_armed);
 
-		/* update the armed status and check that we're not locked down */
-		_throttle_armed = _safety_off && _armed.armed && !_armed.lockdown;
+		/* Update the armed status and check that we're not locked down.
+		 * We also need to arm throttle for the ESC calibration. */
+		_throttle_armed = (_safety_off && _armed.armed && !_armed.lockdown) ||
+				  (_safety_off && _armed.in_esc_calibration_mode);
+
 
 		/* update PWM status if armed or if disarmed PWM values are set */
-		bool pwm_on = _armed.armed || _num_disarmed_set > 0;
+		bool pwm_on = _armed.armed || _num_disarmed_set > 0 || _armed.in_esc_calibration_mode;
 
 		if (_pwm_on != pwm_on) {
 			_pwm_on = pwm_on;
@@ -1446,7 +1473,7 @@ PX4FMU::control_callback(uintptr_t handle,
 	}
 
 	/* throttle not arming - mark throttle input as invalid */
-	if (arm_nothrottle()) {
+	if (arm_nothrottle() && !_armed.in_esc_calibration_mode) {
 		if ((control_group == actuator_controls_s::GROUP_INDEX_ATTITUDE ||
 		     control_group == actuator_controls_s::GROUP_INDEX_ATTITUDE_ALTERNATE) &&
 		    control_index == actuator_controls_s::INDEX_THROTTLE) {

From a71cfe92f11dc380f3797692e1caf8c44339690d Mon Sep 17 00:00:00 2001
From: Julian Oes <julian@oes.ch>
Date: Mon, 25 Jul 2016 11:29:40 +0200
Subject: [PATCH 1092/1230] fmu: whitespace fix

---
 src/drivers/px4fmu/fmu.cpp | 5 +++--
 1 file changed, 3 insertions(+), 2 deletions(-)

diff --git a/src/drivers/px4fmu/fmu.cpp b/src/drivers/px4fmu/fmu.cpp
index 268bd8124a..691f31d971 100644
--- a/src/drivers/px4fmu/fmu.cpp
+++ b/src/drivers/px4fmu/fmu.cpp
@@ -1050,8 +1050,9 @@ PX4FMU::cycle()
 			uint16_t pwm_limited[_max_actuators];
 
 			/* the PWM limit call takes care of out of band errors, NaN and constrains */
-			pwm_limit_calc(_throttle_armed, arm_nothrottle(), num_outputs, _reverse_pwm_mask, _disarmed_pwm, _min_pwm, _max_pwm,
-				       outputs, pwm_limited, &_pwm_limit);
+			pwm_limit_calc(_throttle_armed, arm_nothrottle(), num_outputs, _reverse_pwm_mask,
+				       _disarmed_pwm, _min_pwm, _max_pwm, outputs, pwm_limited, &_pwm_limit);
+
 
 			/* overwrite outputs in case of lockdown with disarmed PWM values */
 			if (_armed.lockdown) {

From 632e4630b17c5c13a6f132956f137e6d794a5924 Mon Sep 17 00:00:00 2001
From: David Sidrane <david_s5@nscdg.com>
Date: Wed, 20 Jul 2016 14:49:03 -1000
Subject: [PATCH 1093/1230] TAP:ESC motors and LED working - The code is still
 hardcoded for 4 ESC

But the basics are working. N.B. To Stop the moros a value of 0 need to be
sent. LED color mask are misleeding - LED is On or Off per ESC.
---
 src/drivers/tap_esc/drv_tap_esc.h | 124 +++++++++++---
 src/drivers/tap_esc/tap_esc.cpp   | 268 +++++++++++++++++++++---------
 2 files changed, 290 insertions(+), 102 deletions(-)

diff --git a/src/drivers/tap_esc/drv_tap_esc.h b/src/drivers/tap_esc/drv_tap_esc.h
index 01cc7e078e..a84cdee4ac 100644
--- a/src/drivers/tap_esc/drv_tap_esc.h
+++ b/src/drivers/tap_esc/drv_tap_esc.h
@@ -59,49 +59,121 @@
 #define TAP_ESC_MAX_PACKET_LEN 20
 #define TAP_ESC_MAX_MOTOR_NUM 8
 
+/* ESC_POS maps the values stored in the channelMapTable to reorder the ESC's
+ * id so that that match the mux setting, so that the ressonder's data
+ * will be read.
+ * The index on channelMapTable[p] p is the physical ESC
+ * The value it is set to is the logical value from ESC_POS[p]
+ *  Phy Log
+ *  0   0
+ *  1   1
+ *  2   4
+ *  3   3
+ *  4   2
+ *  5   5
+ *   ....
+ *
+ */
+#define ESC_POS {0, 1, 4, 3, 2, 5, 7, 8}
+
 #define RPMMAX 1900
-#define RPMMIN 1100
+#define RPMMIN 1200
 
-#define RUN_CHANNEL_VALUE_MASK	 (uint16_t)0x07ff
-#define RUN_RED_LED_ON_MASK	 (uint16_t)0x0800
-#define RUN_GREEN_LED_ON_MASK	 (uint16_t)0x1000
-#define RUN_BLUE_LED_ON_MASK	 (uint16_t)0x2000
-#define RUN_LED_ON_MASK	 (uint16_t)0x3800
-#define RUN_FEEDBACK_ENABLE_MASK	(uint16_t)0x4000
-#define RUN_REVERSE_MASK	 (uint16_t)0x8000
 
+#define MAX_BOOT_TIME_MS		 (500) // Minimum time to wait after Power on before sending commands
 
 #pragma pack(push,1)
-typedef  struct {
-	uint8_t ESC_ID;
-	uint8_t ESC_STATUS;
-	int16_t speed;
-#ifdef VOLTAGE_SENSOR_HAVE
-	uint16_t voltage;
-#endif
-#ifdef CURRENT_SENSOR_HAVE
-	uint16_t current;
-#endif
-#ifdef TEMPERATURE_SENSOR_HAVE
-	uint8_t temperature;
-#endif
 
-} ESC_FEEDBACK_DATA;
+/****** Run ***********/
+
+#define RUN_CHANNEL_VALUE_MASK   (uint16_t)0x07ff
+#define RUN_RED_LED_ON_MASK      (uint16_t)0x0800
+#define RUN_GREEN_LED_ON_MASK    (uint16_t)0x1000
+#define RUN_BLUE_LED_ON_MASK     (uint16_t)0x2000
+#define RUN_LED_ON_MASK          (uint16_t)0x3800
+#define RUN_FEEDBACK_ENABLE_MASK (uint16_t)0x4000
+#define RUN_REVERSE_MASK         (uint16_t)0x8000
+
+typedef struct {
+
+	uint16_t rpm_flags[TAP_ESC_MAX_MOTOR_NUM];
+} RunReq;
+
+typedef struct {
+	uint8_t channelID;
+	uint8_t ESCStatus;
+	int16_t speed; // -32767 - 32768
+#if defined(ESC_HAVE_VOLTAGE_SENSOR)
+	uint16_t voltage; // 0.00 - 100.00 V
+#endif
+#if defined(ESC_HAVE_CURRENT_SENSOR)
+	uint16_t current; // 0.0 - 200.0 A
+#endif
+#if defined(ESC_HAVE_TEMPERATURE_SENSOR)
+	uint8_t temperature; // 0 - 256 degree celsius
+#endif
+} RunInfoRepsonse;
+/****** Run ***********/
+
+/****** ConFigInfoBasic ***********/
+typedef  struct {
+	uint8_t  maxChannelInUse;
+	uint8_t  channelMapTable[TAP_ESC_MAX_MOTOR_NUM];
+	uint8_t  monitorMsgType;
+	uint8_t  controlMode;
+	uint16_t minChannelValue;
+	uint16_t maxChannelValue;
+} ConfigInfoBasicRequest;
+
+typedef  struct {
+	uint8_t  channelID;
+	ConfigInfoBasicRequest resp;
+} ConfigInfoBasicResponse;
+
+#define ESC_CHANNEL_MAP_CHANNEL           0x0f
+#define ESC_CHANNEL_MAP_RUNNING_DIRECTION 0xf0
+/****** ConFigInfoBasicResponse ***********/
+
+/****** InfoRequest  ***********/
+typedef enum {
+	REQEST_INFO_BASIC = 0,
+	REQEST_INFO_FUll,
+	REQEST_INFO_RUN,
+	REQEST_INFO_STUDY,
+	REQEST_INFO_COMM,
+	REQEST_INFO_DEVICE,
+} InfoTypes;
+
+typedef  struct {
+	uint8_t  channelID;
+	uint8_t  requestInfoType;
+} InfoRequest;
+
+/****** InfoRequest ***********/
 
 typedef  struct {
 	uint8_t head;
 	uint8_t len;
 	uint8_t msg_id;
-	uint8_t data[100];
+	union {
+		InfoRequest 			reqInfo;
+		ConfigInfoBasicRequest 	reqConfigInfoBasic;
+		RunReq					reqRun;
+
+		ConfigInfoBasicResponse rspConfigInfoBasic;
+		RunInfoRepsonse			rspRunInfo;
+		uint8_t bytes[100];
+	} d;
 	uint8_t crc_data;
 
-} ESC_FEEDBACK_PACKET;
+} EscPacket;
 
+#define UART_BUFFER_SIZE 128
 typedef  struct {
 	uint8_t head;
 	uint8_t tail;
 	uint8_t dat_cnt;
-	uint8_t esc_feedback_buf[128];
+	uint8_t esc_feedback_buf[UART_BUFFER_SIZE];
 } ESC_UART_BUF;
 
 #pragma pack(pop)
@@ -140,6 +212,8 @@ typedef enum {
 	ESC_STATUS_ERROR_HARDWARE,
 	ESC_STATUS_ERROR_LOSE_PROPELLER,
 	ESC_STATUS_ERROR_OVER_CURRENT,
+	ESC_STATUS_ERROR_MOTOR_HIGH_SPEED_LOSE_STEP,
+	ESC_STATUS_ERROR_LOSE_CMD,
 } ESCBUS_ENUM_ESC_STATUS;
 
 
diff --git a/src/drivers/tap_esc/tap_esc.cpp b/src/drivers/tap_esc/tap_esc.cpp
index 65bb927790..cfc40430fa 100644
--- a/src/drivers/tap_esc/tap_esc.cpp
+++ b/src/drivers/tap_esc/tap_esc.cpp
@@ -39,6 +39,7 @@
 #include <errno.h>
 #include <termios.h>
 
+#include <systemlib/px4_macros.h>
 #include <drivers/device/device.h>
 #include <uORB/uORB.h>
 #include <uORB/topics/actuator_controls.h>
@@ -60,15 +61,15 @@
 #define B250000 250000
 #endif
 
-#define VOLTAGE_SENSOR_HAVE
+#define ESC_HAVE_CURRENT_SENSOR
 
 #include "drv_tap_esc.h"
 
 /*
  * This driver connects to TAP ESCs via serial.
  */
-static const uint8_t crcTable[256] = TAP_ESC_CRC;
-static int _uart_fd = -1;
+
+static int _uart_fd = -1; //todo:refactor in to class
 class TAP_ESC : public device::CDev
 {
 public:
@@ -85,13 +86,18 @@ public:
 		MODE_5CAP,
 		MODE_6CAP,
 	};
-	TAP_ESC();
+	TAP_ESC(int channels_count);
 	virtual ~TAP_ESC();
 	virtual int	init();
 	virtual int	ioctl(file *filp, int cmd, unsigned long arg);
 	void cycle();
+protected:
+	void select_responder(uint8_t sel);
 private:
 
+	static const uint8_t crcTable[256];
+	static const uint8_t device_mux_map[TAP_ESC_MAX_MOTOR_NUM];
+
 	bool IS_armed;
 
 	unsigned	_poll_fds_num;
@@ -104,6 +110,8 @@ private:
 	actuator_outputs_s      _outputs;
 	static actuator_armed_s	_armed;
 
+	//todo:refactor dynamic based on _num_outputs or _channels_count
+	// It needs to support the numbe of ESC
 	int	_control_subs[actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS];
 
 	px4_pollfd_struct_t	_poll_fds[actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS];
@@ -114,6 +122,7 @@ private:
 
 	orb_advert_t        _esc_feedback_pub = nullptr;
 	esc_status_s      _esc_feedback;
+	uint8_t           _channels_count; // The numbe of ESC channels
 
 	MixerGroup	*_mixers;
 	uint32_t	_groups_required;
@@ -122,22 +131,26 @@ private:
 	unsigned	_pwm_default_rate;
 	unsigned	_current_update_rate;
 	ESC_UART_BUF uartbuf;
-	ESC_FEEDBACK_DATA feed_back_data;
-	ESC_FEEDBACK_PACKET  feed_back_packet;
+	EscPacket  		_packet;
 	void		subscribe();
 
 	void		work_start();
 	void		work_stop();
 	void send_esc_outputs(const float *pwm, const unsigned num_pwm);
 	uint8_t crc8_esc(uint8_t *p, uint8_t len);
+	uint8_t crc_packet(EscPacket &p);
+	int send_packet(EscPacket &p, int responder);
 	void read_data_from_uart();
-	bool parse_tap_esc_feedback(ESC_UART_BUF *serial_buf, ESC_FEEDBACK_PACKET *packetdata);
+	bool parse_tap_esc_feedback(ESC_UART_BUF *serial_buf, EscPacket *packetdata);
 	static int control_callback(uintptr_t handle,
 				    uint8_t control_group,
 				    uint8_t control_index,
 				    float &input);
 };
 
+const uint8_t TAP_ESC::crcTable[256] = TAP_ESC_CRC;
+const uint8_t TAP_ESC::device_mux_map[TAP_ESC_MAX_MOTOR_NUM] = ESC_POS;
+
 actuator_armed_s TAP_ESC::_armed = {};
 
 namespace
@@ -147,7 +160,7 @@ TAP_ESC	*tap_esc;
 
 # define TAP_ESC_DEVICE_PATH	"/dev/tap_esc"
 
-TAP_ESC::TAP_ESC():
+TAP_ESC::TAP_ESC(int channels_count):
 	CDev("tap_esc", TAP_ESC_DEVICE_PATH),
 	IS_armed(false),
 	_poll_fds_num(0),
@@ -159,6 +172,7 @@ TAP_ESC::TAP_ESC():
 	_control_subs{ -1},
 	_esc_feedback_pub(nullptr),
 	_esc_feedback{},
+	_channels_count(channels_count),
 	_mixers(nullptr),
 	_groups_required(0),
 	_groups_subscribed(0),
@@ -208,14 +222,126 @@ TAP_ESC::init()
 
 	ASSERT(!_initialized);
 
-	/* do regular cdev init */
-	ret = CDev::init();
+	/* Respect boot time requierd by the ESC FW */
 
-	if (ret != OK) {
+	hrt_abstime uptime_us = hrt_absolute_time();
+
+	if (uptime_us < MAX_BOOT_TIME_MS * 1000) {
+		usleep((MAX_BOOT_TIME_MS * 1000) - uptime_us);
+	}
+
+	/* Issue Basic Config */
+
+	EscPacket packet = {0xfe, sizeof(ConfigInfoBasicRequest), ESCBUS_MSG_ID_CONFIG_BASIC};
+	ConfigInfoBasicRequest   &config = packet.d.reqConfigInfoBasic;
+	memset(&config, sizeof(ConfigInfoBasicRequest), 0);
+	config.maxChannelInUse = _channels_count;
+
+	/* Asign the id's to the ESCs to match the mux */
+
+	for (uint8_t phy_chan_index = 0; phy_chan_index < _channels_count; phy_chan_index++) {
+		config.channelMapTable[phy_chan_index] = device_mux_map[phy_chan_index] &
+				ESC_CHANNEL_MAP_CHANNEL; // Use ESC_CHANNEL_MAP_RUNNING_DIRECTION;
+	}
+
+	config.maxChannelValue = RPMMAX;
+	config.minChannelValue = RPMMIN;
+
+	ret = send_packet(packet, 0);
+
+	if (ret < 0) {
 		return ret;
 	}
 
-	return OK;
+	/* Verify All ESC got the config */
+
+	for (uint8_t cid = 0; cid < _channels_count; cid++) {
+
+		/* Send the InfoRequest querying  CONFIG_BASIC */
+		EscPacket packet_info = {0xfe, sizeof(InfoRequest), ESCBUS_MSG_ID_REQUEST_INFO};
+		InfoRequest &info_req = packet_info.d.reqInfo;
+		info_req.channelID = cid;
+		info_req.requestInfoType = REQEST_INFO_BASIC;
+
+		ret = send_packet(packet_info, cid);
+
+		if (ret < 0) {
+			return ret;
+		}
+
+		/* Get a response */
+
+		int retries = 10;
+		bool valid = false;
+
+		while (retries--) {
+
+			read_data_from_uart();
+
+			if (parse_tap_esc_feedback(&uartbuf, &_packet)) {
+				valid = (_packet.msg_id == ESCBUS_MSG_ID_CONFIG_INFO_BASIC
+					 && _packet.d.rspConfigInfoBasic.channelID == cid
+					 && 0 == memcmp(&_packet.d.rspConfigInfoBasic.resp, &config, sizeof(ConfigInfoBasicRequest)));
+				break;
+
+			} else {
+
+				/* Give it time to come in */
+
+				usleep(1000);
+			}
+		}
+
+		if (retries == 0 || !valid) {
+			return -EIO;
+		}
+	}
+
+	/* To Unlock the ESC from the Power up state we need to issue 10
+	 * ESCBUS_MSG_ID_RUN request with all the values 0;
+	 */
+
+	EscPacket unlock_packet = {0xfe, _channels_count, ESCBUS_MSG_ID_RUN};
+	unlock_packet.len *= sizeof(unlock_packet.d.reqRun.rpm_flags[0]);
+	memset(unlock_packet.d.bytes, 0, sizeof(packet.d.bytes));
+
+	int unlock_times = 10;
+
+	while (unlock_times--) {
+
+		send_packet(unlock_packet, -1);
+
+		/* Min Packet to Packet time is 1 Ms so use 2 */
+
+		usleep(1000 * 2);
+	}
+
+	/* do regular cdev init */
+
+	ret = CDev::init();
+
+	return ret;
+}
+
+int TAP_ESC::send_packet(EscPacket &packet, int responder)
+{
+	if (responder >= 0) {
+
+		if (responder > _channels_count) {
+			return -EINVAL;
+		}
+
+		select_responder(responder);
+	}
+
+	int packet_len = crc_packet(packet);
+	int ret = ::write(_uart_fd, &packet.head, packet_len);
+
+	if (ret != packet_len) {
+		PX4_WARN("TX ERROR: ret: %d, errno: %d", ret, errno);
+	}
+
+	return ret;
 }
 
 void
@@ -257,12 +383,27 @@ uint8_t TAP_ESC::crc8_esc(uint8_t *p, uint8_t len)
 	return crc;
 }
 
+uint8_t TAP_ESC::crc_packet(EscPacket &p)
+{
+	/* Calculate the crc over Len,ID,data */
+	p.d.bytes[p.len] = crc8_esc(&p.len, p.len + 2);
+	return p.len + offsetof(EscPacket, d) + 1;
+}
+void TAP_ESC::select_responder(uint8_t sel)
+{
+	px4_arch_gpiowrite(GPIO_S0, sel & 1);
+	px4_arch_gpiowrite(GPIO_S1, sel & 2);
+	px4_arch_gpiowrite(GPIO_S2, sel & 4);
+
+}
+
+
 void TAP_ESC:: send_esc_outputs(const float *pwm, const unsigned num_pwm)
 {
 
 	uint16_t rpm[TAP_ESC_MAX_MOTOR_NUM];
+	memset(rpm, 0, sizeof(rpm));
 	uint8_t motor_cnt = num_pwm;
-	unsigned char ESCData[TAP_ESC_MAX_PACKET_LEN] = {0};
 	static uint8_t which_to_respone = 0;
 
 	for (uint8_t i = 0; i < motor_cnt; i++) {
@@ -276,55 +417,22 @@ void TAP_ESC:: send_esc_outputs(const float *pwm, const unsigned num_pwm)
 		}
 	}
 
-	switch (which_to_respone) {
-	case 0: rpm[0] |= RUN_FEEDBACK_ENABLE_MASK;
-		break;
-
-	case 1: rpm[1] |= RUN_FEEDBACK_ENABLE_MASK;
-		break;
-
-	case 2: rpm[2] |= RUN_FEEDBACK_ENABLE_MASK;
-		break;
-
-	case 3: rpm[3] |= RUN_FEEDBACK_ENABLE_MASK;
-		break;
-
-	case 4: rpm[4] |= RUN_FEEDBACK_ENABLE_MASK;
-		break;
-
-	case 5: rpm[5] |= RUN_FEEDBACK_ENABLE_MASK;
-		break;
-
-	case 6: rpm[6] |= RUN_FEEDBACK_ENABLE_MASK;
-		break;
-
-	case 7: rpm[7] |= RUN_FEEDBACK_ENABLE_MASK;
-
-	default: break;
+	rpm[which_to_respone] |= (RUN_FEEDBACK_ENABLE_MASK | RUN_BLUE_LED_ON_MASK);
 
 
+	EscPacket packet = {0xfe, _channels_count, ESCBUS_MSG_ID_RUN};
+	packet.len *= sizeof(packet.d.reqRun.rpm_flags[0]);
+
+	for (uint8_t i = 0; i < _channels_count; i++) {
+		packet.d.reqRun.rpm_flags[i] = rpm[i];
 	}
 
-	if (++which_to_respone == motor_cnt) {
+	int ret = send_packet(packet, which_to_respone);
+
+	if (++which_to_respone == _channels_count) {
 		which_to_respone = 0;
 	}
 
-	//rpm[2]|=RUN_FEEDBACK_ENABLE_MASK;
-
-	ESCData[0] = 0xFE;
-	ESCData[1] = motor_cnt * 2;
-	ESCData[2] = ESCBUS_MSG_ID_RUN;
-
-	for (uint8_t i = 0; i < motor_cnt; i++) {
-		ESCData[3 + 2 * i] = rpm[i] & 0x00ff;
-		ESCData[3 + 2 * i + 1] = rpm[i] >> 8;
-	}
-
-	ESCData[3 + 2 * motor_cnt] = crc8_esc(ESCData + 1, motor_cnt * 2 + 2);
-
-	int ret = ::write(_uart_fd, &ESCData[0], motor_cnt * 2 + 4);
-
-
 	if (ret < 1) {
 		PX4_WARN("TX ERROR: ret: %d, errno: %d", ret, errno);
 	}
@@ -332,30 +440,32 @@ void TAP_ESC:: send_esc_outputs(const float *pwm, const unsigned num_pwm)
 
 void TAP_ESC::read_data_from_uart()
 {
-	uint8_t tmp_serial_buf[128] = {0};
+	uint8_t tmp_serial_buf[UART_BUFFER_SIZE] = {0};
 
-	int len =::read(_uart_fd, &tmp_serial_buf[0], 128);
+	int len =::read(_uart_fd, &tmp_serial_buf[0], arraySize(tmp_serial_buf));
 
-	if (len > 0 && (uartbuf.dat_cnt + len < 128)) {
+	if (len > 0 && (uartbuf.dat_cnt + len < UART_BUFFER_SIZE)) {
 		for (int i = 0; i < len; i++) {
 			uartbuf.esc_feedback_buf[uartbuf.tail++] = tmp_serial_buf[i];
 			uartbuf.dat_cnt++;
 
-			if (uartbuf.tail >= 128) {
+			if (uartbuf.tail >= UART_BUFFER_SIZE) {
 				uartbuf.tail = 0;
 			}
 		}
 	}
 }
 
-bool TAP_ESC:: parse_tap_esc_feedback(ESC_UART_BUF *serial_buf, ESC_FEEDBACK_PACKET *packetdata)
+bool TAP_ESC:: parse_tap_esc_feedback(ESC_UART_BUF *serial_buf, EscPacket *packetdata)
 {
 	static PARSR_ESC_STATE state = HEAD;
 	static uint8_t data_index = 0;
 	static uint8_t crc_data_cal;
 
 	if (serial_buf->dat_cnt > 0) {
-		for (int i = 0; i < serial_buf->dat_cnt; i++) {
+		int count = serial_buf->dat_cnt;
+
+		for (int i = 0; i < count; i++) {
 			switch (state) {
 			case HEAD: if (serial_buf->esc_feedback_buf[serial_buf->head] == 0xFE) {
 					packetdata->head = 0xFE; //just_keep the format
@@ -364,7 +474,7 @@ bool TAP_ESC:: parse_tap_esc_feedback(ESC_UART_BUF *serial_buf, ESC_FEEDBACK_PAC
 
 				break;
 
-			case LEN:  if (serial_buf->esc_feedback_buf[serial_buf->head] < 100) {
+			case LEN:  if (serial_buf->esc_feedback_buf[serial_buf->head] < sizeof(packetdata->d)) {
 					packetdata->len = serial_buf->esc_feedback_buf[serial_buf->head];
 					state = ID;
 
@@ -385,7 +495,7 @@ bool TAP_ESC:: parse_tap_esc_feedback(ESC_UART_BUF *serial_buf, ESC_FEEDBACK_PAC
 
 				break;
 
-			case DATA: packetdata->data[data_index++] = serial_buf->esc_feedback_buf[serial_buf->head];
+			case DATA: packetdata->d.bytes[data_index++] = serial_buf->esc_feedback_buf[serial_buf->head];
 
 				if (data_index >= packetdata->len) {
 
@@ -415,7 +525,7 @@ bool TAP_ESC:: parse_tap_esc_feedback(ESC_UART_BUF *serial_buf, ESC_FEEDBACK_PAC
 
 			}
 
-			if (++serial_buf->head >= 128) {
+			if (++serial_buf->head >= UART_BUFFER_SIZE) {
 				serial_buf->head = 0;
 			}
 
@@ -434,7 +544,6 @@ TAP_ESC::cycle()
 {
 
 	if (!_initialized) {
-
 		_current_update_rate = 0;
 		/* advertise the mixed control outputs, insist on the first group output */
 		_outputs_pub = orb_advertise(ORB_ID(actuator_outputs), &_outputs);
@@ -603,13 +712,13 @@ TAP_ESC::cycle()
 		send_esc_outputs(motor_out, esc_count);
 		read_data_from_uart();
 
-		if (parse_tap_esc_feedback(&uartbuf, &feed_back_packet) == true) {
-			if (feed_back_packet.msg_id == ESCBUS_MSG_ID_RUN_INFO) {
-				feed_back_data = *(ESC_FEEDBACK_DATA *)feed_back_packet.data;
-				_esc_feedback.esc[feed_back_data.ESC_ID].esc_rpm = feed_back_data.speed;
-				_esc_feedback.esc[feed_back_data.ESC_ID].esc_voltage = feed_back_data.voltage;
-				_esc_feedback.esc[feed_back_data.ESC_ID].esc_state = feed_back_data.ESC_STATUS;
-				_esc_feedback.esc[feed_back_data.ESC_ID].esc_vendor = esc_status_s::ESC_VENDOR_TAP;
+		if (parse_tap_esc_feedback(&uartbuf, &_packet) == true) {
+			if (_packet.msg_id == ESCBUS_MSG_ID_RUN_INFO) {
+				RunInfoRepsonse &feed_back_data = _packet.d.rspRunInfo;
+				_esc_feedback.esc[feed_back_data.channelID].esc_rpm = feed_back_data.speed;
+//				_esc_feedback.esc[feed_back_data.channelID].esc_voltage = feed_back_data.voltage;
+				_esc_feedback.esc[feed_back_data.channelID].esc_state = feed_back_data.ESCStatus;
+				_esc_feedback.esc[feed_back_data.channelID].esc_vendor = esc_status_s::ESC_VENDOR_TAP;
 				// printf("vol is %d\n",feed_back_data.voltage );
 				// printf("speed is %d\n",feed_back_data.speed );
 
@@ -776,6 +885,7 @@ volatile bool _task_should_exit = false; // flag indicating if tap_esc task shou
 static char _device[32] = {};
 static bool _is_running = false;         // flag indicating if tap_esc app is running
 static px4_task_t _task_handle = -1;     // handle to the task main thread
+static int _supported_channel_count = 0;
 
 static bool _flow_control_enabled = false;
 
@@ -802,7 +912,7 @@ int tap_esc_start(void)
 
 	if (tap_esc == nullptr) {
 
-		tap_esc = new TAP_ESC;
+		tap_esc = new TAP_ESC(_supported_channel_count);
 
 		if (tap_esc == nullptr) {
 			ret = -ENOMEM;
@@ -947,7 +1057,7 @@ void start()
 	_task_handle = px4_task_spawn_cmd("tap_esc_main",
 					  SCHED_DEFAULT,
 					  SCHED_PRIORITY_MAX,
-					  1500,
+					  2500,
 					  (px4_main_t)&task_main_trampoline,
 					  nullptr);
 
@@ -973,7 +1083,7 @@ void stop()
 
 void usage()
 {
-	PX4_INFO("usage: tap_esc start -d /dev/ttyS2");
+	PX4_INFO("usage: tap_esc start -d /dev/ttyS2 -n <1-8>");
 	PX4_INFO("       tap_esc stop");
 	PX4_INFO("       tap_esc status");
 }
@@ -996,12 +1106,16 @@ int tap_esc_main(int argc, char *argv[])
 		verb = argv[1];
 	}
 
-	while ((ch = px4_getopt(argc, argv, "d:", &myoptind, &myoptarg)) != EOF) {
+	while ((ch = px4_getopt(argc, argv, "d:n:", &myoptind, &myoptarg)) != EOF) {
 		switch (ch) {
 		case 'd':
 			device = myoptarg;
 			strncpy(tap_esc_drv::_device, device, strlen(device));
 			break;
+
+		case 'n':
+			tap_esc_drv::_supported_channel_count = atoi(myoptarg);
+			break;
 		}
 	}
 
@@ -1013,7 +1127,7 @@ int tap_esc_main(int argc, char *argv[])
 		}
 
 		// Check on required arguments
-		if (device == nullptr || strlen(device) == 0) {
+		if (tap_esc_drv::_supported_channel_count == 0 || device == nullptr || strlen(device) == 0) {
 			tap_esc_drv::usage();
 			return 1;
 		}

From a8e28c7232dcc265ca369c933ba215b9c4e48881 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Beat=20K=C3=BCng?= <beat-kueng@gmx.net>
Date: Fri, 22 Jul 2016 14:12:09 +0200
Subject: [PATCH 1094/1230] tap_esc: fix argument ordering in memset

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

diff --git a/src/drivers/tap_esc/tap_esc.cpp b/src/drivers/tap_esc/tap_esc.cpp
index cfc40430fa..5f57e9c655 100644
--- a/src/drivers/tap_esc/tap_esc.cpp
+++ b/src/drivers/tap_esc/tap_esc.cpp
@@ -234,7 +234,7 @@ TAP_ESC::init()
 
 	EscPacket packet = {0xfe, sizeof(ConfigInfoBasicRequest), ESCBUS_MSG_ID_CONFIG_BASIC};
 	ConfigInfoBasicRequest   &config = packet.d.reqConfigInfoBasic;
-	memset(&config, sizeof(ConfigInfoBasicRequest), 0);
+	memset(&config, 0, sizeof(ConfigInfoBasicRequest));
 	config.maxChannelInUse = _channels_count;
 
 	/* Asign the id's to the ESCs to match the mux */

From 3f9f320f18c0658ae6e5890443262aca959e77bb Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Beat=20K=C3=BCng?= <beat-kueng@gmx.net>
Date: Fri, 22 Jul 2016 14:13:58 +0200
Subject: [PATCH 1095/1230] tap_esc: improve error handling & reporting

---
 src/drivers/tap_esc/tap_esc.cpp | 28 ++++++++++++++++++----------
 1 file changed, 18 insertions(+), 10 deletions(-)

diff --git a/src/drivers/tap_esc/tap_esc.cpp b/src/drivers/tap_esc/tap_esc.cpp
index 5f57e9c655..ca66457dfb 100644
--- a/src/drivers/tap_esc/tap_esc.cpp
+++ b/src/drivers/tap_esc/tap_esc.cpp
@@ -114,7 +114,7 @@ private:
 	// It needs to support the numbe of ESC
 	int	_control_subs[actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS];
 
-	px4_pollfd_struct_t	_poll_fds[actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS];
+	pollfd	_poll_fds[actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS];
 
 	actuator_controls_s _controls[actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS];
 
@@ -155,7 +155,7 @@ actuator_armed_s TAP_ESC::_armed = {};
 
 namespace
 {
-TAP_ESC	*tap_esc;
+TAP_ESC	*tap_esc = nullptr;
 }
 
 # define TAP_ESC_DEVICE_PATH	"/dev/tap_esc"
@@ -292,7 +292,7 @@ TAP_ESC::init()
 			}
 		}
 
-		if (retries == 0 || !valid) {
+		if (!valid) {
 			return -EIO;
 		}
 	}
@@ -921,6 +921,7 @@ int tap_esc_start(void)
 			ret = tap_esc->init();
 
 			if (ret != OK) {
+				PX4_ERR("failed to initialize tap_esc (%i)", ret);
 				delete tap_esc;
 				tap_esc = nullptr;
 			}
@@ -946,7 +947,7 @@ int tap_esc_stop(void)
 int initialise_uart()
 {
 	// open uart
-	_uart_fd = px4_open(_device, O_RDWR | O_NOCTTY | O_NONBLOCK);
+	_uart_fd = open(_device, O_RDWR | O_NOCTTY | O_NONBLOCK);
 	int termios_state = -1;
 
 	if (_uart_fd < 0) {
@@ -964,14 +965,14 @@ int initialise_uart()
 
 	// set baud rate
 	if (cfsetispeed(&uart_config, speed) < 0 || cfsetospeed(&uart_config, speed) < 0) {
-		warnx("ERR SET BAUD %s: %d\n", _device, termios_state);
-		::close(_uart_fd);
+		PX4_ERR("failed to set baudrate for %s: %d\n", _device, termios_state);
+		close(_uart_fd);
 		return -1;
 	}
 
 	if ((termios_state = tcsetattr(_uart_fd, TCSANOW, &uart_config)) < 0) {
-		PX4_WARN("ERR SET CONF %s\n", _device);
-		px4_close(_uart_fd);
+		PX4_ERR("tcsetattr failed for %s\n", _device);
+		close(_uart_fd);
 		return -1;
 	}
 
@@ -1027,7 +1028,9 @@ void task_main(int argc, char *argv[])
 	}
 
 	if (tap_esc_start() != OK) {
-		PX4_ERR("Failed to start TAP_ESC.");
+		PX4_ERR("failed to start tap_esc.");
+		_is_running = false;
+		return;
 	}
 
 
@@ -1062,7 +1065,8 @@ void start()
 					  nullptr);
 
 	if (_task_handle < 0) {
-		warn("task start failed");
+		PX4_ERR("task start failed");
+		_task_handle = -1;
 		return;
 	}
 }
@@ -1119,6 +1123,10 @@ int tap_esc_main(int argc, char *argv[])
 		}
 	}
 
+	if (!tap_esc && tap_esc_drv::_task_handle != -1) {
+		tap_esc_drv::_task_handle = -1;
+	}
+
 	// Start/load the driver.
 	if (!strcmp(verb, "start")) {
 		if (tap_esc_drv::_is_running) {

From 8602ff78560fdcf00bb05ec40b1c76414c9750f3 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Beat=20K=C3=BCng?= <beat-kueng@gmx.net>
Date: Mon, 25 Jul 2016 12:24:27 +0200
Subject: [PATCH 1096/1230] tap_esc: fix code style, IS_armed -> _is_armed

---
 src/drivers/tap_esc/tap_esc.cpp | 31 +++++++++++++++++--------------
 1 file changed, 17 insertions(+), 14 deletions(-)

diff --git a/src/drivers/tap_esc/tap_esc.cpp b/src/drivers/tap_esc/tap_esc.cpp
index ca66457dfb..da74012f57 100644
--- a/src/drivers/tap_esc/tap_esc.cpp
+++ b/src/drivers/tap_esc/tap_esc.cpp
@@ -98,7 +98,7 @@ private:
 	static const uint8_t crcTable[256];
 	static const uint8_t device_mux_map[TAP_ESC_MAX_MOTOR_NUM];
 
-	bool IS_armed;
+	bool _is_armed;
 
 	unsigned	_poll_fds_num;
 	Mode		_mode;
@@ -162,7 +162,7 @@ TAP_ESC	*tap_esc = nullptr;
 
 TAP_ESC::TAP_ESC(int channels_count):
 	CDev("tap_esc", TAP_ESC_DEVICE_PATH),
-	IS_armed(false),
+	_is_armed(false),
 	_poll_fds_num(0),
 	_mode(MODE_4PWM),
 	_num_outputs(0),
@@ -442,7 +442,7 @@ void TAP_ESC::read_data_from_uart()
 {
 	uint8_t tmp_serial_buf[UART_BUFFER_SIZE] = {0};
 
-	int len =::read(_uart_fd, &tmp_serial_buf[0], arraySize(tmp_serial_buf));
+	int len =::read(_uart_fd, tmp_serial_buf, arraySize(tmp_serial_buf));
 
 	if (len > 0 && (uartbuf.dat_cnt + len < UART_BUFFER_SIZE)) {
 		for (int i = 0; i < len; i++) {
@@ -467,14 +467,16 @@ bool TAP_ESC:: parse_tap_esc_feedback(ESC_UART_BUF *serial_buf, EscPacket *packe
 
 		for (int i = 0; i < count; i++) {
 			switch (state) {
-			case HEAD: if (serial_buf->esc_feedback_buf[serial_buf->head] == 0xFE) {
+			case HEAD:
+				if (serial_buf->esc_feedback_buf[serial_buf->head] == 0xFE) {
 					packetdata->head = 0xFE; //just_keep the format
 					state = LEN;
 				}
 
 				break;
 
-			case LEN:  if (serial_buf->esc_feedback_buf[serial_buf->head] < sizeof(packetdata->d)) {
+			case LEN:
+				if (serial_buf->esc_feedback_buf[serial_buf->head] < sizeof(packetdata->d)) {
 					packetdata->len = serial_buf->esc_feedback_buf[serial_buf->head];
 					state = ID;
 
@@ -484,7 +486,8 @@ bool TAP_ESC:: parse_tap_esc_feedback(ESC_UART_BUF *serial_buf, EscPacket *packe
 
 				break;
 
-			case ID:   if (serial_buf->esc_feedback_buf[serial_buf->head] < ESCBUS_MSG_ID_MAX_NUM) {
+			case ID:
+				if (serial_buf->esc_feedback_buf[serial_buf->head] < ESCBUS_MSG_ID_MAX_NUM) {
 					packetdata->msg_id = serial_buf->esc_feedback_buf[serial_buf->head];
 					data_index = 0;
 					state = DATA;
@@ -495,7 +498,8 @@ bool TAP_ESC:: parse_tap_esc_feedback(ESC_UART_BUF *serial_buf, EscPacket *packe
 
 				break;
 
-			case DATA: packetdata->d.bytes[data_index++] = serial_buf->esc_feedback_buf[serial_buf->head];
+			case DATA:
+				packetdata->d.bytes[data_index++] = serial_buf->esc_feedback_buf[serial_buf->head];
 
 				if (data_index >= packetdata->len) {
 
@@ -505,7 +509,8 @@ bool TAP_ESC:: parse_tap_esc_feedback(ESC_UART_BUF *serial_buf, EscPacket *packe
 
 				break;
 
-			case CRC: if (crc_data_cal == serial_buf->esc_feedback_buf[serial_buf->head]) {
+			case CRC:
+				if (crc_data_cal == serial_buf->esc_feedback_buf[serial_buf->head]) {
 					packetdata->crc_data = serial_buf->esc_feedback_buf[serial_buf->head];
 
 					if (++serial_buf->head >= 128) {
@@ -520,7 +525,8 @@ bool TAP_ESC:: parse_tap_esc_feedback(ESC_UART_BUF *serial_buf, EscPacket *packe
 				state = HEAD;
 				break;
 
-			default : state = HEAD;
+			default:
+				state = HEAD;
 				break;
 
 			}
@@ -638,7 +644,7 @@ TAP_ESC::cycle()
 		}
 
 		/* can we mix? */
-		if (IS_armed && _mixers != nullptr) {
+		if (_is_armed && _mixers != nullptr) {
 
 
 			/* do mixing */
@@ -735,9 +741,6 @@ TAP_ESC::cycle()
 		/* and publish for anyone that cares to see */
 		orb_publish(ORB_ID(actuator_outputs), _outputs_pub, &_outputs);
 
-
-		/* overwrite outputs in case of lockdown with disarmed PWM values */
-
 	}
 
 	bool updated;
@@ -747,7 +750,7 @@ TAP_ESC::cycle()
 	if (updated) {
 		orb_copy(ORB_ID(actuator_armed), _armed_sub, &_armed);
 		/* do not obey the lockdown value, as lockdown is for PWMSim */
-		IS_armed = _armed.armed;
+		_is_armed = _armed.armed;
 
 		/* reset all outputs */
 		for (size_t i = 0; i < sizeof(_outputs.output) / sizeof(_outputs.output[0]); i++) {

From 40450681874027ae9ed4e4923dc35137cc45d6ac Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Beat=20K=C3=BCng?= <beat-kueng@gmx.net>
Date: Mon, 25 Jul 2016 12:25:15 +0200
Subject: [PATCH 1097/1230] tap_esc: properly initialize _outputs

---
 src/drivers/tap_esc/tap_esc.cpp | 5 +++++
 1 file changed, 5 insertions(+)

diff --git a/src/drivers/tap_esc/tap_esc.cpp b/src/drivers/tap_esc/tap_esc.cpp
index da74012f57..3cecdbfe93 100644
--- a/src/drivers/tap_esc/tap_esc.cpp
+++ b/src/drivers/tap_esc/tap_esc.cpp
@@ -191,6 +191,11 @@ TAP_ESC::TAP_ESC(int channels_count):
 	uartbuf.tail = 0;
 	uartbuf.dat_cnt = 0;
 	memset(uartbuf.esc_feedback_buf, 0, sizeof(uartbuf.esc_feedback_buf));
+
+	for (size_t i = 0; i < sizeof(_outputs.output) / sizeof(_outputs.output[0]); i++) {
+		_outputs.output[i] = NAN;
+	}
+	_outputs.noutputs = 0;
 }
 
 TAP_ESC::~TAP_ESC()

From c55c978145ec8ee3a8a9c4b824de5f714aefbddd Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Beat=20K=C3=BCng?= <beat-kueng@gmx.net>
Date: Mon, 25 Jul 2016 12:25:45 +0200
Subject: [PATCH 1098/1230] tap_esc: reset outputs only when armed really
 changed (not on each topic update)

---
 src/drivers/tap_esc/tap_esc.cpp | 10 ++++++----
 1 file changed, 6 insertions(+), 4 deletions(-)

diff --git a/src/drivers/tap_esc/tap_esc.cpp b/src/drivers/tap_esc/tap_esc.cpp
index 3cecdbfe93..f13d6deb2e 100644
--- a/src/drivers/tap_esc/tap_esc.cpp
+++ b/src/drivers/tap_esc/tap_esc.cpp
@@ -754,13 +754,15 @@ TAP_ESC::cycle()
 
 	if (updated) {
 		orb_copy(ORB_ID(actuator_armed), _armed_sub, &_armed);
+		if (_is_armed != _armed.armed) {
+			/* reset all outputs */
+			for (size_t i = 0; i < sizeof(_outputs.output) / sizeof(_outputs.output[0]); i++) {
+				_outputs.output[i] = NAN;
+			}
+		}
 		/* do not obey the lockdown value, as lockdown is for PWMSim */
 		_is_armed = _armed.armed;
 
-		/* reset all outputs */
-		for (size_t i = 0; i < sizeof(_outputs.output) / sizeof(_outputs.output[0]); i++) {
-			_outputs.output[i] = NAN;
-		}
 	}
 
 

From d6d3a56116b066f192300733c76418abeef573d9 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Beat=20K=C3=BCng?= <beat-kueng@gmx.net>
Date: Mon, 25 Jul 2016 12:26:51 +0200
Subject: [PATCH 1099/1230] tap_esc: add buffer size check for
 _esc_feedback.esc[]

---
 src/drivers/tap_esc/tap_esc.cpp | 28 +++++++++++++++-------------
 1 file changed, 15 insertions(+), 13 deletions(-)

diff --git a/src/drivers/tap_esc/tap_esc.cpp b/src/drivers/tap_esc/tap_esc.cpp
index f13d6deb2e..1536ed2b99 100644
--- a/src/drivers/tap_esc/tap_esc.cpp
+++ b/src/drivers/tap_esc/tap_esc.cpp
@@ -445,7 +445,7 @@ void TAP_ESC:: send_esc_outputs(const float *pwm, const unsigned num_pwm)
 
 void TAP_ESC::read_data_from_uart()
 {
-	uint8_t tmp_serial_buf[UART_BUFFER_SIZE] = {0};
+	uint8_t tmp_serial_buf[UART_BUFFER_SIZE];
 
 	int len =::read(_uart_fd, tmp_serial_buf, arraySize(tmp_serial_buf));
 
@@ -518,7 +518,7 @@ bool TAP_ESC:: parse_tap_esc_feedback(ESC_UART_BUF *serial_buf, EscPacket *packe
 				if (crc_data_cal == serial_buf->esc_feedback_buf[serial_buf->head]) {
 					packetdata->crc_data = serial_buf->esc_feedback_buf[serial_buf->head];
 
-					if (++serial_buf->head >= 128) {
+					if (++serial_buf->head >= UART_BUFFER_SIZE) {
 						serial_buf->head = 0;
 					}
 
@@ -726,20 +726,22 @@ TAP_ESC::cycle()
 		if (parse_tap_esc_feedback(&uartbuf, &_packet) == true) {
 			if (_packet.msg_id == ESCBUS_MSG_ID_RUN_INFO) {
 				RunInfoRepsonse &feed_back_data = _packet.d.rspRunInfo;
-				_esc_feedback.esc[feed_back_data.channelID].esc_rpm = feed_back_data.speed;
-//				_esc_feedback.esc[feed_back_data.channelID].esc_voltage = feed_back_data.voltage;
-				_esc_feedback.esc[feed_back_data.channelID].esc_state = feed_back_data.ESCStatus;
-				_esc_feedback.esc[feed_back_data.channelID].esc_vendor = esc_status_s::ESC_VENDOR_TAP;
-				// printf("vol is %d\n",feed_back_data.voltage );
-				// printf("speed is %d\n",feed_back_data.speed );
+				if (feed_back_data.channelID < esc_status_s::CONNECTED_ESC_MAX) {
+					_esc_feedback.esc[feed_back_data.channelID].esc_rpm = feed_back_data.speed;
+//					_esc_feedback.esc[feed_back_data.channelID].esc_voltage = feed_back_data.voltage;
+					_esc_feedback.esc[feed_back_data.channelID].esc_state = feed_back_data.ESCStatus;
+					_esc_feedback.esc[feed_back_data.channelID].esc_vendor = esc_status_s::ESC_VENDOR_TAP;
+					// printf("vol is %d\n",feed_back_data.voltage );
+					// printf("speed is %d\n",feed_back_data.speed );
 
-				_esc_feedback.esc_connectiontype = esc_status_s::ESC_CONNECTION_TYPE_SERIAL;
-				_esc_feedback.counter++;
-				_esc_feedback.esc_count = esc_count;
+					_esc_feedback.esc_connectiontype = esc_status_s::ESC_CONNECTION_TYPE_SERIAL;
+					_esc_feedback.counter++;
+					_esc_feedback.esc_count = esc_count;
 
-				_esc_feedback.timestamp = hrt_absolute_time();
+					_esc_feedback.timestamp = hrt_absolute_time();
 
-				orb_publish(ORB_ID(esc_status), _esc_feedback_pub, &_esc_feedback);
+					orb_publish(ORB_ID(esc_status), _esc_feedback_pub, &_esc_feedback);
+				}
 			}
 		}
 

From d660fb094b44bdd3a6231d250dc462a050ae4cc9 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Beat=20K=C3=BCng?= <beat-kueng@gmx.net>
Date: Mon, 25 Jul 2016 12:28:31 +0200
Subject: [PATCH 1100/1230] tap_esc: add RPMSTOPPED macro, make sure driver
 starts with stopped motors

---
 src/drivers/tap_esc/drv_tap_esc.h |  1 +
 src/drivers/tap_esc/tap_esc.cpp   | 10 +++++-----
 2 files changed, 6 insertions(+), 5 deletions(-)

diff --git a/src/drivers/tap_esc/drv_tap_esc.h b/src/drivers/tap_esc/drv_tap_esc.h
index a84cdee4ac..abf210751b 100644
--- a/src/drivers/tap_esc/drv_tap_esc.h
+++ b/src/drivers/tap_esc/drv_tap_esc.h
@@ -78,6 +78,7 @@
 
 #define RPMMAX 1900
 #define RPMMIN 1200
+#define RPMSTOPPED (RPMMIN - 10)
 
 
 #define MAX_BOOT_TIME_MS		 (500) // Minimum time to wait after Power on before sending commands
diff --git a/src/drivers/tap_esc/tap_esc.cpp b/src/drivers/tap_esc/tap_esc.cpp
index 1536ed2b99..80c6f5c1f1 100644
--- a/src/drivers/tap_esc/tap_esc.cpp
+++ b/src/drivers/tap_esc/tap_esc.cpp
@@ -417,8 +417,8 @@ void TAP_ESC:: send_esc_outputs(const float *pwm, const unsigned num_pwm)
 		if (rpm[i] > RPMMAX) {
 			rpm[i] = RPMMAX;
 
-		} else if (rpm[i] < RPMMIN) {
-			rpm[i] = RPMMIN;
+		} else if (rpm[i] < RPMSTOPPED) {
+			rpm[i] = RPMSTOPPED;
 		}
 	}
 
@@ -678,7 +678,7 @@ TAP_ESC::cycle()
 					 * This will be clearly visible on the servo status and will limit the risk of accidentally
 					 * spinning motors. It would be deadly in flight.
 					 */
-					_outputs.output[i] = 900;
+					_outputs.output[i] = RPMSTOPPED;
 				}
 			}
 
@@ -694,7 +694,7 @@ TAP_ESC::cycle()
 			if (test_motor_updated) {
 				struct test_motor_s test_motor;
 				orb_copy(ORB_ID(test_motor), _test_motor_sub, &test_motor);
-				_outputs.output[test_motor.motor_number] = 900.f + (1100.f * test_motor.value); //scale to [900, 2000]
+				_outputs.output[test_motor.motor_number] = RPMSTOPPED + ((RPMMAX - RPMSTOPPED) * test_motor.value);
 				PX4_INFO("setting motor %i to %.1lf", test_motor.motor_number,
 					 (double)_outputs.output[test_motor.motor_number]);
 			}
@@ -702,7 +702,7 @@ TAP_ESC::cycle()
 			/* set the invalid values to the minimum */
 			for (unsigned i = 0; i < num_outputs; i++) {
 				if (!PX4_ISFINITE(_outputs.output[i])) {
-					_outputs.output[i] = 900;
+					_outputs.output[i] = RPMSTOPPED;
 				}
 			}
 

From 6245a9b1345187e71642df482e176559a1e86c21 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Beat=20K=C3=BCng?= <beat-kueng@gmx.net>
Date: Mon, 25 Jul 2016 12:53:34 +0200
Subject: [PATCH 1101/1230] motor_test: make -m & -p optional (select all
 motors/0 output if not given)

---
 src/systemcmds/motor_test/motor_test.c | 30 +++++++++++++++++---------
 1 file changed, 20 insertions(+), 10 deletions(-)

diff --git a/src/systemcmds/motor_test/motor_test.c b/src/systemcmds/motor_test/motor_test.c
index a1175c289f..76ec68d0d4 100644
--- a/src/systemcmds/motor_test/motor_test.c
+++ b/src/systemcmds/motor_test/motor_test.c
@@ -77,6 +77,8 @@ void motor_test(unsigned channel, float value)
 		/* advertise and publish */
 		_test_motor_pub = orb_advertise(ORB_ID(test_motor), &_test_motor);
 	}
+
+	printf("motor %d set to %.2f\n", channel, (double)value);
 }
 
 static void usage(const char *reason)
@@ -88,27 +90,24 @@ static void usage(const char *reason)
 	errx(1,
 	     "usage:\n"
 	     "motor_test\n"
-	     "    -m <channel>            Motor to test (0..7)\n"
-	     "    -p <power>              Power (0..100)\n");
+	     "    -m <channel>            Motor to test (0..7), all if -m not given\n"
+	     "    -p <power>              Power (0..100), 0 if -p not given\n"
+	     "motor_test stop             Stop all motors\n");
 }
 
 int motor_test_main(int argc, char *argv[])
 {
-	unsigned long channel = 0;
+	int channel = -1; //default to all channels
 	unsigned long lval;
 	float value = 0.0f;
 	int ch;
 
-	if (argc != 5) {
-		usage("please specify motor and power");
-	}
-
 	while ((ch = getopt(argc, argv, "m:p:")) != EOF) {
 		switch (ch) {
 
 		case 'm':
 			/* Read in motor number */
-			channel = strtoul(optarg, NULL, 0);
+			channel = (int)strtoul(optarg, NULL, 0);
 			break;
 
 		case 'p':
@@ -127,9 +126,20 @@ int motor_test_main(int argc, char *argv[])
 		}
 	}
 
-	motor_test(channel, value);
+	if (argc > 1 && strcmp("stop", argv[1]) == 0) {
+		channel = -1;
+		value = 0.f;
+	}
 
-	printf("motor %d set to %.2f\n", channel, (double)value);
+	if (channel == -1) {
+		for (int i = 0; i < 8; ++i) {
+			motor_test(i, value);
+			usleep(10000);
+		}
+
+	} else {
+		motor_test(channel, value);
+	}
 
 	exit(0);
 }

From cbcbce3a28d27fef82e4a83a87076a228b1db74a Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Beat=20K=C3=BCng?= <beat-kueng@gmx.net>
Date: Mon, 25 Jul 2016 12:55:03 +0200
Subject: [PATCH 1102/1230] tap_esc: use correct number of outputs

---
 src/drivers/tap_esc/tap_esc.cpp | 22 ++++++++++++----------
 1 file changed, 12 insertions(+), 10 deletions(-)

diff --git a/src/drivers/tap_esc/tap_esc.cpp b/src/drivers/tap_esc/tap_esc.cpp
index 80c6f5c1f1..ab69cfd1d2 100644
--- a/src/drivers/tap_esc/tap_esc.cpp
+++ b/src/drivers/tap_esc/tap_esc.cpp
@@ -103,14 +103,13 @@ private:
 	unsigned	_poll_fds_num;
 	Mode		_mode;
 	// subscriptions
-	unsigned	_num_outputs;
 	int	_armed_sub;
 	int _test_motor_sub;
 	orb_advert_t        	_outputs_pub = nullptr;
 	actuator_outputs_s      _outputs;
 	static actuator_armed_s	_armed;
 
-	//todo:refactor dynamic based on _num_outputs or _channels_count
+	//todo:refactor dynamic based on _channels_count
 	// It needs to support the numbe of ESC
 	int	_control_subs[actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS];
 
@@ -122,7 +121,7 @@ private:
 
 	orb_advert_t        _esc_feedback_pub = nullptr;
 	esc_status_s      _esc_feedback;
-	uint8_t           _channels_count; // The numbe of ESC channels
+	uint8_t           _channels_count; // The number of ESC channels
 
 	MixerGroup	*_mixers;
 	uint32_t	_groups_required;
@@ -165,7 +164,6 @@ TAP_ESC::TAP_ESC(int channels_count):
 	_is_armed(false),
 	_poll_fds_num(0),
 	_mode(MODE_4PWM),
-	_num_outputs(0),
 	_armed_sub(-1),
 	_test_motor_sub(-1),
 	_outputs_pub(nullptr),
@@ -195,6 +193,7 @@ TAP_ESC::TAP_ESC(int channels_count):
 	for (size_t i = 0; i < sizeof(_outputs.output) / sizeof(_outputs.output[0]); i++) {
 		_outputs.output[i] = NAN;
 	}
+
 	_outputs.noutputs = 0;
 }
 
@@ -713,19 +712,20 @@ TAP_ESC::cycle()
 
 		}
 
-		const unsigned esc_count = 4;
-
+		const unsigned esc_count = num_outputs;
 		float motor_out[TAP_ESC_MAX_MOTOR_NUM];
-		motor_out[0] = _outputs.output[0];
-		motor_out[1] = _outputs.output[1];
-		motor_out[2] = _outputs.output[1];
-		motor_out[3] = 900;
+
+		for (int i = 0; i < esc_count; ++i) {
+			motor_out[i] = _outputs.output[i];
+		}
+
 		send_esc_outputs(motor_out, esc_count);
 		read_data_from_uart();
 
 		if (parse_tap_esc_feedback(&uartbuf, &_packet) == true) {
 			if (_packet.msg_id == ESCBUS_MSG_ID_RUN_INFO) {
 				RunInfoRepsonse &feed_back_data = _packet.d.rspRunInfo;
+
 				if (feed_back_data.channelID < esc_status_s::CONNECTED_ESC_MAX) {
 					_esc_feedback.esc[feed_back_data.channelID].esc_rpm = feed_back_data.speed;
 //					_esc_feedback.esc[feed_back_data.channelID].esc_voltage = feed_back_data.voltage;
@@ -756,12 +756,14 @@ TAP_ESC::cycle()
 
 	if (updated) {
 		orb_copy(ORB_ID(actuator_armed), _armed_sub, &_armed);
+
 		if (_is_armed != _armed.armed) {
 			/* reset all outputs */
 			for (size_t i = 0; i < sizeof(_outputs.output) / sizeof(_outputs.output[0]); i++) {
 				_outputs.output[i] = NAN;
 			}
 		}
+
 		/* do not obey the lockdown value, as lockdown is for PWMSim */
 		_is_armed = _armed.armed;
 

From 3a0f4c84a516a6a5f9c71c532112015c3c9f3faa Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Beat=20K=C3=BCng?= <beat-kueng@gmx.net>
Date: Mon, 25 Jul 2016 13:26:17 +0200
Subject: [PATCH 1103/1230] motor_test: add iterate command

---
 src/systemcmds/motor_test/motor_test.c | 43 +++++++++++++++++++-------
 1 file changed, 31 insertions(+), 12 deletions(-)

diff --git a/src/systemcmds/motor_test/motor_test.c b/src/systemcmds/motor_test/motor_test.c
index 76ec68d0d4..258e38e1ef 100644
--- a/src/systemcmds/motor_test/motor_test.c
+++ b/src/systemcmds/motor_test/motor_test.c
@@ -75,7 +75,7 @@ void motor_test(unsigned channel, float value)
 
 	} else {
 		/* advertise and publish */
-		_test_motor_pub = orb_advertise(ORB_ID(test_motor), &_test_motor);
+		_test_motor_pub = orb_advertise_queue(ORB_ID(test_motor), &_test_motor, 4);
 	}
 
 	printf("motor %d set to %.2f\n", channel, (double)value);
@@ -92,7 +92,8 @@ static void usage(const char *reason)
 	     "motor_test\n"
 	     "    -m <channel>            Motor to test (0..7), all if -m not given\n"
 	     "    -p <power>              Power (0..100), 0 if -p not given\n"
-	     "motor_test stop             Stop all motors\n");
+	     "motor_test stop             Stop all motors\n"
+	     "motor_test iterate          Iterate all motors starting and stopping one after the other\n");
 }
 
 int motor_test_main(int argc, char *argv[])
@@ -126,19 +127,37 @@ int motor_test_main(int argc, char *argv[])
 		}
 	}
 
-	if (argc > 1 && strcmp("stop", argv[1]) == 0) {
-		channel = -1;
-		value = 0.f;
+	bool run_test = true;
+
+	if (argc > 1) {
+		if (strcmp("stop", argv[1]) == 0) {
+			channel = -1;
+			value = 0.f;
+
+		} else if (strcmp("iterate", argv[1]) == 0) {
+			value = 0.3f;
+
+			for (int i = 0; i < 8; ++i) {
+				motor_test(i, value);
+				usleep(500000);
+				motor_test(i, 0.f);
+				usleep(10000);
+			}
+
+			run_test = false;
+		}
 	}
 
-	if (channel == -1) {
-		for (int i = 0; i < 8; ++i) {
-			motor_test(i, value);
-			usleep(10000);
-		}
+	if (run_test) {
+		if (channel == -1) {
+			for (int i = 0; i < 8; ++i) {
+				motor_test(i, value);
+				usleep(10000);
+			}
 
-	} else {
-		motor_test(channel, value);
+		} else {
+			motor_test(channel, value);
+		}
 	}
 
 	exit(0);

From 50a14cfd29b98bad608d8faf9a1ce02ed6d5ef3d Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Beat=20K=C3=BCng?= <beat-kueng@gmx.net>
Date: Mon, 25 Jul 2016 14:46:06 +0200
Subject: [PATCH 1104/1230] tap_esc: disable the _mode param, use the number of
 configured channels instead

Not sure why the Mode enum exists in the first place...
---
 src/drivers/tap_esc/tap_esc.cpp | 7 +++++--
 1 file changed, 5 insertions(+), 2 deletions(-)

diff --git a/src/drivers/tap_esc/tap_esc.cpp b/src/drivers/tap_esc/tap_esc.cpp
index ab69cfd1d2..2fb3e0b2d4 100644
--- a/src/drivers/tap_esc/tap_esc.cpp
+++ b/src/drivers/tap_esc/tap_esc.cpp
@@ -163,7 +163,7 @@ TAP_ESC::TAP_ESC(int channels_count):
 	CDev("tap_esc", TAP_ESC_DEVICE_PATH),
 	_is_armed(false),
 	_poll_fds_num(0),
-	_mode(MODE_4PWM),
+	_mode(MODE_4PWM), //FIXME: what is this mode used for???
 	_armed_sub(-1),
 	_test_motor_sub(-1),
 	_outputs_pub(nullptr),
@@ -623,8 +623,10 @@ TAP_ESC::cycle()
 			}
 		}
 
-		size_t num_outputs = 0;
+		size_t num_outputs = _channels_count;
 
+		/*
+		// FIXME: don't know what this mode should be used for. It's hardcoded in initialization and never changed.
 		switch (_mode) {
 		case MODE_2PWM:
 			num_outputs = 2;
@@ -646,6 +648,7 @@ TAP_ESC::cycle()
 			num_outputs = 0;
 			break;
 		}
+		*/
 
 		/* can we mix? */
 		if (_is_armed && _mixers != nullptr) {

From 415fbae31e27fe81c7f70fafe615c6046f0a9540 Mon Sep 17 00:00:00 2001
From: David Sidrane <david_s5@nscdg.com>
Date: Wed, 27 Jul 2016 04:33:28 -1000
Subject: [PATCH 1105/1230] Fix build for HW w/o mux

---
 src/drivers/tap_esc/tap_esc.cpp | 3 ++-
 1 file changed, 2 insertions(+), 1 deletion(-)

diff --git a/src/drivers/tap_esc/tap_esc.cpp b/src/drivers/tap_esc/tap_esc.cpp
index 2fb3e0b2d4..d2afacd36c 100644
--- a/src/drivers/tap_esc/tap_esc.cpp
+++ b/src/drivers/tap_esc/tap_esc.cpp
@@ -395,10 +395,11 @@ uint8_t TAP_ESC::crc_packet(EscPacket &p)
 }
 void TAP_ESC::select_responder(uint8_t sel)
 {
+#if defined(GPIO_S0)
 	px4_arch_gpiowrite(GPIO_S0, sel & 1);
 	px4_arch_gpiowrite(GPIO_S1, sel & 2);
 	px4_arch_gpiowrite(GPIO_S2, sel & 4);
-
+#endif
 }
 
 

From 0cf77a30b1dc4c3ae488e08562d7255ae7dfda82 Mon Sep 17 00:00:00 2001
From: sander <sander@droneslab.com>
Date: Fri, 22 Jul 2016 16:21:32 +0200
Subject: [PATCH 1106/1230] Init tecs with transition airspeed for vtols
 without airspeed sensor

---
 src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp | 8 +++++++-
 1 file changed, 7 insertions(+), 1 deletion(-)

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 dc6e75f63f..4107d17ae8 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
@@ -280,6 +280,7 @@ private:
 		float airspeed_min;
 		float airspeed_trim;
 		float airspeed_max;
+		float airspeed_trans;
 
 		float pitch_limit_min;
 		float pitch_limit_max;
@@ -336,6 +337,7 @@ private:
 		param_t airspeed_min;
 		param_t airspeed_trim;
 		param_t airspeed_max;
+		param_t airspeed_trans;
 
 		param_t pitch_limit_min;
 		param_t pitch_limit_max;
@@ -615,6 +617,7 @@ FixedwingPositionControl::FixedwingPositionControl() :
 	_parameter_handles.airspeed_min = param_find("FW_AIRSPD_MIN");
 	_parameter_handles.airspeed_trim = param_find("FW_AIRSPD_TRIM");
 	_parameter_handles.airspeed_max = param_find("FW_AIRSPD_MAX");
+	_parameter_handles.airspeed_trans = param_find("VT_ARSP_TRANS");
 
 	_parameter_handles.pitch_limit_min = param_find("FW_P_LIM_MIN");
 	_parameter_handles.pitch_limit_max = param_find("FW_P_LIM_MAX");
@@ -699,6 +702,7 @@ FixedwingPositionControl::parameters_update()
 	param_get(_parameter_handles.airspeed_min, &(_parameters.airspeed_min));
 	param_get(_parameter_handles.airspeed_trim, &(_parameters.airspeed_trim));
 	param_get(_parameter_handles.airspeed_max, &(_parameters.airspeed_max));
+	param_get(_parameter_handles.airspeed_trans, &(_parameters.airspeed_trans));
 
 	param_get(_parameter_handles.pitch_limit_min, &(_parameters.pitch_limit_min));
 	param_get(_parameter_handles.pitch_limit_max, &(_parameters.pitch_limit_max));
@@ -2334,7 +2338,9 @@ void FixedwingPositionControl::tecs_update_pitch_throttle(float alt_sp, float v_
 	// we're in transition
 	if (_vehicle_status.is_vtol && _vehicle_status.in_transition_mode) {
 		_was_in_transition = true;
-		_asp_after_transition = _ctrl_state.airspeed;
+		// set this to transition airspeed to init tecs correctly
+		// some vtols fly without airspeed sensor
+		_asp_after_transition = _parameters.airspeed_trans;
 
 	} else if (_was_in_transition) {
 		// after transition we ramp up desired airspeed from the speed we had coming out of the transition

From ce3749d04fb11ec0407c0ae301e356df4845f9ea Mon Sep 17 00:00:00 2001
From: sander <sander@droneslab.com>
Date: Sat, 23 Jul 2016 22:52:24 +0200
Subject: [PATCH 1107/1230] Make tecs init airspeed mode dependent

---
 .../fw_pos_control_l1/fw_pos_control_l1_main.cpp    | 13 +++++++++++--
 1 file changed, 11 insertions(+), 2 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 4107d17ae8..4894f4cbed 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
@@ -281,6 +281,7 @@ private:
 		float airspeed_trim;
 		float airspeed_max;
 		float airspeed_trans;
+		int	airspeed_mode;
 
 		float pitch_limit_min;
 		float pitch_limit_max;
@@ -338,6 +339,7 @@ private:
 		param_t airspeed_trim;
 		param_t airspeed_max;
 		param_t airspeed_trans;
+		param_t airspeed_mode;
 
 		param_t pitch_limit_min;
 		param_t pitch_limit_max;
@@ -618,6 +620,7 @@ FixedwingPositionControl::FixedwingPositionControl() :
 	_parameter_handles.airspeed_trim = param_find("FW_AIRSPD_TRIM");
 	_parameter_handles.airspeed_max = param_find("FW_AIRSPD_MAX");
 	_parameter_handles.airspeed_trans = param_find("VT_ARSP_TRANS");
+	_parameter_handles.airspeed_mode = param_find("FW_ARSP_MODE");
 
 	_parameter_handles.pitch_limit_min = param_find("FW_P_LIM_MIN");
 	_parameter_handles.pitch_limit_max = param_find("FW_P_LIM_MAX");
@@ -703,6 +706,7 @@ FixedwingPositionControl::parameters_update()
 	param_get(_parameter_handles.airspeed_trim, &(_parameters.airspeed_trim));
 	param_get(_parameter_handles.airspeed_max, &(_parameters.airspeed_max));
 	param_get(_parameter_handles.airspeed_trans, &(_parameters.airspeed_trans));
+	param_get(_parameter_handles.airspeed_mode, &(_parameters.airspeed_mode));
 
 	param_get(_parameter_handles.pitch_limit_min, &(_parameters.pitch_limit_min));
 	param_get(_parameter_handles.pitch_limit_max, &(_parameters.pitch_limit_max));
@@ -2339,8 +2343,13 @@ void FixedwingPositionControl::tecs_update_pitch_throttle(float alt_sp, float v_
 	if (_vehicle_status.is_vtol && _vehicle_status.in_transition_mode) {
 		_was_in_transition = true;
 		// set this to transition airspeed to init tecs correctly
-		// some vtols fly without airspeed sensor
-		_asp_after_transition = _parameters.airspeed_trans;
+		if(_parameters.airspeed_mode == control_state_s::AIRSPD_MODE_DISABLED){
+			// some vtols fly without airspeed sensor
+			_asp_after_transition = _parameters.airspeed_trans;
+		} else {
+			_asp_after_transition = _ctrl_state.airspeed;
+		}
+		_asp_after_transition = math::constrain(_asp_after_transition, _parameters.airspeed_min, _parameters.airspeed_max);
 
 	} else if (_was_in_transition) {
 		// after transition we ramp up desired airspeed from the speed we had coming out of the transition

From 0ce08ed9f863139edb2d42e776ae59e5372f8be7 Mon Sep 17 00:00:00 2001
From: sander <sander@droneslab.com>
Date: Sun, 24 Jul 2016 00:18:01 +0200
Subject: [PATCH 1108/1230] Code style

---
 src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp | 7 +++++--
 1 file changed, 5 insertions(+), 2 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 4894f4cbed..a3b03fedc2 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
@@ -281,7 +281,7 @@ private:
 		float airspeed_trim;
 		float airspeed_max;
 		float airspeed_trans;
-		int	airspeed_mode;
+		int airspeed_mode;
 
 		float pitch_limit_min;
 		float pitch_limit_max;
@@ -2342,13 +2342,16 @@ void FixedwingPositionControl::tecs_update_pitch_throttle(float alt_sp, float v_
 	// we're in transition
 	if (_vehicle_status.is_vtol && _vehicle_status.in_transition_mode) {
 		_was_in_transition = true;
+
 		// set this to transition airspeed to init tecs correctly
-		if(_parameters.airspeed_mode == control_state_s::AIRSPD_MODE_DISABLED){
+		if (_parameters.airspeed_mode == control_state_s::AIRSPD_MODE_DISABLED) {
 			// some vtols fly without airspeed sensor
 			_asp_after_transition = _parameters.airspeed_trans;
+
 		} else {
 			_asp_after_transition = _ctrl_state.airspeed;
 		}
+
 		_asp_after_transition = math::constrain(_asp_after_transition, _parameters.airspeed_min, _parameters.airspeed_max);
 
 	} else if (_was_in_transition) {

From 4b414d96eb9f8ae5f47d6d181770547c284cbfd3 Mon Sep 17 00:00:00 2001
From: sander <sander@droneslab.com>
Date: Wed, 27 Jul 2016 00:36:14 +0200
Subject: [PATCH 1109/1230] Init _mission_throttle

---
 src/modules/navigator/navigator_main.cpp | 3 ++-
 1 file changed, 2 insertions(+), 1 deletion(-)

diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp
index c3e606da0d..cb7ce3284a 100644
--- a/src/modules/navigator/navigator_main.cpp
+++ b/src/modules/navigator/navigator_main.cpp
@@ -160,7 +160,8 @@ Navigator::Navigator() :
 	_param_cruising_speed_hover(this, "MPC_XY_CRUISE", false),
 	_param_cruising_speed_plane(this, "FW_AIRSPD_TRIM", false),
 	_param_cruising_throttle_plane(this, "FW_THR_CRUISE", false),
-	_mission_cruising_speed(-1.0f)
+	_mission_cruising_speed(-1.0f),
+	_mission_throttle(-1.0f)
 {
 	/* Create a list of our possible navigation types */
 	_navigation_mode_array[0] = &_mission;

From 9f5b081257a1fc71e05a18d4809c152f7a344e7c Mon Sep 17 00:00:00 2001
From: Eric Ye <ericye16@gmail.com>
Date: Thu, 28 Jul 2016 14:47:57 -0700
Subject: [PATCH 1110/1230] Wrap help line to fit through mini-dm. (#5157)

It currently gets cut off on mini-dm, split it into multiple lines
so all of it comes through.
---
 src/drivers/gps/gps.cpp | 3 ++-
 1 file changed, 2 insertions(+), 1 deletion(-)

diff --git a/src/drivers/gps/gps.cpp b/src/drivers/gps/gps.cpp
index bd3eb10807..4640be99e3 100644
--- a/src/drivers/gps/gps.cpp
+++ b/src/drivers/gps/gps.cpp
@@ -1135,6 +1135,7 @@ gps_main(int argc, char *argv[])
 	return 0;
 
 out:
-	PX4_ERR("unrecognized command, try 'start', 'stop', 'test', 'reset' or 'status'\n [-d /dev/ttyS0-n][-f (for enabling fake)][-s (to enable sat info)]");
+	PX4_ERR("unrecognized command, try 'start', 'stop', 'test', 'reset' or 'status'");
+	PX4_ERR("[-d " GPS_DEFAULT_UART_PORT "][-f (for enabling fake)][-s (to enable sat info)]");
 	return 1;
 }

From 3b7ba915a8c7e669a63287e0f42a5eefd0095f40 Mon Sep 17 00:00:00 2001
From: Sebastian Quilter <s.quilter@aol.com>
Date: Thu, 28 Jul 2016 14:49:37 -0700
Subject: [PATCH 1111/1230] small refactor (#5155)

---
 src/modules/mavlink/mavlink_messages.cpp | 4 +---
 1 file changed, 1 insertion(+), 3 deletions(-)

diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp
index 86d7ba395f..a2c27610f3 100644
--- a/src/modules/mavlink/mavlink_messages.cpp
+++ b/src/modules/mavlink/mavlink_messages.cpp
@@ -227,9 +227,7 @@ void get_mavlink_mode_state(struct vehicle_status_s *status, uint8_t *mavlink_st
 			break;
 
 		case vehicle_status_s::NAVIGATION_STATE_OFFBOARD:
-			*mavlink_base_mode	|= MAV_MODE_FLAG_AUTO_ENABLED
-								| MAV_MODE_FLAG_STABILIZE_ENABLED
-								| MAV_MODE_FLAG_GUIDED_ENABLED;
+			*mavlink_base_mode |= auto_mode_flags;
 			custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_OFFBOARD;
 			break;
 

From b6a9ff756c69ea5ddae3479d4d16f0692577a589 Mon Sep 17 00:00:00 2001
From: James Goppert <james.goppert@gmail.com>
Date: Thu, 28 Jul 2016 09:52:46 -0400
Subject: [PATCH 1112/1230] Rename mainapp to px4.

---
 Documentation/px4_hil/UserGuide.md            |  4 +-
 Tools/decode_backtrace.py                     | 12 +++---
 Tools/sitl_multiple_run.sh                    |  4 +-
 Tools/sitl_run.sh                             | 12 +++---
 cmake/test/px4_simple_app_correct.txt         |  2 +-
 posix-configs/SITL/README.md                  |  4 +-
 .../bebop/{mainapp.config => px4.config}      |  0
 .../rpi/{mainapp.config => px4.config}        |  0
 src/firmware/posix/CMakeLists.txt             | 42 +++++++++----------
 src/firmware/qurt/CMakeLists.txt              | 10 ++---
 src/platforms/posix/main.cpp                  |  6 +--
 src/platforms/qurt/px4_layer/main.cpp         |  2 +-
 12 files changed, 49 insertions(+), 49 deletions(-)
 rename posix-configs/bebop/{mainapp.config => px4.config} (100%)
 rename posix-configs/rpi/{mainapp.config => px4.config} (100%)

diff --git a/Documentation/px4_hil/UserGuide.md b/Documentation/px4_hil/UserGuide.md
index 28483498da..30da6d453a 100644
--- a/Documentation/px4_hil/UserGuide.md
+++ b/Documentation/px4_hil/UserGuide.md
@@ -66,8 +66,8 @@ Linux or Eagle with a working IP interface (?? does this need further instructio
 > adb shell
 # bash
 root@linaro-developer:/# cd ???
-root@linaro-developer:/# ./mainapp
-App name: mainapp
+root@linaro-developer:/# ./px4
+App name: px4
 Enter a command and its args:
 uorb start
 muorb start
diff --git a/Tools/decode_backtrace.py b/Tools/decode_backtrace.py
index 62a9c16b46..64f8a8ea30 100755
--- a/Tools/decode_backtrace.py
+++ b/Tools/decode_backtrace.py
@@ -43,10 +43,10 @@ import subprocess
 # If the following lines were pasted into the shell after running decode_backtrace.py
 #
 # INFO  Backtrace: 10
-# INFO  ./mainapp(px4_backtrace+0x27) [0x42b212]
-# INFO  ./mainapp() [0x42d608]
-# INFO  ./mainapp() [0x42d57e]
-# INFO  ./mainapp() [0x4ba48d]
+# INFO  ./px4(px4_backtrace+0x27) [0x42b212]
+# INFO  ./px4() [0x42d608]
+# INFO  ./px4() [0x42d57e]
+# INFO  ./px4() [0x4ba48d]
 #
 # The output would be:
 #
@@ -63,7 +63,7 @@ def usage():
 	msg = """
 Usage: Tools/decode_backtrace.py <builddir>
 
-This will load the symbols for <builddir>/src/firmware/posix/mainapp
+This will load the symbols for <builddir>/src/firmware/posix/px4
 The user just needs to copy and paste the backtrace into the terminal
 where decode_backtrace.py is running.
 
@@ -75,7 +75,7 @@ func = []
 
 # Load the symbols from the binary
 def load_symbol_map():
-	output = subprocess.check_output(["nm", "-p", "-C", os.sys.argv[1]+"/src/firmware/posix/mainapp"])
+	output = subprocess.check_output(["nm", "-p", "-C", os.sys.argv[1]+"/src/firmware/posix/px4"])
 	data = output.split("\n")
 	data.sort()
 
diff --git a/Tools/sitl_multiple_run.sh b/Tools/sitl_multiple_run.sh
index 1378931899..f306679edd 100755
--- a/Tools/sitl_multiple_run.sh
+++ b/Tools/sitl_multiple_run.sh
@@ -16,7 +16,7 @@ src_path=`pwd`
 rc_script="posix-configs/SITL/init/rcS_multiple"
 build_path=${src_path}/build_posix_sitl_default
 
-pkill mainapp
+pkill px4
 sleep 2
 
 cd $build_path/src/firmware/posix
@@ -39,7 +39,7 @@ while [ $n -le $sitl_num ]; do
 
  cd $n
 
- sudo -b -u $user ../mainapp -d rcS >out.log 2>err.log
+ sudo -b -u $user ../px4 -d rcS >out.log 2>err.log
 
  cd ../
 
diff --git a/Tools/sitl_run.sh b/Tools/sitl_run.sh
index fbb93c86fa..6e467c31fd 100755
--- a/Tools/sitl_run.sh
+++ b/Tools/sitl_run.sh
@@ -43,7 +43,7 @@ fi
 # kill process names that might stil
 # be running from last time
 pkill gazebo
-pkill mainapp
+pkill px4
 jmavsim_pid=`jps | grep Simulator | cut -d" " -f1`
 if [ -n "$jmavsim_pid" ]
 then
@@ -117,18 +117,18 @@ set +e
 # Start Java simulator
 if [ "$debugger" == "lldb" ]
 then
-	lldb -- mainapp ../../../../${rc_script}_${program}_${model}
+	lldb -- px4 ../../../../${rc_script}_${program}_${model}
 elif [ "$debugger" == "gdb" ]
 then
-	gdb --args mainapp ../../../../${rc_script}_${program}_${model}
+	gdb --args px4 ../../../../${rc_script}_${program}_${model}
 elif [ "$debugger" == "ddd" ]
 then
-	ddd --debugger gdb --args mainapp ../../../../${rc_script}_${program}_${model}
+	ddd --debugger gdb --args px4 ../../../../${rc_script}_${program}_${model}
 elif [ "$debugger" == "valgrind" ]
 then
-	valgrind ./mainapp ../../../../${rc_script}_${program}_${model}
+	valgrind ./px4 ../../../../${rc_script}_${program}_${model}
 else
-	$sudo_enabled ./mainapp $chroot_enabled ../../../../${rc_script}_${program}_${model}
+	$sudo_enabled ./px4 $chroot_enabled ../../../../${rc_script}_${program}_${model}
 fi
 
 if [ "$program" == "jmavsim" ]
diff --git a/cmake/test/px4_simple_app_correct.txt b/cmake/test/px4_simple_app_correct.txt
index 317d4f5982..9504a8e27a 100644
--- a/cmake/test/px4_simple_app_correct.txt
+++ b/cmake/test/px4_simple_app_correct.txt
@@ -2,7 +2,7 @@ INFO  Shell id is 47996278451456
 WARN  1          starting task wkr_high (file /home/jgoppert/git/px4/cmake-Firmware/src/platforms/posix/px4_layer/px4_posix_tasks.cpp line 146)
 WARN  54         starting task wkr_low (file /home/jgoppert/git/px4/cmake-Firmware/src/platforms/posix/px4_layer/px4_posix_tasks.cpp line 146)
 WARN  100        starting task wkr_hrt (file /home/jgoppert/git/px4/cmake-Firmware/src/platforms/posix/px4_layer/px4_posix_tasks.cpp line 146)
-App name: mainapp
+App name: px4
 Enter a command and its args:
 ----------------------------------
 Running: uorb
diff --git a/posix-configs/SITL/README.md b/posix-configs/SITL/README.md
index 20ed19fd4a..2588c4b09b 100644
--- a/posix-configs/SITL/README.md
+++ b/posix-configs/SITL/README.md
@@ -39,12 +39,12 @@ NOTE: This is only necessary if you are not using the instructions above.
 
 1. Launch PX4 from the SITL build directory "`./Firmware/Build/posix_sitl.build/`" using the command below.  The optional `<startup_file>` can be used to cause a set of commands to be entered into the PX4 shell at startup.  
 ```
-> ./mainapp <startup_file> 
+> ./px4 <startup_file> 
 ```
 
 There is a sample startup script at `posix_configs/SITL/init/rcS`. 
 ```
-> ./mainapp ../../posix-configs/SITL/init/rcS
+> ./px4 ../../posix-configs/SITL/init/rcS
 ```
 
 Without the `<startup_file>`, the commands can be entered at the shell prompt one at a time.  An example startup file is given below.  This example shows that the "mavlink" module has selected port 14556 for its socket server (as shown in the SITL diagram) and will listen for connections on this port.
diff --git a/posix-configs/bebop/mainapp.config b/posix-configs/bebop/px4.config
similarity index 100%
rename from posix-configs/bebop/mainapp.config
rename to posix-configs/bebop/px4.config
diff --git a/posix-configs/rpi/mainapp.config b/posix-configs/rpi/px4.config
similarity index 100%
rename from posix-configs/rpi/mainapp.config
rename to posix-configs/rpi/px4.config
diff --git a/src/firmware/posix/CMakeLists.txt b/src/firmware/posix/CMakeLists.txt
index 6a43db5c11..2cb46d1ce0 100644
--- a/src/firmware/posix/CMakeLists.txt
+++ b/src/firmware/posix/CMakeLists.txt
@@ -13,7 +13,7 @@ if ("${BOARD}" STREQUAL "eagle" OR ("${BOARD}" STREQUAL "excelsior"))
 
 	set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -Wno-missing-prototypes -Wno-missing-declarations")
 	LINUX_APP(
-		APP_NAME mainapp
+		APP_NAME px4
 		IDL_NAME px4muorb
 		APPS_DEST "/home/linaro"
 		SOURCES
@@ -32,19 +32,19 @@ if ("${BOARD}" STREQUAL "eagle" OR ("${BOARD}" STREQUAL "excelsior"))
 	px4_add_adb_push(OUT upload
 			 OS ${OS}
 			 BOARD ${BOARD}
-			 FILES ${CMAKE_CURRENT_BINARY_DIR}/mainapp
-				 ${CMAKE_SOURCE_DIR}/posix-configs/eagle/flight/mainapp.config
-			 DEPENDS mainapp
+			 FILES ${CMAKE_CURRENT_BINARY_DIR}/px4
+				 ${CMAKE_SOURCE_DIR}/posix-configs/eagle/flight/px4.config
+			 DEPENDS px4
 			 DEST /home/linaro)
 
 elseif ("${BOARD}" STREQUAL "rpi")
 
-	add_executable(mainapp
+	add_executable(px4
 		${CMAKE_SOURCE_DIR}/src/platforms/posix/main.cpp
 		apps.h
 		)
 
-	target_link_libraries(mainapp
+	target_link_libraries(px4
 		-Wl,--start-group
 		${module_libraries}
 		df_driver_framework
@@ -56,9 +56,9 @@ elseif ("${BOARD}" STREQUAL "rpi")
 	px4_add_scp_push(OUT upload
 			 OS ${OS}
 			 BOARD ${BOARD}
-			 FILES ${CMAKE_CURRENT_BINARY_DIR}/mainapp
-				 ${CMAKE_SOURCE_DIR}/posix-configs/rpi/mainapp.config
-			 DEPENDS mainapp
+			 FILES ${CMAKE_CURRENT_BINARY_DIR}/px4
+				 ${CMAKE_SOURCE_DIR}/posix-configs/rpi/px4.config
+			 DEPENDS px4
 			 DEST /home/pi)
 
 elseif ("${BOARD}" STREQUAL "bebop")
@@ -66,13 +66,13 @@ elseif ("${BOARD}" STREQUAL "bebop")
 	set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -static")
 	set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -static")
 
-	add_executable(mainapp
+	add_executable(px4
 		${CMAKE_SOURCE_DIR}/src/platforms/posix/main.cpp
 		apps.h
 		)
 
 	if (NOT APPLE)
-		target_link_libraries(mainapp
+		target_link_libraries(px4
 			-Wl,--start-group
 			${module_libraries}
 			${df_driver_libs}
@@ -80,7 +80,7 @@ elseif ("${BOARD}" STREQUAL "bebop")
 			-Wl,--end-group
 			)
 	else()
-		target_link_libraries(mainapp
+		target_link_libraries(px4
 			${module_libraries}
 			${df_driver_libs}
 			pthread m
@@ -90,19 +90,19 @@ elseif ("${BOARD}" STREQUAL "bebop")
 	px4_add_adb_push_to_bebop(OUT upload
 			 OS ${OS}
 			 BOARD ${BOARD}
-			 FILES ${CMAKE_CURRENT_BINARY_DIR}/mainapp
-						 ${CMAKE_SOURCE_DIR}/posix-configs/bebop/mainapp.config
-			 DEPENDS mainapp
+			 FILES ${CMAKE_CURRENT_BINARY_DIR}/px4
+						 ${CMAKE_SOURCE_DIR}/posix-configs/bebop/px4.config
+			 DEPENDS px4
 			 DEST /usr/bin)
 			 
 else()
 
-	add_executable(mainapp
+	add_executable(px4
 		${CMAKE_SOURCE_DIR}/src/platforms/posix/main.cpp
 		apps.h
 		)
 	if (NOT APPLE)
-		target_link_libraries(mainapp
+		target_link_libraries(px4
 			-Wl,--start-group
 			${module_libraries}
 			${df_driver_libs}
@@ -110,7 +110,7 @@ else()
 			-Wl,--end-group
 			)
 	else()
-		target_link_libraries(mainapp
+		target_link_libraries(px4
 			${module_libraries}
 			${df_driver_libs}
 			pthread m
@@ -124,7 +124,7 @@ add_custom_target(run_config
 	WORKING_DIRECTORY ${CMAKE_SOURCE_DIR}
 	USES_TERMINAL
 	)
-add_dependencies(run_config mainapp)
+add_dependencies(run_config px4)
 
 foreach(viewer none jmavsim gazebo replay)
 	foreach(debugger none gdb lldb ddd valgrind)
@@ -149,7 +149,7 @@ foreach(viewer none jmavsim gazebo replay)
 				WORKING_DIRECTORY ${CMAKE_SOURCE_DIR}
 				USES_TERMINAL
 				)
-			add_dependencies(${_targ_name} mainapp)
+			add_dependencies(${_targ_name} px4)
 		endforeach()
 	endforeach()
 endforeach()
@@ -158,7 +158,7 @@ endforeach()
 # install
 #
 
-install(TARGETS mainapp DESTINATION bin)
+install(TARGETS px4 DESTINATION bin)
 install(DIRECTORY ${PROJECT_SOURCE_DIR}/ROMFS DESTINATION ${CMAKE_INSTALL_DATADIR}/${PROJECT_NAME})
 install(DIRECTORY ${PROJECT_SOURCE_DIR}/posix-configs DESTINATION ${CMAKE_INSTALL_DATADIR}/${PROJECT_NAME})
 
diff --git a/src/firmware/qurt/CMakeLists.txt b/src/firmware/qurt/CMakeLists.txt
index bacebe70ef..81be14afc5 100644
--- a/src/firmware/qurt/CMakeLists.txt
+++ b/src/firmware/qurt/CMakeLists.txt
@@ -19,11 +19,11 @@ if ("${QURT_ENABLE_STUBS}" STREQUAL "1")
                 ${CMAKE_CURRENT_BINARY_DIR}
                 ${FASTRPC_DSP_INCLUDES}
                 )
-	add_executable(mainapp
+	add_executable(px4
 		${CMAKE_BINARY_DIR}/src/firmware/qurt/px4muorb_skel.c
 		${CMAKE_BINARY_DIR}/apps.h)
 
-	target_link_libraries(mainapp
+	target_link_libraries(px4
 		-Wl,--start-group
 		${module_libraries}
                 ${target_libraries}
@@ -37,7 +37,7 @@ else()
 	message("module_external_libraries = ${module_external_libraries}")
 	# Generate the DSP lib and stubs but not the apps side executable
 	# The Apps side executable is generated via the posix_eagle_xxxx target
-	QURT_LIB(LIB_NAME mainapp
+	QURT_LIB(LIB_NAME px4
 		IDL_NAME px4muorb
 		SOURCES
 			${CMAKE_BINARY_DIR}/apps.h
@@ -52,10 +52,10 @@ else()
 	px4_add_adb_push(OUT upload
 			 OS ${OS}
 			 BOARD ${BOARD}
-			 FILES ${CMAKE_CURRENT_BINARY_DIR}/libmainapp.so
+			 FILES ${CMAKE_CURRENT_BINARY_DIR}/libpx4.so
 				 ${CMAKE_CURRENT_BINARY_DIR}/libpx4muorb_skel.so
 				 ${CMAKE_SOURCE_DIR}/posix-configs/eagle/flight/px4.config
-			 DEPENDS mainapp px4muorb_skel
+			 DEPENDS px4 px4muorb_skel
 			 DEST /usr/share/data/adsp)
 endif()
 
diff --git a/src/platforms/posix/main.cpp b/src/platforms/posix/main.cpp
index 34c879419f..646b94610f 100644
--- a/src/platforms/posix/main.cpp
+++ b/src/platforms/posix/main.cpp
@@ -134,10 +134,10 @@ static void run_cmd(const vector<string> &appargs, bool exit_on_fail, bool silen
 static void usage()
 {
 
-	cout << "./mainapp [-d] [startup_config] -h" << std::endl;
+	cout << "./px4 [-d] [startup_config] -h" << std::endl;
 	cout << "   -d            - Optional flag to run the app in daemon mode and does not listen for user input." <<
 	     std::endl;
-	cout << "                   This is needed if mainapp is intended to be run as a upstart job on linux" << std::endl;
+	cout << "                   This is needed if px4 is intended to be run as a upstart job on linux" << std::endl;
 	cout << "<startup_config> - config file for starting/stopping px4 modules" << std::endl;
 	cout << "   -h            - help/usage information" << std::endl;
 }
@@ -243,7 +243,7 @@ int main(int argc, char **argv)
 	DriverFramework::Framework::initialize();
 	px4::init_once();
 
-	px4::init(argc, argv, "mainapp");
+	px4::init(argc, argv, "px4");
 
 	// if commandfile is present, process the commands from the file
 	if (commands_file != nullptr) {
diff --git a/src/platforms/qurt/px4_layer/main.cpp b/src/platforms/qurt/px4_layer/main.cpp
index 0e6953afdf..39006cc1f1 100644
--- a/src/platforms/qurt/px4_layer/main.cpp
+++ b/src/platforms/qurt/px4_layer/main.cpp
@@ -216,7 +216,7 @@ int dspal_entry(int argc, char *argv[])
 	init_app_map(apps);
 	DriverFramework::Framework::initialize();
 	px4::init_once();
-	px4::init(argc, (char **)argv, "mainapp");
+	px4::init(argc, (char **)argv, "px4");
 	process_commands(apps, get_commands());
 	sleep(1); // give time for all commands to execute before starting external function
 

From 55d21242fe1bc5274a6078f205da938d48e4a26a Mon Sep 17 00:00:00 2001
From: Julian Oes <julian@oes.ch>
Date: Fri, 29 Jul 2016 11:15:10 +0200
Subject: [PATCH 1113/1230] Snapdragon: keep copying mainapp.config for now

mainapp.config can't be renamed to px4.config just yet because it would
clash with the name used for the DSP side.
---
 src/firmware/posix/CMakeLists.txt | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/src/firmware/posix/CMakeLists.txt b/src/firmware/posix/CMakeLists.txt
index 2cb46d1ce0..3e8e8b5227 100644
--- a/src/firmware/posix/CMakeLists.txt
+++ b/src/firmware/posix/CMakeLists.txt
@@ -33,7 +33,7 @@ if ("${BOARD}" STREQUAL "eagle" OR ("${BOARD}" STREQUAL "excelsior"))
 			 OS ${OS}
 			 BOARD ${BOARD}
 			 FILES ${CMAKE_CURRENT_BINARY_DIR}/px4
-				 ${CMAKE_SOURCE_DIR}/posix-configs/eagle/flight/px4.config
+				 ${CMAKE_SOURCE_DIR}/posix-configs/eagle/flight/mainapp.config
 			 DEPENDS px4
 			 DEST /home/linaro)
 
@@ -94,7 +94,7 @@ elseif ("${BOARD}" STREQUAL "bebop")
 						 ${CMAKE_SOURCE_DIR}/posix-configs/bebop/px4.config
 			 DEPENDS px4
 			 DEST /usr/bin)
-			 
+
 else()
 
 	add_executable(px4

From a9a5f3a19ecc2a85cfc87d4a95e8f7c9951c5645 Mon Sep 17 00:00:00 2001
From: Julian Oes <julian@oes.ch>
Date: Wed, 13 Jul 2016 14:37:01 +0200
Subject: [PATCH 1114/1230] px4io: add FMU fail test mode

In order to test what happens in px4iofirmware when the FMU stops
sending PWM or control commands, I added a test mode. When the test mode
is activated, no controls are sent.
---
 src/drivers/px4io/px4io.cpp | 62 ++++++++++++++++++++++++++++++++-----
 1 file changed, 55 insertions(+), 7 deletions(-)

diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp
index e97b8db898..4d6cf7d449 100644
--- a/src/drivers/px4io/px4io.cpp
+++ b/src/drivers/px4io/px4io.cpp
@@ -218,6 +218,16 @@ public:
 	 */
 	int			print_debug();
 
+	/*
+	 * To test what happens if IO stops receiving updates from FMU.
+	 *
+	 * @param is_fail	true for failure condition, false for normal operation.
+	 */
+	void			test_fmu_fail(bool is_fail)
+	{
+		_test_fmu_fail = is_fail;
+	};
+
 #ifdef CONFIG_ARCH_BOARD_PX4FMU_V1
 	/**
 	 * Set the DSM VCC is controlled by relay one flag
@@ -315,6 +325,7 @@ private:
 	float			_analog_rc_rssi_volt; ///< analog RSSI voltage
 
 	float			_last_throttle; ///< last throttle value for battery calculation
+	bool			_test_fmu_fail; ///< To test what happens if IO looses FMU
 
 #ifdef CONFIG_ARCH_BOARD_PX4FMU_V1
 	bool			_dsm_vcc_ctl;		///< true if relay 1 controls DSM satellite RX power
@@ -549,7 +560,8 @@ PX4IO::PX4IO(device::Device *interface) :
 	_rssi_pwm_min(0),
 	_analog_rc_rssi_stable(false),
 	_analog_rc_rssi_volt(-1.0f),
-	_last_throttle(0.0f)
+	_last_throttle(0.0f),
+	_test_fmu_fail(false)
 #ifdef CONFIG_ARCH_BOARD_PX4FMU_V1
 	, _dsm_vcc_ctl(false)
 #endif
@@ -1349,8 +1361,12 @@ PX4IO::io_set_control_state(unsigned group)
 		_last_throttle = controls.control[3];
 	}
 
-	/* copy values to registers in IO */
-	return io_reg_set(PX4IO_PAGE_CONTROLS, group * PX4IO_PROTOCOL_MAX_CONTROL_COUNT, regs, _max_controls);
+	if (!_test_fmu_fail) {
+		/* copy values to registers in IO */
+		return io_reg_set(PX4IO_PAGE_CONTROLS, group * PX4IO_PROTOCOL_MAX_CONTROL_COUNT, regs, _max_controls);
+	} else {
+		return OK;
+	}
 }
 
 
@@ -2735,8 +2751,14 @@ PX4IO::ioctl(file *filep, int cmd, unsigned long arg)
 				ret = -EINVAL;
 
 			} else {
-				/* send a direct PWM value */
-				ret = io_reg_set(PX4IO_PAGE_DIRECT_PWM, channel, arg);
+				if (!_test_fmu_fail) {
+					/* send a direct PWM value */
+					ret = io_reg_set(PX4IO_PAGE_DIRECT_PWM, channel, arg);
+				} else {
+					/* Just silently accept the ioctl without doing anything
+					 * in test mode. */
+					ret = OK;
+				}
 			}
 
 			break;
@@ -3037,7 +3059,12 @@ PX4IO::write(file * /*filp*/, const char *buffer, size_t len)
 	if (count > 0) {
 
 		perf_begin(_perf_write);
-		int ret = io_reg_set(PX4IO_PAGE_DIRECT_PWM, 0, (uint16_t *)buffer, count);
+
+		int ret = OK;
+		/* The write() is silently ignored in test mode. */
+		if (!_test_fmu_fail) {
+			ret = io_reg_set(PX4IO_PAGE_DIRECT_PWM, 0, (uint16_t *)buffer, count);
+		}
 		perf_end(_perf_write);
 
 		if (ret != OK) {
@@ -3869,8 +3896,29 @@ px4io_main(int argc, char *argv[])
 		exit(0);
 	}
 
+	if (!strcmp(argv[1], "test_fmu_fail")) {
+		if (g_dev != nullptr) {
+			g_dev->test_fmu_fail(true);
+			exit(0);
+		} else {
+
+			errx(1, "px4io must be started first");
+		}
+	}
+
+	if (!strcmp(argv[1], "test_fmu_ok")) {
+		if (g_dev != nullptr) {
+			g_dev->test_fmu_fail(false);
+			exit(0);
+		} else {
+
+			errx(1, "px4io must be started first");
+		}
+	}
+
 out:
 	errx(1, "need a command, try 'start', 'stop', 'status', 'test', 'monitor', 'debug <level>',\n"
 	     "'recovery', 'limit <rate>', 'current', 'bind', 'checkcrc', 'safety_on', 'safety_off',\n"
-	     "'forceupdate', 'update', 'sbus1_out', 'sbus2_out', 'rssi_analog' or 'rssi_pwm'");
+	     "'forceupdate', 'update', 'sbus1_out', 'sbus2_out', 'rssi_analog' or 'rssi_pwm',\n"
+	     "'test_fmu_fail', 'test_fmu_ok'");
 }

From 31d5c59ab7e611c4f0d7ac4fb99ca4dd4fd13870 Mon Sep 17 00:00:00 2001
From: Julian Oes <julian@oes.ch>
Date: Wed, 13 Jul 2016 14:38:03 +0200
Subject: [PATCH 1115/1230] px4iofirmware: remove unused define

---
 src/modules/px4iofirmware/controls.c | 1 -
 1 file changed, 1 deletion(-)

diff --git a/src/modules/px4iofirmware/controls.c b/src/modules/px4iofirmware/controls.c
index 99570d1056..091be1a209 100644
--- a/src/modules/px4iofirmware/controls.c
+++ b/src/modules/px4iofirmware/controls.c
@@ -51,7 +51,6 @@
 
 #include "px4io.h"
 
-#define RC_FAILSAFE_TIMEOUT		2000000		/**< two seconds failsafe timeout */
 #define RC_CHANNEL_HIGH_THRESH		5000	/* 75% threshold */
 #define RC_CHANNEL_LOW_THRESH		-8000	/* 10% threshold */
 

From 57b3bbf657079c862d50290b9df50b7b2adc8800 Mon Sep 17 00:00:00 2001
From: Julian Oes <julian@oes.ch>
Date: Wed, 13 Jul 2016 14:38:25 +0200
Subject: [PATCH 1116/1230] px4iofirmware: don't set FMU_OK flags immediately

The FMU_OK flags are later checked anyway based on
system_state.fmu_data_received_time.
---
 src/modules/px4iofirmware/registers.c | 4 +---
 1 file changed, 1 insertion(+), 3 deletions(-)

diff --git a/src/modules/px4iofirmware/registers.c b/src/modules/px4iofirmware/registers.c
index 901dc061da..660a8245ce 100644
--- a/src/modules/px4iofirmware/registers.c
+++ b/src/modules/px4iofirmware/registers.c
@@ -284,8 +284,6 @@ registers_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num
 		}
 
 		system_state.fmu_data_received_time = hrt_absolute_time();
-		r_status_flags |= PX4IO_P_STATUS_FLAGS_FMU_OK;
-		r_status_flags &= ~PX4IO_P_STATUS_FLAGS_RAW_PWM;
 
 		break;
 
@@ -306,7 +304,7 @@ registers_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num
 		}
 
 		system_state.fmu_data_received_time = hrt_absolute_time();
-		r_status_flags |= PX4IO_P_STATUS_FLAGS_FMU_OK | PX4IO_P_STATUS_FLAGS_RAW_PWM;
+		r_status_flags |= PX4IO_P_STATUS_FLAGS_RAW_PWM;
 
 		break;
 

From ec035b72684271367cd02f2c1bb0da40185e3866 Mon Sep 17 00:00:00 2001
From: Julian Oes <julian@oes.ch>
Date: Wed, 13 Jul 2016 14:39:42 +0200
Subject: [PATCH 1117/1230] px4iofirmware: no override in multirotor mode

This fixes a bug where multirotors got into override mode when the FMU
is dead/not responding.
The main bug was that the check was for FMU_OK || MANUAL_OVERRIDE_OK
in order to get further in the override checks.

Also a mixer_tick was called inside the controls_tick even though these
are called in px4io.c after each other anyway.
---
 src/modules/px4iofirmware/controls.c | 27 ++++++++++-----------------
 1 file changed, 10 insertions(+), 17 deletions(-)

diff --git a/src/modules/px4iofirmware/controls.c b/src/modules/px4iofirmware/controls.c
index 091be1a209..256e2d0902 100644
--- a/src/modules/px4iofirmware/controls.c
+++ b/src/modules/px4iofirmware/controls.c
@@ -481,14 +481,12 @@ controls_tick()
 	/*
 	 * Check for manual override.
 	 *
-	 * The PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK flag must be set, and we
-	 * must have R/C input (NO FAILSAFE!).
-	 * Override is enabled if either the hardcoded channel / value combination
-	 * is selected, or the AP has requested it.
+	 * Firstly, manual override must be enabled, RC input available and a mixer loaded.
 	 */
-	if ((!(r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK) || (r_setup_arming & PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK)) &&
+	if ((r_setup_arming & PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK) &&
 	    (r_status_flags & PX4IO_P_STATUS_FLAGS_RC_OK) &&
-	    !(r_raw_rc_flags & PX4IO_P_RAW_RC_FLAGS_FAILSAFE)) {
+	    !(r_raw_rc_flags & PX4IO_P_RAW_RC_FLAGS_FAILSAFE) &&
+	    (r_status_flags & PX4IO_P_STATUS_FLAGS_MIXER_OK)) {
 
 		bool override = false;
 
@@ -499,29 +497,24 @@ controls_tick()
 		 * requested override.
 		 *
 		 */
-		if ((r_status_flags & PX4IO_P_STATUS_FLAGS_RC_OK) && (REG_TO_SIGNED(rc_value_override) < RC_CHANNEL_LOW_THRESH)) {
+		if ((r_status_flags & PX4IO_P_STATUS_FLAGS_RC_OK) &&
+		    (REG_TO_SIGNED(rc_value_override) < RC_CHANNEL_LOW_THRESH)) {
 			override = true;
 		}
 
 		/*
-		  if the FMU is dead then enable override if we have a
-		  mixer and OVERRIDE_IMMEDIATE is set
+		 * If the FMU is dead then enable override if we have a mixer
+		 * and we want to immediately override (instead of using the RC channel
+		 * as in the case above.
 		 */
 		if (!(r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK) &&
-		    (r_setup_arming & PX4IO_P_SETUP_ARMING_OVERRIDE_IMMEDIATE) &&
-		    (r_status_flags & PX4IO_P_STATUS_FLAGS_MIXER_OK)) {
+		    (r_setup_arming & PX4IO_P_SETUP_ARMING_OVERRIDE_IMMEDIATE)) {
 			override = true;
 		}
 
 		if (override) {
-
 			r_status_flags |= PX4IO_P_STATUS_FLAGS_OVERRIDE;
 
-			/* mix new RC input control values to servos */
-			if (dsm_updated || sbus_updated || ppm_updated || st24_updated || sumd_updated) {
-				mixer_tick();
-			}
-
 		} else {
 			r_status_flags &= ~(PX4IO_P_STATUS_FLAGS_OVERRIDE);
 		}

From de76c398ca508c9334503baffd6fffd9082a96da Mon Sep 17 00:00:00 2001
From: Julian Oes <julian@oes.ch>
Date: Wed, 13 Jul 2016 14:42:43 +0200
Subject: [PATCH 1118/1230] px4io: whitespace fixes

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

diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp
index 4d6cf7d449..652dc93073 100644
--- a/src/drivers/px4io/px4io.cpp
+++ b/src/drivers/px4io/px4io.cpp
@@ -1364,6 +1364,7 @@ PX4IO::io_set_control_state(unsigned group)
 	if (!_test_fmu_fail) {
 		/* copy values to registers in IO */
 		return io_reg_set(PX4IO_PAGE_CONTROLS, group * PX4IO_PROTOCOL_MAX_CONTROL_COUNT, regs, _max_controls);
+
 	} else {
 		return OK;
 	}
@@ -2754,6 +2755,7 @@ PX4IO::ioctl(file *filep, int cmd, unsigned long arg)
 				if (!_test_fmu_fail) {
 					/* send a direct PWM value */
 					ret = io_reg_set(PX4IO_PAGE_DIRECT_PWM, channel, arg);
+
 				} else {
 					/* Just silently accept the ioctl without doing anything
 					 * in test mode. */
@@ -3061,10 +3063,12 @@ PX4IO::write(file * /*filp*/, const char *buffer, size_t len)
 		perf_begin(_perf_write);
 
 		int ret = OK;
+
 		/* The write() is silently ignored in test mode. */
 		if (!_test_fmu_fail) {
 			ret = io_reg_set(PX4IO_PAGE_DIRECT_PWM, 0, (uint16_t *)buffer, count);
 		}
+
 		perf_end(_perf_write);
 
 		if (ret != OK) {
@@ -3900,6 +3904,7 @@ px4io_main(int argc, char *argv[])
 		if (g_dev != nullptr) {
 			g_dev->test_fmu_fail(true);
 			exit(0);
+
 		} else {
 
 			errx(1, "px4io must be started first");
@@ -3910,6 +3915,7 @@ px4io_main(int argc, char *argv[])
 		if (g_dev != nullptr) {
 			g_dev->test_fmu_fail(false);
 			exit(0);
+
 		} else {
 
 			errx(1, "px4io must be started first");

From 79a1b84b09d5e731ea93129d84481e03325c8b08 Mon Sep 17 00:00:00 2001
From: Julian Oes <julian@oes.ch>
Date: Wed, 13 Jul 2016 15:05:36 +0200
Subject: [PATCH 1119/1230] px4iofirmware: clean up override decision

The override checking was scattered across two places and is now unified
in controls_tick(). The part in mixer_tick only decides which mixer (or
none) to use give the override flag.
---
 src/modules/px4iofirmware/controls.c |  8 +++++++-
 src/modules/px4iofirmware/mixer.cpp  | 27 +++++++++------------------
 2 files changed, 16 insertions(+), 19 deletions(-)

diff --git a/src/modules/px4iofirmware/controls.c b/src/modules/px4iofirmware/controls.c
index 256e2d0902..17d18b66ae 100644
--- a/src/modules/px4iofirmware/controls.c
+++ b/src/modules/px4iofirmware/controls.c
@@ -486,6 +486,7 @@ controls_tick()
 	if ((r_setup_arming & PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK) &&
 	    (r_status_flags & PX4IO_P_STATUS_FLAGS_RC_OK) &&
 	    !(r_raw_rc_flags & PX4IO_P_RAW_RC_FLAGS_FAILSAFE) &&
+	    !(r_setup_arming & PX4IO_P_SETUP_ARMING_RC_HANDLING_DISABLED) &&
 	    (r_status_flags & PX4IO_P_STATUS_FLAGS_MIXER_OK)) {
 
 		bool override = false;
@@ -506,9 +507,14 @@ controls_tick()
 		 * If the FMU is dead then enable override if we have a mixer
 		 * and we want to immediately override (instead of using the RC channel
 		 * as in the case above.
+		 *
+		 * Also, do not enter manual override if we asked for termination
+		 * failsafe and FMU is lost.
 		 */
 		if (!(r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK) &&
-		    (r_setup_arming & PX4IO_P_SETUP_ARMING_OVERRIDE_IMMEDIATE)) {
+		    (r_setup_arming & PX4IO_P_SETUP_ARMING_OVERRIDE_IMMEDIATE) &&
+		    !(r_setup_arming & PX4IO_P_SETUP_ARMING_TERMINATION_FAILSAFE)
+		    ) {
 			override = true;
 		}
 
diff --git a/src/modules/px4iofirmware/mixer.cpp b/src/modules/px4iofirmware/mixer.cpp
index 9c2dfdc5aa..5be37bba21 100644
--- a/src/modules/px4iofirmware/mixer.cpp
+++ b/src/modules/px4iofirmware/mixer.cpp
@@ -121,16 +121,10 @@ mixer_tick(void)
 	 * Decide which set of controls we're using.
 	 */
 
-	bool override_enabled = ((r_status_flags & PX4IO_P_STATUS_FLAGS_OVERRIDE) &&
-				 (r_status_flags & PX4IO_P_STATUS_FLAGS_RC_OK) &&
-				 (r_status_flags & PX4IO_P_STATUS_FLAGS_MIXER_OK) &&
-				 !(r_setup_arming & PX4IO_P_SETUP_ARMING_RC_HANDLING_DISABLED));
-
-	/* do not mix if RAW_PWM mode is on and FMU is good */
+	/* Do not mix if we have raw PWM and FMU is ok. */
 	if ((r_status_flags & PX4IO_P_STATUS_FLAGS_RAW_PWM) &&
 	    (r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK)) {
 
-		/* don't actually mix anything - we already have raw PWM values */
 		source = MIX_NONE;
 
 	} else {
@@ -143,19 +137,16 @@ mixer_tick(void)
 			source = MIX_FMU;
 		}
 
-		if (override_enabled &&
-		    !(r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK) &&
-		    /* do not enter manual override if we asked for termination failsafe and FMU is lost */
-		    !(r_setup_arming & PX4IO_P_SETUP_ARMING_TERMINATION_FAILSAFE)) {
+		else if (r_status_flags & PX4IO_P_STATUS_FLAGS_OVERRIDE) {
 
-			/* if allowed, mix from RC inputs directly */
-			source = MIX_OVERRIDE;
+			if (r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK) {
 
-		} else 	if (override_enabled &&
-			    (r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK)) {
-
-			/* if allowed, mix from RC inputs directly up to available rc channels */
-			source = MIX_OVERRIDE_FMU_OK;
+				/* if allowed, mix from RC inputs directly up to available rc channels */
+				source = MIX_OVERRIDE_FMU_OK;
+			} else {
+				/* if allowed, mix from RC inputs directly */
+				source = MIX_OVERRIDE;
+			}
 		}
 	}
 

From dcb7c0e4c2c9700a2f48aa04e416b3d0563915df Mon Sep 17 00:00:00 2001
From: Julian Oes <julian@oes.ch>
Date: Wed, 13 Jul 2016 15:10:32 +0200
Subject: [PATCH 1120/1230] px4iofirmware: correct newline

---
 src/modules/px4iofirmware/controls.c | 3 +--
 1 file changed, 1 insertion(+), 2 deletions(-)

diff --git a/src/modules/px4iofirmware/controls.c b/src/modules/px4iofirmware/controls.c
index 17d18b66ae..91476c95f6 100644
--- a/src/modules/px4iofirmware/controls.c
+++ b/src/modules/px4iofirmware/controls.c
@@ -513,8 +513,7 @@ controls_tick()
 		 */
 		if (!(r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK) &&
 		    (r_setup_arming & PX4IO_P_SETUP_ARMING_OVERRIDE_IMMEDIATE) &&
-		    !(r_setup_arming & PX4IO_P_SETUP_ARMING_TERMINATION_FAILSAFE)
-		    ) {
+		    !(r_setup_arming & PX4IO_P_SETUP_ARMING_TERMINATION_FAILSAFE)) {
 			override = true;
 		}
 

From 78a9472b2557a20375391149197691740a86e5a1 Mon Sep 17 00:00:00 2001
From: Julian Oes <julian@oes.ch>
Date: Wed, 13 Jul 2016 19:32:35 +0200
Subject: [PATCH 1121/1230] px4iofirmware: astyle

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

diff --git a/src/modules/px4iofirmware/mixer.cpp b/src/modules/px4iofirmware/mixer.cpp
index 5be37bba21..36128fc119 100644
--- a/src/modules/px4iofirmware/mixer.cpp
+++ b/src/modules/px4iofirmware/mixer.cpp
@@ -143,6 +143,7 @@ mixer_tick(void)
 
 				/* if allowed, mix from RC inputs directly up to available rc channels */
 				source = MIX_OVERRIDE_FMU_OK;
+
 			} else {
 				/* if allowed, mix from RC inputs directly */
 				source = MIX_OVERRIDE;

From 2241ab9cac674ffcc9ab305132b3bc945f213f5b Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Beat=20K=C3=BCng?= <beat-kueng@gmx.net>
Date: Thu, 28 Jul 2016 17:42:31 +0200
Subject: [PATCH 1122/1230] mavlink: add a shell using SERIAL_CONTROL MAVLink
 message

---
 src/modules/mavlink/CMakeLists.txt       |   1 +
 src/modules/mavlink/mavlink_main.cpp     |  41 ++++++
 src/modules/mavlink/mavlink_main.h       |  10 +-
 src/modules/mavlink/mavlink_receiver.cpp |  30 ++++
 src/modules/mavlink/mavlink_receiver.h   |   5 +-
 src/modules/mavlink/mavlink_shell.cpp    | 172 +++++++++++++++++++++++
 src/modules/mavlink/mavlink_shell.h      |  93 ++++++++++++
 7 files changed, 349 insertions(+), 3 deletions(-)
 create mode 100644 src/modules/mavlink/mavlink_shell.cpp
 create mode 100644 src/modules/mavlink/mavlink_shell.h

diff --git a/src/modules/mavlink/CMakeLists.txt b/src/modules/mavlink/CMakeLists.txt
index bc573e500e..023fddf476 100644
--- a/src/modules/mavlink/CMakeLists.txt
+++ b/src/modules/mavlink/CMakeLists.txt
@@ -49,6 +49,7 @@ px4_add_module(
 		mavlink_receiver.cpp
 		mavlink_ftp.cpp
 		mavlink_log_handler.cpp
+		mavlink_shell.cpp
 	DEPENDS
 		platforms__common
 	)
diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp
index ed0713421b..3818ddf818 100644
--- a/src/modules/mavlink/mavlink_main.cpp
+++ b/src/modules/mavlink/mavlink_main.cpp
@@ -185,6 +185,7 @@ Mavlink::Mavlink() :
 	_parameters_manager(nullptr),
 	_mavlink_ftp(nullptr),
 	_mavlink_log_handler(nullptr),
+	_mavlink_shell(nullptr),
 	_mode(MAVLINK_MODE_NORMAL),
 	_channel(MAVLINK_COMM_0),
 	_radio_id(0),
@@ -1517,6 +1518,35 @@ Mavlink::get_rate_mult()
 	return _rate_mult;
 }
 
+MavlinkShell*
+Mavlink::get_shell()
+{
+	if (!_mavlink_shell) {
+		_mavlink_shell = new MavlinkShell();
+		if (!_mavlink_shell) {
+			PX4_ERR("Failed to allocate a shell");
+		} else {
+			int ret = _mavlink_shell->start();
+			if (ret != 0) {
+				PX4_ERR("Failed to start shell (%i)", ret);
+				delete _mavlink_shell;
+				_mavlink_shell = nullptr;
+			}
+		}
+	}
+
+	return _mavlink_shell;
+}
+
+void
+Mavlink::close_shell()
+{
+	if (_mavlink_shell) {
+		delete _mavlink_shell;
+		_mavlink_shell = nullptr;
+	}
+}
+
 void
 Mavlink::update_rate_mult()
 {
@@ -2078,6 +2108,17 @@ Mavlink::task_main(int argc, char *argv[])
 			_logbuffer.put(&mavlink_log);
 		}
 
+		/* check for shell output */
+		if (_mavlink_shell && _mavlink_shell->available() > 0) {
+			mavlink_serial_control_t msg;
+			msg.baudrate = 0;
+			msg.flags = SERIAL_CONTROL_FLAG_REPLY;
+			msg.timeout = 0;
+			msg.device = SERIAL_CONTROL_DEV_SHELL;
+			msg.count = _mavlink_shell->read(msg.data, sizeof(msg.data));
+			mavlink_msg_serial_control_send_struct(get_channel(), &msg);
+		}
+
 		/* check for requested subscriptions */
 		if (_subscribe_to_stream != nullptr) {
 			if (OK == configure_stream(_subscribe_to_stream, _subscribe_to_stream_rate)) {
diff --git a/src/modules/mavlink/mavlink_main.h b/src/modules/mavlink/mavlink_main.h
index b2f616a98a..a4c695cfa3 100644
--- a/src/modules/mavlink/mavlink_main.h
+++ b/src/modules/mavlink/mavlink_main.h
@@ -69,6 +69,7 @@
 #include "mavlink_parameters.h"
 #include "mavlink_ftp.h"
 #include "mavlink_log_handler.h"
+#include "mavlink_shell.h"
 
 enum Protocol {
 	SERIAL = 0,
@@ -411,7 +412,7 @@ public:
 	bool			accepting_commands() { return true; /* non-trivial side effects ((!_config_link_on) || (_mode == MAVLINK_MODE_CONFIG));*/ }
 
 	/**
-	 * Wether or not the system should be logging
+	 * Whether or not the system should be logging
 	 */
 	bool			get_logging_enabled() { return _logging_enabled; }
 
@@ -420,6 +421,12 @@ public:
 	int			get_data_rate() { return _datarate; }
 	void			set_data_rate(int rate) { if (rate > 0) _datarate = rate; }
 
+	/** get the Mavlink shell. Create a new one if there isn't one. It is *always* created via MavlinkReceiver thread.
+	 *  Returns nullptr if shell cannot be created */
+	MavlinkShell		*get_shell();
+	/** close the Mavlink shell if it is open */
+	void			close_shell();
+
 protected:
 	Mavlink			*next;
 
@@ -451,6 +458,7 @@ private:
 	MavlinkParametersManager	*_parameters_manager;
 	MavlinkFTP			*_mavlink_ftp;
 	MavlinkLogHandler		*_mavlink_log_handler;
+	MavlinkShell			*_mavlink_shell;
 
 	MAVLINK_MODE 		_mode;
 
diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp
index 81f123ef03..d0a97ab1bd 100644
--- a/src/modules/mavlink/mavlink_receiver.cpp
+++ b/src/modules/mavlink/mavlink_receiver.cpp
@@ -263,6 +263,10 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg)
 		handle_message_battery_status(msg);
 		break;
 
+	case MAVLINK_MSG_ID_SERIAL_CONTROL:
+		handle_message_serial_control(msg);
+		break;
+
 	default:
 		break;
 	}
@@ -1191,6 +1195,32 @@ MavlinkReceiver::handle_message_battery_status(mavlink_message_t *msg)
 	}
 }
 
+void
+MavlinkReceiver::handle_message_serial_control(mavlink_message_t *msg)
+{
+	mavlink_serial_control_t serial_control_mavlink;
+	mavlink_msg_serial_control_decode(msg, &serial_control_mavlink);
+
+	// we only support shell commands
+	if (serial_control_mavlink.device != SERIAL_CONTROL_DEV_SHELL
+			|| (serial_control_mavlink.flags & SERIAL_CONTROL_FLAG_REPLY)) {
+		return;
+	}
+
+	MavlinkShell* shell = _mavlink->get_shell();
+	if (shell) {
+		// we ignore the timeout, EXCLUSIVE & BLOCKING flags of the SERIAL_CONTROL message
+		if (serial_control_mavlink.count > 0) {
+			shell->write(serial_control_mavlink.data, serial_control_mavlink.count);
+		}
+
+		// if no response requested, assume the shell is no longer used
+		if ((serial_control_mavlink.flags & SERIAL_CONTROL_FLAG_RESPOND) == 0) {
+			_mavlink->close_shell();
+		}
+	}
+}
+
 switch_pos_t
 MavlinkReceiver::decode_switch_pos(uint16_t buttons, unsigned sw)
 {
diff --git a/src/modules/mavlink/mavlink_receiver.h b/src/modules/mavlink/mavlink_receiver.h
index 8d3b3d0396..b2521a6813 100644
--- a/src/modules/mavlink/mavlink_receiver.h
+++ b/src/modules/mavlink/mavlink_receiver.h
@@ -32,8 +32,8 @@
  ****************************************************************************/
 
 /**
- * @file mavlink_orb_listener.h
- * MAVLink 1.0 uORB listener definition
+ * @file mavlink_receiver.h
+ * MAVLink receiver thread
  *
  * @author Lorenz Meier <lorenz@px4.io>
  * @author Anton Babushkin <anton@px4.io>
@@ -143,6 +143,7 @@ private:
 	void handle_message_adsb_vehicle(mavlink_message_t *msg);
 	void handle_message_gps_rtcm_data(mavlink_message_t *msg);
 	void handle_message_battery_status(mavlink_message_t *msg);
+	void handle_message_serial_control(mavlink_message_t *msg);
 
 	void *receive_thread(void *arg);
 
diff --git a/src/modules/mavlink/mavlink_shell.cpp b/src/modules/mavlink/mavlink_shell.cpp
new file mode 100644
index 0000000000..ea509151ef
--- /dev/null
+++ b/src/modules/mavlink/mavlink_shell.cpp
@@ -0,0 +1,172 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2016 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file mavlink_shell.cpp
+ * A shell to be used via MAVLink
+ *
+ * @author Beat Küng <beat-kueng@gmx.net>
+ */
+
+#include "mavlink_shell.h"
+#include <px4_defines.h>
+
+#include <unistd.h>
+#include <errno.h>
+#include <sys/ioctl.h>
+
+
+#ifdef __PX4_NUTTX
+#include <apps/nsh.h>
+#endif /* __PX4_NUTTX */
+
+
+MavlinkShell::MavlinkShell()
+{
+}
+
+MavlinkShell::~MavlinkShell()
+{
+	//closing the pipes will stop the thread as well
+	if (_to_shell_fd >= 0) {
+		PX4_INFO("Stopping mavlink shell");
+		close(_to_shell_fd);
+	}
+	if (_from_shell_fd >= 0) {
+		close(_from_shell_fd);
+	}
+}
+
+int MavlinkShell::start()
+{
+	//this currently only works for NuttX
+#ifndef __PX4_NUTTX
+	return -1;
+#endif /* __PX4_NUTTX */
+
+
+	PX4_INFO("Starting mavlink shell");
+
+	int p1[2], p2[2];
+
+	/* Create the shell task and redirect its stdin & stdout. If we used pthread, we would redirect
+	 * stdin/out of the calling process as well, so we need px4_task_spawn_cmd. However NuttX only
+	 * keeps (duplicates) the first 3 fd's when creating a new task, all others are not inherited.
+	 * This means we need to temporarily change the first 3 fd's of the current task (or at least
+	 * the first 2 if stdout=stderr).
+	 * And we hope :-) that during the temporary phase, no other thread from the same task writes to
+	 * stdout (as it would end up in the pipe).
+	 */
+
+	if (pipe(p1) != 0) {
+		return -errno;
+	}
+	if (pipe(p2) != 0) {
+		close(p1[0]);
+		close(p1[1]);
+		return -errno;
+	}
+
+	int ret = 0;
+
+	_from_shell_fd  = p1[0];
+	_to_shell_fd = p2[1];
+	_shell_fds[0]  = p2[0];
+	_shell_fds[1] = p1[1];
+
+	int fd_backups[2]; //we don't touch stderr, we will redirect it to stdout in the startup of the shell task
+	for (int i = 0; i < 2; ++i) {
+		fd_backups[i] = dup(i);
+		if (fd_backups[i] == -1) {
+			ret = -errno;
+		}
+	}
+	dup2(_shell_fds[0], 0);
+	dup2(_shell_fds[1], 1);
+
+	if (ret == 0) {
+		_task = px4_task_spawn_cmd("mavlink_shell",
+				SCHED_DEFAULT,
+				SCHED_PRIORITY_DEFAULT,
+				2048,
+				&MavlinkShell::shell_start_thread,
+				nullptr);
+		if (_task < 0) {
+			ret = -1;
+		}
+	}
+
+	//restore fd's
+	for (int i = 0; i < 2; ++i) {
+		if (dup2(fd_backups[i], i) == -1) {
+			ret = -errno;
+		}
+		close(fd_backups[i]);
+	}
+
+	//close unused pipe fd's
+	close(_shell_fds[0]);
+	close(_shell_fds[1]);
+
+	return ret;
+}
+
+int MavlinkShell::shell_start_thread(int argc, char *argv[])
+{
+	dup2(1, 2); //redirect stderror to stdout
+
+#ifdef __PX4_NUTTX
+	nsh_consolemain(0, NULL);
+#endif /* __PX4_NUTTX */
+
+	return 0;
+}
+
+size_t MavlinkShell::write(uint8_t* buffer, size_t len)
+{
+	return ::write(_to_shell_fd, buffer, len);
+}
+
+size_t MavlinkShell::read(uint8_t* buffer, size_t len)
+{
+	return ::read(_from_shell_fd, buffer, len);
+}
+
+size_t MavlinkShell::available()
+{
+	int ret = 0;
+	if (ioctl(_from_shell_fd, FIONREAD, (unsigned long)&ret) == OK) {
+		return ret;
+	}
+	return 0;
+}
diff --git a/src/modules/mavlink/mavlink_shell.h b/src/modules/mavlink/mavlink_shell.h
new file mode 100644
index 0000000000..f4a788a023
--- /dev/null
+++ b/src/modules/mavlink/mavlink_shell.h
@@ -0,0 +1,93 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2016 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file mavlink_shell.h
+ * A shell to be used via MAVLink
+ *
+ * @author Beat Küng <beat-kueng@gmx.net>
+ */
+
+
+#include <stddef.h>
+#include <stdint.h>
+#include <px4_tasks.h>
+
+#pragma once
+
+class MavlinkShell
+{
+public:
+	MavlinkShell();
+
+	~MavlinkShell();
+
+	/**
+	 * Start the mavlink shell.
+	 *
+	 * @return		0 on success.
+	 */
+	int		start();
+
+
+	/**
+	 * Write to the shell
+	 * @return number of written bytes
+	 */
+	size_t write(uint8_t* buffer, size_t len);
+
+	/**
+	 * Read from the shell. This is blocking, if 0 bytes are available, this will block.
+	 * @param len buffer length
+	 * @return number of bytes read.
+	 */
+	size_t read(uint8_t* buffer, size_t len);
+
+	/**
+	 * Get the number of bytes that can be read.
+	 */
+	size_t available();
+
+private:
+
+	int _to_shell_fd = -1; /** fd to write to the shell */
+	int _from_shell_fd = -1; /** fd to read from the shell */
+	int _shell_fds[2] = {-1, -1}; /** stdin & out used by the shell */
+	px4_task_t _task;
+
+	static int shell_start_thread(int argc, char *argv[]);
+
+	/* do not allow copying this class */
+	MavlinkShell(const MavlinkShell &) = delete;
+	MavlinkShell operator=(const MavlinkShell &) = delete;
+};

From e8d44da2c0633bacd4b0c7a472ca2118210cea3e Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Beat=20K=C3=BCng?= <beat-kueng@gmx.net>
Date: Thu, 28 Jul 2016 17:43:52 +0200
Subject: [PATCH 1123/1230] Tools: add mavlink_shell.py script to start a shell
 over mavlink

---
 Tools/mavlink_shell.py | 154 +++++++++++++++++++++++++++++++++++++++++
 1 file changed, 154 insertions(+)
 create mode 100755 Tools/mavlink_shell.py

diff --git a/Tools/mavlink_shell.py b/Tools/mavlink_shell.py
new file mode 100755
index 0000000000..fc77ad6a22
--- /dev/null
+++ b/Tools/mavlink_shell.py
@@ -0,0 +1,154 @@
+#!/usr/bin/env python
+
+"""
+Open a shell over MAVLink.
+
+@author: Beat Kueng (beat-kueng@gmx.net)
+"""
+
+
+from __future__ import print_function
+import sys, select
+
+try:
+    from pymavlink import mavutil
+    import serial
+except:
+    print("Failed to import pymavlink.")
+    print("You may need to install it with 'pip install pymavlink pyserial'")
+    exit(-1)
+from argparse import ArgumentParser
+
+
+class MavlinkSerialPort():
+    '''an object that looks like a serial port, but
+    transmits using mavlink SERIAL_CONTROL packets'''
+    def __init__(self, portname, baudrate, devnum=0, debug=0):
+        self.baudrate = 0
+        self._debug = debug
+        self.buf = ''
+        self.port = devnum
+        self.debug("Connecting with MAVLink to %s ..." % portname)
+        self.mav = mavutil.mavlink_connection(portname, autoreconnect=True, baud=baudrate)
+        self.mav.wait_heartbeat()
+        self.debug("HEARTBEAT OK\n")
+        self.debug("Locked serial device\n")
+
+    def debug(self, s, level=1):
+        '''write some debug text'''
+        if self._debug >= level:
+            print(s)
+
+    def write(self, b):
+        '''write some bytes'''
+        self.debug("sending '%s' (0x%02x) of len %u\n" % (b, ord(b[0]), len(b)), 2)
+        while len(b) > 0:
+            n = len(b)
+            if n > 70:
+                n = 70
+            buf = [ord(x) for x in b[:n]]
+            buf.extend([0]*(70-len(buf)))
+            self.mav.mav.serial_control_send(self.port,
+                                             mavutil.mavlink.SERIAL_CONTROL_FLAG_EXCLUSIVE |
+                                             mavutil.mavlink.SERIAL_CONTROL_FLAG_RESPOND,
+                                             0,
+                                             0,
+                                             n,
+                                             buf)
+            b = b[n:]
+
+    def close(self):
+        self.mav.mav.serial_control_send(self.port, 0, 0, 0, 0, [0]*70)
+
+    def _recv(self):
+        '''read some bytes into self.buf'''
+        m = self.mav.recv_match(condition='SERIAL_CONTROL.count!=0',
+                                type='SERIAL_CONTROL', blocking=True,
+                                timeout=0.03)
+        if m is not None:
+            if self._debug > 2:
+                print(m)
+            data = m.data[:m.count]
+            self.buf += ''.join(str(chr(x)) for x in data)
+
+    def read(self, n):
+        '''read some bytes'''
+        if len(self.buf) == 0:
+            self._recv()
+        if len(self.buf) > 0:
+            if n > len(self.buf):
+                n = len(self.buf)
+            ret = self.buf[:n]
+            self.buf = self.buf[n:]
+            if self._debug >= 2:
+                for b in ret:
+                    self.debug("read 0x%x" % ord(b), 2)
+            return ret
+        return ''
+
+
+def main():
+    parser = ArgumentParser(description=__doc__)
+    parser.add_argument('port', metavar='PORT', nargs='?', default = None,
+            help='Mavlink port name: serial: DEVICE[,BAUD], udp: IP:PORT, tcp: tcp:IP:PORT. Eg: \
+/dev/ttyUSB0 or 0.0.0.0:14550. Auto-detect serial if not given.')
+    parser.add_argument("--baudrate", "-b", dest="baudrate", type=int,
+                      help="Mavlink port baud rate (default=115200)", default=115200)
+    args = parser.parse_args()
+
+
+    if args.port == None:
+        serial_list = mavutil.auto_detect_serial(preferred_list=['*FTDI*',
+            "*Arduino_Mega_2560*", "*3D_Robotics*", "*USB_to_UART*", '*PX4*', '*FMU*'])
+
+        if len(serial_list) == 0:
+            print("Error: no serial connection found")
+            return
+
+        if len(serial_list) > 1:
+            print('Auto-detected serial ports are:')
+            for port in serial_list:
+                print(" {:}".format(port))
+        print('Using port {:}'.format(serial_list[0]))
+        args.port = serial_list[0].device
+
+
+    print("Connecting to MAVLINK...")
+    mav_serialport = MavlinkSerialPort(args.port, args.baudrate, devnum=10)
+
+    mav_serialport.write('\n') # make sure the shell is started
+
+    try:
+        while True:
+            i, o, e = select.select([sys.stdin], [], [], 0.001)
+            if (i):
+                data = sys.stdin.readline()
+
+                # Erase stdin output (otherwise we have it twice)
+                # FIXME: this assumes a prompt of 5 chars, and does not work in all cases
+                CURSOR_UP_ONE = '\x1b[1A'
+                CURSOR_RIGHT = '\x1b[5C'
+                ERASE_LINE = '\x1b[2K'
+                ERASE_END_LINE = '\x1b[K'
+                sys.stdout.write(CURSOR_UP_ONE + CURSOR_RIGHT + ERASE_END_LINE)
+
+                # TODO: add a command history?
+
+                #sys.stdout.write(data)
+                mav_serialport.write(data)
+
+            data = mav_serialport.read(4096)
+            if data and len(data) > 0:
+                sys.stdout.write(data)
+                sys.stdout.flush()
+
+    except serial.serialutil.SerialException as e:
+        print(e)
+
+    except KeyboardInterrupt:
+        mav_serialport.close()
+
+
+if __name__ == '__main__':
+    main()
+

From 302719527a4213893f094e66be2f7c650eaa3e6e Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Beat=20K=C3=BCng?= <beat-kueng@gmx.net>
Date: Thu, 28 Jul 2016 17:45:02 +0200
Subject: [PATCH 1124/1230] nuttx defconfig: increase nr of file descriptors,
 due to mavlink shell

---
 nuttx-configs/mindpx-v2/nsh/defconfig | 2 +-
 nuttx-configs/px4fmu-v2/nsh/defconfig | 2 +-
 nuttx-configs/px4fmu-v4/nsh/defconfig | 2 +-
 nuttx-configs/tap-v1/nsh/defconfig    | 2 +-
 4 files changed, 4 insertions(+), 4 deletions(-)

diff --git a/nuttx-configs/mindpx-v2/nsh/defconfig b/nuttx-configs/mindpx-v2/nsh/defconfig
index ae23ec7bde..9d97d4543f 100644
--- a/nuttx-configs/mindpx-v2/nsh/defconfig
+++ b/nuttx-configs/mindpx-v2/nsh/defconfig
@@ -440,7 +440,7 @@ CONFIG_SIG_SIGWORK=4
 CONFIG_MAX_TASKS=32
 CONFIG_MAX_TASK_ARGS=10
 CONFIG_NPTHREAD_KEYS=4
-CONFIG_NFILE_DESCRIPTORS=47
+CONFIG_NFILE_DESCRIPTORS=51
 CONFIG_NFILE_STREAMS=8
 CONFIG_NAME_MAX=32
 CONFIG_PREALLOC_MQ_MSGS=4
diff --git a/nuttx-configs/px4fmu-v2/nsh/defconfig b/nuttx-configs/px4fmu-v2/nsh/defconfig
index 2322043b6d..edc4c2c9c5 100644
--- a/nuttx-configs/px4fmu-v2/nsh/defconfig
+++ b/nuttx-configs/px4fmu-v2/nsh/defconfig
@@ -442,7 +442,7 @@ CONFIG_SIG_SIGWORK=4
 CONFIG_MAX_TASKS=32
 CONFIG_MAX_TASK_ARGS=10
 CONFIG_NPTHREAD_KEYS=4
-CONFIG_NFILE_DESCRIPTORS=47
+CONFIG_NFILE_DESCRIPTORS=51
 CONFIG_NFILE_STREAMS=8
 CONFIG_NAME_MAX=32
 CONFIG_PREALLOC_MQ_MSGS=4
diff --git a/nuttx-configs/px4fmu-v4/nsh/defconfig b/nuttx-configs/px4fmu-v4/nsh/defconfig
index ccb6b5c132..2520f708fb 100644
--- a/nuttx-configs/px4fmu-v4/nsh/defconfig
+++ b/nuttx-configs/px4fmu-v4/nsh/defconfig
@@ -442,7 +442,7 @@ CONFIG_SIG_SIGWORK=4
 CONFIG_MAX_TASKS=32
 CONFIG_MAX_TASK_ARGS=10
 CONFIG_NPTHREAD_KEYS=4
-CONFIG_NFILE_DESCRIPTORS=47
+CONFIG_NFILE_DESCRIPTORS=51
 CONFIG_NFILE_STREAMS=8
 CONFIG_NAME_MAX=32
 CONFIG_PREALLOC_MQ_MSGS=4
diff --git a/nuttx-configs/tap-v1/nsh/defconfig b/nuttx-configs/tap-v1/nsh/defconfig
index 00144af2ad..d1076b2aa4 100644
--- a/nuttx-configs/tap-v1/nsh/defconfig
+++ b/nuttx-configs/tap-v1/nsh/defconfig
@@ -402,7 +402,7 @@ CONFIG_SIG_SIGWORK=4
 CONFIG_MAX_TASKS=32
 CONFIG_MAX_TASK_ARGS=10
 CONFIG_NPTHREAD_KEYS=4
-CONFIG_NFILE_DESCRIPTORS=42
+CONFIG_NFILE_DESCRIPTORS=46
 CONFIG_NFILE_STREAMS=8
 CONFIG_NAME_MAX=32
 CONFIG_PREALLOC_MQ_MSGS=4

From 499d362b8b18c597fdd1e5ffa15b79ef933563b2 Mon Sep 17 00:00:00 2001
From: Roman <bapstroman@gmail.com>
Date: Tue, 19 Jul 2016 11:37:54 +0200
Subject: [PATCH 1125/1230] added transition switch

---
 msg/manual_control_setpoint.msg     |  1 +
 msg/rc_channels.msg                 |  3 +-
 src/modules/sensors/sensor_params.c | 46 +++++++++++++++++++++++++++++
 src/modules/sensors/sensors.cpp     | 18 +++++++++++
 4 files changed, 67 insertions(+), 1 deletion(-)

diff --git a/msg/manual_control_setpoint.msg b/msg/manual_control_setpoint.msg
index 8295eba074..599e63d5a9 100644
--- a/msg/manual_control_setpoint.msg
+++ b/msg/manual_control_setpoint.msg
@@ -51,4 +51,5 @@ uint8 loiter_switch		 # loiter 2 position switch (optional): _MISSION_, LOITER
 uint8 acro_switch		 # acro 2 position switch (optional): _MANUAL_, ACRO
 uint8 offboard_switch	 # offboard 2 position switch (optional): _NORMAL_, OFFBOARD
 uint8 kill_switch		 # throttle kill: _NORMAL_, KILL
+uint8 transition_switch	 # VTOL transition switch: _HOVER, FORWARD_FLIGHT
 int8 mode_slot			 # the slot a specific model selector is in
diff --git a/msg/rc_channels.msg b/msg/rc_channels.msg
index 769a393216..52d89df5fb 100644
--- a/msg/rc_channels.msg
+++ b/msg/rc_channels.msg
@@ -20,11 +20,12 @@ uint8 RC_CHANNELS_FUNCTION_PARAM_2=17
 uint8 RC_CHANNELS_FUNCTION_PARAM_3_5=18
 uint8 RC_CHANNELS_FUNCTION_RATTITUDE=19
 uint8 RC_CHANNELS_FUNCTION_KILLSWITCH=20
+uint8 RC_CHANNELS_FUNCTION_TRANSITION=21
 
 uint64 timestamp_last_valid					# Timestamp of last valid RC signal
 float32[18] channels						# Scaled to -1..1 (throttle: 0..1)
 uint8 channel_count						# Number of valid channels
-int8[21] function						# Functions mapping
+int8[22] function						# Functions mapping
 uint8 rssi							# Receive signal strength indicator (0-100)
 bool signal_lost						# Control signal lost, should be checked together with topic timeout
 uint32 frame_drop_count						# Number of dropped frames
diff --git a/src/modules/sensors/sensor_params.c b/src/modules/sensors/sensor_params.c
index 57313d2afb..b4e8983f2a 100644
--- a/src/modules/sensors/sensor_params.c
+++ b/src/modules/sensors/sensor_params.c
@@ -2516,6 +2516,34 @@ PARAM_DEFINE_INT32(RC_MAP_KILL_SW, 0);
  */
 PARAM_DEFINE_INT32(RC_MAP_FLAPS, 0);
 
+/**
+ * VTOL transition switch channel mapping
+ *
+ * @min 0
+ * @max 18
+ * @group Radio Switches
+ * @value 0 Unassigned
+ * @value 1 Channel 1
+ * @value 2 Channel 2
+ * @value 3 Channel 3
+ * @value 4 Channel 4
+ * @value 5 Channel 5
+ * @value 6 Channel 6
+ * @value 7 Channel 7
+ * @value 8 Channel 8
+ * @value 9 Channel 9
+ * @value 10 Channel 10
+ * @value 11 Channel 11
+ * @value 12 Channel 12
+ * @value 13 Channel 13
+ * @value 14 Channel 14
+ * @value 15 Channel 15
+ * @value 16 Channel 16
+ * @value 17 Channel 17
+ * @value 18 Channel 18
+ */
+PARAM_DEFINE_INT32(RC_MAP_TRANS_SW, 0);
+
 /**
  * AUX1 Passthrough RC Channel
  *
@@ -2930,6 +2958,24 @@ PARAM_DEFINE_FLOAT(RC_OFFB_TH, 0.5f);
  */
 PARAM_DEFINE_FLOAT(RC_KILLSWITCH_TH, 0.25f);
 
+/**
+ * Threshold for the VTOL transition switch
+ *
+ * 0-1 indicate where in the full channel range the threshold sits
+ * 		0 : min
+ * 		1 : max
+ * sign indicates polarity of comparison
+ * 		positive : true when channel>th
+ * 		negative : true when channel<th
+ *
+ * @min -1
+ * @max 1
+ * @group Radio Switches
+ *
+ *
+ */
+PARAM_DEFINE_FLOAT(RC_TRANS_TH, 0.25f);
+
 /**
  * PWM input channel that provides RSSI.
  *
diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp
index 52f250396f..c8fd89da5c 100644
--- a/src/modules/sensors/sensors.cpp
+++ b/src/modules/sensors/sensors.cpp
@@ -305,6 +305,7 @@ private:
 		int rc_map_acro_sw;
 		int rc_map_offboard_sw;
 		int rc_map_kill_sw;
+		int rc_map_trans_sw;
 
 		int rc_map_flaps;
 
@@ -328,6 +329,7 @@ private:
 		float rc_acro_th;
 		float rc_offboard_th;
 		float rc_killswitch_th;
+		float rc_trans_th;
 		bool rc_assist_inv;
 		bool rc_auto_inv;
 		bool rc_rattitude_inv;
@@ -337,6 +339,7 @@ private:
 		bool rc_acro_inv;
 		bool rc_offboard_inv;
 		bool rc_killswitch_inv;
+		bool rc_trans_inv;
 
 		float battery_voltage_scaling;
 		float battery_current_scaling;
@@ -375,6 +378,7 @@ private:
 		param_t rc_map_acro_sw;
 		param_t rc_map_offboard_sw;
 		param_t rc_map_kill_sw;
+		param_t rc_map_trans_sw;
 
 		param_t rc_map_flaps;
 
@@ -402,6 +406,7 @@ private:
 		param_t rc_acro_th;
 		param_t rc_offboard_th;
 		param_t rc_killswitch_th;
+		param_t rc_trans_th;
 
 		param_t battery_voltage_scaling;
 		param_t battery_current_scaling;
@@ -650,6 +655,7 @@ Sensors::Sensors() :
 	_parameter_handles.rc_map_acro_sw = param_find("RC_MAP_ACRO_SW");
 	_parameter_handles.rc_map_offboard_sw = param_find("RC_MAP_OFFB_SW");
 	_parameter_handles.rc_map_kill_sw = param_find("RC_MAP_KILL_SW");
+	_parameter_handles.rc_map_trans_sw = param_find("RC_MAP_TRANS_SW");
 
 	_parameter_handles.rc_map_aux1 = param_find("RC_MAP_AUX1");
 	_parameter_handles.rc_map_aux2 = param_find("RC_MAP_AUX2");
@@ -678,6 +684,8 @@ Sensors::Sensors() :
 	_parameter_handles.rc_acro_th = param_find("RC_ACRO_TH");
 	_parameter_handles.rc_offboard_th = param_find("RC_OFFB_TH");
 	_parameter_handles.rc_killswitch_th = param_find("RC_KILLSWITCH_TH");
+	_parameter_handles.rc_trans_th = param_find("RC_TRANS_TH");
+
 
 	/* Differential pressure offset */
 	_parameter_handles.diff_pres_offset_pa = param_find("SENS_DPRES_OFF");
@@ -850,6 +858,10 @@ Sensors::parameters_update()
 		PX4_WARN("%s", paramerr);
 	}
 
+	if (param_get(_parameter_handles.rc_map_trans_sw, &(_parameters.rc_map_trans_sw)) != OK) {
+		warnx("%s", paramerr);
+	}
+
 	if (param_get(_parameter_handles.rc_map_flaps, &(_parameters.rc_map_flaps)) != OK) {
 		PX4_WARN("%s", paramerr);
 	}
@@ -894,6 +906,9 @@ Sensors::parameters_update()
 	param_get(_parameter_handles.rc_killswitch_th, &(_parameters.rc_killswitch_th));
 	_parameters.rc_killswitch_inv = (_parameters.rc_killswitch_th < 0);
 	_parameters.rc_killswitch_th = fabs(_parameters.rc_killswitch_th);
+	param_get(_parameter_handles.rc_trans_th, &(_parameters.rc_trans_th));
+	_parameters.rc_trans_inv = (_parameters.rc_trans_th < 0);
+	_parameters.rc_trans_th = fabs(_parameters.rc_trans_th);
 
 	/* update RC function mappings */
 	_rc.function[rc_channels_s::RC_CHANNELS_FUNCTION_THROTTLE] = _parameters.rc_map_throttle - 1;
@@ -909,6 +924,7 @@ Sensors::parameters_update()
 	_rc.function[rc_channels_s::RC_CHANNELS_FUNCTION_ACRO] = _parameters.rc_map_acro_sw - 1;
 	_rc.function[rc_channels_s::RC_CHANNELS_FUNCTION_OFFBOARD] = _parameters.rc_map_offboard_sw - 1;
 	_rc.function[rc_channels_s::RC_CHANNELS_FUNCTION_KILLSWITCH] = _parameters.rc_map_kill_sw - 1;
+	_rc.function[rc_channels_s::RC_CHANNELS_FUNCTION_TRANSITION] = _parameters.rc_map_trans_sw - 1;
 
 	_rc.function[rc_channels_s::RC_CHANNELS_FUNCTION_FLAPS] = _parameters.rc_map_flaps - 1;
 
@@ -2112,6 +2128,8 @@ Sensors::rc_poll()
 						 _parameters.rc_offboard_th, _parameters.rc_offboard_inv);
 			manual.kill_switch = get_rc_sw2pos_position(rc_channels_s::RC_CHANNELS_FUNCTION_KILLSWITCH,
 					     _parameters.rc_killswitch_th, _parameters.rc_killswitch_inv);
+			manual.transition_switch = get_rc_sw2pos_position(rc_channels_s::RC_CHANNELS_FUNCTION_TRANSITION,
+					     _parameters.rc_trans_th, _parameters.rc_trans_inv);
 
 			/* publish manual_control_setpoint topic */
 			if (_manual_control_pub != nullptr) {

From 9ece090dbe95b4f5074c0514fc427e7962953fb5 Mon Sep 17 00:00:00 2001
From: Roman <bapstroman@gmail.com>
Date: Tue, 19 Jul 2016 13:11:15 +0200
Subject: [PATCH 1126/1230] vtol_att_control: use transition switch instead of
 aux1

Signed-off-by: Roman <bapstroman@gmail.com>
---
 src/modules/vtol_att_control/vtol_att_control_main.cpp | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/src/modules/vtol_att_control/vtol_att_control_main.cpp b/src/modules/vtol_att_control/vtol_att_control_main.cpp
index 57e0a98170..330c80821b 100644
--- a/src/modules/vtol_att_control/vtol_att_control_main.cpp
+++ b/src/modules/vtol_att_control/vtol_att_control_main.cpp
@@ -481,7 +481,7 @@ VtolAttitudeControl::handle_command()
 bool
 VtolAttitudeControl::is_fixed_wing_requested()
 {
-	bool to_fw = _manual_control_sp.aux1 > 0.0f;
+	bool to_fw = _manual_control_sp.transition_switch == manual_control_setpoint_s::SWITCH_POS_ON;
 
 	// listen to transition commands if not in manual
 	if (!_v_control_mode.flag_control_manual_enabled) {

From 1f1839d978b5e9edca7258c9acc1d4e2b6a8d71b Mon Sep 17 00:00:00 2001
From: Roman <bapstroman@gmail.com>
Date: Tue, 19 Jul 2016 14:41:21 +0200
Subject: [PATCH 1127/1230] sensors: fixed code style

Signed-off-by: Roman <bapstroman@gmail.com>
---
 src/modules/sensors/sensors.cpp | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp
index c8fd89da5c..91096ed0d3 100644
--- a/src/modules/sensors/sensors.cpp
+++ b/src/modules/sensors/sensors.cpp
@@ -2129,7 +2129,7 @@ Sensors::rc_poll()
 			manual.kill_switch = get_rc_sw2pos_position(rc_channels_s::RC_CHANNELS_FUNCTION_KILLSWITCH,
 					     _parameters.rc_killswitch_th, _parameters.rc_killswitch_inv);
 			manual.transition_switch = get_rc_sw2pos_position(rc_channels_s::RC_CHANNELS_FUNCTION_TRANSITION,
-					     _parameters.rc_trans_th, _parameters.rc_trans_inv);
+						   _parameters.rc_trans_th, _parameters.rc_trans_inv);
 
 			/* publish manual_control_setpoint topic */
 			if (_manual_control_pub != nullptr) {

From e70223c2be3b273efc546390ab5608f21ec09cef Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Fri, 29 Jul 2016 13:43:24 +0200
Subject: [PATCH 1128/1230] Allow transition commands also in manual if no
 switch is used

---
 .../vtol_att_control/vtol_att_control_main.cpp     | 14 +++++++++-----
 1 file changed, 9 insertions(+), 5 deletions(-)

diff --git a/src/modules/vtol_att_control/vtol_att_control_main.cpp b/src/modules/vtol_att_control/vtol_att_control_main.cpp
index 330c80821b..06a37c67c5 100644
--- a/src/modules/vtol_att_control/vtol_att_control_main.cpp
+++ b/src/modules/vtol_att_control/vtol_att_control_main.cpp
@@ -89,7 +89,7 @@ VtolAttitudeControl::VtolAttitudeControl() :
 	_vtol_vehicle_status_pub(nullptr),
 	_v_rates_sp_pub(nullptr),
 	_v_att_sp_pub(nullptr),
-
+	_transition_command(vtol_vehicle_status_s::VEHICLE_VTOL_STATE_MC),
 	_abort_front_transition(false)
 
 {
@@ -481,11 +481,15 @@ VtolAttitudeControl::handle_command()
 bool
 VtolAttitudeControl::is_fixed_wing_requested()
 {
-	bool to_fw = _manual_control_sp.transition_switch == manual_control_setpoint_s::SWITCH_POS_ON;
+	bool to_fw = false;
 
-	// listen to transition commands if not in manual
-	if (!_v_control_mode.flag_control_manual_enabled) {
-		to_fw = _transition_command == vtol_vehicle_status_s::VEHICLE_VTOL_STATE_FW;
+	if (_manual_control_sp.transition_switch != manual_control_setpoint_s::SWITCH_POS_NONE &&
+	    _v_control_mode.flag_control_manual_enabled) {
+		to_fw = (_manual_control_sp.transition_switch == manual_control_setpoint_s::SWITCH_POS_ON);
+
+	} else {
+		// listen to transition commands if not in manual or mode switch is not mapped
+		to_fw = (_transition_command == vtol_vehicle_status_s::VEHICLE_VTOL_STATE_FW);
 	}
 
 	// handle abort request

From 6aa935fed6b31d119697e1b13a8af3dcfdc3dc31 Mon Sep 17 00:00:00 2001
From: Daniel Agar <daniel@agar.ca>
Date: Fri, 27 May 2016 16:10:03 -0400
Subject: [PATCH 1129/1230] FW don't allow ACRO or RATTITUDE

---
 .../commander_tests/state_machine_helper_test.cpp        | 8 ++++++--
 src/modules/commander/state_machine_helper.cpp           | 9 +++++++--
 2 files changed, 13 insertions(+), 4 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 78bc4bc22b..570f2b5886 100644
--- a/src/modules/commander/commander_tests/state_machine_helper_test.cpp
+++ b/src/modules/commander/commander_tests/state_machine_helper_test.cpp
@@ -337,8 +337,8 @@ bool StateMachineHelperTest::mainStateTransitionTest(void)
 
 		// TRANSITION_CHANGED tests
 
-		{ "transition: MANUAL to ACRO",
-			MTT_ALL_NOT_VALID,
+		{ "transition: MANUAL to ACRO - rotary",
+			MTT_ROTARY_WING,
 			commander_state_s::MAIN_STATE_MANUAL, commander_state_s::MAIN_STATE_ACRO, TRANSITION_CHANGED },
 
 		{ "transition: ACRO to MANUAL",
@@ -399,6 +399,10 @@ bool StateMachineHelperTest::mainStateTransitionTest(void)
 
 		// TRANSITION_DENIED tests
 
+		{ "transition: MANUAL to ACRO - not rotary",
+			MTT_ALL_NOT_VALID,
+			commander_state_s::MAIN_STATE_MANUAL, commander_state_s::MAIN_STATE_ACRO, TRANSITION_DENIED },
+
 		{ "no transition: MANUAL to AUTO_MISSION - global position not valid",
 			MTT_ALL_NOT_VALID,
 			commander_state_s::MAIN_STATE_MANUAL, commander_state_s::MAIN_STATE_AUTO_MISSION, TRANSITION_DENIED },
diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp
index dadc652009..e979d07e04 100644
--- a/src/modules/commander/state_machine_helper.cpp
+++ b/src/modules/commander/state_machine_helper.cpp
@@ -348,12 +348,17 @@ main_state_transition(struct vehicle_status_s *status, main_state_t new_main_sta
 	/* transition may be denied even if the same state is requested because conditions may have changed */
 	switch (new_main_state) {
 	case commander_state_s::MAIN_STATE_MANUAL:
-	case commander_state_s::MAIN_STATE_ACRO:
-	case commander_state_s::MAIN_STATE_RATTITUDE:
 	case commander_state_s::MAIN_STATE_STAB:
 		ret = TRANSITION_CHANGED;
 		break;
 
+	case commander_state_s::MAIN_STATE_ACRO:
+	case commander_state_s::MAIN_STATE_RATTITUDE:
+		if (status->is_rotary_wing) {
+			ret = TRANSITION_CHANGED;
+		}
+		break;
+
 	case commander_state_s::MAIN_STATE_ALTCTL:
 		/* need at minimum altitude estimate */
 		/* TODO: add this for fixedwing as well */

From 99aa5f49fcc3ead861ed21cba0c4f7d58702ffd6 Mon Sep 17 00:00:00 2001
From: Daniel Agar <daniel@agar.ca>
Date: Tue, 31 May 2016 15:38:44 -0400
Subject: [PATCH 1130/1230] FW ALTCTL requires altitude

---
 src/modules/commander/state_machine_helper.cpp | 6 ++----
 1 file changed, 2 insertions(+), 4 deletions(-)

diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp
index e979d07e04..1ebf522340 100644
--- a/src/modules/commander/state_machine_helper.cpp
+++ b/src/modules/commander/state_machine_helper.cpp
@@ -361,10 +361,8 @@ main_state_transition(struct vehicle_status_s *status, main_state_t new_main_sta
 
 	case commander_state_s::MAIN_STATE_ALTCTL:
 		/* need at minimum altitude estimate */
-		/* TODO: add this for fixedwing as well */
-		if (!status->is_rotary_wing ||
-		    (status_flags->condition_local_altitude_valid ||
-		     status_flags->condition_global_position_valid)) {
+		if (status_flags->condition_local_altitude_valid ||
+		    status_flags->condition_global_position_valid) {
 			ret = TRANSITION_CHANGED;
 		}
 		break;

From 6ab9dc0acf89a09fa91a83622fbb1beb3e00d410 Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Fri, 29 Jul 2016 13:27:58 +0200
Subject: [PATCH 1131/1230] Testing cleanup from Daniel Agar

---
 CMakeLists.txt                                |   2 +-
 Makefile                                      |  24 +-
 ROMFS/px4fmu_test/init.d/rcS                  |  69 +-
 cmake/configs/nuttx_px4fmu-v2_default.cmake   |  30 +-
 cmake/configs/nuttx_px4fmu-v2_test.cmake      |  25 +-
 cmake/configs/nuttx_px4fmu-v4_default.cmake   |  13 +-
 cmake/configs/posix_sitl_default.cmake        |   5 +-
 cmake/configs/posix_sitl_test.cmake           | 110 ---
 posix-configs/SITL/init/rcS_test_gazebo_iris  |  25 -
 posix-configs/SITL/init/rcS_test_jmavsim_iris |  25 -
 posix-configs/SITL/init/rcS_tests             |  12 +
 src/drivers/sf0x/sf0x_tests/CMakeLists.txt    |  43 ++
 .../drivers/sf0x/sf0x_tests/SF0XTest.cpp      |  53 +-
 src/lib/rc/CMakeLists.txt                     |   1 +
 src/lib/rc/rc_tests/CMakeLists.txt            |  43 ++
 src/lib/rc/rc_tests/RCTest.cpp                | 306 ++++++++
 .../lib/rc/rc_tests/test_data}/dsm_x_data.txt |   0
 .../rc/rc_tests/test_data}/sbus2_r7008SB.txt  |   0
 .../lib/rc/rc_tests/test_data}/st24_data.txt  |   0
 .../lib/rc/rc_tests/test_data}/sumd_data.txt  |   0
 src/modules/mavlink/mavlink_ftp.h             |   2 +-
 .../mavlink_tests/mavlink_ftp_test.cpp        |   4 +-
 .../uORB/uORB_tests/uORBTest_UnitTest.cpp     |   2 +-
 src/modules/unit_test/unit_test.h             |  14 +
 src/systemcmds/tests/CMakeLists.txt           |  33 +-
 src/systemcmds/tests/test_autodeclination.cpp |  38 +
 src/systemcmds/tests/test_conv.cpp            |   4 +-
 src/systemcmds/tests/test_eigen.cpp           | 564 ---------------
 src/systemcmds/tests/test_float.c             | 287 --------
 src/systemcmds/tests/test_float.cpp           | 149 ++++
 src/systemcmds/tests/test_int.c               | 152 ----
 src/systemcmds/tests/test_int.cpp             |  61 ++
 src/systemcmds/tests/test_mathlib.cpp         | 404 ++++++-----
 src/systemcmds/tests/test_matrix.cpp          | 679 ++++++++++++++++++
 src/systemcmds/tests/test_mixer.cpp           |  87 ++-
 .../systemcmds/tests/test_perf.c              |  49 +-
 src/systemcmds/tests/test_uart_baudchange.c   |  34 -
 src/systemcmds/tests/test_uart_console.c      |  34 -
 src/systemcmds/tests/tests.h                  |  72 +-
 src/systemcmds/tests/tests_main.c             | 244 ++-----
 unittests/CMakeLists.txt                      | 127 +---
 unittests/autodeclination_test.cpp            |  18 -
 unittests/conversion_test.cpp                 |  10 -
 unittests/data/fit_linear_voltage.m           |  14 -
 unittests/data/px4io_v1.3.csv                 |  70 --
 unittests/dsm_test.cpp                        |  74 --
 unittests/mixer_test.cpp                      |  12 -
 unittests/sbus2_test.cpp                      |  89 ---
 unittests/st24_test.cpp                       |  63 --
 unittests/sumd_test.cpp                       |  63 --
 unittests/uorb_stub.cpp                       |   2 -
 .../uorb_unittests/uORBCommunicatorMock.cpp   | 211 ------
 .../uorb_unittests/uORBCommunicatorMock.hpp   | 140 ----
 .../uORBCommunicatorMockLoopback.cpp          | 138 ----
 .../uORBCommunicatorMockLoopback.hpp          | 133 ----
 .../uORBCommunicator_gtests.cpp               | 483 -------------
 56 files changed, 1930 insertions(+), 3416 deletions(-)
 delete mode 100644 cmake/configs/posix_sitl_test.cmake
 delete mode 100644 posix-configs/SITL/init/rcS_test_gazebo_iris
 delete mode 100644 posix-configs/SITL/init/rcS_test_jmavsim_iris
 create mode 100644 posix-configs/SITL/init/rcS_tests
 create mode 100644 src/drivers/sf0x/sf0x_tests/CMakeLists.txt
 rename unittests/sf0x_test.cpp => src/drivers/sf0x/sf0x_tests/SF0XTest.cpp (57%)
 create mode 100644 src/lib/rc/rc_tests/CMakeLists.txt
 create mode 100644 src/lib/rc/rc_tests/RCTest.cpp
 rename {unittests/testdata => src/lib/rc/rc_tests/test_data}/dsm_x_data.txt (100%)
 rename {unittests/testdata => src/lib/rc/rc_tests/test_data}/sbus2_r7008SB.txt (100%)
 rename {unittests/testdata => src/lib/rc/rc_tests/test_data}/st24_data.txt (100%)
 rename {unittests/testdata => src/lib/rc/rc_tests/test_data}/sumd_data.txt (100%)
 create mode 100644 src/systemcmds/tests/test_autodeclination.cpp
 delete mode 100644 src/systemcmds/tests/test_eigen.cpp
 delete mode 100644 src/systemcmds/tests/test_float.c
 create mode 100644 src/systemcmds/tests/test_float.cpp
 delete mode 100644 src/systemcmds/tests/test_int.c
 create mode 100644 src/systemcmds/tests/test_int.cpp
 create mode 100644 src/systemcmds/tests/test_matrix.cpp
 rename unittests/uorb_unittests/uORBGtestTopics.hpp => src/systemcmds/tests/test_perf.c (68%)
 delete mode 100644 unittests/autodeclination_test.cpp
 delete mode 100644 unittests/conversion_test.cpp
 delete mode 100644 unittests/data/fit_linear_voltage.m
 delete mode 100644 unittests/data/px4io_v1.3.csv
 delete mode 100644 unittests/dsm_test.cpp
 delete mode 100644 unittests/mixer_test.cpp
 delete mode 100644 unittests/sbus2_test.cpp
 delete mode 100644 unittests/st24_test.cpp
 delete mode 100644 unittests/sumd_test.cpp
 delete mode 100644 unittests/uorb_unittests/uORBCommunicatorMock.cpp
 delete mode 100644 unittests/uorb_unittests/uORBCommunicatorMock.hpp
 delete mode 100644 unittests/uorb_unittests/uORBCommunicatorMockLoopback.cpp
 delete mode 100644 unittests/uorb_unittests/uORBCommunicatorMockLoopback.hpp
 delete mode 100644 unittests/uorb_unittests/uORBCommunicator_gtests.cpp

diff --git a/CMakeLists.txt b/CMakeLists.txt
index b3b7efe008..99a6061681 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -246,7 +246,7 @@ endforeach()
 px4_add_git_submodule(TARGET git_genmsg PATH "Tools/genmsg")
 px4_add_git_submodule(TARGET git_gencpp PATH "Tools/gencpp")
 px4_add_git_submodule(TARGET git_mavlink PATH "mavlink/include/mavlink/v1.0")
-px4_add_git_submodule(TARGET git_gtest PATH "unittets/gtest")
+px4_add_git_submodule(TARGET git_gtest PATH "unittests/gtest")
 px4_add_git_submodule(TARGET git_uavcan PATH "src/modules/uavcan/libuavcan")
 px4_add_git_submodule(TARGET git_nuttx PATH "NuttX")
 px4_add_git_submodule(TARGET git_driverframework PATH "src/lib/DriverFramework")
diff --git a/Makefile b/Makefile
index f65ac29c1a..4447c73093 100644
--- a/Makefile
+++ b/Makefile
@@ -174,9 +174,6 @@ posix_sitl_default:
 posix_sitl_lpe:
 	$(call cmake-build,$@)
 
-posix_sitl_test:
-	$(call cmake-build,$@)
-
 posix_sitl_replay:
 	$(call cmake-build,$@)
 
@@ -271,14 +268,14 @@ checks_uavcan: \
 	check_px4fmu-v4_default_and_uavcan
 
 checks_sitls: \
-	check_posix_sitl_default \
-	check_posix_sitl_test \
+	check_posix_sitl_default
 
 checks_last: \
-	check_unittest \
+	check_tests \
 	check_format \
 
-check: checks_defaults checks_tests checks_alts checks_uavcan checks_bootloaders checks_sitls checks_last
+check: checks_defaults checks_tests checks_alts checks_uavcan checks_bootloaders checks_last
+quick_check: check_px4fmu-v2_default check_px4fmu-v4_default check_tests check_format
 
 check_format:
 	$(call colorecho,"Checking formatting with astyle")
@@ -300,15 +297,18 @@ ifeq ($(VECTORCONTROL),1)
 	@rm -rf ROMFS/px4fmu_common/uavcan
 endif
 
-unittest: posix_sitl_test
+unittest: posix_sitl_default
 	$(call cmake-build-other,unittest, ../unittests)
 	@(cd build_unittest && ctest -j2 --output-on-failure)
 
-tests: posix_sitl_test unittest
-
-test_onboard_sitl:
-	@HEADLESS=1 make posix_sitl_test gazebo_iris
+run_tests_posix: posix_sitl_default
+	@mkdir -p build_posix_sitl_default/src/firmware/posix/rootfs/fs/microsd
+	@mkdir -p build_posix_sitl_default/src/firmware/posix/rootfs/eeprom
+	@touch build_posix_sitl_default/src/firmware/posix/rootfs/eeprom/parameters
+	@(cd build_posix_sitl_default/src/firmware/posix && ./mainapp -d ../../../../posix-configs/SITL/init/rcS_tests | tee test_output)
+	@(cd build_posix_sitl_default/src/firmware/posix && grep --color=always "All tests passed" test_output)
 
+tests: check_unittest run_tests_posix
 
 # QGroundControl flashable firmware
 qgc_firmware: \
diff --git a/ROMFS/px4fmu_test/init.d/rcS b/ROMFS/px4fmu_test/init.d/rcS
index b9eadfa865..53f7524be0 100644
--- a/ROMFS/px4fmu_test/init.d/rcS
+++ b/ROMFS/px4fmu_test/init.d/rcS
@@ -111,87 +111,22 @@ fi
 
 sh /etc/init.d/rc.sensors
 
-# Check for flow sensor
-if px4flow start
-then
-fi
-
-if ll40ls start
-then
-fi
+ver all
 
 #
 # Run unit tests at board boot, reporting failure as needed.
 # Add new unit tests using the same pattern as below.
 #
 
-echo
-echo "--------------------------------------------------------------------------------"
-echo "[mavlink_tests]             STARTING TEST"
-if mavlink_tests
-then
-	echo "[mavlink_tests]             PASS"
-else
-	set unit_test_failure 1
-	set unit_test_failure_list "${unit_test_failure_list} mavlink_tests"
-	echo "[mavlink_tests]             FAILED"
-fi
-echo "--------------------------------------------------------------------------------"
-echo
-echo "--------------------------------------------------------------------------------"
-echo "[commander_tests]             STARTING TEST"
-if commander_tests
-then
-	echo "[commander_tests]             PASS"
-else
-	set unit_test_failure 1
-	set unit_test_failure_list "${unit_test_failure_list} commander_tests"
-	echo "[commander_tests]             FAILED"
-fi
-echo "--------------------------------------------------------------------------------"
-echo
-echo "--------------------------------------------------------------------------------"
-echo "[controllib_test]             STARTING TEST"
-if controllib_test
-then
-	echo "[controllib_test]             PASS"
-else
-	set unit_test_failure 1
-	set unit_test_failure_list "${unit_test_failure_list} controllib_tests"
-	echo "[controllib_test]             FAILED"
-fi
-echo "--------------------------------------------------------------------------------"
-echo
-echo "--------------------------------------------------------------------------------"
-echo "[uorb_tests]             STARTING TEST"
-if uorb_tests
-then
-	echo "[uorb_tests]             PASS"
-else
-	set unit_test_failure 1
-	set unit_test_failure_list "${unit_test_failure_list} uorb_tests"
-	echo "[uorb_tests]             FAILED"
-fi
-echo "--------------------------------------------------------------------------------"
-
 if tests all
-then
-else
-	set unit_test_failure 1
-	set unit_test_failure_list "${unit_test_failure_list} system_tests"
-fi
-
-if [ $unit_test_failure == 0 ]
 then
 	echo
 	echo "All Unit Tests PASSED"
 	rgbled rgb 20 255 20
 else
 	echo
-	echo "Some Unit Tests FAILED:${unit_test_failure_list}"
+	echo "Some Unit Tests FAILED"
 	rgbled rgb 255 20 20
 fi
 
-ver all
-
 free
diff --git a/cmake/configs/nuttx_px4fmu-v2_default.cmake b/cmake/configs/nuttx_px4fmu-v2_default.cmake
index 8907cf82dd..c38d726702 100644
--- a/cmake/configs/nuttx_px4fmu-v2_default.cmake
+++ b/cmake/configs/nuttx_px4fmu-v2_default.cmake
@@ -53,22 +53,34 @@ set(config_module_list
 	# System commands
 	#
 	systemcmds/bl_update
+	systemcmds/config
+	systemcmds/dumpfile
+	#systemcmds/esc_calib
 	systemcmds/mixer
+	#systemcmds/motor_ramp
+	systemcmds/mtd
+	systemcmds/nshterm
 	systemcmds/param
 	systemcmds/perf
 	systemcmds/pwm
-	#systemcmds/esc_calib
 	systemcmds/reboot
-	#systemcmds/topic_listener
-	systemcmds/top
-	systemcmds/config
-	systemcmds/nshterm
-	systemcmds/mtd
-	systemcmds/dumpfile
-	systemcmds/ver
 	#systemcmds/sd_bench
+	systemcmds/top
+	#systemcmds/topic_listener
+	systemcmds/ver
+
+	#
+	# Testing
+	#
+	#drivers/sf0x/sf0x_tests
+	#drivers/test_ppm
+	#lib/rc/rc_tests
+	#modules/commander/commander_tests
+	#modules/controllib_test
+	#modules/mavlink/mavlink_tests
+	#modules/unit_test
+	#modules/uORB/uORB_tests
 	#systemcmds/tests
-	#systemcmds/motor_ramp
 
 	#
 	# General system control
diff --git a/cmake/configs/nuttx_px4fmu-v2_test.cmake b/cmake/configs/nuttx_px4fmu-v2_test.cmake
index 80f8649e3d..b72c5a8190 100644
--- a/cmake/configs/nuttx_px4fmu-v2_test.cmake
+++ b/cmake/configs/nuttx_px4fmu-v2_test.cmake
@@ -53,32 +53,34 @@ set(config_module_list
 	# System commands
 	#
 	systemcmds/bl_update
+	systemcmds/config
+	systemcmds/dumpfile
+	#systemcmds/esc_calib
 	systemcmds/mixer
+	systemcmds/motor_ramp
+	systemcmds/mtd
+	systemcmds/nshterm
 	systemcmds/param
 	systemcmds/perf
 	systemcmds/pwm
-	systemcmds/esc_calib
 	systemcmds/reboot
-	#systemcmds/topic_listener
-	systemcmds/top
-	systemcmds/config
-	systemcmds/nshterm
-	systemcmds/mtd
-	systemcmds/dumpfile
-	systemcmds/ver
 	#systemcmds/sd_bench
-	systemcmds/tests
-	systemcmds/motor_ramp
+	systemcmds/top
+	#systemcmds/topic_listener
+	systemcmds/ver
 
 	#
 	# Testing
 	#
+	drivers/sf0x/sf0x_tests
 	drivers/test_ppm
+	#lib/rc/rc_tests
 	modules/commander/commander_tests
 	modules/controllib_test
 	modules/mavlink/mavlink_tests
 	modules/unit_test
 	modules/uORB/uORB_tests
+	systemcmds/tests
 
 	#
 	# General system control
@@ -95,9 +97,10 @@ set(config_module_list
 	# Estimation modules (EKF/ SO3 / other filters)
 	#
 	modules/attitude_estimator_q
-	modules/ekf_att_pos_estimator
+	#modules/ekf_att_pos_estimator
 	modules/position_estimator_inav
 	modules/local_position_estimator
+	modules/ekf2
 
 	#
 	# Vehicle Control
diff --git a/cmake/configs/nuttx_px4fmu-v4_default.cmake b/cmake/configs/nuttx_px4fmu-v4_default.cmake
index 545dfb76da..fe356e49d6 100644
--- a/cmake/configs/nuttx_px4fmu-v4_default.cmake
+++ b/cmake/configs/nuttx_px4fmu-v4_default.cmake
@@ -69,9 +69,20 @@ set(config_module_list
 	systemcmds/dumpfile
 	systemcmds/ver
 	systemcmds/sd_bench
-	systemcmds/tests
 	systemcmds/motor_ramp
 
+	#
+	# Testing
+	#
+	drivers/sf0x/sf0x_tests
+	drivers/test_ppm
+	modules/commander/commander_tests
+	modules/controllib_test
+	modules/mavlink/mavlink_tests
+	modules/unit_test
+	modules/uORB/uORB_tests
+	systemcmds/tests
+
 	#
 	# General system control
 	#
diff --git a/cmake/configs/posix_sitl_default.cmake b/cmake/configs/posix_sitl_default.cmake
index 2de3ecedf8..6fa6d9c0b7 100644
--- a/cmake/configs/posix_sitl_default.cmake
+++ b/cmake/configs/posix_sitl_default.cmake
@@ -70,6 +70,7 @@ set(config_module_list
 	lib/launchdetection
 	lib/mathlib
 	lib/mathlib/math/filter
+	lib/rc
 	lib/runway_takeoff
 	lib/tailsitter_recovery
 	lib/terrain_estimation
@@ -79,9 +80,11 @@ set(config_module_list
 	#
 	# Testing
 	#
+	drivers/sf0x/sf0x_tests
+	lib/rc/rc_tests
 	modules/commander/commander_tests
 	modules/controllib_test
-	#modules/mavlink/mavlink_tests
+	#modules/mavlink/mavlink_tests #TODO: fix mavlink_tests
 	modules/unit_test
 	modules/uORB/uORB_tests
 	systemcmds/tests
diff --git a/cmake/configs/posix_sitl_test.cmake b/cmake/configs/posix_sitl_test.cmake
deleted file mode 100644
index c45b2fc39e..0000000000
--- a/cmake/configs/posix_sitl_test.cmake
+++ /dev/null
@@ -1,110 +0,0 @@
-include(posix/px4_impl_posix)
-
-set(CMAKE_TOOLCHAIN_FILE ${CMAKE_SOURCE_DIR}/cmake/toolchains/Toolchain-native.cmake)
-
-set(config_module_list
-	drivers/boards/sitl
-	drivers/device
-	drivers/gps
-	drivers/pwm_out_sim
-
-	platforms/common
-	platforms/posix/drivers/accelsim
-	platforms/posix/drivers/adcsim
-	platforms/posix/drivers/airspeedsim
-	platforms/posix/drivers/barosim
-	platforms/posix/drivers/gpssim
-	platforms/posix/drivers/gyrosim
-	platforms/posix/drivers/ledsim
-	platforms/posix/drivers/rgbledsim
-	platforms/posix/drivers/tonealrmsim
-	platforms/posix/px4_layer
-	platforms/posix/work_queue
-
-	systemcmds/esc_calib
-	systemcmds/mixer
-	systemcmds/param
-	systemcmds/perf
-	systemcmds/reboot
-	systemcmds/sd_bench
-	systemcmds/topic_listener
-	systemcmds/ver
-
-	modules/attitude_estimator_ekf
-	modules/attitude_estimator_q
-	modules/commander
-	modules/dataman
-	modules/ekf2
-	modules/ekf_att_pos_estimator
-	modules/fw_att_control
-	modules/fw_pos_control_l1
-	modules/land_detector
-	modules/logger
-	modules/mavlink
-	modules/mc_att_control
-	modules/mc_att_control_multiplatform
-	modules/mc_pos_control
-	modules/mc_pos_control_multiplatform
-	modules/navigator
-	modules/param
-	modules/position_estimator_inav
-	modules/local_position_estimator
-	modules/sdlog2
-	modules/sensors
-	modules/simulator
-	modules/systemlib
-	modules/systemlib/mixer
-	modules/uORB
-	modules/vtol_att_control
-
-	lib/controllib
-	lib/conversion
-	lib/DriverFramework/framework
-	lib/ecl
-	lib/external_lgpl
-	lib/geo
-	lib/geo_lookup
-	lib/launchdetection
-	lib/mathlib
-	lib/mathlib/math/filter
-	lib/runway_takeoff
-	lib/tailsitter_recovery
-	lib/terrain_estimation
-
-	examples/px4_simple_app
-
-	#
-	# Testing
-	#
-	modules/commander/commander_tests
-	modules/controllib_test
-	#modules/mavlink/mavlink_tests
-	modules/unit_test
-	modules/uORB/uORB_tests
-	systemcmds/tests
-
-	)
-
-set(config_extra_builtin_cmds
-	serdis
-	sercon
-	)
-
-set(config_sitl_rcS
-	posix-configs/SITL/init/rcS_test
-	CACHE FILEPATH "init script for sitl"
-	)
-
-set(config_sitl_viewer
-	jmavsim
-	CACHE STRING "viewer for sitl"
-	)
-set_property(CACHE config_sitl_viewer
-	PROPERTY STRINGS "jmavsim;none")
-
-set(config_sitl_debugger
-	disable
-	CACHE STRING "debugger for sitl"
-	)
-set_property(CACHE config_sitl_debugger
-	PROPERTY STRINGS "disable;gdb;lldb")
diff --git a/posix-configs/SITL/init/rcS_test_gazebo_iris b/posix-configs/SITL/init/rcS_test_gazebo_iris
deleted file mode 100644
index 8aeacf1eff..0000000000
--- a/posix-configs/SITL/init/rcS_test_gazebo_iris
+++ /dev/null
@@ -1,25 +0,0 @@
-uorb start
-param load
-
-dataman start
-
-simulator start -s
-rgbledsim start
-tone_alarm start
-gyrosim start
-accelsim start
-barosim start
-adcsim start
-gpssim start
-pwm_out_sim mode_pwm
-sleep 1
-sensors start
-
-commander_tests
-controllib_test
-uorb_tests
-tests all
-
-ver all
-
-shutdown
diff --git a/posix-configs/SITL/init/rcS_test_jmavsim_iris b/posix-configs/SITL/init/rcS_test_jmavsim_iris
deleted file mode 100644
index 8aeacf1eff..0000000000
--- a/posix-configs/SITL/init/rcS_test_jmavsim_iris
+++ /dev/null
@@ -1,25 +0,0 @@
-uorb start
-param load
-
-dataman start
-
-simulator start -s
-rgbledsim start
-tone_alarm start
-gyrosim start
-accelsim start
-barosim start
-adcsim start
-gpssim start
-pwm_out_sim mode_pwm
-sleep 1
-sensors start
-
-commander_tests
-controllib_test
-uorb_tests
-tests all
-
-ver all
-
-shutdown
diff --git a/posix-configs/SITL/init/rcS_tests b/posix-configs/SITL/init/rcS_tests
new file mode 100644
index 0000000000..c41a301e2b
--- /dev/null
+++ b/posix-configs/SITL/init/rcS_tests
@@ -0,0 +1,12 @@
+uorb start
+param load
+param set SYS_RESTART_TYPE 0
+dataman start
+
+ver all
+
+sleep 1
+
+tests all
+
+shutdown
diff --git a/src/drivers/sf0x/sf0x_tests/CMakeLists.txt b/src/drivers/sf0x/sf0x_tests/CMakeLists.txt
new file mode 100644
index 0000000000..3ded382609
--- /dev/null
+++ b/src/drivers/sf0x/sf0x_tests/CMakeLists.txt
@@ -0,0 +1,43 @@
+############################################################################
+#
+#   Copyright (c) 2016 PX4 Development Team. All rights reserved.
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions
+# are met:
+#
+# 1. Redistributions of source code must retain the above copyright
+#    notice, this list of conditions and the following disclaimer.
+# 2. Redistributions in binary form must reproduce the above copyright
+#    notice, this list of conditions and the following disclaimer in
+#    the documentation and/or other materials provided with the
+#    distribution.
+# 3. Neither the name PX4 nor the names of its contributors may be
+#    used to endorse or promote products derived from this software
+#    without specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.
+#
+############################################################################
+px4_add_module(
+	MODULE drivers__sf0x__sf0x_tests
+	MAIN sf0x_tests
+	COMPILE_FLAGS
+	SRCS
+		SF0XTest.cpp
+		../sf0x_parser.cpp
+	DEPENDS
+		platforms__common
+	)
+# vim: set noet ft=cmake fenc=utf-8 ff=unix : 
diff --git a/unittests/sf0x_test.cpp b/src/drivers/sf0x/sf0x_tests/SF0XTest.cpp
similarity index 57%
rename from unittests/sf0x_test.cpp
rename to src/drivers/sf0x/sf0x_tests/SF0XTest.cpp
index 56e673a06d..af95214c64 100644
--- a/unittests/sf0x_test.cpp
+++ b/src/drivers/sf0x/sf0x_tests/SF0XTest.cpp
@@ -1,19 +1,35 @@
-#include <stdio.h>
-#include <stdlib.h>
-#include <string.h>
-#include <unistd.h>
+#include <unit_test/unit_test.h>
 
-#include <drivers/drv_hrt.h>
 #include <drivers/sf0x/sf0x_parser.h>
+
 #include <systemlib/err.h>
 
-#include "gtest/gtest.h"
+#include <stdio.h>
+#include <unistd.h>
 
-TEST(SF0XTest, SF0X)
+extern "C" __EXPORT int sf0x_tests_main(int argc, char *argv[]);
+
+class SF0XTest : public UnitTest
+{
+public:
+	virtual bool run_tests(void);
+
+private:
+	bool sf0xTest();
+};
+
+bool SF0XTest::run_tests(void)
+{
+	ut_run_test(sf0xTest);
+
+	return (_tests_failed == 0);
+}
+
+bool SF0XTest::sf0xTest(void)
 {
 	const char _LINE_MAX = 20;
-	char _linebuf[_LINE_MAX];
-	_linebuf[0] = '\0';
+	//char _linebuf[_LINE_MAX];
+	//_linebuf[0] = '\0';
 
 	const char *lines[] = {"0.01\r\n",
 			       "0.02\r\n",
@@ -39,7 +55,6 @@ TEST(SF0XTest, SF0X)
 	unsigned _parsebuf_index = 0;
 
 	for (unsigned l = 0; l < sizeof(lines) / sizeof(lines[0]); l++) {
-
 		//printf("\n%s", _linebuf);
 
 		int parse_ret;
@@ -48,6 +63,19 @@ TEST(SF0XTest, SF0X)
 			parse_ret = sf0x_parser(lines[l][i], _parserbuf, &_parsebuf_index, &state, &dist_m);
 
 			if (parse_ret == 0) {
+				if (l == 0) {
+					ut_test(dist_m - 0.010000f < 0.001f);
+
+				} else if (l == 1) {
+					ut_test(dist_m - 0.020000f < 0.001f);
+
+				} else if (l == 2) {
+					ut_test(dist_m - 0.030000f < 0.001f);
+
+				} else if (l == 3) {
+					ut_test(dist_m - 0.040000f < 0.001f);
+				}
+
 				//printf("\nparsed: %f %s\n", dist_m, (parse_ret == 0) ? "OK" : "");
 			}
 		}
@@ -56,5 +84,8 @@ TEST(SF0XTest, SF0X)
 
 	}
 
-	warnx("test finished");
+	return true;
 }
+
+ut_declare_test_c(sf0x_tests_main, SF0XTest)
+
diff --git a/src/lib/rc/CMakeLists.txt b/src/lib/rc/CMakeLists.txt
index 15bbd71bb9..8f9471124e 100644
--- a/src/lib/rc/CMakeLists.txt
+++ b/src/lib/rc/CMakeLists.txt
@@ -34,6 +34,7 @@ px4_add_module(
 	MODULE lib__rc
 	COMPILE_FLAGS
 		-Os
+		-Wno-unused-result
 	SRCS
 		st24.c
 		sumd.c
diff --git a/src/lib/rc/rc_tests/CMakeLists.txt b/src/lib/rc/rc_tests/CMakeLists.txt
new file mode 100644
index 0000000000..42a5a1b1ba
--- /dev/null
+++ b/src/lib/rc/rc_tests/CMakeLists.txt
@@ -0,0 +1,43 @@
+############################################################################
+#
+#   Copyright (c) 2016 PX4 Development Team. All rights reserved.
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions
+# are met:
+#
+# 1. Redistributions of source code must retain the above copyright
+#    notice, this list of conditions and the following disclaimer.
+# 2. Redistributions in binary form must reproduce the above copyright
+#    notice, this list of conditions and the following disclaimer in
+#    the documentation and/or other materials provided with the
+#    distribution.
+# 3. Neither the name PX4 nor the names of its contributors may be
+#    used to endorse or promote products derived from this software
+#    without specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.
+#
+############################################################################
+px4_add_module(
+	MODULE lib__rc__rc_tests
+	MAIN rc_tests
+	COMPILE_FLAGS
+		-Wno-unused-result
+	SRCS
+		RCTest.cpp
+	DEPENDS
+		platforms__common
+	)
+# vim: set noet ft=cmake fenc=utf-8 ff=unix : 
diff --git a/src/lib/rc/rc_tests/RCTest.cpp b/src/lib/rc/rc_tests/RCTest.cpp
new file mode 100644
index 0000000000..d7a834d5c8
--- /dev/null
+++ b/src/lib/rc/rc_tests/RCTest.cpp
@@ -0,0 +1,306 @@
+#include <unit_test/unit_test.h>
+
+#include <systemlib/err.h>
+
+#include <stdio.h>
+#include <unistd.h>
+
+#include <drivers/drv_hrt.h>
+
+#define DSM_DEBUG
+#include <lib/rc/sbus.h>
+#include <lib/rc/dsm.h>
+#include <lib/rc/st24.h>
+#include <lib/rc/sumd.h>
+
+#if !defined(CONFIG_ARCH_BOARD_SITL)
+#define TEST_DATA_PATH "/fs/microsd"
+#else
+#define TEST_DATA_PATH "../../../../src/lib/rc/rc_tests/test_data/"
+#endif
+
+extern "C" __EXPORT int rc_tests_main(int argc, char *argv[]);
+
+class RCTest : public UnitTest
+{
+public:
+	virtual bool run_tests(void);
+
+private:
+	bool dsmTest();
+	bool sbus2Test();
+	bool st24Test();
+	bool sumdTest();
+};
+
+bool RCTest::run_tests(void)
+{
+	ut_run_test(dsmTest);
+	ut_run_test(sbus2Test);
+	ut_run_test(st24Test);
+	ut_run_test(sumdTest);
+
+	return (_tests_failed == 0);
+}
+
+bool RCTest::dsmTest(void)
+{
+
+	const char *filepath = TEST_DATA_PATH "dsm_x_data.txt";
+
+	FILE *fp;
+	fp = fopen(filepath, "rt");
+
+	ut_test(fp != nullptr);
+	//warnx("loading data from: %s", filepath);
+
+	float f;
+	unsigned x;
+	int ret;
+
+	// Trash the first 20 lines
+	for (unsigned i = 0; i < 20; i++) {
+		char buf[200];
+		(void)fgets(buf, sizeof(buf), fp);
+	}
+
+	// Init the parser
+	uint8_t frame[20];
+	uint16_t rc_values[18];
+	uint16_t num_values;
+	bool dsm_11_bit;
+	unsigned dsm_frame_drops = 0;
+	uint16_t max_channels = sizeof(rc_values) / sizeof(rc_values[0]);
+
+	int rate_limiter = 0;
+	unsigned last_drop = 0;
+
+	while (EOF != (ret = fscanf(fp, "%f,%x,,", &f, &x))) {
+
+		ut_test(ret > 0);
+
+		frame[0] = x;
+		unsigned len = 1;
+
+		// Pipe the data into the parser
+		bool result = dsm_parse(f * 1e6f, &frame[0], len, rc_values, &num_values,
+					&dsm_11_bit, &dsm_frame_drops, max_channels);
+
+		if (result) {
+			//warnx("decoded packet with %d channels and %s encoding:", num_values, (dsm_11_bit) ? "11 bit" : "10 bit");
+
+			for (unsigned i = 0; i < num_values; i++) {
+				//printf("chan #%u:\t%d\n", i, (int)rc_values[i]);
+			}
+		}
+
+		if (last_drop != (dsm_frame_drops)) {
+			//warnx("frame dropped, now #%d", (dsm_frame_drops));
+			last_drop = dsm_frame_drops;
+		}
+
+		rate_limiter++;
+	}
+
+	ut_test(ret == EOF);
+
+	return true;
+}
+
+bool RCTest::sbus2Test(void)
+{
+	const char *filepath = TEST_DATA_PATH "sbus2_r7008SB.txt";
+
+	FILE *fp;
+	fp = fopen(filepath, "rt");
+
+	ut_test(fp != nullptr);
+	//warnx("loading data from: %s", filepath);
+
+	// if (argc < 2)
+	// 	errx(1, "Need a filename for the input file");
+
+	//int byte_offset = 7;
+
+	// if (argc > 2) {
+	// 	char* end;
+	// 	byte_offset = strtol(argv[2],&end,10);
+	// }
+
+	//warnx("RUNNING TEST WITH BYTE OFFSET OF: %d", byte_offset);
+
+	float f;
+	unsigned x;
+	int ret;
+
+	// Trash the first 20 lines
+	for (unsigned i = 0; i < 20; i++) {
+		char buf[200];
+		(void)fgets(buf, sizeof(buf), fp);
+	}
+
+	// Init the parser
+	uint8_t frame[SBUS_BUFFER_SIZE];
+	uint16_t rc_values[18];
+	uint16_t num_values;
+	unsigned sbus_frame_drops = 0;
+	unsigned sbus_frame_resets = 0;
+	bool sbus_failsafe;
+	bool sbus_frame_drop;
+	uint16_t max_channels = sizeof(rc_values) / sizeof(rc_values[0]);
+
+	int rate_limiter = 0;
+	unsigned last_drop = 0;
+
+	while (EOF != (ret = fscanf(fp, "%f,%x,,", &f, &x))) {
+
+		ut_test(ret > 0);
+
+		frame[0] = x;
+		unsigned len = 1;
+
+		// Pipe the data into the parser
+		hrt_abstime now = hrt_absolute_time();
+
+		// if (rate_limiter % byte_offset == 0) {
+		bool result = sbus_parse(now, &frame[0], len, rc_values, &num_values,
+					 &sbus_failsafe, &sbus_frame_drop, &sbus_frame_drops, max_channels);
+
+		if (result) {
+			//warnx("decoded packet");
+		}
+
+		// }
+
+		if (last_drop != (sbus_frame_drops + sbus_frame_resets)) {
+			warnx("frame dropped, now #%d", (sbus_frame_drops + sbus_frame_resets));
+			last_drop = sbus_frame_drops + sbus_frame_resets;
+		}
+
+		rate_limiter++;
+	}
+
+	ut_test(ret == EOF);
+
+	return true;
+}
+
+bool RCTest::st24Test(void)
+{
+	const char *filepath = TEST_DATA_PATH "st24_data.txt";
+
+	//warnx("loading data from: %s", filepath);
+
+	FILE *fp;
+
+	fp = fopen(filepath, "rt");
+	ut_test(fp != nullptr);
+
+	float f;
+	unsigned x;
+	int ret;
+
+	// Trash the first 20 lines
+	for (unsigned i = 0; i < 20; i++) {
+		char buf[200];
+		(void)fgets(buf, sizeof(buf), fp);
+	}
+
+	float last_time = 0;
+
+	while (EOF != (ret = fscanf(fp, "%f,%x,,", &f, &x))) {
+		ut_test(ret > 0);
+
+		if (((f - last_time) * 1000 * 1000) > 3000) {
+			// warnx("FRAME RESET\n\n");
+		}
+
+		uint8_t b = static_cast<uint8_t>(x);
+
+		last_time = f;
+
+		// Pipe the data into the parser
+		//hrt_abstime now = hrt_absolute_time();
+
+		uint8_t rssi;
+		uint8_t rx_count;
+		uint16_t channel_count;
+		uint16_t channels[20];
+
+		if (!st24_decode(b, &rssi, &rx_count, &channel_count, channels, sizeof(channels) / sizeof(channels[0]))) {
+			//warnx("decoded: %u channels (converted to PPM range)", (unsigned)channel_count);
+
+			for (unsigned i = 0; i < channel_count; i++) {
+				//int16_t val = channels[i];
+				//warnx("channel %u: %d 0x%03X", i, static_cast<int>(val), static_cast<int>(val));
+			}
+		}
+	}
+
+	ut_test(ret == EOF);
+
+	return true;
+}
+
+bool RCTest::sumdTest(void)
+{
+	const char *filepath = TEST_DATA_PATH "sumd_data.txt";
+
+	//warnx("loading data from: %s", filepath);
+
+	FILE *fp;
+
+	fp = fopen(filepath, "rt");
+	ut_test(fp != nullptr);
+
+	float f;
+	unsigned x;
+	int ret;
+
+	// Trash the first 20 lines
+	for (unsigned i = 0; i < 20; i++) {
+		char buf[200];
+		(void)fgets(buf, sizeof(buf), fp);
+	}
+
+	float last_time = 0;
+
+	while (EOF != (ret = fscanf(fp, "%f,%x,,", &f, &x))) {
+		ut_test(ret > 0);
+
+		if (((f - last_time) * 1000 * 1000) > 3000) {
+			// warnx("FRAME RESET\n\n");
+		}
+
+		uint8_t b = static_cast<uint8_t>(x);
+
+		last_time = f;
+
+		// Pipe the data into the parser
+		//hrt_abstime now = hrt_absolute_time();
+
+		uint8_t rssi;
+		uint8_t rx_count;
+		uint16_t channel_count;
+		uint16_t channels[32];
+
+
+		if (!sumd_decode(b, &rssi, &rx_count, &channel_count, channels, 32)) {
+			//warnx("decoded: %u channels (converted to PPM range)", (unsigned)channel_count);
+
+			for (unsigned i = 0; i < channel_count; i++) {
+				//int16_t val = channels[i];
+				//warnx("channel %u: %d 0x%03X", i, static_cast<int>(val), static_cast<int>(val));
+			}
+		}
+	}
+
+	ut_test(ret == EOF);
+
+	return true;
+}
+
+
+
+ut_declare_test_c(rc_tests_main, RCTest)
+
diff --git a/unittests/testdata/dsm_x_data.txt b/src/lib/rc/rc_tests/test_data/dsm_x_data.txt
similarity index 100%
rename from unittests/testdata/dsm_x_data.txt
rename to src/lib/rc/rc_tests/test_data/dsm_x_data.txt
diff --git a/unittests/testdata/sbus2_r7008SB.txt b/src/lib/rc/rc_tests/test_data/sbus2_r7008SB.txt
similarity index 100%
rename from unittests/testdata/sbus2_r7008SB.txt
rename to src/lib/rc/rc_tests/test_data/sbus2_r7008SB.txt
diff --git a/unittests/testdata/st24_data.txt b/src/lib/rc/rc_tests/test_data/st24_data.txt
similarity index 100%
rename from unittests/testdata/st24_data.txt
rename to src/lib/rc/rc_tests/test_data/st24_data.txt
diff --git a/unittests/testdata/sumd_data.txt b/src/lib/rc/rc_tests/test_data/sumd_data.txt
similarity index 100%
rename from unittests/testdata/sumd_data.txt
rename to src/lib/rc/rc_tests/test_data/sumd_data.txt
diff --git a/src/modules/mavlink/mavlink_ftp.h b/src/modules/mavlink/mavlink_ftp.h
index 998c6b3cc6..017ac07d67 100644
--- a/src/modules/mavlink/mavlink_ftp.h
+++ b/src/modules/mavlink/mavlink_ftp.h
@@ -50,7 +50,7 @@ class MavlinkFtpTest;
 class MavlinkFTP : public MavlinkStream
 {
 public:
-	/// @brief Contructor is only public so unit test code can new objects.
+	/// @brief Constructor is only public so unit test code can new objects.
 	MavlinkFTP(Mavlink *mavlink);
 	~MavlinkFTP();
 
diff --git a/src/modules/mavlink/mavlink_tests/mavlink_ftp_test.cpp b/src/modules/mavlink/mavlink_tests/mavlink_ftp_test.cpp
index e45b91252e..39af11f955 100644
--- a/src/modules/mavlink/mavlink_tests/mavlink_ftp_test.cpp
+++ b/src/modules/mavlink/mavlink_tests/mavlink_ftp_test.cpp
@@ -236,7 +236,7 @@ bool MavlinkFtpTest::_list_test(void)
 	return true;
 }
 
-/// @brief Tests for correct reponse to a List command on a valid directory, but with an offset that
+/// @brief Tests for correct response to a List command on a valid directory, but with an offset that
 /// is beyond the last directory entry.
 bool MavlinkFtpTest::_list_eof_test(void)
 {
@@ -263,7 +263,7 @@ bool MavlinkFtpTest::_list_eof_test(void)
 	return true;
 }
 
-/// @brief Tests for correct reponse to an Open command on a file which does not exist.
+/// @brief Tests for correct response to an Open command on a file which does not exist.
 bool MavlinkFtpTest::_open_badfile_test(void)
 {
 	MavlinkFTP::PayloadHeader		payload;
diff --git a/src/modules/uORB/uORB_tests/uORBTest_UnitTest.cpp b/src/modules/uORB/uORB_tests/uORBTest_UnitTest.cpp
index 7bcf9d7e4f..0ff04ed402 100644
--- a/src/modules/uORB/uORB_tests/uORBTest_UnitTest.cpp
+++ b/src/modules/uORB/uORB_tests/uORBTest_UnitTest.cpp
@@ -130,7 +130,7 @@ int uORBTest::UnitTest::pubsublatency_main(void)
 
 	pubsubtest_passed = true;
 
-	if (static_cast<float>(latency_integral / maxruns) > 40.0f) {
+	if (static_cast<float>(latency_integral / maxruns) > 80.0f) {
 		pubsubtest_res = uORB::ERROR;
 
 	} else {
diff --git a/src/modules/unit_test/unit_test.h b/src/modules/unit_test/unit_test.h
index f512100bb6..aeddaac88e 100644
--- a/src/modules/unit_test/unit_test.h
+++ b/src/modules/unit_test/unit_test.h
@@ -37,6 +37,17 @@
 
 #include <systemlib/err.h>
 
+#define ut_declare_test_c(test_function, test_class)	\
+	extern "C" {										\
+		int test_function(int argc, char *argv[])		\
+		{												\
+			test_class* test = new test_class();		\
+			bool success = test->run_tests();			\
+			test->print_results();						\
+			return success ? 0 : -1;					\
+		}												\
+	}
+
 /// @brief Base class to be used for unit tests.
 class __EXPORT UnitTest
 {
@@ -92,6 +103,9 @@ protected:
 		}								\
 	} while (0)
 
+/// @brief Used to assert a value within a unit test.
+#define ut_test(test) ut_assert("test", test)
+
 /// @brief Used to compare two integer values within a unit test. If possible use ut_compare instead of ut_assert
 /// since it will give you better error reporting of the actual values being compared.
 #define ut_compare(message, v1, v2)								\
diff --git a/src/systemcmds/tests/CMakeLists.txt b/src/systemcmds/tests/CMakeLists.txt
index bbd989058c..06e6d86225 100644
--- a/src/systemcmds/tests/CMakeLists.txt
+++ b/src/systemcmds/tests/CMakeLists.txt
@@ -33,14 +33,26 @@
 
 set(srcs
 	test_adc.c
+	test_autodeclination.cpp
 	test_bson.c
-	test_float.c
+	test_conv.cpp
+	test_file.c
+	test_file2.c
+	test_float.cpp
 	test_gpio.c
 	test_hott_telemetry.c
 	test_hrt.c
-	test_int.c
+	test_int.cpp
 	test_jig_voltages.c
 	test_led.c
+	test_mathlib.cpp
+	test_matrix.cpp
+	test_mixer.cpp
+	test_mount.c
+	test_params.c
+	test_perf.c
+	test_ppm_loopback.c
+	test_rc.c
 	test_sensors.c
 	test_servo.c
 	test_sleep.c
@@ -48,16 +60,7 @@ set(srcs
 	test_uart_console.c
 	test_uart_loopback.c
 	test_uart_send.c
-	test_mixer.cpp
-	test_mathlib.cpp
-	test_file.c
-	test_file2.c
 	tests_main.c
-	test_params.c
-	test_ppm_loopback.c
-	test_rc.c
-	test_conv.cpp
-	test_mount.c
 	)
 
 if(${OS} STREQUAL "nuttx")
@@ -69,11 +72,15 @@ endif()
 px4_add_module(
 	MODULE systemcmds__tests
 	MAIN tests
-	STACK_MAIN 9000
-	STACK_MAX 9000
+	STACK_MAIN 10000
+	STACK_MAX 10000
 	COMPILE_FLAGS
+		${MODULE_CFLAGS}
 		-Wno-unused-result
 		-Wno-float-equal
+		-Wno-missing-declarations
+		-Wno-double-promotion
+		-Wno-unknown-warning-option
 		-Os
 	SRCS ${srcs}
 	DEPENDS
diff --git a/src/systemcmds/tests/test_autodeclination.cpp b/src/systemcmds/tests/test_autodeclination.cpp
new file mode 100644
index 0000000000..af66c517ca
--- /dev/null
+++ b/src/systemcmds/tests/test_autodeclination.cpp
@@ -0,0 +1,38 @@
+#include <unit_test/unit_test.h>
+
+#include <drivers/drv_hrt.h>
+#include <geo/geo.h>
+#include <px4iofirmware/px4io.h>
+#include <systemlib/err.h>
+#include <systemlib/mixer/mixer.h>
+
+#include <math.h>
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+#include <unistd.h>
+
+class AutoDeclinationTest : public UnitTest
+{
+public:
+	virtual bool run_tests(void);
+
+private:
+	bool autodeclination_check();
+};
+
+bool AutoDeclinationTest::autodeclination_check(void)
+{
+	ut_assert("declination differs more than 1 degree", get_mag_declination(47.0, 8.0) - 0.6f < 0.5f);
+
+	return true;
+}
+
+bool AutoDeclinationTest::run_tests(void)
+{
+	ut_run_test(autodeclination_check);
+
+	return (_tests_failed == 0);
+}
+
+ut_declare_test_c(test_autodeclination, AutoDeclinationTest)
diff --git a/src/systemcmds/tests/test_conv.cpp b/src/systemcmds/tests/test_conv.cpp
index e451755959..c26455eda8 100644
--- a/src/systemcmds/tests/test_conv.cpp
+++ b/src/systemcmds/tests/test_conv.cpp
@@ -59,7 +59,7 @@
 
 int test_conv(int argc, char *argv[])
 {
-	PX4_INFO("Testing system conversions");
+	//PX4_INFO("Testing system conversions");
 
 	for (int i = -10000; i <= 10000; i += 1) {
 		float f = i / 10000.0f;
@@ -72,7 +72,7 @@ int test_conv(int argc, char *argv[])
 		}
 	}
 
-	PX4_INFO("All conversions clean");
+	//PX4_INFO("All conversions clean");
 
 	return 0;
 }
diff --git a/src/systemcmds/tests/test_eigen.cpp b/src/systemcmds/tests/test_eigen.cpp
deleted file mode 100644
index 2ede1ad66c..0000000000
--- a/src/systemcmds/tests/test_eigen.cpp
+++ /dev/null
@@ -1,564 +0,0 @@
-/****************************************************************************
-*
-*   Copyright (c) 2013-2015 PX4 Development Team. All rights reserved.
-*
-* Redistribution and use in source and binary forms, with or without
-* modification, are permitted provided that the following conditions
-* are met:
-*
-* 1. Redistributions of source code must retain the above copyright
-*    notice, this list of conditions and the following disclaimer.
-* 2. Redistributions in binary form must reproduce the above copyright
-*    notice, this list of conditions and the following disclaimer in
-*    the documentation and/or other materials provided with the
-*    distribution.
-* 3. Neither the name PX4 nor the names of its contributors may be
-*    used to endorse or promote products derived from this software
-*    without specific prior written permission.
-*
-* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
-* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
-* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
-* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
-* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
-* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
-* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
-* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
-* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
-* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
-* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
-* POSSIBILITY OF SUCH DAMAGE.
-*
-****************************************************************************/
-
-/**
- * @file test_eigen.cpp
- *
- * Eigen test (based of mathlib test)
- * @author Johan Jansen <jnsn.johan@gmail.com>
- * @author Nuno Marques <n.marques21@hotmail.com>
- */
-
-#include <cmath>
-#include <px4_eigen.h>
-#include <mathlib/mathlib.h>
-#include <float.h>
-#include <stdio.h>
-#include <stdlib.h>
-#include <unistd.h>
-#include <string.h>
-#include <time.h>
-#include <systemlib/err.h>
-#include <drivers/drv_hrt.h>
-
-#include "tests.h"
-
-using namespace Eigen;
-
-typedef Matrix<float, 10, 1> Vector10f;
-
-static constexpr size_t OPERATOR_ITERATIONS = 30000;
-
-const float min = -M_PI_F;
-const float max = M_PI_F;
-const float step = M_PI_F / 12;
-const float epsilon_f = 1E-4;
-uint8_t err_num;
-
-#define TEST_OP(_title, _op)															\
-	{																						\
-		const hrt_abstime t0 = hrt_absolute_time();						\
-		for (size_t j = 0; j < OPERATOR_ITERATIONS; j++) {									\
-			_op;																			\
-		}																					\
-		printf("%llu: %s: finished in: %.6fus\n", (unsigned long long)t0, _title, static_cast<double>(hrt_absolute_time() - t0) / OPERATOR_ITERATIONS); \
-	}
-
-#define VERIFY_OP(_title, _op, __OP_TEST__)												\
-	{																						\
-		_op;																				\
-		if (!(__OP_TEST__)) {																 \
-			printf(_title ": Failed! (" # __OP_TEST__ ")\n");									   \
-			++err_num;																	\
-		}																					\
-	}
-
-#define TEST_OP_VERIFY(_title, _op, __OP_TEST__)										\
-	VERIFY_OP(_title, _op, __OP_TEST__)												\
-	TEST_OP(_title, _op)
-
-#define EXPECT_QUATERNION(q_exp, q_act, epsilon)	\
-	(fabsf(q_exp.x() - q_act.x()) <= epsilon &&			\
-	 fabsf(q_exp.y() - q_act.y()) <= epsilon && 			\
-	 fabsf(q_exp.z() - q_act.z()) <= epsilon &&			\
-	 fabsf(q_exp.w() - q_act.w()) <= epsilon)
-
-#define EXPECT_NEAR(expected, actual, epsilon)		\
-	(fabsf(expected - actual) <= epsilon)
-
-/**
- * @brief
- *	Prints an Eigen::Matrix to stdout
- **/
-template<typename T>
-void printEigen(const Eigen::MatrixBase<T> &b)
-{
-	for (int i = 0; i < b.rows(); ++i) {
-		printf("(");
-
-		for (int j = 0; j < b.cols(); ++j) {
-			if (j > 0) {
-				printf(",");
-			}
-
-			printf("%.3f", static_cast<double>(b(i, j)));
-		}
-
-		printf(")%s\n", i + 1 < b.rows() ? "," : "");
-	}
-}
-
-// Methods definition
-Eigen::Quaternionf quatFromEuler(const Eigen::Vector3f &rpy);
-Eigen::Vector3f eulerFromQuat(const Eigen::Quaternionf &q);
-Eigen::Matrix3f matrixFromEuler(const Eigen::Vector3f &rpy);
-Eigen::Quaternionf eigenqFromPx4q(const math::Quaternion &q);
-math::Quaternion px4qFromEigenq(const Eigen::Quaternionf &q);
-
-/**
- * @brief
- *	Construct new Eigen::Quaternion from euler angles
- *	Right order is YPR.
- **/
-Eigen::Quaternionf quatFromEuler(const Eigen::Vector3f &rpy)
-{
-	return Eigen::Quaternionf(
-		       Eigen::AngleAxisf(rpy.z(), Eigen::Vector3f::UnitZ()) *
-		       Eigen::AngleAxisf(rpy.y(), Eigen::Vector3f::UnitY()) *
-		       Eigen::AngleAxisf(rpy.x(), Eigen::Vector3f::UnitX())
-	       );
-}
-
-/**
- * @brief
- *	Construct new Eigen::Vector3f of euler angles from quaternion
- *	Right order is YPR.
- **/
-Eigen::Vector3f eulerFromQuat(const Eigen::Quaternionf &q)
-{
-	return q.toRotationMatrix().eulerAngles(2, 1, 0).reverse();
-}
-
-/**
- * @brief
- *	Construct new Eigen::Matrix3f from euler angles
- **/
-Eigen::Matrix3f matrixFromEuler(const Eigen::Vector3f &rpy)
-{
-	return quatFromEuler(rpy).toRotationMatrix();
-}
-
-/**
- * @brief
- *	Adjust PX4 math::quaternion to Eigen::Quaternionf
- **/
-Eigen::Quaternionf eigenqFromPx4q(const math::Quaternion &q)
-{
-	return Eigen::Quaternionf(q.data[1], q.data[2], q.data[3], q.data[0]);
-}
-
-/**
- * @brief
- *	Adjust Eigen::Quaternionf to PX4 math::quaternion
- **/
-math::Quaternion px4qFromEigenq(const Eigen::Quaternionf &q)
-{
-	return math::Quaternion(q.w(), q.x(), q.y(), q.z());
-}
-
-/**
- * @brief
- *	Testing main routine
- **/
-int test_eigen(int argc, char *argv[])
-{
-	int rc = 0;
-	warnx("Testing Eigen math...");
-
-	{
-		Eigen::Vector2f v;
-		Eigen::Vector2f v1(1.0f, 2.0f);
-		Eigen::Vector2f v2(1.0f, -1.0f);
-		float data[2] = {1.0f, 2.0f};
-		TEST_OP("Constructor Vector2f()", Eigen::Vector2f v3);
-		TEST_OP_VERIFY("Constructor Vector2f(Vector2f)", Eigen::Vector2f v3(v1), v3.isApprox(v1));
-		TEST_OP_VERIFY("Constructor Vector2f(float[])", Eigen::Vector2f v3(data), v3[0] == data[0] && v3[1] == data[1]);
-		TEST_OP_VERIFY("Constructor Vector2f(float, float)", Eigen::Vector2f v3(1.0f, 2.0f), v3(0) == 1.0f && v3(1) == 2.0f);
-		TEST_OP_VERIFY("Vector2f = Vector2f", v = v1, v.isApprox(v1));
-		VERIFY_OP("Vector2f + Vector2f", v = v + v1, v.isApprox(v1 + v1));
-		VERIFY_OP("Vector2f - Vector2f", v = v - v1, v.isApprox(v1));
-		VERIFY_OP("Vector2f += Vector2f", v += v1, v.isApprox(v1 + v1));
-		VERIFY_OP("Vector2f -= Vector2f", v -= v1, v.isApprox(v1));
-		TEST_OP_VERIFY("Vector2f dot Vector2f", v.dot(v1), fabs(v.dot(v1) - 5.0f) <= FLT_EPSILON);
-	}
-
-	{
-		Eigen::Vector3f v;
-		Eigen::Vector3f v1(1.0f, 2.0f, 0.0f);
-		Eigen::Vector3f v2(1.0f, -1.0f, 2.0f);
-		float data[3] = {1.0f, 2.0f, 3.0f};
-		TEST_OP("Constructor Vector3f()", Eigen::Vector3f v3);
-		TEST_OP("Constructor Vector3f(Vector3f)", Eigen::Vector3f v3(v1));
-		TEST_OP("Constructor Vector3f(float[])", Eigen::Vector3f v3(data));
-		TEST_OP("Constructor Vector3f(float, float, float)", Eigen::Vector3f v3(1.0f, 2.0f, 3.0f));
-		TEST_OP("Vector3f = Vector3f", v = v1);
-		TEST_OP("Vector3f + Vector3f", v + v1);
-		TEST_OP("Vector3f - Vector3f", v - v1);
-		TEST_OP("Vector3f += Vector3f", v += v1);
-		TEST_OP("Vector3f -= Vector3f", v -= v1);
-		TEST_OP("Vector3f * float", v1 * 2.0f);
-		TEST_OP("Vector3f / float", v1 / 2.0f);
-		TEST_OP("Vector3f *= float", v1 *= 2.0f);
-		TEST_OP("Vector3f /= float", v1 /= 2.0f);
-		TEST_OP("Vector3f dot Vector3f", v.dot(v1));
-		TEST_OP("Vector3f cross Vector3f", v1.cross(v2));
-		TEST_OP("Vector3f length", v1.norm());
-		TEST_OP("Vector3f length squared", v1.squaredNorm());
-#pragma GCC diagnostic push
-#pragma GCC diagnostic ignored "-Wunused-variable"
-		// Need pragma here instead of moving variable out of TEST_OP and just reference because
-		// TEST_OP measures performance of vector operations.
-		TEST_OP("Vector<3> element read", volatile float a = v1(0));
-		TEST_OP("Vector<3> element read direct", volatile float a = v1.data()[0]);
-#pragma GCC diagnostic pop
-		TEST_OP("Vector<3> element write", v1(0) = 1.0f);
-		TEST_OP("Vector<3> element write direct", v1.data()[0] = 1.0f);
-	}
-
-	{
-		Eigen::Vector4f v(0.0f, 0.0f, 0.0f, 0.0f);
-		Eigen::Vector4f v1(1.0f, 2.0f, 0.0f, -1.0f);
-		Eigen::Vector4f v2(1.0f, -1.0f, 2.0f, 0.0f);
-		Eigen::Vector4f vres;
-		float data[4] = {1.0f, 2.0f, 3.0f, 4.0f};
-		TEST_OP("Constructor Vector<4>()", Eigen::Vector4f v3);
-		TEST_OP("Constructor Vector<4>(Vector<4>)", Eigen::Vector4f v3(v1));
-		TEST_OP("Constructor Vector<4>(float[])", Eigen::Vector4f v3(data));
-		TEST_OP("Constructor Vector<4>(float, float, float, float)", Eigen::Vector4f v3(1.0f, 2.0f, 3.0f, 4.0f));
-		TEST_OP("Vector<4> = Vector<4>", v = v1);
-		TEST_OP("Vector<4> + Vector<4>", v + v1);
-		TEST_OP("Vector<4> - Vector<4>", v - v1);
-		TEST_OP("Vector<4> += Vector<4>", v += v1);
-		TEST_OP("Vector<4> -= Vector<4>", v -= v1);
-		TEST_OP("Vector<4> dot Vector<4>", v.dot(v1));
-	}
-
-	{
-		Vector10f v1;
-		v1.Zero();
-		float data[10];
-		TEST_OP("Constructor Vector<10>()", Vector10f v3);
-		TEST_OP("Constructor Vector<10>(Vector<10>)", Vector10f v3(v1));
-		TEST_OP("Constructor Vector<10>(float[])", Vector10f v3(data));
-	}
-
-	{
-		Eigen::Matrix3f m1;
-		m1.setIdentity();
-		Eigen::Matrix3f m2;
-		m2.setIdentity();
-		Eigen::Vector3f v1(1.0f, 2.0f, 0.0f);
-		TEST_OP("Matrix3f * Vector3f", m1 * v1);
-		TEST_OP("Matrix3f + Matrix3f", m1 + m2);
-		TEST_OP("Matrix3f * Matrix3f", m1 * m2);
-	}
-
-	{
-		Eigen::Matrix<float, 10, 10> m1;
-		m1.setIdentity();
-		Eigen::Matrix<float, 10, 10> m2;
-		m2.setIdentity();
-		Vector10f v1;
-		v1.Zero();
-		TEST_OP("Matrix<10, 10> * Vector<10>", m1 * v1);
-		TEST_OP("Matrix<10, 10> + Matrix<10, 10>", m1 + m2);
-		TEST_OP("Matrix<10, 10> * Matrix<10, 10>", m1 * m2);
-	}
-
-	{
-		printf("%llu: Nonsymmetric matrix operations test...\n", (unsigned long long)hrt_absolute_time());
-		// test nonsymmetric +, -, +=, -=
-
-		const Eigen::Matrix<float, 2, 3> m1_orig =
-			(Eigen::Matrix<float, 2, 3>() << 1, 2, 3,
-			 4, 5, 6).finished();
-
-		Eigen::Matrix<float, 2, 3> m1(m1_orig);
-
-		Eigen::Matrix<float, 2, 3> m2;
-		m2 << 2, 4, 6,
-		8, 10, 12;
-
-		Eigen::Matrix<float, 2, 3> m3;
-		m3 << 3, 6, 9,
-		12, 15, 18;
-
-		if (m1 + m2 != m3) {
-			printf("%llu: Matrix<2, 3> + Matrix<2, 3> failed!\n", (unsigned long long)hrt_absolute_time());
-			printEigen(m1 + m2);
-			printf("!=\n");
-			printEigen(m3);
-			++err_num;
-			rc = 1;
-		}
-
-		if (m3 - m2 != m1) {
-			printf("%llu: Matrix<2, 3> - Matrix<2, 3> failed!\n", (unsigned long long)hrt_absolute_time());
-			printEigen(m3 - m2);
-			printf("!=\n");
-			printEigen(m1);
-			++err_num;
-			rc = 1;
-		}
-
-		m1 += m2;
-
-		if (m1 != m3) {
-			printf("%llu: Matrix<2, 3> += Matrix<2, 3> failed!\n", (unsigned long long)hrt_absolute_time());
-			printEigen(m1);
-			printf("!=\n");
-			printEigen(m3);
-			++err_num;
-			rc = 1;
-		}
-
-		m1 -= m2;
-
-		if (m1 != m1_orig) {
-			printf("%llu: Matrix<2, 3> -= Matrix<2, 3> failed!\n", (unsigned long long)hrt_absolute_time());
-			printEigen(m1);
-			printf("!=\n");
-			printEigen(m1_orig);
-			++err_num;
-			rc = 1;
-		}
-	}
-
-	/* QUATERNION TESTS */
-	{
-		// Test conversion rotation matrix to quaternion and back
-		// against existing PX4 mathlib
-		math::Matrix<3, 3> R_orig;
-		Eigen::Quaternionf q_true;
-		Eigen::Quaternionf q;
-		Eigen::Matrix3f R;
-
-		printf("%llu: Conversion method: Quaternion transformation methods test...\n", (unsigned long long)hrt_absolute_time());
-		printf("%llu: Conversion method: Testing known values...\n", (unsigned long long)hrt_absolute_time());
-
-		/******************************************** TEST 1 ****************************************************/
-		q_true = {0.0f, 0.0f, 0.0f, 1.0f};
-		math::Quaternion q_px4 = {1.0f, 0.0f, 0.0f, 0.0f};
-		Eigen::Quaternionf q_eigen(eigenqFromPx4q(q_px4));
-
-		if (!EXPECT_QUATERNION(q_true, q_eigen, FLT_EPSILON)) {
-			printf("%llu: Value of: Quaternion1 [1.0, 0.0, 0.0, 0.0]\n", (unsigned long long)hrt_absolute_time());
-			printf("Actual:  \t[%8.5f, %8.5f, %8.5f, %8.5f]\n", q_eigen.x(), q_eigen.y(), q_eigen.z(), q_eigen.w());
-			printf("Expected: \t[%8.5f, %8.5f, %8.5f, %8.5f]\n", q_true.x(), q_true.y(), q_true.z(), q_true.w());
-			++err_num;
-			rc = 1;
-		}
-
-		/********************************************************************************************************/
-		/******************************************** TEST 2 ****************************************************/
-		q_true = {1.0f, 0.0f, 0.0f, 0.0f};
-		Eigen::Quaternionf q2_eigen = {0.0f, 0.0f, 0.0f, 1.0f};
-		math::Quaternion q2_px4(px4qFromEigenq(q2_eigen));
-		Eigen::Quaternionf q2_eigen_(q2_px4.data[3], q2_px4.data[0], q2_px4.data[1], q2_px4.data[2]);
-
-		if (!EXPECT_QUATERNION(q_true, q2_eigen_, FLT_EPSILON)) {
-			printf("%llu: Value of: Quaternion2 [0.0, 0.0, 0.0, 1.0]\n", (unsigned long long)hrt_absolute_time());
-			printf("Actual:  \t[%8.5f, %8.5f, %8.5f, %8.5f]\n", q2_px4.data[0], q2_px4.data[1], q2_px4.data[2], q2_px4.data[3]);
-			printf("Expected: \t[%8.5f, %8.5f, %8.5f, %8.5f]\n", q_true.x(), q_true.y(), q_true.z(), q_true.w());
-			++err_num;
-			rc = 1;
-		}
-
-		/********************************************************************************************************/
-		/******************************************** TEST 3 ****************************************************/
-		q_true = quatFromEuler(Eigen::Vector3f(0.3f, 0.2f, 0.1f));
-		q = {0.9833474432563558f, 0.14357217502739184f, 0.10602051106179561f, 0.0342707985504821f};
-
-		if (!EXPECT_QUATERNION(q_true, q, FLT_EPSILON)) {
-			printf("%llu: Value of: Quaternion [0.9833, 0.1436, 0.1060, 0.0343]\n", (unsigned long long)hrt_absolute_time());
-			printf("Actual:  \t[%8.5f, %8.5f, %8.5f, %8.5f]\n", q.w(), q.x(), q.y(), q.z());
-			printf("Expected: \t[%8.5f, %8.5f, %8.5f, %8.5f]\n", q_true.w(), q_true.x(), q_true.y(), q_true.z());
-			++err_num;
-			rc = 1;
-		}
-
-		/********************************************************************************************************/
-		/******************************************** TEST 4 ****************************************************/
-		q_true = quatFromEuler(Eigen::Vector3f(-1.5f, -0.2f, 0.5f));
-		q = {0.7222365948153096f, -0.6390766544101811f, -0.2385737751841646f, 0.11418355701173476f};
-
-		if (!EXPECT_QUATERNION(q_true, q, FLT_EPSILON)) {
-			printf("%llu: Value of: Quaternion [0.7222, -0.6391, -0.2386, 0.1142]\n", (unsigned long long)hrt_absolute_time());
-			printf("Actual:  \t[%8.5f, %8.5f, %8.5f, %8.5f]\n", q.w(), q.x(), q.y(), q.z());
-			printf("Expected: \t[%8.5f, %8.5f, %8.5f, %8.5f]\n", q_true.w(), q_true.x(), q_true.y(), q_true.z());
-			++err_num;
-			rc = 1;
-		}
-
-		/********************************************************************************************************/
-		/******************************************** TEST 5 ****************************************************/
-		q_true = quatFromEuler(Eigen::Vector3f(M_PI_2_F, -M_PI_2_F, -M_PI_F / 3));
-		q = {0.6830127018922193f, 0.18301270189221933f, -0.6830127018922193f, 0.18301270189221933f};
-
-		for (size_t i = 0; i < 4; i++) {
-			if (!EXPECT_QUATERNION(q_true, q, FLT_EPSILON)) {
-				printf("%llu: It[%d]: Value of: Quaternion [0.6830, 0.1830, -0.6830, 0.1830]\n",
-				       (unsigned long long)hrt_absolute_time(), (int)i);
-				printf("Actual:  \t[%8.5f, %8.5f, %8.5f, %8.5f]\n", q.w(), q.x(), q.y(), q.z());
-				printf("Expected: \t[%8.5f, %8.5f, %8.5f, %8.5f]\n", q_true.w(), q_true.x(), q_true.y(), q_true.z());
-				++err_num;
-				rc = 1;
-			}
-		}
-
-		/********************************************************************************************************/
-		/******************************************** TEST 6 ****************************************************/
-		printf("%llu: Conversion method: Testing transformation range...\n", (unsigned long long)hrt_absolute_time());
-
-		for (float roll = min; roll <= max; roll += step) {
-			for (float pitch = min; pitch <= max; pitch += step) {
-				for (float yaw = min; yaw <= max; yaw += step) {
-
-					q = Eigen::Quaternionf(quatFromEuler(Eigen::Vector3f(roll, pitch, yaw)));
-
-					R = q.toRotationMatrix();
-					R_orig.from_euler(roll, pitch, yaw);
-
-					for (size_t i = 0; i < 3; i++) {
-						for (size_t j = 0; j < 3; j++) {
-							if (!EXPECT_NEAR(R_orig(i, j), R(i, j), epsilon_f)) {
-								printf("%llu: (%d, %d) Value of: Quaternion constructor or 'toRotationMatrix'\n",
-								       (unsigned long long)hrt_absolute_time(), (int)i, (int)j);
-								printf("Actual:  \t%8.5f\n", R(i, j));
-								printf("Expected: \t%8.5f\n", R_orig(i, j));
-								++err_num;
-								rc = 1;
-							}
-						}
-					}
-				}
-			}
-		}
-	}
-
-	{
-		// Test rotation method (rotate vector by quaternion)
-		Eigen::Vector3f vector = {1.0f, 1.0f, 1.0f};
-		Eigen::Vector3f vector_q;
-		Eigen::Vector3f vector_r;
-		Eigen::Quaternionf q;
-		Eigen::Matrix3f R;
-
-		printf("%llu: Rotation method: Quaternion vector rotation method test...\n", (unsigned long long)hrt_absolute_time());
-		printf("%llu: Rotation method: Testing known values...\n", (unsigned long long)hrt_absolute_time());
-
-		/******************************************** TEST 1 ****************************************************/
-		q = quatFromEuler(Eigen::Vector3f(0.0f, 0.0f, M_PI_2_F));
-		vector_q = q._transformVector(vector);
-		Eigen::Vector3f vector_true = { -1.00f, 1.00f, 1.00f};
-
-		for (size_t i = 0; i < 3; i++) {
-			if (!EXPECT_NEAR(vector_true(i), vector_q(i), FLT_EPSILON)) {
-				printf("%llu: It[%d]: Value of: Quaternion method 'rotate'\n", (unsigned long long)hrt_absolute_time(), (int)i);
-				printf("Actual:  \t[%8.5f, %8.5f, %8.5f]\n", vector_q(0), vector_q(1), vector_q(2));
-				printf("Expected: \t[%8.5f, %8.5f, %8.5f]\n", vector_true(0), vector_true(1), vector_true(2));
-				++err_num;
-				rc = 1;
-			}
-		}
-
-		/********************************************************************************************************/
-		/******************************************** TEST 2 ****************************************************/
-		q = quatFromEuler(Eigen::Vector3f(0.1f, 0.2f, 0.3f));
-		vector_q = q._transformVector(vector);
-		vector_true = {0.8795481794122900f, 1.2090975499501229f, 0.874344391414010f};
-
-		for (size_t i = 0; i < 3; i++) {
-			if (!EXPECT_NEAR(vector_true(i), vector_q(i), epsilon_f)) {
-				printf("%llu: It[%d]: Value of: Quaternion method 'rotate'\n", (unsigned long long)hrt_absolute_time(), (int)i);
-				printf("Actual:  \t[%8.5f, %8.5f, %8.5f]\n", (double)vector_q(0), (double)vector_q(1), (double)vector_q(2));
-				printf("Expected: \t[%8.5f, %8.5f, %8.5f]\n", (double)vector_true(0), (double)vector_true(1), (double)vector_true(2));
-				++err_num;
-				rc = 1;
-			}
-		}
-
-		/********************************************************************************************************/
-		/******************************************** TEST 3 ****************************************************/
-		q = quatFromEuler(Eigen::Vector3f(0.5f, -0.2f, -1.5f));
-		vector_q = q._transformVector(vector);
-		vector_true = {0.447416342848463f, -0.6805264343934600f, 1.528627615949624f};
-
-		for (size_t i = 0; i < 3; i++) {
-			if (!EXPECT_NEAR(vector_true(i), vector_q(i), epsilon_f)) {
-				printf("%llu: It[%d]: Value of: Quaternion method 'rotate'\n", (unsigned long long)hrt_absolute_time(), (int)i);
-				printf("Actual:  \t[%8.5f, %8.5f, %8.5f]\n", (double)vector_q(0), (double)vector_q(1), (double)vector_q(2));
-				printf("Expected: \t[%8.5f, %8.5f, %8.5f]\n", (double)vector_true(0), (double)vector_true(1), (double)vector_true(2));
-				++err_num;
-				rc = 1;
-			}
-		}
-
-		/********************************************************************************************************/
-		/******************************************** TEST 4 ****************************************************/
-		q = quatFromEuler(Eigen::Vector3f(-M_PI_F / 3.0f, -M_PI_2_F, M_PI_2_F));
-		vector_q = q._transformVector(vector);
-		vector_true = { -1.366030f, 0.366025f, 1.000000f};
-
-		for (size_t i = 0; i < 3; i++) {
-			if (!EXPECT_NEAR(vector_true(i), vector_q(i), epsilon_f)) {
-				printf("%llu: It[%d]: Value of: Quaternion method 'rotate'\n", (unsigned long long)hrt_absolute_time(), (int)i);
-				printf("Actual:  \t[%8.5f, %8.5f, %8.5f]\n", (double)vector_q(0), (double)vector_q(1), (double)vector_q(2));
-				printf("Expected: \t[%8.5f, %8.5f, %8.5f]\n", (double)vector_true(0), (double)vector_true(1), (double)vector_true(2));
-				++err_num;
-				rc = 1;
-			}
-		}
-
-		/********************************************************************************************************/
-		/******************************************** TEST 5 ****************************************************/
-		printf("%llu: Rotation method: Testing transformation range...\n", (unsigned long long)hrt_absolute_time());
-
-		Eigen::Vector3f vectorR(1.0f, 1.0f, 1.0f);
-
-		for (float roll = min; roll <= max; roll += step) {
-			for (float pitch = min; pitch <= max; pitch += step) {
-				for (float yaw = min; yaw <= max; yaw += step) {
-
-					R = matrixFromEuler(Eigen::Vector3f(roll, pitch, yaw));
-					q = quatFromEuler(Eigen::Vector3f(roll, pitch, yaw));
-
-					vector_r = R * vectorR;
-					vector_q = q._transformVector(vectorR);
-
-					for (int i = 0; i < 3; i++) {
-						if (!EXPECT_NEAR(vector_r(i), vector_q(i), epsilon_f)) {
-							printf("%llu: (%d) Value of: Quaternion method 'rotate'\n", (unsigned long long)hrt_absolute_time(), i);
-							printf("Actual:  \t%8.5f\n", vector_q(i));
-							printf("Expected: \t%8.5f\n", vector_r(i));
-							++err_num;
-							rc = 1;
-						}
-					}
-				}
-			}
-		}
-	}
-	printf("%llu: Finished Eigen math tests with %d error(s)...\n", (unsigned long long)hrt_absolute_time(), err_num);
-	return rc;
-}
diff --git a/src/systemcmds/tests/test_float.c b/src/systemcmds/tests/test_float.c
deleted file mode 100644
index 83b102b41e..0000000000
--- a/src/systemcmds/tests/test_float.c
+++ /dev/null
@@ -1,287 +0,0 @@
-/****************************************************************************
- *
- *   Copyright (c) 2012-2015 PX4 Development Team. All rights reserved.
- *   Author: @author Lorenz Meier <lm@inf.ethz.ch>
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions
- * are met:
- *
- * 1. Redistributions of source code must retain the above copyright
- *    notice, this list of conditions and the following disclaimer.
- * 2. Redistributions in binary form must reproduce the above copyright
- *    notice, this list of conditions and the following disclaimer in
- *    the documentation and/or other materials provided with the
- *    distribution.
- * 3. Neither the name PX4 nor the names of its contributors may be
- *    used to endorse or promote products derived from this software
- *    without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
- * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
- * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- *
- ****************************************************************************/
-
-/**
- * @file tests_float.c
- * Floating point tests
- */
-
-#include <px4_config.h>
-#include <sys/types.h>
-#include <stdint.h>
-#include <stdio.h>
-#include <stdlib.h>
-#include <unistd.h>
-#include <fcntl.h>
-#include <errno.h>
-#include "tests.h"
-#include <math.h>
-#include <float.h>
-
-typedef union {
-	float f;
-	double d;
-	uint8_t b[8];
-} test_float_double_t;
-
-int test_float(int argc, char *argv[])
-{
-	int ret = 0;
-
-	printf("\n--- SINGLE PRECISION TESTS ---\n");
-	printf("The single precision test involves calls to fabsf(),\nif test fails check this function as well.\n\n");
-
-	float f1 = 1.55f;
-
-	float sinf_zero = sinf(0.0f);
-	float sinf_one = sinf(1.0f);
-	float sqrt_two = sqrt(2.0f);
-
-	if (fabsf(sinf_zero) < FLT_EPSILON) {
-		printf("\t success: sinf(0.0f) == 0.0f\n");
-
-	} else {
-		printf("\t FAIL: sinf(0.0f) != 0.0f, result: %8.4f\n", (double)sinf_zero);
-		ret = -4;
-	}
-
-	fflush(stdout);
-
-	if (fabsf((sinf_one - 0.841470956802368164062500000000f)) < FLT_EPSILON) {
-		printf("\t success: sinf(1.0f) == 0.84147f\n");
-
-	} else {
-		printf("\t FAIL: sinf(1.0f) != 0.84147f, result: %8.4f\n", (double)sinf_one);
-		ret = -1;
-	}
-
-	fflush(stdout);
-
-	float asinf_one = asinf(1.0f);
-
-	if (fabsf((asinf_one - 1.570796251296997070312500000000f)) < FLT_EPSILON * 1.5f) {
-		printf("\t success: asinf(1.0f) == 1.57079f\n");
-
-	} else {
-		printf("\t FAIL: asinf(1.0f) != 1.57079f, result: %f\n", (double)asinf_one);
-		ret = -1;
-	}
-
-	fflush(stdout);
-
-	float cosf_one = cosf(1.0f);
-
-	if (fabsf((cosf_one - 0.540302336215972900390625000000f)) < FLT_EPSILON) {
-		printf("\t success: cosf(1.0f) == 0.54030f\n");
-
-	} else {
-		printf("\t FAIL: cosf(1.0f) != 0.54030f, result: %8.4f\n", (double)cosf_one);
-		ret = -1;
-	}
-
-	fflush(stdout);
-
-
-	float acosf_one = acosf(1.0f);
-
-	if (fabsf((acosf_one - 0.000000000000000000000000000000f)) < FLT_EPSILON) {
-		printf("\t success: acosf(1.0f) == 0.0f\n");
-
-	} else {
-		printf("\t FAIL: acosf(1.0f) != 0.0f, result: %8.4f\n", (double)acosf_one);
-		ret = -1;
-	}
-
-	fflush(stdout);
-
-
-	float sinf_zero_one = sinf(0.1f);
-
-	if (fabsf(sinf_zero_one - 0.0998334166f) < FLT_EPSILON) {
-		printf("\t success: sinf(0.1f) == 0.09983f\n");
-
-	} else {
-		printf("\t FAIL: sinf(0.1f) != 0.09983f, result: %8.4f\n", (double)sinf_zero_one);
-		ret = -2;
-	}
-
-	if (fabsf(sqrt_two - 1.41421356f) < FLT_EPSILON) {
-		printf("\t success: sqrt(2.0f) == 1.41421f\n");
-
-	} else {
-		printf("\t FAIL: sqrt(2.0f) != 1.41421f, result: %8.4f\n", (double)sinf_zero_one);
-		ret = -3;
-	}
-
-	float atan2f_ones = atan2f(1.0f, 1.0f);
-
-	if (fabsf(atan2f_ones - 0.785398163397448278999490867136f) < 2.0f * FLT_EPSILON) {
-		printf("\t success: atan2f(1.0f, 1.0f) == 0.78539f\n");
-
-	} else {
-		printf("\t FAIL: atan2f(1.0f, 1.0f) != 0.78539f, result: %8.4f\n", (double)atan2f_ones);
-		ret = -4;
-	}
-
-	char sbuf[30];
-	sprintf(sbuf, "%8.4f", (double)0.553415f);
-
-	if (sbuf[0] == ' ' && sbuf[1] == ' ' && sbuf[2] == '0' &&
-	    sbuf[3] == '.' && sbuf[4] == '5' && sbuf[5] == '5'
-	    && sbuf[6] == '3' && sbuf[7] == '4' && sbuf[8] == '\0') {
-		printf("\t success: printf(\"%%8.4f\", 0.553415f) == %8.4f\n", (double)0.553415f);
-
-	} else {
-		printf("\t FAIL: printf(\"%%8.4f\", 0.553415f) != \"  0.5534\", result: %s\n", sbuf);
-		ret = -5;
-	}
-
-	sprintf(sbuf, "%8.4f", (double) - 0.553415f);
-
-	if (sbuf[0] == ' ' && sbuf[1] == '-' && sbuf[2] == '0' &&
-	    sbuf[3] == '.' && sbuf[4] == '5' && sbuf[5] == '5'
-	    && sbuf[6] == '3' && sbuf[7] == '4' && sbuf[8] == '\0') {
-		printf("\t success: printf(\"%%8.4f\", -0.553415f) == %8.4f\n", (double) - 0.553415f);
-
-	} else {
-		printf("\t FAIL: printf(\"%%8.4f\", -0.553415f) != \" -0.5534\", result: %s\n", sbuf);
-		ret = -6;
-	}
-
-
-
-
-
-	printf("\n--- DOUBLE PRECISION TESTS ---\n");
-
-	double d1 = 1.0111;
-	double d2 = 2.0;
-
-	double d1d2 = d1 * d2;
-
-	if (fabs(d1d2 - 2.022200000000000219557705349871) < DBL_EPSILON) {
-		printf("\t success: 1.0111 * 2.0 == 2.0222\n");
-
-	} else {
-		printf("\t FAIL: 1.0111 * 2.0 != 2.0222, result: %8.4f\n", d1d2);
-		ret = -7;
-	}
-
-	fflush(stdout);
-
-	// Assign value of f1 to d1
-	d1 = f1;
-
-	if (fabsf(f1 - (float)d1) < FLT_EPSILON) {
-		printf("\t success: (float) 1.55f == 1.55 (double)\n");
-
-	} else {
-		printf("\t FAIL: (float) 1.55f != 1.55 (double), result: %8.4f\n", (double)f1);
-		ret = -8;
-	}
-
-	fflush(stdout);
-
-
-	double sin_zero = sin(0.0);
-	double sin_one = sin(1.0);
-	double atan2_ones = atan2(1.0, 1.0);
-
-	if (fabs(sin_zero - 0.0) < DBL_EPSILON) {
-		printf("\t success: sin(0.0) == 0.0\n");
-
-	} else {
-		printf("\t FAIL: sin(0.0) != 0.0, result: %8.4f\n", sin_zero);
-		ret = -9;
-	}
-
-	if (fabs(sin_one - 0.841470984807896504875657228695) < DBL_EPSILON) {
-		printf("\t success: sin(1.0) == 0.84147098480\n");
-
-	} else {
-		printf("\t FAIL: sin(1.0) != 1.0, result: %8.4f\n", sin_one);
-		ret = -10;
-	}
-
-	if (fabs(atan2_ones - 0.785398163397448278999490867136) < 2.0 * DBL_EPSILON) {
-		printf("\t success: atan2(1.0, 1.0) == 0.785398\n");
-
-	} else {
-		printf("\t FAIL: atan2(1.0, 1.0) != 0.785398, result: %8.4f\n", atan2_ones);
-		ret = -11;
-	}
-
-	printf("\t testing pow() with magic value\n");
-	printf("\t   (44330.0 * (1.0 - pow((96286LL / 101325.0), 0.190295)));\n");
-	fflush(stdout);
-	usleep(20000);
-	double powres = (44330.0 * (1.0 - pow((96286LL / 101325.0), 0.190295)));
-	printf("\t success: result: %8.4f\n", (double)powres);
-
-	sprintf(sbuf, "%8.4f", 0.553415);
-
-	if (sbuf[0] == ' ' && sbuf[1] == ' ' && sbuf[2] == '0' &&
-	    sbuf[3] == '.' && sbuf[4] == '5' && sbuf[5] == '5'
-	    && sbuf[6] == '3' && sbuf[7] == '4' && sbuf[8] == '\0') {
-		printf("\t success: printf(\"%%8.4f\", 0.553415) == %8.4f\n", 0.553415);
-
-	} else {
-		printf("\t FAIL: printf(\"%%8.4f\", 0.553415) != \"  0.5534\", result: %s\n", sbuf);
-		ret = -12;
-	}
-
-	sprintf(sbuf, "%8.4f", -0.553415);
-
-	if (sbuf[0] == ' ' && sbuf[1] == '-' && sbuf[2] == '0' &&
-	    sbuf[3] == '.' && sbuf[4] == '5' && sbuf[5] == '5'
-	    && sbuf[6] == '3' && sbuf[7] == '4' && sbuf[8] == '\0') {
-		printf("\t success: printf(\"%%8.4f\", -0.553415) == %8.4f\n", -0.553415);
-
-	} else {
-		printf("\t FAIL: printf(\"%%8.4f\", -0.553415) != \" -0.5534\", result: %s\n", sbuf);
-		ret = -13;
-	}
-
-
-	if (ret == 0) {
-		printf("\n SUCCESS: All float and double tests passed.\n");
-
-	} else {
-		printf("\n FAIL: One or more tests failed.\n");
-	}
-
-	printf("\n");
-
-	return ret;
-}
diff --git a/src/systemcmds/tests/test_float.cpp b/src/systemcmds/tests/test_float.cpp
new file mode 100644
index 0000000000..9174c1dd4e
--- /dev/null
+++ b/src/systemcmds/tests/test_float.cpp
@@ -0,0 +1,149 @@
+#include <unit_test/unit_test.h>
+
+#include <errno.h>
+#include <fcntl.h>
+#include <float.h>
+#include <math.h>
+#include <px4_config.h>
+#include <stdint.h>
+#include <stdio.h>
+#include <stdlib.h>
+#include <sys/types.h>
+#include <unistd.h>
+
+typedef union {
+	float f;
+	double d;
+	uint8_t b[8];
+} test_float_double_t;
+
+
+
+class FloatTest : public UnitTest
+{
+public:
+	virtual bool run_tests(void);
+
+private:
+	bool singlePrecisionTests();
+	bool doublePrecisionTests();
+};
+
+bool FloatTest::singlePrecisionTests(void)
+{
+	float sinf_zero = sinf(0.0f);
+	float sinf_one = sinf(1.0f);
+	float sqrt_two = sqrt(2.0f);
+
+	ut_assert("sinf(0.0f) == 0.0f", fabsf(sinf_zero) < FLT_EPSILON);
+	ut_assert("sinf(1.0f) == 0.84147f", fabsf((sinf_one - 0.841470956802368164062500000000f)) < FLT_EPSILON);
+
+	float asinf_one = asinf(1.0f);
+	ut_assert("asinf(1.0f) == 1.57079f", fabsf((asinf_one - 1.570796251296997070312500000000f)) < FLT_EPSILON * 1.5f);
+
+	float cosf_one = cosf(1.0f);
+	ut_assert("cosf(1.0f) == 0.54030f", fabsf((cosf_one - 0.540302336215972900390625000000f)) < FLT_EPSILON);
+
+	float acosf_one = acosf(1.0f);
+	ut_assert("acosf(1.0f) == 0.0f", fabsf((acosf_one - 0.000000000000000000000000000000f)) < FLT_EPSILON);
+
+	float sinf_zero_one = sinf(0.1f);
+	ut_assert("sinf(0.1f) == 0.09983f", fabsf(sinf_zero_one - 0.0998334166f) < FLT_EPSILON);
+
+	ut_assert("sqrt(2.0f) == 1.41421f", fabsf(sqrt_two - 1.41421356f) < FLT_EPSILON);
+
+	float atan2f_ones = atan2f(1.0f, 1.0f);
+	ut_assert("atan2f(1.0f, 1.0f) == 0.78539f",
+		  fabsf(atan2f_ones - 0.785398163397448278999490867136f) < 2.0f * FLT_EPSILON);
+
+	char sbuf[30];
+	sprintf(sbuf, "%8.4f", (double)0.553415f);
+	ut_compare("sbuf[0]", sbuf[0], ' ');
+	ut_compare("sbuf[1]", sbuf[1], ' ');
+	ut_compare("sbuf[2]", sbuf[2], '0');
+	ut_compare("sbuf[3]", sbuf[3], '.');
+	ut_compare("sbuf[4]", sbuf[4], '5');
+	ut_compare("sbuf[5]", sbuf[5], '5');
+	ut_compare("sbuf[6]", sbuf[6], '3');
+	ut_compare("sbuf[7]", sbuf[7], '4');
+	ut_compare("sbuf[8]", sbuf[8], '\0');
+
+	sprintf(sbuf, "%8.4f", (double) - 0.553415f);
+	ut_compare("sbuf[0]", sbuf[0], ' ');
+	ut_compare("sbuf[1]", sbuf[1], '-');
+	ut_compare("sbuf[2]", sbuf[2], '0');
+	ut_compare("sbuf[3]", sbuf[3], '.');
+	ut_compare("sbuf[4]", sbuf[4], '5');
+	ut_compare("sbuf[5]", sbuf[5], '5');
+	ut_compare("sbuf[6]", sbuf[6], '3');
+	ut_compare("sbuf[7]", sbuf[7], '4');
+	ut_compare("sbuf[8]", sbuf[8], '\0');
+
+	return true;
+}
+
+
+bool FloatTest::doublePrecisionTests(void)
+{
+	float f1 = 1.55f;
+
+	double d1 = 1.0111;
+	double d2 = 2.0;
+
+	double d1d2 = d1 * d2;
+
+	ut_assert("1.0111 * 2.0 == 2.0222", fabs(d1d2 - 2.022200000000000219557705349871) < DBL_EPSILON);
+
+	// Assign value of f1 to d1
+	d1 = f1;
+
+	ut_assert("(float) 1.55f == 1.55 (double)", fabsf(f1 - (float)d1) < FLT_EPSILON);
+
+
+	double sin_zero = sin(0.0);
+	double sin_one = sin(1.0);
+	double atan2_ones = atan2(1.0, 1.0);
+
+	ut_assert("sin(0.0) == 0.0", fabs(sin_zero - 0.0) < DBL_EPSILON);
+	ut_assert("sin(1.0) == 0.84147098480", fabs(sin_one - 0.841470984807896504875657228695) < DBL_EPSILON);
+	ut_assert("atan2(1.0, 1.0) == 0.785398", fabs(atan2_ones - 0.785398163397448278999490867136) < 2.0 * DBL_EPSILON);
+	ut_assert("testing pow() with magic value",
+		  (44330.0 * (1.0 - pow((96286LL / 101325.0), 0.190295))) - 428.2293 <  DBL_EPSILON);
+
+
+	char sbuf[30];
+	sprintf(sbuf, "%8.4f", 0.553415);
+	ut_compare("sbuf[0]", sbuf[0], ' ');
+	ut_compare("sbuf[1]", sbuf[1], ' ');
+	ut_compare("sbuf[2]", sbuf[2], '0');
+	ut_compare("sbuf[3]", sbuf[3], '.');
+	ut_compare("sbuf[4]", sbuf[4], '5');
+	ut_compare("sbuf[5]", sbuf[5], '5');
+	ut_compare("sbuf[6]", sbuf[6], '3');
+	ut_compare("sbuf[7]", sbuf[7], '4');
+	ut_compare("sbuf[8]", sbuf[8], '\0');
+
+
+	sprintf(sbuf, "%8.4f", -0.553415);
+	ut_compare("sbuf[0]", sbuf[0], ' ');
+	ut_compare("sbuf[1]", sbuf[1], '-');
+	ut_compare("sbuf[2]", sbuf[2], '0');
+	ut_compare("sbuf[3]", sbuf[3], '.');
+	ut_compare("sbuf[4]", sbuf[4], '5');
+	ut_compare("sbuf[5]", sbuf[5], '5');
+	ut_compare("sbuf[6]", sbuf[6], '3');
+	ut_compare("sbuf[7]", sbuf[7], '4');
+	ut_compare("sbuf[8]", sbuf[8], '\0');
+
+	return true;
+}
+
+bool FloatTest::run_tests(void)
+{
+	ut_run_test(singlePrecisionTests);
+	ut_run_test(doublePrecisionTests);
+
+	return (_tests_failed == 0);
+}
+
+ut_declare_test_c(test_float, FloatTest)
diff --git a/src/systemcmds/tests/test_int.c b/src/systemcmds/tests/test_int.c
deleted file mode 100644
index 7db4b48f9b..0000000000
--- a/src/systemcmds/tests/test_int.c
+++ /dev/null
@@ -1,152 +0,0 @@
-/****************************************************************************
- * px4/sensors/test_gpio.c
- *
- *  Copyright (C) 2012 PX4 Development Team. All rights reserved.
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions
- * are met:
- *
- * 1. Redistributions of source code must retain the above copyright
- *    notice, this list of conditions and the following disclaimer.
- * 2. Redistributions in binary form must reproduce the above copyright
- *    notice, this list of conditions and the following disclaimer in
- *    the documentation and/or other materials provided with the
- *    distribution.
- * 3. Neither the name NuttX nor the names of its contributors may be
- *    used to endorse or promote products derived from this software
- *    without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
- * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
- * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- *
- ****************************************************************************/
-
-/****************************************************************************
- * Included Files
- ****************************************************************************/
-
-#include <inttypes.h>
-
-#include <px4_config.h>
-#include <px4_defines.h>
-
-#include <sys/types.h>
-
-#include <stdio.h>
-#include <stdlib.h>
-#include <stdint.h>
-#include <unistd.h>
-#include <fcntl.h>
-#include <errno.h>
-
-#include <arch/board/board.h>
-
-#include "tests.h"
-
-#include <math.h>
-#include <float.h>
-
-
-/****************************************************************************
- * Pre-processor Definitions
- ****************************************************************************/
-
-/****************************************************************************
- * Private Types
- ****************************************************************************/
-
-/****************************************************************************
- * Private Function Prototypes
- ****************************************************************************/
-
-/****************************************************************************
- * Private Data
- ****************************************************************************/
-
-/****************************************************************************
- * Public Data
- ****************************************************************************/
-
-/****************************************************************************
- * Private Functions
- ****************************************************************************/
-
-
-/****************************************************************************
- * Public Functions
- ****************************************************************************/
-
-/****************************************************************************
- * Name: test_led
- ****************************************************************************/
-
-typedef union {
-	int32_t i;
-	int64_t l;
-	uint8_t b[8];
-} test_32_64_t;
-
-int test_int(int argc, char *argv[])
-{
-	int ret = 0;
-
-	printf("\n--- 64 BIT MATH TESTS ---\n");
-
-	int64_t large = 354156329598;
-
-	int64_t calc = large * 5;
-
-	if (calc == 1770781647990) {
-		printf("\t success: 354156329598 * 5 == %" PRId64 "\n", calc);
-
-	} else {
-		printf("\t FAIL: 354156329598 * 5 != %" PRId64 "\n", calc);
-		ret = -1;
-	}
-
-	fflush(stdout);
-
-
-
-
-
-	printf("\n--- 32 BIT / 64 BIT MIXED MATH TESTS ---\n");
-
-
-	int32_t small = 50;
-	int32_t large_int = 2147483647; // MAX INT value
-
-	uint64_t small_times_large = large_int * (uint64_t)small;
-
-	if (small_times_large == 107374182350) {
-		printf("\t success: 64bit calculation: 50 * 2147483647 (max int val) == %" PRId64 "\n", small_times_large);
-
-	} else {
-		printf("\t FAIL: 50 * 2147483647 != %" PRId64 ", 64bit cast might fail\n", small_times_large);
-		ret = -1;
-	}
-
-	fflush(stdout);
-
-	if (ret == 0) {
-		printf("\n SUCCESS: All float and double tests passed.\n");
-
-	} else {
-		printf("\n FAIL: One or more tests failed.\n");
-	}
-
-	printf("\n");
-
-	return ret;
-}
diff --git a/src/systemcmds/tests/test_int.cpp b/src/systemcmds/tests/test_int.cpp
new file mode 100644
index 0000000000..65944e1731
--- /dev/null
+++ b/src/systemcmds/tests/test_int.cpp
@@ -0,0 +1,61 @@
+#include <unit_test/unit_test.h>
+
+#include <errno.h>
+#include <fcntl.h>
+#include <float.h>
+#include <math.h>
+#include <px4_config.h>
+#include <stdint.h>
+#include <stdio.h>
+#include <stdlib.h>
+#include <sys/types.h>
+#include <unistd.h>
+
+typedef union {
+	int32_t i;
+	int64_t l;
+	uint8_t b[8];
+} test_32_64_t;
+
+class IntTest : public UnitTest
+{
+public:
+	virtual bool run_tests(void);
+
+private:
+	bool math64bitTests();
+	bool math3264MixedMathTests();
+};
+
+bool IntTest::math64bitTests(void)
+{
+	int64_t large = 354156329598;
+	int64_t calc = large * 5;
+
+	ut_assert("354156329598 * 5 == 1770781647990", calc == 1770781647990);
+
+	return true;
+}
+
+bool IntTest::math3264MixedMathTests(void)
+{
+	int32_t small = 50;
+	int32_t large_int = 2147483647; // MAX INT value
+
+	uint64_t small_times_large = large_int * (uint64_t)small;
+
+	ut_assert("64bit calculation: 50 * 2147483647 (max int val) == 107374182350", small_times_large == 107374182350);
+
+	return true;
+}
+
+
+bool IntTest::run_tests(void)
+{
+	ut_run_test(math64bitTests);
+	ut_run_test(math3264MixedMathTests);
+
+	return (_tests_failed == 0);
+}
+
+ut_declare_test_c(test_int, IntTest)
diff --git a/src/systemcmds/tests/test_mathlib.cpp b/src/systemcmds/tests/test_mathlib.cpp
index 0883f42e8a..9de1f06686 100644
--- a/src/systemcmds/tests/test_mathlib.cpp
+++ b/src/systemcmds/tests/test_mathlib.cpp
@@ -31,11 +31,18 @@
  *
  ****************************************************************************/
 
-/**
- * @file test_mathlib.cpp
- *
- * Mathlib test
- */
+#include <unit_test/unit_test.h>
+
+#include <errno.h>
+#include <fcntl.h>
+#include <float.h>
+#include <math.h>
+#include <px4_config.h>
+#include <stdint.h>
+#include <stdio.h>
+#include <stdlib.h>
+#include <sys/types.h>
+#include <unistd.h>
 
 #include <px4_log.h>
 #include <stdio.h>
@@ -47,17 +54,33 @@
 #include <systemlib/err.h>
 #include <drivers/drv_hrt.h>
 
+class MathlibTest : public UnitTest
+{
+public:
+	virtual bool run_tests(void);
+
+private:
+	bool testVector2();
+	bool testVector3();
+	bool testVector4();
+	bool testVector10();
+	bool testMatrix3x3();
+	bool testMatrix10x10();
+	bool testMatrixNonsymmetric();
+	bool testRotationMatrixQuaternion();
+	bool testQuaternionfrom_dcm();
+	bool testQuaternionfrom_euler();
+	bool testQuaternionRotate();
+};
+
 #include "tests.h"
 
 #define TEST_OP(_title, _op) { unsigned int n = 30000; hrt_abstime t0, t1; t0 = hrt_absolute_time(); for (unsigned int j = 0; j < n; j++) { _op; }; t1 = hrt_absolute_time(); PX4_INFO(_title ": %.6fus", (double)(t1 - t0) / n); }
 
 using namespace math;
 
-int test_mathlib(int argc, char *argv[])
+bool MathlibTest::testVector2(void)
 {
-	int rc = 0;
-	PX4_INFO("testing mathlib");
-
 	{
 		Vector<2> v;
 		Vector<2> v1(1.0f, 2.0f);
@@ -75,6 +98,11 @@ int test_mathlib(int argc, char *argv[])
 		TEST_OP("Vector<2> * Vector<2>", v * v1);
 		TEST_OP("Vector<2> %% Vector<2>", v1 % v2);
 	}
+	return true;
+}
+
+bool MathlibTest::testVector3(void)
+{
 
 	{
 		Vector<3> v;
@@ -108,7 +136,11 @@ int test_mathlib(int argc, char *argv[])
 		TEST_OP("Vector<3> element write", v1(0) = 1.0f);
 		TEST_OP("Vector<3> element write direct", v1.data[0] = 1.0f);
 	}
+	return true;
+}
 
+bool MathlibTest::testVector4(void)
+{
 	{
 		Vector<4> v;
 		Vector<4> v1(1.0f, 2.0f, 0.0f, -1.0f);
@@ -125,7 +157,11 @@ int test_mathlib(int argc, char *argv[])
 		TEST_OP("Vector<4> -= Vector<4>", v -= v1);
 		TEST_OP("Vector<4> * Vector<4>", v * v1);
 	}
+	return true;
+}
 
+bool MathlibTest::testVector10(void)
+{
 	{
 		Vector<10> v1;
 		v1.zero();
@@ -134,7 +170,11 @@ int test_mathlib(int argc, char *argv[])
 		TEST_OP("Constructor Vector<10>(Vector<10>)", Vector<10> v3(v1));
 		TEST_OP("Constructor Vector<10>(float[])", Vector<10> v3(data));
 	}
+	return true;
+}
 
+bool MathlibTest::testMatrix3x3(void)
+{
 	{
 		Matrix<3, 3> m1;
 		m1.identity();
@@ -145,7 +185,11 @@ int test_mathlib(int argc, char *argv[])
 		TEST_OP("Matrix<3, 3> + Matrix<3, 3>", m1 + m2);
 		TEST_OP("Matrix<3, 3> * Matrix<3, 3>", m1 * m2);
 	}
+	return true;
+}
 
+bool MathlibTest::testMatrix10x10(void)
+{
 	{
 		Matrix<10, 10> m1;
 		m1.identity();
@@ -157,9 +201,14 @@ int test_mathlib(int argc, char *argv[])
 		TEST_OP("Matrix<10, 10> + Matrix<10, 10>", m1 + m2);
 		TEST_OP("Matrix<10, 10> * Matrix<10, 10>", m1 * m2);
 	}
+	return true;
+}
 
+bool MathlibTest::testMatrixNonsymmetric(void)
+{
+	int rc = true;
 	{
-		PX4_INFO("Nonsymmetric matrix operations test");
+		//PX4_INFO("Nonsymmetric matrix operations test");
 		// test nonsymmetric +, -, +=, -=
 
 		float data1[2][3] = {{1, 2, 3}, {4, 5, 6}};
@@ -175,17 +224,21 @@ int test_mathlib(int argc, char *argv[])
 			(m1 + m2).print();
 			printf("!=\n");
 			m3.print();
-			rc = 1;
+			rc = false;
 		}
 
+		ut_assert("m1 + m2 == m3", m1 + m2 == m3);
+
 		if (m3 - m2 != m1) {
 			PX4_ERR("Matrix<2, 3> - Matrix<2, 3> failed!");
 			(m3 - m2).print();
 			printf("!=\n");
 			m1.print();
-			rc = 1;
+			rc = false;
 		}
 
+		ut_assert("m3 - m2 == m1", m3 - m2 == m1);
+
 		m1 += m2;
 
 		if (m1 != m3) {
@@ -193,9 +246,11 @@ int test_mathlib(int argc, char *argv[])
 			m1.print();
 			printf("!=\n");
 			m3.print();
-			rc = 1;
+			rc = false;
 		}
 
+		ut_assert("m1 == m3", m1 == m3);
+
 		m1 -= m2;
 		Matrix<2, 3> m1_orig(data1);
 
@@ -204,160 +259,181 @@ int test_mathlib(int argc, char *argv[])
 			m1.print();
 			printf("!=\n");
 			m1_orig.print();
-			rc = 1;
+			rc = false;
 		}
 
-	}
+		ut_assert("m1 == m1_orig", m1 == m1_orig);
 
-	{
-		// test conversion rotation matrix to quaternion and back
-		math::Matrix<3, 3> R_orig;
-		math::Matrix<3, 3> R;
-		math::Quaternion q;
-		float diff = 0.1f;
-		float tol = 0.00001f;
-
-		PX4_INFO("Quaternion transformation methods test.");
-
-		for (float roll = -M_PI_F; roll <= M_PI_F; roll += diff) {
-			for (float pitch = -M_PI_2_F; pitch <= M_PI_2_F; pitch += diff) {
-				for (float yaw = -M_PI_F; yaw <= M_PI_F; yaw += diff) {
-					R_orig.from_euler(roll, pitch, yaw);
-					q.from_dcm(R_orig);
-					R = q.to_dcm();
-
-					for (int i = 0; i < 3; i++) {
-						for (int j = 0; j < 3; j++) {
-							if (fabsf(R_orig.data[i][j] - R.data[i][j]) > 0.00001f) {
-								PX4_WARN("Quaternion method 'from_dcm' or 'to_dcm' outside tolerance!");
-								rc = 1;
-							}
-						}
-					}
-				}
-			}
-		}
-
-		// test against some known values
-		tol = 0.0001f;
-		math::Quaternion q_true = {1.0f, 0.0f, 0.0f, 0.0f};
-		R_orig.identity();
-		q.from_dcm(R_orig);
-
-		for (unsigned i = 0; i < 4; i++) {
-			if (fabsf(q.data[i] - q_true.data[i]) > tol) {
-				PX4_WARN("Quaternion method 'from_dcm()' outside tolerance!");
-				rc = 1;
-			}
-		}
-
-		q_true.from_euler(0.3f, 0.2f, 0.1f);
-		q = {0.9833f, 0.1436f, 0.1060f, 0.0343f};
-
-		for (unsigned i = 0; i < 4; i++) {
-			if (fabsf(q.data[i] - q_true.data[i]) > tol) {
-				PX4_WARN("Quaternion method 'from_euler()' outside tolerance!");
-				rc = 1;
-			}
-		}
-
-		q_true.from_euler(-1.5f, -0.2f, 0.5f);
-		q = {0.7222f, -0.6391f, -0.2386f, 0.1142f};
-
-		for (unsigned i = 0; i < 4; i++) {
-			if (fabsf(q.data[i] - q_true.data[i]) > tol) {
-				PX4_WARN("Quaternion method 'from_euler()' outside tolerance!");
-				rc = 1;
-			}
-		}
-
-		q_true.from_euler(M_PI_2_F, -M_PI_2_F, -M_PI_F / 3);
-		q = {0.6830f, 0.1830f, -0.6830f, 0.1830f};
-
-		for (unsigned i = 0; i < 4; i++) {
-			if (fabsf(q.data[i] - q_true.data[i]) > tol) {
-				PX4_WARN("Quaternion method 'from_euler()' outside tolerance!");
-				rc = 1;
-			}
-		}
-
-	}
-
-	{
-		// test quaternion method "rotate" (rotate vector by quaternion)
-		Vector<3> vector = {1.0f, 1.0f, 1.0f};
-		Vector<3> vector_q;
-		Vector<3> vector_r;
-		Quaternion q;
-		Matrix<3, 3> R;
-		float diff = 0.1f;
-		float tol = 0.00001f;
-
-		PX4_INFO("Quaternion vector rotation method test.");
-
-		for (float roll = -M_PI_F; roll <= M_PI_F; roll += diff) {
-			for (float pitch = -M_PI_2_F; pitch <= M_PI_2_F; pitch += diff) {
-				for (float yaw = -M_PI_F; yaw <= M_PI_F; yaw += diff) {
-					R.from_euler(roll, pitch, yaw);
-					q.from_euler(roll, pitch, yaw);
-					vector_r = R * vector;
-					vector_q = q.conjugate(vector);
-
-					for (int i = 0; i < 3; i++) {
-						if (fabsf(vector_r(i) - vector_q(i)) > tol) {
-							PX4_WARN("Quaternion method 'rotate' outside tolerance");
-							rc = 1;
-						}
-					}
-				}
-			}
-		}
-
-		// test some values calculated with matlab
-		tol = 0.0001f;
-		q.from_euler(M_PI_2_F, 0.0f, 0.0f);
-		vector_q = q.conjugate(vector);
-		Vector<3> vector_true = {1.00f, -1.00f, 1.00f};
-
-		for (unsigned i = 0; i < 3; i++) {
-			if (fabsf(vector_true(i) - vector_q(i)) > tol) {
-				PX4_WARN("Quaternion method 'rotate' outside tolerance");
-				rc = 1;
-			}
-		}
-
-		q.from_euler(0.3f, 0.2f, 0.1f);
-		vector_q = q.conjugate(vector);
-		vector_true = {1.1566, 0.7792, 1.0273};
-
-		for (unsigned i = 0; i < 3; i++) {
-			if (fabsf(vector_true(i) - vector_q(i)) > tol) {
-				PX4_WARN("Quaternion method 'rotate' outside tolerance");
-				rc = 1;
-			}
-		}
-
-		q.from_euler(-1.5f, -0.2f, 0.5f);
-		vector_q = q.conjugate(vector);
-		vector_true = {0.5095, 1.4956, -0.7096};
-
-		for (unsigned i = 0; i < 3; i++) {
-			if (fabsf(vector_true(i) - vector_q(i)) > tol) {
-				PX4_WARN("Quaternion method 'rotate' outside tolerance");
-				rc = 1;
-			}
-		}
-
-		q.from_euler(M_PI_2_F, -M_PI_2_F, -M_PI_F / 3.0f);
-		vector_q = q.conjugate(vector);
-		vector_true = { -1.3660, 0.3660, 1.0000};
-
-		for (unsigned i = 0; i < 3; i++) {
-			if (fabsf(vector_true(i) - vector_q(i)) > tol) {
-				PX4_WARN("Quaternion method 'rotate' outside tolerance");
-				rc = 1;
-			}
-		}
 	}
 	return rc;
 }
+
+bool MathlibTest::testRotationMatrixQuaternion(void)
+{
+	// test conversion rotation matrix to quaternion and back
+	math::Matrix<3, 3> R_orig;
+	math::Matrix<3, 3> R;
+	math::Quaternion q;
+	float diff = 0.2f;
+	float tol = 0.00001f;
+
+	//PX4_INFO("Quaternion transformation methods test.");
+
+	for (float roll = -M_PI_F; roll <= M_PI_F; roll += diff) {
+		for (float pitch = -M_PI_2_F; pitch <= M_PI_2_F; pitch += diff) {
+			for (float yaw = -M_PI_F; yaw <= M_PI_F; yaw += diff) {
+				R_orig.from_euler(roll, pitch, yaw);
+				q.from_dcm(R_orig);
+				R = q.to_dcm();
+
+				for (int i = 0; i < 3; i++) {
+					for (int j = 0; j < 3; j++) {
+						ut_assert("Quaternion method 'from_dcm' or 'to_dcm' outside tolerance!", fabsf(R_orig.data[i][j] - R.data[i][j]) < tol);
+					}
+				}
+			}
+		}
+	}
+
+	return true;
+}
+
+
+bool MathlibTest::testQuaternionfrom_dcm(void)
+{
+	// test against some known values
+	float tol = 0.0001f;
+	math::Quaternion q_true = {1.0f, 0.0f, 0.0f, 0.0f};
+
+	math::Matrix<3, 3> R_orig;
+	R_orig.identity();
+
+	math::Quaternion q;
+	q.from_dcm(R_orig);
+
+	for (unsigned i = 0; i < 4; i++) {
+		ut_assert("Quaternion method 'from_dcm()' outside tolerance!", fabsf(q.data[i] - q_true.data[i]) < tol);
+	}
+
+	return true;
+}
+
+bool MathlibTest::testQuaternionfrom_euler(void)
+{
+	float tol = 0.0001f;
+	math::Quaternion q_true = {1.0f, 0.0f, 0.0f, 0.0f};
+
+	math::Matrix<3, 3> R_orig;
+	R_orig.identity();
+
+	math::Quaternion q;
+	q.from_dcm(R_orig);
+
+	q_true.from_euler(0.3f, 0.2f, 0.1f);
+	q = {0.9833f, 0.1436f, 0.1060f, 0.0343f};
+
+	for (unsigned i = 0; i < 4; i++) {
+		ut_assert("Quaternion method 'from_euler()' outside tolerance!", fabsf(q.data[i] - q_true.data[i]) < tol);
+	}
+
+	q_true.from_euler(-1.5f, -0.2f, 0.5f);
+	q = {0.7222f, -0.6391f, -0.2386f, 0.1142f};
+
+	for (unsigned i = 0; i < 4; i++) {
+		ut_assert("Quaternion method 'from_euler()' outside tolerance!", fabsf(q.data[i] - q_true.data[i]) < tol);
+	}
+
+	q_true.from_euler(M_PI_2_F, -M_PI_2_F, -M_PI_F / 3);
+	q = {0.6830f, 0.1830f, -0.6830f, 0.1830f};
+
+	for (unsigned i = 0; i < 4; i++) {
+		ut_assert("Quaternion method 'from_euler()' outside tolerance!", fabsf(q.data[i] - q_true.data[i]) < tol);
+	}
+
+	return true;
+}
+
+bool MathlibTest::testQuaternionRotate(void)
+{
+	// test quaternion method "rotate" (rotate vector by quaternion)
+	Vector<3> vector = {1.0f, 1.0f, 1.0f};
+	Vector<3> vector_q;
+	Vector<3> vector_r;
+	Quaternion q;
+	Matrix<3, 3> R;
+	float diff = 0.2f;
+	float tol = 0.00001f;
+
+	//PX4_INFO("Quaternion vector rotation method test.");
+
+	for (float roll = -M_PI_F; roll <= M_PI_F; roll += diff) {
+		for (float pitch = -M_PI_2_F; pitch <= M_PI_2_F; pitch += diff) {
+			for (float yaw = -M_PI_F; yaw <= M_PI_F; yaw += diff) {
+				R.from_euler(roll, pitch, yaw);
+				q.from_euler(roll, pitch, yaw);
+				vector_r = R * vector;
+				vector_q = q.conjugate(vector);
+
+				for (int i = 0; i < 3; i++) {
+					ut_assert("Quaternion method 'rotate' outside tolerance", fabsf(vector_r(i) - vector_q(i)) < tol);
+				}
+			}
+		}
+	}
+
+
+	// test some values calculated with matlab
+	tol = 0.0001f;
+	q.from_euler(M_PI_2_F, 0.0f, 0.0f);
+	vector_q = q.conjugate(vector);
+	Vector<3> vector_true = {1.00f, -1.00f, 1.00f};
+
+	for (unsigned i = 0; i < 3; i++) {
+		ut_assert("Quaternion method 'rotate' outside tolerance", fabsf(vector_true(i) - vector_q(i)) < tol);
+	}
+
+	q.from_euler(0.3f, 0.2f, 0.1f);
+	vector_q = q.conjugate(vector);
+	vector_true = {1.1566, 0.7792, 1.0273};
+
+	for (unsigned i = 0; i < 3; i++) {
+		ut_assert("Quaternion method 'rotate' outside tolerance", fabsf(vector_true(i) - vector_q(i)) < tol);
+	}
+
+	q.from_euler(-1.5f, -0.2f, 0.5f);
+	vector_q = q.conjugate(vector);
+	vector_true = {0.5095, 1.4956, -0.7096};
+
+	for (unsigned i = 0; i < 3; i++) {
+		ut_assert("Quaternion method 'rotate' outside tolerance", fabsf(vector_true(i) - vector_q(i)) < tol);
+	}
+
+	q.from_euler(M_PI_2_F, -M_PI_2_F, -M_PI_F / 3.0f);
+	vector_q = q.conjugate(vector);
+	vector_true = { -1.3660, 0.3660, 1.0000};
+
+	for (unsigned i = 0; i < 3; i++) {
+		ut_assert("Quaternion method 'rotate' outside tolerance", fabsf(vector_true(i) - vector_q(i)) < tol);
+	}
+
+	return true;
+}
+
+bool MathlibTest::run_tests(void)
+{
+	ut_run_test(testVector2);
+	ut_run_test(testVector3);
+	ut_run_test(testVector4);
+	ut_run_test(testVector10);
+	ut_run_test(testMatrix3x3);
+	ut_run_test(testMatrix10x10);
+	ut_run_test(testMatrixNonsymmetric);
+	ut_run_test(testRotationMatrixQuaternion);
+	ut_run_test(testQuaternionfrom_dcm);
+	ut_run_test(testQuaternionfrom_euler);
+	ut_run_test(testQuaternionRotate);
+
+	return (_tests_failed == 0);
+}
+
+ut_declare_test_c(test_mathlib, MathlibTest)
diff --git a/src/systemcmds/tests/test_matrix.cpp b/src/systemcmds/tests/test_matrix.cpp
new file mode 100644
index 0000000000..2ef48e0c4f
--- /dev/null
+++ b/src/systemcmds/tests/test_matrix.cpp
@@ -0,0 +1,679 @@
+
+#include <unit_test/unit_test.h>
+
+#include <matrix/math.hpp>
+#include <matrix/filter.hpp>
+#include <matrix/integration.hpp>
+
+using namespace matrix;
+
+class MatrixTest : public UnitTest
+{
+public:
+	virtual bool run_tests(void);
+
+private:
+	bool attitudeTests();
+	bool filterTests();
+	bool helperTests();
+	bool integrationTests();
+	bool inverseTests();
+	bool matrixAssignmentTests();
+	bool matrixMultTests();
+	bool matrixScalarMultTests();
+	bool setIdentityTests();
+	bool sliceTests();
+	bool squareMatrixTests();
+	bool transposeTests();
+	bool vectorTests();
+	bool vector2Tests();
+	bool vector3Tests();
+	bool vectorAssignmentTests();
+};
+
+bool MatrixTest::run_tests(void)
+{
+	ut_run_test(attitudeTests);
+	ut_run_test(filterTests);
+	ut_run_test(helperTests);
+	ut_run_test(integrationTests);
+	ut_run_test(inverseTests);
+	ut_run_test(matrixAssignmentTests);
+	ut_run_test(matrixMultTests);
+	ut_run_test(matrixScalarMultTests);
+	ut_run_test(setIdentityTests);
+	ut_run_test(sliceTests);
+	ut_run_test(squareMatrixTests);
+	ut_run_test(transposeTests);
+	ut_run_test(vectorTests);
+	ut_run_test(vector2Tests);
+	ut_run_test(vector3Tests);
+	ut_run_test(vectorAssignmentTests);
+
+	return (_tests_failed == 0);
+}
+
+
+ut_declare_test_c(test_matrix, MatrixTest)
+
+
+template class matrix::Quaternion<float>;
+template class matrix::Euler<float>;
+template class matrix::Dcm<float>;
+
+bool MatrixTest::attitudeTests(void)
+{
+	double eps = 1e-6;
+
+	// check data
+	Eulerf euler_check(0.1f, 0.2f, 0.3f);
+	Quatf q_check(0.98334744f, 0.0342708f, 0.10602051f, .14357218f);
+	float dcm_data[] =  {
+		0.93629336f, -0.27509585f,  0.21835066f,
+		0.28962948f,  0.95642509f, -0.03695701f,
+		-0.19866933f,  0.0978434f,  0.97517033f
+	};
+	Dcmf dcm_check(dcm_data);
+
+	// euler ctor
+	ut_test(isEqual(euler_check, Vector3f(0.1f, 0.2f, 0.3f)));
+
+
+	// euler default ctor
+	Eulerf e;
+	Eulerf e_zero = zeros<float, 3, 1>();
+	ut_test(isEqual(e, e_zero));
+	ut_test(isEqual(e, e));
+
+	// euler vector ctor
+	Vector<float, 3> v;
+	v(0) = 0.1f;
+	v(1) = 0.2f;
+	v(2) = 0.3f;
+	Eulerf euler_copy(v);
+	ut_test(isEqual(euler_copy, euler_check));
+
+	// quaternion ctor
+	Quatf q0(1, 2, 3, 4);
+	Quatf q(q0);
+	ut_test(fabs(q(0) - 1) < eps);
+	ut_test(fabs(q(1) - 2) < eps);
+	ut_test(fabs(q(2) - 3) < eps);
+	ut_test(fabs(q(3) - 4) < eps);
+
+	// quat normalization
+	q.normalize();
+	ut_test(isEqual(q, Quatf(0.18257419f,  0.36514837f,
+				 0.54772256f,  0.73029674f)));
+	ut_test(isEqual(q0.unit(), q));
+
+	// quat default ctor
+	q = Quatf();
+	ut_test(isEqual(q, Quatf(1, 0, 0, 0)));
+
+	// euler to quaternion
+	q = Quatf(euler_check);
+	ut_test(isEqual(q, q_check));
+
+	// euler to dcm
+	Dcmf dcm(euler_check);
+	ut_test(isEqual(dcm, dcm_check));
+
+	// quaternion to euler
+	Eulerf e1(q_check);
+	ut_test(isEqual(e1, euler_check));
+
+	// quaternion to dcm
+	Dcmf dcm1(q_check);
+	ut_test(isEqual(dcm1, dcm_check));
+
+	// dcm default ctor
+	Dcmf dcm2;
+	SquareMatrix<float, 3> I = eye<float, 3>();
+	ut_test(isEqual(dcm2, I));
+
+	// dcm to euler
+	Eulerf e2(dcm_check);
+	ut_test(isEqual(e2, euler_check));
+
+	// dcm to quaterion
+	Quatf q2(dcm_check);
+	ut_test(isEqual(q2, q_check));
+
+	// constants
+	double deg2rad = M_PI / 180.0;
+	double rad2deg = 180.0 / M_PI;
+
+	// euler dcm round trip check
+	for (int roll = -90; roll <= 90; roll += 90) {
+		for (int pitch = -90; pitch <= 90; pitch += 90) {
+			for (int yaw = -179; yaw <= 180; yaw += 90) {
+				// note if theta = pi/2, then roll is set to zero
+				int roll_expected = roll;
+				int yaw_expected = yaw;
+
+				if (pitch == 90) {
+					roll_expected = 0;
+					yaw_expected = yaw - roll;
+
+				} else if (pitch == -90) {
+					roll_expected = 0;
+					yaw_expected = yaw + roll;
+				}
+
+				if (yaw_expected < -180) { yaw_expected += 360; }
+
+				if (yaw_expected > 180) { yaw_expected -= 360; }
+
+				//printf("roll:%d pitch:%d yaw:%d\n", roll, pitch, yaw);
+				Euler<double> euler_expected(
+					deg2rad * double(roll_expected),
+					deg2rad * double(pitch),
+					deg2rad * double(yaw_expected));
+				Euler<double> euler(
+					deg2rad * double(roll),
+					deg2rad * double(pitch),
+					deg2rad * double(yaw));
+				Dcm<double> dcm_from_euler(euler);
+				//dcm_from_euler.print();
+				Euler<double> euler_out(dcm_from_euler);
+				ut_test(isEqual(rad2deg * euler_expected, rad2deg * euler_out));
+
+				Eulerf eulerf_expected(
+					float(deg2rad)*float(roll_expected),
+					float(deg2rad)*float(pitch),
+					float(deg2rad)*float(yaw_expected));
+				Eulerf eulerf(float(deg2rad)*float(roll),
+					      float(deg2rad)*float(pitch),
+					      float(deg2rad)*float(yaw));
+				Dcm<float> dcm_from_eulerf(eulerf);
+				Euler<float> euler_outf(dcm_from_eulerf);
+				ut_test(isEqual(float(rad2deg)*eulerf_expected,
+						float(rad2deg)*euler_outf));
+			}
+		}
+	}
+
+	// quaterion copy ctors
+	float data_v4[] = {1, 2, 3, 4};
+	Vector<float, 4> v4(data_v4);
+	Quatf q_from_v(v4);
+	ut_test(isEqual(q_from_v, v4));
+
+	Matrix<float, 4, 1> m4(data_v4);
+	Quatf q_from_m(m4);
+	ut_test(isEqual(q_from_m, m4));
+
+	// quaternion derivate
+	Vector<float, 4> q_dot = q.derivative(Vector3f(1, 2, 3));
+
+	// quaternion product
+	Quatf q_prod_check(
+		0.93394439f, 0.0674002f, 0.20851f, 0.28236266f);
+	ut_test(isEqual(q_prod_check, q_check * q_check));
+	q_check *= q_check;
+	ut_test(isEqual(q_prod_check, q_check));
+
+	// Quaternion scalar multiplication
+	float scalar = 0.5;
+	Quatf q_scalar_mul(1.0f, 2.0f, 3.0f, 4.0f);
+	Quatf q_scalar_mul_check(1.0f * scalar, 2.0f * scalar,
+				 3.0f * scalar,  4.0f * scalar);
+	Quatf q_scalar_mul_res = scalar * q_scalar_mul;
+	ut_test(isEqual(q_scalar_mul_check, q_scalar_mul_res));
+	Quatf q_scalar_mul_res2 = q_scalar_mul * scalar;
+	ut_test(isEqual(q_scalar_mul_check, q_scalar_mul_res2));
+	Quatf q_scalar_mul_res3(q_scalar_mul);
+	q_scalar_mul_res3 *= scalar;
+	ut_test(isEqual(q_scalar_mul_check, q_scalar_mul_res3));
+
+	// quaternion inverse
+	q = q_check.inversed();
+	ut_test(fabs(q_check(0) - q(0)) < eps);
+	ut_test(fabs(q_check(1) + q(1)) < eps);
+	ut_test(fabs(q_check(2) + q(2)) < eps);
+	ut_test(fabs(q_check(3) + q(3)) < eps);
+
+	q = q_check;
+	q.invert();
+	ut_test(fabs(q_check(0) - q(0)) < eps);
+	ut_test(fabs(q_check(1) + q(1)) < eps);
+	ut_test(fabs(q_check(2) + q(2)) < eps);
+	ut_test(fabs(q_check(3) + q(3)) < eps);
+
+	// rotate quaternion (nonzero rotation)
+	Quatf qI(1.0f, 0.0f, 0.0f, 0.0f);
+	Vector<float, 3> rot;
+	rot(0) = 1.0f;
+	rot(1) = rot(2) = 0.0f;
+	qI.rotate(rot);
+	Quatf q_true(cosf(1.0f / 2), sinf(1.0f / 2), 0.0f, 0.0f);
+	ut_test(fabs(qI(0) - q_true(0)) < eps);
+	ut_test(fabs(qI(1) - q_true(1)) < eps);
+	ut_test(fabs(qI(2) - q_true(2)) < eps);
+	ut_test(fabs(qI(3) - q_true(3)) < eps);
+
+	// rotate quaternion (zero rotation)
+	qI = Quatf(1.0f, 0.0f, 0.0f, 0.0f);
+	rot(0) = 0.0f;
+	rot(1) = rot(2) = 0.0f;
+	qI.rotate(rot);
+	q_true = Quatf(cosf(0.0f), sinf(0.0f), 0.0f, 0.0f);
+	ut_test(fabs(qI(0) - q_true(0)) < eps);
+	ut_test(fabs(qI(1) - q_true(1)) < eps);
+	ut_test(fabs(qI(2) - q_true(2)) < eps);
+	ut_test(fabs(qI(3) - q_true(3)) < eps);
+
+	// get rotation axis from quaternion (nonzero rotation)
+	q = Quatf(cosf(1.0f / 2), 0.0f, sinf(1.0f / 2), 0.0f);
+	rot = q.to_axis_angle();
+	ut_test(fabs(rot(0)) < eps);
+	ut_test(fabs(rot(1) - 1.0f) < eps);
+	ut_test(fabs(rot(2)) < eps);
+
+	// get rotation axis from quaternion (zero rotation)
+	q = Quatf(1.0f, 0.0f, 0.0f, 0.0f);
+	rot = q.to_axis_angle();
+	ut_test(fabs(rot(0)) < eps);
+	ut_test(fabs(rot(1)) < eps);
+	ut_test(fabs(rot(2)) < eps);
+
+	// from axis angle (zero rotation)
+	rot(0) = rot(1) = rot(2) = 0.0f;
+	q.from_axis_angle(rot, 0.0f);
+	q_true = Quatf(1.0f, 0.0f, 0.0f, 0.0f);
+	ut_test(fabs(q(0) - q_true(0)) < eps);
+	ut_test(fabs(q(1) - q_true(1)) < eps);
+	ut_test(fabs(q(2) - q_true(2)) < eps);
+	ut_test(fabs(q(3) - q_true(3)) < eps);
+
+	return true;
+}
+
+bool MatrixTest::filterTests(void)
+{
+	const size_t n_x = 6;
+	const size_t n_y = 5;
+	SquareMatrix<float, n_x> P = eye<float, n_x>();
+	SquareMatrix<float, n_y> R = eye<float, n_y>();
+	Matrix<float, n_y, n_x> C;
+	C.setIdentity();
+	float data[] = {1, 2, 3, 4, 5};
+	Vector<float, n_y> r(data);
+
+	Vector<float, n_x> dx;
+	SquareMatrix<float, n_x> dP;
+	float beta = 0;
+	kalman_correct<float, 6, 5>(P, C, R, r, dx, dP, beta);
+
+	float data_check[] = {0.5, 1, 1.5, 2, 2.5, 0};
+	Vector<float, n_x> dx_check(data_check);
+	ut_test(isEqual(dx, dx_check));
+
+	return true;
+}
+
+bool MatrixTest::helperTests(void)
+{
+	ut_test(fabs(wrap_pi(4.0) - (4.0 - 2 * M_PI)) < 1e-5);
+	ut_test(fabs(wrap_pi(-4.0) - (-4.0 + 2 * M_PI)) < 1e-5);
+	ut_test(fabs(wrap_pi(3.0) - (3.0)) < 1e-3);
+	wrap_pi(NAN);
+
+	Vector3f a(1, 2, 3);
+	Vector3f b(4, 5, 6);
+	ut_test(isEqual(a, a));
+
+	return true;
+}
+
+
+Vector<float, 6> f(float t, const Matrix<float, 6, 1> &y, const Matrix<float, 3, 1> &u);
+
+Vector<float, 6> f(float t, const Matrix<float, 6, 1> &y, const Matrix<float, 3, 1> &u)
+{
+	float v = -sinf(t);
+	return v * ones<float, 6, 1>();
+}
+
+bool MatrixTest::integrationTests(void)
+{
+	Vector<float, 6> y = ones<float, 6, 1>();
+	Vector<float, 3> u = ones<float, 3, 1>();
+	float t0 = 0;
+	float tf = 2;
+	float h = 0.001f;
+	integrate_rk4(f, y, u, t0, tf, h, y);
+	float v = 1 + cosf(tf) - cosf(t0);
+	ut_test(isEqual(y, (ones<float, 6, 1>()*v)));
+
+	return true;
+}
+
+template class matrix::SquareMatrix<float, 3>;
+
+bool MatrixTest::inverseTests(void)
+{
+	float data[9] = {0, 2, 3,
+			 4, 5, 6,
+			 7, 8, 10
+			};
+	float data_check[9] = {
+		-0.4f, -0.8f,  0.6f,
+		-0.4f,  4.2f, -2.4f,
+		0.6f, -2.8f,  1.6f
+	};
+
+	SquareMatrix<float, 3> A(data);
+	SquareMatrix<float, 3> A_I = inv(A);
+	SquareMatrix<float, 3> A_I_check(data_check);
+
+	float eps = 1e-5;
+
+	ut_test((A_I - A_I_check).abs().max() < eps);
+
+	SquareMatrix<float, 3> zero_test = zeros<float, 3, 3>();
+	inv(zero_test);
+
+	return true;
+}
+
+bool MatrixTest::matrixAssignmentTests(void)
+{
+	Matrix3f m;
+	m.setZero();
+	m(0, 0) = 1;
+	m(0, 1) = 2;
+	m(0, 2) = 3;
+	m(1, 0) = 4;
+	m(1, 1) = 5;
+	m(1, 2) = 6;
+	m(2, 0) = 7;
+	m(2, 1) = 8;
+	m(2, 2) = 9;
+
+	float data[9] = {1, 2, 3, 4, 5, 6, 7, 8, 9};
+	Matrix3f m2(data);
+
+	double eps = 1e-6f;
+
+	for (int i = 0; i < 9; i++) {
+		ut_test(fabs(data[i] - m2.data()[i]) < eps);
+	}
+
+	float data_times_2[9] = {2, 4, 6, 8, 10, 12, 14, 16, 18};
+	Matrix3f m3(data_times_2);
+
+	ut_test(isEqual(m, m2));
+	ut_test(!(m == m3));
+
+	m2 *= 2;
+	ut_test(m2 == m3);
+
+	m2 /= 2;
+	m2 -= 1;
+	float data_minus_1[9] = {0, 1, 2, 3, 4, 5, 6, 7, 8};
+	ut_test(Matrix3f(data_minus_1) == m2);
+
+	m2 += 1;
+	ut_test(Matrix3f(data) == m2);
+
+	m3 -= m2;
+
+	ut_test(m3 == m2);
+
+	float data_row_02_swap[9] = {
+		7, 8, 9,
+		4, 5, 6,
+		1, 2, 3,
+	};
+
+	float data_col_02_swap[9] = {
+		3, 2, 1,
+		6, 5, 4,
+		9, 8, 7
+	};
+
+	Matrix3f m4(data);
+
+	ut_test(-m4 == m4 * (-1));
+
+	m4.swapCols(0, 2);
+	ut_test(m4 == Matrix3f(data_col_02_swap));
+	m4.swapCols(0, 2);
+	m4.swapRows(0, 2);
+	ut_test(m4 == Matrix3f(data_row_02_swap));
+	ut_test(fabs(m4.min() - 1) < 1e-5);
+
+	Scalar<float> s;
+	s = 1;
+	ut_test(fabs(s - 1) < 1e-5);
+
+	Matrix<float, 1, 1> m5 = s;
+	ut_test(fabs(m5(0, 0) - s) < 1e-5);
+
+	Matrix<float, 2, 2> m6;
+	m6.setRow(0, Vector2f(1, 1));
+	m6.setCol(0, Vector2f(1, 1));
+
+	return true;
+}
+
+bool MatrixTest::matrixMultTests(void)
+{
+	float data[9] = {1, 0, 0, 0, 1, 0, 1, 0, 1};
+	Matrix3f A(data);
+	float data_check[9] = {1, 0, 0, 0, 1, 0, -1, 0, 1};
+	Matrix3f A_I(data_check);
+	Matrix3f I;
+	I.setIdentity();
+	Matrix3f R = A * A_I;
+	ut_test(isEqual(R, I));
+
+	Matrix3f R2 = A;
+	R2 *= A_I;
+	ut_test(isEqual(R2, I));
+
+
+	Matrix3f A2 = eye<float, 3>() * 2;
+	Matrix3f B = A2.emult(A2);
+	Matrix3f B_check = eye<float, 3>() * 4;
+	ut_test(isEqual(B, B_check));
+
+	return true;
+}
+
+bool MatrixTest::matrixScalarMultTests(void)
+{
+	float data[9] = {1, 2, 3, 4, 5, 6, 7, 8, 9};
+	Matrix3f A(data);
+	A = A * 2;
+	float data_check[9] = {2, 4, 6, 8, 10, 12, 14, 16, 18};
+	Matrix3f A_check(data_check);
+	ut_test(isEqual(A, A_check));
+
+	return true;
+}
+
+
+template class matrix::Matrix<float, 3, 3>;
+
+bool MatrixTest::setIdentityTests(void)
+{
+	Matrix3f A;
+	A.setIdentity();
+
+	for (int i = 0; i < 3; i++) {
+		for (int j = 0; j < 3; j++) {
+			if (i == j) {
+				ut_test(fabs(A(i, j) -  1) < 1e-7);
+
+			} else {
+				ut_test(fabs(A(i, j) -  0) < 1e-7);
+			}
+		}
+	}
+
+	return true;
+}
+
+bool MatrixTest::sliceTests(void)
+{
+	float data[9] = {0, 2, 3,
+			 4, 5, 6,
+			 7, 8, 10
+			};
+	float data_check[6] = {
+		4, 5, 6,
+		7, 8, 10
+	};
+	SquareMatrix<float, 3> A(data);
+	Matrix<float, 2, 3> B_check(data_check);
+	Matrix<float, 2, 3> B(A.slice<2, 3>(1, 0));
+	ut_test(isEqual(B, B_check));
+
+	float data_2[4] = {
+		11, 12,
+		13, 14
+	};
+
+	Matrix<float, 2, 2> C(data_2);
+	A.set(C, 1, 1);
+
+	float data_2_check[9] = {
+		0, 2, 3,
+		4, 11, 12,
+		7, 13, 14
+	};
+	Matrix<float, 3, 3> D(data_2_check);
+	ut_test(isEqual(A, D));
+
+	return true;
+}
+
+
+bool MatrixTest::squareMatrixTests(void)
+{
+	float data[9] = {1, 2, 3,
+			 4, 5, 6,
+			 7, 8, 10
+			};
+	SquareMatrix<float, 3> A(data);
+	Vector3<float> diag_check(1, 5, 10);
+
+	ut_test(isEqual(A.diag(), diag_check));
+
+	float data_check[9] = {
+		1.01158503f,  0.02190432f,  0.03238144f,
+		0.04349195f,  1.05428524f,  0.06539627f,
+		0.07576783f,  0.08708946f,  1.10894048f
+	};
+
+	float dt = 0.01f;
+	SquareMatrix<float, 3> eA = expm(SquareMatrix<float, 3>(A * dt), 5);
+	SquareMatrix<float, 3> eA_check(data_check);
+
+	float eps = 1e-3;
+	ut_test((eA - eA_check).abs().max() < eps);
+
+	return true;
+}
+
+bool MatrixTest::transposeTests(void)
+{
+	float data[6] = {1, 2, 3, 4, 5, 6};
+	Matrix<float, 2, 3> A(data);
+	Matrix<float, 3, 2> A_T = A.transpose();
+	float data_check[6] = {1, 4, 2, 5, 3, 6};
+	Matrix<float, 3, 2> A_T_check(data_check);
+	ut_test(isEqual(A_T, A_T_check));
+
+	return true;
+}
+
+bool MatrixTest::vectorTests(void)
+{
+	float data1[] = {1, 2, 3, 4, 5};
+	float data2[] = {6, 7, 8, 9, 10};
+	Vector<float, 5> v1(data1);
+	ut_test(fabs(v1.norm() - 7.416198487095663f) < 1e-5);
+	Vector<float, 5> v2(data2);
+	ut_test(fabs(v1.dot(v2) - 130.0f) < 1e-5);
+	v2.normalize();
+	Vector<float, 5> v3(v2);
+	ut_test(v2 == v3);
+	float data1_sq[] = {1, 4, 9, 16, 25};
+	Vector<float, 5> v4(data1_sq);
+	ut_test(v1 == v4.pow(0.5));
+
+	return true;
+}
+
+bool MatrixTest::vector2Tests(void)
+{
+	Vector2f a(1, 0);
+	Vector2f b(0, 1);
+	ut_test(fabs(a % b - 1.0f) < 1e-5);
+
+	Vector2f c;
+	ut_test(fabs(c(0) - 0) < 1e-5);
+	ut_test(fabs(c(1) - 0) < 1e-5);
+
+	Matrix<float, 2, 1> d(a);
+	ut_test(fabs(d(0, 0) - 1) < 1e-5);
+	ut_test(fabs(d(1, 0) - 0) < 1e-5);
+
+	Vector2f e(d);
+	ut_test(fabs(e(0) - 1) < 1e-5);
+	ut_test(fabs(e(1) - 0) < 1e-5);
+
+	float data[] = {4, 5};
+	Vector2f f(data);
+	ut_test(fabs(f(0) - 4) < 1e-5);
+	ut_test(fabs(f(1) - 5) < 1e-5);
+	return true;
+}
+
+bool MatrixTest::vector3Tests(void)
+{
+	Vector3f a(1, 0, 0);
+	Vector3f b(0, 1, 0);
+	Vector3f c = a.cross(b);
+	ut_test(c == Vector3f(0, 0, 1));
+	c = a % b;
+	ut_test(c == Vector3f(0, 0, 1));
+	Matrix<float, 3, 1> d(c);
+	Vector3f e(d);
+	ut_test(e == d);
+	float data[] = {4, 5, 6};
+	Vector3f f(data);
+	ut_test(f == Vector3f(4, 5, 6));
+	return true;
+}
+
+bool MatrixTest::vectorAssignmentTests(void)
+{
+	Vector3f v;
+	v(0) = 1;
+	v(1) = 2;
+	v(2) = 3;
+
+	static const float eps = 1e-7f;
+
+	ut_test(fabsf(v(0) - 1) < eps);
+	ut_test(fabsf(v(1) - 2) < eps);
+	ut_test(fabsf(v(2) - 3) < eps);
+
+	Vector3f v2(4, 5, 6);
+
+	ut_test(fabsf(v2(0) - 4) < eps);
+	ut_test(fabsf(v2(1) - 5) < eps);
+	ut_test(fabsf(v2(2) - 6) < eps);
+
+	SquareMatrix<float, 3> m = diag(Vector3f(1, 2, 3));
+	ut_test(fabsf(m(0, 0) - 1) < eps);
+	ut_test(fabsf(m(1, 1) - 2) < eps);
+	ut_test(fabsf(m(2, 2) - 3) < eps);
+
+	return true;
+}
diff --git a/src/systemcmds/tests/test_mixer.cpp b/src/systemcmds/tests/test_mixer.cpp
index 893a65a1b9..88c37d6e9c 100644
--- a/src/systemcmds/tests/test_mixer.cpp
+++ b/src/systemcmds/tests/test_mixer.cpp
@@ -85,22 +85,22 @@ int test_mixer(int argc, char *argv[])
 	uint16_t servo_predicted[output_max];
 	int16_t reverse_pwm_mask = 0;
 
-	PX4_INFO("testing mixer");
+	//PX4_INFO("testing mixer");
 
+#if !defined(CONFIG_ARCH_BOARD_SITL)
 	const char *filename = "/etc/mixers/IO_pass.mix";
+#else
+	const char *filename = "../../../../ROMFS/px4fmu_test/mixers/IO_pass.mix";
+#endif
 
-	if (argc > 1) {
-		filename = argv[1];
-	}
-
-	PX4_INFO("loading: %s", filename);
+	//PX4_INFO("loading: %s", filename);
 
 	char		buf[2048];
 
 	load_mixer_file(filename, &buf[0], sizeof(buf));
 	unsigned loaded = strlen(buf);
 
-	fprintf(stderr, "loaded: \n\"%s\"\n (%d chars)", &buf[0], loaded);
+	//fprintf(stderr, "loaded: \n\"%s\"\n (%d chars)", &buf[0], loaded);
 
 	/* load the mixer in chunks, like
 	 * in the case of a remote load,
@@ -114,11 +114,7 @@ int test_mixer(int argc, char *argv[])
 	/* load at once test */
 	unsigned xx = loaded;
 	mixer_group.load_from_buf(&buf[0], xx);
-	PX4_INFO("complete buffer load: loaded %u mixers", mixer_group.count());
-
-	if (mixer_group.count() != 8) {
-		return 1;
-	}
+	//ASSERT_EQ(mixer_group.count(), 8);
 
 	unsigned empty_load = 2;
 	char empty_buf[2];
@@ -126,7 +122,9 @@ int test_mixer(int argc, char *argv[])
 	empty_buf[1] = '\0';
 	mixer_group.reset();
 	mixer_group.load_from_buf(&empty_buf[0], empty_load);
-	PX4_INFO("empty buffer load: loaded %u mixers, used: %u", mixer_group.count(), empty_load);
+	//PX4_INFO("empty buffer load: loaded %u mixers, used: %u", mixer_group.count(), empty_load);
+
+	//ASSERT_NE(empty_load, 0);
 
 	if (empty_load != 0) {
 		return 1;
@@ -140,7 +138,7 @@ int test_mixer(int argc, char *argv[])
 
 	unsigned transmitted = 0;
 
-	PX4_INFO("transmitted: %d, loaded: %d", transmitted, loaded);
+	//PX4_INFO("transmitted: %d, loaded: %d", transmitted, loaded);
 
 	while (transmitted < loaded) {
 
@@ -155,7 +153,7 @@ int test_mixer(int argc, char *argv[])
 		memcpy(&mixer_text[mixer_text_length], &buf[transmitted], text_length);
 		mixer_text_length += text_length;
 		mixer_text[mixer_text_length] = '\0';
-		fprintf(stderr, "buflen %u, text:\n\"%s\"", mixer_text_length, &mixer_text[0]);
+		//fprintf(stderr, "buflen %u, text:\n\"%s\"", mixer_text_length, &mixer_text[0]);
 
 		/* process the text buffer, adding new mixers as their descriptions can be parsed */
 		unsigned resid = mixer_text_length;
@@ -163,7 +161,7 @@ int test_mixer(int argc, char *argv[])
 
 		/* if anything was parsed */
 		if (resid != mixer_text_length) {
-			fprintf(stderr, "used %u", mixer_text_length - resid);
+			//fprintf(stderr, "used %u", mixer_text_length - resid);
 
 			/* copy any leftover text to the base of the buffer for re-use */
 			if (resid > 0) {
@@ -176,7 +174,7 @@ int test_mixer(int argc, char *argv[])
 		transmitted += text_length;
 	}
 
-	PX4_INFO("chunked load: loaded %u mixers", mixer_group.count());
+	//PX4_INFO("chunked load: loaded %u mixers", mixer_group.count());
 
 	if (mixer_group.count() != 8) {
 		return 1;
@@ -198,7 +196,8 @@ int test_mixer(int argc, char *argv[])
 		r_page_servo_control_max[i] = PWM_DEFAULT_MAX;
 	}
 
-	PX4_INFO("PRE-ARM TEST: DISABLING SAFETY");
+	//PX4_INFO("PRE-ARM TEST: DISABLING SAFETY");
+
 	/* mix */
 	should_prearm = true;
 	mixed = mixer_group.mix(&outputs[0], output_max, NULL);
@@ -209,7 +208,7 @@ int test_mixer(int argc, char *argv[])
 	//warnx("mixed %d outputs (max %d), values:", mixed, output_max);
 	for (unsigned i = 0; i < mixed; i++) {
 
-		fprintf(stderr, "pre-arm:\t %d: out: %8.4f, servo: %d \n", i, (double)outputs[i], (int)r_page_servos[i]);
+		//fprintf(stderr, "pre-arm:\t %d: out: %8.4f, servo: %d \n", i, (double)outputs[i], (int)r_page_servos[i]);
 
 		if (i != actuator_controls_s::INDEX_THROTTLE) {
 			if (r_page_servos[i] < r_page_servo_control_min[i]) {
@@ -233,7 +232,7 @@ int test_mixer(int argc, char *argv[])
 		actuator_controls[i] = 0.1f;
 	}
 
-	PX4_INFO("ARMING TEST: STARTING RAMP");
+	//PX4_INFO("ARMING TEST: STARTING RAMP");
 	unsigned sleep_quantum_us = 10000;
 
 	hrt_abstime starttime = hrt_absolute_time();
@@ -250,7 +249,7 @@ int test_mixer(int argc, char *argv[])
 		//warnx("mixed %d outputs (max %d), values:", mixed, output_max);
 		for (unsigned i = 0; i < mixed; i++) {
 
-			fprintf(stderr, "ramp:\t %d: out: %8.4f, servo: %d \n", i, (double)outputs[i], (int)r_page_servos[i]);
+			//fprintf(stderr, "ramp:\t %d: out: %8.4f, servo: %d \n", i, (double)outputs[i], (int)r_page_servos[i]);
 
 			/* check mixed outputs to be zero during init phase */
 			if (hrt_elapsed_time(&starttime) < INIT_TIME_US &&
@@ -274,7 +273,7 @@ int test_mixer(int argc, char *argv[])
 		}
 	}
 
-	PX4_INFO("ARMING TEST: NORMAL OPERATION");
+	//PX4_INFO("ARMING TEST: NORMAL OPERATION");
 
 	for (int j = -jmax; j <= jmax; j++) {
 
@@ -292,7 +291,7 @@ int test_mixer(int argc, char *argv[])
 			       r_page_servo_control_max, outputs,
 			       r_page_servos, &pwm_limit);
 
-		fprintf(stderr, "mixed %d outputs (max %d)", mixed, output_max);
+		//fprintf(stderr, "mixed %d outputs (max %d)", mixed, output_max);
 
 		for (unsigned i = 0; i < mixed; i++) {
 			servo_predicted[i] = 1500 + outputs[i] * (r_page_servo_control_max[i] - r_page_servo_control_min[i]) / 2.0f;
@@ -306,7 +305,7 @@ int test_mixer(int argc, char *argv[])
 		}
 	}
 
-	PX4_INFO("ARMING TEST: DISARMING");
+	//PX4_INFO("ARMING TEST: DISARMING");
 
 	starttime = hrt_absolute_time();
 	sleepcount = 0;
@@ -324,7 +323,7 @@ int test_mixer(int argc, char *argv[])
 		//warnx("mixed %d outputs (max %d), values:", mixed, output_max);
 		for (unsigned i = 0; i < mixed; i++) {
 
-			fprintf(stderr, "disarmed:\t %d: out: %8.4f, servo: %d \n", i, (double)outputs[i], (int)r_page_servos[i]);
+			//fprintf(stderr, "disarmed:\t %d: out: %8.4f, servo: %d \n", i, (double)outputs[i], (int)r_page_servos[i]);
 
 			/* check mixed outputs to be zero during init phase */
 			if (r_page_servos[i] != r_page_servo_disarmed[i]) {
@@ -337,14 +336,14 @@ int test_mixer(int argc, char *argv[])
 		sleepcount++;
 
 		if (sleepcount % 10 == 0) {
-			printf(".");
-			fflush(stdout);
+			//printf(".");
+			//fflush(stdout);
 		}
 	}
 
-	printf("\n");
+	//printf("\n");
 
-	PX4_INFO("ARMING TEST: REARMING: STARTING RAMP");
+	//PX4_INFO("ARMING TEST: REARMING: STARTING RAMP");
 
 	starttime = hrt_absolute_time();
 	sleepcount = 0;
@@ -366,7 +365,7 @@ int test_mixer(int argc, char *argv[])
 
 			/* check ramp */
 
-			fprintf(stderr, "ramp:\t %d: out: %8.4f, servo: %d \n", i, (double)outputs[i], (int)r_page_servos[i]);
+			//fprintf(stderr, "ramp:\t %d: out: %8.4f, servo: %d \n", i, (double)outputs[i], (int)r_page_servos[i]);
 
 			if (hrt_elapsed_time(&starttime) < RAMP_TIME_US &&
 			    (r_page_servos[i] + 1 <= r_page_servo_disarmed[i] ||
@@ -388,46 +387,42 @@ int test_mixer(int argc, char *argv[])
 		sleepcount++;
 
 		if (sleepcount % 10 == 0) {
-			printf(".");
-			fflush(stdout);
+			//	printf(".");
+			//	fflush(stdout);
 		}
 	}
 
-	printf("\n");
+	//printf("\n");
 
 	/* load multirotor at once test */
 	mixer_group.reset();
 
-	if (argc > 2) {
-		filename = argv[2];
-
-	} else {
-		filename = "/etc/mixers/quad_test.mix";
-	}
+#if !defined(CONFIG_ARCH_BOARD_SITL)
+	filename = "/etc/mixers/quad_test.mix";
+#else
+	filename = "../../../../ROMFS/px4fmu_test/mixers/quad_test.mix";
+#endif
 
 	load_mixer_file(filename, &buf[0], sizeof(buf));
 	loaded = strlen(buf);
 
-	fprintf(stderr, "loaded: \n\"%s\"\n (%d chars)", &buf[0], loaded);
+	//fprintf(stderr, "loaded: \n\"%s\"\n (%d chars)", &buf[0], loaded);
 
 	unsigned mc_loaded = loaded;
 	mixer_group.load_from_buf(&buf[0], mc_loaded);
-	PX4_INFO("complete buffer load: loaded %u mixers", mixer_group.count());
+	//PX4_INFO("complete buffer load: loaded %u mixers", mixer_group.count());
 
 	if (mixer_group.count() != 5) {
 		PX4_ERR("FAIL: Quad test mixer load failed");
 		return 1;
 	}
 
-	PX4_INFO("SUCCESS: No errors in mixer test");
+	//PX4_INFO("SUCCESS: No errors in mixer test");
 	return 0;
 }
 
 static int
-mixer_callback(uintptr_t handle,
-	       uint8_t control_group,
-	       uint8_t control_index,
-	       float &control)
+mixer_callback(uintptr_t handle, uint8_t control_group, uint8_t control_index, float &control)
 {
 	if (control_group != 0) {
 		return -1;
diff --git a/unittests/uorb_unittests/uORBGtestTopics.hpp b/src/systemcmds/tests/test_perf.c
similarity index 68%
rename from unittests/uorb_unittests/uORBGtestTopics.hpp
rename to src/systemcmds/tests/test_perf.c
index ca8572efac..89be5000b2 100644
--- a/unittests/uorb_unittests/uORBGtestTopics.hpp
+++ b/src/systemcmds/tests/test_perf.c
@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- * Copyright (c) 2015 Mark Charlebois. All rights reserved.
+ *   Copyright (c) 2012-2014 PX4 Development Team. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -31,28 +31,39 @@
  *
  ****************************************************************************/
 
-#ifndef _uORBGtestTopics_hpp_
-#define _uORBGtestTopics_hpp_
+#include <px4_config.h>
+#include <px4_posix.h>
 
-#include "uORB/uORB.h"
+#include <systemlib/perf_counter.h>
 
-namespace uORB_test
+#include "tests.h"
+
+int
+test_perf(int argc, char *argv[])
 {
-   struct orb_topic_A
-   {
-     int16_t val;
-   };
+	perf_counter_t cc = perf_alloc(PC_COUNT, "test_count");
+	perf_counter_t ec = perf_alloc(PC_ELAPSED, "test_elapsed");
 
-   struct orb_topic_B
-   {
-     int16_t val;
-   };
+	if ((cc == NULL) || (ec == NULL)) {
+		printf("perf: counter alloc failed\n");
+		return 1;
+	}
 
-   ORB_DEFINE(topicA, struct orb_topic_A, nullptr, "TOPICA:int16 val;");
-   ORB_DEFINE(topicB, struct orb_topic_B, nullptr, "TOPICB:int16 val;");
+	perf_begin(ec);
+	perf_count(cc);
+	perf_count(cc);
+	perf_count(cc);
+	perf_count(cc);
+	printf("perf: expect count of 4\n");
+	perf_print_counter(cc);
+	perf_end(ec);
+	printf("perf: expect count of 1\n");
+	perf_print_counter(ec);
+	printf("perf: expect at least two counters\n");
+	perf_print_all(0);
 
-   ORB_DEFINE(topicA_clone, struct orb_topic_A, nullptr, "TOPICA_CLONE:int16 val;";
-   ORB_DEFINE(topicB_clone, struct orb_topic_B, nullptr, "TOPICB_CLONE:int16 val;");
+	perf_free(cc);
+	perf_free(ec);
+
+	return OK;
 }
-
-#endif // _UnitTest_uORBTopics_hpp_
diff --git a/src/systemcmds/tests/test_uart_baudchange.c b/src/systemcmds/tests/test_uart_baudchange.c
index 7c138efca1..7156543021 100644
--- a/src/systemcmds/tests/test_uart_baudchange.c
+++ b/src/systemcmds/tests/test_uart_baudchange.c
@@ -57,40 +57,6 @@
 #include <math.h>
 #include <float.h>
 
-
-/****************************************************************************
- * Pre-processor Definitions
- ****************************************************************************/
-
-/****************************************************************************
- * Private Types
- ****************************************************************************/
-
-/****************************************************************************
- * Private Function Prototypes
- ****************************************************************************/
-
-/****************************************************************************
- * Private Data
- ****************************************************************************/
-
-/****************************************************************************
- * Public Data
- ****************************************************************************/
-
-/****************************************************************************
- * Private Functions
- ****************************************************************************/
-
-
-/****************************************************************************
- * Public Functions
- ****************************************************************************/
-
-/****************************************************************************
- * Name: test_led
- ****************************************************************************/
-
 int test_uart_baudchange(int argc, char *argv[])
 {
 	int uart2_nwrite = 0;
diff --git a/src/systemcmds/tests/test_uart_console.c b/src/systemcmds/tests/test_uart_console.c
index 682ad5a129..dc1f59c57d 100644
--- a/src/systemcmds/tests/test_uart_console.c
+++ b/src/systemcmds/tests/test_uart_console.c
@@ -56,40 +56,6 @@
 #include <float.h>
 #include <drivers/drv_hrt.h>
 
-
-/****************************************************************************
- * Pre-processor Definitions
- ****************************************************************************/
-
-/****************************************************************************
- * Private Types
- ****************************************************************************/
-
-/****************************************************************************
- * Private Function Prototypes
- ****************************************************************************/
-
-/****************************************************************************
- * Private Data
- ****************************************************************************/
-
-/****************************************************************************
- * Public Data
- ****************************************************************************/
-
-/****************************************************************************
- * Private Functions
- ****************************************************************************/
-
-
-/****************************************************************************
- * Public Functions
- ****************************************************************************/
-
-/****************************************************************************
- * Name: test_led
- ****************************************************************************/
-
 static void *receive_loop(void *arg)
 {
 	int uart_usb = open("/dev/ttyACM0", O_RDONLY | O_NOCTTY);
diff --git a/src/systemcmds/tests/tests.h b/src/systemcmds/tests/tests.h
index 552a0c2d6b..2f5004c36c 100644
--- a/src/systemcmds/tests/tests.h
+++ b/src/systemcmds/tests/tests.h
@@ -70,50 +70,48 @@
 #  endif
 #endif
 
-/****************************************************************************
- * Public Types
- ****************************************************************************/
-
-/****************************************************************************
- * Public Variables
- ****************************************************************************/
-
-/****************************************************************************
- * Public Function Prototypes
- ****************************************************************************/
-
 __BEGIN_DECLS
 
-extern int	test_sensors(int argc, char *argv[]);
-extern int	test_gpio(int argc, char *argv[]);
-extern int	test_hrt(int argc, char *argv[]);
-extern int	test_tone(int argc, char *argv[]);
-extern int	test_led(int argc, char *argv[]);
 extern int	test_adc(int argc, char *argv[]);
-extern int	test_int(int argc, char *argv[]);
-extern int	test_float(int argc, char *argv[]);
-extern int	test_ppm(int argc, char *argv[]);
-extern int	test_servo(int argc, char *argv[]);
-extern int	test_ppm_loopback(int argc, char *argv[]);
-extern int	test_uart_loopback(int argc, char *argv[]);
-extern int	test_uart_baudchange(int argc, char *argv[]);
-extern int	test_cpuload(int argc, char *argv[]);
-extern int	test_uart_send(int argc, char *argv[]);
-extern int	test_sleep(int argc, char *argv[]);
-extern int	test_time(int argc, char *argv[]);
-extern int	test_uart_console(int argc, char *argv[]);
-extern int	test_hott_telemetry(int argc, char *argv[]);
-extern int	test_jig_voltages(int argc, char *argv[]);
-extern int	test_param(int argc, char *argv[]);
+extern int	test_autodeclination(int argc, char *argv[]);
 extern int	test_bson(int argc, char *argv[]);
+extern int	test_conv(int argc, char *argv[]);
 extern int	test_file(int argc, char *argv[]);
 extern int	test_file2(int argc, char *argv[]);
-extern int	test_mixer(int argc, char *argv[]);
-extern int	test_rc(int argc, char *argv[]);
-extern int	test_conv(int argc, char *argv[]);
-extern int	test_mount(int argc, char *argv[]);
+extern int	test_float(int argc, char *argv[]);
+extern int	test_gpio(int argc, char *argv[]);
+extern int	test_hott_telemetry(int argc, char *argv[]);
+extern int	test_hrt(int argc, char *argv[]);
+extern int	test_int(int argc, char *argv[]);
+extern int	test_jig_voltages(int argc, char *argv[]);
+extern int	test_led(int argc, char *argv[]);
 extern int	test_mathlib(int argc, char *argv[]);
-extern int	test_eigen(int argc, char *argv[]);
+extern int	test_matrix(int argc, char *argv[]);
+extern int	test_mixer(int argc, char *argv[]);
+extern int	test_mount(int argc, char *argv[]);
+extern int	test_param(int argc, char *argv[]);
+extern int	test_perf(int argc, char *argv[]);
+extern int	test_ppm(int argc, char *argv[]);
+extern int	test_ppm_loopback(int argc, char *argv[]);
+extern int	test_rc(int argc, char *argv[]);
+extern int	test_sensors(int argc, char *argv[]);
+extern int	test_servo(int argc, char *argv[]);
+extern int	test_sleep(int argc, char *argv[]);
+extern int	test_time(int argc, char *argv[]);
+extern int	test_tone(int argc, char *argv[]);
+extern int	test_uart_baudchange(int argc, char *argv[]);
+extern int	test_uart_console(int argc, char *argv[]);
+extern int	test_uart_loopback(int argc, char *argv[]);
+extern int	test_uart_send(int argc, char *argv[]);
+
+/* external */
+extern int commander_tests_main(int argc, char *argv[]);
+extern int mavlink_tests_main(int argc, char *argv[]);
+extern int controllib_test_main(int argc, char *argv[]);
+extern int uorb_tests_main(int argc, char *argv[]);
+extern int rc_tests_main(int argc, char *argv[]);
+extern int sf0x_tests_main(int argc, char *argv[]);
+
 
 __END_DECLS
 
diff --git a/src/systemcmds/tests/tests_main.c b/src/systemcmds/tests/tests_main.c
index e37a823e13..a733c4eada 100644
--- a/src/systemcmds/tests/tests_main.c
+++ b/src/systemcmds/tests/tests_main.c
@@ -38,20 +38,17 @@
  * @author Lorenz Meier <lm@inf.ethz.ch>
  */
 
+#include "tests.h"
+
 #include <px4_config.h>
 
 #include <sys/types.h>
 
 #include <stdio.h>
-#include <stdlib.h>
 #include <string.h>
-#include <unistd.h>
 #include <fcntl.h>
 #include <errno.h>
 
-#include <arch/board/board.h>
-#include <systemlib/perf_counter.h>
-
 // Not using Eigen at the moment
 #define TESTS_EIGEN_DISABLE
 
@@ -62,8 +59,9 @@
  ****************************************************************************/
 
 static int test_help(int argc, char *argv[]);
+static int test_runner(unsigned option);
+
 static int test_all(int argc, char *argv[]);
-static int test_perf(int argc, char *argv[]);
 static int test_jig(int argc, char *argv[]);
 
 /****************************************************************************
@@ -78,45 +76,52 @@ const struct {
 #define OPT_NOALLTEST	(1<<1)
 #define OPT_NOJIGTEST	(1<<2)
 } tests[] = {
-#ifdef __PX4_NUTTX
-	{"led",			test_led,	0},
-	{"time",		test_time,	OPT_NOJIGTEST},
-	{"sensors",		test_sensors,	0},
-	{"adc",			test_adc,	OPT_NOJIGTEST},
-#endif /* __PX4_NUTTX */
-	{"int",			test_int,	0},
-	{"float",		test_float,	0},
-	{"gpio",		test_gpio,	OPT_NOJIGTEST | OPT_NOALLTEST},
-	{"hrt",			test_hrt,	OPT_NOJIGTEST | OPT_NOALLTEST},
-	{"ppm",			test_ppm,	OPT_NOJIGTEST | OPT_NOALLTEST},
-	{"servo",		test_servo,	OPT_NOJIGTEST | OPT_NOALLTEST},
-	{"ppm_loopback",	test_ppm_loopback,	OPT_NOALLTEST},
-	{"jig_voltages",	test_jig_voltages,	OPT_NOALLTEST},
-	{"uart_loopback",	test_uart_loopback,	OPT_NOJIGTEST | OPT_NOALLTEST},
-	{"uart_baudchange",	test_uart_baudchange,	OPT_NOJIGTEST | OPT_NOALLTEST},
-	{"uart_send",		test_uart_send,	OPT_NOJIGTEST | OPT_NOALLTEST},
-	{"uart_console",	test_uart_console,	OPT_NOJIGTEST | OPT_NOALLTEST},
-	{"hott_telemetry",	test_hott_telemetry,	OPT_NOJIGTEST | OPT_NOALLTEST},
-	{"tone",		test_tone,	0},
-	{"sleep",		test_sleep,	OPT_NOJIGTEST},
-	{"perf",		test_perf,	OPT_NOJIGTEST},
+	{"help",		test_help,	OPT_NOALLTEST | OPT_NOHELP | OPT_NOJIGTEST},
 	{"all",			test_all,	OPT_NOALLTEST | OPT_NOJIGTEST},
 	{"jig",			test_jig,	OPT_NOJIGTEST | OPT_NOALLTEST},
-	{"param",		test_param,	0},
+#ifdef __PX4_NUTTX
+	{"adc",			test_adc,	OPT_NOJIGTEST},
+	{"led",			test_led,	0},
+	{"sensors",		test_sensors,	0},
+	{"time",		test_time,	OPT_NOJIGTEST},
+	{"uart_baudchange",	test_uart_baudchange,	OPT_NOJIGTEST},
+#else
+	{"rc",			rc_tests_main,	0},
+#endif /* __PX4_NUTTX */
+
+	/* external tests */
+	{"commander",		commander_tests_main,	0},
+	{"controllib",		controllib_test_main,	0},
+	//{"mavlink",		mavlink_tests_main,	0}, // TODO: fix mavlink_tests
+	{"sf0x",			sf0x_tests_main,	0},
+	{"uorb",		uorb_tests_main,	0},
+
+	{"autodeclination",		test_autodeclination,	0},
 	{"bson",		test_bson,	0},
+	{"conv",		test_conv, 0},
 	{"file",		test_file,	OPT_NOJIGTEST | OPT_NOALLTEST},
 	{"file2",		test_file2,	OPT_NOJIGTEST},
-	{"mixer",		test_mixer,	OPT_NOJIGTEST | OPT_NOALLTEST},
-	{"rc",			test_rc,	OPT_NOJIGTEST | OPT_NOALLTEST},
-	{"conv",		test_conv,	OPT_NOJIGTEST | OPT_NOALLTEST},
-	{"mount",		test_mount,	OPT_NOJIGTEST | OPT_NOALLTEST},
-#ifndef TESTS_MATHLIB_DISABLE
+	{"float",		test_float,	0},
+	{"gpio",		test_gpio,	OPT_NOJIGTEST | OPT_NOALLTEST},
+	{"hott_telemetry",	test_hott_telemetry,	OPT_NOJIGTEST | OPT_NOALLTEST},
+	{"hrt",			test_hrt,	OPT_NOJIGTEST | OPT_NOALLTEST},
+	{"int",			test_int,	0},
+	{"jig_voltages",	test_jig_voltages,	OPT_NOALLTEST},
 	{"mathlib",		test_mathlib,	0},
-#endif
-#ifndef TESTS_EIGEN_DISABLE
-	{"eigen",		test_eigen,	OPT_NOJIGTEST},
-#endif
-	{"help",		test_help,	OPT_NOALLTEST | OPT_NOHELP | OPT_NOJIGTEST},
+	{"matrix",		test_matrix,	0},
+	{"mixer",		test_mixer,	OPT_NOJIGTEST},
+	{"mount",		test_mount,	OPT_NOJIGTEST | OPT_NOALLTEST},
+	{"param",		test_param,	0},
+	{"perf",		test_perf,	OPT_NOJIGTEST},
+	{"ppm",			test_ppm,	OPT_NOJIGTEST | OPT_NOALLTEST},
+	{"ppm_loopback",	test_ppm_loopback,	OPT_NOALLTEST},
+	{"rc",			test_rc,	OPT_NOJIGTEST | OPT_NOALLTEST},
+	{"servo",		test_servo,	OPT_NOJIGTEST | OPT_NOALLTEST},
+	{"sleep",		test_sleep,	OPT_NOJIGTEST},
+	{"tone",		test_tone,	0},
+	{"uart_console",	test_uart_console,	OPT_NOJIGTEST | OPT_NOALLTEST},
+	{"uart_loopback",	test_uart_loopback,	OPT_NOJIGTEST | OPT_NOALLTEST},
+	{"uart_send",		test_uart_send,	OPT_NOJIGTEST | OPT_NOALLTEST},
 	{NULL,			NULL, 		0}
 };
 
@@ -138,18 +143,30 @@ test_help(int argc, char *argv[])
 
 static int
 test_all(int argc, char *argv[])
+{
+	return test_runner(OPT_NOALLTEST);
+}
+
+static int
+test_jig(int argc, char *argv[])
+{
+	return test_runner(OPT_NOJIGTEST);
+}
+
+static int
+test_runner(unsigned option)
 {
 	unsigned	i;
 	char		*args[2] = {"all", NULL};
 	unsigned int failcount = 0;
 	unsigned int testcount = 0;
-	bool		passed[NTESTS];
+	unsigned		passed[NTESTS];
 
 	printf("\nRunning all tests...\n\n");
 
 	for (i = 0; tests[i].name; i++) {
 		/* Only run tests that are not excluded */
-		if (!(tests[i].options & OPT_NOALLTEST)) {
+		if (!(tests[i].options & option)) {
 			for (int j = 0; j < 80; j++) {
 				printf("-");
 			}
@@ -164,12 +181,12 @@ test_all(int argc, char *argv[])
 				fprintf(stderr, "  [%s] \t\tFAIL\n", tests[i].name);
 				fflush(stderr);
 				failcount++;
-				passed[i] = false;
+				passed[i] = 0;
 
 			} else {
 				printf("  [%s] \t\tPASS\n", tests[i].name);
 				fflush(stdout);
-				passed[i] = true;
+				passed[i] = 1;
 			}
 
 			for (int j = 0; j < 80; j++) {
@@ -185,7 +202,7 @@ test_all(int argc, char *argv[])
 	/* Print summary */
 	printf("\n");
 
-	for (int j = 0; j < 80; j++) {
+	for (unsigned j = 0; j < 80; j++) {
 		printf("#");
 	}
 
@@ -215,12 +232,12 @@ test_all(int argc, char *argv[])
 	printf("\n");
 
 	/* Print failed tests */
-	if (failcount > 0) { printf(" Failed tests:\n\n"); }
+	if (failcount > 0) {
+		printf(" Failed tests:\n\n");
+	}
 
-	unsigned int k;
-
-	for (k = 0; k < i; k++) {
-		if (!passed[k] && !(tests[k].options & OPT_NOALLTEST)) {
+	for (unsigned k = 0; k < i; k++) {
+		if (!passed[k] && !(tests[k].options & option)) {
 			printf(" [%s] to obtain details, please re-run with\n\t nsh> tests %s\n\n", tests[k].name, tests[k].name);
 		}
 	}
@@ -230,129 +247,6 @@ test_all(int argc, char *argv[])
 	return (failcount > 0);
 }
 
-static int
-test_perf(int argc, char *argv[])
-{
-	perf_counter_t	cc, ec;
-
-	cc = perf_alloc(PC_COUNT, "test_count");
-	ec = perf_alloc(PC_ELAPSED, "test_elapsed");
-
-	if ((cc == NULL) || (ec == NULL)) {
-		printf("perf: counter alloc failed\n");
-		return 1;
-	}
-
-	perf_begin(ec);
-	perf_count(cc);
-	perf_count(cc);
-	perf_count(cc);
-	perf_count(cc);
-	printf("perf: expect count of 4\n");
-	perf_print_counter(cc);
-	perf_end(ec);
-	printf("perf: expect count of 1\n");
-	perf_print_counter(ec);
-	printf("perf: expect at least two counters\n");
-	perf_print_all(0);
-
-	perf_free(cc);
-	perf_free(ec);
-
-	return OK;
-}
-
-int test_jig(int argc, char *argv[])
-{
-	unsigned	i;
-	char		*args[2] = {"jig", NULL};
-	unsigned int failcount = 0;
-	unsigned int testcount = 0;
-	bool		passed[NTESTS];
-
-	printf("\nRunning all tests...\n\n");
-
-	for (i = 0; tests[i].name; i++) {
-		/* Only run tests that are not excluded */
-		if (!(tests[i].options & OPT_NOJIGTEST)) {
-			for (int j = 0; j < 80; j++) {
-				printf("-");
-			}
-
-			printf("\n");
-
-			printf("  [%s] \t\tSTARTING TEST\n", tests[i].name);
-			fflush(stdout);
-
-			/* Execute test */
-			if (tests[i].fn(1, args) != 0) {
-				fprintf(stderr, "  [%s] \t\tFAIL\n", tests[i].name);
-				fflush(stderr);
-				failcount++;
-				passed[i] = false;
-
-			} else {
-				printf("  [%s] \t\tPASS\n", tests[i].name);
-				fflush(stdout);
-				passed[i] = true;
-			}
-
-			for (int j = 0; j < 80; j++) {
-				printf("-");
-			}
-
-			printf("\n");
-
-			testcount++;
-		}
-	}
-
-	/* Print summary */
-	printf("\n");
-
-	for (int j = 0; j < 80; j++) {
-		printf("-");
-	}
-
-	printf("\n\n");
-
-	printf("     T E S T    S U M M A R Y\n\n");
-
-	if (failcount == 0) {
-		printf("  ______     __         __            ______     __  __    \n");
-		printf(" /\\  __ \\   /\\ \\       /\\ \\          /\\  __ \\   /\\ \\/ /    \n");
-		printf(" \\ \\  __ \\  \\ \\ \\____  \\ \\ \\____     \\ \\ \\/\\ \\  \\ \\  _\"-.  \n");
-		printf("  \\ \\_\\ \\_\\  \\ \\_____\\  \\ \\_____\\     \\ \\_____\\  \\ \\_\\ \\_\\ \n");
-		printf("   \\/_/\\/_/   \\/_____/   \\/_____/      \\/_____/   \\/_/\\/_/ \n");
-		printf("\n");
-		printf(" All tests passed (%d of %d)\n", testcount, testcount);
-
-	} else {
-		printf("  ______   ______     __     __ \n");
-		printf(" /\\  ___\\ /\\  __ \\   /\\ \\   /\\ \\    \n");
-		printf(" \\ \\  __\\ \\ \\  __ \\  \\ \\ \\  \\ \\ \\__\n");
-		printf("  \\ \\_\\    \\ \\_\\ \\_\\  \\ \\_\\  \\ \\_____\\ \n");
-		printf("   \\/_/     \\/_/\\/_/   \\/_/   \\/_____/ \n");
-		printf("\n");
-		printf(" Some tests failed (%d of %d)\n", failcount, testcount);
-	}
-
-	printf("\n");
-
-	/* Print failed tests */
-	if (failcount > 0) { printf(" Failed tests:\n\n"); }
-
-	for (int k = 0; k < i; k++) {
-		if (!passed[i] && !(tests[k].options & OPT_NOJIGTEST)) {
-			printf(" [%s] to obtain details, please re-run with\n\t nsh> tests %s\n\n", tests[k].name, tests[k].name);
-		}
-	}
-
-	fflush(stdout);
-
-	return 0;
-}
-
 __EXPORT int tests_main(int argc, char *argv[]);
 
 /**
@@ -360,19 +254,17 @@ __EXPORT int tests_main(int argc, char *argv[]);
  */
 int tests_main(int argc, char *argv[])
 {
-	unsigned	i;
-
 	if (argc < 2) {
 		printf("tests: missing test name - 'tests help' for a list of tests\n");
 		return 1;
 	}
 
-	for (i = 0; tests[i].name; i++) {
+	for (unsigned i = 0; tests[i].name; i++) {
 		if (!strcmp(tests[i].name, argv[1])) {
 			return tests[i].fn(argc - 1, argv + 1);
 		}
 	}
 
 	printf("tests: no test called '%s' - 'tests help' for a list of tests\n", argv[1]);
-	return ERROR;
+	return 1;
 }
diff --git a/unittests/CMakeLists.txt b/unittests/CMakeLists.txt
index c2e11ea37a..9df3adcce3 100644
--- a/unittests/CMakeLists.txt
+++ b/unittests/CMakeLists.txt
@@ -1,9 +1,5 @@
 cmake_minimum_required(VERSION 2.8)
 
-include(CMakeForceCompiler)
-#CMAKE_FORCE_C_COMPILER(clang Clang)
-#CMAKE_FORCE_CXX_COMPILER(clang++ Clang)
-
 if("${CMAKE_C_COMPILER_ID}" STREQUAL "Clang")
 	add_compile_options(-Qunused-arguments )
 endif()
@@ -14,28 +10,16 @@ endif()
 project(unittests)
 enable_testing()
 
-include(CheckCXXCompilerFlag)
-CHECK_CXX_COMPILER_FLAG("-std=c++11" COMPILER_SUPPORTS_CXX11)
-CHECK_CXX_COMPILER_FLAG("-std=c++0x" COMPILER_SUPPORTS_CXX0X)
+set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -Wall -Werror -std=gnu99 -g")
+set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall -Werror -std=c++11 -g -fno-exceptions -fno-rtti -fno-threadsafe-statics")
 
-if(COMPILER_SUPPORTS_CXX11)
-	set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11 -fno-exceptions -fno-rtti -fno-threadsafe-statics -D__CUSTOM_FILE_IO__ -D__PX4_UNIT_TESTS -g -Wall -Werror")
-elseif(COMPILER_SUPPORTS_CXX0X)
-	set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++0x -fno-exceptions -fno-rtti -fno-threadsafe-statics -D__CUSTOM_FILE_IO__ -D__PX4_UNIT_TESTS -g -Wall -Werror")
-else()
-	message(STATUS "The compiler ${CMAKE_CXX_COMPILER} has no C++11 support. Please use a different C++ compiler.")
-endif()
-set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -std=gnu99 -g")
-
-set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -g3 -fsanitize=address -fno-omit-frame-pointer")
-set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wno-sign-compare -Wno-unused-but-set-variable")
 
 set(GTEST_DIR ${CMAKE_SOURCE_DIR}/googletest)
 add_subdirectory(${GTEST_DIR})
 include_directories(${GTEST_DIR}/include)
 
 set(PX4_SRC ${CMAKE_SOURCE_DIR}/../src)
-set(PX4_SITL_BUILD ${PX4_SRC}/../build_posix_sitl_test)
+set(PX4_SITL_BUILD ${PX4_SRC}/../build_posix_sitl_default)
 
 include_directories(${CMAKE_SOURCE_DIR})
 include_directories(${PX4_SITL_BUILD}/src)
@@ -52,38 +36,27 @@ include_directories(${PX4_SRC}/modules/uORB)
 include_directories(${PX4_SRC}/platforms)
 include_directories(${PX4_SRC}/platforms/posix/include)
 include_directories(${PX4_SRC}/platforms/posix/px4_layer)
+include_directories(${PX4_SRC}/platforms/posix/work_queue)
 
+add_definitions(-D__CUSTOM_FILE_IO__)
 add_definitions(-D__EXPORT=)
 add_definitions(-D__PX4_POSIX)
 add_definitions(-D__PX4_TESTS)
+add_definitions(-D__PX4_UNIT_TESTS)
 add_definitions(-D_UNIT_TEST=)
 add_definitions(-DERROR=-1)
 add_definitions(-Dmain_t=int)
 add_definitions(-Dnoreturn_function=)
 add_definitions(-DOK=0)
 
-# check
-add_custom_target(check
-		COMMAND ${CMAKE_CTEST_COMMAND} -j2 --output-on-failure
-		WORKING_DIR ${CMAKE_BINARY_DIR}
-		USES_TERMINAL)
-
-function(add_gtest)
-	foreach(test_name ${ARGN})
-		if(${CMAKE_SYSTEM_NAME} MATCHES "Darwin")
-			target_link_libraries(${test_name} gtest_main pthread px4_platform)
-			add_definitions(-D__PX4_DARWIN)
-		else()
-			target_link_libraries(${test_name} gtest_main pthread rt px4_platform)
-			add_definitions(-D__PX4_LINUX)
-		endif()
-		add_test(NAME ${test_name} COMMAND ${test_name} WORKING_DIRECTORY ${CMAKE_SOURCE_DIR})
-		add_dependencies(check ${test_name})
-	endforeach()
-endfunction()
+if(${CMAKE_SYSTEM_NAME} MATCHES "Darwin")
+	add_definitions(-D__PX4_DARWIN)
+else()
+	add_definitions(-D__PX4_LINUX)
+endif()
 
 add_library(px4_platform 
-			${PX4_SITL_BUILD}/src/modules/param/px4_parameters.c                         
+			${PX4_SITL_BUILD}/src/modules/param/px4_parameters.c
 			${PX4_SRC}/drivers/device/device_posix.cpp 
 			${PX4_SRC}/drivers/device/i2c_posix.cpp 
 			${PX4_SRC}/drivers/device/ringbuffer.cpp
@@ -112,8 +85,30 @@ add_library(px4_platform
 			${PX4_SRC}/platforms/posix/work_queue/work_lock.c
 			${PX4_SRC}/platforms/posix/work_queue/work_queue.c
 			${PX4_SRC}/platforms/posix/work_queue/work_thread.c
-			)                           
-target_include_directories(px4_platform PUBLIC ${PX4_SRC}/platforms)                           
+			)
+target_include_directories(px4_platform PUBLIC ${PX4_SRC}/platforms)
+target_include_directories(px4_platform PUBLIC ${PX4_SRC}/platforms/posix/include)
+target_include_directories(px4_platform PUBLIC ${PX4_SRC}/platforms/posix/work_queue)
+
+
+# check
+add_custom_target(check
+		COMMAND ${CMAKE_CTEST_COMMAND} -j2 --output-on-failure
+		WORKING_DIR ${CMAKE_BINARY_DIR}
+		USES_TERMINAL)
+
+# add_gtest
+function(add_gtest)
+	foreach(test_name ${ARGN})
+		if(${CMAKE_SYSTEM_NAME} MATCHES "Darwin")
+			target_link_libraries(${test_name} gtest_main pthread px4_platform)
+		else()
+			target_link_libraries(${test_name} gtest_main pthread rt px4_platform)
+		endif()
+		add_test(NAME ${test_name} COMMAND ${test_name} WORKING_DIRECTORY ${CMAKE_SOURCE_DIR})
+		add_dependencies(check ${test_name})
+	endforeach()
+endfunction()
 
 
 #######################################################################
@@ -122,59 +117,9 @@ target_include_directories(px4_platform PUBLIC ${PX4_SRC}/platforms)
 #    add_executable(example_test example_test.cpp)
 #    add_gtest(example_test)
 
-
-# autodeclination_test
-add_executable(autodeclination_test autodeclination_test.cpp ${PX4_SRC}/lib/geo_lookup/geo_mag_declination.c)
-add_gtest(autodeclination_test)
-
-# mixer_test
-add_custom_command(OUTPUT ${PX4_SRC}/modules/systemlib/mixer/mixer_multirotor.generated.h
-                   COMMAND ${PX4_SRC}/modules/systemlib/mixer/multi_tables.py > ${PX4_SRC}/modules/systemlib/mixer/mixer_multirotor.generated.h)
-add_executable(mixer_test mixer_test.cpp
-                          ${PX4_SRC}/modules/systemlib/mixer/mixer.cpp
-                          ${PX4_SRC}/modules/systemlib/mixer/mixer_group.cpp
-                          ${PX4_SRC}/modules/systemlib/mixer/mixer_load.c
-                          ${PX4_SRC}/modules/systemlib/mixer/mixer_multirotor.cpp
-                          ${PX4_SRC}/modules/systemlib/mixer/mixer_multirotor.generated.h
-                          ${PX4_SRC}/modules/systemlib/mixer/mixer_simple.cpp
-                          ${PX4_SRC}/modules/systemlib/pwm_limit/pwm_limit.c
-                          ${PX4_SRC}/systemcmds/tests/test_mixer.cpp)
-add_gtest(mixer_test)
-
-# conversion_test
-add_executable(conversion_test conversion_test.cpp ${PX4_SRC}/systemcmds/tests/test_conv.cpp)
-add_gtest(conversion_test)
-
-# sbus2_test
-add_executable(sbus2_test sbus2_test.cpp
-						${PX4_SRC}/lib/rc/sbus.c)
-add_gtest(sbus2_test)
-
-# DSM test
-add_executable(dsm_test dsm_test.cpp
-						${PX4_SRC}/lib/rc/dsm.c)
-add_gtest(dsm_test)
-
-# st24_test
-add_executable(rc_input_test st24_test.cpp sumd_test.cpp
-						${PX4_SRC}/lib/rc/st24.c
-						${PX4_SRC}/lib/rc/sumd.c)
-add_gtest(rc_input_test)
-
-# sf0x_test
-add_executable(sf0x_test sf0x_test.cpp
-						${PX4_SRC}/drivers/sf0x/sf0x_parser.cpp)
-add_gtest(sf0x_test)
-
 # param_test
 add_executable(param_test param_test.cpp uorb_stub.cpp
 						${PX4_SRC}/modules/systemlib/bson/tinybson.c
 						${PX4_SRC}/modules/systemlib/param/param.c)
 target_link_libraries(param_test ${PX4_SITL_BUILD}/libmsg_gen.a)
 add_gtest(param_test)
-
-# param_shmem_test
-#add_executable(param_shmem_test param_test.cpp uorb_stub.cpp
-#						${PX4_SRC}/modules/systemlib/bson/tinybson.c
-#						${PX4_SRC}/modules/systemlib/param/param_shmem.c)
-#add_gtest(param_shmem_test)
diff --git a/unittests/autodeclination_test.cpp b/unittests/autodeclination_test.cpp
deleted file mode 100644
index 8c89243f6e..0000000000
--- a/unittests/autodeclination_test.cpp
+++ /dev/null
@@ -1,18 +0,0 @@
-#include <math.h>
-#include <stdio.h>
-#include <stdlib.h>
-#include <string.h>
-#include <unistd.h>
-
-#include <drivers/drv_hrt.h>
-#include <geo/geo.h>
-#include <px4iofirmware/px4io.h>
-#include <systemlib/err.h>
-#include <systemlib/mixer/mixer.h>
-
-#include "gtest/gtest.h"
-
-TEST(AutoDeclinationTest, AutoDeclination)
-{
-	ASSERT_NEAR(get_mag_declination(47.0, 8.0), 0.6, 0.5) << "declination differs more than 1 degree";
-}
diff --git a/unittests/conversion_test.cpp b/unittests/conversion_test.cpp
deleted file mode 100644
index 7a2909dc34..0000000000
--- a/unittests/conversion_test.cpp
+++ /dev/null
@@ -1,10 +0,0 @@
-#include <systemlib/mixer/mixer.h>
-#include <systemlib/err.h>
-#include "../../src/systemcmds/tests/tests.h"
-
-#include "gtest/gtest.h"
-
-TEST(ConversionTest, quad_w_main)
-{
-	ASSERT_EQ(test_conv(0, NULL), 0) << "Conversion test failed";
-}
diff --git a/unittests/data/fit_linear_voltage.m b/unittests/data/fit_linear_voltage.m
deleted file mode 100644
index 7d0c2c27fc..0000000000
--- a/unittests/data/fit_linear_voltage.m
+++ /dev/null
@@ -1,14 +0,0 @@
-close all;
-clear all;
-M = importdata('px4io_v1.3.csv');
-voltage = M.data(:, 1);
-counts = M.data(:, 2);
-plot(counts, voltage, 'b*-', 'LineWidth', 2, 'MarkerSize', 15);
-coeffs = polyfit(counts, voltage, 1);
-fittedC = linspace(min(counts), max(counts), 500);
-fittedV = polyval(coeffs, fittedC);
-hold on
-plot(fittedC, fittedV, 'r-', 'LineWidth', 3);
-
-slope = coeffs(1)
-y_intersection = coeffs(2)
diff --git a/unittests/data/px4io_v1.3.csv b/unittests/data/px4io_v1.3.csv
deleted file mode 100644
index b41ee8f1f9..0000000000
--- a/unittests/data/px4io_v1.3.csv
+++ /dev/null
@@ -1,70 +0,0 @@
-voltage, counts
-4.3, 950
-4.4, 964
-4.5, 986
-4.6, 1009
-4.7, 1032
-4.8, 1055
-4.9, 1078
-5.0, 1101
-5.2, 1124
-5.3, 1148
-5.4, 1171
-5.5, 1195
-6.0, 1304
-6.1, 1329
-6.2, 1352
-7.0, 1517
-7.1, 1540
-7.2, 1564
-7.3, 1585
-7.4, 1610
-7.5, 1636
-8.0, 1728
-8.1, 1752
-8.2, 1755
-8.3, 1798
-8.4, 1821
-9.0, 1963
-9.1, 1987
-9.3, 2010
-9.4, 2033
-10.0, 2174
-10.1, 2198
-10.2, 2221
-10.3, 2245
-10.4, 2268
-11.0, 2385
-11.1, 2409
-11.2, 2432
-11.3, 2456
-11.4, 2480
-11.5, 2502
-11.6, 2526
-11.7, 2550
-11.8, 2573
-11.9, 2597
-12.0, 2621
-12.1, 2644
-12.3, 2668
-12.4, 2692
-12.5, 2716
-12.6, 2737
-12.7, 2761
-13.0, 2832
-13.5, 2950
-13.6, 2973
-14.1, 3068
-14.2, 3091
-14.7, 3209
-15.0, 3280
-15.1, 3304
-15.5, 3374
-15.6, 3397
-15.7, 3420
-16.0, 3492
-16.1, 3514
-16.2, 3538
-16.9, 3680
-17.0, 3704
-17.1, 3728
diff --git a/unittests/dsm_test.cpp b/unittests/dsm_test.cpp
deleted file mode 100644
index 7bd6558139..0000000000
--- a/unittests/dsm_test.cpp
+++ /dev/null
@@ -1,74 +0,0 @@
-#include <stdio.h>
-#include <string.h>
-#include <unistd.h>
-
-#include "../../src/systemcmds/tests/tests.h"
-#include <drivers/drv_hrt.h>
-// Enable DSM parser output
-#define DSM_DEBUG
-#include <rc/dsm.h>
-#include <systemlib/err.h>
-#include <systemlib/mixer/mixer.h>
-
-#include "gtest/gtest.h"
-
-TEST(DSMTest, DSM)
-{
-	const char *filepath = "testdata/dsm_x_data.txt";
-
-	FILE *fp;
-	fp = fopen(filepath, "rt");
-
-	ASSERT_TRUE(fp);
-	warnx("loading data from: %s", filepath);
-
-	float f;
-	unsigned x;
-	int ret;
-
-	// Trash the first 20 lines
-	for (unsigned i = 0; i < 20; i++) {
-		char buf[200];
-		(void)fgets(buf, sizeof(buf), fp);
-	}
-
-	// Init the parser
-	uint8_t frame[20];
-	uint16_t rc_values[18];
-	uint16_t num_values;
-	bool dsm_11_bit;
-	unsigned dsm_frame_drops = 0;
-	uint16_t max_channels = sizeof(rc_values) / sizeof(rc_values[0]);
-
-	int rate_limiter = 0;
-	unsigned last_drop = 0;
-
-	while (EOF != (ret = fscanf(fp, "%f,%x,,", &f, &x))) {
-
-		ASSERT_GT(ret, 0);
-
-		frame[0] = x;
-		unsigned len = 1;
-
-		// Pipe the data into the parser
-		bool result = dsm_parse(f*1e6, &frame[0], len, rc_values, &num_values,
-						&dsm_11_bit, &dsm_frame_drops, max_channels);
-
-		if (result) {
-			warnx("decoded packet with %d channels and %s encoding:", num_values, (dsm_11_bit) ? "11 bit" : "10 bit");
-
-			for (unsigned i = 0; i < num_values; i++) {
-				printf("chan #%u:\t%d\n", i, (int)rc_values[i]);
-			}
-		}
-
-		if (last_drop != (dsm_frame_drops)) {
-			warnx("frame dropped, now #%d", (dsm_frame_drops));
-			last_drop = dsm_frame_drops;
-		}
-
-		rate_limiter++;
-	}
-
-	ASSERT_EQ(ret, EOF);
-}
diff --git a/unittests/mixer_test.cpp b/unittests/mixer_test.cpp
deleted file mode 100644
index 73df8c54f2..0000000000
--- a/unittests/mixer_test.cpp
+++ /dev/null
@@ -1,12 +0,0 @@
-#include <systemlib/mixer/mixer.h>
-#include <systemlib/err.h>
-#include "../../src/systemcmds/tests/tests.h"
-
-#include "gtest/gtest.h"
-
-
-TEST(MixerTest, Mixer)
-{
-	const char *args[] = {"empty", "../ROMFS/px4fmu_test/mixers/IO_pass.mix", "../ROMFS/px4fmu_test/mixers/quad_test.mix"};
-	ASSERT_EQ(test_mixer(3, (char **)args), 0) << "IO_pass.mix failed";
-}
diff --git a/unittests/sbus2_test.cpp b/unittests/sbus2_test.cpp
deleted file mode 100644
index 828f2a6160..0000000000
--- a/unittests/sbus2_test.cpp
+++ /dev/null
@@ -1,89 +0,0 @@
-#include <stdio.h>
-#include <string.h>
-#include <unistd.h>
-
-#include "../../src/systemcmds/tests/tests.h"
-#include <drivers/drv_hrt.h>
-// Enable S.BUS parser output
-#define SBUS_DEBUG
-#include <rc/sbus.h>
-#include <systemlib/err.h>
-#include <systemlib/mixer/mixer.h>
-
-#include "gtest/gtest.h"
-
-TEST(SBUS2Test, SBUS2)
-{
-	const char *filepath = "testdata/sbus2_r7008SB.txt";
-
-	FILE *fp;
-	fp = fopen(filepath, "rt");
-
-	ASSERT_TRUE(fp);
-	warnx("loading data from: %s", filepath);
-
-	// if (argc < 2)
-	// 	errx(1, "Need a filename for the input file");
-
-	int byte_offset = 7;
-
-	// if (argc > 2) {
-	// 	char* end;
-	// 	byte_offset = strtol(argv[2],&end,10);
-	// }
-
-	warnx("RUNNING TEST WITH BYTE OFFSET OF: %d", byte_offset);
-
-	float f;
-	unsigned x;
-	int ret;
-
-	// Trash the first 20 lines
-	for (unsigned i = 0; i < 20; i++) {
-		char buf[200];
-		(void)fgets(buf, sizeof(buf), fp);
-	}
-
-	// Init the parser
-	uint8_t frame[SBUS_BUFFER_SIZE];
-	uint16_t rc_values[18];
-	uint16_t num_values;
-	unsigned sbus_frame_drops = 0;
-	unsigned sbus_frame_resets = 0;
-	bool sbus_failsafe;
-	bool sbus_frame_drop;
-	uint16_t max_channels = sizeof(rc_values) / sizeof(rc_values[0]);
-
-	int rate_limiter = 0;
-	unsigned last_drop = 0;
-
-	while (EOF != (ret = fscanf(fp, "%f,%x,,", &f, &x))) {
-
-		ASSERT_GT(ret, 0);
-
-		frame[0] = x;
-		unsigned len = 1;
-
-		// Pipe the data into the parser
-		hrt_abstime now = hrt_absolute_time();
-
-		// if (rate_limiter % byte_offset == 0) {
-		bool result = sbus_parse(now, &frame[0], len, rc_values, &num_values,
-					 &sbus_failsafe, &sbus_frame_drop, &sbus_frame_drops, max_channels);
-
-		if (result) {
-			//warnx("decoded packet");
-		}
-
-		// }
-
-		if (last_drop != (sbus_frame_drops + sbus_frame_resets)) {
-			warnx("frame dropped, now #%d", (sbus_frame_drops + sbus_frame_resets));
-			last_drop = sbus_frame_drops + sbus_frame_resets;
-		}
-
-		rate_limiter++;
-	}
-
-	ASSERT_EQ(ret, EOF);
-}
diff --git a/unittests/st24_test.cpp b/unittests/st24_test.cpp
deleted file mode 100644
index 5b6a22fd27..0000000000
--- a/unittests/st24_test.cpp
+++ /dev/null
@@ -1,63 +0,0 @@
-#include <stdio.h>
-#include <unistd.h>
-#include <string.h>
-#include <systemlib/err.h>
-#include <drivers/drv_hrt.h>
-#include <rc/st24.h>
-#include "../../src/systemcmds/tests/tests.h"
-
-#include "gtest/gtest.h"
-
-TEST(ST24Test, ST24)
-{
-	const char *filepath = "testdata/st24_data.txt";
-
-	//warnx("loading data from: %s", filepath);
-
-	FILE *fp;
-
-	fp = fopen(filepath, "rt");
-	ASSERT_TRUE(fp);
-
-	float f;
-	unsigned x;
-	int ret;
-
-	// Trash the first 20 lines
-	for (unsigned i = 0; i < 20; i++) {
-		char buf[200];
-		(void)fgets(buf, sizeof(buf), fp);
-	}
-
-	float last_time = 0;
-
-	while (EOF != (ret = fscanf(fp, "%f,%x,,", &f, &x))) {
-		if (((f - last_time) * 1000 * 1000) > 3000) {
-			// warnx("FRAME RESET\n\n");
-		}
-
-		uint8_t b = static_cast<uint8_t>(x);
-
-		last_time = f;
-
-		// Pipe the data into the parser
-		//hrt_abstime now = hrt_absolute_time();
-
-		uint8_t rssi;
-		uint8_t rx_count;
-		uint16_t channel_count;
-		uint16_t channels[20];
-
-
-		if (!st24_decode(b, &rssi, &rx_count, &channel_count, channels, sizeof(channels) / sizeof(channels[0]))) {
-			//warnx("decoded: %u channels (converted to PPM range)", (unsigned)channel_count);
-
-			for (unsigned i = 0; i < channel_count; i++) {
-				//int16_t val = channels[i];
-				//warnx("channel %u: %d 0x%03X", i, static_cast<int>(val), static_cast<int>(val));
-			}
-		}
-	}
-
-	ASSERT_EQ(EOF, ret);
-}
diff --git a/unittests/sumd_test.cpp b/unittests/sumd_test.cpp
deleted file mode 100644
index 40be7a9d8f..0000000000
--- a/unittests/sumd_test.cpp
+++ /dev/null
@@ -1,63 +0,0 @@
-#include <stdio.h>
-#include <unistd.h>
-#include <string.h>
-#include <systemlib/err.h>
-#include <drivers/drv_hrt.h>
-#include <rc/sumd.h>
-#include "../../src/systemcmds/tests/tests.h"
-
-#include "gtest/gtest.h"
-
-TEST(SUMDTest, SUMD)
-{
-	const char *filepath = "testdata/sumd_data.txt";
-
-	//warnx("loading data from: %s", filepath);
-
-	FILE *fp;
-
-	fp = fopen(filepath, "rt");
-	//ASSERT_TRUE(fp);
-
-	float f;
-	unsigned x;
-	int ret;
-
-	// Trash the first 20 lines
-	for (unsigned i = 0; i < 20; i++) {
-		char buf[200];
-		(void)fgets(buf, sizeof(buf), fp);
-	}
-
-	float last_time = 0;
-
-	while (EOF != (ret = fscanf(fp, "%f,%x,,", &f, &x))) {
-		if (((f - last_time) * 1000 * 1000) > 3000) {
-			// warnx("FRAME RESET\n\n");
-		}
-
-		uint8_t b = static_cast<uint8_t>(x);
-
-		last_time = f;
-
-		// Pipe the data into the parser
-		//hrt_abstime now = hrt_absolute_time();
-
-		uint8_t rssi;
-		uint8_t rx_count;
-		uint16_t channel_count;
-		uint16_t channels[32];
-
-
-		if (!sumd_decode(b, &rssi, &rx_count, &channel_count, channels, 32)) {
-			//warnx("decoded: %u channels (converted to PPM range)", (unsigned)channel_count);
-
-			for (unsigned i = 0; i < channel_count; i++) {
-				//int16_t val = channels[i];
-				//warnx("channel %u: %d 0x%03X", i, static_cast<int>(val), static_cast<int>(val));
-			}
-		}
-	}
-
-	ASSERT_EQ(EOF, ret);
-}
diff --git a/unittests/uorb_stub.cpp b/unittests/uorb_stub.cpp
index e4269afc32..cc1926d69c 100644
--- a/unittests/uorb_stub.cpp
+++ b/unittests/uorb_stub.cpp
@@ -19,5 +19,3 @@ int	orb_publish(const struct orb_metadata *meta, orb_advert_t handle, const void
 {
 	return 0;
 }
-
-
diff --git a/unittests/uorb_unittests/uORBCommunicatorMock.cpp b/unittests/uorb_unittests/uORBCommunicatorMock.cpp
deleted file mode 100644
index f964c095e6..0000000000
--- a/unittests/uorb_unittests/uORBCommunicatorMock.cpp
+++ /dev/null
@@ -1,211 +0,0 @@
-/****************************************************************************
- *
- * Copyright (c) 2015 Mark Charlebois. All rights reserved.
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions
- * are met:
- *
- * 1. Redistributions of source code must retain the above copyright
- *    notice, this list of conditions and the following disclaimer.
- * 2. Redistributions in binary form must reproduce the above copyright
- *    notice, this list of conditions and the following disclaimer in
- *    the documentation and/or other materials provided with the
- *    distribution.
- * 3. Neither the name PX4 nor the names of its contributors may be
- *    used to endorse or promote products derived from this software
- *    without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
- * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
- * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- *
- ****************************************************************************/
-
-#include "uORBCommunicatorMock.hpp"
-#include "uORB/uORB.h"
-#include "uORBGtestTopics.hpp"
-#include "px4_log.h"
-#include <string.h>
-#include "uORBManager.hpp"
-
-#define LOG_TAG "uORBCommunicatorMock.cpp"
-
-uORB_test::uORBCommunicatorMock::uORBCommunicatorMock()
-: _rx_handler( nullptr )
-{
-/*
-  _sub_topicA_copy_fd = orb_subscribe( ORB_ID( topicA_copy ), (void*)&_sub_semaphore );
-  _sub_topicB_copy_fd = orb_subscribe( ORB_ID( topicB_copy), nullptr );
-*/
-  _topic_translation_map[ "topicA" ] = "topicA_copy";
-  _topic_translation_map[ "topicB" ] = "topicB_copy";
-  _topic_translation_map[ "topicA_copy" ] = "topicA";
-  _topic_translation_map[ "topicB_copy" ] = "topicB";
-}
-
-int16_t uORB_test::uORBCommunicatorMock::add_subscription
-(
-    const char * messageName,
-    int32_t msgRateInHz
-)
-{
-
-  int16_t rc = 0;
-  PX4_INFO( "got add_subscription for msg[%s] rate[%d]", messageName, msgRateInHz );
-  _msgCounters[messageName]._add_subscriptionCount++;
-
-  /*
-  int16_t rc = -1;
-  if( _rx_handler )
-  {
-    if( _topic_translation_map.find( messageName ) != _topic_translation_map.end() )
-    {
-       rc = _rx_handler->process_add_subscription
-           (
-              _topic_translation_map[messageName],
-              msgRateInHz
-           );
-    }
-  }
-  */
-  return rc;
-}
-
-int16_t uORB_test::uORBCommunicatorMock::remove_subscription
-(
-    const char * messageName
-)
-{
-  int16_t rc = 0;
-  PX4_INFO( "got remove_subscription for msg[%s]", messageName );
-  _msgCounters[messageName]._remove_subscriptionCount++;
-/*
-  int16_t rc = -1;
-  if( _rx_handler )
-  {
-    if( _topic_translation_map.find( messageName ) != _topic_translation_map.end() )
-    {
-       rc = _rx_handler->process_remove_subscription
-           (
-              _topic_translation_map[messageName]
-           );
-    }
-  }
-*/
-  return rc;
-}
-
-int16_t uORB_test::uORBCommunicatorMock::register_handler
-(
-    uORBCommunicator::IChannelRxHandler* handler
-)
-{
-  int16_t rc = 0;
-  _rx_handler = handler;
-  return rc;
-}
-
-
-int16_t uORB_test::uORBCommunicatorMock::send_message
-(
-    const char * messageName,
-    int32_t length,
-    uint8_t* data
-)
-{
-  int16_t rc = 0;
-  PX4_INFO( "send_message for msg[%s] datalen[%d]", messageName, length );
-  if( uORB::Manager::get_instance()->is_remote_subscriber_present( messageName ) )
-  {
-    _msgCounters[messageName]._send_messageCount++;
-    if( strcmp(messageName, "topicA") == 0 )
-    {
-      memcpy( &_topicAData, (void*)data, length );
-    }
-    else if( strcmp(messageName, "topicB") == 0 )
-    {
-      memcpy( &_topicBData, (void*)data, length );
-    }
-    else
-    {
-      //EPRINTF( "error messageName[%s] is not supported", messageName );
-    }
-  }
-/*
-  int16_t rc = -1;
-  if( _rx_handler )
-  {
-    if( _topic_translation_map.find( messageName ) != _topic_translation_map.end() )
-    {
-      rc = _rx_handler->process_received_message
-          ( _topic_translation_map[messageName], length, data );
-    }
-  }
-*/
-  return rc;
-}
-
-
-bool uORB_test::uORBCommunicatorMock::get_remote_topicA_data
-(
-    struct orb_topic_A* data
-)
-{
-  bool rc = false;
-  memcpy( data, &_topicAData, sizeof(_topicAData) );
-  rc = true;
-/*
-  if( orb_copy(ORB_ID(topicA_copy), _sub_topicA_copy_fd, data) == OK )
-  {
-    rc = true;
-  }
-*/
-  return rc;
-}
-
-bool uORB_test::uORBCommunicatorMock::get_remote_topicB_data
-(
-    struct orb_topic_B* data
-)
-{
-  bool rc = false;
-  memcpy( data, &_topicBData, sizeof(_topicBData) );
-  rc = true;
-/*
-
-  if( orb_copy(ORB_ID(topicB_copy), _sub_topicB_copy_fd, data) == OK )
-  {
-    rc = true;
-  }
-*/
-  return rc;
-}
-
-void uORB_test::uORBCommunicatorMock::reset_counters()
-{
-  InterfaceCounters resetCounter;
-  resetCounter._add_subscriptionCount = 0;
-  resetCounter._remove_subscriptionCount = 0;
-  resetCounter._send_messageCount = 0;
-
-  std::map<std::string, InterfaceCounters>::iterator it;
-  for( it = _msgCounters.begin(); it != _msgCounters.end(); ++it )
-  {
-    it->second = resetCounter;
-  }
-}
-
-uORB_test::uORBCommunicatorMock::InterfaceCounters uORB_test::uORBCommunicatorMock::get_interface_counters(const char * messageName )
-{
-  return _msgCounters[ messageName ];
-}
diff --git a/unittests/uorb_unittests/uORBCommunicatorMock.hpp b/unittests/uorb_unittests/uORBCommunicatorMock.hpp
deleted file mode 100644
index acffbdb7ab..0000000000
--- a/unittests/uorb_unittests/uORBCommunicatorMock.hpp
+++ /dev/null
@@ -1,140 +0,0 @@
-
-/****************************************************************************
- *
- * Copyright (c) 2015 Mark Charlebois. All rights reserved.
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions
- * are met:
- *
- * 1. Redistributions of source code must retain the above copyright
- *    notice, this list of conditions and the following disclaimer.
- * 2. Redistributions in binary form must reproduce the above copyright
- *    notice, this list of conditions and the following disclaimer in
- *    the documentation and/or other materials provided with the
- *    distribution.
- * 3. Neither the name PX4 nor the names of its contributors may be
- *    used to endorse or promote products derived from this software
- *    without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
- * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
- * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- *
- ****************************************************************************/
-#ifndef _uORBCommunicatorMock_hpp_
-#define _uORBCommunicatorMock_hpp_
-
-#include "uORB/uORBCommunicator.hpp"
-#include "uORBGtestTopics.hpp"
-#include <map>
-#include <string>
-#include <set>
-
-namespace uORB_test
-{
-   class uORBCommunicatorMock;
-}
-
-class uORB_test::uORBCommunicatorMock : public uORBCommunicator::IChannel
-{
- public:
-
-  //counters to track how many times the iterface is called from
-  // uorb.
-  typedef struct
-  {
-    int64_t _add_subscriptionCount;
-    int64_t _remove_subscriptionCount;
-    int64_t _send_messageCount;
-  }InterfaceCounters;
-
-  uORBCommunicatorMock();
-
-  /**
-   * @brief Interface to notify the remote entity of interest of a
-   * subscription for a message.
-   *
-   * @param messageName
-   *  This represents the uORB message name; This message name should be
-   *  globally unique.
-   * @param msgRate
-   *  The max rate at which the subscriber can accept the messages.
-   * @return
-   *  0 = success; This means the messages is successfully sent to the receiver
-   *    Note: This does not mean that the receiver as received it.
-   *  otherwise = failure.
-   */
-  virtual int16_t add_subscription( const char *messageName, int32_t msgRateInHz );
-
-  /**
-   * @brief Interface to notify the remote entity of removal of a subscription
-   *
-   * @param messageName
-   *  This represents the uORB message name; This message name should be
-   *  globally unique.
-   * @return
-   *  0 = success; This means the messages is successfully sent to the receiver
-   *    Note: This does not necessarily mean that the receiver as received it.
-   *  otherwise = failure.
-   */
-  virtual int16_t remove_subscription( const char * messageName );
-
-  /**
-   * Register Message Handler.  This is internal for the IChannel implementer*
-   */
-  virtual int16_t register_handler( uORBCommunicator::IChannelRxHandler* handler );
-
-
-  /**
-   * @brief Sends the data message over the communication link.
-   * @param messageName
-   *  This represents the uORB message name; This message name should be
-   *  globally unique.
-   * @param length
-   *  The length of the data buffer to be sent.
-   * @param data
-   *  The actual data to be sent.
-   * @return
-   *  0 = success; This means the messages is successfully sent to the receiver
-   *    Note: This does not mean that the receiver as received it.
-   *  otherwise = failure.
-   */
-  virtual int16_t send_message( const char * messageName, int32_t length, uint8_t* data);
-
-  uORBCommunicator::IChannelRxHandler* get_rx_handler()
-  {
-    return _rx_handler;
-  }
-
-  bool get_remote_topicA_data( struct orb_topic_A* data );
-  bool get_remote_topicB_data( struct orb_topic_B* data );
-
-  void reset_counters();
-
-  InterfaceCounters get_interface_counters( const char * messageName );
-
-
- private:
-  uORBCommunicator::IChannelRxHandler* _rx_handler;
-  //int _sub_topicA_copy_fd;
-  //int _sub_topicB_copy_fd;
-
-  std::map<std::string, std::string> _topic_translation_map;
-
-  struct orb_topic_A _topicAData;
-  struct orb_topic_B _topicBData;
-
-  std::map<std::string, InterfaceCounters> _msgCounters;
-};
-
-#endif /* _uORBCommunicatorMock_test_hpp_ */
diff --git a/unittests/uorb_unittests/uORBCommunicatorMockLoopback.cpp b/unittests/uorb_unittests/uORBCommunicatorMockLoopback.cpp
deleted file mode 100644
index 8bb4260590..0000000000
--- a/unittests/uorb_unittests/uORBCommunicatorMockLoopback.cpp
+++ /dev/null
@@ -1,138 +0,0 @@
-/****************************************************************************
- *
- * Copyright (c) 2015 Mark Charlebois. All rights reserved.
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions
- * are met:
- *
- * 1. Redistributions of source code must retain the above copyright
- *    notice, this list of conditions and the following disclaimer.
- * 2. Redistributions in binary form must reproduce the above copyright
- *    notice, this list of conditions and the following disclaimer in
- *    the documentation and/or other materials provided with the
- *    distribution.
- * 3. Neither the name PX4 nor the names of its contributors may be
- *    used to endorse or promote products derived from this software
- *    without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
- * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
- * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- *
- ****************************************************************************/
-
-#include "uORBCommunicatorMockLoopback.hpp"
-#include "uORB/uORB.h"
-#include "uORBGtestTopics.hpp"
-#include "uORBManager.hpp"
-#include <string.h>
-#include "px4_log.h"
-
-#define LOG_TAG "uORBCommunicatorMockLoopback.cpp"
-
-
-uORB_test::uORBCommunicatorMockLoopback::uORBCommunicatorMockLoopback()
-: _rx_handler( nullptr )
-{
-  //_sub_topicA_clone_fd = orb_subscribe( ORB_ID( topicA_clone ), (void*)&_sub_semaphore );
-  //_sub_topicB_clone_fd = orb_subscribe( ORB_ID( topicB_clone ), nullptr );
-
-  _topic_translation_map[ "topicA" ] = "topicA_clone";
-  _topic_translation_map[ "topicB" ] = "topicB_clone";
-  _topic_translation_map[ "topicA_clone" ] = "topicA";
-  _topic_translation_map[ "topicB_clone" ] = "topicB";
-}
-
-int16_t uORB_test::uORBCommunicatorMockLoopback::add_subscription
-(
-    const char * messageName,
-    int32_t msgRateInHz
-)
-{
-
-  int16_t rc = -1;
-  PX4_INFO( "got add_subscription for msg[%s] rate[%d]", messageName, msgRateInHz );
-
-  if( _rx_handler )
-  {
-    if( _topic_translation_map.find( messageName ) != _topic_translation_map.end() )
-    {
-       rc = _rx_handler->process_add_subscription
-           (
-              _topic_translation_map[messageName].c_str(),
-              msgRateInHz
-           );
-    }
-  }
-  return rc;
-}
-
-int16_t uORB_test::uORBCommunicatorMockLoopback::remove_subscription
-(
-    const char * messageName
-)
-{
-  int16_t rc = -1;
-  PX4_INFO( "got remove_subscription for msg[%s]", messageName );
-  if( _rx_handler )
-  {
-    if( _topic_translation_map.find( messageName ) != _topic_translation_map.end() )
-    {
-       rc = _rx_handler->process_remove_subscription
-           (
-              _topic_translation_map[messageName].c_str()
-           );
-    }
-  }
-  return rc;
-}
-
-int16_t uORB_test::uORBCommunicatorMockLoopback::register_handler
-(
-    uORBCommunicator::IChannelRxHandler* handler
-)
-{
-  int16_t rc = 0;
-  _rx_handler = handler;
-  return rc;
-}
-
-
-int16_t uORB_test::uORBCommunicatorMockLoopback::send_message
-(
-    const char * messageName,
-    int32_t length,
-    uint8_t* data
-)
-{
-  int16_t rc = -1;
-  PX4_INFO( "send_message for msg[%s] datalen[%d]", messageName, length );
-  if( _rx_handler )
-  {
-    if( _topic_translation_map.find( messageName ) != _topic_translation_map.end() )
-    {
-      if( uORB::Manager::get_instance()->is_remote_subscriber_present( _topic_translation_map[messageName].c_str() ) )
-      {
-        rc = _rx_handler->process_received_message
-            ( _topic_translation_map[messageName].c_str(), length, data );
-        PX4_INFO( "[uORBCommunicatorMockLoopback::send_message] return from[topic(%s)] _rx_handler->process_received_message[%d]", messageName, rc );
-      }
-      else
-      {
-        // this is eqvuilanet of not sending the message to the remote.
-        rc = 0;
-      }
-    }
-  }
-  return rc;
-}
diff --git a/unittests/uorb_unittests/uORBCommunicatorMockLoopback.hpp b/unittests/uorb_unittests/uORBCommunicatorMockLoopback.hpp
deleted file mode 100644
index 96a6375bb4..0000000000
--- a/unittests/uorb_unittests/uORBCommunicatorMockLoopback.hpp
+++ /dev/null
@@ -1,133 +0,0 @@
-/****************************************************************************
- *
- * Copyright (c) 2015 Mark Charlebois. All rights reserved.
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions
- * are met:
- *
- * 1. Redistributions of source code must retain the above copyright
- *    notice, this list of conditions and the following disclaimer.
- * 2. Redistributions in binary form must reproduce the above copyright
- *    notice, this list of conditions and the following disclaimer in
- *    the documentation and/or other materials provided with the
- *    distribution.
- * 3. Neither the name PX4 nor the names of its contributors may be
- *    used to endorse or promote products derived from this software
- *    without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
- * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
- * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- *
- ****************************************************************************/
-
-#ifndef _uORBCommunicatorMockLoopback_hpp_
-#define _uORBCommunicatorMockLoopback_hpp_
-
-#include "uORB/uORBCommunicator.hpp"
-#include "uORBGtestTopics.hpp"
-#include <map>
-#include <string>
-#include <set>
-
-namespace uORB_test
-{
-   class uORBCommunicatorMockLoopback;
-}
-
-class uORB_test::uORBCommunicatorMockLoopback : public uORBCommunicator::IChannel
-{
- public:
-
-  uORBCommunicatorMockLoopback();
-
-  /**
-   * @brief Interface to notify the remote entity of interest of a
-   * subscription for a message.
-   *
-   * @param messageName
-   *  This represents the uORB message name; This message name should be
-   *  globally unique.
-   * @param msgRate
-   *  The max rate at which the subscriber can accept the messages.
-   * @return
-   *  0 = success; This means the messages is successfully sent to the receiver
-   *    Note: This does not mean that the receiver as received it.
-   *  otherwise = failure.
-   */
-  virtual int16_t add_subscription( const char * messageName, int32_t msgRateInHz );
-
-
-  /**
-   * @brief Interface to notify the remote entity of removal of a subscription
-   *
-   * @param messageName
-   *  This represents the uORB message name; This message name should be
-   *  globally unique.
-   * @return
-   *  0 = success; This means the messages is successfully sent to the receiver
-   *    Note: This does not necessarily mean that the receiver as received it.
-   *  otherwise = failure.
-   */
-  virtual int16_t remove_subscription( const char * messageName );
-
-  /**
-   * Register Message Handler.  This is internal for the IChannel implementer*
-   */
-  virtual int16_t register_handler( uORBCommunicator::IChannelRxHandler* handler );
-
-
-  /**
-   * @brief Sends the data message over the communication link.
-   * @param messageName
-   *  This represents the uORB message name; This message name should be
-   *  globally unique.
-   * @param length
-   *  The length of the data buffer to be sent.
-   * @param data
-   *  The actual data to be sent.
-   * @return
-   *  0 = success; This means the messages is successfully sent to the receiver
-   *    Note: This does not mean that the receiver as received it.
-   *  otherwise = failure.
-   */
-  virtual int16_t send_message( const char * messageName, int32_t length, uint8_t* data);
-
-  uORBCommunicator::IChannelRxHandler* get_rx_handler()
-  {
-    return _rx_handler;
-  }
-
-/*
-  bool get_remote_topicA_data( struct orb_topic_A* data );
-  bool get_remote_topicB_data( struct orb_topic_B* data );
-*/
-
-
- private:
-  uORBCommunicator::IChannelRxHandler* _rx_handler;
-/*
-  int _sub_topicA_clone_fd;
-  int _sub_topicB_clone_fd;
-  pal::Semaphore _sub_semaphore;
-*/
-
-  std::map<std::string, std::string> _topic_translation_map;
-
-/*
-  struct orb_topic_A _topicAData;
-  struct orb_topic_B _topicBData;
-*/
-};
-
-#endif /* _uORBCommunicatorMockLoopback_hpp_ */
diff --git a/unittests/uorb_unittests/uORBCommunicator_gtests.cpp b/unittests/uorb_unittests/uORBCommunicator_gtests.cpp
deleted file mode 100644
index a0b61a250f..0000000000
--- a/unittests/uorb_unittests/uORBCommunicator_gtests.cpp
+++ /dev/null
@@ -1,483 +0,0 @@
-/****************************************************************************
- *
- * Copyright (c) 2015 Mark Charlebois. All rights reserved.
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions
- * are met:
- *
- * 1. Redistributions of source code must retain the above copyright
- *    notice, this list of conditions and the following disclaimer.
- * 2. Redistributions in binary form must reproduce the above copyright
- *    notice, this list of conditions and the following disclaimer in
- *    the documentation and/or other materials provided with the
- *    distribution.
- * 3. Neither the name PX4 nor the names of its contributors may be
- *    used to endorse or promote products derived from this software
- *    without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
- * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
- * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- *
- ****************************************************************************/
-
-#include "uORBCommunicatorMock.hpp"
-#include "uORBCommunicatorMockLoopback.hpp"
-#include "gtest/gtest.h"
-#include "uORB.h"
-#include "uORBManager.hpp"
-#include "uORBGtestTopics.hpp"
-#include "uORBDevices.hpp"
-#include "px4_log.h"
-#include <errno.h>
-
-#define LOG_TAG "uORBCommunicator_gtests.cpp"
-
-namespace px4
-{
-  void init_once();
-}
-
-namespace uORB_test
-{
-
-   class uORBCommunicatorTest : public ::testing::Test
-   {
-    public:
-     uORBCommunicatorTest() :
-       _masterDevice( nullptr )
-     {
-       px4::init_once();
-
-       // create the Master Device and initialize it
-       _masterDevice = new uORB::DeviceMaster(uORB::PUBSUB);
-       if( _masterDevice != nullptr )
-       {
-         _masterDevice->init();
-       }
-
-       // get the uORB::Manager and set the mock instance
-       // explicitly.
-       uORB::Manager::get_instance()->set_uorb_communicator( &_comm_channel );
-     }
-    protected:
-     uORB_test::uORBCommunicatorMock _comm_channel;
-     struct orb_topic_A _topicA;
-     struct orb_topic_B _topicB;
-     orb_advert_t _pub_ptr;
-     int _sub_fd;
-     uORB::DeviceMaster* _masterDevice;
-   };
-
-   //================= Unit tests for add_subscription ===================
-   // this test will validate if the uORB calls the "add_subscription"
-   // method if there is atleast one subscriber in the local system.
-   //=====================================================================
-   TEST_F( uORBCommunicatorTest, add_subscription_case1 )
-   {
-     // case 1: add subscription should not be called if there are no
-     // internal subscriber and only a publisher.
-     // Steps:
-     // 0. reset the interface counters.
-     // 1. advertize a topic
-     // 2. check to see if add_subscription is called.
-     //    the count should be zero.
-
-     //step 0.
-     _comm_channel.reset_counters();
-     uORB_test::uORBCommunicatorMock::InterfaceCounters c = _comm_channel.get_interface_counters( "topicA" );
-     ASSERT_EQ( c._add_subscriptionCount, 0 );
-
-     //step 1.
-     _topicA.val = 1;
-     ASSERT_NE( ( _pub_ptr = orb_advertise(ORB_ID(topicA), &_topicA) ), nullptr ) << "Failed to advertize uORB Topic orb_topic_A: errno: " << errno;
-     PX4_INFO( "publist handle: 0x%08lx", (unsigned long)_pub_ptr );
-
-     //step 2.
-     c = _comm_channel.get_interface_counters( "topicA" );
-     ASSERT_EQ( c._add_subscriptionCount, 0 );
-   }
-
-   TEST_F( uORBCommunicatorTest, add_subscription_case2 )
-   {
-     // case 1: add subscription should not be called if there is atleast one
-     // internal subscriber and only a publisher.
-     // Steps:
-     // 0. reset the interface counters.
-     // 1. advertize a topic
-     // 2. Add in internal subscriber.
-     // 3. check to see if add_subscription is called.
-     //    the count should be 1.
-     // 4. unsubscribe the topic
-
-     //step 0.
-     _comm_channel.reset_counters();
-     uORB_test::uORBCommunicatorMock::InterfaceCounters c = _comm_channel.get_interface_counters( "topicA" );
-     ASSERT_EQ( c._add_subscriptionCount, 0 );
-
-     //step 1.
-     _topicA.val = 1;
-     _pub_ptr = orb_advertise(ORB_ID(topicA), &_topicA);
-     PX4_INFO( "[uORBCommunicatorTest.add_subscription_case2] orb_advertize(topicA) returncode:(%p)", _pub_ptr );
-     ASSERT_TRUE( ( _pub_ptr != nullptr ) ) << "Failed to advertize uORB Topic orb_topic_A: errno: " << errno;
-     PX4_INFO( "publist handle: 0x%08lx", (unsigned long)_pub_ptr );
-
-     // step 2
-     ASSERT_NE( ( _sub_fd = orb_subscribe(ORB_ID(topicA) ) ) , -1 ) << "Subscribe failed: %d" << errno;
-     PX4_INFO( "subscribe fd: %d", _sub_fd );
-
-     //step 3.
-     c = _comm_channel.get_interface_counters( "topicA" );
-     PX4_INFO( "interface counter for topicA: %d", (int)c._add_subscriptionCount );
-     ASSERT_EQ( c._add_subscriptionCount, 1 );
-
-     //step 4.
-     PX4_INFO( "Before unsubscribe for Topic A" );
-     ASSERT_EQ( orb_unsubscribe( _sub_fd ), OK );
-     PX4_INFO( "After unsubscribe for Topic A" );
-   }
-
-   //============ unit tests for remove_subscribtion =======
-   TEST_F( uORBCommunicatorTest, remove_subscribtion )
-   {
-     // remove subscription should be called if there after a subscription is removed.
-     // Steps:
-     // 0. reset the interface counters.
-     // 1. advertize a topic
-     // 2. Add in internal subscriber.
-     // 3. unsubscribe the topic
-     // 4. check the remove_subsciption counter it should be 1.
-
-     //step 0.
-     _comm_channel.reset_counters();
-     uORB_test::uORBCommunicatorMock::InterfaceCounters c = _comm_channel.get_interface_counters( "topicA" );
-     ASSERT_EQ( c._remove_subscriptionCount, 0 );
-
-     //step 1.
-     // step 1.
-     _topicA.val = 1;
-     _pub_ptr = orb_advertise(ORB_ID(topicA), &_topicA);
-     PX4_INFO( "[uORBCommunicatorTest.remove_subscribtion] orb_advertize(topicA) returncode:(%p)", _pub_ptr );
-     ASSERT_TRUE( ( _pub_ptr != nullptr ) ) << "Failed to advertize uORB Topic orb_topic_A: errno: " << errno;
-     PX4_INFO( "publist handle: 0x%08lx", (unsigned long)_pub_ptr );
-
-     c = _comm_channel.get_interface_counters( "topicA" );
-     ASSERT_EQ( c._remove_subscriptionCount, 0 );
-
-
-     // step 2
-     ASSERT_NE( ( _sub_fd = orb_subscribe(ORB_ID(topicA) )), -1  ); // << "Subscribe failed: " << _sub_fd << "errono: "  << errno;
-     PX4_INFO( "subscribe fd: %d", _sub_fd );
-
-     c = _comm_channel.get_interface_counters( "topicA" );
-     ASSERT_EQ( c._remove_subscriptionCount, 0 );
-
-
-     //step 3.
-     ASSERT_EQ( orb_unsubscribe( _sub_fd ), OK );
-
-     //step 4.
-     c = _comm_channel.get_interface_counters( "topicA" );
-     ASSERT_EQ( c._remove_subscriptionCount, 1 );
-   }
-
-   //============ unit tests for send_message =======
-   TEST_F(uORBCommunicatorTest, send_message_case1 )
-   {
-     // Case1: send message should not be called when a topic is advertized
-     // and there are no remote remote subscribers.
-     // Steps:
-     // 0. reset the interface counters.
-     // 1. advertize a topic
-     // 2. check to see that the send message counter is 0.
-     //step 0.
-     _comm_channel.reset_counters();
-     uORB_test::uORBCommunicatorMock::InterfaceCounters c = _comm_channel.get_interface_counters( "topicA_sndmsg" );
-     ASSERT_EQ( c._send_messageCount, 0 );
-
-     //step 1.
-     ORB_DEFINE( topicA_sndmsg, struct orb_topic_A, nullptr, "TOPICA_SNDMSG:int16_t val;" );
-     _topicA.val = 1;
-     _pub_ptr = orb_advertise(ORB_ID(topicA_sndmsg ), &_topicA );
-     ASSERT_TRUE( ( _pub_ptr != nullptr ) ) << "Failed to advertize uORB Topic topicA_sndmsg: errno: " << errno;
-     PX4_INFO( "publist handle: 0x%08lx", (unsigned long)_pub_ptr );
-
-     c = _comm_channel.get_interface_counters( "topicA_sndmsg" );
-     ASSERT_EQ( c._send_messageCount, 0 );
-   }
-
-   TEST_F(uORBCommunicatorTest, send_message_case2 )
-   {
-     // Case2: send message should be called when a topic is advertized
-     // and there are remote remote subscribers.
-     // Steps:
-     // 0. reset the interface counters.
-     // 1. Add a remote subscriber by calling the "process_add_subscription"
-     // 2. advertize a topic
-     // 3. check to see that the send message counter is 1 and check the value.
-     // 4. publish new data.
-     // 5. check to see that send_msg is incremented by 1 and check the value.
-     // 6. remove remote subscriber by calling the "process_remove_subscription"
-     // 7. publish new data.
-     // 8. check to see the sndmessage counter, it should not increment.
-
-     //step 0.
-     _comm_channel.reset_counters();
-     uORB_test::uORBCommunicatorMock::InterfaceCounters c = _comm_channel.get_interface_counters( "topicB" );
-     ASSERT_EQ( c._send_messageCount, 0 );
-
-     //step 1.
-     uORBCommunicator::IChannelRxHandler* rx_handler = _comm_channel.get_rx_handler();
-     // add a local subsciber to avoid the issue of creating a node for the first time
-     // remote.
-     ASSERT_NE( ( _sub_fd = orb_subscribe(ORB_ID(topicB) ) ), -1 ) << "Subscribe failed for topicB: %d" << errno;
-     PX4_INFO( "subscribe fd: %d", _sub_fd );
-     ASSERT_EQ( rx_handler->process_add_subscription( "topicB", 1 ), 0 );
-     c = _comm_channel.get_interface_counters( "topicB" );
-     ASSERT_EQ( c._send_messageCount, 0 );
-
-     // step 2.
-     _topicB.val = 1;
-     _pub_ptr = orb_advertise(ORB_ID(topicB), &_topicB);
-     PX4_INFO( "publist handle: 0x%08lx", (unsigned long)_pub_ptr );
-     ASSERT_TRUE( _pub_ptr != nullptr ) << "Failed to advertize uORB Topic topicB: errno: " << errno;
-
-     //step 3.
-     c = _comm_channel.get_interface_counters( "topicB" );
-     ASSERT_EQ( c._send_messageCount, 1 );
-
-     struct orb_topic_B test_val;
-     _comm_channel.get_remote_topicB_data( &test_val );
-     ASSERT_EQ( test_val.val, 1 );
-
-     //step 4. publish new data.
-     _topicB.val = 2;
-     ASSERT_EQ( orb_publish( ORB_ID(topicB), _pub_ptr, &_topicB), OK );
-
-     //step 5.
-     c = _comm_channel.get_interface_counters( "topicB" );
-     ASSERT_EQ( c._send_messageCount, 2 );
-
-     _comm_channel.get_remote_topicB_data( &test_val );
-     ASSERT_EQ( test_val.val, 2 );
-
-     // step 6.
-     ASSERT_EQ( rx_handler->process_remove_subscription( "topicB" ), 0 );
-     ASSERT_EQ( orb_unsubscribe( _sub_fd ), OK );
-
-     //step 7. publish new data.
-     _topicB.val = 5;
-     ASSERT_EQ( orb_publish( ORB_ID(topicB), _pub_ptr, &_topicB), OK );
-
-     //step 8.
-     c = _comm_channel.get_interface_counters( "topicB" );
-     ASSERT_EQ( c._send_messageCount, 2 );
-
-     _comm_channel.get_remote_topicB_data( &test_val );
-     ASSERT_EQ( test_val.val, 2 );
-
-   }
-
-   //========== UNIT tests to verify the process_receive_message interface
-   //========== of rx handler.
-   TEST_F( uORBCommunicatorTest, rx_handler_rcv_data_case1 )
-   {
-     // this will mimic the process of calling the process_receive_message
-     // from remote and verify that the local subscribers got it
-     // are the steps.
-     // 1. clear the counters.
-     // 2. advertize a topic
-     // 3. add a local subscriber.
-     // 4. call process_receive_message.
-     // 5. check to see that the subscriber got the data and the message is not sent back
-     //    by looking at the counter for snd message.
-
-     //step 1.
-     _comm_channel.reset_counters();
-     uORB_test::uORBCommunicatorMock::InterfaceCounters c = _comm_channel.get_interface_counters( "topicB" );
-     ASSERT_EQ( c._send_messageCount, 0 );
-
-     // step 2.
-     _topicB.val = 1;
-     _pub_ptr = orb_advertise(ORB_ID(topicB), &_topicB);
-     ASSERT_TRUE( _pub_ptr != nullptr  ) << "Failed to advertize uORB Topic topicB: errno: " << errno;
-     PX4_INFO( "publist handle: 0x%08lx", (unsigned long)_pub_ptr );
-
-     // step 3.
-     ASSERT_NE( ( _sub_fd = orb_subscribe(ORB_ID(topicB)) ), -1 ) << "Subscribe failed for topicB: %d" << errno;
-     PX4_INFO( "subscribe fd: %d", _sub_fd );
-
-     //step 4.
-     uORBCommunicator::IChannelRxHandler* rx_handler = _comm_channel.get_rx_handler();
-     struct orb_topic_B testVal;
-     testVal.val = 10;
-     ASSERT_EQ( rx_handler->process_received_message( "topicB", sizeof( testVal ), (uint8_t*)(&testVal) ), 0 );
-
-     // step 5. check the values.
-     struct orb_topic_B receivedVal;
-     ASSERT_EQ( orb_copy(ORB_ID(topicB), _sub_fd, &receivedVal), OK ) << "copy(1) failed: " << errno;
-     ASSERT_EQ( testVal.val, receivedVal.val )
-       << "copy(1) mismatch: " << testVal.val
-       << " expected " << receivedVal.val;
-
-     c = _comm_channel.get_interface_counters( "topicB" );
-     ASSERT_EQ( c._send_messageCount, 0 );
-
-     // cleanup.
-     ASSERT_EQ( orb_unsubscribe( _sub_fd ), OK );
-   }
-
-   TEST_F( uORBCommunicatorTest, rx_handler_rcv_data_case2 )
-   {
-     // this will mimic the process of calling the process_receive_message
-     // from remote and verify that the local subscribers got it
-     // and also the message is  send back to the remote.  The following
-     // are the steps.
-     // 1. clear the counters.
-     // 2. advertize a topic
-     // 3. add a local subscriber.
-     // 4. add remote subscriber by calling the "process_add_subscription.
-     // 5. call process_receive_message.
-     // 6. check to see that the subscriber got the data and the message is not sent back
-     //    by looking at the counter for snd message.
-
-     //step 1.
-     _comm_channel.reset_counters();
-     uORB_test::uORBCommunicatorMock::InterfaceCounters c = _comm_channel.get_interface_counters( "topicB" );
-     ASSERT_EQ( c._send_messageCount, 0 );
-
-     // step 2.
-     _topicB.val = 1;
-     _pub_ptr = orb_advertise(ORB_ID(topicB), &_topicB);
-     ASSERT_TRUE( _pub_ptr != nullptr ) << "Failed to advertize uORB Topic topicB: errno: " << errno;
-     PX4_INFO( "publist handle: 0x%08lx", (unsigned long)_pub_ptr );
-
-     // step 3.
-     ASSERT_NE( ( _sub_fd = orb_subscribe(ORB_ID(topicB)) ), -1 ) << "Subscribe failed for topicB: %d" << errno;
-     PX4_INFO( "subscribe fd: %d", _sub_fd );
-
-     //step 4.
-     uORBCommunicator::IChannelRxHandler* rx_handler = _comm_channel.get_rx_handler();
-     ASSERT_EQ( rx_handler->process_add_subscription( "topicB", 1 ), 0 );
-     c = _comm_channel.get_interface_counters( "topicB" );
-     ASSERT_EQ( c._send_messageCount, 1 );
-
-     //step 5.
-     struct orb_topic_B testVal;
-     testVal.val = 15;
-     ASSERT_EQ( rx_handler->process_received_message( "topicB", sizeof( testVal ), (uint8_t*)(&testVal) ), 0 );
-     c = _comm_channel.get_interface_counters( "topicB" );
-     ASSERT_EQ( c._send_messageCount, 1 );
-
-
-     // step 6. check the values.
-     struct orb_topic_B receivedVal;
-     ASSERT_EQ( orb_copy(ORB_ID(topicB), _sub_fd, &receivedVal), OK ) << "copy(1) failed: " << errno;
-     ASSERT_EQ( testVal.val, receivedVal.val )
-       << "copy(1) mismatch: " << testVal.val
-       << " expected " << receivedVal.val;
-
-     c = _comm_channel.get_interface_counters( "topicB" );
-     ASSERT_EQ( c._send_messageCount, 1 );
-
-     // cleanup.
-     ASSERT_EQ( orb_unsubscribe( _sub_fd ), OK );
-   }
-
-   TEST_F( uORBCommunicatorTest, Loopback )
-   {
-     // create the loopback instance.
-     uORB_test::uORBCommunicatorMockLoopback _comm_channel_loopback;
-
-     //intiailize the uorb to remove the node map entries;
-     //orb_initialize();
-
-     //set the communucation channel interface.
-     uORB::Manager::get_instance()->set_uorb_communicator( &_comm_channel_loopback );
-
-     // now for the actual test.
-     orb_advert_t pub_topicA_ptr;
-     int sub_topicA_fd;
-     int sub_topicAClone_fd;
-
-     struct orb_topic_A topicA;
-     struct orb_topic_A topicAClone;
-     struct orb_topic_A topicALocal;
-
-     // step 1.
-     topicA.val = 10;
-     pub_topicA_ptr = orb_advertise(ORB_ID(topicA), &topicA);
-     PX4_INFO( "[uORBCommunicatorTest.Loopback]orb_advertize(topicA) return code:(%p)", pub_topicA_ptr );
-     ASSERT_TRUE( pub_topicA_ptr != nullptr ) << "Failed to advertize uORB Topic orb_topic_A: errno: " << errno;
-     PX4_INFO( "publist handle: 0x%08lx", (unsigned long)pub_topicA_ptr );
-
-     // step 2.
-     ASSERT_NE( ( sub_topicA_fd = orb_subscribe(ORB_ID(topicA)) ), -1 ) << "Subscribe failed: %d" << errno;
-     PX4_INFO( "subscribe fd: %d", sub_topicA_fd );
-
-     // step 3. check to see that the subscriber got a value of 10.
-     ASSERT_EQ( orb_copy(ORB_ID(topicA), sub_topicA_fd, &topicALocal), OK ) << "copy(1) failed: " << errno;
-     ASSERT_EQ( topicA.val, topicALocal.val )
-       << "copy(1) mismatch: " << topicA.val
-       << " expected " << topicALocal.val;
-
-     // subscribe a remote subscriber.
-     ASSERT_NE( ( sub_topicAClone_fd = orb_subscribe(ORB_ID(topicA_clone)) ), -1 ) << "Subscribe failed: %d" << errno;
-     PX4_INFO( "subscribe fd: %d", sub_topicAClone_fd );
-
-     ASSERT_EQ( orb_copy(ORB_ID(topicA_clone), sub_topicAClone_fd, &topicAClone), OK ) << "copy(1) failed: " << errno;
-     ASSERT_EQ( topicA.val, topicAClone.val )
-       << "copy(1) mismatch: " << topicA.val
-       << " expected " << topicAClone.val;
-
-     // publish a new data and check to see that the data is received on the remote.
-     topicA.val = 15;
-     ASSERT_EQ( orb_publish( ORB_ID(topicA), pub_topicA_ptr, &topicA), OK );
-
-     // check to see that the subscriber got a new value.
-     ASSERT_EQ( orb_copy(ORB_ID(topicA), sub_topicA_fd, &topicALocal), OK ) << "copy(1) failed: " << errno;
-     ASSERT_EQ( topicA.val, topicALocal.val )
-       << "copy(1) mismatch: " << topicA.val
-       << " expected " << topicALocal.val;
-
-     ASSERT_EQ( orb_copy(ORB_ID(topicA_clone), sub_topicAClone_fd, &topicAClone), OK ) << "copy(1) failed: " << errno;
-     ASSERT_EQ( topicA.val, topicAClone.val )
-       << "copy(1) mismatch: " << topicA.val
-       << " expected " << topicAClone.val;
-
-     // remove the remote subscriber and make sure that the data is not received,
-     ASSERT_EQ( orb_unsubscribe( sub_topicAClone_fd ), OK );
-
-     // publish a new data and check to see that the data is received on local this should not crash.
-     topicA.val = 20;
-     ASSERT_EQ( orb_publish( ORB_ID(topicA), pub_topicA_ptr, &topicA), OK );
-
-     // check to see that the subscriber got a new value.
-     ASSERT_EQ( orb_copy(ORB_ID(topicA), sub_topicA_fd, &topicALocal), OK ) << "copy(1) failed: " << errno;
-     ASSERT_EQ( topicA.val, topicALocal.val )
-       << "copy(1) mismatch: " << topicA.val
-       << " expected " << topicALocal.val;
-
-     //remove the local subscriber as well and publish new data; system should not crash.
-     ASSERT_EQ( orb_unsubscribe( sub_topicA_fd ), OK );
-
-     // publish a new data; this should not crash.
-     topicA.val = 25;
-     ASSERT_EQ( orb_publish( ORB_ID(topicA), pub_topicA_ptr, &topicA), OK );
-
-   }
-}
-
-
-
-

From fa421a91e266b79daf2b0149bce3e79181f0f8cb Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Fri, 29 Jul 2016 14:10:44 +0200
Subject: [PATCH 1132/1230] Add missing teest targets for MindPX

---
 cmake/configs/nuttx_mindpx-v2_default.cmake | 13 ++++++++++++-
 1 file changed, 12 insertions(+), 1 deletion(-)

diff --git a/cmake/configs/nuttx_mindpx-v2_default.cmake b/cmake/configs/nuttx_mindpx-v2_default.cmake
index f24a614b92..41c9d32df8 100644
--- a/cmake/configs/nuttx_mindpx-v2_default.cmake
+++ b/cmake/configs/nuttx_mindpx-v2_default.cmake
@@ -72,9 +72,20 @@ set(config_module_list
 	systemcmds/dumpfile
 	systemcmds/ver
 	systemcmds/sd_bench
-	systemcmds/tests
 	systemcmds/motor_ramp
 
+	#
+	# Tests
+	#
+	drivers/sf0x/sf0x_tests
+	drivers/test_ppm
+	modules/commander/commander_tests
+	modules/controllib_test
+	modules/mavlink/mavlink_tests
+	modules/unit_test
+	modules/uORB/uORB_tests
+	systemcmds/tests
+
 	#
 	# General system control
 	#

From 1b0f41e36ea8c36f4169cf01d1e29fe4768c489e Mon Sep 17 00:00:00 2001
From: David Sidrane <david_s5@nscdg.com>
Date: Mon, 18 Jul 2016 13:49:12 -1000
Subject: [PATCH 1133/1230] Added support to MS5611 Driver for MS5607

---
 src/drivers/ms5611/ms5611.cpp | 112 +++++++++++++++++++++++++---------
 1 file changed, 83 insertions(+), 29 deletions(-)

diff --git a/src/drivers/ms5611/ms5611.cpp b/src/drivers/ms5611/ms5611.cpp
index 5b50b1ff91..3858a306b5 100644
--- a/src/drivers/ms5611/ms5611.cpp
+++ b/src/drivers/ms5611/ms5611.cpp
@@ -33,7 +33,7 @@
 
 /**
  * @file ms5611.cpp
- * Driver for the MS5611 barometric pressure sensor connected via I2C or SPI.
+ * Driver for the MS5611 and MS6507 barometric pressure sensor connected via I2C or SPI.
  */
 
 #include <px4_config.h>
@@ -69,6 +69,11 @@
 
 #include "ms5611.h"
 
+enum MS56XX_DEVICE_TYPES {
+	MS5611_DEVICE	= 0,
+	MS5607_DEVICE	= 1
+};
+
 enum MS5611_BUS {
 	MS5611_BUS_ALL = 0,
 	MS5611_BUS_I2C_INTERNAL,
@@ -94,7 +99,7 @@ static const int ERROR = -1;
 #define POW2(_x)		((_x) * (_x))
 
 /*
- * MS5611 internal constants and data structures.
+ * MS5611/MS5607 internal constants and data structures.
  */
 
 /* internal conversion time: 9.17 ms, so should not be read at rates higher than 100 Hz */
@@ -106,7 +111,7 @@ static const int ERROR = -1;
 class MS5611 : public device::CDev
 {
 public:
-	MS5611(device::Device *interface, ms5611::prom_u &prom_buf, const char *path);
+	MS5611(device::Device *interface, ms5611::prom_u &prom_buf, const char *path, enum MS56XX_DEVICE_TYPES device_type);
 	~MS5611();
 
 	virtual int		init();
@@ -128,11 +133,11 @@ protected:
 	unsigned		_measure_ticks;
 
 	ringbuffer::RingBuffer	*_reports;
-
+	enum MS56XX_DEVICE_TYPES _device_type;
 	bool			_collect_phase;
 	unsigned		_measure_phase;
 
-	/* intermediate temperature values per MS5611 datasheet */
+	/* intermediate temperature values per MS5611/MS5607 datasheet */
 	int32_t			_TEMP;
 	int64_t			_OFF;
 	int64_t			_SENS;
@@ -214,12 +219,14 @@ protected:
  */
 extern "C" __EXPORT int ms5611_main(int argc, char *argv[]);
 
-MS5611::MS5611(device::Device *interface, ms5611::prom_u &prom_buf, const char *path) :
+MS5611::MS5611(device::Device *interface, ms5611::prom_u &prom_buf, const char *path,
+	       enum MS56XX_DEVICE_TYPES device_type) :
 	CDev("MS5611", path),
 	_interface(interface),
 	_prom(prom_buf.s),
 	_measure_ticks(0),
 	_reports(nullptr),
+	_device_type(device_type),
 	_collect_phase(false),
 	_measure_phase(0),
 	_TEMP(0),
@@ -691,27 +698,62 @@ MS5611::collect()
 		_TEMP = 2000 + (int32_t)(((int64_t)dT * _prom.c6_temp_coeff_temp) >> 23);
 
 		/* base sensor scale/offset values */
-		_SENS = ((int64_t)_prom.c1_pressure_sens << 15) + (((int64_t)_prom.c3_temp_coeff_pres_sens * dT) >> 8);
-		_OFF  = ((int64_t)_prom.c2_pressure_offset << 16) + (((int64_t)_prom.c4_temp_coeff_pres_offset * dT) >> 7);
+		if (_device_type == MS5611_DEVICE) {
 
-		/* temperature compensation */
-		if (_TEMP < 2000) {
+			/* Perform MS5611 Caculation */
 
-			int32_t T2 = POW2(dT) >> 31;
+			_OFF  = ((int64_t)_prom.c2_pressure_offset << 16) + (((int64_t)_prom.c4_temp_coeff_pres_offset * dT) >> 7);
+			_SENS = ((int64_t)_prom.c1_pressure_sens << 15) + (((int64_t)_prom.c3_temp_coeff_pres_sens * dT) >> 8);
 
-			int64_t f = POW2((int64_t)_TEMP - 2000);
-			int64_t OFF2 = 5 * f >> 1;
-			int64_t SENS2 = 5 * f >> 2;
+			/* MS5611 temperature compensation */
 
-			if (_TEMP < -1500) {
-				int64_t f2 = POW2(_TEMP + 1500);
-				OFF2 += 7 * f2;
-				SENS2 += 11 * f2 >> 1;
+			if (_TEMP < 2000) {
+
+				int32_t T2 = POW2(dT) >> 31;
+
+				int64_t f = POW2((int64_t)_TEMP - 2000);
+				int64_t OFF2 = 5 * f >> 1;
+				int64_t SENS2 = 5 * f >> 2;
+
+				if (_TEMP < -1500) {
+
+					int64_t f2 = POW2(_TEMP + 1500);
+					OFF2 += 7 * f2;
+					SENS2 += 11 * f2 >> 1;
+				}
+
+				_TEMP -= T2;
+				_OFF  -= OFF2;
+				_SENS -= SENS2;
 			}
 
-			_TEMP -= T2;
-			_OFF  -= OFF2;
-			_SENS -= SENS2;
+		} else if (_device_type == MS5607_DEVICE) {
+
+			/* Perform MS5607 Caculation */
+
+			_OFF  = ((int64_t)_prom.c2_pressure_offset << 17) + (((int64_t)_prom.c4_temp_coeff_pres_offset * dT) >> 6);
+			_SENS = ((int64_t)_prom.c1_pressure_sens << 16) + (((int64_t)_prom.c3_temp_coeff_pres_sens * dT) >> 7);
+
+			/* MS5607 temperature compensation */
+
+			if (_TEMP < 2000) {
+
+				int32_t T2 = POW2(dT) >> 31;
+
+				int64_t f = POW2((int64_t)_TEMP - 2000);
+				int64_t OFF2 = 61 * f >> 4;
+				int64_t SENS2 = 2 * f;
+
+				if (_TEMP < -1500) {
+					int64_t f2 = POW2(_TEMP + 1500);
+					OFF2 += 15 * f2;
+					SENS2 += 8 * f2;
+				}
+
+				_TEMP -= T2;
+				_OFF  -= OFF2;
+				_SENS -= SENS2;
+			}
 		}
 
 	} else {
@@ -840,9 +882,9 @@ struct ms5611_bus_option {
 };
 #define NUM_BUS_OPTIONS (sizeof(bus_options)/sizeof(bus_options[0]))
 
-bool	start_bus(struct ms5611_bus_option &bus);
+bool	start_bus(struct ms5611_bus_option &bus, enum MS56XX_DEVICE_TYPES device_type);
 struct ms5611_bus_option &find_bus(enum MS5611_BUS busid);
-void	start(enum MS5611_BUS busid);
+void	start(enum MS5611_BUS busid, enum MS56XX_DEVICE_TYPES device_type);
 void	test(enum MS5611_BUS busid);
 void	reset(enum MS5611_BUS busid);
 void	info();
@@ -900,7 +942,7 @@ crc4(uint16_t *n_prom)
  * Start the driver.
  */
 bool
-start_bus(struct ms5611_bus_option &bus)
+start_bus(struct ms5611_bus_option &bus, enum MS56XX_DEVICE_TYPES device_type)
 {
 	if (bus.dev != nullptr) {
 		errx(1, "bus option already started");
@@ -915,7 +957,7 @@ start_bus(struct ms5611_bus_option &bus)
 		return false;
 	}
 
-	bus.dev = new MS5611(interface, prom_buf, bus.devpath);
+	bus.dev = new MS5611(interface, prom_buf, bus.devpath, device_type);
 
 	if (bus.dev != nullptr && OK != bus.dev->init()) {
 		delete bus.dev;
@@ -947,7 +989,7 @@ start_bus(struct ms5611_bus_option &bus)
  * is either successfully up and running or failed to start.
  */
 void
-start(enum MS5611_BUS busid)
+start(enum MS5611_BUS busid, enum MS56XX_DEVICE_TYPES device_type)
 {
 	uint8_t i;
 	bool started = false;
@@ -963,7 +1005,7 @@ start(enum MS5611_BUS busid)
 			continue;
 		}
 
-		started = started | start_bus(bus_options[i]);
+		started = started | start_bus(bus_options[i], device_type);
 	}
 
 	if (!started) {
@@ -1195,6 +1237,8 @@ usage()
 	warnx("    -I    (intternal I2C bus)");
 	warnx("    -S    (external SPI bus)");
 	warnx("    -s    (internal SPI bus)");
+	warnx("    -T    5611|5607 (default 5611)");
+
 }
 
 } // namespace
@@ -1203,10 +1247,11 @@ int
 ms5611_main(int argc, char *argv[])
 {
 	enum MS5611_BUS busid = MS5611_BUS_ALL;
+	int device_type = 5611; // Default to MS5611
 	int ch;
 
 	/* jump over start/off/etc and look at options first */
-	while ((ch = getopt(argc, argv, "XISs")) != EOF) {
+	while ((ch = getopt(argc, argv, "T:XISs")) != EOF) {
 		switch (ch) {
 		case 'X':
 			busid = MS5611_BUS_I2C_EXTERNAL;
@@ -1224,19 +1269,28 @@ ms5611_main(int argc, char *argv[])
 			busid = MS5611_BUS_SPI_INTERNAL;
 			break;
 
+		case 'T':
+			device_type = atoi(optarg);
+
+			if (device_type == 5611 || device_type == 5607) {
+				break;
+			}
+
+		//no break
 		default:
 			ms5611::usage();
 			exit(0);
 		}
 	}
 
+
 	const char *verb = argv[optind];
 
 	/*
 	 * Start/load the driver.
 	 */
 	if (!strcmp(verb, "start")) {
-		ms5611::start(busid);
+		ms5611::start(busid, device_type == 5607 ? MS5607_DEVICE : MS5611_DEVICE);
 	}
 
 	/*

From 0fa3bd4691d9f8becaa7ec123dd0927fcde3e44b Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Beat=20K=C3=BCng?= <beat-kueng@gmx.net>
Date: Fri, 29 Jul 2016 14:37:26 +0200
Subject: [PATCH 1134/1230] ms5611: use px4_getopt instead of getopt

- px4_getopt is threadsafe
- getopt does not do argument reordering (ms5611 start -T 5607 did not work)
---
 src/drivers/ms5611/ms5611.cpp | 9 ++++++---
 1 file changed, 6 insertions(+), 3 deletions(-)

diff --git a/src/drivers/ms5611/ms5611.cpp b/src/drivers/ms5611/ms5611.cpp
index 3858a306b5..40f0d93ad8 100644
--- a/src/drivers/ms5611/ms5611.cpp
+++ b/src/drivers/ms5611/ms5611.cpp
@@ -66,6 +66,7 @@
 
 #include <systemlib/perf_counter.h>
 #include <systemlib/err.h>
+#include <platforms/px4_getopt.h>
 
 #include "ms5611.h"
 
@@ -1249,9 +1250,11 @@ ms5611_main(int argc, char *argv[])
 	enum MS5611_BUS busid = MS5611_BUS_ALL;
 	int device_type = 5611; // Default to MS5611
 	int ch;
+	int myoptind = 1;
+	const char *myoptarg = NULL;
 
 	/* jump over start/off/etc and look at options first */
-	while ((ch = getopt(argc, argv, "T:XISs")) != EOF) {
+	while ((ch = px4_getopt(argc, argv, "T:XISs", &myoptind, &myoptarg)) != EOF) {
 		switch (ch) {
 		case 'X':
 			busid = MS5611_BUS_I2C_EXTERNAL;
@@ -1270,7 +1273,7 @@ ms5611_main(int argc, char *argv[])
 			break;
 
 		case 'T':
-			device_type = atoi(optarg);
+			device_type = atoi(myoptarg);
 
 			if (device_type == 5611 || device_type == 5607) {
 				break;
@@ -1284,7 +1287,7 @@ ms5611_main(int argc, char *argv[])
 	}
 
 
-	const char *verb = argv[optind];
+	const char *verb = argv[myoptind];
 
 	/*
 	 * Start/load the driver.

From ad2a13dcc82c5529c7746c74d9b653cee04e09fb Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Beat=20K=C3=BCng?= <beat-kueng@gmx.net>
Date: Fri, 29 Jul 2016 14:37:47 +0200
Subject: [PATCH 1135/1230] cmake TAP config: add topic_listener

---
 cmake/configs/nuttx_tap-v1_default.cmake | 1 +
 1 file changed, 1 insertion(+)

diff --git a/cmake/configs/nuttx_tap-v1_default.cmake b/cmake/configs/nuttx_tap-v1_default.cmake
index 0ea32abc9a..2dad824901 100644
--- a/cmake/configs/nuttx_tap-v1_default.cmake
+++ b/cmake/configs/nuttx_tap-v1_default.cmake
@@ -40,6 +40,7 @@ set(config_module_list
 	systemcmds/mtd
 	systemcmds/dumpfile
 	systemcmds/ver
+	systemcmds/topic_listener
 
 	#
 	# General system control

From 2de66b1a9d6f75a1a30621707a3d21c4aba20da7 Mon Sep 17 00:00:00 2001
From: Daniel Agar <daniel@agar.ca>
Date: Fri, 29 Jul 2016 09:05:48 -0400
Subject: [PATCH 1136/1230] FW navigation in high winds (#5097)

---
 .../fw_pos_control_l1_main.cpp                | 28 +++++++++++++++----
 1 file changed, 22 insertions(+), 6 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 a3b03fedc2..572d4c1847 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
@@ -1272,6 +1272,22 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
 	math::Vector<2> ground_speed_2d = {ground_speed(0), ground_speed(1)};
 	calculate_gndspeed_undershoot(current_position, ground_speed_2d, pos_sp_triplet);
 
+	// l1 navigation logic breaks down when wind speed exceeds max airspeed
+	// compute 2D groundspeed from airspeed-heading projection
+	math::Vector<2> air_speed_2d = {_ctrl_state.airspeed * cosf(_yaw), _ctrl_state.airspeed * sinf(_yaw)};
+	math::Vector<2> nav_speed_2d = {0, 0};
+
+	// angle between air_speed_2d and ground_speed_2d
+	float air_gnd_angle = acosf((air_speed_2d * ground_speed_2d) / (air_speed_2d.length() * ground_speed_2d.length()));
+
+	// if angle > 90 degrees or groundspeed is less than threshold, replace groundspeed with airspeed projection
+	if ((fabs(air_gnd_angle) > M_PI) || (ground_speed_2d.length() < 3.0f)) {
+		nav_speed_2d = air_speed_2d;
+
+	} else {
+		nav_speed_2d = ground_speed_2d;
+	}
+
 	/* define altitude error */
 	float altitude_error = pos_sp_triplet.current.alt - _global_pos.alt;
 
@@ -1363,7 +1379,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
 
 		} else if (pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_POSITION) {
 			/* waypoint is a plain navigation waypoint */
-			_l1_control.navigate_waypoints(prev_wp, curr_wp, current_position, ground_speed_2d);
+			_l1_control.navigate_waypoints(prev_wp, curr_wp, current_position, nav_speed_2d);
 			_att_sp.roll_body = _l1_control.nav_roll();
 			_att_sp.yaw_body = _l1_control.nav_bearing();
 
@@ -1376,7 +1392,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
 
 			/* waypoint is a loiter waypoint */
 			_l1_control.navigate_loiter(curr_wp, current_position, pos_sp_triplet.current.loiter_radius,
-						    pos_sp_triplet.current.loiter_direction, ground_speed_2d);
+						    pos_sp_triplet.current.loiter_direction, nav_speed_2d);
 			_att_sp.roll_body = _l1_control.nav_roll();
 			_att_sp.yaw_body = _l1_control.nav_bearing();
 
@@ -1468,14 +1484,14 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
 
 //					warnx("NORET: %d, target_bearing: %d, yaw: %d", (int)land_noreturn_horizontal, (int)math::degrees(target_bearing), (int)math::degrees(_yaw));
 
-				_l1_control.navigate_heading(_target_bearing, _yaw, ground_speed_2d);
+				_l1_control.navigate_heading(_target_bearing, _yaw, nav_speed_2d);
 
 				_land_noreturn_horizontal = true;
 
 			} else {
 
 				/* normal navigation */
-				_l1_control.navigate_waypoints(prev_wp, curr_wp, current_position, ground_speed_2d);
+				_l1_control.navigate_waypoints(prev_wp, curr_wp, current_position, nav_speed_2d);
 			}
 
 			_att_sp.roll_body = _l1_control.nav_roll();
@@ -1688,7 +1704,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
 				 * Update navigation: _runway_takeoff returns the start WP according to mode and phase.
 				 * If we use the navigator heading or not is decided later.
 				 */
-				_l1_control.navigate_waypoints(_runway_takeoff.getStartWP(), curr_wp, current_position, ground_speed_2d);
+				_l1_control.navigate_waypoints(_runway_takeoff.getStartWP(), curr_wp, current_position, nav_speed_2d);
 
 				// update tecs
 				float takeoff_pitch_max_deg = _runway_takeoff.getMaxPitch(_parameters.pitch_limit_max);
@@ -1752,7 +1768,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
 				if (_launch_detection_state != LAUNCHDETECTION_RES_NONE) {
 					/* Launch has been detected, hence we have to control the plane. */
 
-					_l1_control.navigate_waypoints(prev_wp, curr_wp, current_position, ground_speed_2d);
+					_l1_control.navigate_waypoints(prev_wp, curr_wp, current_position, nav_speed_2d);
 					_att_sp.roll_body = _l1_control.nav_roll();
 					_att_sp.yaw_body = _l1_control.nav_bearing();
 

From a18eabead4386f56348e8d94508599c2e47f4bbd Mon Sep 17 00:00:00 2001
From: Julian Oes <julian@oes.ch>
Date: Sun, 24 Jul 2016 11:20:32 +0200
Subject: [PATCH 1137/1230] navigator: whitespace

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

diff --git a/src/modules/navigator/mission.cpp b/src/modules/navigator/mission.cpp
index 3dd79ef419..e9369aacaa 100644
--- a/src/modules/navigator/mission.cpp
+++ b/src/modules/navigator/mission.cpp
@@ -149,7 +149,7 @@ Mission::on_inactive()
 		/* Reset work item type to default if auto take-off has been paused or aborted,
 		   and we landed in manual mode. */
 		if (_work_item_type == WORK_ITEM_TYPE_TAKEOFF) {
-			_work_item_type = WORK_ITEM_TYPE_DEFAULT;		
+			_work_item_type = WORK_ITEM_TYPE_DEFAULT;
 		}
 	}
 }

From 5a262ec6bc29ab070686c6ce26b971cb819cda62 Mon Sep 17 00:00:00 2001
From: Julian Oes <julian@oes.ch>
Date: Sun, 24 Jul 2016 11:21:44 +0200
Subject: [PATCH 1138/1230] uorb: came across a wrong comment

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

diff --git a/src/modules/uORB/uORB.h b/src/modules/uORB/uORB.h
index f7f1a9dff4..9f7e92f65d 100644
--- a/src/modules/uORB/uORB.h
+++ b/src/modules/uORB/uORB.h
@@ -200,7 +200,7 @@ extern int	orb_unsubscribe(int handle) __EXPORT;
 extern int	orb_copy(const struct orb_metadata *meta, int handle, void *buffer) __EXPORT;
 
 /**
- * @see uORB::Manager::orb_advertise()
+ * @see uORB::Manager::orb_check()
  */
 extern int	orb_check(int handle, bool *updated) __EXPORT;
 

From 390a7165dd56440f8cb13df689219613120c63e9 Mon Sep 17 00:00:00 2001
From: Daniel Agar <daniel@agar.ca>
Date: Fri, 29 Jul 2016 11:46:47 -0400
Subject: [PATCH 1139/1230] make tests (#5163)

* make tests use px4 instead of mainapp

* commander_tests ALTCTL - not rotary requires altitude
---
 Makefile                                                    | 2 +-
 .../commander/commander_tests/state_machine_helper_test.cpp | 6 +++++-
 2 files changed, 6 insertions(+), 2 deletions(-)

diff --git a/Makefile b/Makefile
index 4447c73093..e05f9b03a4 100644
--- a/Makefile
+++ b/Makefile
@@ -305,7 +305,7 @@ run_tests_posix: posix_sitl_default
 	@mkdir -p build_posix_sitl_default/src/firmware/posix/rootfs/fs/microsd
 	@mkdir -p build_posix_sitl_default/src/firmware/posix/rootfs/eeprom
 	@touch build_posix_sitl_default/src/firmware/posix/rootfs/eeprom/parameters
-	@(cd build_posix_sitl_default/src/firmware/posix && ./mainapp -d ../../../../posix-configs/SITL/init/rcS_tests | tee test_output)
+	@(cd build_posix_sitl_default/src/firmware/posix && ./px4 -d ../../../../posix-configs/SITL/init/rcS_tests | tee test_output)
 	@(cd build_posix_sitl_default/src/firmware/posix && grep --color=always "All tests passed" test_output)
 
 tests: check_unittest run_tests_posix
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 570f2b5886..27756ffea1 100644
--- a/src/modules/commander/commander_tests/state_machine_helper_test.cpp
+++ b/src/modules/commander/commander_tests/state_machine_helper_test.cpp
@@ -370,7 +370,7 @@ bool StateMachineHelperTest::mainStateTransitionTest(void)
 			commander_state_s::MAIN_STATE_AUTO_RTL, commander_state_s::MAIN_STATE_MANUAL, TRANSITION_CHANGED },
 
 		{ "transition: MANUAL to ALTCTL - not rotary",
-			MTT_ALL_NOT_VALID,
+			MTT_LOC_ALT_VALID,
 			commander_state_s::MAIN_STATE_MANUAL, commander_state_s::MAIN_STATE_ALTCTL, TRANSITION_CHANGED },
 
 		{ "transition: MANUAL to ALTCTL - rotary, global position not valid, local altitude valid",
@@ -423,6 +423,10 @@ bool StateMachineHelperTest::mainStateTransitionTest(void)
 			MTT_GLOBAL_POS_VALID,
 			commander_state_s::MAIN_STATE_MANUAL, commander_state_s::MAIN_STATE_AUTO_RTL, TRANSITION_DENIED },
 
+		{ "transition: MANUAL to ALTCTL - not rotary",
+			MTT_ALL_NOT_VALID,
+			commander_state_s::MAIN_STATE_MANUAL, commander_state_s::MAIN_STATE_ALTCTL, TRANSITION_DENIED },
+
 		{ "no transition: MANUAL to ALTCTL - rotary, global position not valid, local altitude not valid",
 			MTT_ROTARY_WING,
 			commander_state_s::MAIN_STATE_MANUAL, commander_state_s::MAIN_STATE_ALTCTL, TRANSITION_DENIED },

From 5f16c976627bb6170dc6e389905fc4ce56fb3faf Mon Sep 17 00:00:00 2001
From: James Goppert <jgoppert@users.noreply.github.com>
Date: Fri, 29 Jul 2016 16:18:27 -0400
Subject: [PATCH 1140/1230] Added mb12xx sensor enable. (#5164)

---
 ROMFS/px4fmu_common/init.d/rcS      |  6 ++++++
 src/modules/sensors/sensor_params.c | 10 ++++++++++
 2 files changed, 16 insertions(+)

diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS
index 4f101e89f7..c89b4bf84d 100644
--- a/ROMFS/px4fmu_common/init.d/rcS
+++ b/ROMFS/px4fmu_common/init.d/rcS
@@ -593,6 +593,12 @@ then
 		sf0x start
 	fi
 
+	# mb12xx sonar sensor
+	if param compare SENS_EN_MB12XX 1
+	then
+		mb12xx start
+	fi
+
 	if ver hwcmp PX4FMU_V4
 	then
 		frsky_telemetry start -d /dev/ttyS6
diff --git a/src/modules/sensors/sensor_params.c b/src/modules/sensors/sensor_params.c
index b4e8983f2a..710b4154d2 100644
--- a/src/modules/sensors/sensor_params.c
+++ b/src/modules/sensors/sensor_params.c
@@ -3054,6 +3054,16 @@ PARAM_DEFINE_INT32(SENS_EN_LL40LS, 0);
  */
 PARAM_DEFINE_INT32(SENS_EN_SF0X, 0);
 
+/**
+ * Maxbotix Soanr (mb12xx)
+ *
+ * @reboot_required true
+ *
+ * @boolean
+ * @group Sensor Enable
+ */
+PARAM_DEFINE_INT32(SENS_EN_MB12XX, 0);
+
 /**
  * Set the minimum PWM for the MAIN outputs
  *

From b5ef5cabb558bb49ef9c029ccd2d06fe7d33ad17 Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Sat, 30 Jul 2016 11:40:50 +0200
Subject: [PATCH 1141/1230] Updated src/drivers/gps/devices

---
 src/drivers/gps/devices | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/src/drivers/gps/devices b/src/drivers/gps/devices
index a5e3e263aa..3cac6a5bc5 160000
--- a/src/drivers/gps/devices
+++ b/src/drivers/gps/devices
@@ -1 +1 @@
-Subproject commit a5e3e263aaaa613e007717d3ebf5ca7c597bcf51
+Subproject commit 3cac6a5bc5826d7ac495827a46b63f476d82ba86

From e4f20f98cde94bb5948a077017984e1c6cc1a993 Mon Sep 17 00:00:00 2001
From: sander <sander@droneslab.com>
Date: Thu, 28 Jul 2016 20:58:48 +0200
Subject: [PATCH 1142/1230] Implement altitude acceptance radius

---
 src/modules/navigator/mission_block.cpp  | 13 +++++-----
 src/modules/navigator/navigator.h        |  9 +++++++
 src/modules/navigator/navigator_main.cpp | 14 +++++++++++
 src/modules/navigator/navigator_params.c | 30 +++++++++++++++++++++++-
 4 files changed, 59 insertions(+), 7 deletions(-)

diff --git a/src/modules/navigator/mission_block.cpp b/src/modules/navigator/mission_block.cpp
index 6af37faa29..34eb53edbe 100644
--- a/src/modules/navigator/mission_block.cpp
+++ b/src/modules/navigator/mission_block.cpp
@@ -167,13 +167,13 @@ MissionBlock::is_mission_item_reached()
 			&& _navigator->get_vstatus()->is_rotary_wing) {
 			/* require only altitude for takeoff for multicopter, do not use waypoint acceptance radius */
 			if (_navigator->get_global_position()->alt >
-				altitude_amsl - _navigator->get_acceptance_radius()) {
+				altitude_amsl - _navigator->get_altitude_acceptance_radius()) {
 				_waypoint_position_reached = true;
 			}
 		} else if (_mission_item.nav_cmd == NAV_CMD_TAKEOFF) {
 			/* for takeoff mission items use the parameter for the takeoff acceptance radius */
 			if (dist >= 0.0f && dist <= _navigator->get_acceptance_radius()
-				&& dist_z <= _navigator->get_default_acceptance_radius()) {
+				&& dist_z <= _navigator->get_altitude_acceptance_radius()) {
 				_waypoint_position_reached = true;
 			}
 		} else if (!_navigator->get_vstatus()->is_rotary_wing &&
@@ -186,7 +186,7 @@ MissionBlock::is_mission_item_reached()
 			 * radius (+ some margin). Time inside and turn count is handled elsewhere.
 			 */
 			if (dist >= 0.0f && dist <= _navigator->get_acceptance_radius(_mission_item.loiter_radius * 1.2f)
-				&& dist_z <= _navigator->get_default_acceptance_radius()) {
+				&& dist_z <= _navigator->get_altitude_acceptance_radius()) {
 
 				_waypoint_position_reached = true;
 			} else {
@@ -214,7 +214,7 @@ MissionBlock::is_mission_item_reached()
 								&dist_xy, &dist_z);
 
 				if (dist >= 0.0f && dist <= _navigator->get_acceptance_radius(_mission_item.loiter_radius * 1.2f)
-					&& dist_z <= _navigator->get_default_acceptance_radius()) {
+					&& dist_z <= _navigator->get_altitude_acceptance_radius()) {
 
 					// now set the loiter to the final altitude in the NAV_CMD_LOITER_TO_ALT mission item
 					curr_sp->alt = altitude_amsl;
@@ -223,7 +223,7 @@ MissionBlock::is_mission_item_reached()
 
 			} else {
 				if (dist >= 0.0f && dist <= _navigator->get_acceptance_radius(_mission_item.loiter_radius * 1.2f)
-					&& dist_z <= _navigator->get_default_acceptance_radius()) {
+					&& dist_z <= _navigator->get_altitude_acceptance_radius()) {
 
 					_waypoint_position_reached = true;
 
@@ -253,7 +253,8 @@ MissionBlock::is_mission_item_reached()
 			}
 
 			if (dist >= 0.0f && dist <= mission_acceptance_radius
-				&& dist_z <= _navigator->get_default_acceptance_radius()) {
+				&& dist_z <= _navigator->get_altitude_acceptance_radius()) {
+				printf("reached acrad: %f \n", (double)_navigator->get_altitude_acceptance_radius());
 				_waypoint_position_reached = true;
 			}
 		}
diff --git a/src/modules/navigator/navigator.h b/src/modules/navigator/navigator.h
index 02b1548dbd..e652ed6f3b 100644
--- a/src/modules/navigator/navigator.h
+++ b/src/modules/navigator/navigator.h
@@ -166,6 +166,13 @@ public:
 	 */
 	float		get_acceptance_radius();
 
+    /**
+     * Get the altitude acceptance radius
+     *
+     * @return the distance from the target altitude before considering the waypoint reached
+     */
+    float		get_altitude_acceptance_radius();
+
 	/**
 	 * Get the cruising speed
 	 *
@@ -285,6 +292,8 @@ private:
 
 	control::BlockParamFloat _param_loiter_radius;	/**< loiter radius for fixedwing */
 	control::BlockParamFloat _param_acceptance_radius;	/**< acceptance for takeoff */
+    control::BlockParamFloat _param_fw_alt_acceptance_radius;	/**< acceptance radius for fixedwing altitude */
+    control::BlockParamFloat _param_mc_alt_acceptance_radius;	/**< acceptance radius for multicopter altitude */
 	control::BlockParamInt _param_datalinkloss_act;	/**< select data link loss action */
 	control::BlockParamInt _param_rcloss_act;	/**< select data link loss action */
 	
diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp
index cb7ce3284a..2b66a73c8e 100644
--- a/src/modules/navigator/navigator_main.cpp
+++ b/src/modules/navigator/navigator_main.cpp
@@ -155,6 +155,8 @@ Navigator::Navigator() :
 	_follow_target(this, "TAR"),
 	_param_loiter_radius(this, "LOITER_RAD"),
 	_param_acceptance_radius(this, "ACC_RAD"),
+	_param_fw_alt_acceptance_radius(this, "FW_ALT_RAD"),
+	_param_mc_alt_acceptance_radius(this, "MC_ALT_RAD"),
 	_param_datalinkloss_act(this, "DLL_ACT"),
 	_param_rcloss_act(this, "RCL_ACT"),
 	_param_cruising_speed_hover(this, "MPC_XY_CRUISE", false),
@@ -723,6 +725,18 @@ Navigator::get_acceptance_radius()
 	return get_acceptance_radius(_param_acceptance_radius.get());
 }
 
+float
+Navigator::get_altitude_acceptance_radius()
+{
+	if (!this->get_vstatus()->is_rotary_wing) {
+		return _param_fw_alt_acceptance_radius.get();
+	} else {
+		return _param_mc_alt_acceptance_radius.get();
+	}
+}
+
+
+
 float
 Navigator::get_cruising_speed()
 {
diff --git a/src/modules/navigator/navigator_params.c b/src/modules/navigator/navigator_params.c
index f2eaf9ba5c..3b93795b66 100644
--- a/src/modules/navigator/navigator_params.c
+++ b/src/modules/navigator/navigator_params.c
@@ -58,7 +58,7 @@ PARAM_DEFINE_FLOAT(NAV_LOITER_RAD, 50.0f);
  * Acceptance Radius
  *
  * Default acceptance radius, overridden by acceptance radius of waypoint if set.
- * For fixed wing NAV_ACC_RAD is the vertical acceptance, as the L1 turning distance is used for horizontal acceptance.
+ * For fixed wing the L1 turning distance is used for horizontal acceptance.
  *
  * @unit m
  * @min 0.05
@@ -69,6 +69,34 @@ PARAM_DEFINE_FLOAT(NAV_LOITER_RAD, 50.0f);
  */
 PARAM_DEFINE_FLOAT(NAV_ACC_RAD, 10.0f);
 
+/**
+ * FW Altitude Acceptance Radius
+ *
+ * Acceptance radius for fixedwing altitude.
+ *
+ * @unit m
+ * @min 0.05
+ * @max 200.0
+ * @decimal 1
+ * @increment 0.5
+ * @group Mission
+ */
+PARAM_DEFINE_FLOAT(NAV_FW_ALT_RAD, 10.0f);
+
+/**
+ * MC Altitude Acceptance Radius
+ *
+ * Acceptance radius for multicopter altitude.
+ *
+ * @unit m
+ * @min 0.05
+ * @max 200.0
+ * @decimal 1
+ * @increment 0.5
+ * @group Mission
+ */
+PARAM_DEFINE_FLOAT(NAV_MC_ALT_RAD, 3.0f);
+
 /**
  * Set data link loss failsafe mode
  *

From 2bca19fc5e19e383dda41dc6da0671f16a503f5f Mon Sep 17 00:00:00 2001
From: sander <sander@droneslab.com>
Date: Thu, 28 Jul 2016 21:01:56 +0200
Subject: [PATCH 1143/1230] Remove acceptance radius from takeoff altitude for
 MC

---
 src/modules/navigator/mission_block.cpp | 3 +--
 1 file changed, 1 insertion(+), 2 deletions(-)

diff --git a/src/modules/navigator/mission_block.cpp b/src/modules/navigator/mission_block.cpp
index 34eb53edbe..9e9ed4785d 100644
--- a/src/modules/navigator/mission_block.cpp
+++ b/src/modules/navigator/mission_block.cpp
@@ -166,8 +166,7 @@ MissionBlock::is_mission_item_reached()
 		if ((_mission_item.nav_cmd == NAV_CMD_TAKEOFF || _mission_item.nav_cmd == NAV_CMD_VTOL_TAKEOFF)
 			&& _navigator->get_vstatus()->is_rotary_wing) {
 			/* require only altitude for takeoff for multicopter, do not use waypoint acceptance radius */
-			if (_navigator->get_global_position()->alt >
-				altitude_amsl - _navigator->get_altitude_acceptance_radius()) {
+			if (_navigator->get_global_position()->alt >= altitude_amsl) {
 				_waypoint_position_reached = true;
 			}
 		} else if (_mission_item.nav_cmd == NAV_CMD_TAKEOFF) {

From 94fc8bda02a3f802d7767e347a81b506ebbe1aaa Mon Sep 17 00:00:00 2001
From: sander <sander@droneslab.com>
Date: Thu, 28 Jul 2016 22:29:18 +0200
Subject: [PATCH 1144/1230] Fix indenting

---
 src/modules/navigator/navigator.h | 16 ++++++++--------
 1 file changed, 8 insertions(+), 8 deletions(-)

diff --git a/src/modules/navigator/navigator.h b/src/modules/navigator/navigator.h
index e652ed6f3b..7e793b99d9 100644
--- a/src/modules/navigator/navigator.h
+++ b/src/modules/navigator/navigator.h
@@ -166,12 +166,12 @@ public:
 	 */
 	float		get_acceptance_radius();
 
-    /**
-     * Get the altitude acceptance radius
-     *
-     * @return the distance from the target altitude before considering the waypoint reached
-     */
-    float		get_altitude_acceptance_radius();
+	/**
+	 * Get the altitude acceptance radius
+	 *
+	 * @return the distance from the target altitude before considering the waypoint reached
+	 */
+	float		get_altitude_acceptance_radius();
 
 	/**
 	 * Get the cruising speed
@@ -292,8 +292,8 @@ private:
 
 	control::BlockParamFloat _param_loiter_radius;	/**< loiter radius for fixedwing */
 	control::BlockParamFloat _param_acceptance_radius;	/**< acceptance for takeoff */
-    control::BlockParamFloat _param_fw_alt_acceptance_radius;	/**< acceptance radius for fixedwing altitude */
-    control::BlockParamFloat _param_mc_alt_acceptance_radius;	/**< acceptance radius for multicopter altitude */
+	control::BlockParamFloat _param_fw_alt_acceptance_radius;	/**< acceptance radius for fixedwing altitude */
+	control::BlockParamFloat _param_mc_alt_acceptance_radius;	/**< acceptance radius for multicopter altitude */
 	control::BlockParamInt _param_datalinkloss_act;	/**< select data link loss action */
 	control::BlockParamInt _param_rcloss_act;	/**< select data link loss action */
 	

From 788f04ea58aebef10819f212b9753a9453436267 Mon Sep 17 00:00:00 2001
From: sander <sander@droneslab.com>
Date: Thu, 28 Jul 2016 22:31:49 +0200
Subject: [PATCH 1145/1230] Remove debug info

---
 src/modules/navigator/mission_block.cpp | 1 -
 1 file changed, 1 deletion(-)

diff --git a/src/modules/navigator/mission_block.cpp b/src/modules/navigator/mission_block.cpp
index 9e9ed4785d..1f840f2930 100644
--- a/src/modules/navigator/mission_block.cpp
+++ b/src/modules/navigator/mission_block.cpp
@@ -253,7 +253,6 @@ MissionBlock::is_mission_item_reached()
 
 			if (dist >= 0.0f && dist <= mission_acceptance_radius
 				&& dist_z <= _navigator->get_altitude_acceptance_radius()) {
-				printf("reached acrad: %f \n", (double)_navigator->get_altitude_acceptance_radius());
 				_waypoint_position_reached = true;
 			}
 		}

From 36299c59b4b4775f9688a4b8b8315dcc3f5c543d Mon Sep 17 00:00:00 2001
From: Julian Oes <julian@oes.ch>
Date: Sun, 17 Jul 2016 15:24:15 +0100
Subject: [PATCH 1146/1230] unittests: whitespace

---
 unittests/CMakeLists.txt | 131 +++++++++++++++++++++++++++------------
 1 file changed, 93 insertions(+), 38 deletions(-)

diff --git a/unittests/CMakeLists.txt b/unittests/CMakeLists.txt
index 9df3adcce3..53f3922dad 100644
--- a/unittests/CMakeLists.txt
+++ b/unittests/CMakeLists.txt
@@ -1,5 +1,9 @@
 cmake_minimum_required(VERSION 2.8)
 
+include(CMakeForceCompiler)
+#CMAKE_FORCE_C_COMPILER(clang Clang)
+#CMAKE_FORCE_CXX_COMPILER(clang++ Clang)
+
 if("${CMAKE_C_COMPILER_ID}" STREQUAL "Clang")
 	add_compile_options(-Qunused-arguments )
 endif()
@@ -10,16 +14,28 @@ endif()
 project(unittests)
 enable_testing()
 
-set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -Wall -Werror -std=gnu99 -g")
-set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall -Werror -std=c++11 -g -fno-exceptions -fno-rtti -fno-threadsafe-statics")
+include(CheckCXXCompilerFlag)
+CHECK_CXX_COMPILER_FLAG("-std=c++11" COMPILER_SUPPORTS_CXX11)
+CHECK_CXX_COMPILER_FLAG("-std=c++0x" COMPILER_SUPPORTS_CXX0X)
 
+if(COMPILER_SUPPORTS_CXX11)
+	set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11 -fno-exceptions -fno-rtti -fno-threadsafe-statics -D__CUSTOM_FILE_IO__ -D__PX4_UNIT_TESTS -g -Wall -Werror")
+elseif(COMPILER_SUPPORTS_CXX0X)
+	set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++0x -fno-exceptions -fno-rtti -fno-threadsafe-statics -D__CUSTOM_FILE_IO__ -D__PX4_UNIT_TESTS -g -Wall -Werror")
+else()
+	message(STATUS "The compiler ${CMAKE_CXX_COMPILER} has no C++11 support. Please use a different C++ compiler.")
+endif()
+set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -std=gnu99 -g")
+
+set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -g3 -fsanitize=address -fno-omit-frame-pointer")
+set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wno-sign-compare -Wno-unused-but-set-variable")
 
 set(GTEST_DIR ${CMAKE_SOURCE_DIR}/googletest)
 add_subdirectory(${GTEST_DIR})
 include_directories(${GTEST_DIR}/include)
 
 set(PX4_SRC ${CMAKE_SOURCE_DIR}/../src)
-set(PX4_SITL_BUILD ${PX4_SRC}/../build_posix_sitl_default)
+set(PX4_SITL_BUILD ${PX4_SRC}/../build_posix_sitl_test)
 
 include_directories(${CMAKE_SOURCE_DIR})
 include_directories(${PX4_SITL_BUILD}/src)
@@ -36,32 +52,43 @@ include_directories(${PX4_SRC}/modules/uORB)
 include_directories(${PX4_SRC}/platforms)
 include_directories(${PX4_SRC}/platforms/posix/include)
 include_directories(${PX4_SRC}/platforms/posix/px4_layer)
-include_directories(${PX4_SRC}/platforms/posix/work_queue)
 
-add_definitions(-D__CUSTOM_FILE_IO__)
 add_definitions(-D__EXPORT=)
 add_definitions(-D__PX4_POSIX)
 add_definitions(-D__PX4_TESTS)
-add_definitions(-D__PX4_UNIT_TESTS)
 add_definitions(-D_UNIT_TEST=)
 add_definitions(-DERROR=-1)
 add_definitions(-Dmain_t=int)
 add_definitions(-Dnoreturn_function=)
 add_definitions(-DOK=0)
 
-if(${CMAKE_SYSTEM_NAME} MATCHES "Darwin")
-	add_definitions(-D__PX4_DARWIN)
-else()
-	add_definitions(-D__PX4_LINUX)
-endif()
+# check
+add_custom_target(check
+		COMMAND ${CMAKE_CTEST_COMMAND} -j2 --output-on-failure
+		WORKING_DIR ${CMAKE_BINARY_DIR}
+		USES_TERMINAL)
 
-add_library(px4_platform 
+function(add_gtest)
+	foreach(test_name ${ARGN})
+		if(${CMAKE_SYSTEM_NAME} MATCHES "Darwin")
+			target_link_libraries(${test_name} gtest_main pthread px4_platform)
+			add_definitions(-D__PX4_DARWIN)
+		else()
+			target_link_libraries(${test_name} gtest_main pthread rt px4_platform)
+			add_definitions(-D__PX4_LINUX)
+		endif()
+		add_test(NAME ${test_name} COMMAND ${test_name} WORKING_DIRECTORY ${CMAKE_SOURCE_DIR})
+		add_dependencies(check ${test_name})
+	endforeach()
+endfunction()
+
+add_library(px4_platform
 			${PX4_SITL_BUILD}/src/modules/param/px4_parameters.c
-			${PX4_SRC}/drivers/device/device_posix.cpp 
-			${PX4_SRC}/drivers/device/i2c_posix.cpp 
+			${PX4_SRC}/drivers/device/device_posix.cpp
+			${PX4_SRC}/drivers/device/i2c_posix.cpp
 			${PX4_SRC}/drivers/device/ringbuffer.cpp
-			${PX4_SRC}/drivers/device/sim.cpp 
-			${PX4_SRC}/drivers/device/vdev.cpp 
+			${PX4_SRC}/drivers/device/sim.cpp
+			${PX4_SRC}/drivers/device/vdev.cpp
 			${PX4_SRC}/drivers/device/vdev_posix.cpp
 			${PX4_SRC}/drivers/device/vfile.cpp
 			${PX4_SRC}/platforms/posix/px4_layer/drv_hrt.c
@@ -87,28 +114,6 @@ add_library(px4_platform
 			${PX4_SRC}/platforms/posix/work_queue/work_thread.c
 			)
 target_include_directories(px4_platform PUBLIC ${PX4_SRC}/platforms)
-target_include_directories(px4_platform PUBLIC ${PX4_SRC}/platforms/posix/include)
-target_include_directories(px4_platform PUBLIC ${PX4_SRC}/platforms/posix/work_queue)
-
-
-# check
-add_custom_target(check
-		COMMAND ${CMAKE_CTEST_COMMAND} -j2 --output-on-failure
-		WORKING_DIR ${CMAKE_BINARY_DIR}
-		USES_TERMINAL)
-
-# add_gtest
-function(add_gtest)
-	foreach(test_name ${ARGN})
-		if(${CMAKE_SYSTEM_NAME} MATCHES "Darwin")
-			target_link_libraries(${test_name} gtest_main pthread px4_platform)
-		else()
-			target_link_libraries(${test_name} gtest_main pthread rt px4_platform)
-		endif()
-		add_test(NAME ${test_name} COMMAND ${test_name} WORKING_DIRECTORY ${CMAKE_SOURCE_DIR})
-		add_dependencies(check ${test_name})
-	endforeach()
-endfunction()
 
 
 #######################################################################
@@ -117,9 +122,59 @@ endfunction()
 #    add_executable(example_test example_test.cpp)
 #    add_gtest(example_test)
 
+
+# autodeclination_test
+add_executable(autodeclination_test autodeclination_test.cpp ${PX4_SRC}/lib/geo_lookup/geo_mag_declination.c)
+add_gtest(autodeclination_test)
+
+# mixer_test
+add_custom_command(OUTPUT ${PX4_SRC}/modules/systemlib/mixer/mixer_multirotor.generated.h
+                   COMMAND ${PX4_SRC}/modules/systemlib/mixer/multi_tables.py > ${PX4_SRC}/modules/systemlib/mixer/mixer_multirotor.generated.h)
+add_executable(mixer_test mixer_test.cpp
+                          ${PX4_SRC}/modules/systemlib/mixer/mixer.cpp
+                          ${PX4_SRC}/modules/systemlib/mixer/mixer_group.cpp
+                          ${PX4_SRC}/modules/systemlib/mixer/mixer_load.c
+                          ${PX4_SRC}/modules/systemlib/mixer/mixer_multirotor.cpp
+                          ${PX4_SRC}/modules/systemlib/mixer/mixer_multirotor.generated.h
+                          ${PX4_SRC}/modules/systemlib/mixer/mixer_simple.cpp
+                          ${PX4_SRC}/modules/systemlib/pwm_limit/pwm_limit.c
+                          ${PX4_SRC}/systemcmds/tests/test_mixer.cpp)
+add_gtest(mixer_test)
+
+# conversion_test
+add_executable(conversion_test conversion_test.cpp ${PX4_SRC}/systemcmds/tests/test_conv.cpp)
+add_gtest(conversion_test)
+
+# sbus2_test
+add_executable(sbus2_test sbus2_test.cpp
+						${PX4_SRC}/lib/rc/sbus.c)
+add_gtest(sbus2_test)
+
+# DSM test
+add_executable(dsm_test dsm_test.cpp
+						${PX4_SRC}/lib/rc/dsm.c)
+add_gtest(dsm_test)
+
+# st24_test
+add_executable(rc_input_test st24_test.cpp sumd_test.cpp
+						${PX4_SRC}/lib/rc/st24.c
+						${PX4_SRC}/lib/rc/sumd.c)
+add_gtest(rc_input_test)
+
+# sf0x_test
+add_executable(sf0x_test sf0x_test.cpp
+						${PX4_SRC}/drivers/sf0x/sf0x_parser.cpp)
+add_gtest(sf0x_test)
+
 # param_test
 add_executable(param_test param_test.cpp uorb_stub.cpp
 						${PX4_SRC}/modules/systemlib/bson/tinybson.c
 						${PX4_SRC}/modules/systemlib/param/param.c)
 target_link_libraries(param_test ${PX4_SITL_BUILD}/libmsg_gen.a)
 add_gtest(param_test)
+
+# param_shmem_test
+#add_executable(param_shmem_test param_test.cpp uorb_stub.cpp
+#						${PX4_SRC}/modules/systemlib/bson/tinybson.c
+#						${PX4_SRC}/modules/systemlib/param/param_shmem.c)
+#add_gtest(param_shmem_test)

From 401d807261205a4d3ab19cf3fc9a4b068cecdd24 Mon Sep 17 00:00:00 2001
From: Julian Oes <julian@oes.ch>
Date: Sun, 17 Jul 2016 15:24:39 +0100
Subject: [PATCH 1147/1230] systemlib: added library for hysteresis

There have been two cases where a hysteresis function wasn't working
correctly. It is therefore a good idea to abstract the hysteresis
functionality into a library.
---
 .../systemlib/hysteresis/hysteresis.cpp       |  73 +++++++++++
 src/modules/systemlib/hysteresis/hysteresis.h |  78 ++++++++++++
 unittests/CMakeLists.txt                      |   5 +
 unittests/hysteresis_test.cpp                 | 114 ++++++++++++++++++
 4 files changed, 270 insertions(+)
 create mode 100644 src/modules/systemlib/hysteresis/hysteresis.cpp
 create mode 100644 src/modules/systemlib/hysteresis/hysteresis.h
 create mode 100644 unittests/hysteresis_test.cpp

diff --git a/src/modules/systemlib/hysteresis/hysteresis.cpp b/src/modules/systemlib/hysteresis/hysteresis.cpp
new file mode 100644
index 0000000000..a04c21883c
--- /dev/null
+++ b/src/modules/systemlib/hysteresis/hysteresis.cpp
@@ -0,0 +1,73 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2016 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file hysteresis.cpp
+ *
+ * @author Julian Oes <julian@oes.ch>
+ */
+
+#include <px4_log.h>
+#include "systemlib/hysteresis/hysteresis.h"
+
+
+namespace systemlib {
+
+
+void
+Hysteresis::set_state_and_update(const bool new_state)
+{
+	if (new_state != _state) {
+		if (new_state != _requested_state) {
+			_requested_state = new_state;
+			_last_time_to_change_state = hrt_absolute_time();
+		}
+	} else {
+		_requested_state = _state;
+	}
+	update();
+}
+
+void
+Hysteresis::update()
+{
+
+
+	if (_requested_state != _state &&
+	    hrt_elapsed_time(&_last_time_to_change_state) >= _hysteresis_time_us) {
+
+		_state = _requested_state;
+	}
+}
+
+} // namespace systemlib
diff --git a/src/modules/systemlib/hysteresis/hysteresis.h b/src/modules/systemlib/hysteresis/hysteresis.h
new file mode 100644
index 0000000000..ba841071e8
--- /dev/null
+++ b/src/modules/systemlib/hysteresis/hysteresis.h
@@ -0,0 +1,78 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2016 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file hysteresis.h
+ *
+ * Hysteresis of a boolean value.
+ *
+ * @author Julian Oes <julian@oes.ch>
+ */
+
+#pragma once
+
+#include <drivers/drv_hrt.h>
+
+namespace systemlib {
+
+class Hysteresis
+{
+public:
+	Hysteresis(unsigned hysteresis_time_us, bool init_state) :
+		_hysteresis_time_us(hysteresis_time_us),
+		_state(init_state),
+		_requested_state(init_state),
+		_last_time_to_change_state(0)
+	{}
+
+	~Hysteresis()
+	{}
+
+	bool get_state() const {
+		return _state;
+	}
+
+	void set_state_and_update(const bool new_state);
+
+	void update();
+
+private:
+
+	unsigned _hysteresis_time_us;
+	bool _state;
+
+	bool _requested_state;
+	hrt_abstime _last_time_to_change_state;
+};
+
+} // namespace systemlib
diff --git a/unittests/CMakeLists.txt b/unittests/CMakeLists.txt
index 53f3922dad..c6bf0b90be 100644
--- a/unittests/CMakeLists.txt
+++ b/unittests/CMakeLists.txt
@@ -173,6 +173,11 @@ add_executable(param_test param_test.cpp uorb_stub.cpp
 target_link_libraries(param_test ${PX4_SITL_BUILD}/libmsg_gen.a)
 add_gtest(param_test)
 
+# hysteresis_test
+add_executable(hysteresis_test hysteresis_test.cpp
+						${PX4_SRC}/modules/systemlib/hysteresis/hysteresis.cpp)
+add_gtest(hysteresis_test)
+
 # param_shmem_test
 #add_executable(param_shmem_test param_test.cpp uorb_stub.cpp
 #						${PX4_SRC}/modules/systemlib/bson/tinybson.c
diff --git a/unittests/hysteresis_test.cpp b/unittests/hysteresis_test.cpp
new file mode 100644
index 0000000000..a01fc3f2e2
--- /dev/null
+++ b/unittests/hysteresis_test.cpp
@@ -0,0 +1,114 @@
+#include <systemlib/hysteresis/hysteresis.h>
+
+#include "gtest/gtest.h"
+
+const static unsigned hysteresis_time_us = 5000;
+
+TEST(HysteresisTest, InitFalse)
+{
+	systemlib::Hysteresis hysteresis(hysteresis_time_us, false);
+	ASSERT_FALSE(hysteresis.get_state());
+}
+
+TEST(HysteresisTest, InitTrue)
+{
+	systemlib::Hysteresis hysteresis(hysteresis_time_us, true);
+	ASSERT_TRUE(hysteresis.get_state());
+}
+
+TEST(HysteresisTest, ZeroCase)
+{
+	systemlib::Hysteresis hysteresis(0, false);
+	ASSERT_FALSE(hysteresis.get_state());
+
+	// Change and see result immediately.
+	hysteresis.set_state_and_update(true);
+	ASSERT_TRUE(hysteresis.get_state());
+	hysteresis.set_state_and_update(false);
+	ASSERT_FALSE(hysteresis.get_state());
+	hysteresis.set_state_and_update(true);
+	ASSERT_TRUE(hysteresis.get_state());
+
+	// A wait won't change anything.
+	usleep(1000);
+	hysteresis.update();
+	ASSERT_TRUE(hysteresis.get_state());
+}
+
+TEST(HysteresisTest, ChangeAfterTime)
+{
+	systemlib::Hysteresis hysteresis(hysteresis_time_us, false);
+
+	// Change to true.
+	hysteresis.set_state_and_update(true);
+	ASSERT_FALSE(hysteresis.get_state());
+	usleep(3000);
+	hysteresis.update();
+	ASSERT_FALSE(hysteresis.get_state());
+	usleep(3000);
+	hysteresis.update();
+	ASSERT_TRUE(hysteresis.get_state());
+
+	// Change back to false.
+	hysteresis.set_state_and_update(false);
+	ASSERT_TRUE(hysteresis.get_state());
+	usleep(3000);
+	hysteresis.update();
+	ASSERT_TRUE(hysteresis.get_state());
+	usleep(3000);
+	hysteresis.update();
+	ASSERT_FALSE(hysteresis.get_state());
+}
+
+TEST(HysteresisTest, ChangeAfterTimeMultipleSets)
+{
+	systemlib::Hysteresis hysteresis(hysteresis_time_us, false);
+
+	// Change to true.
+	hysteresis.set_state_and_update(true);
+	ASSERT_FALSE(hysteresis.get_state());
+	usleep(3000);
+	hysteresis.set_state_and_update(true);
+	ASSERT_FALSE(hysteresis.get_state());
+	usleep(3000);
+	hysteresis.set_state_and_update(true);
+	ASSERT_TRUE(hysteresis.get_state());
+
+	// Change to false.
+	hysteresis.set_state_and_update(false);
+	ASSERT_TRUE(hysteresis.get_state());
+	usleep(3000);
+	hysteresis.set_state_and_update(false);
+	ASSERT_TRUE(hysteresis.get_state());
+	usleep(3000);
+	hysteresis.set_state_and_update(false);
+	ASSERT_FALSE(hysteresis.get_state());
+}
+
+TEST(HysteresisTest, TakeChangeBack)
+{
+	systemlib::Hysteresis hysteresis(hysteresis_time_us, false);
+
+	// Change to true.
+	hysteresis.set_state_and_update(true);
+	ASSERT_FALSE(hysteresis.get_state());
+	usleep(3000);
+	hysteresis.update();
+	ASSERT_FALSE(hysteresis.get_state());
+	// Change your mind to false.
+	hysteresis.set_state_and_update(false);
+	ASSERT_FALSE(hysteresis.get_state());
+	usleep(6000);
+	hysteresis.update();
+	ASSERT_FALSE(hysteresis.get_state());
+
+	// And true again
+	hysteresis.set_state_and_update(true);
+	ASSERT_FALSE(hysteresis.get_state());
+	usleep(3000);
+	hysteresis.update();
+	ASSERT_FALSE(hysteresis.get_state());
+	usleep(3000);
+	hysteresis.update();
+	ASSERT_TRUE(hysteresis.get_state());
+}

From f365832c0bdaa063cf496e14f86e51e0accc4aa8 Mon Sep 17 00:00:00 2001
From: Julian Oes <julian@oes.ch>
Date: Sun, 17 Jul 2016 16:03:51 +0100
Subject: [PATCH 1148/1230] hysteresis: we needed different hysteresis

Sometimes, we only need a histeresis in one direction.
---
 .../systemlib/hysteresis/hysteresis.cpp       | 11 ++--
 src/modules/systemlib/hysteresis/hysteresis.h | 17 ++++--
 unittests/hysteresis_test.cpp                 | 57 ++++++++++++++++---
 3 files changed, 67 insertions(+), 18 deletions(-)

diff --git a/src/modules/systemlib/hysteresis/hysteresis.cpp b/src/modules/systemlib/hysteresis/hysteresis.cpp
index a04c21883c..0b4e86ed9e 100644
--- a/src/modules/systemlib/hysteresis/hysteresis.cpp
+++ b/src/modules/systemlib/hysteresis/hysteresis.cpp
@@ -61,12 +61,13 @@ Hysteresis::set_state_and_update(const bool new_state)
 void
 Hysteresis::update()
 {
+	if (_requested_state != _state) {
 
-
-	if (_requested_state != _state &&
-	    hrt_elapsed_time(&_last_time_to_change_state) >= _hysteresis_time_us) {
-
-		_state = _requested_state;
+		if (hrt_elapsed_time(&_last_time_to_change_state) >= (_state ?
+								      _hysteresis_time_from_true_us :
+								      _hysteresis_time_from_false_us)) {
+			_state = _requested_state;
+		}
 	}
 }
 
diff --git a/src/modules/systemlib/hysteresis/hysteresis.h b/src/modules/systemlib/hysteresis/hysteresis.h
index ba841071e8..a151ee85dd 100644
--- a/src/modules/systemlib/hysteresis/hysteresis.h
+++ b/src/modules/systemlib/hysteresis/hysteresis.h
@@ -48,16 +48,25 @@ namespace systemlib {
 class Hysteresis
 {
 public:
-	Hysteresis(unsigned hysteresis_time_us, bool init_state) :
-		_hysteresis_time_us(hysteresis_time_us),
+	Hysteresis(bool init_state) :
 		_state(init_state),
 		_requested_state(init_state),
+		_hysteresis_time_from_true_us(0),
+		_hysteresis_time_from_false_us(0),
 		_last_time_to_change_state(0)
 	{}
 
 	~Hysteresis()
 	{}
 
+	void set_hysteresis_time_from(const bool from_state, const unsigned new_hysteresis_time_us) {
+		if (from_state == true) {
+			_hysteresis_time_from_true_us = new_hysteresis_time_us;
+		} else {
+			_hysteresis_time_from_false_us = new_hysteresis_time_us;
+		}
+	}
+
 	bool get_state() const {
 		return _state;
 	}
@@ -68,10 +77,10 @@ public:
 
 private:
 
-	unsigned _hysteresis_time_us;
 	bool _state;
-
 	bool _requested_state;
+	unsigned _hysteresis_time_from_true_us;
+	unsigned _hysteresis_time_from_false_us;
 	hrt_abstime _last_time_to_change_state;
 };
 
diff --git a/unittests/hysteresis_test.cpp b/unittests/hysteresis_test.cpp
index a01fc3f2e2..99be56f317 100644
--- a/unittests/hysteresis_test.cpp
+++ b/unittests/hysteresis_test.cpp
@@ -2,23 +2,23 @@
 
 #include "gtest/gtest.h"
 
-const static unsigned hysteresis_time_us = 5000;
 
 TEST(HysteresisTest, InitFalse)
 {
-	systemlib::Hysteresis hysteresis(hysteresis_time_us, false);
+	systemlib::Hysteresis hysteresis(false);
 	ASSERT_FALSE(hysteresis.get_state());
 }
 
 TEST(HysteresisTest, InitTrue)
 {
-	systemlib::Hysteresis hysteresis(hysteresis_time_us, true);
+	systemlib::Hysteresis hysteresis(true);
 	ASSERT_TRUE(hysteresis.get_state());
 }
 
 TEST(HysteresisTest, ZeroCase)
 {
-	systemlib::Hysteresis hysteresis(0, false);
+	// Default is 0 hysteresis.
+	systemlib::Hysteresis hysteresis(false);
 	ASSERT_FALSE(hysteresis.get_state());
 
 	// Change and see result immediately.
@@ -37,7 +37,36 @@ TEST(HysteresisTest, ZeroCase)
 
 TEST(HysteresisTest, ChangeAfterTime)
 {
-	systemlib::Hysteresis hysteresis(hysteresis_time_us, false);
+	systemlib::Hysteresis hysteresis(false);
+	hysteresis.set_hysteresis_time_from(false, 5000);
+	hysteresis.set_hysteresis_time_from(true, 3000);
+
+	// Change to true.
+	hysteresis.set_state_and_update(true);
+	ASSERT_FALSE(hysteresis.get_state());
+	usleep(4000);
+	hysteresis.update();
+	ASSERT_FALSE(hysteresis.get_state());
+	usleep(2000);
+	hysteresis.update();
+	ASSERT_TRUE(hysteresis.get_state());
+
+	// Change back to false.
+	hysteresis.set_state_and_update(false);
+	ASSERT_TRUE(hysteresis.get_state());
+	usleep(1000);
+	hysteresis.update();
+	ASSERT_TRUE(hysteresis.get_state());
+	usleep(3000);
+	hysteresis.update();
+	ASSERT_FALSE(hysteresis.get_state());
+}
+
+TEST(HysteresisTest, HysteresisChanged)
+{
+	systemlib::Hysteresis hysteresis(false);
+	hysteresis.set_hysteresis_time_from(true, 2000);
+	hysteresis.set_hysteresis_time_from(false, 5000);
 
 	// Change to true.
 	hysteresis.set_state_and_update(true);
@@ -49,20 +78,25 @@ TEST(HysteresisTest, ChangeAfterTime)
 	hysteresis.update();
 	ASSERT_TRUE(hysteresis.get_state());
 
+	// Change hysteresis time.
+	hysteresis.set_hysteresis_time_from(true, 10000);
+
 	// Change back to false.
 	hysteresis.set_state_and_update(false);
 	ASSERT_TRUE(hysteresis.get_state());
-	usleep(3000);
+	usleep(7000);
 	hysteresis.update();
 	ASSERT_TRUE(hysteresis.get_state());
-	usleep(3000);
+	usleep(5000);
 	hysteresis.update();
 	ASSERT_FALSE(hysteresis.get_state());
 }
 
 TEST(HysteresisTest, ChangeAfterTimeMultipleSets)
 {
-	systemlib::Hysteresis hysteresis(hysteresis_time_us, false);
+	systemlib::Hysteresis hysteresis(false);
+	hysteresis.set_hysteresis_time_from(true, 5000);
+	hysteresis.set_hysteresis_time_from(false, 5000);
 
 	// Change to true.
 	hysteresis.set_state_and_update(true);
@@ -87,7 +121,8 @@ TEST(HysteresisTest, ChangeAfterTimeMultipleSets)
 
 TEST(HysteresisTest, TakeChangeBack)
 {
-	systemlib::Hysteresis hysteresis(hysteresis_time_us, false);
+	systemlib::Hysteresis hysteresis(false);
+	hysteresis.set_hysteresis_time_from(false, 5000);
 
 	// Change to true.
 	hysteresis.set_state_and_update(true);
@@ -111,4 +146,8 @@ TEST(HysteresisTest, TakeChangeBack)
 	usleep(3000);
 	hysteresis.update();
 	ASSERT_TRUE(hysteresis.get_state());
+
+	// The other directory is immediate.
+	hysteresis.set_state_and_update(false);
+	ASSERT_FALSE(hysteresis.get_state());
 }

From ee1669e1743299b6bc5f6547f4f3decf67f41678 Mon Sep 17 00:00:00 2001
From: Julian Oes <julian@oes.ch>
Date: Sun, 17 Jul 2016 16:28:34 +0100
Subject: [PATCH 1149/1230] hysteresis: add to systemlib CMakeLists.txt

---
 src/modules/systemlib/CMakeLists.txt | 1 +
 1 file changed, 1 insertion(+)

diff --git a/src/modules/systemlib/CMakeLists.txt b/src/modules/systemlib/CMakeLists.txt
index 04fccf3566..7277ee3ca1 100644
--- a/src/modules/systemlib/CMakeLists.txt
+++ b/src/modules/systemlib/CMakeLists.txt
@@ -49,6 +49,7 @@ set(SRCS
 	bson/tinybson.c
 	circuit_breaker.cpp
 	battery.cpp
+	hysteresis/hysteresis.cpp
 	)
 
 if(${OS} STREQUAL "nuttx")

From 740bfc0b3251224ce7d8d2507ec2d406a5048410 Mon Sep 17 00:00:00 2001
From: Julian Oes <julian@oes.ch>
Date: Sun, 17 Jul 2016 16:29:02 +0100
Subject: [PATCH 1150/1230] MulticopterLandDetector: use hysteresis lib

The hysteresis was not properly implemented in the land detector and is
therefore replaced by the library call, both for the freefall detector
and the land detector.
---
 .../land_detector/MulticopterLandDetector.cpp | 31 +++++++++----------
 .../land_detector/MulticopterLandDetector.h   |  6 ++--
 2 files changed, 18 insertions(+), 19 deletions(-)

diff --git a/src/modules/land_detector/MulticopterLandDetector.cpp b/src/modules/land_detector/MulticopterLandDetector.cpp
index 43b3a935b2..b09823a06e 100644
--- a/src/modules/land_detector/MulticopterLandDetector.cpp
+++ b/src/modules/land_detector/MulticopterLandDetector.cpp
@@ -66,10 +66,13 @@ MulticopterLandDetector::MulticopterLandDetector() : LandDetector(),
 	_manual{},
 	_ctrl_state{},
 	_ctrl_mode{},
-	_landTimer(0),
-	_freefallTimer(0),
-	_min_trust_start(0)
+	_min_trust_start(0),
+	_freefall_hysteresis(false),
+	_landed_hysteresis(true)
 {
+	// Use Trigger time when transitioning from in-air (false) to landed (true).
+	_landed_hysteresis.set_hysteresis_time_from(false, LAND_DETECTOR_TRIGGER_TIME);
+
 	_paramHandle.maxRotation = param_find("LNDMC_ROT_MAX");
 	_paramHandle.maxVelocity = param_find("LNDMC_XY_VEL_MAX");
 	_paramHandle.maxClimbRate = param_find("LNDMC_Z_VEL_MAX");
@@ -113,10 +116,13 @@ LandDetectionResult MulticopterLandDetector::update()
 
 	updateParameterCache(false);
 
-	if (get_freefall_state()) {
+	_landed_hysteresis.set_state_and_update(get_landed_state());
+	_freefall_hysteresis.set_state_and_update(get_freefall_state());
+
+	if (_freefall_hysteresis.get_state()) {
 		_state = LANDDETECTION_RES_FREEFALL;
 
-	} else if (get_landed_state()) {
+	} else if (_landed_hysteresis.get_state()) {
 		_state = LANDDETECTION_RES_LANDED;
 
 	} else {
@@ -133,8 +139,6 @@ bool MulticopterLandDetector::get_freefall_state()
 		return false;
 	}
 
-	const uint64_t now = hrt_absolute_time();
-
 	if (_ctrl_state.timestamp == 0) {
 		// _ctrl_state is not valid yet, we have to assume we're not falling.
 		return false;
@@ -145,14 +149,7 @@ bool MulticopterLandDetector::get_freefall_state()
 			 + _ctrl_state.z_acc * _ctrl_state.z_acc;
 	acc_norm = sqrtf(acc_norm);	//norm of specific force. Should be close to 9.8 m/s^2 when landed.
 
-	bool freefall = (acc_norm < _params.acc_threshold_m_s2);	//true if we are currently falling
-
-	if (!freefall || _freefallTimer == 0) {	//reset timer if uav not falling
-		_freefallTimer = now;
-		return false;
-	}
-
-	return (now - _freefallTimer) / 1000000.0f > _params.ff_trigger_time;
+	return (acc_norm < _params.acc_threshold_m_s2);	//true if we are currently falling
 }
 
 bool MulticopterLandDetector::get_landed_state()
@@ -240,11 +237,10 @@ bool MulticopterLandDetector::get_landed_state()
 
 	if (verticalMovement || rotating || !minimalThrust || horizontalMovement) {
 		// Sensed movement or thottle high, so reset the land detector.
-		_landTimer = now;
 		return false;
 	}
 
-	return (now - _landTimer > LAND_DETECTOR_TRIGGER_TIME);
+	return true;
 }
 
 void MulticopterLandDetector::updateParameterCache(const bool force)
@@ -267,6 +263,7 @@ void MulticopterLandDetector::updateParameterCache(const bool force)
 		param_get(_paramHandle.minManThrottle, &_params.minManThrottle);
 		param_get(_paramHandle.acc_threshold_m_s2, &_params.acc_threshold_m_s2);
 		param_get(_paramHandle.ff_trigger_time, &_params.ff_trigger_time);
+		_freefall_hysteresis.set_hysteresis_time_from(false, 1e6f*_params.ff_trigger_time);
 	}
 }
 
diff --git a/src/modules/land_detector/MulticopterLandDetector.h b/src/modules/land_detector/MulticopterLandDetector.h
index 20bb611cb7..d9e7b11879 100644
--- a/src/modules/land_detector/MulticopterLandDetector.h
+++ b/src/modules/land_detector/MulticopterLandDetector.h
@@ -53,6 +53,7 @@
 #include <uORB/topics/control_state.h>
 #include <uORB/topics/vehicle_control_mode.h>
 #include <systemlib/param/param.h>
+#include <systemlib/hysteresis/hysteresis.h>
 
 namespace landdetection
 {
@@ -136,9 +137,10 @@ private:
 	struct control_state_s			_ctrl_state;
 	struct vehicle_control_mode_s		_ctrl_mode;
 
-	uint64_t _landTimer;			///< timestamp in microseconds since a possible land was detected
-	uint64_t _freefallTimer;		///< timestamp in microseconds since a possible freefall was detected
 	uint64_t _min_trust_start;		///< timestamp when minimum trust was applied first
+
+	systemlib::Hysteresis _freefall_hysteresis;
+	systemlib::Hysteresis _landed_hysteresis;
 };
 
 }

From 93acff86414d202bf643f8bf04a17f69b7e95439 Mon Sep 17 00:00:00 2001
From: Julian Oes <julian@oes.ch>
Date: Sun, 17 Jul 2016 16:30:06 +0100
Subject: [PATCH 1151/1230] MulticopterLandDetector: remove always true call

The result from `!get_freefall_state()` was always true because
`get_freefall_state()` is already called before `get_landed_state()` is
called. Only if we're not in a freefall, we check if we are landed.
---
 src/modules/land_detector/MulticopterLandDetector.cpp | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/src/modules/land_detector/MulticopterLandDetector.cpp b/src/modules/land_detector/MulticopterLandDetector.cpp
index b09823a06e..7cc57818af 100644
--- a/src/modules/land_detector/MulticopterLandDetector.cpp
+++ b/src/modules/land_detector/MulticopterLandDetector.cpp
@@ -203,7 +203,7 @@ bool MulticopterLandDetector::get_landed_state()
 		// quite acrobatic flight.
 		if ((_min_trust_start > 0) &&
 		    (hrt_elapsed_time(&_min_trust_start) > 8 * 1000 * 1000)) {
-			return !get_freefall_state();
+			return true;
 
 		} else {
 			return false;

From f7ad8c03b4090bc57de64b960c71043a4b0c273c Mon Sep 17 00:00:00 2001
From: Julian Oes <julian@oes.ch>
Date: Sun, 17 Jul 2016 16:58:40 +0100
Subject: [PATCH 1152/1230] hysteresis: use 64bit for everything in us

---
 src/modules/systemlib/hysteresis/hysteresis.h | 6 +++---
 1 file changed, 3 insertions(+), 3 deletions(-)

diff --git a/src/modules/systemlib/hysteresis/hysteresis.h b/src/modules/systemlib/hysteresis/hysteresis.h
index a151ee85dd..05b4102639 100644
--- a/src/modules/systemlib/hysteresis/hysteresis.h
+++ b/src/modules/systemlib/hysteresis/hysteresis.h
@@ -59,7 +59,7 @@ public:
 	~Hysteresis()
 	{}
 
-	void set_hysteresis_time_from(const bool from_state, const unsigned new_hysteresis_time_us) {
+	void set_hysteresis_time_from(const bool from_state, const hrt_abstime new_hysteresis_time_us) {
 		if (from_state == true) {
 			_hysteresis_time_from_true_us = new_hysteresis_time_us;
 		} else {
@@ -79,8 +79,8 @@ private:
 
 	bool _state;
 	bool _requested_state;
-	unsigned _hysteresis_time_from_true_us;
-	unsigned _hysteresis_time_from_false_us;
+	hrt_abstime _hysteresis_time_from_true_us;
+	hrt_abstime _hysteresis_time_from_false_us;
 	hrt_abstime _last_time_to_change_state;
 };
 

From fd6ad6565c02b83b2c7e2562ed5ac642bb3a6b2c Mon Sep 17 00:00:00 2001
From: Julian Oes <julian@oes.ch>
Date: Sun, 17 Jul 2016 17:00:59 +0100
Subject: [PATCH 1153/1230] commander: use hysteresis lib for auto_disarm

Since the auto disarm hysteresis was implemented wrongly, it's now
replaced with the hysteresis library call.
---
 src/modules/commander/commander.cpp | 29 +++++++++++++++++------------
 1 file changed, 17 insertions(+), 12 deletions(-)

diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp
index 804b03172c..6e2eafa0a3 100644
--- a/src/modules/commander/commander.cpp
+++ b/src/modules/commander/commander.cpp
@@ -76,6 +76,7 @@
 #include <systemlib/rc_check.h>
 #include <systemlib/state_table.h>
 #include <systemlib/systemlib.h>
+#include <systemlib/hysteresis/hysteresis.h>
 #include <uORB/topics/actuator_armed.h>
 #include <uORB/topics/actuator_controls.h>
 #include <uORB/topics/battery_status.h>
@@ -167,7 +168,8 @@ static hrt_abstime commander_boot_timestamp = 0;
 static unsigned int leds_counter;
 /* To remember when last notification was sent */
 static uint64_t last_print_mode_reject_time = 0;
-static uint64_t _inair_last_time = 0;
+
+static systemlib::Hysteresis auto_disarm_hysteresis(false);
 
 static float eph_threshold = 5.0f;
 static float epv_threshold = 10.0f;
@@ -1646,6 +1648,9 @@ int commander_thread_main(int argc, char *argv[])
 			param_get(_param_ef_time_thres, &ef_time_thres);
 			param_get(_param_geofence_action, &geofence_action);
 			param_get(_param_disarm_land, &disarm_when_landed);
+			auto_disarm_hysteresis.set_hysteresis_time_from(false,
+									(hrt_abstime)disarm_when_landed * 1000000);
+
 			param_get(_param_low_bat_act, &low_bat_action);
 			param_get(_param_offboard_loss_timeout, &offboard_loss_timeout);
 			param_get(_param_offboard_loss_act, &offboard_loss_act);
@@ -2011,22 +2016,22 @@ int commander_thread_main(int argc, char *argv[])
 				}
 			}
 
-			if (disarm_when_landed > 0) {
-				if (land_detector.landed) {
-					if (_inair_last_time > 0 &&
-					    (hrt_elapsed_time(&_inair_last_time) > (hrt_abstime)disarm_when_landed * 1000 * 1000)) {
-						arm_disarm(false, &mavlink_log_pub, "auto disarm on land");
-						_inair_last_time = 0;
-					}
-				} else {
-					_inair_last_time = land_detector.timestamp;
-				}
-			}
 
 			was_landed = land_detector.landed;
 			was_falling = land_detector.freefall;
 		}
 
+		// Check for auto-disarm
+		if (armed.armed && land_detector.landed && disarm_when_landed > 0) {
+			auto_disarm_hysteresis.set_state_and_update(true);
+		} else {
+			auto_disarm_hysteresis.set_state_and_update(false);
+		}
+
+		if (auto_disarm_hysteresis.get_state()) {
+			arm_disarm(false, &mavlink_log_pub, "auto disarm on land");
+		}
+
 		if (!rtl_on) {
 			// store the last good main_state when not in an navigation
 			// hold state

From 3d0fa410a83288e55cc36e083178c6f7d13c6f69 Mon Sep 17 00:00:00 2001
From: Julian Oes <julian@oes.ch>
Date: Sun, 17 Jul 2016 17:18:27 +0100
Subject: [PATCH 1154/1230] hysteresis: astyle

---
 src/modules/systemlib/hysteresis/hysteresis.cpp |  9 ++++++---
 src/modules/systemlib/hysteresis/hysteresis.h   | 10 +++++++---
 2 files changed, 13 insertions(+), 6 deletions(-)

diff --git a/src/modules/systemlib/hysteresis/hysteresis.cpp b/src/modules/systemlib/hysteresis/hysteresis.cpp
index 0b4e86ed9e..33aee834c1 100644
--- a/src/modules/systemlib/hysteresis/hysteresis.cpp
+++ b/src/modules/systemlib/hysteresis/hysteresis.cpp
@@ -41,7 +41,8 @@
 #include "systemlib/hysteresis/hysteresis.h"
 
 
-namespace systemlib {
+namespace systemlib
+{
 
 
 void
@@ -52,9 +53,11 @@ Hysteresis::set_state_and_update(const bool new_state)
 			_requested_state = new_state;
 			_last_time_to_change_state = hrt_absolute_time();
 		}
+
 	} else {
 		_requested_state = _state;
 	}
+
 	update();
 }
 
@@ -64,8 +67,8 @@ Hysteresis::update()
 	if (_requested_state != _state) {
 
 		if (hrt_elapsed_time(&_last_time_to_change_state) >= (_state ?
-								      _hysteresis_time_from_true_us :
-								      _hysteresis_time_from_false_us)) {
+				_hysteresis_time_from_true_us :
+				_hysteresis_time_from_false_us)) {
 			_state = _requested_state;
 		}
 	}
diff --git a/src/modules/systemlib/hysteresis/hysteresis.h b/src/modules/systemlib/hysteresis/hysteresis.h
index 05b4102639..97cd6c049e 100644
--- a/src/modules/systemlib/hysteresis/hysteresis.h
+++ b/src/modules/systemlib/hysteresis/hysteresis.h
@@ -43,7 +43,8 @@
 
 #include <drivers/drv_hrt.h>
 
-namespace systemlib {
+namespace systemlib
+{
 
 class Hysteresis
 {
@@ -59,15 +60,18 @@ public:
 	~Hysteresis()
 	{}
 
-	void set_hysteresis_time_from(const bool from_state, const hrt_abstime new_hysteresis_time_us) {
+	void set_hysteresis_time_from(const bool from_state, const hrt_abstime new_hysteresis_time_us)
+	{
 		if (from_state == true) {
 			_hysteresis_time_from_true_us = new_hysteresis_time_us;
+
 		} else {
 			_hysteresis_time_from_false_us = new_hysteresis_time_us;
 		}
 	}
 
-	bool get_state() const {
+	bool get_state() const
+	{
 		return _state;
 	}
 

From 9f928308c4c0fbe5848400ead852913c5b711a23 Mon Sep 17 00:00:00 2001
From: Julian Oes <julian@oes.ch>
Date: Sun, 17 Jul 2016 17:26:14 +0100
Subject: [PATCH 1155/1230] land_detector: astyle

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

diff --git a/src/modules/land_detector/MulticopterLandDetector.cpp b/src/modules/land_detector/MulticopterLandDetector.cpp
index 7cc57818af..f049c85132 100644
--- a/src/modules/land_detector/MulticopterLandDetector.cpp
+++ b/src/modules/land_detector/MulticopterLandDetector.cpp
@@ -263,7 +263,7 @@ void MulticopterLandDetector::updateParameterCache(const bool force)
 		param_get(_paramHandle.minManThrottle, &_params.minManThrottle);
 		param_get(_paramHandle.acc_threshold_m_s2, &_params.acc_threshold_m_s2);
 		param_get(_paramHandle.ff_trigger_time, &_params.ff_trigger_time);
-		_freefall_hysteresis.set_hysteresis_time_from(false, 1e6f*_params.ff_trigger_time);
+		_freefall_hysteresis.set_hysteresis_time_from(false, 1e6f * _params.ff_trigger_time);
 	}
 }
 

From 50cac88e5b7c55d01679f2fabf22aa5cb5378838 Mon Sep 17 00:00:00 2001
From: Julian Oes <julian@oes.ch>
Date: Sun, 24 Jul 2016 11:18:39 +0200
Subject: [PATCH 1156/1230] land_detector: big refactor, share update function

This is a big refactor and general cleanup of the land detector. The
main functional change is to share the hysteresis lib across all land
detectors.
---
 .../land_detector/FixedwingLandDetector.cpp   |  83 +++-------
 .../land_detector/FixedwingLandDetector.h     |  73 +++------
 src/modules/land_detector/LandDetector.cpp    | 119 ++++++++++----
 src/modules/land_detector/LandDetector.h      | 152 +++++++++++-------
 .../land_detector/MulticopterLandDetector.cpp |  99 ++++--------
 .../land_detector/MulticopterLandDetector.h   |  57 ++-----
 .../land_detector/VtolLandDetector.cpp        |  51 +++---
 src/modules/land_detector/VtolLandDetector.h  |  51 +++---
 .../land_detector/land_detector_main.cpp      |  34 +++-
 9 files changed, 337 insertions(+), 382 deletions(-)

diff --git a/src/modules/land_detector/FixedwingLandDetector.cpp b/src/modules/land_detector/FixedwingLandDetector.cpp
index af3cfaa61f..0721e68e8e 100644
--- a/src/modules/land_detector/FixedwingLandDetector.cpp
+++ b/src/modules/land_detector/FixedwingLandDetector.cpp
@@ -31,22 +31,23 @@
  *
  ****************************************************************************/
 
-/**
+/*
  * @file FixedwingLandDetector.cpp
- * Land detection algorithm for fixedwings
  *
  * @author Johan Jansen <jnsn.johan@gmail.com>
  * @author Lorenz Meier <lorenz@px4.io>
+ * @author Julian Oes <julian@oes.ch>
  */
 
-#include "FixedwingLandDetector.h"
-
 #include <px4_config.h>
 #include <px4_defines.h>
 #include <cmath>
 #include <drivers/drv_hrt.h>
 
-namespace landdetection
+#include "FixedwingLandDetector.h"
+
+
+namespace land_detector
 {
 
 FixedwingLandDetector::FixedwingLandDetector() : LandDetector(),
@@ -58,12 +59,10 @@ FixedwingLandDetector::FixedwingLandDetector() : LandDetector(),
 	_controlState{},
 	_arming{},
 	_airspeed{},
-	_parameterSub(-1),
 	_velocity_xy_filtered(0.0f),
 	_velocity_z_filtered(0.0f),
 	_airspeed_filtered(0.0f),
-	_accel_horz_lp(0.0f),
-	_landDetectTrigger(0)
+	_accel_horz_lp(0.0f)
 {
 	_paramHandle.maxVelocity = param_find("LNDFW_VEL_XY_MAX");
 	_paramHandle.maxClimbRate = param_find("LNDFW_VEL_Z_MAX");
@@ -71,54 +70,41 @@ FixedwingLandDetector::FixedwingLandDetector() : LandDetector(),
 	_paramHandle.maxIntVelocity = param_find("LNDFW_VELI_MAX");
 }
 
-void FixedwingLandDetector::initialize()
+void FixedwingLandDetector::_initialize_topics()
 {
 	_controlStateSub = orb_subscribe(ORB_ID(control_state));
 	_armingSub = orb_subscribe(ORB_ID(actuator_armed));
 	_airspeedSub = orb_subscribe(ORB_ID(airspeed));
-
-	updateParameterCache(true);
 }
 
-void FixedwingLandDetector::updateSubscriptions()
+void FixedwingLandDetector::_update_topics()
 {
-	orb_update(ORB_ID(control_state), _controlStateSub, &_controlState);
-	orb_update(ORB_ID(actuator_armed), _armingSub, &_arming);
-	orb_update(ORB_ID(airspeed), _airspeedSub, &_airspeed);
+	_orb_update(ORB_ID(control_state), _controlStateSub, &_controlState);
+	_orb_update(ORB_ID(actuator_armed), _armingSub, &_arming);
+	_orb_update(ORB_ID(airspeed), _airspeedSub, &_airspeed);
 }
 
-LandDetectionResult FixedwingLandDetector::update()
+void FixedwingLandDetector::_update_params()
 {
-	// First poll for new data from our subscriptions
-	updateSubscriptions();
-
-	if (get_freefall_state()) {
-		_state = LANDDETECTION_RES_FREEFALL;
-
-	} else if (get_landed_state()) {
-		_state = LANDDETECTION_RES_LANDED;
-
-	} else {
-		_state = LANDDETECTION_RES_FLYING;
-	}
-
-	return _state;
+	param_get(_paramHandle.maxVelocity, &_params.maxVelocity);
+	param_get(_paramHandle.maxClimbRate, &_params.maxClimbRate);
+	param_get(_paramHandle.maxAirSpeed, &_params.maxAirSpeed);
+	param_get(_paramHandle.maxIntVelocity, &_params.maxIntVelocity);
 }
 
-bool FixedwingLandDetector::get_freefall_state()
+bool FixedwingLandDetector::_get_freefall_state()
 {
-	//TODO
+	// TODO
 	return false;
 }
 
-bool FixedwingLandDetector::get_landed_state()
+bool FixedwingLandDetector::_get_landed_state()
 {
 	// only trigger flight conditions if we are armed
 	if (!_arming.armed) {
 		return true;
 	}
 
-	const uint64_t now = hrt_absolute_time();
 	bool landDetected = false;
 
 	if (hrt_elapsed_time(&_controlState.timestamp) < 500 * 1000) {
@@ -147,14 +133,10 @@ bool FixedwingLandDetector::get_landed_state()
 		    && _airspeed_filtered < _params.maxAirSpeed
 		    && _accel_horz_lp < _params.maxIntVelocity) {
 
-			// these conditions need to be stable for a period of time before we trust them
-			if (now > _landDetectTrigger) {
-				landDetected = true;
-			}
+			landDetected = true;
 
 		} else {
-			// reset land detect trigger
-			_landDetectTrigger = now + LAND_DETECTOR_TRIGGER_TIME;
+			landDetected = false;
 		}
 
 	} else {
@@ -165,23 +147,4 @@ bool FixedwingLandDetector::get_landed_state()
 	return landDetected;
 }
 
-void FixedwingLandDetector::updateParameterCache(const bool force)
-{
-	bool updated;
-	parameter_update_s paramUpdate;
-
-	orb_check(_parameterSub, &updated);
-
-	if (updated) {
-		orb_copy(ORB_ID(parameter_update), _parameterSub, &paramUpdate);
-	}
-
-	if (updated || force) {
-		param_get(_paramHandle.maxVelocity, &_params.maxVelocity);
-		param_get(_paramHandle.maxClimbRate, &_params.maxClimbRate);
-		param_get(_paramHandle.maxAirSpeed, &_params.maxAirSpeed);
-		param_get(_paramHandle.maxIntVelocity, &_params.maxIntVelocity);
-	}
-}
-
-}
+} // namespace land_detector
diff --git a/src/modules/land_detector/FixedwingLandDetector.h b/src/modules/land_detector/FixedwingLandDetector.h
index f450f94b82..e52ce1d95e 100644
--- a/src/modules/land_detector/FixedwingLandDetector.h
+++ b/src/modules/land_detector/FixedwingLandDetector.h
@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2013-2015 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2013-2016 PX4 Development Team. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -33,23 +33,22 @@
 
 /**
  * @file FixedwingLandDetector.h
- * Land detection algorithm for fixedwing
+ * Land detector implementation for fixedwing.
  *
  * @author Johan Jansen <jnsn.johan@gmail.com>
+ * @author Morten Lysgaard <morten@lysgaard.no>
+ * @author Julian Oes <julian@oes.ch>
  */
 
-#ifndef __FIXED_WING_LAND_DETECTOR_H__
-#define __FIXED_WING_LAND_DETECTOR_H__
+#pragma once
 
-#include "LandDetector.h"
 #include <uORB/topics/control_state.h>
 #include <uORB/topics/actuator_armed.h>
-#include <uORB/topics/parameter_update.h>
-#include <uORB/topics/vehicle_status.h>
 #include <uORB/topics/airspeed.h>
-#include <systemlib/param/param.h>
 
-namespace landdetection
+#include "LandDetector.h"
+
+namespace land_detector
 {
 
 class FixedwingLandDetector : public LandDetector
@@ -58,46 +57,22 @@ public:
 	FixedwingLandDetector();
 
 protected:
-	/**
-	* @brief  blocking loop, should be run in a separate thread or task. Runs at 50Hz
-	**/
-	LandDetectionResult update() override;
+	virtual void _initialize_topics() override;
 
-	/**
-	* @brief Initializes the land detection algorithm
-	**/
-	void initialize() override;
+	virtual void _update_params() override;
 
-	/**
-	* @brief  polls all subscriptions and pulls any data that has changed
-	**/
-	void updateSubscriptions();
+	virtual void _update_topics() override;
 
-	/**
-	* @brief get UAV landed state
-	**/
-	bool get_landed_state();
-
-	/**
-	* @brief returns true if UAV is in free-fall state
-	**/
-	bool get_freefall_state();
+	virtual bool _get_landed_state() override;
 
+	virtual bool _get_freefall_state() override;
 private:
-	/**
-	* @brief download and update local parameter cache
-	**/
-	void updateParameterCache(const bool force);
-
-	/**
-	* @brief Handles for interesting parameters
-	**/
 	struct {
 		param_t maxVelocity;
 		param_t maxClimbRate;
 		param_t maxAirSpeed;
 		param_t maxIntVelocity;
-	}		_paramHandle;
+	} _paramHandle;
 
 	struct {
 		float maxVelocity;
@@ -106,22 +81,18 @@ private:
 		float maxIntVelocity;
 	} _params;
 
-private:
-	int					_controlStateSub;	/**< notification of local position */
-	int					_armingSub;
-	int					_airspeedSub;
-	struct control_state_s			_controlState;		/**< the result from local position subscription */
-	struct actuator_armed_s			_arming;
-	struct airspeed_s			_airspeed;
-	int 					_parameterSub;
+	int _controlStateSub;
+	int _armingSub;
+	int _airspeedSub;
+
+	struct control_state_s _controlState;
+	struct actuator_armed_s _arming;
+	struct airspeed_s _airspeed;
 
 	float _velocity_xy_filtered;
 	float _velocity_z_filtered;
 	float _airspeed_filtered;
 	float _accel_horz_lp;
-	uint64_t _landDetectTrigger;
 };
 
-}
-
-#endif //__FIXED_WING_LAND_DETECTOR_H__
+} // namespace land_detector
diff --git a/src/modules/land_detector/LandDetector.cpp b/src/modules/land_detector/LandDetector.cpp
index a80b1dad48..abceeddfe4 100644
--- a/src/modules/land_detector/LandDetector.cpp
+++ b/src/modules/land_detector/LandDetector.cpp
@@ -31,32 +31,35 @@
  *
  ****************************************************************************/
 
-/**
+/*
  * @file LandDetector.cpp
- * Land detection algorithm
  *
  * @author Johan Jansen <jnsn.johan@gmail.com>
- * @author Morten Lysgaard <morten@lysgaard.no>
+ * @author Julian Oes <julian@oes.ch>
  */
 
-#include "LandDetector.h"
-#include <unistd.h>                 //usleep
-#include <drivers/drv_hrt.h>
 #include <px4_config.h>
 #include <px4_defines.h>
+#include <drivers/drv_hrt.h>
 
-namespace landdetection
+#include "LandDetector.h"
+
+
+namespace land_detector
 {
 
+
 LandDetector::LandDetector() :
 	_landDetectedPub(nullptr),
 	_landDetected{0, false, false},
-	_arming_time(0),
+	_freefall_hysteresis(false),
+	_landed_hysteresis(true),
 	_taskShouldExit(false),
 	_taskIsRunning(false),
 	_work{}
 {
-	// ctor
+	// Use Trigger time when transitioning from in-air (false) to landed (true).
+	_landed_hysteresis.set_hysteresis_time_from(false, LAND_DETECTOR_TRIGGER_TIME_US);
 }
 
 LandDetector::~LandDetector()
@@ -67,60 +70,115 @@ LandDetector::~LandDetector()
 
 int LandDetector::start()
 {
+	_taskShouldExit = false;
+
 	/* schedule a cycle to start things */
-	work_queue(HPWORK, &_work, (worker_t)&LandDetector::cycle_trampoline, this, 0);
+	work_queue(HPWORK, &_work, (worker_t)&LandDetector::_cycle_trampoline, this, 0);
 
 	return 0;
 }
 
-void LandDetector::shutdown()
+void LandDetector::stop()
 {
 	_taskShouldExit = true;
 }
 
 void
-LandDetector::cycle_trampoline(void *arg)
+LandDetector::_cycle_trampoline(void *arg)
 {
 	LandDetector *dev = reinterpret_cast<LandDetector *>(arg);
 
-	dev->cycle();
+	dev->_cycle();
 }
 
-void LandDetector::cycle()
+void LandDetector::_cycle()
 {
 	if (!_taskIsRunning) {
-		// advertise the first land detected uORB
+		PX4_INFO("we are doing the init");
+		// Advertise the first land detected uORB.
 		_landDetected.timestamp = hrt_absolute_time();
 		_landDetected.landed = false;
 		_landDetected.freefall = false;
 
-		// initialize land detection algorithm
-		initialize();
+		// Initialize uORB topics.
+		_initialize_topics();
 
-		// task is now running, keep doing so until shutdown() has been called
+		_check_params(true);
+
+		// Task is now running, keep doing so until we need to stop.
 		_taskIsRunning = true;
-		_taskShouldExit = false;
 	}
 
-	LandDetectionResult current_state = update();
-	bool landDetected = (current_state == LANDDETECTION_RES_LANDED);
-	bool freefallDetected = (current_state == LANDDETECTION_RES_FREEFALL);
+	_check_params(false);
+
+	_update_topics();
+
+	_update_state();
+
+	bool landDetected = (_state == LandDetectionState::LANDED);
+	bool freefallDetected = (_state == LandDetectionState::FREEFALL);
+
+	// Only publish very first time or when the result has changed.
+	if ((_landDetectedPub == nullptr) ||
+	    (_landDetected.landed != landDetected) ||
+	    (_landDetected.freefall != freefallDetected)) {
 
-	if (_landDetectedPub == nullptr || _landDetected.landed != landDetected || _landDetected.freefall != freefallDetected) {
 		_landDetected.timestamp = hrt_absolute_time();
-		_landDetected.landed = (current_state == LANDDETECTION_RES_LANDED);
-		_landDetected.freefall = (current_state == LANDDETECTION_RES_FREEFALL);
+		_landDetected.landed = (_state == LandDetectionState::LANDED);
+		_landDetected.freefall = (_state == LandDetectionState::FREEFALL);
+
 		int instance;
-		orb_publish_auto(ORB_ID(vehicle_land_detected), &_landDetectedPub, &_landDetected, &instance, ORB_PRIO_DEFAULT);
+		orb_publish_auto(ORB_ID(vehicle_land_detected), &_landDetectedPub, &_landDetected,
+				 &instance, ORB_PRIO_DEFAULT);
 	}
 
 	if (!_taskShouldExit) {
-		work_queue(HPWORK, &_work, (worker_t)&LandDetector::cycle_trampoline, this,
-			   USEC2TICK(1000000 / LAND_DETECTOR_UPDATE_RATE));
+
+		// Schedule next cycle.
+		work_queue(HPWORK, &_work, (worker_t)&LandDetector::_cycle_trampoline, this,
+			   USEC2TICK(1000000 / LAND_DETECTOR_UPDATE_RATE_HZ));
+
+	} else {
+		_taskIsRunning = false;
 	}
 }
 
-bool LandDetector::orb_update(const struct orb_metadata *meta, int handle, void *buffer)
+void LandDetector::_check_params(const bool force)
+{
+	bool updated;
+	parameter_update_s paramUpdate;
+
+	orb_check(_parameterSub, &updated);
+
+	if (updated) {
+		orb_copy(ORB_ID(parameter_update), _parameterSub, &paramUpdate);
+	}
+
+	if (updated || force) {
+		_update_params();
+	}
+}
+
+void LandDetector::_update_state()
+{
+	bool landed = _get_landed_state();
+	_landed_hysteresis.set_state_and_update(landed);
+	_freefall_hysteresis.set_state_and_update(_get_freefall_state());
+
+	if (_freefall_hysteresis.get_state()) {
+		_state = LandDetectionState::FREEFALL;
+
+	} else if (_landed_hysteresis.get_state()) {
+		_state = LandDetectionState::LANDED;
+
+	} else {
+		_state = LandDetectionState::FLYING;
+	}
+
+	return;
+}
+
+bool LandDetector::_orb_update(const struct orb_metadata *meta, int handle, void *buffer)
 {
 	bool newData = false;
 
@@ -140,4 +198,5 @@ bool LandDetector::orb_update(const struct orb_metadata *meta, int handle, void
 	return true;
 }
 
-}
+
+} // namespace land_detector
diff --git a/src/modules/land_detector/LandDetector.h b/src/modules/land_detector/LandDetector.h
index 05b229b7a5..1b71e8d475 100644
--- a/src/modules/land_detector/LandDetector.h
+++ b/src/modules/land_detector/LandDetector.h
@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2013-2015 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2013-2016 PX4 Development Team. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -31,101 +31,131 @@
  *
  ****************************************************************************/
 
-/**
+/*
  * @file LandDetector.h
- * Abstract interface for land detector algorithms
+Land detector interface for multicopter, fixedwing and VTOL implementations.
  *
  * @author Johan Jansen <jnsn.johan@gmail.com>
+ * @author Julian Oes <julian@oes.ch>
  */
 
-#ifndef __LAND_DETECTOR_H__
-#define __LAND_DETECTOR_H__
+#pragma once
 
 #include <px4_workqueue.h>
+#include <systemlib/hysteresis/hysteresis.h>
 #include <uORB/uORB.h>
 #include <uORB/topics/vehicle_land_detected.h>
 
-namespace landdetection
+namespace land_detector
 {
 
-enum LandDetectionResult {
-	LANDDETECTION_RES_FLYING = 0,	/**< UAV is flying */
-	LANDDETECTION_RES_LANDED = 1,	/**< Land has been detected */
-	LANDDETECTION_RES_FREEFALL = 2	/**< Free-fall has been detected */
-};
 
 class LandDetector
 {
 public:
+	enum class LandDetectionState {
+		FLYING = 0,
+		LANDED = 1,
+		FREEFALL = 2
+	};
 
 	LandDetector();
 	virtual ~LandDetector();
 
-	/**
-	 * @return true if this task is currently running
-	 **/
-	inline bool isRunning() const {return _taskIsRunning;}
-
-	/**
-	 * @return the current landed state
+	/*
+	 * @return true if this task is currently running.
 	 */
-	bool isLanded() { return _landDetected.landed; }
+	inline bool is_running() const
+	{
+		return _taskIsRunning;
+	}
 
-	/**
-	 * @brief  Tells the Land Detector task that it should exit
-	 **/
-	void shutdown();
 
-	/**
-	 * @brief Blocking function that should be called from it's own task thread. This method will
-	 *        run the underlying algorithm at the desired update rate and publish if the landing state changes.
-	 **/
+	/*
+	 * @return current state.
+	 */
+	LandDetectionState get_state() const
+	{
+		return _state;
+	}
+
+	/*
+	 * Tells the task that it should exit.
+	 */
+	void stop();
+
+	/*
+	 * Get the work queue going.
+	 */
 	int start();
 
-	static void	cycle_trampoline(void *arg);
-
 protected:
+	/*
+	 * Called once to initialize uORB topics.
+	 */
+	virtual void _initialize_topics() = 0;
 
-	/**
-	* @brief Pure abstract method that must be overriden by sub-classes. This actually runs the underlying algorithm
-	* @return true if a landing was detected and this should be broadcast to the rest of the system
-	**/
-	virtual LandDetectionResult update() = 0;
+	/*
+	 * Update uORB topics.
+	 */
+	virtual void _update_topics() = 0;
 
-	/**
-	* @brief Pure abstract method that is called by this class once for initializing the uderlying algorithm (memory allocation,
-	*        uORB topic subscription, creating file descriptors, etc.)
-	**/
-	virtual void initialize() = 0;
 
-	/**
-	* @brief Convenience function for polling uORB subscriptions
-	* @return true if there was new data and it was successfully copied
-	**/
-	bool orb_update(const struct orb_metadata *meta, int handle, void *buffer);
+	/*
+	 * Update parameters.
+	 */
+	virtual void _update_params() = 0;
 
-	static constexpr uint32_t LAND_DETECTOR_UPDATE_RATE = 50;        /**< Run algorithm at 50Hz */
+	/*
+	 * @return true if UAV is in a landed state.
+	 */
+	virtual bool _get_landed_state() = 0;
 
-	static constexpr uint64_t LAND_DETECTOR_TRIGGER_TIME = 2000000;  /**< usec that landing conditions have to hold
-                                                                          before triggering a land */
-	static constexpr uint64_t LAND_DETECTOR_ARM_PHASE_TIME =
-		2000000;	/**< time interval in which wider acceptance thresholds are used after arming */
+	/*
+	 * @return true if UAV is in free-fall state.
+	 */
+	virtual bool _get_freefall_state() = 0;
 
-protected:
-	orb_advert_t				_landDetectedPub;		/**< publisher for position in local frame */
-	struct vehicle_land_detected_s		_landDetected;			/**< local vehicle position */
-	uint64_t				_arming_time;			/**< timestamp of arming time */
-	LandDetectionResult
-	_state;		/**< Result of land detection. Can be LANDDETECTION_RES_FLYING, LANDDETECTION_RES_LANDED or LANDDETECTION_RES_FREEFALL */
+	/*
+	 * Convenience function for polling uORB subscriptions.
+	 *
+	 * @return true if there was new data and it was successfully copied
+	 */
+	static bool _orb_update(const struct orb_metadata *meta, int handle, void *buffer);
+
+	/* Run main land detector loop at this rate in Hz. */
+	static constexpr uint32_t LAND_DETECTOR_UPDATE_RATE_HZ = 50;
+
+	/* Time in us that landing conditions have to hold before triggering a land. */
+	static constexpr uint64_t LAND_DETECTOR_TRIGGER_TIME_US = 2000000;
+
+	/* Time interval in us in which wider acceptance thresholds are used after arming. */
+	static constexpr uint64_t LAND_DETECTOR_ARM_PHASE_TIME_US = 2000000;
+
+	orb_advert_t _landDetectedPub;
+	struct vehicle_land_detected_s _landDetected;
+
+	int _parameterSub;
+
+	LandDetectionState _state;
+
+	systemlib::Hysteresis _freefall_hysteresis;
+	systemlib::Hysteresis _landed_hysteresis;
 
 private:
-	bool _taskShouldExit;                                               /**< true if it is requested that this task should exit */
-	bool _taskIsRunning;                                                /**< task has reached main loop and is currently running */
-	struct work_s	_work;
+	static void _cycle_trampoline(void *arg);
 
-	void		cycle();
+	void _cycle();
+
+	void _check_params(const bool force);
+
+	void _update_state();
+
+	bool _taskShouldExit;
+	bool _taskIsRunning;
+
+	struct work_s	_work;
 };
 
-}
 
-#endif //__LAND_DETECTOR_H__
+} // namespace land_detector
diff --git a/src/modules/land_detector/MulticopterLandDetector.cpp b/src/modules/land_detector/MulticopterLandDetector.cpp
index f049c85132..7bd347b422 100644
--- a/src/modules/land_detector/MulticopterLandDetector.cpp
+++ b/src/modules/land_detector/MulticopterLandDetector.cpp
@@ -33,19 +33,20 @@
 
 /**
  * @file MulticopterLandDetector.cpp
- * Land detection algorithm for multicopters
  *
  * @author Johan Jansen <jnsn.johan@gmail.com>
  * @author Morten Lysgaard <morten@lysgaard.no>
+ * @author Julian Oes <julian@oes.ch>
  */
 
-#include "MulticopterLandDetector.h"
-
 #include <cmath>
 #include <drivers/drv_hrt.h>
 #include <mathlib/mathlib.h>
 
-namespace landdetection
+#include "MulticopterLandDetector.h"
+
+
+namespace land_detector
 {
 
 MulticopterLandDetector::MulticopterLandDetector() : LandDetector(),
@@ -54,7 +55,6 @@ MulticopterLandDetector::MulticopterLandDetector() : LandDetector(),
 	_vehicleLocalPositionSub(-1),
 	_actuatorsSub(-1),
 	_armingSub(-1),
-	_parameterSub(-1),
 	_attitudeSub(-1),
 	_manualSub(-1),
 	_ctrl_state_sub(-1),
@@ -67,12 +67,8 @@ MulticopterLandDetector::MulticopterLandDetector() : LandDetector(),
 	_ctrl_state{},
 	_ctrl_mode{},
 	_min_trust_start(0),
-	_freefall_hysteresis(false),
-	_landed_hysteresis(true)
+	_arming_time(0)
 {
-	// Use Trigger time when transitioning from in-air (false) to landed (true).
-	_landed_hysteresis.set_hysteresis_time_from(false, LAND_DETECTOR_TRIGGER_TIME);
-
 	_paramHandle.maxRotation = param_find("LNDMC_ROT_MAX");
 	_paramHandle.maxVelocity = param_find("LNDMC_XY_VEL_MAX");
 	_paramHandle.maxClimbRate = param_find("LNDMC_Z_VEL_MAX");
@@ -82,7 +78,7 @@ MulticopterLandDetector::MulticopterLandDetector() : LandDetector(),
 	_paramHandle.ff_trigger_time = param_find("LNDMC_FFALL_TTRI");
 }
 
-void MulticopterLandDetector::initialize()
+void MulticopterLandDetector::_initialize_topics()
 {
 	// subscribe to position, attitude, arming and velocity changes
 	_vehicleLocalPositionSub = orb_subscribe(ORB_ID(vehicle_local_position));
@@ -93,46 +89,34 @@ void MulticopterLandDetector::initialize()
 	_manualSub = orb_subscribe(ORB_ID(manual_control_setpoint));
 	_ctrl_state_sub = orb_subscribe(ORB_ID(control_state));
 	_vehicle_control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode));
-
-	// download parameters
-	updateParameterCache(true);
 }
 
-void MulticopterLandDetector::updateSubscriptions()
+void MulticopterLandDetector::_update_topics()
 {
-	orb_update(ORB_ID(vehicle_local_position), _vehicleLocalPositionSub, &_vehicleLocalPosition);
-	orb_update(ORB_ID(vehicle_attitude), _attitudeSub, &_vehicleAttitude);
-	orb_update(ORB_ID(actuator_controls_0), _actuatorsSub, &_actuators);
-	orb_update(ORB_ID(actuator_armed), _armingSub, &_arming);
-	orb_update(ORB_ID(manual_control_setpoint), _manualSub, &_manual);
-	orb_update(ORB_ID(control_state), _ctrl_state_sub, &_ctrl_state);
-	orb_update(ORB_ID(vehicle_control_mode), _vehicle_control_mode_sub, &_ctrl_mode);
+	_orb_update(ORB_ID(vehicle_local_position), _vehicleLocalPositionSub, &_vehicleLocalPosition);
+	_orb_update(ORB_ID(vehicle_attitude), _attitudeSub, &_vehicleAttitude);
+	_orb_update(ORB_ID(actuator_controls_0), _actuatorsSub, &_actuators);
+	_orb_update(ORB_ID(actuator_armed), _armingSub, &_arming);
+	_orb_update(ORB_ID(manual_control_setpoint), _manualSub, &_manual);
+	_orb_update(ORB_ID(control_state), _ctrl_state_sub, &_ctrl_state);
+	_orb_update(ORB_ID(vehicle_control_mode), _vehicle_control_mode_sub, &_ctrl_mode);
 }
 
-LandDetectionResult MulticopterLandDetector::update()
+void MulticopterLandDetector::_update_params()
 {
-	// first poll for new data from our subscriptions
-	updateSubscriptions();
-
-	updateParameterCache(false);
-
-	_landed_hysteresis.set_state_and_update(get_landed_state());
-	_freefall_hysteresis.set_state_and_update(get_freefall_state());
-
-	if (_freefall_hysteresis.get_state()) {
-		_state = LANDDETECTION_RES_FREEFALL;
-
-	} else if (_landed_hysteresis.get_state()) {
-		_state = LANDDETECTION_RES_LANDED;
-
-	} else {
-		_state = LANDDETECTION_RES_FLYING;
-	}
-
-	return _state;
+	param_get(_paramHandle.maxClimbRate, &_params.maxClimbRate);
+	param_get(_paramHandle.maxVelocity, &_params.maxVelocity);
+	param_get(_paramHandle.maxRotation, &_params.maxRotation_rad_s);
+	_params.maxRotation_rad_s = math::radians(_params.maxRotation_rad_s);
+	param_get(_paramHandle.maxThrottle, &_params.maxThrottle);
+	param_get(_paramHandle.minManThrottle, &_params.minManThrottle);
+	param_get(_paramHandle.acc_threshold_m_s2, &_params.acc_threshold_m_s2);
+	param_get(_paramHandle.ff_trigger_time, &_params.ff_trigger_time);
+	_freefall_hysteresis.set_hysteresis_time_from(false, 1e6f * _params.ff_trigger_time);
 }
 
-bool MulticopterLandDetector::get_freefall_state()
+
+bool MulticopterLandDetector::_get_freefall_state()
 {
 	if (_params.acc_threshold_m_s2 < 0.1f
 	    || _params.acc_threshold_m_s2 > 10.0f) {	//if parameter is set to zero or invalid, disable free-fall detection.
@@ -152,7 +136,7 @@ bool MulticopterLandDetector::get_freefall_state()
 	return (acc_norm < _params.acc_threshold_m_s2);	//true if we are currently falling
 }
 
-bool MulticopterLandDetector::get_landed_state()
+bool MulticopterLandDetector::_get_landed_state()
 {
 	// Time base for this function
 	const uint64_t now = hrt_absolute_time();
@@ -177,6 +161,7 @@ bool MulticopterLandDetector::get_landed_state()
 	// only trigger flight conditions if we are armed
 	if (!_arming.armed) {
 		_arming_time = 0;
+
 		return true;
 
 	} else if (_arming_time == 0) {
@@ -203,6 +188,7 @@ bool MulticopterLandDetector::get_landed_state()
 		// quite acrobatic flight.
 		if ((_min_trust_start > 0) &&
 		    (hrt_elapsed_time(&_min_trust_start) > 8 * 1000 * 1000)) {
+
 			return true;
 
 		} else {
@@ -214,7 +200,7 @@ bool MulticopterLandDetector::get_landed_state()
 
 	// Widen acceptance thresholds for landed state right after arming
 	// so that motor spool-up and other effects do not trigger false negatives.
-	if (hrt_elapsed_time(&_arming_time) < LAND_DETECTOR_ARM_PHASE_TIME) {
+	if (hrt_elapsed_time(&_arming_time) < LAND_DETECTOR_ARM_PHASE_TIME_US) {
 		armThresholdFactor = 2.5f;
 	}
 
@@ -243,28 +229,5 @@ bool MulticopterLandDetector::get_landed_state()
 	return true;
 }
 
-void MulticopterLandDetector::updateParameterCache(const bool force)
-{
-	bool updated;
-	parameter_update_s paramUpdate;
-
-	orb_check(_parameterSub, &updated);
-
-	if (updated) {
-		orb_copy(ORB_ID(parameter_update), _parameterSub, &paramUpdate);
-	}
-
-	if (updated || force) {
-		param_get(_paramHandle.maxClimbRate, &_params.maxClimbRate);
-		param_get(_paramHandle.maxVelocity, &_params.maxVelocity);
-		param_get(_paramHandle.maxRotation, &_params.maxRotation_rad_s);
-		_params.maxRotation_rad_s = math::radians(_params.maxRotation_rad_s);
-		param_get(_paramHandle.maxThrottle, &_params.maxThrottle);
-		param_get(_paramHandle.minManThrottle, &_params.minManThrottle);
-		param_get(_paramHandle.acc_threshold_m_s2, &_params.acc_threshold_m_s2);
-		param_get(_paramHandle.ff_trigger_time, &_params.ff_trigger_time);
-		_freefall_hysteresis.set_hysteresis_time_from(false, 1e6f * _params.ff_trigger_time);
-	}
-}
 
 }
diff --git a/src/modules/land_detector/MulticopterLandDetector.h b/src/modules/land_detector/MulticopterLandDetector.h
index d9e7b11879..dbfaf9aa75 100644
--- a/src/modules/land_detector/MulticopterLandDetector.h
+++ b/src/modules/land_detector/MulticopterLandDetector.h
@@ -33,29 +33,28 @@
 
 /**
  * @file MulticopterLandDetector.h
- * Land detection algorithm for multicopters
+ * Land detection implementation for multicopters.
  *
  * @author Johan Jansen <jnsn.johan@gmail.com>
  * @author Morten Lysgaard <morten@lysgaard.no>
+ * @author Julian Oes <julian@oes.ch>
  */
 
-#ifndef __MULTICOPTER_LAND_DETECTOR_H__
-#define __MULTICOPTER_LAND_DETECTOR_H__
+#pragma once
 
-#include "LandDetector.h"
+#include <systemlib/param/param.h>
 #include <uORB/topics/vehicle_local_position.h>
 #include <uORB/topics/vehicle_attitude.h>
-#include <uORB/topics/vehicle_status.h>
 #include <uORB/topics/actuator_controls.h>
 #include <uORB/topics/actuator_armed.h>
 #include <uORB/topics/parameter_update.h>
 #include <uORB/topics/manual_control_setpoint.h>
 #include <uORB/topics/control_state.h>
 #include <uORB/topics/vehicle_control_mode.h>
-#include <systemlib/param/param.h>
-#include <systemlib/hysteresis/hysteresis.h>
 
-namespace landdetection
+#include "LandDetector.h"
+
+namespace land_detector
 {
 
 class MulticopterLandDetector : public LandDetector
@@ -64,36 +63,15 @@ public:
 	MulticopterLandDetector();
 
 protected:
-	/**
-	* @brief  polls all subscriptions and pulls any data that has changed
-	**/
-	virtual void updateSubscriptions();
+	virtual void _initialize_topics() override;
 
-	/**
-	* @brief Runs one iteration of the land detection algorithm
-	**/
-	virtual LandDetectionResult update() override;
+	virtual void _update_params() override;
 
-	/**
-	* @brief Initializes the land detection algorithm
-	**/
-	virtual void initialize() override;
+	virtual void _update_topics() override;
 
-	/**
-	* @brief download and update local parameter cache
-	**/
-	virtual void updateParameterCache(const bool force);
-
-	/**
-	* @brief get multicopter landed state
-	**/
-	bool get_landed_state();
-
-	/**
-	* @brief returns true if multicopter is in free-fall state
-	**/
-	bool get_freefall_state();
+	virtual bool _get_landed_state() override;
 
+	virtual bool _get_freefall_state() override;
 private:
 
 	/**
@@ -107,7 +85,7 @@ private:
 		param_t minManThrottle;
 		param_t acc_threshold_m_s2;
 		param_t ff_trigger_time;
-	}		_paramHandle;
+	} _paramHandle;
 
 	struct {
 		float maxClimbRate;
@@ -119,11 +97,9 @@ private:
 		float ff_trigger_time;
 	} _params;
 
-private:
 	int _vehicleLocalPositionSub;
 	int _actuatorsSub;
 	int _armingSub;
-	int _parameterSub;
 	int _attitudeSub;
 	int _manualSub;
 	int _ctrl_state_sub;
@@ -138,11 +114,8 @@ private:
 	struct vehicle_control_mode_s		_ctrl_mode;
 
 	uint64_t _min_trust_start;		///< timestamp when minimum trust was applied first
-
-	systemlib::Hysteresis _freefall_hysteresis;
-	systemlib::Hysteresis _landed_hysteresis;
+	uint64_t _arming_time;
 };
 
-}
 
-#endif //__MULTICOPTER_LAND_DETECTOR_H__
+} // namespace land_detector
diff --git a/src/modules/land_detector/VtolLandDetector.cpp b/src/modules/land_detector/VtolLandDetector.cpp
index 50a39c5fea..1acd99bd83 100644
--- a/src/modules/land_detector/VtolLandDetector.cpp
+++ b/src/modules/land_detector/VtolLandDetector.cpp
@@ -36,12 +36,14 @@
  * Land detection algorithm for VTOL
  *
  * @author Roman Bapst <bapstroma@gmail.com>
+ * @author Julian Oes <julian@oes.ch>
  */
 
-#include "VtolLandDetector.h"
 #include <drivers/drv_hrt.h>
 
-namespace landdetection
+#include "VtolLandDetector.h"
+
+namespace land_detector
 {
 
 VtolLandDetector::VtolLandDetector() : MulticopterLandDetector(),
@@ -56,29 +58,24 @@ VtolLandDetector::VtolLandDetector() : MulticopterLandDetector(),
 	_paramHandle.maxAirSpeed = param_find("LNDFW_AIRSPD_MAX");
 }
 
-void VtolLandDetector::initialize()
+void VtolLandDetector::_initialize_topics()
 {
-	MulticopterLandDetector::initialize();
+	MulticopterLandDetector::_initialize_topics();
+
 	_airspeedSub = orb_subscribe(ORB_ID(airspeed));
-
-	// download parameters
-	updateParameterCache(true);
 }
 
-void VtolLandDetector::updateSubscriptions()
+void VtolLandDetector::_update_topics()
 {
-	MulticopterLandDetector::updateSubscriptions();
+	MulticopterLandDetector::_update_topics();
 
-	orb_update(ORB_ID(airspeed), _airspeedSub, &_airspeed);
+	_orb_update(ORB_ID(airspeed), _airspeedSub, &_airspeed);
 }
 
-LandDetectionResult VtolLandDetector::update()
+bool VtolLandDetector::_get_landed_state()
 {
-	updateSubscriptions();
-	updateParameterCache(false);
-
 	// this is returned from the mutlicopter land detector
-	bool landed = get_landed_state();
+	bool landed = MulticopterLandDetector::_get_landed_state();
 
 	// for vtol we additionally consider airspeed
 	if (hrt_elapsed_time(&_airspeed.timestamp) < 500 * 1000) {
@@ -97,27 +94,19 @@ LandDetectionResult VtolLandDetector::update()
 
 	_was_in_air = !landed;
 
-	_state = (landed) ? LANDDETECTION_RES_LANDED : LANDDETECTION_RES_FLYING;
-
-	return _state;
+	return landed;
 }
 
-void VtolLandDetector::updateParameterCache(const bool force)
+bool VtolLandDetector::_get_freefall_state()
 {
-	MulticopterLandDetector::updateParameterCache(force);
+	return MulticopterLandDetector::_get_freefall_state();
+}
 
-	bool updated;
-	parameter_update_s paramUpdate;
+void VtolLandDetector::_update_params()
+{
+	MulticopterLandDetector::_update_params();
 
-	orb_check(_parameterSub, &updated);
-
-	if (updated) {
-		orb_copy(ORB_ID(parameter_update), _parameterSub, &paramUpdate);
-	}
-
-	if (updated || force) {
-		param_get(_paramHandle.maxAirSpeed, &_params.maxAirSpeed);
-	}
+	param_get(_paramHandle.maxAirSpeed, &_params.maxAirSpeed);
 }
 
 }
diff --git a/src/modules/land_detector/VtolLandDetector.h b/src/modules/land_detector/VtolLandDetector.h
index a9b409f610..54ca2735fd 100644
--- a/src/modules/land_detector/VtolLandDetector.h
+++ b/src/modules/land_detector/VtolLandDetector.h
@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2013-2015 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2013-2016 PX4 Development Team. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -33,18 +33,19 @@
 
 /**
  * @file VtolLandDetector.h
- * Land detection algorithm for vtol
+ * Land detection implementation for VTOL also called hybrids.
  *
  * @author Roman Bapst <bapstr@gmail.com>
+ * @author Julian Oes <julian@oes.ch>
  */
 
-#ifndef __VTOL_LAND_DETECTOR_H__
-#define __VTOL_LAND_DETECTOR_H__
+#pragma once
 
-#include "MulticopterLandDetector.h"
 #include <uORB/topics/airspeed.h>
 
-namespace landdetection
+#include "MulticopterLandDetector.h"
+
+namespace land_detector
 {
 
 class VtolLandDetector : public MulticopterLandDetector
@@ -52,34 +53,21 @@ class VtolLandDetector : public MulticopterLandDetector
 public:
 	VtolLandDetector();
 
+protected:
+	virtual void _initialize_topics() override;
+
+	virtual void _update_params() override;
+
+	virtual void _update_topics() override;
+
+	virtual bool _get_landed_state() override;
+
+	virtual bool _get_freefall_state() override;
 
 private:
-	/**
-	* @brief  polls all subscriptions and pulls any data that has changed
-	**/
-	void updateSubscriptions() override;
-
-	/**
-	* @brief Runs one iteration of the land detection algorithm
-	**/
-	LandDetectionResult update() override;
-
-	/**
-	* @brief Initializes the land detection algorithm
-	**/
-	void initialize() override;
-
-	/**
-	* @brief download and update local parameter cache
-	**/
-	void updateParameterCache(const bool force) override;
-
-	/**
-	* @brief Handles for interesting parameters
-	**/
 	struct {
 		param_t maxAirSpeed;
-	}		_paramHandle;
+	} _paramHandle;
 
 	struct {
 		float maxAirSpeed;
@@ -94,6 +82,5 @@ private:
 	float _airspeed_filtered; /**< low pass filtered airspeed */
 };
 
-#endif
 
-}
+} // namespace land_detector
diff --git a/src/modules/land_detector/land_detector_main.cpp b/src/modules/land_detector/land_detector_main.cpp
index bc9b9f8d76..3ccb81ea1e 100644
--- a/src/modules/land_detector/land_detector_main.cpp
+++ b/src/modules/land_detector/land_detector_main.cpp
@@ -55,7 +55,8 @@
 #include "MulticopterLandDetector.h"
 #include "VtolLandDetector.h"
 
-namespace landdetection
+
+namespace land_detector
 {
 
 //Function prototypes
@@ -83,7 +84,7 @@ static void land_detector_stop()
 		return;
 	}
 
-	land_detector_task->shutdown();
+	land_detector_task->stop();
 
 	// Wait for task to die
 	int i = 0;
@@ -92,7 +93,7 @@ static void land_detector_stop()
 		/* wait 20ms */
 		usleep(20000);
 
-	} while (land_detector_task->isRunning() && ++i < 50);
+	} while (land_detector_task->is_running() && ++i < 50);
 
 
 	delete land_detector_task;
@@ -146,8 +147,8 @@ static int land_detector_start(const char *mode)
 	usleep(10000);
 
 	/* check if the waiting involving dots and a newline are still needed */
-	if (!land_detector_task->isRunning()) {
-		while (!land_detector_task->isRunning()) {
+	if (!land_detector_task->is_running()) {
+		while (!land_detector_task->is_running()) {
 			usleep(50000);
 
 			if (hrt_absolute_time() > timeout) {
@@ -191,8 +192,27 @@ int land_detector_main(int argc, char *argv[])
 	if (!strcmp(argv[1], "status")) {
 		if (land_detector_task) {
 
-			if (land_detector_task->isRunning()) {
-				PX4_WARN("running (%s): %s", _currentMode, (land_detector_task->isLanded()) ? "LANDED" : "IN AIR");
+			if (land_detector_task->is_running()) {
+				PX4_INFO("running (%s): %s", _currentMode);
+				LandDetector::LandDetectionState state = land_detector_task->get_state();
+
+				switch (state) {
+				case LandDetector::LandDetectionState::FLYING:
+					PX4_INFO("State: Flying");
+					break;
+
+				case LandDetector::LandDetectionState::LANDED:
+					PX4_INFO("State: Landed");
+					break;
+
+				case LandDetector::LandDetectionState::FREEFALL:
+					PX4_INFO("State: Freefall");
+					break;
+
+				default:
+					PX4_ERR("State: unknown");
+					break;
+				}
 
 			} else {
 				PX4_WARN("exists, but not running (%s)", _currentMode);

From 6bf4bd5f3548b0e3e55e705cbf6ba168c320d625 Mon Sep 17 00:00:00 2001
From: Julian Oes <julian@oes.ch>
Date: Sun, 24 Jul 2016 11:55:40 +0200
Subject: [PATCH 1157/1230] land_detector: remove unused variable

---
 src/modules/land_detector/VtolLandDetector.cpp | 1 -
 src/modules/land_detector/VtolLandDetector.h   | 1 -
 2 files changed, 2 deletions(-)

diff --git a/src/modules/land_detector/VtolLandDetector.cpp b/src/modules/land_detector/VtolLandDetector.cpp
index 1acd99bd83..de058d812f 100644
--- a/src/modules/land_detector/VtolLandDetector.cpp
+++ b/src/modules/land_detector/VtolLandDetector.cpp
@@ -50,7 +50,6 @@ VtolLandDetector::VtolLandDetector() : MulticopterLandDetector(),
 	_paramHandle(),
 	_params(),
 	_airspeedSub(-1),
-	_parameterSub(-1),
 	_airspeed{},
 	_was_in_air(false),
 	_airspeed_filtered(0)
diff --git a/src/modules/land_detector/VtolLandDetector.h b/src/modules/land_detector/VtolLandDetector.h
index 54ca2735fd..31a1e448fb 100644
--- a/src/modules/land_detector/VtolLandDetector.h
+++ b/src/modules/land_detector/VtolLandDetector.h
@@ -74,7 +74,6 @@ private:
 	} _params;
 
 	int _airspeedSub;
-	int _parameterSub;
 
 	struct airspeed_s _airspeed;
 

From 8f1d350d3cafdabd515d09dc6e54122b9a489b1f Mon Sep 17 00:00:00 2001
From: Julian Oes <julian@oes.ch>
Date: Fri, 29 Jul 2016 18:21:59 +0200
Subject: [PATCH 1158/1230] unit_test: add ut_assert_true and ut_assert_false

---
 src/modules/unit_test/unit_test.h | 22 ++++++++++++++++++++++
 1 file changed, 22 insertions(+)

diff --git a/src/modules/unit_test/unit_test.h b/src/modules/unit_test/unit_test.h
index aeddaac88e..42be09c165 100644
--- a/src/modules/unit_test/unit_test.h
+++ b/src/modules/unit_test/unit_test.h
@@ -106,6 +106,28 @@ protected:
 /// @brief Used to assert a value within a unit test.
 #define ut_test(test) ut_assert("test", test)
 
+/// @brief To assert specifically to true.
+#define ut_assert_true(test)						\
+	do {									\
+		if (test != true) {							\
+			_print_assert("result not true", #test, __FILE__, __LINE__);	\
+			return false;						\
+		} else {							\
+			_assertions++;						\
+		}								\
+	} while (0)
+
+/// @brief To assert specifically to true.
+#define ut_assert_false(test)						\
+	do {									\
+		if (test != false) {							\
+			_print_assert("result not false", #test, __FILE__, __LINE__);	\
+			return false;						\
+		} else {							\
+			_assertions++;						\
+		}								\
+	} while (0)
+
 /// @brief Used to compare two integer values within a unit test. If possible use ut_compare instead of ut_assert
 /// since it will give you better error reporting of the actual values being compared.
 #define ut_compare(message, v1, v2)								\

From 0d80a77e461f05846f56f34ad2bb21104dbaa403 Mon Sep 17 00:00:00 2001
From: Julian Oes <julian@oes.ch>
Date: Fri, 29 Jul 2016 18:23:19 +0200
Subject: [PATCH 1159/1230] unit_tests: move hysteresis test to new framework

---
 src/systemcmds/tests/CMakeLists.txt      |   3 +-
 src/systemcmds/tests/test_hysteresis.cpp | 196 +++++++++++++++++++++++
 src/systemcmds/tests/tests.h             |   1 +
 src/systemcmds/tests/tests_main.c        |   1 +
 unittests/CMakeLists.txt                 | 136 +++++-----------
 unittests/hysteresis_test.cpp            | 153 ------------------
 6 files changed, 238 insertions(+), 252 deletions(-)
 create mode 100644 src/systemcmds/tests/test_hysteresis.cpp
 delete mode 100644 unittests/hysteresis_test.cpp

diff --git a/src/systemcmds/tests/CMakeLists.txt b/src/systemcmds/tests/CMakeLists.txt
index 06e6d86225..a391052af3 100644
--- a/src/systemcmds/tests/CMakeLists.txt
+++ b/src/systemcmds/tests/CMakeLists.txt
@@ -34,6 +34,7 @@
 set(srcs
 	test_adc.c
 	test_autodeclination.cpp
+	test_hysteresis.cpp
 	test_bson.c
 	test_conv.cpp
 	test_file.c
@@ -86,4 +87,4 @@ px4_add_module(
 	DEPENDS
 		platforms__common
 	)
-# vim: set noet ft=cmake fenc=utf-8 ff=unix : 
+# vim: set noet ft=cmake fenc=utf-8 ff=unix :
diff --git a/src/systemcmds/tests/test_hysteresis.cpp b/src/systemcmds/tests/test_hysteresis.cpp
new file mode 100644
index 0000000000..6c6dd956e8
--- /dev/null
+++ b/src/systemcmds/tests/test_hysteresis.cpp
@@ -0,0 +1,196 @@
+#include <unit_test/unit_test.h>
+
+#include <systemlib/hysteresis/hysteresis.h>
+
+class HysteresisTest : public UnitTest
+{
+public:
+	virtual bool run_tests(void);
+
+private:
+	bool _init_false();
+	bool _init_true();
+	bool _zero_case();
+	bool _change_after_time();
+	bool _hysteresis_changed();
+	bool _change_after_multiple_sets();
+	bool _take_change_back();
+};
+
+bool HysteresisTest::run_tests()
+{
+	ut_run_test(_init_false);
+	ut_run_test(_init_true);
+	ut_run_test(_zero_case);
+	ut_run_test(_change_after_time);
+	ut_run_test(_hysteresis_changed);
+	ut_run_test(_change_after_multiple_sets);
+	ut_run_test(_take_change_back);
+
+	return (_tests_failed == 0);
+}
+
+bool HysteresisTest::_init_false()
+{
+	systemlib::Hysteresis hysteresis(false);
+	ut_assert_false(hysteresis.get_state());
+
+	return true;
+}
+
+bool HysteresisTest::_init_true()
+{
+	systemlib::Hysteresis hysteresis(true);
+	ut_assert_true(hysteresis.get_state());
+
+	return true;
+}
+
+bool HysteresisTest::_zero_case()
+{
+	// Default is 0 hysteresis.
+	systemlib::Hysteresis hysteresis(false);
+	ut_assert_false(hysteresis.get_state());
+
+	// Change and see result immediately.
+	hysteresis.set_state_and_update(true);
+	ut_assert_true(hysteresis.get_state());
+	hysteresis.set_state_and_update(false);
+	ut_assert_false(hysteresis.get_state());
+	hysteresis.set_state_and_update(true);
+	ut_assert_true(hysteresis.get_state());
+
+	// A wait won't change anything.
+	usleep(1000);
+	hysteresis.update();
+	ut_assert_true(hysteresis.get_state());
+
+	return true;
+}
+
+bool HysteresisTest::_change_after_time()
+{
+	systemlib::Hysteresis hysteresis(false);
+	hysteresis.set_hysteresis_time_from(false, 5000);
+	hysteresis.set_hysteresis_time_from(true, 3000);
+
+	// Change to true.
+	hysteresis.set_state_and_update(true);
+	ut_assert_false(hysteresis.get_state());
+	usleep(4000);
+	hysteresis.update();
+	ut_assert_false(hysteresis.get_state());
+	usleep(2000);
+	hysteresis.update();
+	ut_assert_true(hysteresis.get_state());
+
+	// Change back to false.
+	hysteresis.set_state_and_update(false);
+	ut_assert_true(hysteresis.get_state());
+	usleep(1000);
+	hysteresis.update();
+	ut_assert_true(hysteresis.get_state());
+	usleep(3000);
+	hysteresis.update();
+	ut_assert_false(hysteresis.get_state());
+
+	return true;
+}
+
+bool HysteresisTest::_hysteresis_changed()
+{
+	systemlib::Hysteresis hysteresis(false);
+	hysteresis.set_hysteresis_time_from(true, 2000);
+	hysteresis.set_hysteresis_time_from(false, 5000);
+
+	// Change to true.
+	hysteresis.set_state_and_update(true);
+	ut_assert_false(hysteresis.get_state());
+	usleep(3000);
+	hysteresis.update();
+	ut_assert_false(hysteresis.get_state());
+	usleep(3000);
+	hysteresis.update();
+	ut_assert_true(hysteresis.get_state());
+
+	// Change hysteresis time.
+	hysteresis.set_hysteresis_time_from(true, 10000);
+
+	// Change back to false.
+	hysteresis.set_state_and_update(false);
+	ut_assert_true(hysteresis.get_state());
+	usleep(7000);
+	hysteresis.update();
+	ut_assert_true(hysteresis.get_state());
+	usleep(5000);
+	hysteresis.update();
+	ut_assert_false(hysteresis.get_state());
+
+	return true;
+}
+
+bool HysteresisTest::_change_after_multiple_sets()
+{
+	systemlib::Hysteresis hysteresis(false);
+	hysteresis.set_hysteresis_time_from(true, 5000);
+	hysteresis.set_hysteresis_time_from(false, 5000);
+
+	// Change to true.
+	hysteresis.set_state_and_update(true);
+	ut_assert_false(hysteresis.get_state());
+	usleep(3000);
+	hysteresis.set_state_and_update(true);
+	ut_assert_false(hysteresis.get_state());
+	usleep(3000);
+	hysteresis.set_state_and_update(true);
+	ut_assert_true(hysteresis.get_state());
+
+	// Change to false.
+	hysteresis.set_state_and_update(false);
+	ut_assert_true(hysteresis.get_state());
+	usleep(3000);
+	hysteresis.set_state_and_update(false);
+	ut_assert_true(hysteresis.get_state());
+	usleep(3000);
+	hysteresis.set_state_and_update(false);
+	ut_assert_false(hysteresis.get_state());
+
+	return true;
+}
+
+bool HysteresisTest::_take_change_back()
+{
+	systemlib::Hysteresis hysteresis(false);
+	hysteresis.set_hysteresis_time_from(false, 5000);
+
+	// Change to true.
+	hysteresis.set_state_and_update(true);
+	ut_assert_false(hysteresis.get_state());
+	usleep(3000);
+	hysteresis.update();
+	ut_assert_false(hysteresis.get_state());
+	// Change your mind to false.
+	hysteresis.set_state_and_update(false);
+	ut_assert_false(hysteresis.get_state());
+	usleep(6000);
+	hysteresis.update();
+	ut_assert_false(hysteresis.get_state());
+
+	// And true again
+	hysteresis.set_state_and_update(true);
+	ut_assert_false(hysteresis.get_state());
+	usleep(3000);
+	hysteresis.update();
+	ut_assert_false(hysteresis.get_state());
+	usleep(3000);
+	hysteresis.update();
+	ut_assert_true(hysteresis.get_state());
+
+	// The other directory is immediate.
+	hysteresis.set_state_and_update(false);
+	ut_assert_false(hysteresis.get_state());
+
+	return true;
+}
+
+ut_declare_test_c(test_hysteresis, HysteresisTest)
diff --git a/src/systemcmds/tests/tests.h b/src/systemcmds/tests/tests.h
index 2f5004c36c..80e5d27d6d 100644
--- a/src/systemcmds/tests/tests.h
+++ b/src/systemcmds/tests/tests.h
@@ -74,6 +74,7 @@ __BEGIN_DECLS
 
 extern int	test_adc(int argc, char *argv[]);
 extern int	test_autodeclination(int argc, char *argv[]);
+extern int	test_hysteresis(int argc, char *argv[]);
 extern int	test_bson(int argc, char *argv[]);
 extern int	test_conv(int argc, char *argv[]);
 extern int	test_file(int argc, char *argv[]);
diff --git a/src/systemcmds/tests/tests_main.c b/src/systemcmds/tests/tests_main.c
index a733c4eada..9e17214df0 100644
--- a/src/systemcmds/tests/tests_main.c
+++ b/src/systemcmds/tests/tests_main.c
@@ -97,6 +97,7 @@ const struct {
 	{"uorb",		uorb_tests_main,	0},
 
 	{"autodeclination",		test_autodeclination,	0},
+	{"hysteresis",		test_hysteresis,	0},
 	{"bson",		test_bson,	0},
 	{"conv",		test_conv, 0},
 	{"file",		test_file,	OPT_NOJIGTEST | OPT_NOALLTEST},
diff --git a/unittests/CMakeLists.txt b/unittests/CMakeLists.txt
index c6bf0b90be..9df3adcce3 100644
--- a/unittests/CMakeLists.txt
+++ b/unittests/CMakeLists.txt
@@ -1,9 +1,5 @@
 cmake_minimum_required(VERSION 2.8)
 
-include(CMakeForceCompiler)
-#CMAKE_FORCE_C_COMPILER(clang Clang)
-#CMAKE_FORCE_CXX_COMPILER(clang++ Clang)
-
 if("${CMAKE_C_COMPILER_ID}" STREQUAL "Clang")
 	add_compile_options(-Qunused-arguments )
 endif()
@@ -14,28 +10,16 @@ endif()
 project(unittests)
 enable_testing()
 
-include(CheckCXXCompilerFlag)
-CHECK_CXX_COMPILER_FLAG("-std=c++11" COMPILER_SUPPORTS_CXX11)
-CHECK_CXX_COMPILER_FLAG("-std=c++0x" COMPILER_SUPPORTS_CXX0X)
+set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -Wall -Werror -std=gnu99 -g")
+set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall -Werror -std=c++11 -g -fno-exceptions -fno-rtti -fno-threadsafe-statics")
 
-if(COMPILER_SUPPORTS_CXX11)
-	set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11 -fno-exceptions -fno-rtti -fno-threadsafe-statics -D__CUSTOM_FILE_IO__ -D__PX4_UNIT_TESTS -g -Wall -Werror")
-elseif(COMPILER_SUPPORTS_CXX0X)
-	set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++0x -fno-exceptions -fno-rtti -fno-threadsafe-statics -D__CUSTOM_FILE_IO__ -D__PX4_UNIT_TESTS -g -Wall -Werror")
-else()
-	message(STATUS "The compiler ${CMAKE_CXX_COMPILER} has no C++11 support. Please use a different C++ compiler.")
-endif()
-set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -std=gnu99 -g")
-
-set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -g3 -fsanitize=address -fno-omit-frame-pointer")
-set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wno-sign-compare -Wno-unused-but-set-variable")
 
 set(GTEST_DIR ${CMAKE_SOURCE_DIR}/googletest)
 add_subdirectory(${GTEST_DIR})
 include_directories(${GTEST_DIR}/include)
 
 set(PX4_SRC ${CMAKE_SOURCE_DIR}/../src)
-set(PX4_SITL_BUILD ${PX4_SRC}/../build_posix_sitl_test)
+set(PX4_SITL_BUILD ${PX4_SRC}/../build_posix_sitl_default)
 
 include_directories(${CMAKE_SOURCE_DIR})
 include_directories(${PX4_SITL_BUILD}/src)
@@ -52,43 +36,32 @@ include_directories(${PX4_SRC}/modules/uORB)
 include_directories(${PX4_SRC}/platforms)
 include_directories(${PX4_SRC}/platforms/posix/include)
 include_directories(${PX4_SRC}/platforms/posix/px4_layer)
+include_directories(${PX4_SRC}/platforms/posix/work_queue)
 
+add_definitions(-D__CUSTOM_FILE_IO__)
 add_definitions(-D__EXPORT=)
 add_definitions(-D__PX4_POSIX)
 add_definitions(-D__PX4_TESTS)
+add_definitions(-D__PX4_UNIT_TESTS)
 add_definitions(-D_UNIT_TEST=)
 add_definitions(-DERROR=-1)
 add_definitions(-Dmain_t=int)
 add_definitions(-Dnoreturn_function=)
 add_definitions(-DOK=0)
 
-# check
-add_custom_target(check
-		COMMAND ${CMAKE_CTEST_COMMAND} -j2 --output-on-failure
-		WORKING_DIR ${CMAKE_BINARY_DIR}
-		USES_TERMINAL)
+if(${CMAKE_SYSTEM_NAME} MATCHES "Darwin")
+	add_definitions(-D__PX4_DARWIN)
+else()
+	add_definitions(-D__PX4_LINUX)
+endif()
 
-function(add_gtest)
-	foreach(test_name ${ARGN})
-		if(${CMAKE_SYSTEM_NAME} MATCHES "Darwin")
-			target_link_libraries(${test_name} gtest_main pthread px4_platform)
-			add_definitions(-D__PX4_DARWIN)
-		else()
-			target_link_libraries(${test_name} gtest_main pthread rt px4_platform)
-			add_definitions(-D__PX4_LINUX)
-		endif()
-		add_test(NAME ${test_name} COMMAND ${test_name} WORKING_DIRECTORY ${CMAKE_SOURCE_DIR})
-		add_dependencies(check ${test_name})
-	endforeach()
-endfunction()
-
-add_library(px4_platform
+add_library(px4_platform 
 			${PX4_SITL_BUILD}/src/modules/param/px4_parameters.c
-			${PX4_SRC}/drivers/device/device_posix.cpp
-			${PX4_SRC}/drivers/device/i2c_posix.cpp
+			${PX4_SRC}/drivers/device/device_posix.cpp 
+			${PX4_SRC}/drivers/device/i2c_posix.cpp 
 			${PX4_SRC}/drivers/device/ringbuffer.cpp
-			${PX4_SRC}/drivers/device/sim.cpp
-			${PX4_SRC}/drivers/device/vdev.cpp
+			${PX4_SRC}/drivers/device/sim.cpp 
+			${PX4_SRC}/drivers/device/vdev.cpp 
 			${PX4_SRC}/drivers/device/vdev_posix.cpp
 			${PX4_SRC}/drivers/device/vfile.cpp
 			${PX4_SRC}/platforms/posix/px4_layer/drv_hrt.c
@@ -114,6 +87,28 @@ add_library(px4_platform
 			${PX4_SRC}/platforms/posix/work_queue/work_thread.c
 			)
 target_include_directories(px4_platform PUBLIC ${PX4_SRC}/platforms)
+target_include_directories(px4_platform PUBLIC ${PX4_SRC}/platforms/posix/include)
+target_include_directories(px4_platform PUBLIC ${PX4_SRC}/platforms/posix/work_queue)
+
+
+# check
+add_custom_target(check
+		COMMAND ${CMAKE_CTEST_COMMAND} -j2 --output-on-failure
+		WORKING_DIR ${CMAKE_BINARY_DIR}
+		USES_TERMINAL)
+
+# add_gtest
+function(add_gtest)
+	foreach(test_name ${ARGN})
+		if(${CMAKE_SYSTEM_NAME} MATCHES "Darwin")
+			target_link_libraries(${test_name} gtest_main pthread px4_platform)
+		else()
+			target_link_libraries(${test_name} gtest_main pthread rt px4_platform)
+		endif()
+		add_test(NAME ${test_name} COMMAND ${test_name} WORKING_DIRECTORY ${CMAKE_SOURCE_DIR})
+		add_dependencies(check ${test_name})
+	endforeach()
+endfunction()
 
 
 #######################################################################
@@ -122,64 +117,9 @@ target_include_directories(px4_platform PUBLIC ${PX4_SRC}/platforms)
 #    add_executable(example_test example_test.cpp)
 #    add_gtest(example_test)
 
-
-# autodeclination_test
-add_executable(autodeclination_test autodeclination_test.cpp ${PX4_SRC}/lib/geo_lookup/geo_mag_declination.c)
-add_gtest(autodeclination_test)
-
-# mixer_test
-add_custom_command(OUTPUT ${PX4_SRC}/modules/systemlib/mixer/mixer_multirotor.generated.h
-                   COMMAND ${PX4_SRC}/modules/systemlib/mixer/multi_tables.py > ${PX4_SRC}/modules/systemlib/mixer/mixer_multirotor.generated.h)
-add_executable(mixer_test mixer_test.cpp
-                          ${PX4_SRC}/modules/systemlib/mixer/mixer.cpp
-                          ${PX4_SRC}/modules/systemlib/mixer/mixer_group.cpp
-                          ${PX4_SRC}/modules/systemlib/mixer/mixer_load.c
-                          ${PX4_SRC}/modules/systemlib/mixer/mixer_multirotor.cpp
-                          ${PX4_SRC}/modules/systemlib/mixer/mixer_multirotor.generated.h
-                          ${PX4_SRC}/modules/systemlib/mixer/mixer_simple.cpp
-                          ${PX4_SRC}/modules/systemlib/pwm_limit/pwm_limit.c
-                          ${PX4_SRC}/systemcmds/tests/test_mixer.cpp)
-add_gtest(mixer_test)
-
-# conversion_test
-add_executable(conversion_test conversion_test.cpp ${PX4_SRC}/systemcmds/tests/test_conv.cpp)
-add_gtest(conversion_test)
-
-# sbus2_test
-add_executable(sbus2_test sbus2_test.cpp
-						${PX4_SRC}/lib/rc/sbus.c)
-add_gtest(sbus2_test)
-
-# DSM test
-add_executable(dsm_test dsm_test.cpp
-						${PX4_SRC}/lib/rc/dsm.c)
-add_gtest(dsm_test)
-
-# st24_test
-add_executable(rc_input_test st24_test.cpp sumd_test.cpp
-						${PX4_SRC}/lib/rc/st24.c
-						${PX4_SRC}/lib/rc/sumd.c)
-add_gtest(rc_input_test)
-
-# sf0x_test
-add_executable(sf0x_test sf0x_test.cpp
-						${PX4_SRC}/drivers/sf0x/sf0x_parser.cpp)
-add_gtest(sf0x_test)
-
 # param_test
 add_executable(param_test param_test.cpp uorb_stub.cpp
 						${PX4_SRC}/modules/systemlib/bson/tinybson.c
 						${PX4_SRC}/modules/systemlib/param/param.c)
 target_link_libraries(param_test ${PX4_SITL_BUILD}/libmsg_gen.a)
 add_gtest(param_test)
-
-# hysteresis_test
-add_executable(hysteresis_test hysteresis_test.cpp
-						${PX4_SRC}/modules/systemlib/hysteresis/hysteresis.cpp)
-add_gtest(hysteresis_test)
-
-# param_shmem_test
-#add_executable(param_shmem_test param_test.cpp uorb_stub.cpp
-#						${PX4_SRC}/modules/systemlib/bson/tinybson.c
-#						${PX4_SRC}/modules/systemlib/param/param_shmem.c)
-#add_gtest(param_shmem_test)
diff --git a/unittests/hysteresis_test.cpp b/unittests/hysteresis_test.cpp
deleted file mode 100644
index 99be56f317..0000000000
--- a/unittests/hysteresis_test.cpp
+++ /dev/null
@@ -1,153 +0,0 @@
-#include <systemlib/hysteresis/hysteresis.h>
-
-#include "gtest/gtest.h"
-
-
-TEST(HysteresisTest, InitFalse)
-{
-	systemlib::Hysteresis hysteresis(false);
-	ASSERT_FALSE(hysteresis.get_state());
-}
-
-TEST(HysteresisTest, InitTrue)
-{
-	systemlib::Hysteresis hysteresis(true);
-	ASSERT_TRUE(hysteresis.get_state());
-}
-
-TEST(HysteresisTest, ZeroCase)
-{
-	// Default is 0 hysteresis.
-	systemlib::Hysteresis hysteresis(false);
-	ASSERT_FALSE(hysteresis.get_state());
-
-	// Change and see result immediately.
-	hysteresis.set_state_and_update(true);
-	ASSERT_TRUE(hysteresis.get_state());
-	hysteresis.set_state_and_update(false);
-	ASSERT_FALSE(hysteresis.get_state());
-	hysteresis.set_state_and_update(true);
-	ASSERT_TRUE(hysteresis.get_state());
-
-	// A wait won't change anything.
-	usleep(1000);
-	hysteresis.update();
-	ASSERT_TRUE(hysteresis.get_state());
-}
-
-TEST(HysteresisTest, ChangeAfterTime)
-{
-	systemlib::Hysteresis hysteresis(false);
-	hysteresis.set_hysteresis_time_from(false, 5000);
-	hysteresis.set_hysteresis_time_from(true, 3000);
-
-	// Change to true.
-	hysteresis.set_state_and_update(true);
-	ASSERT_FALSE(hysteresis.get_state());
-	usleep(4000);
-	hysteresis.update();
-	ASSERT_FALSE(hysteresis.get_state());
-	usleep(2000);
-	hysteresis.update();
-	ASSERT_TRUE(hysteresis.get_state());
-
-	// Change back to false.
-	hysteresis.set_state_and_update(false);
-	ASSERT_TRUE(hysteresis.get_state());
-	usleep(1000);
-	hysteresis.update();
-	ASSERT_TRUE(hysteresis.get_state());
-	usleep(3000);
-	hysteresis.update();
-	ASSERT_FALSE(hysteresis.get_state());
-}
-
-TEST(HysteresisTest, HysteresisChanged)
-{
-	systemlib::Hysteresis hysteresis(false);
-	hysteresis.set_hysteresis_time_from(true, 2000);
-	hysteresis.set_hysteresis_time_from(false, 5000);
-
-	// Change to true.
-	hysteresis.set_state_and_update(true);
-	ASSERT_FALSE(hysteresis.get_state());
-	usleep(3000);
-	hysteresis.update();
-	ASSERT_FALSE(hysteresis.get_state());
-	usleep(3000);
-	hysteresis.update();
-	ASSERT_TRUE(hysteresis.get_state());
-
-	// Change hysteresis time.
-	hysteresis.set_hysteresis_time_from(true, 10000);
-
-	// Change back to false.
-	hysteresis.set_state_and_update(false);
-	ASSERT_TRUE(hysteresis.get_state());
-	usleep(7000);
-	hysteresis.update();
-	ASSERT_TRUE(hysteresis.get_state());
-	usleep(5000);
-	hysteresis.update();
-	ASSERT_FALSE(hysteresis.get_state());
-}
-
-TEST(HysteresisTest, ChangeAfterTimeMultipleSets)
-{
-	systemlib::Hysteresis hysteresis(false);
-	hysteresis.set_hysteresis_time_from(true, 5000);
-	hysteresis.set_hysteresis_time_from(false, 5000);
-
-	// Change to true.
-	hysteresis.set_state_and_update(true);
-	ASSERT_FALSE(hysteresis.get_state());
-	usleep(3000);
-	hysteresis.set_state_and_update(true);
-	ASSERT_FALSE(hysteresis.get_state());
-	usleep(3000);
-	hysteresis.set_state_and_update(true);
-	ASSERT_TRUE(hysteresis.get_state());
-
-	// Change to false.
-	hysteresis.set_state_and_update(false);
-	ASSERT_TRUE(hysteresis.get_state());
-	usleep(3000);
-	hysteresis.set_state_and_update(false);
-	ASSERT_TRUE(hysteresis.get_state());
-	usleep(3000);
-	hysteresis.set_state_and_update(false);
-	ASSERT_FALSE(hysteresis.get_state());
-}
-
-TEST(HysteresisTest, TakeChangeBack)
-{
-	systemlib::Hysteresis hysteresis(false);
-	hysteresis.set_hysteresis_time_from(false, 5000);
-
-	// Change to true.
-	hysteresis.set_state_and_update(true);
-	ASSERT_FALSE(hysteresis.get_state());
-	usleep(3000);
-	hysteresis.update();
-	ASSERT_FALSE(hysteresis.get_state());
-	// Change your mind to false.
-	hysteresis.set_state_and_update(false);
-	ASSERT_FALSE(hysteresis.get_state());
-	usleep(6000);
-	hysteresis.update();
-	ASSERT_FALSE(hysteresis.get_state());
-
-	// And true again
-	hysteresis.set_state_and_update(true);
-	ASSERT_FALSE(hysteresis.get_state());
-	usleep(3000);
-	hysteresis.update();
-	ASSERT_FALSE(hysteresis.get_state());
-	usleep(3000);
-	hysteresis.update();
-	ASSERT_TRUE(hysteresis.get_state());
-
-	// The other directory is immediate.
-	hysteresis.set_state_and_update(false);
-	ASSERT_FALSE(hysteresis.get_state());
-}

From c835fb36a6ae5ef6c4941231582612f819fefe86 Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Sun, 31 Jul 2016 11:42:31 +0200
Subject: [PATCH 1160/1230] 5611: Bump sampling frequency back up to 40 Hz from
 25 Hz to help with altitude estimation

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

diff --git a/src/drivers/ms5611/ms5611.cpp b/src/drivers/ms5611/ms5611.cpp
index 40f0d93ad8..300c0aac8f 100644
--- a/src/drivers/ms5611/ms5611.cpp
+++ b/src/drivers/ms5611/ms5611.cpp
@@ -104,7 +104,7 @@ static const int ERROR = -1;
  */
 
 /* internal conversion time: 9.17 ms, so should not be read at rates higher than 100 Hz */
-#define MS5611_CONVERSION_INTERVAL	40000	/* microseconds */
+#define MS5611_CONVERSION_INTERVAL	25000	/* microseconds */
 #define MS5611_MEASUREMENT_RATIO	3	/* pressure measurements per temperature measurement */
 #define MS5611_BARO_DEVICE_PATH_EXT	"/dev/ms5611_ext"
 #define MS5611_BARO_DEVICE_PATH_INT	"/dev/ms5611_int"

From a1c858534232ca242bbf03b5466686c6d99cc2f9 Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Sun, 31 Jul 2016 14:01:32 +0200
Subject: [PATCH 1161/1230] EKF2: Report zero as position until local pos is
 valid

---
 src/modules/ekf2/ekf2_main.cpp | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/src/modules/ekf2/ekf2_main.cpp b/src/modules/ekf2/ekf2_main.cpp
index 352de9430f..a613e18b61 100644
--- a/src/modules/ekf2/ekf2_main.cpp
+++ b/src/modules/ekf2/ekf2_main.cpp
@@ -745,8 +745,8 @@ void Ekf2::task_main()
 
 			// Position of body origin in local NED frame
 			_ekf.get_position(pos);
-			lpos.x = pos[0];
-			lpos.y = pos[1];
+			lpos.x = (_ekf.local_position_is_valid()) ? pos[0] : 0.0f;
+			lpos.y = (_ekf.local_position_is_valid()) ? pos[1] : 0.0f;
 			lpos.z = pos[2];
 
 			// Velocity of body origin in local NED frame (m/s)

From 11f1a119340fccf58ae941482f4864afa3155eb7 Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Fri, 29 Jul 2016 13:21:14 +0200
Subject: [PATCH 1162/1230] Lock yaw integral if we hit a yaw limit

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

diff --git a/src/modules/mc_att_control/mc_att_control_main.cpp b/src/modules/mc_att_control/mc_att_control_main.cpp
index 1d6e9609fc..695db0a668 100644
--- a/src/modules/mc_att_control/mc_att_control_main.cpp
+++ b/src/modules/mc_att_control/mc_att_control_main.cpp
@@ -102,6 +102,11 @@ extern "C" __EXPORT int mc_att_control_main(int argc, char *argv[]);
 #define MANUAL_THROTTLE_MAX_MULTICOPTER	0.9f
 #define ATTITUDE_TC_DEFAULT 0.2f
 
+#define AXIS_INDEX_ROLL 0
+#define AXIS_INDEX_PITCH 1
+#define AXIS_INDEX_YAW 2
+#define AXIS_COUNT 3
+
 class MulticopterAttitudeControl
 {
 public:
@@ -774,12 +779,14 @@ MulticopterAttitudeControl::control_attitude_rates(float dt)
 
 	/* update integral only if not saturated on low limit and if motor commands are not saturated */
 	if (_thrust_sp > MIN_TAKEOFF_THRUST && !_motor_limits.lower_limit && !_motor_limits.upper_limit) {
-		for (int i = 0; i < 3; i++) {
+		for (int i = AXIS_INDEX_ROLL; i < AXIS_COUNT; i++) {
 			if (fabsf(_att_control(i)) < _thrust_sp) {
 				float rate_i = _rates_int(i) + _params.rate_i(i) * rates_err(i) * dt;
 
 				if (PX4_ISFINITE(rate_i) && rate_i > -RATES_I_LIMIT && rate_i < RATES_I_LIMIT &&
-				    _att_control(i) > -RATES_I_LIMIT && _att_control(i) < RATES_I_LIMIT) {
+				    _att_control(i) > -RATES_I_LIMIT && _att_control(i) < RATES_I_LIMIT &&
+				    /* if the axis is the yaw axis, do not update the integral if the limit is hit */
+				    !((i == AXIS_INDEX_YAW) && _motor_limits.yaw)) {
 					_rates_int(i) = rate_i;
 				}
 			}

From 4e81263d3a8aed72dce93c303997e91249de2c0c Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Sun, 31 Jul 2016 15:56:35 +0200
Subject: [PATCH 1163/1230] Updated Tools/sitl_gazebo

---
 Tools/sitl_gazebo | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/Tools/sitl_gazebo b/Tools/sitl_gazebo
index 1a2c61b759..1848c42b83 160000
--- a/Tools/sitl_gazebo
+++ b/Tools/sitl_gazebo
@@ -1 +1 @@
-Subproject commit 1a2c61b7598920b3396fb4d8ed544befe3d7cc2f
+Subproject commit 1848c42b830ee336b15ff531c0e7fece49301030

From 21bc5d171629a6e853c73a4b44a167bfe0f3e690 Mon Sep 17 00:00:00 2001
From: Julian Oes <julian@oes.ch>
Date: Sun, 31 Jul 2016 16:49:11 +0200
Subject: [PATCH 1164/1230] land_detector: remove leftover printf (#5178)

---
 src/modules/land_detector/LandDetector.cpp | 1 -
 1 file changed, 1 deletion(-)

diff --git a/src/modules/land_detector/LandDetector.cpp b/src/modules/land_detector/LandDetector.cpp
index abceeddfe4..5ed6961b59 100644
--- a/src/modules/land_detector/LandDetector.cpp
+++ b/src/modules/land_detector/LandDetector.cpp
@@ -94,7 +94,6 @@ LandDetector::_cycle_trampoline(void *arg)
 void LandDetector::_cycle()
 {
 	if (!_taskIsRunning) {
-		PX4_INFO("we are doing the init");
 		// Advertise the first land detected uORB.
 		_landDetected.timestamp = hrt_absolute_time();
 		_landDetected.landed = false;

From d809faec6389b4d34b8df6125125e0e36df74fda Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Sun, 31 Jul 2016 16:52:40 +0200
Subject: [PATCH 1165/1230] Fixes for Gazebo

---
 Tools/sitl_gazebo | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/Tools/sitl_gazebo b/Tools/sitl_gazebo
index 1848c42b83..928ada4739 160000
--- a/Tools/sitl_gazebo
+++ b/Tools/sitl_gazebo
@@ -1 +1 @@
-Subproject commit 1848c42b830ee336b15ff531c0e7fece49301030
+Subproject commit 928ada473931ebc137ac153882da4b0abb1c85d5

From dd70b3752a212796beedb5cfa17d894b4400ac9d Mon Sep 17 00:00:00 2001
From: Julian Oes <julian@oes.ch>
Date: Sun, 31 Jul 2016 17:04:01 +0200
Subject: [PATCH 1166/1230] mpu9250: set accel DLPF to 41 Hz (#5177)

Previously, the accel DLPF was not set, so it's not clear what the
settings are or should be.
---
 src/drivers/mpu9250/mpu9250.cpp | 5 +++++
 1 file changed, 5 insertions(+)

diff --git a/src/drivers/mpu9250/mpu9250.cpp b/src/drivers/mpu9250/mpu9250.cpp
index e9bc3e2462..02a21f52fd 100644
--- a/src/drivers/mpu9250/mpu9250.cpp
+++ b/src/drivers/mpu9250/mpu9250.cpp
@@ -169,6 +169,8 @@
 #define BITS_DLPF_CFG_3600HZ		0x07
 #define BITS_DLPF_CFG_MASK		0x07
 
+#define BITS_ACCEL_CONFIG2_41HZ		0x03
+
 #define BIT_RAW_RDY_EN			0x01
 #define BIT_INT_ANYRD_2CLEAR		0x10
 
@@ -471,6 +473,9 @@ int MPU9250::reset()
 	write_checked_reg(MPUREG_INT_PIN_CFG, BIT_INT_ANYRD_2CLEAR); // INT: Clear on any read
 	usleep(1000);
 
+	write_checked_reg(MPUREG_ACCEL_CONFIG2, BITS_ACCEL_CONFIG2_41HZ);
+	usleep(1000);
+
 	uint8_t retries = 10;
 
 	while (retries--) {

From a38263b8e7062c2d3240058e919e071e1a2e971d Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Sun, 31 Jul 2016 17:42:53 +0200
Subject: [PATCH 1167/1230] Enforce enough braking power when users set higher
 horizontal speeds

---
 src/modules/mc_pos_control/mc_pos_control_main.cpp | 5 +++++
 src/modules/mc_pos_control/mc_pos_control_params.c | 2 +-
 2 files changed, 6 insertions(+), 1 deletion(-)

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 bc5d7a3bfc..0f3a453a2f 100644
--- a/src/modules/mc_pos_control/mc_pos_control_main.cpp
+++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp
@@ -585,6 +585,11 @@ MulticopterPositionControl::parameters_update(bool force)
 		_params.hold_max_z = (v < 0.0f ? 0.0f : v);
 		param_get(_params_handles.acc_hor_max, &v);
 		_params.acc_hor_max = v;
+		/*
+		 * increase the maximum horizontal acceleration such that stopping
+		 * within 1 s from full speed is feasible
+		 */
+		_params.acc_hor_max = math::max(_params.vel_cruise(0), _params.acc_hor_max);
 		param_get(_params_handles.alt_mode, &v_i);
 		_params.alt_mode = v_i;
 
diff --git a/src/modules/mc_pos_control/mc_pos_control_params.c b/src/modules/mc_pos_control/mc_pos_control_params.c
index 4429eea405..2d30adc1cf 100644
--- a/src/modules/mc_pos_control/mc_pos_control_params.c
+++ b/src/modules/mc_pos_control/mc_pos_control_params.c
@@ -456,7 +456,7 @@ PARAM_DEFINE_FLOAT(MPC_VELD_LP, 5.0f);
  * @decimal 2
  * @group Multicopter Position Control
  */
-PARAM_DEFINE_FLOAT(MPC_ACC_HOR_MAX, 10.0f);
+PARAM_DEFINE_FLOAT(MPC_ACC_HOR_MAX, 5.0f);
 
 /**
  * Altitude control mode, note mode 1 only tested with LPE

From 5710a0c0405632b0fc3859b8df49d71ea1f6cdd4 Mon Sep 17 00:00:00 2001
From: sander <sander@droneslab.com>
Date: Sun, 31 Jul 2016 22:09:44 +0200
Subject: [PATCH 1168/1230] Remove default geofence from gazebo_standard_vtol

---
 posix-configs/SITL/init/rcS_gazebo_standard_vtol | 3 ---
 1 file changed, 3 deletions(-)

diff --git a/posix-configs/SITL/init/rcS_gazebo_standard_vtol b/posix-configs/SITL/init/rcS_gazebo_standard_vtol
index 7f6c8bab34..71420634da 100644
--- a/posix-configs/SITL/init/rcS_gazebo_standard_vtol
+++ b/posix-configs/SITL/init/rcS_gazebo_standard_vtol
@@ -44,9 +44,6 @@ param set RTL_LAND_DELAY 0
 param set COM_DISARM_LAND 5
 param set COM_DL_LOSS_EN 1
 param set MPC_ACC_HOR_MAX 2
-param set GF_ACTION 3
-param set GF_MAX_HOR_DIST 500
-param set GF_MAX_VER_DIST 500
 replay tryapplyparams
 simulator start -s 
 rgbledsim start

From 623b99327d0ac9d47afb6562a071ca120750346f Mon Sep 17 00:00:00 2001
From: Julian Oes <julian@oes.ch>
Date: Mon, 1 Aug 2016 11:15:21 +0200
Subject: [PATCH 1169/1230] param: lock the bus as short as possible (#5187)

Since the FRAM and the baro are on the same bus on the Pixracer, we
currently need to lock down everything (instead of just this SPI bus)
for the time when the params are written.
Therefore, we need to keep this locking as short as possible.

This change makes the locking even shorter by moving all param_get and
param_name and param_size calls out of the lock.
---
 src/modules/systemlib/param/param.c | 68 +++++++++++++++++------------
 1 file changed, 41 insertions(+), 27 deletions(-)

diff --git a/src/modules/systemlib/param/param.c b/src/modules/systemlib/param/param.c
index 265c5f7e14..2033fe36b4 100644
--- a/src/modules/systemlib/param/param.c
+++ b/src/modules/systemlib/param/param.c
@@ -895,49 +895,63 @@ param_export(int fd, bool only_unsaved)
 
 		/* append the appropriate BSON type object */
 
-		/* lock as short as possible */
-		param_bus_lock(true);
 
 		switch (param_type(s->param)) {
 
-		case PARAM_TYPE_INT32:
-			param_get(s->param, &i);
+		case PARAM_TYPE_INT32: {
+				param_get(s->param, &i);
+				const char *name = param_name(s->param);
 
-			if (bson_encoder_append_int(&encoder, param_name(s->param), i)) {
-				debug("BSON append failed for '%s'", param_name(s->param));
-				param_bus_lock(false);
-				goto out;
+				/* lock as short as possible */
+				param_bus_lock(true);
+
+				if (bson_encoder_append_int(&encoder, name, i)) {
+					param_bus_lock(false);
+					debug("BSON append failed for '%s'", name);
+					goto out;
+				}
 			}
-
 			break;
 
-		case PARAM_TYPE_FLOAT:
-			param_get(s->param, &f);
+		case PARAM_TYPE_FLOAT: {
 
-			if (bson_encoder_append_double(&encoder, param_name(s->param), f)) {
-				debug("BSON append failed for '%s'", param_name(s->param));
-				param_bus_lock(false);
-				goto out;
+				param_get(s->param, &f);
+				const char *name = param_name(s->param);
+
+				/* lock as short as possible */
+				param_bus_lock(true);
+
+				if (bson_encoder_append_double(&encoder, name, f)) {
+					param_bus_lock(false);
+					debug("BSON append failed for '%s'", name);
+					goto out;
+				}
 			}
-
 			break;
 
-		case PARAM_TYPE_STRUCT ... PARAM_TYPE_STRUCT_MAX:
-			if (bson_encoder_append_binary(&encoder,
-						       param_name(s->param),
-						       BSON_BIN_BINARY,
-						       param_size(s->param),
-						       param_get_value_ptr(s->param))) {
-				debug("BSON append failed for '%s'", param_name(s->param));
-				param_bus_lock(false);
-				goto out;
-			}
+		case PARAM_TYPE_STRUCT ... PARAM_TYPE_STRUCT_MAX: {
 
+				const char *name = param_name(s->param);
+				const size_t size = param_size(s->param);
+				const void *value_ptr = param_get_value_ptr(s->param);
+
+				/* lock as short as possible */
+				param_bus_lock(true);
+
+				if (bson_encoder_append_binary(&encoder,
+							       name,
+							       BSON_BIN_BINARY,
+							       size,
+							       value_ptr)) {
+					param_bus_lock(false);
+					debug("BSON append failed for '%s'", name);
+					goto out;
+				}
+			}
 			break;
 
 		default:
 			debug("unrecognized parameter type");
-			param_bus_lock(false);
 			goto out;
 		}
 

From ccea9c9e6f3db3fe414d27d350145dcbaa023db3 Mon Sep 17 00:00:00 2001
From: Julian Oes <julian@oes.ch>
Date: Mon, 1 Aug 2016 11:18:36 +0200
Subject: [PATCH 1170/1230] fmu: flash safety button correctly even disabled
 (#5189)

If the circuit breaker to disable IO safety is engaged, the safety
button should still blink in the appropriate pattern: double flash for
safety off, solid on for armed.
---
 src/drivers/px4fmu/fmu.cpp | 16 +++++++++++++---
 1 file changed, 13 insertions(+), 3 deletions(-)

diff --git a/src/drivers/px4fmu/fmu.cpp b/src/drivers/px4fmu/fmu.cpp
index 691f31d971..abb3a36898 100644
--- a/src/drivers/px4fmu/fmu.cpp
+++ b/src/drivers/px4fmu/fmu.cpp
@@ -276,6 +276,7 @@ private:
 	void rc_io_invert();
 	void rc_io_invert(bool invert);
 	void safety_check_button(void);
+	void flash_safety_button(void);
 };
 
 const PX4FMU::GPIOConfig PX4FMU::_gpio_tab[] =	BOARD_FMU_GPIO_TAB;
@@ -465,7 +466,15 @@ PX4FMU:: safety_check_button(void)
 		counter = 0;
 	}
 
-	/* Select the appropriate LED flash pattern depending on the current IO/FMU arm state */
+#endif
+}
+
+void
+PX4FMU::flash_safety_button()
+{
+#ifdef GPIO_BTN_SAFETY
+
+	/* Select the appropriate LED flash pattern depending on the current arm state */
 	uint16_t pattern = LED_PATTERN_FMU_REFUSE_TO_ARM;
 
 	/* cycle the blink state machine at 10Hz */
@@ -1083,8 +1092,6 @@ PX4FMU::cycle()
 		struct safety_s safety = {};
 
 		if (_safety_disabled) {
-			/* safety switch disabled, turn LED on solid */
-			px4_arch_gpiowrite(GPIO_LED_SAFETY, 0);
 			_safety_off = true;
 
 		} else {
@@ -1092,6 +1099,9 @@ PX4FMU::cycle()
 			safety_check_button();
 		}
 
+		/* Make the safety button flash anyway, no matter if it's used or not. */
+		flash_safety_button();
+
 		safety.timestamp = hrt_absolute_time();
 
 		if (_safety_off) {

From a58c2f416f2e351fdc3d6600361decb84ece7ab3 Mon Sep 17 00:00:00 2001
From: James Goppert <jgoppert@users.noreply.github.com>
Date: Mon, 1 Aug 2016 15:30:31 -0400
Subject: [PATCH 1171/1230] Update sitl_gazebo. (#5195)

---
 Tools/sitl_gazebo | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/Tools/sitl_gazebo b/Tools/sitl_gazebo
index 928ada4739..7d7a26d968 160000
--- a/Tools/sitl_gazebo
+++ b/Tools/sitl_gazebo
@@ -1 +1 @@
-Subproject commit 928ada473931ebc137ac153882da4b0abb1c85d5
+Subproject commit 7d7a26d968d58d0e66cb6bdad5ac41a513ec181a

From 457526ebe7c27190c49ba8f477283ecd28b8d8f0 Mon Sep 17 00:00:00 2001
From: Vasily Evseenko <svpcom@gmail.com>
Date: Tue, 2 Aug 2016 10:30:40 +0400
Subject: [PATCH 1172/1230] Don't push bad values from lidar to EKF2 (#5196)

Report terrain altitude
---
 src/modules/ekf2/ekf2_main.cpp | 14 +++++++++++---
 1 file changed, 11 insertions(+), 3 deletions(-)

diff --git a/src/modules/ekf2/ekf2_main.cpp b/src/modules/ekf2/ekf2_main.cpp
index a613e18b61..67d12ef0a6 100644
--- a/src/modules/ekf2/ekf2_main.cpp
+++ b/src/modules/ekf2/ekf2_main.cpp
@@ -486,6 +486,9 @@ void Ekf2::task_main()
 
 		if (range_finder_updated) {
 			orb_copy(ORB_ID(distance_sensor), _range_finder_sub, &range_finder);
+			if (range_finder.min_distance >= range_finder.current_distance || range_finder.max_distance <= range_finder.current_distance) {
+				range_finder_updated = false;
+			}
 		}
 
 		orb_check(_ev_pos_sub, &vision_position_updated);
@@ -816,9 +819,14 @@ void Ekf2::task_main()
 				global_pos.eph = sqrt(pos_var(0) + pos_var(1));; // Standard deviation of position estimate horizontally
 				global_pos.epv = sqrt(pos_var(2)); // Standard deviation of position vertically
 
-				// TODO: implement terrain estimator
-				global_pos.terrain_alt = 0.0f; // Terrain altitude in m, WGS84
-				global_pos.terrain_alt_valid = false; // Terrain altitude estimate is valid
+				if (lpos.dist_bottom_valid) {
+					global_pos.terrain_alt = lpos.ref_alt - terrain_vpos; // Terrain altitude in m, WGS84
+					global_pos.terrain_alt_valid = true; // Terrain altitude estimate is valid
+	                        } else {
+					global_pos.terrain_alt = 0.0f; // Terrain altitude in m, WGS84
+					global_pos.terrain_alt_valid = false; // Terrain altitude estimate is valid
+				}
+
 				// TODO use innovatun consistency check timouts to set this
 				global_pos.dead_reckoning = false; // True if this position is estimated through dead-reckoning
 

From aefa319fc4538b70b29fc39690ad69cedc2b0962 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Beat=20K=C3=BCng?= <beat-kueng@gmx.net>
Date: Tue, 2 Aug 2016 09:04:44 +0200
Subject: [PATCH 1173/1230] fw_pos_control_l1: fix compiler problem (implicit
 float conversion) (#5198)
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

issue (GCC 6.1.1):
../src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp:1284:27: error: implicit conversion from ‘float’ to ‘double’ to match other operand of binary expression [-Werror=double-promotion]
  if ((fabs(air_gnd_angle) > M_PI) || (ground_speed_2d.length() < 3.0f)) {
---
 src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

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 572d4c1847..d956bfcdfd 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
@@ -1281,7 +1281,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
 	float air_gnd_angle = acosf((air_speed_2d * ground_speed_2d) / (air_speed_2d.length() * ground_speed_2d.length()));
 
 	// if angle > 90 degrees or groundspeed is less than threshold, replace groundspeed with airspeed projection
-	if ((fabs(air_gnd_angle) > M_PI) || (ground_speed_2d.length() < 3.0f)) {
+	if ((fabsf(air_gnd_angle) > (float)M_PI) || (ground_speed_2d.length() < 3.0f)) {
 		nav_speed_2d = air_speed_2d;
 
 	} else {

From c22e7ed5c980df5d386d068a4c643942c9166cd2 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Beat=20K=C3=BCng?= <beat-kueng@gmx.net>
Date: Tue, 2 Aug 2016 08:16:36 +0200
Subject: [PATCH 1174/1230] Tools/mavlink_shell.py: implement a simple shell
 with history

---
 Tools/mavlink_shell.py | 78 +++++++++++++++++++++++++++++++++++-------
 1 file changed, 65 insertions(+), 13 deletions(-)

diff --git a/Tools/mavlink_shell.py b/Tools/mavlink_shell.py
index fc77ad6a22..64ce2bdf14 100755
--- a/Tools/mavlink_shell.py
+++ b/Tools/mavlink_shell.py
@@ -9,6 +9,7 @@ Open a shell over MAVLink.
 
 from __future__ import print_function
 import sys, select
+import termios
 
 try:
     from pymavlink import mavutil
@@ -118,24 +119,72 @@ def main():
 
     mav_serialport.write('\n') # make sure the shell is started
 
+    # setup the console, so we can read one char at a time
+    fd_in = sys.stdin.fileno()
+    old_attr = termios.tcgetattr(fd_in)
+    new_attr = termios.tcgetattr(fd_in)
+    new_attr[3] = new_attr[3] & ~termios.ECHO # lflags
+    new_attr[3] = new_attr[3] & ~termios.ICANON
+
     try:
+        termios.tcsetattr(fd_in, termios.TCSANOW, new_attr)
+        cur_line = ''
+        command_history = []
+        cur_history_index = 0
+
+        def erase_last_n_chars(N):
+            if N == 0: return
+            CURSOR_BACK_N = '\x1b['+str(N)+'D'
+            ERASE_END_LINE = '\x1b[K'
+            sys.stdout.write(CURSOR_BACK_N + ERASE_END_LINE)
+
         while True:
-            i, o, e = select.select([sys.stdin], [], [], 0.001)
-            if (i):
-                data = sys.stdin.readline()
+            while True:
+                i, o, e = select.select([sys.stdin], [], [], 0)
+                if not i: break
+                ch = sys.stdin.read(1)
 
-                # Erase stdin output (otherwise we have it twice)
-                # FIXME: this assumes a prompt of 5 chars, and does not work in all cases
-                CURSOR_UP_ONE = '\x1b[1A'
-                CURSOR_RIGHT = '\x1b[5C'
-                ERASE_LINE = '\x1b[2K'
-                ERASE_END_LINE = '\x1b[K'
-                sys.stdout.write(CURSOR_UP_ONE + CURSOR_RIGHT + ERASE_END_LINE)
+                # provide a simple shell with command history
+                if ch == '\n':
+                    if len(cur_line) > 0:
+                        # erase current text (mavlink shell will echo it as well)
+                        erase_last_n_chars(len(cur_line))
 
-                # TODO: add a command history?
+                        # add to history
+                        if len(command_history) == 0 or command_history[-1] != cur_line:
+                            command_history.append(cur_line)
+                            if len(command_history) > 50:
+                                del command_history[0]
+                        cur_history_index = len(command_history)
+                    mav_serialport.write(cur_line+'\n')
+                    cur_line = ''
+                elif ord(ch) == 127: # backslash
+                    if len(cur_line) > 0:
+                        erase_last_n_chars(1)
+                        cur_line = cur_line[:-1]
+                        sys.stdout.write(ch)
+                elif ord(ch) == 033:
+                    ch = sys.stdin.read(1) # skip one
+                    ch = sys.stdin.read(1)
+                    if ch == 'A': # arrow up
+                        if cur_history_index > 0:
+                            cur_history_index -= 1
+                    elif ch == 'B': # arrow down
+                        if cur_history_index < len(command_history):
+                            cur_history_index += 1
+                    # TODO: else: support line editing
 
-                #sys.stdout.write(data)
-                mav_serialport.write(data)
+                    erase_last_n_chars(len(cur_line))
+                    if cur_history_index == len(command_history):
+                        cur_line = ''
+                    else:
+                        cur_line = command_history[cur_history_index]
+                    sys.stdout.write(cur_line)
+
+                elif ord(ch) > 3:
+                    cur_line += ch
+                    sys.stdout.write(ch)
+                sys.stdout.flush()
 
             data = mav_serialport.read(4096)
             if data and len(data) > 0:
@@ -148,6 +197,9 @@ def main():
     except KeyboardInterrupt:
         mav_serialport.close()
 
+    finally:
+        termios.tcsetattr(fd_in, termios.TCSADRAIN, old_attr)
+
 
 if __name__ == '__main__':
     main()

From 437221bec2d93791f2fff82d5dc57250fe892ba4 Mon Sep 17 00:00:00 2001
From: James Goppert <jgoppert@users.noreply.github.com>
Date: Tue, 2 Aug 2016 05:02:01 -0400
Subject: [PATCH 1175/1230] Make LPE default estimator. (#4483)

---
 src/modules/systemlib/system_params.c | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/src/modules/systemlib/system_params.c b/src/modules/systemlib/system_params.c
index 6072b7f2c5..f6ea3d045b 100644
--- a/src/modules/systemlib/system_params.c
+++ b/src/modules/systemlib/system_params.c
@@ -104,7 +104,7 @@ PARAM_DEFINE_INT32(SYS_RESTART_TYPE, 2);
  * @reboot_required true
  * @group System
  */
-PARAM_DEFINE_INT32(SYS_MC_EST_GROUP, 0);
+PARAM_DEFINE_INT32(SYS_MC_EST_GROUP, 1);
 
 /**
  * TELEM2 as companion computer link

From 670b0f7c6ddb4aa93754878271facea71ba314ac Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lorenz@px4.io>
Date: Tue, 2 Aug 2016 14:54:16 +0200
Subject: [PATCH 1176/1230] Commander: MAVLink is an off-vehicle API we should
 not depend internally on

---
 src/modules/commander/commander_helper.cpp | 42 ++++++++++++++--------
 1 file changed, 27 insertions(+), 15 deletions(-)

diff --git a/src/modules/commander/commander_helper.cpp b/src/modules/commander/commander_helper.cpp
index 0fbee034f7..8bd5cc3324 100644
--- a/src/modules/commander/commander_helper.cpp
+++ b/src/modules/commander/commander_helper.cpp
@@ -62,8 +62,6 @@
 #include <drivers/drv_led.h>
 #include <drivers/drv_rgbled.h>
 
-#include <v2.0/common/mavlink.h> // For MAV_TYPE
-
 #include "commander_helper.h"
 #include "DevMgr.hpp"
 
@@ -75,30 +73,44 @@ using namespace DriverFramework;
 #endif
 static const int ERROR = -1;
 
+#define VEHICLE_TYPE_QUADROTOR 2
+#define VEHICLE_TYPE_COAXIAL 3
+#define VEHICLE_TYPE_HELICOPTER 4
+#define VEHICLE_TYPE_HEXAROTOR 13
+#define VEHICLE_TYPE_OCTOROTOR 14
+#define VEHICLE_TYPE_TRICOPTER 15
+#define VEHICLE_TYPE_VTOL_DUOROTOR 19
+#define VEHICLE_TYPE_VTOL_QUADROTOR 20
+#define VEHICLE_TYPE_VTOL_TILTROTOR 21
+#define VEHICLE_TYPE_VTOL_RESERVED2 22
+#define VEHICLE_TYPE_VTOL_RESERVED3 23
+#define VEHICLE_TYPE_VTOL_RESERVED4 24
+#define VEHICLE_TYPE_VTOL_RESERVED5 25
+
 #define BLINK_MSG_TIME	700000	// 3 fast blinks
 
 bool is_multirotor(const struct vehicle_status_s *current_status)
 {
-	return ((current_status->system_type == MAV_TYPE_QUADROTOR) ||
-		(current_status->system_type == MAV_TYPE_HEXAROTOR) ||
-		(current_status->system_type == MAV_TYPE_OCTOROTOR) ||
-		(current_status->system_type == MAV_TYPE_TRICOPTER));
+	return ((current_status->system_type == VEHICLE_TYPE_QUADROTOR) ||
+		(current_status->system_type == VEHICLE_TYPE_HEXAROTOR) ||
+		(current_status->system_type == VEHICLE_TYPE_OCTOROTOR) ||
+		(current_status->system_type == VEHICLE_TYPE_TRICOPTER));
 }
 
 bool is_rotary_wing(const struct vehicle_status_s *current_status)
 {
-	return is_multirotor(current_status) || (current_status->system_type == MAV_TYPE_HELICOPTER)
-		   || (current_status->system_type == MAV_TYPE_COAXIAL);
+	return is_multirotor(current_status) || (current_status->system_type == VEHICLE_TYPE_HELICOPTER)
+		   || (current_status->system_type == VEHICLE_TYPE_COAXIAL);
 }
 
 bool is_vtol(const struct vehicle_status_s * current_status) {
-	return (current_status->system_type == MAV_TYPE_VTOL_DUOROTOR ||
-		current_status->system_type == MAV_TYPE_VTOL_QUADROTOR ||
-		current_status->system_type == MAV_TYPE_VTOL_TILTROTOR ||
-		current_status->system_type == MAV_TYPE_VTOL_RESERVED2 ||
-		current_status->system_type == MAV_TYPE_VTOL_RESERVED3 ||
-		current_status->system_type == MAV_TYPE_VTOL_RESERVED4 ||
-		current_status->system_type == MAV_TYPE_VTOL_RESERVED5);
+	return (current_status->system_type == VEHICLE_TYPE_VTOL_DUOROTOR ||
+		current_status->system_type == VEHICLE_TYPE_VTOL_QUADROTOR ||
+		current_status->system_type == VEHICLE_TYPE_VTOL_TILTROTOR ||
+		current_status->system_type == VEHICLE_TYPE_VTOL_RESERVED2 ||
+		current_status->system_type == VEHICLE_TYPE_VTOL_RESERVED3 ||
+		current_status->system_type == VEHICLE_TYPE_VTOL_RESERVED4 ||
+		current_status->system_type == VEHICLE_TYPE_VTOL_RESERVED5);
 }
 
 static hrt_abstime blink_msg_end = 0;	// end time for currently blinking LED message, 0 if no blink message

From e2d1524a7a4b8ff1d3ae645f1830f3cdac45fd75 Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lorenz@px4.io>
Date: Tue, 2 Aug 2016 14:54:40 +0200
Subject: [PATCH 1177/1230] Mission feasibility checker: Better feedback for
 landing waypoints

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

diff --git a/src/modules/navigator/mission_feasibility_checker.cpp b/src/modules/navigator/mission_feasibility_checker.cpp
index 720778770b..8cbb3ef599 100644
--- a/src/modules/navigator/mission_feasibility_checker.cpp
+++ b/src/modules/navigator/mission_feasibility_checker.cpp
@@ -324,11 +324,11 @@ bool MissionFeasibilityChecker::checkFixedWingLanding(dm_item_t dm_current, size
 				} else {
 					/* Last wp is in flare region */
 					//xxx give recommendations
-					mavlink_log_critical(_mavlink_log_pub, "Warning: Landing: last waypoint in flare region");
+					mavlink_log_critical(_mavlink_log_pub, "Last waypoint too close to landing waypoint");
 					return false;
 				}
 			} else {
-				mavlink_log_critical(_mavlink_log_pub, "Warning: starting with land waypoint");
+				mavlink_log_critical(_mavlink_log_pub, "Invalid mission: starts with land waypoint");
 				return false;
 			}
 		}

From 033e9e140d0fb38d4cf85441682a65fcae0723f8 Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lorenz@px4.io>
Date: Tue, 2 Aug 2016 14:55:04 +0200
Subject: [PATCH 1178/1230] Update MAVLink 2.0 version

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

diff --git a/mavlink/include/mavlink/v2.0 b/mavlink/include/mavlink/v2.0
index 1e2920c68d..17231c3777 160000
--- a/mavlink/include/mavlink/v2.0
+++ b/mavlink/include/mavlink/v2.0
@@ -1 +1 @@
-Subproject commit 1e2920c68df46c25b7a14ca6cc47a478c6081d81
+Subproject commit 17231c377749da84cffdf16cda74e15dd1a48f7e

From bae54168197d12bfce80d025bd95139b318ffa3b Mon Sep 17 00:00:00 2001
From: Andreas Antener <antener_a@gmx.ch>
Date: Tue, 2 Aug 2016 21:05:00 +0200
Subject: [PATCH 1179/1230] removed duplicate memset

---
 src/modules/vtol_att_control/vtol_att_control_main.cpp | 1 -
 1 file changed, 1 deletion(-)

diff --git a/src/modules/vtol_att_control/vtol_att_control_main.cpp b/src/modules/vtol_att_control/vtol_att_control_main.cpp
index 06a37c67c5..555563fbfe 100644
--- a/src/modules/vtol_att_control/vtol_att_control_main.cpp
+++ b/src/modules/vtol_att_control/vtol_att_control_main.cpp
@@ -104,7 +104,6 @@ VtolAttitudeControl::VtolAttitudeControl() :
 	memset(&_fw_virtual_v_rates_sp, 0, sizeof(_fw_virtual_v_rates_sp));
 	memset(&_manual_control_sp, 0, sizeof(_manual_control_sp));
 	memset(&_v_control_mode, 0, sizeof(_v_control_mode));
-	memset(&_vtol_vehicle_status, 0, sizeof(_vtol_vehicle_status));
 	memset(&_actuators_out_0, 0, sizeof(_actuators_out_0));
 	memset(&_actuators_out_1, 0, sizeof(_actuators_out_1));
 	memset(&_actuators_mc_in, 0, sizeof(_actuators_mc_in));

From 45a4472a47a519ab25e63821aacde34320310112 Mon Sep 17 00:00:00 2001
From: sander <sander@droneslab.com>
Date: Sun, 31 Jul 2016 22:04:37 +0200
Subject: [PATCH 1180/1230] Do not apply back transition duration for QuadChute

---
 src/modules/vtol_att_control/standard.cpp | 14 +++++++++++---
 1 file changed, 11 insertions(+), 3 deletions(-)

diff --git a/src/modules/vtol_att_control/standard.cpp b/src/modules/vtol_att_control/standard.cpp
index e54e0a5bb5..1184d594eb 100644
--- a/src/modules/vtol_att_control/standard.cpp
+++ b/src/modules/vtol_att_control/standard.cpp
@@ -146,9 +146,17 @@ void Standard::update_vtol_state()
 
 		} else if (_vtol_schedule.flight_mode == FW_MODE) {
 			// transition to mc mode
-			_vtol_schedule.flight_mode = TRANSITION_TO_MC;
-			_flag_enable_mc_motors = true;
-			_vtol_schedule.transition_start = hrt_absolute_time();
+			if (_vtol_vehicle_status->vtol_transition_failsafe == true){
+				// Failsafe event, engage mc motors immediately
+				_vtol_schedule.flight_mode = MC_MODE;
+				_flag_enable_mc_motors = true;
+
+			} else {
+				// Regular backtransition
+				_vtol_schedule.flight_mode = TRANSITION_TO_MC;
+				_flag_enable_mc_motors = true;
+				_vtol_schedule.transition_start = hrt_absolute_time();
+			}
 
 		} else if (_vtol_schedule.flight_mode == TRANSITION_TO_FW) {
 			// failsafe back to mc mode

From 0cc44a82c5c1e069d1ca09b659722c63031a1d26 Mon Sep 17 00:00:00 2001
From: sander <sander@droneslab.com>
Date: Sun, 31 Jul 2016 22:45:33 +0200
Subject: [PATCH 1181/1230] Code style

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

diff --git a/src/modules/vtol_att_control/standard.cpp b/src/modules/vtol_att_control/standard.cpp
index 1184d594eb..d72415adc4 100644
--- a/src/modules/vtol_att_control/standard.cpp
+++ b/src/modules/vtol_att_control/standard.cpp
@@ -146,7 +146,7 @@ void Standard::update_vtol_state()
 
 		} else if (_vtol_schedule.flight_mode == FW_MODE) {
 			// transition to mc mode
-			if (_vtol_vehicle_status->vtol_transition_failsafe == true){
+			if (_vtol_vehicle_status->vtol_transition_failsafe == true) {
 				// Failsafe event, engage mc motors immediately
 				_vtol_schedule.flight_mode = MC_MODE;
 				_flag_enable_mc_motors = true;

From 92185945bcc337f0bccd52d0cf5a4ffd0f14d430 Mon Sep 17 00:00:00 2001
From: Andreas Antener <antener_a@gmx.ch>
Date: Mon, 1 Aug 2016 13:32:25 +0200
Subject: [PATCH 1182/1230] make quadchute work during transitions

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

diff --git a/src/modules/vtol_att_control/standard.cpp b/src/modules/vtol_att_control/standard.cpp
index d72415adc4..93a8cfbdbe 100644
--- a/src/modules/vtol_att_control/standard.cpp
+++ b/src/modules/vtol_att_control/standard.cpp
@@ -232,6 +232,8 @@ void Standard::update_vtol_state()
 
 void Standard::update_transition_state()
 {
+	VtolType::update_transition_state();
+
 	// copy virtual attitude setpoint to real attitude setpoint
 	memcpy(_v_att_sp, _mc_virtual_att_sp, sizeof(vehicle_attitude_setpoint_s));
 

From 92ddc30b69e33db623caacf2ef97cee17883455a Mon Sep 17 00:00:00 2001
From: Andreas Antener <antener_a@gmx.ch>
Date: Mon, 1 Aug 2016 13:57:39 +0200
Subject: [PATCH 1183/1230] moved min alt param

---
 src/modules/vtol_att_control/standard_params.c       | 12 ------------
 .../vtol_att_control/vtol_att_control_params.c       | 11 +++++++++++
 2 files changed, 11 insertions(+), 12 deletions(-)

diff --git a/src/modules/vtol_att_control/standard_params.c b/src/modules/vtol_att_control/standard_params.c
index 114dc17474..b4ab3e4c42 100644
--- a/src/modules/vtol_att_control/standard_params.c
+++ b/src/modules/vtol_att_control/standard_params.c
@@ -72,15 +72,3 @@ PARAM_DEFINE_FLOAT(VT_DWN_PITCH_MAX, 5.0f);
  * @group VTOL Attitude Control
  */
 PARAM_DEFINE_FLOAT(VT_FWD_THRUST_SC, 0.0f);
-
-
-/**
- * QuadChute
- *
- * Minimum altitude for fixed wing flight, when in fixed wing the altitude drops below this altitude
- * the vehicle will transition back to MC mode and enter failsafe RTL
- * @min 0.0
- * @max 200.0
- * @group VTOL Attitude Control
- */
-PARAM_DEFINE_FLOAT(VT_FW_MIN_ALT, 0.0f);
diff --git a/src/modules/vtol_att_control/vtol_att_control_params.c b/src/modules/vtol_att_control/vtol_att_control_params.c
index bfcc712242..a8326e2cb1 100644
--- a/src/modules/vtol_att_control/vtol_att_control_params.c
+++ b/src/modules/vtol_att_control/vtol_att_control_params.c
@@ -304,3 +304,14 @@ PARAM_DEFINE_FLOAT(VT_TRANS_MIN_TM, 2.0f);
  * @group VTOL Attitude Control
  */
 PARAM_DEFINE_INT32(VT_NAV_FORCE_VT, 1);
+
+/**
+ * QuadChute
+ *
+ * Minimum altitude for fixed wing flight, when in fixed wing the altitude drops below this altitude
+ * the vehicle will transition back to MC mode and enter failsafe RTL
+ * @min 0.0
+ * @max 200.0
+ * @group VTOL Attitude Control
+ */
+PARAM_DEFINE_FLOAT(VT_FW_MIN_ALT, 0.0f);

From 6f1eda2b18e409bdfc9e839c51feab3b66ccab78 Mon Sep 17 00:00:00 2001
From: Andreas Antener <antener_a@gmx.ch>
Date: Mon, 1 Aug 2016 14:20:16 +0200
Subject: [PATCH 1184/1230] added single point to decide if we need quadchute,
 let the reason be passed from the source where we know what's going on

---
 src/modules/vtol_att_control/standard.cpp     |  2 +-
 .../vtol_att_control_main.cpp                 |  6 ++---
 .../vtol_att_control/vtol_att_control_main.h  |  2 +-
 src/modules/vtol_att_control/vtol_type.cpp    | 26 +++++++++----------
 src/modules/vtol_att_control/vtol_type.h      |  5 ++++
 5 files changed, 22 insertions(+), 19 deletions(-)

diff --git a/src/modules/vtol_att_control/standard.cpp b/src/modules/vtol_att_control/standard.cpp
index 93a8cfbdbe..694317785d 100644
--- a/src/modules/vtol_att_control/standard.cpp
+++ b/src/modules/vtol_att_control/standard.cpp
@@ -285,7 +285,7 @@ void Standard::update_transition_state()
 		if (_params_standard.front_trans_timeout > FLT_EPSILON) {
 			if ((float)hrt_elapsed_time(&_vtol_schedule.transition_start) > (_params_standard.front_trans_timeout * 1000000.0f)) {
 				// transition timeout occured, abort transition
-				_attc->abort_front_transition();
+				_attc->abort_front_transition("Transition timeout");
 			}
 		}
 
diff --git a/src/modules/vtol_att_control/vtol_att_control_main.cpp b/src/modules/vtol_att_control/vtol_att_control_main.cpp
index 555563fbfe..8f623461ec 100644
--- a/src/modules/vtol_att_control/vtol_att_control_main.cpp
+++ b/src/modules/vtol_att_control/vtol_att_control_main.cpp
@@ -510,10 +510,10 @@ VtolAttitudeControl::is_fixed_wing_requested()
  * Abort front transition
  */
 void
-VtolAttitudeControl::abort_front_transition()
+VtolAttitudeControl::abort_front_transition(char *reason)
 {
-	if (!_abort_front_transition) {
-		mavlink_log_critical(&_mavlink_log_pub, "Transition timeout or FW min alt occured, aborting");
+	if(!_abort_front_transition) {
+		mavlink_log_critical(&_mavlink_log_pub, "Abort: %s", reason);
 		_abort_front_transition = true;
 		_vtol_vehicle_status.vtol_transition_failsafe = true;
 	}
diff --git a/src/modules/vtol_att_control/vtol_att_control_main.h b/src/modules/vtol_att_control/vtol_att_control_main.h
index 3412a53990..762d6bd2b2 100644
--- a/src/modules/vtol_att_control/vtol_att_control_main.h
+++ b/src/modules/vtol_att_control/vtol_att_control_main.h
@@ -110,7 +110,7 @@ public:
 
 	int start();	/* start the task and return OK on success */
 	bool is_fixed_wing_requested();
-	void abort_front_transition();
+	void abort_front_transition(char *reason);
 
 	struct vehicle_attitude_s 			*get_att() {return &_v_att;}
 	struct vehicle_attitude_setpoint_s		*get_att_sp() {return &_v_att_sp;}
diff --git a/src/modules/vtol_att_control/vtol_type.cpp b/src/modules/vtol_att_control/vtol_type.cpp
index ae3ed92ef5..71c7432bde 100644
--- a/src/modules/vtol_att_control/vtol_type.cpp
+++ b/src/modules/vtol_att_control/vtol_type.cpp
@@ -180,27 +180,25 @@ void VtolType::update_fw_state()
 		waiting_on_tecs();
 	}
 
-	// quadchute
-	if (_params->fw_min_alt > FLT_EPSILON && _armed->armed) {
-		if (-(_local_pos->z) < _params->fw_min_alt) {
-			_attc->abort_front_transition();
-		}
-	}
-
+	check_quadchute_condition();
 }
 
 void VtolType::update_transition_state()
 {
-	// quadchute
-	if (_params->fw_min_alt > FLT_EPSILON && _armed->armed) {
-		if (-(_local_pos->z) < _params->fw_min_alt) {
-			_attc->abort_front_transition();
-		}
-	}
-
+	check_quadchute_condition();
 }
 
 bool VtolType::can_transition_on_ground()
 {
 	return !_armed->armed || _land_detected->landed;
 }
+
+void VtolType::check_quadchute_condition()
+{
+	// fixed-wing minimum altitude
+	if(_params->fw_min_alt > FLT_EPSILON && _armed->armed){
+		if(-(_local_pos->z) < _params->fw_min_alt){
+			_attc->abort_front_transition("Minimum altitude");
+		}
+	}
+}
diff --git a/src/modules/vtol_att_control/vtol_type.h b/src/modules/vtol_att_control/vtol_type.h
index 4570e39d8e..96f11fd51e 100644
--- a/src/modules/vtol_att_control/vtol_type.h
+++ b/src/modules/vtol_att_control/vtol_type.h
@@ -123,6 +123,11 @@ public:
 	 */
 	virtual void waiting_on_tecs() {};
 
+	/**
+	 * Checks for fixed-wing failsafe condition and issues abort request if needed.
+	 */
+	void check_quadchute_condition();
+
 	/**
 	 * Returns true if we're allowed to do a mode transition on the ground.
 	 */

From d089c427ec6e830f03aeccc0251461f53bb10b33 Mon Sep 17 00:00:00 2001
From: Andreas Antener <antener_a@gmx.ch>
Date: Mon, 1 Aug 2016 14:42:29 +0200
Subject: [PATCH 1185/1230] quadchute code style fix

---
 src/modules/vtol_att_control/vtol_att_control_main.cpp | 2 +-
 src/modules/vtol_att_control/vtol_type.cpp             | 4 ++--
 2 files changed, 3 insertions(+), 3 deletions(-)

diff --git a/src/modules/vtol_att_control/vtol_att_control_main.cpp b/src/modules/vtol_att_control/vtol_att_control_main.cpp
index 8f623461ec..6b6cbc4538 100644
--- a/src/modules/vtol_att_control/vtol_att_control_main.cpp
+++ b/src/modules/vtol_att_control/vtol_att_control_main.cpp
@@ -512,7 +512,7 @@ VtolAttitudeControl::is_fixed_wing_requested()
 void
 VtolAttitudeControl::abort_front_transition(char *reason)
 {
-	if(!_abort_front_transition) {
+	if (!_abort_front_transition) {
 		mavlink_log_critical(&_mavlink_log_pub, "Abort: %s", reason);
 		_abort_front_transition = true;
 		_vtol_vehicle_status.vtol_transition_failsafe = true;
diff --git a/src/modules/vtol_att_control/vtol_type.cpp b/src/modules/vtol_att_control/vtol_type.cpp
index 71c7432bde..138fc45637 100644
--- a/src/modules/vtol_att_control/vtol_type.cpp
+++ b/src/modules/vtol_att_control/vtol_type.cpp
@@ -196,8 +196,8 @@ bool VtolType::can_transition_on_ground()
 void VtolType::check_quadchute_condition()
 {
 	// fixed-wing minimum altitude
-	if(_params->fw_min_alt > FLT_EPSILON && _armed->armed){
-		if(-(_local_pos->z) < _params->fw_min_alt){
+	if (_params->fw_min_alt > FLT_EPSILON && _armed->armed) {
+		if (-(_local_pos->z) < _params->fw_min_alt) {
 			_attc->abort_front_transition("Minimum altitude");
 		}
 	}

From 86d1488e523991bb2eb2a3530bcd661588cf4c38 Mon Sep 17 00:00:00 2001
From: Andreas Antener <antener_a@gmx.ch>
Date: Mon, 1 Aug 2016 15:08:05 +0200
Subject: [PATCH 1186/1230] added missing const keyword

---
 src/modules/vtol_att_control/vtol_att_control_main.cpp | 2 +-
 src/modules/vtol_att_control/vtol_att_control_main.h   | 2 +-
 2 files changed, 2 insertions(+), 2 deletions(-)

diff --git a/src/modules/vtol_att_control/vtol_att_control_main.cpp b/src/modules/vtol_att_control/vtol_att_control_main.cpp
index 6b6cbc4538..1dbc1b39f3 100644
--- a/src/modules/vtol_att_control/vtol_att_control_main.cpp
+++ b/src/modules/vtol_att_control/vtol_att_control_main.cpp
@@ -510,7 +510,7 @@ VtolAttitudeControl::is_fixed_wing_requested()
  * Abort front transition
  */
 void
-VtolAttitudeControl::abort_front_transition(char *reason)
+VtolAttitudeControl::abort_front_transition(const char *reason)
 {
 	if (!_abort_front_transition) {
 		mavlink_log_critical(&_mavlink_log_pub, "Abort: %s", reason);
diff --git a/src/modules/vtol_att_control/vtol_att_control_main.h b/src/modules/vtol_att_control/vtol_att_control_main.h
index 762d6bd2b2..ab9545fb9e 100644
--- a/src/modules/vtol_att_control/vtol_att_control_main.h
+++ b/src/modules/vtol_att_control/vtol_att_control_main.h
@@ -110,7 +110,7 @@ public:
 
 	int start();	/* start the task and return OK on success */
 	bool is_fixed_wing_requested();
-	void abort_front_transition(char *reason);
+	void abort_front_transition(const char *reason);
 
 	struct vehicle_attitude_s 			*get_att() {return &_v_att;}
 	struct vehicle_attitude_setpoint_s		*get_att_sp() {return &_v_att_sp;}

From 579d420db1c9088f611ecac77758b326b71ab007 Mon Sep 17 00:00:00 2001
From: Andreas Antener <antener_a@gmx.ch>
Date: Wed, 3 Aug 2016 13:56:07 +0200
Subject: [PATCH 1187/1230] VTOL: enable MC motors after instant back
 transition

---
 src/modules/vtol_att_control/standard.cpp | 7 +++++++
 1 file changed, 7 insertions(+)

diff --git a/src/modules/vtol_att_control/standard.cpp b/src/modules/vtol_att_control/standard.cpp
index 694317785d..312ccc7734 100644
--- a/src/modules/vtol_att_control/standard.cpp
+++ b/src/modules/vtol_att_control/standard.cpp
@@ -324,6 +324,13 @@ void Standard::update_mc_state()
 {
 	VtolType::update_mc_state();
 
+	// enable MC motors here in case we transitioned directly to MC mode
+	if (_flag_enable_mc_motors) {
+		set_max_mc(2000);
+		set_idle_mc();
+		_flag_enable_mc_motors = false;
+	}
+
 	// if the thrust scale param is zero then the pusher-for-pitch strategy is disabled and we can return
 	if (_params_standard.forward_thrust_scale < FLT_EPSILON) {
 		return;

From 391eb7f8df6be061751eb056fe48153c7a80978f Mon Sep 17 00:00:00 2001
From: James Goppert <jgoppert@users.noreply.github.com>
Date: Wed, 3 Aug 2016 13:19:26 -0400
Subject: [PATCH 1188/1230] Sitl update. (#5220)

---
 Tools/sitl_gazebo | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/Tools/sitl_gazebo b/Tools/sitl_gazebo
index 7d7a26d968..d03d8356c5 160000
--- a/Tools/sitl_gazebo
+++ b/Tools/sitl_gazebo
@@ -1 +1 @@
-Subproject commit 7d7a26d968d58d0e66cb6bdad5ac41a513ec181a
+Subproject commit d03d8356c54c6321278273a7afaba76ace454372

From 547592fddf75d0d7a97fa983e55ef1889257e635 Mon Sep 17 00:00:00 2001
From: Nate Weibley <nate.weibley@prioria.com>
Date: Wed, 3 Aug 2016 14:06:40 -0400
Subject: [PATCH 1189/1230] Use a persistent file* and seek less for log
 download

Opening/seeking/closing the file for each data chunk was extremely
expensive and drastically slowed the download process as the position
in the file increased. Over USB with large files this change + nuttx
update results in ~ 10x speedup for log downloads.
---
 src/modules/mavlink/mavlink_log_handler.cpp | 46 +++++++++++++++------
 src/modules/mavlink/mavlink_log_handler.h   |  2 +
 2 files changed, 35 insertions(+), 13 deletions(-)

diff --git a/src/modules/mavlink/mavlink_log_handler.cpp b/src/modules/mavlink/mavlink_log_handler.cpp
index 721a22c757..443c31183f 100644
--- a/src/modules/mavlink/mavlink_log_handler.cpp
+++ b/src/modules/mavlink/mavlink_log_handler.cpp
@@ -207,11 +207,15 @@ MavlinkLogHandler::_log_request_data(const mavlink_message_t *msg)
 	}
 	//-- If we were sending log entries, stop it
 	_pLogHandlerHelper->current_status = LogListHelper::LOG_HANDLER_IDLE;
-	//-- Init send log dataset
-	_pLogHandlerHelper->current_log_filename[0] = 0;
-	_pLogHandlerHelper->current_log_index = request.id;
-	uint32_t time_utc = 0;
-	_pLogHandlerHelper->get_entry(_pLogHandlerHelper->current_log_index, _pLogHandlerHelper->current_log_size, time_utc, _pLogHandlerHelper->current_log_filename);
+	if (_pLogHandlerHelper->current_log_index != request.id) {
+		//-- Init send log dataset
+		_pLogHandlerHelper->current_log_filename[0] = 0;
+		_pLogHandlerHelper->current_log_index = request.id;
+		uint32_t time_utc = 0;
+		_pLogHandlerHelper->get_entry(_pLogHandlerHelper->current_log_index, _pLogHandlerHelper->current_log_size, time_utc, _pLogHandlerHelper->current_log_filename);
+		_pLogHandlerHelper->open_for_transmit();
+	}
+
 	_pLogHandlerHelper->current_log_data_offset = request.ofs;
 	if (_pLogHandlerHelper->current_log_data_offset >= _pLogHandlerHelper->current_log_size) {
 		_pLogHandlerHelper->current_log_data_remaining = 0;
@@ -331,10 +335,11 @@ LogListHelper::LogListHelper()
 	, last_entry(0)
 	, log_count(0)
 	, current_status(LOG_HANDLER_IDLE)
-	, current_log_index(0)
+	, current_log_index(UINT16_MAX)
 	, current_log_size(0)
 	, current_log_data_offset(0)
 	, current_log_data_remaining(0)
+	, current_log_filep(0)
 {
 	_init();
 }
@@ -379,24 +384,39 @@ LogListHelper::get_entry(int idx, uint32_t& size, uint32_t& date, char* filename
 	return result;
 }
 
+//-------------------------------------------------------------------
+bool
+LogListHelper::open_for_transmit()
+{
+	if (current_log_filep) {
+		::fclose(current_log_filep);
+		current_log_filep = 0;
+	}
+	current_log_filep = ::fopen(current_log_filename, "rb");
+	if (!current_log_filep) {
+		PX4LOG_WARN("MavlinkLogHandler::open_for_transmit Could not open %s\n", current_log_filename);
+		return false;
+	}
+	return true;
+}
+
 //-------------------------------------------------------------------
 size_t
 LogListHelper::get_log_data(uint8_t len, uint8_t* buffer)
 {
 	if(!current_log_filename[0])
 		return 0;
-	FILE* f = fopen(current_log_filename, "r");
-	if (!f) {
-		PX4LOG_WARN("MavlinkLogHandler::get_log_data Could not open %s\n", current_log_filename);
+	if (!current_log_filep) {
+		PX4LOG_WARN("MavlinkLogHandler::get_log_data file not open %s\n", current_log_filename);
 		return 0;
 	}
-	if(fseek(f, current_log_data_offset, SEEK_SET)) {
-		fclose(f);
+	long int offset = current_log_data_offset - ftell(current_log_filep);
+	if(offset && fseek(current_log_filep, offset, SEEK_CUR)) {
+		fclose(current_log_filep);
 		PX4LOG_WARN("MavlinkLogHandler::get_log_data Seek error in %s\n", current_log_filename);
 		return 0;
 	}
-	size_t result = fread(buffer, 1, len, f);
-	fclose(f);
+	size_t result = fread(buffer, 1, len, current_log_filep);
 	return result;
 }
 
diff --git a/src/modules/mavlink/mavlink_log_handler.h b/src/modules/mavlink/mavlink_log_handler.h
index 8f42404672..231fc1d39f 100644
--- a/src/modules/mavlink/mavlink_log_handler.h
+++ b/src/modules/mavlink/mavlink_log_handler.h
@@ -58,6 +58,7 @@ public:
 public:
 
 	bool        get_entry           (int idx, uint32_t& size, uint32_t& date, char* filename = 0);
+	bool        open_for_transmit();
 	size_t      get_log_data        (uint8_t len, uint8_t* buffer);
 
 	enum {
@@ -75,6 +76,7 @@ public:
 	uint32_t    current_log_size;
 	uint32_t    current_log_data_offset;
 	uint32_t    current_log_data_remaining;
+	FILE*       current_log_filep;
 	char        current_log_filename[128];
 
 private:

From e287b05d678589650f7e6423dae351f72f006290 Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Wed, 3 Aug 2016 21:10:34 +0200
Subject: [PATCH 1190/1230] NuttX: Add file change which allows really fast log
 download

---
 NuttX | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/NuttX b/NuttX
index 9526676011..55e8d557ec 160000
--- a/NuttX
+++ b/NuttX
@@ -1 +1 @@
-Subproject commit 9526676011e82b3762914063794dfd9ec9ec5b87
+Subproject commit 55e8d557ecba65c82f7ce8fd326575470f0e5acc

From 5b06b40ed9b1aa6823af0fabb774c0d3a514f98f Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Wed, 3 Aug 2016 23:04:35 +0200
Subject: [PATCH 1191/1230] Set SITL as default target now that Pixhawk is not
 necessarily the default HW

---
 CMakeLists.txt | 2 +-
 Makefile       | 2 +-
 2 files changed, 2 insertions(+), 2 deletions(-)

diff --git a/CMakeLists.txt b/CMakeLists.txt
index 99a6061681..9397b87c99 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -132,7 +132,7 @@ cmake_minimum_required(VERSION 2.8 FATAL_ERROR)
 set(CMAKE_BUILD_TYPE "" CACHE STRING "build type")
 set_property(CACHE CMAKE_BUILD_TYPE PROPERTY
 	STRINGS ";Debug;Release;RelWithDebInfo;MinSizeRel")
-set(CONFIG "nuttx_px4fmu-v2_default" CACHE STRING "desired configuration")
+set(CONFIG "posix_sitl_default" CACHE STRING "desired configuration")
 file(GLOB_RECURSE configs RELATIVE cmake/configs "cmake/configs/*.cmake")
 set_property(CACHE CONFIG PROPERTY STRINGS ${configs})
 set(THREADS "4" CACHE STRING
diff --git a/Makefile b/Makefile
index e05f9b03a4..f80a24399c 100644
--- a/Makefile
+++ b/Makefile
@@ -77,7 +77,7 @@ endif
 # in that directory with the target upload.
 
 #  explicity set default build target
-all: px4fmu-v2_default
+all: posix_sitl_default
 
 # Parsing
 # --------------------------------------------------------------------

From feccb9bfc13bb87e7372a1de7321164fd0a17bd7 Mon Sep 17 00:00:00 2001
From: James Goppert <james.goppert@gmail.com>
Date: Wed, 3 Aug 2016 17:17:07 -0400
Subject: [PATCH 1192/1230] Make integraiton test script work with kinetic.

---
 integrationtests/run_tests.bash | 7 ++++++-
 1 file changed, 6 insertions(+), 1 deletion(-)

diff --git a/integrationtests/run_tests.bash b/integrationtests/run_tests.bash
index b17b7511dc..da22cb5eef 100755
--- a/integrationtests/run_tests.bash
+++ b/integrationtests/run_tests.bash
@@ -25,7 +25,12 @@ TEST_RESULT_TARGET_DIR=$JOB_DIR/test_results
 # EXPORT_CHARTS=/sitl/testing/export_charts.py
 
 # source ROS env
-source /opt/ros/indigo/setup.bash
+if [ -f /opt/ros/indigo/setup.bash ]
+then
+	source /opt/ros/indigo/setup.bash
+else
+	source /opt/ros/kinetic/setup.bash
+fi
 source $SRC_DIR/integrationtests/setup_gazebo_ros.bash $SRC_DIR
 
 echo "deleting previous test results ($TEST_RESULT_TARGET_DIR)"

From f3c5c2c5a9e020c8b435f55cb513f7fd865d3f13 Mon Sep 17 00:00:00 2001
From: Andreas Daniel Antener <antener_a@gmx.ch>
Date: Wed, 3 Aug 2016 23:41:01 +0200
Subject: [PATCH 1193/1230] use thrust scale parameter for vtol pusher support
 (#5207)

---
 src/modules/vtol_att_control/standard.cpp | 3 ++-
 1 file changed, 2 insertions(+), 1 deletion(-)

diff --git a/src/modules/vtol_att_control/standard.cpp b/src/modules/vtol_att_control/standard.cpp
index 312ccc7734..85f48dfcf8 100644
--- a/src/modules/vtol_att_control/standard.cpp
+++ b/src/modules/vtol_att_control/standard.cpp
@@ -361,7 +361,8 @@ void Standard::update_mc_state()
 		// desired roll angle in heading frame stays the same
 		float roll_new = -atan2f(body_z_sp(1), body_z_sp(2));
 
-		_pusher_throttle = (sinf(-pitch_forward) - sinf(_params_standard.down_pitch_max)) * _v_att_sp->thrust;
+		_pusher_throttle = (sinf(-pitch_forward) - sinf(_params_standard.down_pitch_max))
+				   * _v_att_sp->thrust * _params_standard.forward_thrust_scale;
 
 		// limit desired pitch
 		float pitch_new = -_params_standard.down_pitch_max;

From eeb73888fdbf657f2cd13b5681e605fcbddb1bcb Mon Sep 17 00:00:00 2001
From: James Goppert <jgoppert@users.noreply.github.com>
Date: Wed, 3 Aug 2016 18:46:25 -0400
Subject: [PATCH 1194/1230] update sitl_gazebo (#5224)

---
 Tools/sitl_gazebo               | 2 +-
 integrationtests/run_tests.bash | 6 +++++-
 2 files changed, 6 insertions(+), 2 deletions(-)

diff --git a/Tools/sitl_gazebo b/Tools/sitl_gazebo
index d03d8356c5..00dcabb1c2 160000
--- a/Tools/sitl_gazebo
+++ b/Tools/sitl_gazebo
@@ -1 +1 @@
-Subproject commit d03d8356c54c6321278273a7afaba76ace454372
+Subproject commit 00dcabb1c2c001b40713903da1a4a7f71d01ef27
diff --git a/integrationtests/run_tests.bash b/integrationtests/run_tests.bash
index da22cb5eef..2a3a4c2a0d 100755
--- a/integrationtests/run_tests.bash
+++ b/integrationtests/run_tests.bash
@@ -28,8 +28,12 @@ TEST_RESULT_TARGET_DIR=$JOB_DIR/test_results
 if [ -f /opt/ros/indigo/setup.bash ]
 then
 	source /opt/ros/indigo/setup.bash
-else
+elif [ -f /opt/ros/kinetic/setup.bash ]
+then
 	source /opt/ros/kinetic/setup.bash
+else
+	echo "could not find /opt/ros/{ros-distro}/setup.bash"
+	exit 1
 fi
 source $SRC_DIR/integrationtests/setup_gazebo_ros.bash $SRC_DIR
 

From 422cf7e21ce4be26f02096739389e568389e9e23 Mon Sep 17 00:00:00 2001
From: Mark Whitehorn <kd0aij@gmail.com>
Date: Thu, 4 Aug 2016 00:22:33 -0600
Subject: [PATCH 1195/1230] assign timestamp in vtol vehicle_rates_setpoint
 message (#5227)

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

diff --git a/src/modules/vtol_att_control/vtol_att_control_main.cpp b/src/modules/vtol_att_control/vtol_att_control_main.cpp
index 1dbc1b39f3..820f7cfa56 100644
--- a/src/modules/vtol_att_control/vtol_att_control_main.cpp
+++ b/src/modules/vtol_att_control/vtol_att_control_main.cpp
@@ -584,6 +584,7 @@ VtolAttitudeControl::parameters_update()
 */
 void VtolAttitudeControl::fill_mc_att_rates_sp()
 {
+	_v_rates_sp.timestamp 	= _mc_virtual_v_rates_sp.timestamp;
 	_v_rates_sp.roll 	= _mc_virtual_v_rates_sp.roll;
 	_v_rates_sp.pitch 	= _mc_virtual_v_rates_sp.pitch;
 	_v_rates_sp.yaw 	= _mc_virtual_v_rates_sp.yaw;
@@ -595,6 +596,7 @@ void VtolAttitudeControl::fill_mc_att_rates_sp()
 */
 void VtolAttitudeControl::fill_fw_att_rates_sp()
 {
+	_v_rates_sp.timestamp 	= _fw_virtual_v_rates_sp.timestamp;
 	_v_rates_sp.roll 	= _fw_virtual_v_rates_sp.roll;
 	_v_rates_sp.pitch 	= _fw_virtual_v_rates_sp.pitch;
 	_v_rates_sp.yaw 	= _fw_virtual_v_rates_sp.yaw;

From cfa203ca2243a7432a096b47dd7043befbaae3e5 Mon Sep 17 00:00:00 2001
From: Eike <ecm@gmx.de>
Date: Thu, 4 Aug 2016 11:32:26 +0200
Subject: [PATCH 1196/1230] Remove LPOS.Z reset (#5228)

---
 .../local_position_estimator/BlockLocalPositionEstimator.cpp     | 1 -
 1 file changed, 1 deletion(-)

diff --git a/src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp b/src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp
index c226560482..bdabd56b79 100644
--- a/src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp
+++ b/src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp
@@ -259,7 +259,6 @@ void BlockLocalPositionEstimator::update()
 		// we just armed, we are at origin on the ground
 		_x(X_x) = 0;
 		_x(X_y) = 0;
-		_x(X_z) = 0;
 
 		// reset flow integral
 		_flowX = 0;

From d748f6ca710139fde08107da96c7a24df3c2d165 Mon Sep 17 00:00:00 2001
From: Julian Oes <julian@oes.ch>
Date: Wed, 3 Aug 2016 17:23:22 +0200
Subject: [PATCH 1197/1230] attitude_estimator_q: filter accel and gyro data

Since accel and gyro are not filtered in the drivers anymore, we need to
filter them in this estimator in order to achieve a similar performance.
---
 .../attitude_estimator_q_main.cpp             | 30 ++++++++++++++-----
 1 file changed, 23 insertions(+), 7 deletions(-)

diff --git a/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp b/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp
index 33ff8ab605..7a4df0f060 100644
--- a/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp
+++ b/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp
@@ -178,6 +178,14 @@ private:
 	math::LowPassFilter2p _lp_pitch_rate;
 	math::LowPassFilter2p _lp_yaw_rate;
 
+	/* Low pass filter for accel/gyro */
+	math::LowPassFilter2p _lp_accel_x;
+	math::LowPassFilter2p _lp_accel_y;
+	math::LowPassFilter2p _lp_accel_z;
+	math::LowPassFilter2p _lp_gyro_x;
+	math::LowPassFilter2p _lp_gyro_y;
+	math::LowPassFilter2p _lp_gyro_z;
+
 	hrt_abstime _vel_prev_t = 0;
 
 	bool		_inited = false;
@@ -207,7 +215,13 @@ AttitudeEstimatorQ::AttitudeEstimatorQ() :
 	_pos_acc(0, 0, 0),
 	_lp_roll_rate(250.0f, 30.0f),
 	_lp_pitch_rate(250.0f, 30.0f),
-	_lp_yaw_rate(250.0f, 20.0f)
+	_lp_yaw_rate(250.0f, 20.0f),
+	_lp_accel_x(250.0f, 30.0f),
+	_lp_accel_y(250.0f, 30.0f),
+	_lp_accel_z(250.0f, 30.0f),
+	_lp_gyro_x(250.0f, 30.0f),
+	_lp_gyro_y(250.0f, 30.0f),
+	_lp_gyro_z(250.0f, 30.0f)
 {
 	_params_handles.w_acc		= param_find("ATT_W_ACC");
 	_params_handles.w_mag		= param_find("ATT_W_MAG");
@@ -328,15 +342,17 @@ void AttitudeEstimatorQ::task_main()
 			// Feed validator with recent sensor data
 
 			if (sensors.timestamp > 0) {
-				_gyro(0) = sensors.gyro_rad[0];
-				_gyro(1) = sensors.gyro_rad[1];
-				_gyro(2) = sensors.gyro_rad[2];
+				// Filter gyro signal since it is not fildered in the drivers.
+				_gyro(0) = _lp_gyro_x.apply(sensors.gyro_rad[0]);
+				_gyro(1) = _lp_gyro_y.apply(sensors.gyro_rad[1]);
+				_gyro(2) = _lp_gyro_z.apply(sensors.gyro_rad[2]);
 			}
 
 			if (sensors.accelerometer_timestamp_relative != sensor_combined_s::RELATIVE_TIMESTAMP_INVALID) {
-				_accel(0) = sensors.accelerometer_m_s2[0];
-				_accel(1) = sensors.accelerometer_m_s2[1];
-				_accel(2) = sensors.accelerometer_m_s2[2];
+				// Filter accel signal since it is not fildered in the drivers.
+				_accel(0) = _lp_accel_x.apply(sensors.accelerometer_m_s2[0]);
+				_accel(1) = _lp_accel_y.apply(sensors.accelerometer_m_s2[1]);
+				_accel(2) = _lp_accel_z.apply(sensors.accelerometer_m_s2[2]);
 
 				if (_accel.length() < 0.01f) {
 					warnx("WARNING: degenerate accel!");

From ea9c8b968a45734e418dbade866dfe6221718b06 Mon Sep 17 00:00:00 2001
From: Julian Oes <julian@oes.ch>
Date: Wed, 3 Aug 2016 17:24:10 +0200
Subject: [PATCH 1198/1230] attitude_estimator_q: don't spam console

We should not spam the console just because the input data is
degenerate, it would only make things worse because everything would
slow down due to the printfs.
---
 .../attitude_estimator_q/attitude_estimator_q_main.cpp        | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp b/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp
index 7a4df0f060..05591d1307 100644
--- a/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp
+++ b/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp
@@ -355,7 +355,7 @@ void AttitudeEstimatorQ::task_main()
 				_accel(2) = _lp_accel_z.apply(sensors.accelerometer_m_s2[2]);
 
 				if (_accel.length() < 0.01f) {
-					warnx("WARNING: degenerate accel!");
+					PX4_DEBUG("WARNING: degenerate accel!");
 					continue;
 				}
 			}
@@ -366,7 +366,7 @@ void AttitudeEstimatorQ::task_main()
 				_mag(2) = sensors.magnetometer_ga[2];
 
 				if (_mag.length() < 0.01f) {
-					warnx("WARNING: degenerate mag!");
+					PX4_DEBUG("WARNING: degenerate mag!");
 					continue;
 				}
 			}

From 00d4eae373ef1eeb184000c1586ee4a40328e267 Mon Sep 17 00:00:00 2001
From: Julian Oes <julian@oes.ch>
Date: Wed, 3 Aug 2016 17:42:08 +0200
Subject: [PATCH 1199/1230] attitude_estimator_q: don't filter output rates

Since we're already filtering the incoming gyro and accel sensor
signals, we don't need to filter the output rates again.
---
 .../attitude_estimator_q_main.cpp                  | 14 +++-----------
 1 file changed, 3 insertions(+), 11 deletions(-)

diff --git a/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp b/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp
index 05591d1307..281551832a 100644
--- a/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp
+++ b/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp
@@ -173,11 +173,6 @@ private:
 	Vector<3>	_vel_prev;
 	Vector<3>	_pos_acc;
 
-	/* Low pass filter for attitude rates */
-	math::LowPassFilter2p _lp_roll_rate;
-	math::LowPassFilter2p _lp_pitch_rate;
-	math::LowPassFilter2p _lp_yaw_rate;
-
 	/* Low pass filter for accel/gyro */
 	math::LowPassFilter2p _lp_accel_x;
 	math::LowPassFilter2p _lp_accel_y;
@@ -213,9 +208,6 @@ private:
 AttitudeEstimatorQ::AttitudeEstimatorQ() :
 	_vel_prev(0, 0, 0),
 	_pos_acc(0, 0, 0),
-	_lp_roll_rate(250.0f, 30.0f),
-	_lp_pitch_rate(250.0f, 30.0f),
-	_lp_yaw_rate(250.0f, 20.0f),
 	_lp_accel_x(250.0f, 30.0f),
 	_lp_accel_y(250.0f, 30.0f),
 	_lp_accel_z(250.0f, 30.0f),
@@ -519,11 +511,11 @@ void AttitudeEstimatorQ::task_main()
 			ctrl_state.z_acc = _accel(2);
 
 			/* attitude rates for control state */
-			ctrl_state.roll_rate = _lp_roll_rate.apply(_rates(0));
+			ctrl_state.roll_rate = _rates(0);
 
-			ctrl_state.pitch_rate = _lp_pitch_rate.apply(_rates(1));
+			ctrl_state.pitch_rate = _rates(1);
 
-			ctrl_state.yaw_rate = _lp_yaw_rate.apply(_rates(2));
+			ctrl_state.yaw_rate = _rates(2);
 
 			ctrl_state.airspeed_valid = false;
 

From 49844f52d5991a2f3cf3f77ef774d0733c621f00 Mon Sep 17 00:00:00 2001
From: Vasily Evseenko <svpcom@gmail.com>
Date: Thu, 28 Jul 2016 21:22:00 +0300
Subject: [PATCH 1200/1230] Fix SF10a driver. Add support for SF11c and rename
 to SF1xx

---
 ROMFS/px4fmu_common/init.d/rc.sensors         |   2 +-
 ROMFS/px4fmu_test/init.d/rc.sensors           |   2 +-
 ROMFS/tap_common/init.d/rc.sensors            |   2 +-
 cmake/configs/nuttx_px4fmu-v4_default.cmake   |   2 +-
 src/drivers/{sf10a => sf1xx}/CMakeLists.txt   |   6 +-
 .../{sf10a/sf10a.cpp => sf1xx/sf1xx.cpp}      | 144 +++++++++---------
 6 files changed, 75 insertions(+), 83 deletions(-)
 rename src/drivers/{sf10a => sf1xx}/CMakeLists.txt (97%)
 rename src/drivers/{sf10a/sf10a.cpp => sf1xx/sf1xx.cpp} (86%)

diff --git a/ROMFS/px4fmu_common/init.d/rc.sensors b/ROMFS/px4fmu_common/init.d/rc.sensors
index e356f939d7..3fc86b0873 100644
--- a/ROMFS/px4fmu_common/init.d/rc.sensors
+++ b/ROMFS/px4fmu_common/init.d/rc.sensors
@@ -201,7 +201,7 @@ else
 	fi
 fi
 
-if sf10a start
+if sf1xx start
 then
 fi
 
diff --git a/ROMFS/px4fmu_test/init.d/rc.sensors b/ROMFS/px4fmu_test/init.d/rc.sensors
index 9a4d12192e..a39bc4edba 100644
--- a/ROMFS/px4fmu_test/init.d/rc.sensors
+++ b/ROMFS/px4fmu_test/init.d/rc.sensors
@@ -190,7 +190,7 @@ else
 	fi
 fi
 
-if sf10a start
+if sf1xx start
 then
 fi
 
diff --git a/ROMFS/tap_common/init.d/rc.sensors b/ROMFS/tap_common/init.d/rc.sensors
index fdb835baff..4e767bdc6c 100644
--- a/ROMFS/tap_common/init.d/rc.sensors
+++ b/ROMFS/tap_common/init.d/rc.sensors
@@ -35,7 +35,7 @@ if meas_airspeed start
 then
 fi
 
-if sf10a start
+if sf1xx start
 then
 fi
 
diff --git a/cmake/configs/nuttx_px4fmu-v4_default.cmake b/cmake/configs/nuttx_px4fmu-v4_default.cmake
index fe356e49d6..866e19dc0d 100644
--- a/cmake/configs/nuttx_px4fmu-v4_default.cmake
+++ b/cmake/configs/nuttx_px4fmu-v4_default.cmake
@@ -23,7 +23,7 @@ set(config_module_list
 	drivers/mb12xx
 	drivers/srf02
 	drivers/sf0x
-	drivers/sf10a
+	drivers/sf1xx
 	drivers/ll40ls
 	drivers/trone
 	drivers/gps
diff --git a/src/drivers/sf10a/CMakeLists.txt b/src/drivers/sf1xx/CMakeLists.txt
similarity index 97%
rename from src/drivers/sf10a/CMakeLists.txt
rename to src/drivers/sf1xx/CMakeLists.txt
index 5db2934a74..098bd54171 100644
--- a/src/drivers/sf10a/CMakeLists.txt
+++ b/src/drivers/sf1xx/CMakeLists.txt
@@ -32,12 +32,12 @@
 ############################################################################
 px4_add_module(
 
-	MODULE drivers__sf10a
-	MAIN sf10a
+	MODULE drivers__sf1xx
+	MAIN sf1xx
 	COMPILE_FLAGS
 		-Os
 	SRCS
-		sf10a.cpp
+		sf1xx.cpp
 	DEPENDS
 		platforms__common
 	)
diff --git a/src/drivers/sf10a/sf10a.cpp b/src/drivers/sf1xx/sf1xx.cpp
similarity index 86%
rename from src/drivers/sf10a/sf10a.cpp
rename to src/drivers/sf1xx/sf1xx.cpp
index ecfa4020d1..6dc55d4916 100644
--- a/src/drivers/sf10a/sf10a.cpp
+++ b/src/drivers/sf1xx/sf1xx.cpp
@@ -32,11 +32,12 @@
  ****************************************************************************/
 
 /**
- * @file sf10a.cpp
+ * @file sf1xx.cpp
  *
  * @author ecmnet <ecm@gmx.de>
+ * @author Vasily Evseenko <svpcom@gmail.com>
  *
- * Driver for the Lightware SF10x lidar range finder series.
+ * Driver for the Lightware SF1xx lidar range finder series.
  * Default I2C address 0x55 is used.
  */
 
@@ -75,18 +76,18 @@
 #include <board_config.h>
 
 /* Configuration Constants */
-#define SF10A_BUS 		PX4_I2C_BUS_EXPANSION
-#define SF10A_BASEADDR 	0x55
-#define SF10A_DEVICE_PATH	"/dev/sf10a"
+#define SF1XX_BUS 		PX4_I2C_BUS_EXPANSION
+#define SF1XX_BASEADDR 	0x55
+#define SF1XX_DEVICE_PATH	"/dev/sf1xx"
 
 /* Device limits */
-#define SF10A_MIN_DISTANCE 	(0.01f)
-#define SF10A_MAX_DISTANCE 	(25.0f)
+#define SF1XX_MIN_DISTANCE 	(0.01f)
+#define SF1XX_MAX_DISTANCE 	(120.0f)
 
 
 /* conversion rates */
 
-#define SF10A_CONVERSION_INTERVAL 		31250		// Maximum rate according to datasheet is 32Hz
+#define SF1XX_CONVERSION_INTERVAL 		50000		// Maximum rate according to datasheet is 20Hz
 
 
 /* oddly, ERROR is not defined for c++ */
@@ -99,11 +100,11 @@ static const int ERROR = -1;
 # error This requires CONFIG_SCHED_WORKQUEUE.
 #endif
 
-class SF10A : public device::I2C
+class SF1XX : public device::I2C
 {
 public:
-	SF10A(int bus = SF10A_BUS, int address = SF10A_BASEADDR);
-	virtual ~SF10A();
+	SF1XX(int bus = SF1XX_BUS, int address = SF1XX_BASEADDR);
+	virtual ~SF1XX();
 
 	virtual int 		init();
 
@@ -160,8 +161,8 @@ private:
 
 	/**
 	* Set the min and max distance thresholds if you want the end points of the sensors
-	* range to be brought in at all, otherwise it will use the defaults SF10A_MIN_DISTANCE
-	* and SF10A_MAX_DISTANCE
+	* range to be brought in at all, otherwise it will use the defaults SF1XX_MIN_DISTANCE
+	* and SF1XX_MAX_DISTANCE
 	*/
 	void				set_minimum_distance(float min);
 	void				set_maximum_distance(float max);
@@ -189,21 +190,21 @@ private:
 /*
  * Driver 'main' command.
  */
-extern "C" __EXPORT int sf10a_main(int argc, char *argv[]);
+extern "C" __EXPORT int sf1xx_main(int argc, char *argv[]);
 
-SF10A::SF10A(int bus, int address) :
-	I2C("SF10A", SF10A_DEVICE_PATH, bus, address, 100000),
-	_min_distance(SF10A_MIN_DISTANCE),
-	_max_distance(SF10A_MAX_DISTANCE),
+SF1XX::SF1XX(int bus, int address) :
+	I2C("SF1XX", SF1XX_DEVICE_PATH, bus, address, 400000),
+	_min_distance(SF1XX_MIN_DISTANCE),
+	_max_distance(SF1XX_MAX_DISTANCE),
 	_reports(nullptr),
 	_sensor_ok(false),
 	_measure_ticks(0),
 	_class_instance(-1),
 	_orb_class_instance(-1),
 	_distance_sensor_topic(nullptr),
-	_sample_perf(perf_alloc(PC_ELAPSED, "sf10a_read")),
-	_comms_errors(perf_alloc(PC_COUNT, "sf10a_com_err")),
-	_buffer_overflows(perf_alloc(PC_COUNT, "sf10a_buf_of"))
+	_sample_perf(perf_alloc(PC_ELAPSED, "sf1xx_read")),
+	_comms_errors(perf_alloc(PC_COUNT, "sf1xx_com_err")),
+	_buffer_overflows(perf_alloc(PC_COUNT, "sf1xx_buf_of"))
 
 {
 	/* enable debug() calls */
@@ -213,7 +214,7 @@ SF10A::SF10A(int bus, int address) :
 	memset(&_work, 0, sizeof(_work));
 }
 
-SF10A::~SF10A()
+SF1XX::~SF1XX()
 {
 	/* make sure we are truly inactive */
 	stop();
@@ -234,7 +235,7 @@ SF10A::~SF10A()
 }
 
 int
-SF10A::init()
+SF1XX::init()
 {
 	int ret = ERROR;
 
@@ -246,7 +247,7 @@ SF10A::init()
 	/* allocate basic report buffers */
 	_reports = new ringbuffer::RingBuffer(2, sizeof(distance_sensor_s));
 
-	set_address(SF10A_BASEADDR);
+	set_address(SF1XX_BASEADDR);
 
 	if (_reports == nullptr) {
 		return ret;
@@ -264,50 +265,50 @@ SF10A::init()
 		DEVICE_LOG("failed to create distance_sensor object. Did you start uOrb?");
 	}
 
-
+        // Select altitude register
 	int ret2 = measure();
 
 	if (ret2 == 0) {
 		ret = OK;
 		_sensor_ok = true;
-		DEVICE_LOG("SF10x with address %d found", SF10A_BASEADDR);
+		DEVICE_LOG("SF1xx with address %d found", SF1XX_BASEADDR);
 	}
 
 	return ret;
 }
 
 int
-SF10A::probe()
+SF1XX::probe()
 {
 	return measure();
 }
 
 void
-SF10A::set_minimum_distance(float min)
+SF1XX::set_minimum_distance(float min)
 {
 	_min_distance = min;
 }
 
 void
-SF10A::set_maximum_distance(float max)
+SF1XX::set_maximum_distance(float max)
 {
 	_max_distance = max;
 }
 
 float
-SF10A::get_minimum_distance()
+SF1XX::get_minimum_distance()
 {
 	return _min_distance;
 }
 
 float
-SF10A::get_maximum_distance()
+SF1XX::get_maximum_distance()
 {
 	return _max_distance;
 }
 
 int
-SF10A::ioctl(struct file *filp, int cmd, unsigned long arg)
+SF1XX::ioctl(struct file *filp, int cmd, unsigned long arg)
 {
 	switch (cmd) {
 
@@ -334,7 +335,7 @@ SF10A::ioctl(struct file *filp, int cmd, unsigned long arg)
 					bool want_start = (_measure_ticks == 0);
 
 					/* set interval for next measurement to minimum legal value */
-					_measure_ticks = USEC2TICK(SF10A_CONVERSION_INTERVAL);
+					_measure_ticks = USEC2TICK(SF1XX_CONVERSION_INTERVAL);
 
 					/* if we need to start the poll state machine, do it */
 					if (want_start) {
@@ -354,7 +355,7 @@ SF10A::ioctl(struct file *filp, int cmd, unsigned long arg)
 					int ticks = USEC2TICK(1000000 / arg);
 
 					/* check against maximum rate */
-					if (ticks < USEC2TICK(SF10A_CONVERSION_INTERVAL)) {
+					if (ticks < USEC2TICK(SF1XX_CONVERSION_INTERVAL)) {
 						return -EINVAL;
 					}
 
@@ -422,7 +423,7 @@ SF10A::ioctl(struct file *filp, int cmd, unsigned long arg)
 }
 
 ssize_t
-SF10A::read(struct file *filp, char *buffer, size_t buflen)
+SF1XX::read(struct file *filp, char *buffer, size_t buflen)
 {
 	unsigned count = buflen / sizeof(struct distance_sensor_s);
 	struct distance_sensor_s *rbuf = reinterpret_cast<struct distance_sensor_s *>(buffer);
@@ -463,7 +464,7 @@ SF10A::read(struct file *filp, char *buffer, size_t buflen)
 		}
 
 		/* wait for it to complete */
-		usleep(SF10A_CONVERSION_INTERVAL);
+		usleep(SF1XX_CONVERSION_INTERVAL);
 
 		/* run the collection phase */
 		if (OK != collect()) {
@@ -482,17 +483,16 @@ SF10A::read(struct file *filp, char *buffer, size_t buflen)
 }
 
 int
-SF10A::measure()
+SF1XX::measure()
 {
 	int ret;
 
 	/*
-	 * Send the command to begin a measurement.
+	 * Send the command '0' -- read altitude
 	 */
 
-	uint8_t cmd[1];
-	cmd[0] = SF10A_BASEADDR;
-	ret = transfer(cmd, 1, nullptr, 0);
+	uint8_t cmd = 0;
+	ret = transfer(&cmd, 1, nullptr, 0);
 
 	if (OK != ret) {
 		perf_count(_comms_errors);
@@ -506,16 +506,14 @@ SF10A::measure()
 }
 
 int
-SF10A::collect()
+SF1XX::collect()
 {
 	int	ret = -EIO;
 
 	/* read from the sensor */
 	uint8_t val[2] = {0, 0};
-	uint8_t cmd = SF10A_BASEADDR;
 	perf_begin(_sample_perf);
 
-	ret = transfer(&cmd, 1, nullptr, 0);
 	ret = transfer(nullptr, 0, &val[0], 2);
 
 	if (ret < 0) {
@@ -558,34 +556,35 @@ SF10A::collect()
 }
 
 void
-SF10A::start()
+SF1XX::start()
 {
 	/* reset the report ring and state machine */
 	_reports->flush();
 
+        /* set register to '0' */
+        measure();
+
 	/* schedule a cycle to start things */
-	work_queue(HPWORK, &_work, (worker_t)&SF10A::cycle_trampoline, this, 5);
+	work_queue(HPWORK, &_work, (worker_t)&SF1XX::cycle_trampoline, this, 5);
 }
 
 void
-SF10A::stop()
+SF1XX::stop()
 {
 	work_cancel(HPWORK, &_work);
 }
 
 void
-SF10A::cycle_trampoline(void *arg)
+SF1XX::cycle_trampoline(void *arg)
 {
-	SF10A *dev = (SF10A *)arg;
+	SF1XX *dev = (SF1XX *)arg;
 
 	dev->cycle();
 }
 
 void
-SF10A::cycle()
+SF1XX::cycle()
 {
-	set_address(SF10A_BASEADDR);
-
 	/* Collect results */
 	if (OK != collect()) {
 		DEVICE_DEBUG("collection error");
@@ -594,24 +593,17 @@ SF10A::cycle()
 		return;
 	}
 
-
-	/* Trigger measurement */
-	if (OK != measure()) {
-		DEVICE_DEBUG("measure error lidar");
-	}
-
-
-
 	/* schedule a fresh cycle call when the measurement is done */
 	work_queue(HPWORK,
 		   &_work,
-		   (worker_t)&SF10A::cycle_trampoline,
+		   (worker_t)&SF1XX::cycle_trampoline,
 		   this,
-		   USEC2TICK(SF10A_CONVERSION_INTERVAL));
+		   USEC2TICK(SF1XX_CONVERSION_INTERVAL));
+
 }
 
 void
-SF10A::print_info()
+SF1XX::print_info()
 {
 	perf_print_counter(_sample_perf);
 	perf_print_counter(_comms_errors);
@@ -623,7 +615,7 @@ SF10A::print_info()
 /**
  * Local functions in support of the shell command.
  */
-namespace sf10a
+namespace sf1xx
 {
 
 /* oddly, ERROR is not defined for c++ */
@@ -632,7 +624,7 @@ namespace sf10a
 #endif
 const int ERROR = -1;
 
-SF10A	*g_dev;
+SF1XX	*g_dev;
 
 void	start();
 void	stop();
@@ -653,7 +645,7 @@ start()
 	}
 
 	/* create the driver */
-	g_dev = new SF10A(SF10A_BUS);
+	g_dev = new SF1XX(SF1XX_BUS);
 
 	if (g_dev == nullptr) {
 		goto fail;
@@ -664,7 +656,7 @@ start()
 	}
 
 	/* set the poll rate to default, starts automatic data collection */
-	fd = open(SF10A_DEVICE_PATH, O_RDONLY);
+	fd = open(SF1XX_DEVICE_PATH, O_RDONLY);
 
 	if (fd < 0) {
 		goto fail;
@@ -714,10 +706,10 @@ test()
 	ssize_t sz;
 	int ret;
 
-	int fd = open(SF10A_DEVICE_PATH, O_RDONLY);
+	int fd = open(SF1XX_DEVICE_PATH, O_RDONLY);
 
 	if (fd < 0) {
-		err(1, "%s open failed (try 'sf10a start' if the driver is not running", SF10A_DEVICE_PATH);
+		err(1, "%s open failed (try 'sf1xx start' if the driver is not running", SF1XX_DEVICE_PATH);
 	}
 
 	/* do a simple demand read */
@@ -777,7 +769,7 @@ test()
 void
 reset()
 {
-	int fd = open(SF10A_DEVICE_PATH, O_RDONLY);
+	int fd = open(SF1XX_DEVICE_PATH, O_RDONLY);
 
 	if (fd < 0) {
 		err(1, "failed ");
@@ -813,41 +805,41 @@ info()
 } /* namespace */
 
 int
-sf10a_main(int argc, char *argv[])
+sf1xx_main(int argc, char *argv[])
 {
 	/*
 	 * Start/load the driver.
 	 */
 	if (!strcmp(argv[1], "start")) {
-		sf10a::start();
+		sf1xx::start();
 	}
 
 	/*
 	 * Stop the driver
 	 */
 	if (!strcmp(argv[1], "stop")) {
-		sf10a::stop();
+		sf1xx::stop();
 	}
 
 	/*
 	 * Test the driver/device.
 	 */
 	if (!strcmp(argv[1], "test")) {
-		sf10a::test();
+		sf1xx::test();
 	}
 
 	/*
 	 * Reset the driver.
 	 */
 	if (!strcmp(argv[1], "reset")) {
-		sf10a::reset();
+		sf1xx::reset();
 	}
 
 	/*
 	 * Print driver information.
 	 */
 	if (!strcmp(argv[1], "info") || !strcmp(argv[1], "status")) {
-		sf10a::info();
+		sf1xx::info();
 	}
 
 	errx(1, "unrecognized command, try 'start', 'test', 'reset' or 'info'");

From 6d3aba1c5f778ad19045cf61ed5a66bcbaed9963 Mon Sep 17 00:00:00 2001
From: Vasily Evseenko <svpcom@gmail.com>
Date: Thu, 28 Jul 2016 21:36:08 +0300
Subject: [PATCH 1201/1230] Fix code style

---
 src/drivers/sf1xx/sf1xx.cpp | 6 +++---
 1 file changed, 3 insertions(+), 3 deletions(-)

diff --git a/src/drivers/sf1xx/sf1xx.cpp b/src/drivers/sf1xx/sf1xx.cpp
index 6dc55d4916..043f123f9a 100644
--- a/src/drivers/sf1xx/sf1xx.cpp
+++ b/src/drivers/sf1xx/sf1xx.cpp
@@ -265,7 +265,7 @@ SF1XX::init()
 		DEVICE_LOG("failed to create distance_sensor object. Did you start uOrb?");
 	}
 
-        // Select altitude register
+	// Select altitude register
 	int ret2 = measure();
 
 	if (ret2 == 0) {
@@ -561,8 +561,8 @@ SF1XX::start()
 	/* reset the report ring and state machine */
 	_reports->flush();
 
-        /* set register to '0' */
-        measure();
+	/* set register to '0' */
+	measure();
 
 	/* schedule a cycle to start things */
 	work_queue(HPWORK, &_work, (worker_t)&SF1XX::cycle_trampoline, this, 5);

From 6689a3d1b1df17d42c76949ec14006888a2d4667 Mon Sep 17 00:00:00 2001
From: Vasily Evseenko <svpcom@gmail.com>
Date: Fri, 29 Jul 2016 16:17:02 +0300
Subject: [PATCH 1202/1230] Add SENS_EN_SF1XX param to specify sensor model Fix
 missing orb_unadvertise on shutdown Fix missing closing of file descriptors

---
 src/drivers/sf1xx/sf1xx.cpp         | 81 +++++++++++++++++++++--------
 src/modules/sensors/sensor_params.c | 15 ++++++
 2 files changed, 75 insertions(+), 21 deletions(-)

diff --git a/src/drivers/sf1xx/sf1xx.cpp b/src/drivers/sf1xx/sf1xx.cpp
index 043f123f9a..1f282b459a 100644
--- a/src/drivers/sf1xx/sf1xx.cpp
+++ b/src/drivers/sf1xx/sf1xx.cpp
@@ -80,15 +80,6 @@
 #define SF1XX_BASEADDR 	0x55
 #define SF1XX_DEVICE_PATH	"/dev/sf1xx"
 
-/* Device limits */
-#define SF1XX_MIN_DISTANCE 	(0.01f)
-#define SF1XX_MAX_DISTANCE 	(120.0f)
-
-
-/* conversion rates */
-
-#define SF1XX_CONVERSION_INTERVAL 		50000		// Maximum rate according to datasheet is 20Hz
-
 
 /* oddly, ERROR is not defined for c++ */
 #ifdef ERROR
@@ -122,12 +113,13 @@ protected:
 private:
 	float				_min_distance;
 	float				_max_distance;
+	int                             _conversion_interval;
 	work_s				_work;
-	ringbuffer::RingBuffer	*_reports;
+	ringbuffer::RingBuffer  *_reports;
 	bool				_sensor_ok;
-	int					_measure_ticks;
-	int					_class_instance;
-	int					_orb_class_instance;
+	int				_measure_ticks;
+	int				_class_instance;
+	int				_orb_class_instance;
 
 	orb_advert_t		_distance_sensor_topic;
 
@@ -194,8 +186,9 @@ extern "C" __EXPORT int sf1xx_main(int argc, char *argv[]);
 
 SF1XX::SF1XX(int bus, int address) :
 	I2C("SF1XX", SF1XX_DEVICE_PATH, bus, address, 400000),
-	_min_distance(SF1XX_MIN_DISTANCE),
-	_max_distance(SF1XX_MAX_DISTANCE),
+	_min_distance(-1.0f),
+	_max_distance(-1.0f),
+	_conversion_interval(-1),
 	_reports(nullptr),
 	_sensor_ok(false),
 	_measure_ticks(0),
@@ -224,6 +217,10 @@ SF1XX::~SF1XX()
 		delete _reports;
 	}
 
+	if (_distance_sensor_topic != nullptr) {
+		orb_unadvertise(_distance_sensor_topic);
+	}
+
 	if (_class_instance != -1) {
 		unregister_class_devname(RANGE_FINDER_BASE_DEVICE_PATH, _class_instance);
 	}
@@ -238,6 +235,43 @@ int
 SF1XX::init()
 {
 	int ret = ERROR;
+	int hw_model;
+	param_get(param_find("SENS_EN_SF1XX"), &hw_model);
+
+	switch (hw_model) {
+	case 0:
+		DEVICE_LOG("disabled.");
+		return ret;
+
+	case 1:  /* SF10/a (25m 32Hz) */
+		_min_distance = 0.01f;
+		_max_distance = 25.0f;
+		_conversion_interval = 31250;
+		break;
+
+	case 2:  /* SF10/b (50m 32Hz) */
+		_min_distance = 0.01f;
+		_max_distance = 50.0f;
+		_conversion_interval = 31250;
+		break;
+
+	case 3:  /* SF10/c (100m 16Hz) */
+		_min_distance = 0.01f;
+		_max_distance = 100.0f;
+		_conversion_interval = 62500;
+		break;
+
+	case 4:
+		/* SF11/c (120m 20Hz) */
+		_min_distance = 0.01f;
+		_max_distance = 120.0f;
+		_conversion_interval = 50000;
+		break;
+
+	default:
+		DEVICE_LOG("invalid HW model %d.", hw_model);
+		return ret;
+	}
 
 	/* do I2C init (and probe) first */
 	if (I2C::init() != OK) {
@@ -271,7 +305,8 @@ SF1XX::init()
 	if (ret2 == 0) {
 		ret = OK;
 		_sensor_ok = true;
-		DEVICE_LOG("SF1xx with address %d found", SF1XX_BASEADDR);
+		DEVICE_LOG("(%dm %dHz) with address %d found", (int)_max_distance,
+			   (int)(1e6f / _conversion_interval), SF1XX_BASEADDR);
 	}
 
 	return ret;
@@ -335,7 +370,7 @@ SF1XX::ioctl(struct file *filp, int cmd, unsigned long arg)
 					bool want_start = (_measure_ticks == 0);
 
 					/* set interval for next measurement to minimum legal value */
-					_measure_ticks = USEC2TICK(SF1XX_CONVERSION_INTERVAL);
+					_measure_ticks = USEC2TICK(_conversion_interval);
 
 					/* if we need to start the poll state machine, do it */
 					if (want_start) {
@@ -355,7 +390,7 @@ SF1XX::ioctl(struct file *filp, int cmd, unsigned long arg)
 					int ticks = USEC2TICK(1000000 / arg);
 
 					/* check against maximum rate */
-					if (ticks < USEC2TICK(SF1XX_CONVERSION_INTERVAL)) {
+					if (ticks < USEC2TICK(_conversion_interval)) {
 						return -EINVAL;
 					}
 
@@ -464,7 +499,7 @@ SF1XX::read(struct file *filp, char *buffer, size_t buflen)
 		}
 
 		/* wait for it to complete */
-		usleep(SF1XX_CONVERSION_INTERVAL);
+		usleep(_conversion_interval);
 
 		/* run the collection phase */
 		if (OK != collect()) {
@@ -598,7 +633,7 @@ SF1XX::cycle()
 		   &_work,
 		   (worker_t)&SF1XX::cycle_trampoline,
 		   this,
-		   USEC2TICK(SF1XX_CONVERSION_INTERVAL));
+		   USEC2TICK(_conversion_interval));
 
 }
 
@@ -638,7 +673,7 @@ void	info();
 void
 start()
 {
-	int fd;
+	int fd = -1;
 
 	if (g_dev != nullptr) {
 		errx(1, "already started");
@@ -663,9 +698,11 @@ start()
 	}
 
 	if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) {
+		::close(fd);
 		goto fail;
 	}
 
+	::close(fd);
 	exit(0);
 
 fail:
@@ -760,6 +797,7 @@ test()
 		errx(1, "failed to set default poll rate");
 	}
 
+	::close(fd);
 	errx(0, "PASS");
 }
 
@@ -783,6 +821,7 @@ reset()
 		err(1, "driver poll restart failed");
 	}
 
+	::close(fd);
 	exit(0);
 }
 
diff --git a/src/modules/sensors/sensor_params.c b/src/modules/sensors/sensor_params.c
index 710b4154d2..6d62f81968 100644
--- a/src/modules/sensors/sensor_params.c
+++ b/src/modules/sensors/sensor_params.c
@@ -3064,6 +3064,21 @@ PARAM_DEFINE_INT32(SENS_EN_SF0X, 0);
  */
 PARAM_DEFINE_INT32(SENS_EN_MB12XX, 0);
 
+/**
+ * Lightware SF1xx laser rangefinder
+ *
+ * @reboot_required true
+ * @min 0
+ * @max 4
+ * @group Sensor Enable
+ * @value 0 Disabled
+ * @value 1 SF10/a (25m)
+ * @value 2 SF10/b (50m)
+ * @value 3 SF10/c (100m)
+ * @value 4 SF11/c (120m)
+ */
+PARAM_DEFINE_INT32(SENS_EN_SF1XX, 0);
+
 /**
  * Set the minimum PWM for the MAIN outputs
  *

From 078aca19665c4088dde329a8d26ab0be991fd9da Mon Sep 17 00:00:00 2001
From: Vasily Evseenko <svpcom@gmail.com>
Date: Fri, 29 Jul 2016 18:29:04 +0300
Subject: [PATCH 1203/1230] Fix SENS_EN_SF1XX description

---
 src/modules/sensors/sensor_params.c | 8 ++++----
 1 file changed, 4 insertions(+), 4 deletions(-)

diff --git a/src/modules/sensors/sensor_params.c b/src/modules/sensors/sensor_params.c
index 6d62f81968..d66fdb3121 100644
--- a/src/modules/sensors/sensor_params.c
+++ b/src/modules/sensors/sensor_params.c
@@ -3072,10 +3072,10 @@ PARAM_DEFINE_INT32(SENS_EN_MB12XX, 0);
  * @max 4
  * @group Sensor Enable
  * @value 0 Disabled
- * @value 1 SF10/a (25m)
- * @value 2 SF10/b (50m)
- * @value 3 SF10/c (100m)
- * @value 4 SF11/c (120m)
+ * @value 1 SF10/a
+ * @value 2 SF10/b
+ * @value 3 SF10/c
+ * @value 4 SF11/c
  */
 PARAM_DEFINE_INT32(SENS_EN_SF1XX, 0);
 

From b4d9f449046d2bdb81205104e81b5947bf3b9e09 Mon Sep 17 00:00:00 2001
From: Vasily Evseenko <svpcom@gmail.com>
Date: Sun, 31 Jul 2016 00:42:25 +0300
Subject: [PATCH 1204/1230] Wait conversion_interval before first measurement

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

diff --git a/src/drivers/sf1xx/sf1xx.cpp b/src/drivers/sf1xx/sf1xx.cpp
index 1f282b459a..7c317bd775 100644
--- a/src/drivers/sf1xx/sf1xx.cpp
+++ b/src/drivers/sf1xx/sf1xx.cpp
@@ -600,7 +600,7 @@ SF1XX::start()
 	measure();
 
 	/* schedule a cycle to start things */
-	work_queue(HPWORK, &_work, (worker_t)&SF1XX::cycle_trampoline, this, 5);
+	work_queue(HPWORK, &_work, (worker_t)&SF1XX::cycle_trampoline, this, USEC2TICK(_conversion_interval));
 }
 
 void

From aa0c89c3ec8e5e4f3fc456c37cf81c0448e78208 Mon Sep 17 00:00:00 2001
From: Vasily Evseenko <svpcom@gmail.com>
Date: Mon, 1 Aug 2016 01:45:46 +0300
Subject: [PATCH 1205/1230] Fix i2c collision with rgbled

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

diff --git a/src/drivers/sf1xx/sf1xx.cpp b/src/drivers/sf1xx/sf1xx.cpp
index 7c317bd775..a2b9e13f5f 100644
--- a/src/drivers/sf1xx/sf1xx.cpp
+++ b/src/drivers/sf1xx/sf1xx.cpp
@@ -38,7 +38,7 @@
  * @author Vasily Evseenko <svpcom@gmail.com>
  *
  * Driver for the Lightware SF1xx lidar range finder series.
- * Default I2C address 0x55 is used.
+ * Default I2C address 0x66 is used.
  */
 
 #include <px4_config.h>
@@ -77,7 +77,7 @@
 
 /* Configuration Constants */
 #define SF1XX_BUS 		PX4_I2C_BUS_EXPANSION
-#define SF1XX_BASEADDR 	0x55
+#define SF1XX_BASEADDR 	0x66
 #define SF1XX_DEVICE_PATH	"/dev/sf1xx"
 
 

From aaeeb1684d621e7fad6b64a455c19ef37ee7d72b Mon Sep 17 00:00:00 2001
From: Julian Oes <julian@oes.ch>
Date: Thu, 4 Aug 2016 16:04:02 +0200
Subject: [PATCH 1206/1230] DriverFramework: update submodule (#5233)

This brings support for the Ak8963 for Bebop.
---
 src/lib/DriverFramework | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/src/lib/DriverFramework b/src/lib/DriverFramework
index 9f53ea2c2a..8658b2e40e 160000
--- a/src/lib/DriverFramework
+++ b/src/lib/DriverFramework
@@ -1 +1 @@
-Subproject commit 9f53ea2c2a4f3ea330153e6da02dbad8632f3cb1
+Subproject commit 8658b2e40ee41c75d0a2a445fccbb07fd637f836

From 8aa3b245a59726ede1a9534425cd73f1d90bef96 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Beat=20K=C3=BCng?= <beat-kueng@gmx.net>
Date: Thu, 4 Aug 2016 17:18:03 +0200
Subject: [PATCH 1207/1230] fix sdlog2: use LOG_DGPS_MSG for second gps (#5235)

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

diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c
index a787a76e15..e0815b7a3a 100644
--- a/src/modules/sdlog2/sdlog2.c
+++ b/src/modules/sdlog2/sdlog2.c
@@ -1759,7 +1759,7 @@ int sdlog2_thread_main(int argc, char *argv[])
 
 			/* --- GPS POSITION - UNIT #2 --- */
 			if (copy_if_updated_multi(ORB_ID(vehicle_gps_position), 1, &subs.gps_pos_sub[1], &buf.dual_gps_pos)) {
-				log_msg.msg_type = LOG_GPS_MSG;
+				log_msg.msg_type = LOG_DGPS_MSG;
 				log_msg.body.log_GPS.gps_time = buf.dual_gps_pos.time_utc_usec;
 				log_msg.body.log_GPS.fix_type = buf.dual_gps_pos.fix_type;
 				log_msg.body.log_GPS.eph = buf.dual_gps_pos.eph;

From 815c367294e81d22fdf81a7784357b967f7281c1 Mon Sep 17 00:00:00 2001
From: Bart Slinger <bartslinger@users.noreply.github.com>
Date: Thu, 4 Aug 2016 17:26:17 +0200
Subject: [PATCH 1208/1230] Add helicopter airframe icon (#5234)

---
 Tools/px4airframes/xmlout.py | 2 ++
 1 file changed, 2 insertions(+)

diff --git a/Tools/px4airframes/xmlout.py b/Tools/px4airframes/xmlout.py
index 378c057f70..758343f8c8 100644
--- a/Tools/px4airframes/xmlout.py
+++ b/Tools/px4airframes/xmlout.py
@@ -69,6 +69,8 @@ class XMLOutput():
                 xml_group.attrib["image"] = "VTOLTiltRotor"
             elif (group.GetName() == "Coaxial Helicopter"):
                 xml_group.attrib["image"] = "HelicopterCoaxial"
+            elif (group.GetName() == "Helicopter"):
+                xml_group.attrib["image"] = "Helicopter"
             elif (group.GetName() == "Hexarotor Coaxial"):
                 xml_group.attrib["image"] = "Y6A"
             elif (group.GetName() == "Y6B"):

From db174cf8b1b108b864724a8448dba80f2440f357 Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Fri, 5 Aug 2016 10:12:44 +0200
Subject: [PATCH 1209/1230] Disable EKF2 3D fusion temporarily in SITL, fix
 missing fast-init params for some configs

---
 posix-configs/SITL/init/rcS_gazebo_iris          | 1 +
 posix-configs/SITL/init/rcS_gazebo_plane         | 3 +++
 posix-configs/SITL/init/rcS_gazebo_solo          | 1 +
 posix-configs/SITL/init/rcS_gazebo_standard_vtol | 3 +++
 posix-configs/SITL/init/rcS_gazebo_tailsitter    | 3 +++
 posix-configs/SITL/init/rcS_gazebo_typhoon_h480  | 1 +
 posix-configs/SITL/init/rcS_jmavsim_iris         | 1 +
 posix-configs/SITL/init/rcS_multiple_gazebo_iris | 6 ++++--
 8 files changed, 17 insertions(+), 2 deletions(-)

diff --git a/posix-configs/SITL/init/rcS_gazebo_iris b/posix-configs/SITL/init/rcS_gazebo_iris
index 02add40647..d985929601 100644
--- a/posix-configs/SITL/init/rcS_gazebo_iris
+++ b/posix-configs/SITL/init/rcS_gazebo_iris
@@ -41,6 +41,7 @@ param set MPC_Z_VEL_P 0.6
 param set MPC_Z_VEL_I 0.15
 param set EKF2_GBIAS_INIT 0.01
 param set EKF2_ANGERR_INIT 0.01
+param set EKF2_MAG_TYPE 1
 replay tryapplyparams
 simulator start -s
 rgbledsim start
diff --git a/posix-configs/SITL/init/rcS_gazebo_plane b/posix-configs/SITL/init/rcS_gazebo_plane
index 19844e69bd..b4f40ce2dd 100644
--- a/posix-configs/SITL/init/rcS_gazebo_plane
+++ b/posix-configs/SITL/init/rcS_gazebo_plane
@@ -25,6 +25,9 @@ param set COM_RC_IN_MODE 1
 param set NAV_DLL_ACT 2
 param set NAV_ACC_RAD 15.0
 param set FW_THR_IDLE 0.8
+param set EKF2_GBIAS_INIT 0.01
+param set EKF2_ANGERR_INIT 0.01
+param set EKF2_MAG_TYPE 1
 replay tryapplyparams
 simulator start -s
 rgbledsim start
diff --git a/posix-configs/SITL/init/rcS_gazebo_solo b/posix-configs/SITL/init/rcS_gazebo_solo
index a075fdff30..008261b6ec 100644
--- a/posix-configs/SITL/init/rcS_gazebo_solo
+++ b/posix-configs/SITL/init/rcS_gazebo_solo
@@ -38,6 +38,7 @@ param set MPC_Z_VEL_P 0.6
 param set MPC_Z_VEL_I 0.15
 param set EKF2_GBIAS_INIT 0.01
 param set EKF2_ANGERR_INIT 0.01
+param set EKF2_MAG_TYPE 1
 replay tryapplyparams
 simulator start -s
 rgbledsim start
diff --git a/posix-configs/SITL/init/rcS_gazebo_standard_vtol b/posix-configs/SITL/init/rcS_gazebo_standard_vtol
index 71420634da..a1f448bb58 100644
--- a/posix-configs/SITL/init/rcS_gazebo_standard_vtol
+++ b/posix-configs/SITL/init/rcS_gazebo_standard_vtol
@@ -44,6 +44,9 @@ param set RTL_LAND_DELAY 0
 param set COM_DISARM_LAND 5
 param set COM_DL_LOSS_EN 1
 param set MPC_ACC_HOR_MAX 2
+param set EKF2_GBIAS_INIT 0.01
+param set EKF2_ANGERR_INIT 0.01
+param set EKF2_MAG_TYPE 1
 replay tryapplyparams
 simulator start -s 
 rgbledsim start
diff --git a/posix-configs/SITL/init/rcS_gazebo_tailsitter b/posix-configs/SITL/init/rcS_gazebo_tailsitter
index ea37d6f431..2ba72c5b8c 100644
--- a/posix-configs/SITL/init/rcS_gazebo_tailsitter
+++ b/posix-configs/SITL/init/rcS_gazebo_tailsitter
@@ -35,6 +35,9 @@ param set SENS_DPRES_OFF 0.001
 param set SENS_BOARD_X_OFF 0.000001
 param set COM_RC_IN_MODE 1
 param set NAV_DLL_ACT 2
+param set EKF2_GBIAS_INIT 0.01
+param set EKF2_ANGERR_INIT 0.01
+param set EKF2_MAG_TYPE 1
 replay tryapplyparams
 simulator start -s
 rgbledsim start
diff --git a/posix-configs/SITL/init/rcS_gazebo_typhoon_h480 b/posix-configs/SITL/init/rcS_gazebo_typhoon_h480
index 392ff03c94..87c620c750 100644
--- a/posix-configs/SITL/init/rcS_gazebo_typhoon_h480
+++ b/posix-configs/SITL/init/rcS_gazebo_typhoon_h480
@@ -40,6 +40,7 @@ param set MPC_XY_VEL_P 0.15
 param set MPC_XY_VEL_I 0.2
 param set EKF2_GBIAS_INIT 0.01
 param set EKF2_ANGERR_INIT 0.01
+param set EKF2_MAG_TYPE 1
 replay tryapplyparams
 simulator start -s
 rgbledsim start
diff --git a/posix-configs/SITL/init/rcS_jmavsim_iris b/posix-configs/SITL/init/rcS_jmavsim_iris
index 1e44f2b13a..26db22d8f0 100644
--- a/posix-configs/SITL/init/rcS_jmavsim_iris
+++ b/posix-configs/SITL/init/rcS_jmavsim_iris
@@ -41,6 +41,7 @@ param set MPC_Z_VEL_MAX 2.0
 param set MPC_Z_VEL_P 0.4
 param set EKF2_GBIAS_INIT 0.01
 param set EKF2_ANGERR_INIT 0.01
+param set EKF2_MAG_TYPE 1
 replay tryapplyparams
 simulator start -s
 rgbledsim start
diff --git a/posix-configs/SITL/init/rcS_multiple_gazebo_iris b/posix-configs/SITL/init/rcS_multiple_gazebo_iris
index fbd5b5a643..d1ee2acc1e 100644
--- a/posix-configs/SITL/init/rcS_multiple_gazebo_iris
+++ b/posix-configs/SITL/init/rcS_multiple_gazebo_iris
@@ -32,6 +32,9 @@ param set COM_RC_IN_MODE 1
 param set NAV_ACC_RAD 2.0
 param set RTL_RETURN_ALT 30.0
 param set RTL_DESCEND_ALT 10.0
+param set EKF2_GBIAS_INIT 0.01
+param set EKF2_ANGERR_INIT 0.01
+param set EKF2_MAG_TYPE 1
 replay tryapplyparams
 rgbledsim start
 tone_alarm start
@@ -46,8 +49,7 @@ sensors start
 commander start
 land_detector start multicopter
 navigator start
-attitude_estimator_q start
-position_estimator_inav start
+ekf2 start
 mc_pos_control start
 mc_att_control start
 mixer load /dev/pwm_output0 quad_w.main.mix

From 074a71173bfd6eb276d9d78801ab6b80b017ff18 Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Fri, 5 Aug 2016 10:13:24 +0200
Subject: [PATCH 1210/1230] Systemlib: Address init and copy constructors

---
 src/modules/systemlib/state_table.h | 5 ++++-
 1 file changed, 4 insertions(+), 1 deletion(-)

diff --git a/src/modules/systemlib/state_table.h b/src/modules/systemlib/state_table.h
index f28b9180c1..7f657f4fc8 100644
--- a/src/modules/systemlib/state_table.h
+++ b/src/modules/systemlib/state_table.h
@@ -51,7 +51,10 @@ public:
 	};
 
 	StateTable(Tran const *table, unsigned nStates, unsigned nSignals)
-		: myTable(table), myNsignals(nSignals) {}
+		: myState(0), myTable(table), myNsignals(nSignals) {}
+
+	StateTable(const StateTable &) = delete;
+	StateTable &operator=(const StateTable &) = delete;
 
 #define NO_ACTION &StateTable::doNothing
 #define ACTION(_target) StateTable::Action(_target)

From 0481c002df60b797efa0caf68111a4493b115669 Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Fri, 5 Aug 2016 10:13:52 +0200
Subject: [PATCH 1211/1230] Navigator: Initialize subscription and disallow
 copy constructors for classes that have pointer members

---
 src/modules/navigator/follow_target.h               | 4 ++++
 src/modules/navigator/geofence.h                    | 4 ++++
 src/modules/navigator/mission_block.h               | 3 +++
 src/modules/navigator/mission_feasibility_checker.h | 4 ++++
 src/modules/navigator/navigator_main.cpp            | 1 +
 5 files changed, 16 insertions(+)

diff --git a/src/modules/navigator/follow_target.h b/src/modules/navigator/follow_target.h
index 52fa108560..cc6891a8ac 100644
--- a/src/modules/navigator/follow_target.h
+++ b/src/modules/navigator/follow_target.h
@@ -52,6 +52,10 @@ class FollowTarget : public MissionBlock
 
 public:
 	FollowTarget(Navigator *navigator, const char *name);
+
+	FollowTarget(const FollowTarget &) = delete;
+	FollowTarget &operator=(const FollowTarget &) = delete;
+
 	~FollowTarget();
 
 	void on_inactive() override;
diff --git a/src/modules/navigator/geofence.h b/src/modules/navigator/geofence.h
index ee8f61e484..21f5072158 100644
--- a/src/modules/navigator/geofence.h
+++ b/src/modules/navigator/geofence.h
@@ -59,6 +59,10 @@ class Geofence : public control::SuperBlock
 {
 public:
 	Geofence(Navigator *navigator);
+
+	Geofence(const Geofence &) = delete;
+	Geofence &operator=(const Geofence &) = delete;
+
 	~Geofence();
 
 	/* Altitude mode, corresponding to the param GF_ALTMODE */
diff --git a/src/modules/navigator/mission_block.h b/src/modules/navigator/mission_block.h
index 3b95c7a4ee..54da136d52 100644
--- a/src/modules/navigator/mission_block.h
+++ b/src/modules/navigator/mission_block.h
@@ -64,6 +64,9 @@ public:
 	 */
 	MissionBlock(Navigator *navigator, const char *name);
 
+	MissionBlock(const MissionBlock &) = delete;
+	MissionBlock &operator=(const MissionBlock &) = delete;
+
 	/**
 	 * Destructor
 	 */
diff --git a/src/modules/navigator/mission_feasibility_checker.h b/src/modules/navigator/mission_feasibility_checker.h
index db19e60dc6..6c5dda73ab 100644
--- a/src/modules/navigator/mission_feasibility_checker.h
+++ b/src/modules/navigator/mission_feasibility_checker.h
@@ -78,6 +78,10 @@ private:
 public:
 
 	MissionFeasibilityChecker();
+
+	MissionFeasibilityChecker(const MissionFeasibilityChecker &) = delete;
+	MissionFeasibilityChecker &operator=(const MissionFeasibilityChecker &) = delete;
+
 	~MissionFeasibilityChecker() {}
 
 	/*
diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp
index 2b66a73c8e..6804fe1be0 100644
--- a/src/modules/navigator/navigator_main.cpp
+++ b/src/modules/navigator/navigator_main.cpp
@@ -105,6 +105,7 @@ Navigator::Navigator() :
 	_mavlink_log_pub(nullptr),
 	_global_pos_sub(-1),
 	_gps_pos_sub(-1),
+	_sensor_combined_sub(-1),
 	_home_pos_sub(-1),
 	_vstatus_sub(-1),
 	_land_detected_sub(-1),

From e600e29ea4c12ecef8c6d3c838146941513d78ea Mon Sep 17 00:00:00 2001
From: Michael Schaeuble <schaeuble.michael@gmail.com>
Date: Sun, 10 Jul 2016 23:08:58 +0200
Subject: [PATCH 1212/1230] Add DF wrapper for AK8963

---
 cmake/configs/posix_bebop_default.cmake       |   4 +
 posix-configs/bebop/px4.config                |   1 +
 .../drivers/df_ak8963_wrapper/CMakeLists.txt  |  46 ++
 .../df_ak8963_wrapper/df_ak8963_wrapper.cpp   | 459 ++++++++++++++++++
 4 files changed, 510 insertions(+)
 create mode 100644 src/platforms/posix/drivers/df_ak8963_wrapper/CMakeLists.txt
 create mode 100644 src/platforms/posix/drivers/df_ak8963_wrapper/df_ak8963_wrapper.cpp

diff --git a/cmake/configs/posix_bebop_default.cmake b/cmake/configs/posix_bebop_default.cmake
index 0393e2081d..a1c75d9565 100644
--- a/cmake/configs/posix_bebop_default.cmake
+++ b/cmake/configs/posix_bebop_default.cmake
@@ -23,7 +23,11 @@ set(config_module_list
 	drivers/device
 	modules/sensors
 	platforms/posix/drivers/df_ms5607_wrapper
+<<<<<<< c4eb65862f307de4b679e24f44fab0c565d7c2a3
 	platforms/posix/drivers/df_mpu6050_wrapper
+=======
+	platforms/posix/drivers/df_ak8963_wrapper
+>>>>>>> Add DF wrapper for AK8963
 
 	#
 	# System commands
diff --git a/posix-configs/bebop/px4.config b/posix-configs/bebop/px4.config
index 25370f234c..b86c389dee 100644
--- a/posix-configs/bebop/px4.config
+++ b/posix-configs/bebop/px4.config
@@ -5,6 +5,7 @@ param set SYS_AUTOSTART 4001
 param set MAV_BROADCAST 1
 df_ms5607_wrapper start
 df_mpu6050_wrapper start -R 8
+df_ak8963_wrapper start
 sensors start
 sleep 1
 mavlink start -u 14556 -r 1000000
diff --git a/src/platforms/posix/drivers/df_ak8963_wrapper/CMakeLists.txt b/src/platforms/posix/drivers/df_ak8963_wrapper/CMakeLists.txt
new file mode 100644
index 0000000000..9ac1b2ef75
--- /dev/null
+++ b/src/platforms/posix/drivers/df_ak8963_wrapper/CMakeLists.txt
@@ -0,0 +1,46 @@
+############################################################################
+#
+#   Copyright (c) 2016 PX4 Development Team. All rights reserved.
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions
+# are met:
+#
+# 1. Redistributions of source code must retain the above copyright
+#    notice, this list of conditions and the following disclaimer.
+# 2. Redistributions in binary form must reproduce the above copyright
+#    notice, this list of conditions and the following disclaimer in
+#    the documentation and/or other materials provided with the
+#    distribution.
+# 3. Neither the name PX4 nor the names of its contributors may be
+#    used to endorse or promote products derived from this software
+#    without specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.
+#
+############################################################################
+
+include_directories(../../../../lib/DriverFramework/drivers)
+
+px4_add_module(
+	MODULE platforms__posix__drivers__df_ak8963_wrapper
+	MAIN df_ak8963_wrapper
+	SRCS
+		df_ak8963_wrapper.cpp
+	DEPENDS
+		platforms__common
+		df_driver_framework
+		df_ak8963
+	)
+# vim: set noet ft=cmake fenc=utf-8 ff=unix :
diff --git a/src/platforms/posix/drivers/df_ak8963_wrapper/df_ak8963_wrapper.cpp b/src/platforms/posix/drivers/df_ak8963_wrapper/df_ak8963_wrapper.cpp
new file mode 100644
index 0000000000..c58f06ccdd
--- /dev/null
+++ b/src/platforms/posix/drivers/df_ak8963_wrapper/df_ak8963_wrapper.cpp
@@ -0,0 +1,459 @@
+/****************************************************************************
+ *
+ * Copyright (c) 2016 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file df_ak8963_wrapper.cpp
+ * Lightweight driver to access the AK8963 of the DriverFramework.
+ */
+
+#include <px4_config.h>
+
+#include <sys/types.h>
+#include <sys/stat.h>
+#include <stdint.h>
+#include <stddef.h>
+#include <stdlib.h>
+#include <string.h>
+#include <math.h>
+#include <unistd.h>
+#include <px4_getopt.h>
+#include <errno.h>
+
+#include <systemlib/perf_counter.h>
+#include <systemlib/err.h>
+
+#include <drivers/drv_mag.h>
+
+#include <uORB/topics/parameter_update.h>
+
+#include <board_config.h>
+
+#include <lib/conversion/rotation.h>
+
+#include <ak8963/AK8963.hpp>
+#include <DevMgr.hpp>
+
+
+extern "C" { __EXPORT int df_ak8963_wrapper_main(int argc, char *argv[]); }
+
+using namespace DriverFramework;
+
+
+class DfAK8963Wrapper : public AK8963
+{
+public:
+	DfAK8963Wrapper(enum Rotation rotation);
+	~DfAK8963Wrapper();
+
+
+	/**
+	 * Start automatic measurement.
+	 *
+	 * @return 0 on success
+	 */
+	int		start();
+
+	/**
+	 * Stop automatic measurement.
+	 *
+	 * @return 0 on success
+	 */
+	int		stop();
+
+private:
+	int _publish(struct mag_sensor_data &data);
+
+	void _update_mag_calibration();
+
+	orb_advert_t		_mag_topic;
+
+	int			_param_update_sub;
+
+	struct mag_calibration_s {
+		float x_offset;
+		float x_scale;
+		float y_offset;
+		float y_scale;
+		float z_offset;
+		float z_scale;
+	} _mag_calibration;
+
+	math::Matrix<3, 3>      _rotation_matrix;
+
+	int			_mag_orb_class_instance;
+
+	perf_counter_t		_mag_sample_perf;
+
+};
+
+DfAK8963Wrapper::DfAK8963Wrapper(enum Rotation rotation) :
+	AK8963(MAG_DEVICE_PATH),
+	_mag_topic(nullptr),
+	_param_update_sub(-1),
+	_mag_calibration{},
+	_mag_orb_class_instance(-1),
+	_mag_sample_perf(perf_alloc(PC_ELAPSED, "df_mag_read"))
+{
+	// Set sane default calibration values
+	_mag_calibration.x_scale = 1.0f;
+	_mag_calibration.y_scale = 1.0f;
+	_mag_calibration.z_scale = 1.0f;
+	_mag_calibration.x_offset = 0.0f;
+	_mag_calibration.y_offset = 0.0f;
+	_mag_calibration.z_offset = 0.0f;
+
+	// Get sensor rotation matrix
+	get_rot_matrix(rotation, &_rotation_matrix);
+}
+
+DfAK8963Wrapper::~DfAK8963Wrapper()
+{
+	perf_free(_mag_sample_perf);
+}
+
+int DfAK8963Wrapper::start()
+{
+	/* Subscribe to param update topic. */
+	if (_param_update_sub < 0) {
+		_param_update_sub = orb_subscribe(ORB_ID(parameter_update));
+	}
+
+	/* Init device and start sensor. */
+	int ret = init();
+
+	if (ret != 0) {
+		PX4_ERR("AK8963 init fail: %d", ret);
+		return ret;
+	}
+
+	ret = AK8963::start();
+
+	if (ret != 0) {
+		PX4_ERR("AK8963 start fail: %d", ret);
+		return ret;
+	}
+
+	/* Force getting the calibration values. */
+	_update_mag_calibration();
+
+	return 0;
+}
+
+int DfAK8963Wrapper::stop()
+{
+	/* Stop sensor. */
+	int ret = AK8963::stop();
+
+	if (ret != 0) {
+		PX4_ERR("AK8963 stop fail: %d", ret);
+		return ret;
+	}
+
+	return 0;
+}
+
+void DfAK8963Wrapper::_update_mag_calibration()
+{
+	// TODO: replace magic number
+	for (unsigned i = 0; i < 3; ++i) {
+
+		// TODO: remove printfs and add error counter
+
+		char str[30];
+		(void)sprintf(str, "CAL_MAG%u_ID", i);
+		int32_t device_id;
+		int res = param_get(param_find(str), &device_id);
+
+		if (res != OK) {
+			PX4_ERR("Could not access param %s", str);
+			continue;
+		}
+
+		if ((uint32_t)device_id != m_id.dev_id) {
+			continue;
+		}
+
+		(void)sprintf(str, "CAL_MAG%u_XSCALE", i);
+		res = param_get(param_find(str), &_mag_calibration.x_scale);
+
+		if (res != OK) {
+			PX4_ERR("Could not access param %s", str);
+		}
+
+		(void)sprintf(str, "CAL_MAG%u_YSCALE", i);
+		res = param_get(param_find(str), &_mag_calibration.y_scale);
+
+		if (res != OK) {
+			PX4_ERR("Could not access param %s", str);
+		}
+
+		(void)sprintf(str, "CAL_MAG%u_ZSCALE", i);
+		res = param_get(param_find(str), &_mag_calibration.z_scale);
+
+		if (res != OK) {
+			PX4_ERR("Could not access param %s", str);
+		}
+
+		(void)sprintf(str, "CAL_MAG%u_XOFF", i);
+		res = param_get(param_find(str), &_mag_calibration.x_offset);
+
+		if (res != OK) {
+			PX4_ERR("Could not access param %s", str);
+		}
+
+		(void)sprintf(str, "CAL_MAG%u_YOFF", i);
+		res = param_get(param_find(str), &_mag_calibration.y_offset);
+
+		if (res != OK) {
+			PX4_ERR("Could not access param %s", str);
+		}
+
+		(void)sprintf(str, "CAL_MAG%u_ZOFF", i);
+		res = param_get(param_find(str), &_mag_calibration.z_offset);
+
+		if (res != OK) {
+			PX4_ERR("Could not access param %s", str);
+		}
+	}
+}
+
+
+int DfAK8963Wrapper::_publish(struct mag_sensor_data &data)
+{
+	/* Check if calibration values are still up-to-date. */
+	bool updated;
+	orb_check(_param_update_sub, &updated);
+
+	if (updated) {
+		parameter_update_s parameter_update;
+		orb_copy(ORB_ID(parameter_update), _param_update_sub, &parameter_update);
+
+		_update_mag_calibration();
+	}
+
+	/* Publish mag first. */
+	perf_begin(_mag_sample_perf);
+
+	mag_report mag_report = {};
+	mag_report.timestamp = hrt_absolute_time();
+
+  // TODO: check sensor orientation
+	/* The standard external mag by 3DR has x pointing to the
+	 * right, y pointing backwards, and z down, therefore switch x
+	 * and y and invert y. */
+	const float tmp = data.field_x_ga;
+	data.field_x_ga = -data.field_y_ga;
+	data.field_y_ga = tmp;
+
+	// TODO: remove these (or get the values)
+	mag_report.x_raw = NAN;
+	mag_report.y_raw = NAN;
+	mag_report.z_raw = NAN;
+
+
+	math::Vector<3> mag_val((data.field_x_ga - _mag_calibration.x_offset) * _mag_calibration.x_scale,
+				(data.field_y_ga - _mag_calibration.y_offset) * _mag_calibration.y_scale,
+				(data.field_z_ga - _mag_calibration.z_offset) * _mag_calibration.z_scale);
+
+	// apply sensor rotation on the accel measurement
+	mag_val = _rotation_matrix * mag_val;
+
+	mag_report.x = mag_val(0);
+	mag_report.y = mag_val(1);
+	mag_report.z = mag_val(2);
+
+
+	// TODO: get these right
+	//mag_report.scaling = -1.0f;
+	//mag_report.range_m_s2 = -1.0f;
+
+	mag_report.device_id = m_id.dev_id;
+
+	// TODO: when is this ever blocked?
+	if (!(m_pub_blocked)) {
+
+		if (_mag_topic == nullptr) {
+			_mag_topic = orb_advertise_multi(ORB_ID(sensor_mag), &mag_report,
+							 &_mag_orb_class_instance, ORB_PRIO_HIGH);
+
+		} else {
+			orb_publish(ORB_ID(sensor_mag), _mag_topic, &mag_report);
+		}
+
+	}
+
+	perf_end(_mag_sample_perf);
+
+	/* Notify anyone waiting for data. */
+	DevMgr::updateNotify(*this);
+
+	return 0;
+};
+
+
+namespace df_ak8963_wrapper
+{
+
+DfAK8963Wrapper *g_dev = nullptr;
+
+int start(enum Rotation rotation);
+int stop();
+int info();
+void usage();
+
+int start(enum Rotation rotation)
+{
+	g_dev = new DfAK8963Wrapper(rotation);
+
+	if (g_dev == nullptr) {
+		PX4_ERR("failed instantiating DfAK8963Wrapper object");
+		return -1;
+	}
+
+	int ret = g_dev->start();
+
+	if (ret != 0) {
+		PX4_ERR("DfAK8963Wrapper start failed");
+		return ret;
+	}
+
+	// Open the MAG sensor
+	DevHandle h;
+	DevMgr::getHandle(MAG_DEVICE_PATH, h);
+
+	if (!h.isValid()) {
+		DF_LOG_INFO("Error: unable to obtain a valid handle for the receiver at: %s (%d)",
+			    MAG_DEVICE_PATH, h.getError());
+		return -1;
+	}
+
+	DevMgr::releaseHandle(h);
+
+	return 0;
+}
+
+int stop()
+{
+	if (g_dev == nullptr) {
+		PX4_ERR("driver not running");
+		return 1;
+	}
+
+	int ret = g_dev->stop();
+
+	if (ret != 0) {
+		PX4_ERR("driver could not be stopped");
+		return ret;
+	}
+
+	delete g_dev;
+	g_dev = nullptr;
+	return 0;
+}
+
+/**
+ * Print a little info about the driver.
+ */
+int
+info()
+{
+	if (g_dev == nullptr) {
+		PX4_ERR("driver not running");
+		return 1;
+	}
+
+	PX4_DEBUG("state @ %p", g_dev);
+
+	return 0;
+}
+
+void
+usage()
+{
+	PX4_INFO("Usage: df_ak8963_wrapper 'start', 'info', 'stop'");
+	PX4_INFO("options:");
+	PX4_INFO("    -R rotation");
+}
+
+} // namespace df_ak8963_wrapper
+
+
+int
+df_ak8963_wrapper_main(int argc, char *argv[])
+{
+	int ch;
+	enum Rotation rotation = ROTATION_NONE;
+	int ret = 0;
+	int myoptind = 1;
+	const char *myoptarg = NULL;
+
+	/* jump over start/off/etc and look at options first */
+	while ((ch = px4_getopt(argc, argv, "R:", &myoptind, &myoptarg)) != EOF) {
+		switch (ch) {
+		case 'R':
+			rotation = (enum Rotation)atoi(myoptarg);
+			break;
+
+		default:
+			df_ak8963_wrapper::usage();
+			return 0;
+		}
+	}
+
+	if (argc <= 1) {
+		df_ak8963_wrapper::usage();
+		return 1;
+	}
+
+	const char *verb = argv[myoptind];
+
+
+	if (!strcmp(verb, "start")) {
+		ret = df_ak8963_wrapper::start(rotation);
+	}
+
+	else if (!strcmp(verb, "stop")) {
+		ret = df_ak8963_wrapper::stop();
+	}
+
+	else if (!strcmp(verb, "info")) {
+		ret = df_ak8963_wrapper::info();
+	}
+
+	else {
+		df_ak8963_wrapper::usage();
+		return 1;
+	}
+
+	return ret;
+}

From 0b7fa4f5ad72798a422c24290dba2f76f9bda8b0 Mon Sep 17 00:00:00 2001
From: Michael Schaeuble <schaeuble.michael@gmail.com>
Date: Thu, 21 Jul 2016 17:06:40 +0200
Subject: [PATCH 1213/1230] Apply sensor calibration after coordinate frame
 rotation

---
 .../df_ak8963_wrapper/df_ak8963_wrapper.cpp   | 32 ++++++++++---------
 1 file changed, 17 insertions(+), 15 deletions(-)

diff --git a/src/platforms/posix/drivers/df_ak8963_wrapper/df_ak8963_wrapper.cpp b/src/platforms/posix/drivers/df_ak8963_wrapper/df_ak8963_wrapper.cpp
index c58f06ccdd..9238a5e7de 100644
--- a/src/platforms/posix/drivers/df_ak8963_wrapper/df_ak8963_wrapper.cpp
+++ b/src/platforms/posix/drivers/df_ak8963_wrapper/df_ak8963_wrapper.cpp
@@ -90,6 +90,8 @@ public:
 	 */
 	int		stop();
 
+	void print_calibration();
+
 private:
 	int _publish(struct mag_sensor_data &data);
 
@@ -247,6 +249,15 @@ void DfAK8963Wrapper::_update_mag_calibration()
 	}
 }
 
+void DfAK8963Wrapper::print_calibration()
+{
+	PX4_INFO("calibration x_offset: %f", (double)_mag_calibration.x_offset);
+	PX4_INFO("calibration x_scale: %f", (double)_mag_calibration.x_scale);
+	PX4_INFO("calibration y_offset: %f", (double)_mag_calibration.y_offset);
+	PX4_INFO("calibration y_scale: %f", (double)_mag_calibration.y_scale);
+	PX4_INFO("calibration z_offset: %f", (double)_mag_calibration.z_offset);
+	PX4_INFO("calibration z_scale: %f", (double)_mag_calibration.z_scale);
+}
 
 int DfAK8963Wrapper::_publish(struct mag_sensor_data &data)
 {
@@ -267,30 +278,19 @@ int DfAK8963Wrapper::_publish(struct mag_sensor_data &data)
 	mag_report mag_report = {};
 	mag_report.timestamp = hrt_absolute_time();
 
-  // TODO: check sensor orientation
-	/* The standard external mag by 3DR has x pointing to the
-	 * right, y pointing backwards, and z down, therefore switch x
-	 * and y and invert y. */
-	const float tmp = data.field_x_ga;
-	data.field_x_ga = -data.field_y_ga;
-	data.field_y_ga = tmp;
-
 	// TODO: remove these (or get the values)
 	mag_report.x_raw = NAN;
 	mag_report.y_raw = NAN;
 	mag_report.z_raw = NAN;
 
-
-	math::Vector<3> mag_val((data.field_x_ga - _mag_calibration.x_offset) * _mag_calibration.x_scale,
-				(data.field_y_ga - _mag_calibration.y_offset) * _mag_calibration.y_scale,
-				(data.field_z_ga - _mag_calibration.z_offset) * _mag_calibration.z_scale);
+	math::Vector<3> mag_val(data.field_x_ga, data.field_y_ga, data.field_z_ga);
 
 	// apply sensor rotation on the accel measurement
 	mag_val = _rotation_matrix * mag_val;
 
-	mag_report.x = mag_val(0);
-	mag_report.y = mag_val(1);
-	mag_report.z = mag_val(2);
+	mag_report.x = (mag_val(0) - _mag_calibration.x_offset) * _mag_calibration.x_scale;;
+	mag_report.y = (mag_val(1) - _mag_calibration.y_offset) * _mag_calibration.y_scale;;
+	mag_report.z = (mag_val(2) - _mag_calibration.z_offset) * _mag_calibration.z_scale;;
 
 
 	// TODO: get these right
@@ -394,6 +394,8 @@ info()
 
 	PX4_DEBUG("state @ %p", g_dev);
 
+  g_dev->print_calibration();
+
 	return 0;
 }
 

From 6aa8fcdf53d53b5ba04539488a580ad07ff8a661 Mon Sep 17 00:00:00 2001
From: Michael Schaeuble <schaeuble.michael@gmail.com>
Date: Thu, 21 Jul 2016 17:09:23 +0200
Subject: [PATCH 1214/1230] Enable commander module for Parrot Bebop

---
 src/modules/commander/accelerometer_calibration.cpp |  8 ++++----
 src/modules/commander/gyro_calibration.cpp          |  6 +++---
 src/modules/commander/mag_calibration.cpp           | 12 ++++++------
 3 files changed, 13 insertions(+), 13 deletions(-)

diff --git a/src/modules/commander/accelerometer_calibration.cpp b/src/modules/commander/accelerometer_calibration.cpp
index 7b3cae8d3f..72e82cc1e2 100644
--- a/src/modules/commander/accelerometer_calibration.cpp
+++ b/src/modules/commander/accelerometer_calibration.cpp
@@ -175,7 +175,7 @@ typedef struct  {
 
 int do_accel_calibration(orb_advert_t *mavlink_log_pub)
 {
-#if !defined(__PX4_QURT) && !defined(__PX4_POSIX_EAGLE) && !defined(__PX4_POSIX_RPI)
+#if !defined(__PX4_QURT) && !defined(__PX4_POSIX_EAGLE) && !defined(__PX4_POSIX_RPI) && !defined(__PX4_POSIX_BEBOP)
 	int fd;
 #endif
 
@@ -195,7 +195,7 @@ int do_accel_calibration(orb_advert_t *mavlink_log_pub)
 
 	/* reset all sensors */
 	for (unsigned s = 0; s < max_accel_sens; s++) {
-#if !defined(__PX4_QURT) && !defined(__PX4_POSIX_EAGLE) && !defined(__PX4_POSIX_RPI)
+#if !defined(__PX4_QURT) && !defined(__PX4_POSIX_EAGLE) && !defined(__PX4_POSIX_RPI) && !defined(__PX4_POSIX_BEBOP)
 		sprintf(str, "%s%u", ACCEL_BASE_DEVICE_PATH, s);
 		/* reset all offsets to zero and all scales to one */
 		fd = px4_open(str, 0);
@@ -329,7 +329,7 @@ int do_accel_calibration(orb_advert_t *mavlink_log_pub)
 			return ERROR;
 		}
 
-#if !defined(__PX4_QURT) && !defined(__PX4_POSIX_EAGLE) && !defined(__PX4_POSIX_RPI)
+#if !defined(__PX4_QURT) && !defined(__PX4_POSIX_EAGLE) && !defined(__PX4_POSIX_RPI) && !defined(__PX4_POSIX_BEBOP)
 		sprintf(str, "%s%u", ACCEL_BASE_DEVICE_PATH, i);
 		fd = px4_open(str, 0);
 
@@ -419,7 +419,7 @@ calibrate_return do_accel_calibration_measurements(orb_advert_t *mavlink_log_pub
 			break;
 		}
 
-#if defined(__PX4_QURT) || defined(__PX4_POSIX_EAGLE) || defined(__PX4_POSIX_RPI)
+#if defined(__PX4_QURT) || defined(__PX4_POSIX_EAGLE) || defined(__PX4_POSIX_RPI) || defined(__PX4_POSIX_BEBOP)
 		// For QURT respectively the driver framework, we need to get the device ID by copying one report.
 		struct accel_report	accel_report;
 		orb_copy(ORB_ID(sensor_accel), worker_data.subs[i], &accel_report);
diff --git a/src/modules/commander/gyro_calibration.cpp b/src/modules/commander/gyro_calibration.cpp
index 945935e38d..26fa5886a9 100644
--- a/src/modules/commander/gyro_calibration.cpp
+++ b/src/modules/commander/gyro_calibration.cpp
@@ -185,7 +185,7 @@ int do_gyro_calibration(orb_advert_t *mavlink_log_pub)
 
 		// Reset all offsets to 0 and scales to 1
 		(void)memcpy(&worker_data.gyro_scale[s], &gyro_scale_zero, sizeof(gyro_scale_zero));
-#if !defined(__PX4_QURT) && !defined(__PX4_POSIX_EAGLE) && !defined(__PX4_POSIX_RPI)
+#if !defined(__PX4_QURT) && !defined(__PX4_POSIX_EAGLE) && !defined(__PX4_POSIX_RPI) && !defined(__PX4_POSIX_BEBOP)
 		sprintf(str, "%s%u", GYRO_BASE_DEVICE_PATH, s);
 		int fd = px4_open(str, 0);
 		if (fd >= 0) {
@@ -238,7 +238,7 @@ int do_gyro_calibration(orb_advert_t *mavlink_log_pub)
 	for (unsigned s = 0; s < gyro_count; s++) {
 
 		worker_data.gyro_sensor_sub[s] = orb_subscribe_multi(ORB_ID(sensor_gyro), s);
-#if defined(__PX4_QURT) || defined(__PX4_POSIX_EAGLE) || defined(__PX4_POSIX_RPI)
+#if defined(__PX4_QURT) || defined(__PX4_POSIX_EAGLE) || defined(__PX4_POSIX_RPI) || defined(__PX4_POSIX_BEBOP)
 		// For QURT respectively the driver framework, we need to get the device ID by copying one report.
 		struct gyro_report gyro_report;
 		orb_copy(ORB_ID(sensor_gyro), worker_data.gyro_sensor_sub[s], &gyro_report);
@@ -336,7 +336,7 @@ int do_gyro_calibration(orb_advert_t *mavlink_log_pub)
 				(void)sprintf(str, "CAL_GYRO%u_ID", s);
 				failed |= (OK != param_set_no_notification(param_find(str), &(worker_data.device_id[s])));
 
-#if !defined(__PX4_QURT) && !defined(__PX4_POSIX_EAGLE) && !defined(__PX4_POSIX_RPI)
+#if !defined(__PX4_QURT) && !defined(__PX4_POSIX_EAGLE) && !defined(__PX4_POSIX_RPI) && !defined(__PX4_POSIX_BEBOP)
 				/* apply new scaling and offsets */
 				(void)sprintf(str, "%s%u", GYRO_BASE_DEVICE_PATH, s);
 				int fd = px4_open(str, 0);
diff --git a/src/modules/commander/mag_calibration.cpp b/src/modules/commander/mag_calibration.cpp
index 10f1c5510d..20b93f10fa 100644
--- a/src/modules/commander/mag_calibration.cpp
+++ b/src/modules/commander/mag_calibration.cpp
@@ -124,7 +124,7 @@ int do_mag_calibration(orb_advert_t *mavlink_log_pub)
 	_last_mag_progress = 0;
 
 	for (unsigned cur_mag = 0; cur_mag < max_mags; cur_mag++) {
-#if !defined(__PX4_QURT) && !defined(__PX4_POSIX_RPI)
+#if !defined(__PX4_QURT) && !defined(__PX4_POSIX_RPI) && !defined(__PX4_POSIX_BEBOP)
 		// Reset mag id to mag not available
 		(void)sprintf(str, "CAL_MAG%u_ID", cur_mag);
 		result = param_set_no_notification(param_find(str), &(device_ids[cur_mag]));
@@ -166,7 +166,7 @@ int do_mag_calibration(orb_advert_t *mavlink_log_pub)
 #endif
 
 /* for calibration, commander will run on apps, so orb messages are used to get info from dsp */
-#if !defined(__PX4_QURT) && !defined(__PX4_POSIX_RPI)
+#if !defined(__PX4_QURT) && !defined(__PX4_POSIX_RPI) && !defined(__PX4_POSIX_BEBOP)
 		// Attempt to open mag
 		(void)sprintf(str, "%s%u", MAG_BASE_DEVICE_PATH, cur_mag);
 		int fd = px4_open(str, O_RDONLY);
@@ -508,7 +508,7 @@ calibrate_return mag_calibrate_all(orb_advert_t *mavlink_log_pub)
 			// Mag in this slot is available
 			worker_data.sub_mag[cur_mag] = orb_subscribe_multi(ORB_ID(sensor_mag), cur_mag);
 
-#if defined(__PX4_QURT) || defined(__PX4_POSIX_RPI)
+#if defined(__PX4_QURT) || defined(__PX4_POSIX_RPI) || defined(__PX4_POSIX_BEBOP)
 			// For QURT respectively the driver framework, we need to get the device ID by copying one report.
 			struct mag_report	mag_report;
 			orb_copy(ORB_ID(sensor_mag), worker_data.sub_mag[cur_mag], &mag_report);
@@ -664,7 +664,7 @@ calibrate_return mag_calibrate_all(orb_advert_t *mavlink_log_pub)
 		for (unsigned cur_mag=0; cur_mag<max_mags; cur_mag++) {
 			if (device_ids[cur_mag] != 0) {
 				struct mag_calibration_s mscale;
-#if !defined(__PX4_QURT) && !defined(__PX4_POSIX_RPI)
+#if !defined(__PX4_QURT) && !defined(__PX4_POSIX_RPI) && !defined(__PX4_POSIX_BEBOP)
 				int fd_mag = -1;
 
 				// Set new scale
@@ -688,7 +688,7 @@ calibrate_return mag_calibrate_all(orb_advert_t *mavlink_log_pub)
 					mscale.y_offset = sphere_y[cur_mag];
 					mscale.z_offset = sphere_z[cur_mag];
 
-#if !defined(__PX4_QURT) && !defined(__PX4_POSIX_RPI)
+#if !defined(__PX4_QURT) && !defined(__PX4_POSIX_RPI) && !defined(__PX4_POSIX_BEBOP)
 					if (px4_ioctl(fd_mag, MAGIOCSSCALE, (long unsigned int)&mscale) != OK) {
 						calibration_log_critical(mavlink_log_pub, CAL_ERROR_APPLY_CAL_MSG, cur_mag);
 						result = calibrate_return_error;
@@ -696,7 +696,7 @@ calibrate_return mag_calibrate_all(orb_advert_t *mavlink_log_pub)
 #endif
 				}
 
-#if !defined(__PX4_QURT) && !defined(__PX4_POSIX_RPI)
+#if !defined(__PX4_QURT) && !defined(__PX4_POSIX_RPI) && !defined(__PX4_POSIX_BEBOP)
 				// Mag device no longer needed
 				if (fd_mag >= 0) {
 					px4_close(fd_mag);

From 8351afdca8f402d8279e46832682946f1a2830eb Mon Sep 17 00:00:00 2001
From: Michael Schaeuble <schaeuble.michael@gmail.com>
Date: Thu, 21 Jul 2016 17:10:27 +0200
Subject: [PATCH 1215/1230] Initialize mag scaling with reasonable defaults

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

diff --git a/src/modules/commander/mag_calibration.cpp b/src/modules/commander/mag_calibration.cpp
index 20b93f10fa..7cfcc6a700 100644
--- a/src/modules/commander/mag_calibration.cpp
+++ b/src/modules/commander/mag_calibration.cpp
@@ -664,6 +664,10 @@ calibrate_return mag_calibrate_all(orb_advert_t *mavlink_log_pub)
 		for (unsigned cur_mag=0; cur_mag<max_mags; cur_mag++) {
 			if (device_ids[cur_mag] != 0) {
 				struct mag_calibration_s mscale;
+				mscale.x_scale = 1.0;
+				mscale.y_scale = 1.0;
+				mscale.z_scale = 1.0;
+
 #if !defined(__PX4_QURT) && !defined(__PX4_POSIX_RPI) && !defined(__PX4_POSIX_BEBOP)
 				int fd_mag = -1;
 

From a3a855203c4ee2dc9f8fb57eb8506aed0ff69d2a Mon Sep 17 00:00:00 2001
From: Michael Schaeuble <schaeuble.michael@gmail.com>
Date: Thu, 21 Jul 2016 17:12:49 +0200
Subject: [PATCH 1216/1230] Update Bebop mainapp.config with new functionality

---
 posix-configs/bebop/px4.config | 6 +++++-
 1 file changed, 5 insertions(+), 1 deletion(-)

diff --git a/posix-configs/bebop/px4.config b/posix-configs/bebop/px4.config
index b86c389dee..df9e4f9472 100644
--- a/posix-configs/bebop/px4.config
+++ b/posix-configs/bebop/px4.config
@@ -5,10 +5,14 @@ param set SYS_AUTOSTART 4001
 param set MAV_BROADCAST 1
 df_ms5607_wrapper start
 df_mpu6050_wrapper start -R 8
-df_ak8963_wrapper start
+df_ak8963_wrapper start -R 4
 sensors start
+attitude_estimator_q start
+position_estimator_inav start
+commander start
 sleep 1
 mavlink start -u 14556 -r 1000000
 sleep 1
 mavlink stream -u 14556 -s HIGHRES_IMU -r 50
+mavlink stream -u 14556 -s ATTITUDE -r 50
 mavlink boot_complete

From 8566cabd76c0f204cbd7299ef52da161b9b0864c Mon Sep 17 00:00:00 2001
From: Michael Schaeuble <schaeuble.michael@gmail.com>
Date: Wed, 3 Aug 2016 16:46:37 +0200
Subject: [PATCH 1217/1230] Make some code-style fixes

---
 cmake/configs/posix_bebop_default.cmake                   | 4 +---
 .../posix/drivers/df_ak8963_wrapper/df_ak8963_wrapper.cpp | 8 ++++----
 2 files changed, 5 insertions(+), 7 deletions(-)

diff --git a/cmake/configs/posix_bebop_default.cmake b/cmake/configs/posix_bebop_default.cmake
index a1c75d9565..5538ffe35a 100644
--- a/cmake/configs/posix_bebop_default.cmake
+++ b/cmake/configs/posix_bebop_default.cmake
@@ -23,11 +23,8 @@ set(config_module_list
 	drivers/device
 	modules/sensors
 	platforms/posix/drivers/df_ms5607_wrapper
-<<<<<<< c4eb65862f307de4b679e24f44fab0c565d7c2a3
 	platforms/posix/drivers/df_mpu6050_wrapper
-=======
 	platforms/posix/drivers/df_ak8963_wrapper
->>>>>>> Add DF wrapper for AK8963
 
 	#
 	# System commands
@@ -105,4 +102,5 @@ set(config_module_list
 set(config_df_driver_list
 	ms5607
 	mpu6050
+	ak8963
 )
diff --git a/src/platforms/posix/drivers/df_ak8963_wrapper/df_ak8963_wrapper.cpp b/src/platforms/posix/drivers/df_ak8963_wrapper/df_ak8963_wrapper.cpp
index 9238a5e7de..473a53e031 100644
--- a/src/platforms/posix/drivers/df_ak8963_wrapper/df_ak8963_wrapper.cpp
+++ b/src/platforms/posix/drivers/df_ak8963_wrapper/df_ak8963_wrapper.cpp
@@ -288,9 +288,9 @@ int DfAK8963Wrapper::_publish(struct mag_sensor_data &data)
 	// apply sensor rotation on the accel measurement
 	mag_val = _rotation_matrix * mag_val;
 
-	mag_report.x = (mag_val(0) - _mag_calibration.x_offset) * _mag_calibration.x_scale;;
-	mag_report.y = (mag_val(1) - _mag_calibration.y_offset) * _mag_calibration.y_scale;;
-	mag_report.z = (mag_val(2) - _mag_calibration.z_offset) * _mag_calibration.z_scale;;
+	mag_report.x = (mag_val(0) - _mag_calibration.x_offset) * _mag_calibration.x_scale;
+	mag_report.y = (mag_val(1) - _mag_calibration.y_offset) * _mag_calibration.y_scale;
+	mag_report.z = (mag_val(2) - _mag_calibration.z_offset) * _mag_calibration.z_scale;
 
 
 	// TODO: get these right
@@ -394,7 +394,7 @@ info()
 
 	PX4_DEBUG("state @ %p", g_dev);
 
-  g_dev->print_calibration();
+	g_dev->print_calibration();
 
 	return 0;
 }

From a956429c4ccd80dfd8ae8f20fdfa8d777dfa221d Mon Sep 17 00:00:00 2001
From: Julian Oes <julian@oes.ch>
Date: Thu, 4 Aug 2016 16:46:03 +0200
Subject: [PATCH 1218/1230] navigator: mission check refactor

The mission feasability checker was called with the same arguments
twice which made it hard to understand when a mission is marked valid.

The mission check should run in these two cases:
- When initializing (if home comes up) if there is already a mission saved.
- When the mission gets updated.
---
 src/modules/navigator/mission.cpp | 59 ++++++++++++++-----------------
 src/modules/navigator/mission.h   |  3 +-
 2 files changed, 28 insertions(+), 34 deletions(-)

diff --git a/src/modules/navigator/mission.cpp b/src/modules/navigator/mission.cpp
index e9369aacaa..1cd019d214 100644
--- a/src/modules/navigator/mission.cpp
+++ b/src/modules/navigator/mission.cpp
@@ -81,7 +81,6 @@ Mission::Mission(Navigator *navigator, const char *name) :
 	_need_takeoff(true),
 	_mission_type(MISSION_TYPE_NONE),
 	_inited(false),
-	_home_inited(false),
 	_need_mission_reset(false),
 	_missionFeasibilityChecker(),
 	_min_current_sp_distance_xy(FLT_MAX),
@@ -100,6 +99,11 @@ Mission::~Mission()
 void
 Mission::on_inactive()
 {
+	/* Without home a mission can't be valid yet anyway, let's wait. */
+	if (!_navigator->home_position_valid()) {
+		return;
+	}
+
 	if (_inited) {
 		/* check if missions have changed so that feedback to ground station is given */
 		bool onboard_updated = false;
@@ -138,11 +142,12 @@ Mission::on_inactive()
 			_current_offboard_mission_index = mission_state.current_seq;
 		}
 
+		/* On init let's check the mission, maybe there is already one available. */
+		check_mission_valid();
+
 		_inited = true;
 	}
 
-	check_mission_valid();
-
 	/* require takeoff after non-loiter or landing */
 	if (!_navigator->get_can_loiter_at_sp() || _navigator->get_land_detected()->landed) {
 		_need_takeoff = true;
@@ -274,24 +279,14 @@ Mission::update_offboard_mission()
 			/* otherwise, just leave it */
 		}
 
-		/* Check mission feasibility, for now do not handle the return value,
-		 * however warnings are issued to the gcs via mavlink from inside the MissionFeasiblityChecker */
-		dm_item_t dm_current = DM_KEY_WAYPOINTS_OFFBOARD(_offboard_mission.dataman_id);
+		check_mission_valid();
 
-		failed = !_missionFeasibilityChecker.checkMissionFeasible(_navigator->get_mavlink_log_pub(), (_navigator->get_vstatus()->is_rotary_wing || _navigator->get_vstatus()->is_vtol),
-				dm_current, (size_t) _offboard_mission.count, _navigator->get_geofence(),
-				_navigator->get_home_position()->alt, _navigator->home_position_valid(),
-				_navigator->get_global_position()->lat, _navigator->get_global_position()->lon,
-				_param_dist_1wp.get(), _navigator->get_mission_result()->warning, _navigator->get_default_acceptance_radius(),
-				_navigator->get_land_detected()->landed);
+		failed = !_navigator->get_mission_result()->valid;
 
-		_navigator->get_mission_result()->valid = !failed;
 		if (!failed) {
 			/* reset mission failure if we have an updated valid mission */
 			_navigator->get_mission_result()->mission_failure = false;
 		}
-		_navigator->increment_mission_instance_count();
-		_navigator->set_mission_result_updated();
 
 	} else {
 		PX4_WARN("offboard mission update failed, handle: %d", _navigator->get_offboard_mission_sub());
@@ -1131,28 +1126,28 @@ Mission::set_mission_finished()
 	_navigator->set_mission_result_updated();
 }
 
-bool
+void
 Mission::check_mission_valid()
 {
-	/* check if the home position became valid in the meantime */
-	if (!_home_inited && _navigator->home_position_valid()) {
 
-		dm_item_t dm_current = DM_KEY_WAYPOINTS_OFFBOARD(_offboard_mission.dataman_id);
+	dm_item_t dm_current = DM_KEY_WAYPOINTS_OFFBOARD(_offboard_mission.dataman_id);
 
-		_navigator->get_mission_result()->valid = _missionFeasibilityChecker.checkMissionFeasible(_navigator->get_mavlink_log_pub(), (_navigator->get_vstatus()->is_rotary_wing || _navigator->get_vstatus()->is_vtol),
-				dm_current, (size_t) _offboard_mission.count, _navigator->get_geofence(),
-				_navigator->get_home_position()->alt, _navigator->home_position_valid(),
-				_navigator->get_global_position()->lat, _navigator->get_global_position()->lon,
-				_param_dist_1wp.get(), _navigator->get_mission_result()->warning, _navigator->get_default_acceptance_radius(),
-				_navigator->get_land_detected()->landed);
+	_navigator->get_mission_result()->valid =
+		_missionFeasibilityChecker.checkMissionFeasible(
+			_navigator->get_mavlink_log_pub(),
+			(_navigator->get_vstatus()->is_rotary_wing || _navigator->get_vstatus()->is_vtol),
+			dm_current, (size_t) _offboard_mission.count, _navigator->get_geofence(),
+			_navigator->get_home_position()->alt,
+			_navigator->home_position_valid(),
+			_navigator->get_global_position()->lat,
+			_navigator->get_global_position()->lon,
+			_param_dist_1wp.get(),
+			_navigator->get_mission_result()->warning,
+			_navigator->get_default_acceptance_radius(),
+			_navigator->get_land_detected()->landed);
 
-		_navigator->increment_mission_instance_count();
-		_navigator->set_mission_result_updated();
-
-		_home_inited = true;
-	}
-
-	return _navigator->get_mission_result()->valid;
+	_navigator->increment_mission_instance_count();
+	_navigator->set_mission_result_updated();
 }
 
 void
diff --git a/src/modules/navigator/mission.h b/src/modules/navigator/mission.h
index 2bcfa85508..cd43327550 100644
--- a/src/modules/navigator/mission.h
+++ b/src/modules/navigator/mission.h
@@ -211,7 +211,7 @@ private:
 	/**
 	 * Check wether a mission is ready to go
 	 */
-	bool check_mission_valid();
+	void check_mission_valid();
 
 	/**
 	 * Reset offboard mission
@@ -244,7 +244,6 @@ private:
 	} _mission_type;
 
 	bool _inited;
-	bool _home_inited;
 	bool _need_mission_reset;
 
 	MissionFeasibilityChecker _missionFeasibilityChecker; /**< class that checks if a mission is feasible */

From 95e80cc29b54de7b3728f910dc8841e78d76d9a9 Mon Sep 17 00:00:00 2001
From: sander <sander@droneslab.com>
Date: Thu, 4 Aug 2016 11:08:07 +0200
Subject: [PATCH 1219/1230] Only RTL when mission finishes mid air on DL Lost

---
 src/modules/commander/state_machine_helper.cpp | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp
index 1ebf522340..964d1abc5a 100644
--- a/src/modules/commander/state_machine_helper.cpp
+++ b/src/modules/commander/state_machine_helper.cpp
@@ -746,9 +746,9 @@ bool set_nav_state(struct vehicle_status_s *status, struct commander_state_s *in
 
 		/* datalink loss disabled:
 		 * check if both, RC and datalink are lost during the mission
-		 * or RC is lost after the mission is finished: this should always trigger RCRECOVER */
+		 * or RC is lost after the mission finishes in air: this should always trigger RCRECOVER */
 		} else if (!data_link_loss_enabled && ((status->rc_signal_lost && status->data_link_lost) ||
-						       (status->rc_signal_lost && mission_finished))) {
+							   (status->rc_signal_lost && !landed && mission_finished))) {
 			status->failsafe = true;
 
 			if (status_flags->condition_global_position_valid && status_flags->condition_home_position_valid) {

From e8a87538b8749d16a6c9ed7ff70f8403d3fffb09 Mon Sep 17 00:00:00 2001
From: sander <sander@droneslab.com>
Date: Thu, 4 Aug 2016 11:09:09 +0200
Subject: [PATCH 1220/1230] Code style

---
 .../commander/state_machine_helper.cpp        | 245 +++++++++++++-----
 1 file changed, 173 insertions(+), 72 deletions(-)

diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp
index 964d1abc5a..0b0d833d77 100644
--- a/src/modules/commander/state_machine_helper.cpp
+++ b/src/modules/commander/state_machine_helper.cpp
@@ -97,7 +97,7 @@ static const bool arming_transitions[vehicle_status_s::ARMING_STATE_MAX][vehicle
 };
 
 // You can index into the array with an arming_state_t in order to get its textual representation
-static const char * const state_names[vehicle_status_s::ARMING_STATE_MAX] = {
+static const char *const state_names[vehicle_status_s::ARMING_STATE_MAX] = {
 	"ARMING_STATE_INIT",
 	"ARMING_STATE_STANDBY",
 	"ARMING_STATE_ARMED",
@@ -111,15 +111,15 @@ static hrt_abstime last_preflight_check = 0;	///< initialize so it gets checked
 static int last_prearm_ret = 1;			///< initialize to fail
 
 transition_result_t arming_state_transition(struct vehicle_status_s *status,
-					    struct battery_status_s *battery,
-					    const struct safety_s *safety,
-					    arming_state_t new_arming_state,
-					    struct actuator_armed_s *armed,
-					    bool fRunPreArmChecks,
-					    orb_advert_t *mavlink_log_pub,	///< uORB handle for mavlink log
-					    status_flags_s *status_flags,
-					    float avionics_power_rail_voltage,
-					    bool can_arm_without_gps)
+		struct battery_status_s *battery,
+		const struct safety_s *safety,
+		arming_state_t new_arming_state,
+		struct actuator_armed_s *armed,
+		bool fRunPreArmChecks,
+		orb_advert_t *mavlink_log_pub,	///< uORB handle for mavlink log
+		status_flags_s *status_flags,
+		float avionics_power_rail_voltage,
+		bool can_arm_without_gps)
 {
 	// Double check that our static arrays are still valid
 	ASSERT(vehicle_status_s::ARMING_STATE_INIT == 0);
@@ -142,7 +142,7 @@ transition_result_t arming_state_transition(struct vehicle_status_s *status,
 
 		/* only perform the pre-arm check if we have to */
 		if (fRunPreArmChecks && new_arming_state == vehicle_status_s::ARMING_STATE_ARMED
-				&& status->hil_state == vehicle_status_s::HIL_STATE_OFF) {
+		    && status->hil_state == vehicle_status_s::HIL_STATE_OFF) {
 
 			prearm_ret = preflight_check(status, mavlink_log_pub, true /* pre-arm */, false /* force_report */,
 						     status_flags, battery, can_arm_without_gps);
@@ -150,9 +150,9 @@ transition_result_t arming_state_transition(struct vehicle_status_s *status,
 
 		/* re-run the pre-flight check as long as sensors are failing */
 		if (!status_flags->condition_system_sensors_initialized
-				&& (new_arming_state == vehicle_status_s::ARMING_STATE_ARMED
-				|| new_arming_state == vehicle_status_s::ARMING_STATE_STANDBY)
-				&& status->hil_state == vehicle_status_s::HIL_STATE_OFF) {
+		    && (new_arming_state == vehicle_status_s::ARMING_STATE_ARMED
+			|| new_arming_state == vehicle_status_s::ARMING_STATE_STANDBY)
+		    && status->hil_state == vehicle_status_s::HIL_STATE_OFF) {
 
 			if (last_preflight_check == 0 || hrt_absolute_time() - last_preflight_check > 1000 * 1000) {
 				prearm_ret = preflight_check(status, mavlink_log_pub, false /* pre-flight */, false /* force_report */,
@@ -160,6 +160,7 @@ transition_result_t arming_state_transition(struct vehicle_status_s *status,
 				status_flags->condition_system_sensors_initialized = !prearm_ret;
 				last_preflight_check = hrt_absolute_time();
 				last_prearm_ret = prearm_ret;
+
 			} else {
 				prearm_ret = last_prearm_ret;
 			}
@@ -168,9 +169,9 @@ transition_result_t arming_state_transition(struct vehicle_status_s *status,
 		/*
 		 * Perform an atomic state update
 		 */
-		#ifdef __PX4_NUTTX
+#ifdef __PX4_NUTTX
 		irqstate_t flags = px4_enter_critical_section();
-		#endif
+#endif
 
 		/* enforce lockdown in HIL */
 		if (status->hil_state == vehicle_status_s::HIL_STATE_ON) {
@@ -197,7 +198,7 @@ transition_result_t arming_state_transition(struct vehicle_status_s *status,
 				//      Do not perform pre-arm checks if coming from in air restore
 				//      Allow if vehicle_status_s::HIL_STATE_ON
 				if (status->arming_state != vehicle_status_s::ARMING_STATE_IN_AIR_RESTORE &&
-					status->hil_state == vehicle_status_s::HIL_STATE_OFF) {
+				    status->hil_state == vehicle_status_s::HIL_STATE_OFF) {
 
 					// Fail transition if pre-arm check fails
 					if (prearm_ret) {
@@ -205,7 +206,8 @@ transition_result_t arming_state_transition(struct vehicle_status_s *status,
 						feedback_provided = true;
 						valid_transition = false;
 
-					// Fail transition if we need safety switch press
+						// Fail transition if we need safety switch press
+
 					} else if (safety->safety_switch_available && !safety->safety_off) {
 
 						mavlink_and_console_log_critical(mavlink_log_pub, "NOT ARMING: Press safety switch first!");
@@ -229,14 +231,19 @@ transition_result_t arming_state_transition(struct vehicle_status_s *status,
 						if (status_flags->condition_power_input_valid && (avionics_power_rail_voltage > 0.0f)) {
 							// Check avionics rail voltages
 							if (avionics_power_rail_voltage < 4.5f) {
-								mavlink_and_console_log_critical(mavlink_log_pub, "NOT ARMING: Avionics power low: %6.2f Volt", (double)avionics_power_rail_voltage);
+								mavlink_and_console_log_critical(mavlink_log_pub, "NOT ARMING: Avionics power low: %6.2f Volt",
+												 (double)avionics_power_rail_voltage);
 								feedback_provided = true;
 								valid_transition = false;
+
 							} else if (avionics_power_rail_voltage < 4.9f) {
-								mavlink_and_console_log_critical(mavlink_log_pub, "CAUTION: Avionics power low: %6.2f Volt", (double)avionics_power_rail_voltage);
+								mavlink_and_console_log_critical(mavlink_log_pub, "CAUTION: Avionics power low: %6.2f Volt",
+												 (double)avionics_power_rail_voltage);
 								feedback_provided = true;
+
 							} else if (avionics_power_rail_voltage > 5.4f) {
-								mavlink_and_console_log_critical(mavlink_log_pub, "CAUTION: Avionics power high: %6.2f Volt", (double)avionics_power_rail_voltage);
+								mavlink_and_console_log_critical(mavlink_log_pub, "CAUTION: Avionics power high: %6.2f Volt",
+												 (double)avionics_power_rail_voltage);
 								feedback_provided = true;
 							}
 						}
@@ -244,7 +251,8 @@ transition_result_t arming_state_transition(struct vehicle_status_s *status,
 
 				}
 
-			} else if (new_arming_state == vehicle_status_s::ARMING_STATE_STANDBY && status->arming_state == vehicle_status_s::ARMING_STATE_ARMED_ERROR) {
+			} else if (new_arming_state == vehicle_status_s::ARMING_STATE_STANDBY
+				   && status->arming_state == vehicle_status_s::ARMING_STATE_ARMED_ERROR) {
 				new_arming_state = vehicle_status_s::ARMING_STATE_STANDBY_ERROR;
 			}
 		}
@@ -261,34 +269,39 @@ transition_result_t arming_state_transition(struct vehicle_status_s *status,
 
 				if (status_flags->condition_system_sensors_initialized) {
 					mavlink_and_console_log_critical(mavlink_log_pub, "Preflight check resolved, reboot before arming");
+
 				} else {
 					mavlink_and_console_log_critical(mavlink_log_pub, "Preflight check failed, refusing to arm");
 				}
+
 				feedback_provided = true;
 
 			} else if ((new_arming_state == vehicle_status_s::ARMING_STATE_STANDBY) &&
-					status_flags->condition_system_sensors_initialized) {
+				   status_flags->condition_system_sensors_initialized) {
 				mavlink_and_console_log_critical(mavlink_log_pub, "Preflight check resolved, reboot to complete");
 				feedback_provided = true;
+
 			} else {
 				// Silent ignore
 				feedback_provided = true;
 			}
 
-		// Sensors need to be initialized for STANDBY state, except for HIL
+			// Sensors need to be initialized for STANDBY state, except for HIL
+
 		} else if ((status->hil_state != vehicle_status_s::HIL_STATE_ON) &&
-			(new_arming_state == vehicle_status_s::ARMING_STATE_STANDBY) &&
-			(status->arming_state != vehicle_status_s::ARMING_STATE_STANDBY_ERROR)) {
+			   (new_arming_state == vehicle_status_s::ARMING_STATE_STANDBY) &&
+			   (status->arming_state != vehicle_status_s::ARMING_STATE_STANDBY_ERROR)) {
 
 			if (!status_flags->condition_system_sensors_initialized) {
 
 				if (status_flags->condition_system_hotplug_timeout) {
 					if (!status_flags->condition_system_prearm_error_reported) {
 						mavlink_and_console_log_critical(mavlink_log_pub,
-							"Not ready to fly: Sensors not set up correctly");
+										 "Not ready to fly: Sensors not set up correctly");
 						status_flags->condition_system_prearm_error_reported = true;
 					}
 				}
+
 				feedback_provided = true;
 				valid_transition = false;
 			}
@@ -296,29 +309,32 @@ transition_result_t arming_state_transition(struct vehicle_status_s *status,
 
 		// Finish up the state transition
 		if (valid_transition) {
-			armed->armed = new_arming_state == vehicle_status_s::ARMING_STATE_ARMED || new_arming_state == vehicle_status_s::ARMING_STATE_ARMED_ERROR;
-			armed->ready_to_arm = new_arming_state == vehicle_status_s::ARMING_STATE_ARMED || new_arming_state == vehicle_status_s::ARMING_STATE_STANDBY;
+			armed->armed = new_arming_state == vehicle_status_s::ARMING_STATE_ARMED
+				       || new_arming_state == vehicle_status_s::ARMING_STATE_ARMED_ERROR;
+			armed->ready_to_arm = new_arming_state == vehicle_status_s::ARMING_STATE_ARMED
+					      || new_arming_state == vehicle_status_s::ARMING_STATE_STANDBY;
 			ret = TRANSITION_CHANGED;
 			status->arming_state = new_arming_state;
 		}
 
 		/* reset feedback state */
 		if (status->arming_state != vehicle_status_s::ARMING_STATE_STANDBY_ERROR &&
-			status->arming_state != vehicle_status_s::ARMING_STATE_INIT &&
-			valid_transition) {
+		    status->arming_state != vehicle_status_s::ARMING_STATE_INIT &&
+		    valid_transition) {
 			status_flags->condition_system_prearm_error_reported = false;
 		}
 
 		/* end of atomic state update */
-		#ifdef __PX4_NUTTX
+#ifdef __PX4_NUTTX
 		px4_leave_critical_section(flags);
-		#endif
+#endif
 	}
 
 	if (ret == TRANSITION_DENIED) {
 		/* print to MAVLink and console if we didn't provide any feedback yet */
 		if (!feedback_provided) {
-			mavlink_and_console_log_critical(mavlink_log_pub, "TRANSITION_DENIED: %s - %s", state_names[status->arming_state], state_names[new_arming_state]);
+			mavlink_and_console_log_critical(mavlink_log_pub, "TRANSITION_DENIED: %s - %s", state_names[status->arming_state],
+							 state_names[new_arming_state]);
 		}
 	}
 
@@ -357,29 +373,36 @@ main_state_transition(struct vehicle_status_s *status, main_state_t new_main_sta
 		if (status->is_rotary_wing) {
 			ret = TRANSITION_CHANGED;
 		}
+
 		break;
 
 	case commander_state_s::MAIN_STATE_ALTCTL:
+
 		/* need at minimum altitude estimate */
 		if (status_flags->condition_local_altitude_valid ||
 		    status_flags->condition_global_position_valid) {
 			ret = TRANSITION_CHANGED;
 		}
+
 		break;
 
 	case commander_state_s::MAIN_STATE_POSCTL:
+
 		/* need at minimum local position estimate */
 		if (status_flags->condition_local_position_valid ||
 		    status_flags->condition_global_position_valid) {
 			ret = TRANSITION_CHANGED;
 		}
+
 		break;
 
 	case commander_state_s::MAIN_STATE_AUTO_LOITER:
+
 		/* need global position estimate */
 		if (status_flags->condition_global_position_valid) {
 			ret = TRANSITION_CHANGED;
 		}
+
 		break;
 
 	case commander_state_s::MAIN_STATE_AUTO_FOLLOW_TARGET:
@@ -387,10 +410,12 @@ main_state_transition(struct vehicle_status_s *status, main_state_t new_main_sta
 	case commander_state_s::MAIN_STATE_AUTO_RTL:
 	case commander_state_s::MAIN_STATE_AUTO_TAKEOFF:
 	case commander_state_s::MAIN_STATE_AUTO_LAND:
+
 		/* need global position and home position */
 		if (status_flags->condition_global_position_valid && status_flags->condition_home_position_valid) {
 			ret = TRANSITION_CHANGED;
 		}
+
 		break;
 
 	case commander_state_s::MAIN_STATE_OFFBOARD:
@@ -406,11 +431,13 @@ main_state_transition(struct vehicle_status_s *status, main_state_t new_main_sta
 	default:
 		break;
 	}
+
 	if (ret == TRANSITION_CHANGED) {
 		if (internal_state->main_state != new_main_state) {
 			main_state_prev = internal_state->main_state;
 			internal_state->main_state = new_main_state;
 			internal_state->timestamp = hrt_absolute_time();
+
 		} else {
 			ret = TRANSITION_NOT_CHANGED;
 		}
@@ -422,7 +449,8 @@ main_state_transition(struct vehicle_status_s *status, main_state_t new_main_sta
 /**
  * Transition from one hil state to another
  */
-transition_result_t hil_state_transition(hil_state_t new_state, orb_advert_t status_pub, struct vehicle_status_s *current_status, orb_advert_t *mavlink_log_pub)
+transition_result_t hil_state_transition(hil_state_t new_state, orb_advert_t status_pub,
+		struct vehicle_status_s *current_status, orb_advert_t *mavlink_log_pub)
 {
 	transition_result_t ret = TRANSITION_DENIED;
 
@@ -503,6 +531,7 @@ transition_result_t hil_state_transition(hil_state_t new_state, orb_advert_t sta
 
 						printf("Disabling %s: %s\n", devname, (block_ret == OK) ? "OK" : "ERROR");
 					}
+
 					closedir(d);
 
 					ret = TRANSITION_CHANGED;
@@ -519,14 +548,17 @@ transition_result_t hil_state_transition(hil_state_t new_state, orb_advert_t sta
 				// Handle VDev devices
 				const char *devname;
 				unsigned int handle = 0;
-				for(;;) {
-					devname = px4_get_device_names(&handle);
-					if (devname == NULL)
-						break;
 
-						/* skip mavlink */
+				for (;;) {
+					devname = px4_get_device_names(&handle);
+
+					if (devname == NULL) {
+						break;
+					}
+
+					/* skip mavlink */
 					if (!strcmp("/dev/mavlink", devname)) {
-							continue;
+						continue;
 					}
 
 
@@ -534,7 +566,7 @@ transition_result_t hil_state_transition(hil_state_t new_state, orb_advert_t sta
 
 					if (sensfd < 0) {
 						warn("failed opening device %s", devname);
-							continue;
+						continue;
 					}
 
 					int block_ret = px4_ioctl(sensfd, DEVIOCSPUBBLOCK, 1);
@@ -547,7 +579,8 @@ transition_result_t hil_state_transition(hil_state_t new_state, orb_advert_t sta
 				// Handle DF devices
 				const char *df_dev_path;
 				unsigned int index = 0;
-				for(;;) {
+
+				for (;;) {
 					if (DevMgr::getNextDeviceName(index, &df_dev_path) < 0) {
 						break;
 					}
@@ -574,6 +607,7 @@ transition_result_t hil_state_transition(hil_state_t new_state, orb_advert_t sta
 				mavlink_and_console_log_critical(mavlink_log_pub, "Not switching to HIL when armed");
 				ret = TRANSITION_DENIED;
 			}
+
 			break;
 
 		default:
@@ -588,6 +622,7 @@ transition_result_t hil_state_transition(hil_state_t new_state, orb_advert_t sta
 		// XXX also set lockdown here
 		orb_publish(ORB_ID(vehicle_status), status_pub, current_status);
 	}
+
 	return ret;
 }
 
@@ -601,7 +636,8 @@ bool set_nav_state(struct vehicle_status_s *status, struct commander_state_s *in
 {
 	navigation_state_t nav_state_old = status->nav_state;
 
-	bool armed = (status->arming_state == vehicle_status_s::ARMING_STATE_ARMED || status->arming_state == vehicle_status_s::ARMING_STATE_ARMED_ERROR);
+	bool armed = (status->arming_state == vehicle_status_s::ARMING_STATE_ARMED
+		      || status->arming_state == vehicle_status_s::ARMING_STATE_ARMED_ERROR);
 	status->failsafe = false;
 
 	/* evaluate main state to decide in normal (non-failsafe) mode */
@@ -611,16 +647,20 @@ bool set_nav_state(struct vehicle_status_s *status, struct commander_state_s *in
 	case commander_state_s::MAIN_STATE_RATTITUDE:
 	case commander_state_s::MAIN_STATE_STAB:
 	case commander_state_s::MAIN_STATE_ALTCTL:
+
 		/* require RC for all manual modes */
 		if (rc_loss_enabled && (status->rc_signal_lost || status_flags->rc_signal_lost_cmd) && armed) {
 			status->failsafe = true;
 
 			if (status_flags->condition_global_position_valid && status_flags->condition_home_position_valid) {
 				status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_RCRECOVER;
+
 			} else if (status_flags->condition_local_position_valid) {
 				status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LAND;
+
 			} else if (status_flags->condition_local_altitude_valid) {
 				status->nav_state = vehicle_status_s::NAVIGATION_STATE_DESCEND;
+
 			} else {
 				status->nav_state = vehicle_status_s::NAVIGATION_STATE_TERMINATION;
 			}
@@ -652,10 +692,10 @@ bool set_nav_state(struct vehicle_status_s *status, struct commander_state_s *in
 				break;
 			}
 		}
+
 		break;
 
-	case commander_state_s::MAIN_STATE_POSCTL:
-		{
+	case commander_state_s::MAIN_STATE_POSCTL: {
 			const bool rc_lost = rc_loss_enabled && (status->rc_signal_lost || status_flags->rc_signal_lost_cmd);
 
 			if (rc_lost && armed) {
@@ -677,20 +717,23 @@ bool set_nav_state(struct vehicle_status_s *status, struct commander_state_s *in
 					status->nav_state = vehicle_status_s::NAVIGATION_STATE_TERMINATION;
 				}
 
-			/* As long as there is RC, we can fallback to ALTCTL, or STAB. */
-			/* A local position estimate is enough for POSCTL for multirotors,
-			 * this enables POSCTL using e.g. flow.
-			 * For fixedwing, a global position is needed. */
+				/* As long as there is RC, we can fallback to ALTCTL, or STAB. */
+				/* A local position estimate is enough for POSCTL for multirotors,
+				 * this enables POSCTL using e.g. flow.
+				 * For fixedwing, a global position is needed. */
+
 			} else if (((status->is_rotary_wing && !status_flags->condition_local_position_valid) ||
-				   (!status->is_rotary_wing && !status_flags->condition_global_position_valid))
+				    (!status->is_rotary_wing && !status_flags->condition_global_position_valid))
 				   && armed) {
 				status->failsafe = true;
 
 				if (status_flags->condition_local_altitude_valid) {
 					status->nav_state = vehicle_status_s::NAVIGATION_STATE_ALTCTL;
+
 				} else {
 					status->nav_state = vehicle_status_s::NAVIGATION_STATE_STAB;
 				}
+
 			} else {
 				status->nav_state = vehicle_status_s::NAVIGATION_STATE_POSCTL;
 			}
@@ -708,66 +751,85 @@ bool set_nav_state(struct vehicle_status_s *status, struct commander_state_s *in
 		/* first look at the commands */
 		if (status->engine_failure_cmd) {
 			status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LANDENGFAIL;
+
 		} else if (status_flags->data_link_lost_cmd) {
 			status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_RTGS;
+
 		} else if (status_flags->gps_failure_cmd) {
 			status->nav_state = vehicle_status_s::NAVIGATION_STATE_DESCEND;
 			status->failsafe = true;
+
 		} else if (status_flags->rc_signal_lost_cmd) {
 			status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_RCRECOVER;
+
 		} else if (status_flags->vtol_transition_failure_cmd) {
 			status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_RTL;
 
-		/* finished handling commands which have priority, now handle failures */
+			/* finished handling commands which have priority, now handle failures */
+
 		} else if (status_flags->gps_failure) {
 			status->nav_state = vehicle_status_s::NAVIGATION_STATE_DESCEND;
 			status->failsafe = true;
+
 		} else if (status->engine_failure) {
 			status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LANDENGFAIL;
+
 		} else if (status_flags->vtol_transition_failure) {
 			status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_RTL;
+
 		} else if (status->mission_failure) {
 			status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_RTL;
 
-		/* datalink loss enabled:
-		 * check for datalink lost: this should always trigger RTGS */
+			/* datalink loss enabled:
+			 * check for datalink lost: this should always trigger RTGS */
+
 		} else if (data_link_loss_enabled && status->data_link_lost) {
 			status->failsafe = true;
 
 			if (status_flags->condition_global_position_valid && status_flags->condition_home_position_valid) {
 				status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_RTGS;
+
 			} else if (status_flags->condition_local_position_valid) {
 				status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LAND;
+
 			} else if (status_flags->condition_local_altitude_valid) {
 				status->nav_state = vehicle_status_s::NAVIGATION_STATE_DESCEND;
+
 			} else {
 				status->nav_state = vehicle_status_s::NAVIGATION_STATE_TERMINATION;
 			}
 
-		/* datalink loss disabled:
-		 * check if both, RC and datalink are lost during the mission
-		 * or RC is lost after the mission finishes in air: this should always trigger RCRECOVER */
+			/* datalink loss disabled:
+			 * check if both, RC and datalink are lost during the mission
+			 * or RC is lost after the mission finishes in air: this should always trigger RCRECOVER */
+
 		} else if (!data_link_loss_enabled && ((status->rc_signal_lost && status->data_link_lost) ||
-							   (status->rc_signal_lost && !landed && mission_finished))) {
+						       (status->rc_signal_lost && !landed && mission_finished))) {
 			status->failsafe = true;
 
 			if (status_flags->condition_global_position_valid && status_flags->condition_home_position_valid) {
 				status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_RCRECOVER;
+
 			} else if (status_flags->condition_local_position_valid) {
 				status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LAND;
+
 			} else if (status_flags->condition_local_altitude_valid) {
 				status->nav_state = vehicle_status_s::NAVIGATION_STATE_DESCEND;
+
 			} else {
 				status->nav_state = vehicle_status_s::NAVIGATION_STATE_TERMINATION;
 			}
 
-		/* stay where you are if you should stay in failsafe, otherwise everything is perfect */
-		} else if (!stay_in_failsafe){
+			/* stay where you are if you should stay in failsafe, otherwise everything is perfect */
+
+		} else if (!stay_in_failsafe) {
 			status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION;
 		}
+
 		break;
 
 	case commander_state_s::MAIN_STATE_AUTO_LOITER:
+
 		/* go into failsafe on a engine failure */
 		if (status->engine_failure) {
 			status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LANDENGFAIL;
@@ -776,46 +838,58 @@ bool set_nav_state(struct vehicle_status_s *status, struct commander_state_s *in
 			status->nav_state = vehicle_status_s::NAVIGATION_STATE_DESCEND;
 			status->failsafe = true;
 
-		/* also go into failsafe if just datalink is lost */
+			/* also go into failsafe if just datalink is lost */
+
 		} else if (status->data_link_lost && data_link_loss_enabled) {
 			status->failsafe = true;
 
 			if (status_flags->condition_global_position_valid && status_flags->condition_home_position_valid) {
 				status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_RTGS;
+
 			} else if (status_flags->condition_local_position_valid) {
 				status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LAND;
+
 			} else if (status_flags->condition_local_altitude_valid) {
 				status->nav_state = vehicle_status_s::NAVIGATION_STATE_DESCEND;
+
 			} else {
 				status->nav_state = vehicle_status_s::NAVIGATION_STATE_TERMINATION;
 			}
 
-		/* go into failsafe if RC is lost and datalink loss is not set up */
+			/* go into failsafe if RC is lost and datalink loss is not set up */
+
 		} else if (status->rc_signal_lost && !data_link_loss_enabled) {
 			status->failsafe = true;
 
 			if (status_flags->condition_global_position_valid && status_flags->condition_home_position_valid) {
 				status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_RTGS;
+
 			} else if (status_flags->condition_local_position_valid) {
 				status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LAND;
+
 			} else if (status_flags->condition_local_altitude_valid) {
 				status->nav_state = vehicle_status_s::NAVIGATION_STATE_DESCEND;
+
 			} else {
 				status->nav_state = vehicle_status_s::NAVIGATION_STATE_TERMINATION;
 			}
 
-		/* don't bother if RC is lost if datalink is connected */
+			/* don't bother if RC is lost if datalink is connected */
+
 		} else if (status->rc_signal_lost) {
 
 			/* this mode is ok, we don't need RC for loitering */
 			status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER;
+
 		} else {
 			/* everything is perfect */
 			status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER;
 		}
+
 		break;
 
 	case commander_state_s::MAIN_STATE_AUTO_RTL:
+
 		/* require global position and home, also go into failsafe on an engine failure */
 
 		if (status->engine_failure) {
@@ -826,89 +900,111 @@ bool set_nav_state(struct vehicle_status_s *status, struct commander_state_s *in
 			status->failsafe = true;
 
 		} else if ((!status_flags->condition_global_position_valid ||
-					!status_flags->condition_home_position_valid)) {
+			    !status_flags->condition_home_position_valid)) {
 			status->failsafe = true;
 
 			if (status_flags->condition_local_position_valid) {
 				status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LAND;
+
 			} else if (status_flags->condition_local_altitude_valid) {
 				status->nav_state = vehicle_status_s::NAVIGATION_STATE_DESCEND;
+
 			} else {
 				status->nav_state = vehicle_status_s::NAVIGATION_STATE_TERMINATION;
 			}
+
 		} else {
 			status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_RTL;
 		}
+
 		break;
 
 	case commander_state_s::MAIN_STATE_AUTO_FOLLOW_TARGET:
+
 		/* require global position and home */
 
 		if (status->engine_failure) {
 			status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LANDENGFAIL;
+
 		} else if (!status_flags->condition_global_position_valid) {
 			status->failsafe = true;
 
 			if (status_flags->condition_local_position_valid) {
 				status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LAND;
+
 			} else if (status_flags->condition_local_altitude_valid) {
 				status->nav_state = vehicle_status_s::NAVIGATION_STATE_DESCEND;
+
 			} else {
 				status->nav_state = vehicle_status_s::NAVIGATION_STATE_TERMINATION;
 			}
+
 		} else {
 			status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_FOLLOW_TARGET;
 		}
+
 		break;
 
 	case commander_state_s::MAIN_STATE_AUTO_TAKEOFF:
+
 		/* require global position and home */
 
 		if (status->engine_failure) {
 			status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LANDENGFAIL;
+
 		} else if (status_flags->gps_failure || (!status_flags->condition_global_position_valid ||
-					!status_flags->condition_home_position_valid)) {
+				!status_flags->condition_home_position_valid)) {
 			status->failsafe = true;
 
 			if (status_flags->condition_local_position_valid) {
 				status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LAND;
+
 			} else if (status_flags->condition_local_altitude_valid) {
 				status->nav_state = vehicle_status_s::NAVIGATION_STATE_DESCEND;
+
 			} else {
 				status->nav_state = vehicle_status_s::NAVIGATION_STATE_TERMINATION;
 			}
+
 		} else {
 			status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_TAKEOFF;
 		}
+
 		break;
 
 	case commander_state_s::MAIN_STATE_AUTO_LAND:
+
 		/* require global position and home */
 
 		if (status->engine_failure) {
 			status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LANDENGFAIL;
+
 		} else if (status_flags->gps_failure || (!status_flags->condition_global_position_valid ||
-					!status_flags->condition_home_position_valid)) {
+				!status_flags->condition_home_position_valid)) {
 			status->failsafe = true;
 
 			if (status_flags->condition_local_altitude_valid) {
 				status->nav_state = vehicle_status_s::NAVIGATION_STATE_DESCEND;
+
 			} else {
 				status->nav_state = vehicle_status_s::NAVIGATION_STATE_TERMINATION;
 			}
+
 		} else {
 			status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LAND;
 		}
+
 		break;
 
 	case commander_state_s::MAIN_STATE_OFFBOARD:
+
 		/* require offboard control, otherwise stay where you are */
 		if (status_flags->offboard_control_signal_lost && !status->rc_signal_lost) {
 			status->failsafe = true;
 
 			if (status_flags->offboard_control_loss_timeout && offb_loss_rc_act < 5 && offb_loss_rc_act >= 0) {
 				if (offb_loss_rc_act == 3 && status_flags->condition_global_position_valid
-						&& status_flags->condition_home_position_valid) {
+				    && status_flags->condition_home_position_valid) {
 					status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_RTL;
 
 				} else if (offb_loss_rc_act == 0 && status_flags->condition_global_position_valid) {
@@ -947,7 +1043,7 @@ bool set_nav_state(struct vehicle_status_s *status, struct commander_state_s *in
 
 			if (status_flags->offboard_control_loss_timeout && offb_loss_act < 3 && offb_loss_act >= 0) {
 				if (offb_loss_act == 2 && status_flags->condition_global_position_valid
-						&& status_flags->condition_home_position_valid) {
+				    && status_flags->condition_home_position_valid) {
 					status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_RTL;
 
 				} else if (offb_loss_act == 1 && status_flags->condition_global_position_valid) {
@@ -978,6 +1074,7 @@ bool set_nav_state(struct vehicle_status_s *status, struct commander_state_s *in
 		} else {
 			status->nav_state = vehicle_status_s::NAVIGATION_STATE_OFFBOARD;
 		}
+
 	default:
 		break;
 	}
@@ -985,14 +1082,16 @@ bool set_nav_state(struct vehicle_status_s *status, struct commander_state_s *in
 	return status->nav_state != nav_state_old;
 }
 
-int preflight_check(struct vehicle_status_s *status, orb_advert_t *mavlink_log_pub, bool prearm, bool force_report, status_flags_s *status_flags, battery_status_s *battery, bool can_arm_without_gps)
+int preflight_check(struct vehicle_status_s *status, orb_advert_t *mavlink_log_pub, bool prearm, bool force_report,
+		    status_flags_s *status_flags, battery_status_s *battery, bool can_arm_without_gps)
 {
 	/*
 	 */
 	bool reportFailures = force_report || (!status_flags->condition_system_prearm_error_reported &&
-		status_flags->condition_system_hotplug_timeout);
+					       status_flags->condition_system_hotplug_timeout);
 
 	bool checkAirspeed = false;
+
 	/* Perform airspeed check only if circuit breaker is not
 	 * engaged and it's not a rotary wing */
 	if (!status_flags->circuit_breaker_engaged_airspd_check && (!status->is_rotary_wing || status->is_vtol)) {
@@ -1000,11 +1099,12 @@ int preflight_check(struct vehicle_status_s *status, orb_advert_t *mavlink_log_p
 	}
 
 	bool preflight_ok = Commander::preflightCheck(mavlink_log_pub, true, true, true, true,
-				checkAirspeed, (status->rc_input_mode == vehicle_status_s::RC_IN_MODE_DEFAULT),
-				!can_arm_without_gps, true, reportFailures);
+			    checkAirspeed, (status->rc_input_mode == vehicle_status_s::RC_IN_MODE_DEFAULT),
+			    !can_arm_without_gps, true, reportFailures);
 
 	if (!status_flags->circuit_breaker_engaged_usb_check && status_flags->usb_connected && prearm) {
 		preflight_ok = false;
+
 		if (reportFailures) {
 			mavlink_and_console_log_critical(mavlink_log_pub, "ARMING DENIED: Flying with USB is not safe");
 		}
@@ -1012,6 +1112,7 @@ int preflight_check(struct vehicle_status_s *status, orb_advert_t *mavlink_log_p
 
 	if (battery->warning == battery_status_s::BATTERY_WARNING_CRITICAL) {
 		preflight_ok = false;
+
 		if (reportFailures) {
 			mavlink_and_console_log_critical(mavlink_log_pub, "ARMING DENIED: VERY LOW BATTERY");
 		}

From bc7178c538f557f6989ccd1b5a5ed2ca6126d4ab Mon Sep 17 00:00:00 2001
From: sander <sander@droneslab.com>
Date: Thu, 4 Aug 2016 11:26:21 +0200
Subject: [PATCH 1221/1230] Allow mission with active DL and DLL failsafe off

---
 src/modules/commander/state_machine_helper.cpp | 5 ++---
 1 file changed, 2 insertions(+), 3 deletions(-)

diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp
index 0b0d833d77..e279ee159a 100644
--- a/src/modules/commander/state_machine_helper.cpp
+++ b/src/modules/commander/state_machine_helper.cpp
@@ -801,10 +801,9 @@ bool set_nav_state(struct vehicle_status_s *status, struct commander_state_s *in
 
 			/* datalink loss disabled:
 			 * check if both, RC and datalink are lost during the mission
-			 * or RC is lost after the mission finishes in air: this should always trigger RCRECOVER */
+			 * or all links are lost after the mission finishes in air: this should always trigger RCRECOVER */
 
-		} else if (!data_link_loss_enabled && ((status->rc_signal_lost && status->data_link_lost) ||
-						       (status->rc_signal_lost && !landed && mission_finished))) {
+		} else if (!data_link_loss_enabled && status->rc_signal_lost && status->data_link_lost && !landed && mission_finished) {
 			status->failsafe = true;
 
 			if (status_flags->condition_global_position_valid && status_flags->condition_home_position_valid) {

From 699b6a2cb340f4b6ec3979ae7748ff9c56cfdb89 Mon Sep 17 00:00:00 2001
From: James Goppert <jgoppert@users.noreply.github.com>
Date: Fri, 5 Aug 2016 06:23:59 -0400
Subject: [PATCH 1222/1230] Improvements to SITL to make paths more flexible.
 (#5181)

---
 CMakeLists.txt                                |  65 ++--
 Makefile                                      |  33 +-
 Tools/sitl_gazebo                             |   2 +-
 Tools/sitl_run.sh                             |  83 +++--
 cmake/common/px4_base.cmake                   |  90 ++---
 cmake/configs/nuttx_mindpx-v2_default.cmake   |   2 +-
 .../nuttx_px4-stm32f4discovery_default.cmake  |   2 +-
 cmake/configs/nuttx_px4fmu-v1_default.cmake   |   2 +-
 cmake/configs/nuttx_px4fmu-v2_default.cmake   |   2 +-
 cmake/configs/nuttx_px4fmu-v2_ekf2.cmake      |   2 +-
 cmake/configs/nuttx_px4fmu-v2_test.cmake      |   2 +-
 cmake/configs/nuttx_px4fmu-v4_default.cmake   |   2 +-
 cmake/configs/nuttx_tap-v1_default.cmake      |   2 +-
 cmake/configs/posix_bebop_default.cmake       |   2 +-
 cmake/configs/posix_eagle_default.cmake       |   4 +-
 cmake/configs/posix_eagle_hil.cmake           |   4 +-
 .../posix_eagle_legacy_driver_default.cmake   |   4 +-
 cmake/configs/posix_eagle_muorb.cmake         |   4 +-
 cmake/configs/posix_rpi_cross.cmake           |   2 +-
 cmake/configs/posix_rpi_native.cmake          |   2 +-
 cmake/configs/posix_sdflight_default.cmake    |   4 +-
 cmake/configs/posix_sitl_broadcast.cmake      |   2 +-
 cmake/configs/posix_sitl_default.cmake        |   2 +-
 cmake/configs/posix_sitl_replay.cmake         |   2 +-
 cmake/configs/qurt_eagle_hello.cmake          |   4 +-
 cmake/configs/qurt_eagle_hil.cmake            |   4 +-
 .../qurt_eagle_legacy_driver_default.cmake    |   4 +-
 cmake/configs/qurt_eagle_muorb.cmake          |   4 +-
 cmake/configs/qurt_eagle_test.cmake           |   4 +-
 cmake/configs/qurt_eagle_travis.cmake         |   4 +-
 cmake/configs/qurt_sdflight_default.cmake     |   4 +-
 cmake/nuttx/px4_impl_nuttx.cmake              |  52 +--
 cmake/posix/px4_impl_posix.cmake              |   4 +-
 cmake/qurt/px4_impl_qurt.cmake                |   4 +-
 cmake/ros-CMakeLists.txt                      | 334 ------------------
 integrationtests/run_tests.bash               |  91 +++--
 integrationtests/setup_gazebo_ros.bash        |  19 +-
 launch/mavros_posix_sitl.launch               |  13 +-
 launch/mavros_posix_tests_iris.launch         |   1 +
 .../mavros_posix_tests_standard_vtol.launch   |   2 +-
 launch/posix_sitl.launch                      |  20 +-
 posix-configs/SITL/README.md                  |   2 +-
 posix-configs/SITL/init/rcS_gazebo_iris       |   2 +-
 .../SITL/init/rcS_gazebo_iris_opt_flow        |   2 +-
 posix-configs/SITL/init/rcS_gazebo_plane      |   2 +-
 posix-configs/SITL/init/rcS_gazebo_solo       |   2 +-
 .../SITL/init/rcS_gazebo_standard_vtol        |   2 +-
 posix-configs/SITL/init/rcS_gazebo_tailsitter |   2 +-
 .../SITL/init/rcS_gazebo_typhoon_h480         |   2 +-
 posix-configs/SITL/init/rcS_jmavsim_iris      |   2 +-
 posix-configs/SITL/init/rcS_lpe_gazebo_iris   |   2 +-
 .../SITL/init/rcS_lpe_gazebo_iris_opt_flow    |   2 +-
 posix-configs/SITL/init/rcS_lpe_jmavsim_iris  |   2 +-
 .../init/{rcS_tests => rcS_tests_none_iris}   |   0
 posix-configs/SITL/init/rc_iris_ros           |   2 +-
 src/firmware/nuttx/CMakeLists.txt             |  18 +-
 src/firmware/nuttx/gdbinit.in                 |   2 +-
 src/firmware/posix/CMakeLists.txt             |  88 ++++-
 src/firmware/qurt/CMakeLists.txt              |  12 +-
 src/lib/rc/rc_tests/CMakeLists.txt            |   1 +
 src/lib/rc/rc_tests/RCTest.cpp                |   4 +-
 src/modules/muorb/krait/CMakeLists.txt        |   4 +-
 src/modules/param/CMakeLists.txt              |   4 +-
 src/modules/px4iofirmware/CMakeLists.txt      |   6 +-
 src/modules/systemlib/CMakeLists.txt          |   2 +-
 src/modules/uavcan/uavcan_servers.hpp         |   2 +-
 src/platforms/posix/main.cpp                  | 227 ++++++++++--
 src/platforms/posix/px4_layer/CMakeLists.txt  |   4 +-
 src/platforms/qurt/px4_layer/CMakeLists.txt   |   2 +-
 src/systemcmds/tests/test_mixer.cpp           |   4 +-
 src/systemcmds/topic_listener/CMakeLists.txt  |   4 +-
 unittests/CMakeLists.txt                      |  14 +-
 72 files changed, 642 insertions(+), 671 deletions(-)
 delete mode 100644 cmake/ros-CMakeLists.txt
 rename posix-configs/SITL/init/{rcS_tests => rcS_tests_none_iris} (100%)

diff --git a/CMakeLists.txt b/CMakeLists.txt
index 9397b87c99..498b6ba7d8 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -121,6 +121,14 @@
 # and leads to wrong toolchain detection
 cmake_minimum_required(VERSION 2.8 FATAL_ERROR)
 
+set(PX4_SOURCE_DIR "${CMAKE_CURRENT_SOURCE_DIR}")
+set(PX4_BINARY_DIR "${CMAKE_CURRENT_BINARY_DIR}")
+
+execute_process(
+	COMMAND Tools/check_submodules.sh
+	WORKING_DIRECTORY ${PX4_SOURCE_DIR}
+	)
+
 # Use clang
 #SET (CMAKE_C_COMPILER /usr/bin/clang-3.6)
 #SET (CMAKE_CXX_COMPILER /usr/bin/clang++-3.6)
@@ -155,6 +163,9 @@ message(STATUS "${target_name}")
 # Define GNU standard installation directories
 include(GNUInstallDirs)
 
+# Add support for external project building
+include(ExternalProject)
+
 # Setup install paths
 if(NOT CMAKE_INSTALL_PREFIX)
 	if (${OS} STREQUAL "posix")
@@ -163,12 +174,8 @@ if(NOT CMAKE_INSTALL_PREFIX)
 endif()
 message(STATUS "CMAKE_INSTALL_PREFIX: ${CMAKE_INSTALL_PREFIX}")
 
-# switch to ros CMake file if building ros
-if (${OS} STREQUAL "ros")
-	include("cmake/ros-CMakeLists.txt")
-else()  # otherwise use the rest of this file
-
-list(APPEND CMAKE_MODULE_PATH ${CMAKE_SOURCE_DIR}/cmake)
+list(APPEND CMAKE_MODULE_PATH "${PX4_SOURCE_DIR}/cmake")
+message(STATUS "cmake module path: ${CMAKE_MODULE_PATH}")
 set(config_module "configs/${CONFIG}")
 include(${config_module})
 
@@ -199,6 +206,9 @@ if(DEFINED config_df_driver_list)
 	message("DF Drivers: ${config_df_driver_list}")
 endif()
 
+# force static lib build
+set(BUILD_SHARED_LIBS OFF)
+
 #=============================================================================
 # project definition
 #
@@ -219,8 +229,22 @@ set(version "${version_major}.${version_minor}.${version_patch}")
 set(package-contact "px4users@googlegroups.com")
 
 #=============================================================================
-# programs
+# find programs and packages
 #
+
+# see if catkin was invoked to build this
+if (CATKIN_DEVEL_PREFIX)
+	message(STATUS "catkin ENABLED")
+	find_package(catkin REQUIRED)
+	if (catkin_FOUND)
+		catkin_package()
+	else()
+		message(FATAL_ERROR "catkin not found")
+	endif()
+else()
+	message(STATUS "catkin DISABLED")
+endif()
+
 find_package(PythonInterp REQUIRED)
 
 #=============================================================================
@@ -257,7 +281,7 @@ px4_add_git_submodule(TARGET git_matrix PATH "src/lib/matrix")
 px4_add_git_submodule(TARGET git_cmake_hexagon PATH "cmake/cmake_hexagon")
 
 add_custom_target(submodule_clean
-	WORKING_DIRECTORY ${CMAKE_SOURCE_DIR}
+	WORKING_DIRECTORY ${PX4_SOURCE_DIR}
 	COMMAND git submodule deinit -f .
 	COMMAND rm -rf .git/modules/*
 	)
@@ -267,12 +291,12 @@ add_custom_target(submodule_clean
 #
 add_custom_target(check_format
 	COMMAND Tools/check_code_style.sh
-	WORKING_DIRECTORY ${CMAKE_SOURCE_DIR}
+	WORKING_DIRECTORY ${PX4_SOURCE_DIR}
 	)
 
 add_custom_target(config
 	COMMAND cmake-gui .
-	WORKING_DIRECTORY ${CMAKE_BINARY_DIR}
+	WORKING_DIRECTORY ${PX4_BINARY_DIR}
 	)
 
 #=============================================================================
@@ -321,7 +345,7 @@ add_custom_target(xml_gen
 # external projects
 #
 
-set(ep_base ${CMAKE_BINARY_DIR}/external)
+set(ep_base ${PX4_BINARY_DIR}/external)
 set_property(DIRECTORY PROPERTY EP_BASE ${ep_base})
 
 # add external project install folders to build
@@ -350,7 +374,7 @@ foreach(module ${config_module_list})
 	if(external_module)
 		STRING(REGEX REPLACE "//" "/" EXT_MODULE ${module})
 		STRING(REGEX REPLACE "/" "__" EXT_MODULE_PREFIX ${EXT_MODULE})
-		add_subdirectory(${module} ${CMAKE_BINARY_DIR}/${EXT_MODULE_PREFIX})
+		add_subdirectory(${module} ${PX4_BINARY_DIR}/${EXT_MODULE_PREFIX})
 	else()
 		add_subdirectory(src/${module})
 	endif()
@@ -375,7 +399,7 @@ endif()
 #=============================================================================
 # generate git version
 #
-px4_create_git_hash_header(HEADER ${CMAKE_BINARY_DIR}/build_git_version.h)
+px4_create_git_hash_header(HEADER ${PX4_BINARY_DIR}/build_git_version.h)
 
 #=============================================================================
 # packaging
@@ -394,17 +418,14 @@ set(CPACK_GENERATOR "ZIP")
 set(CPACK_PACKAGE_FILE_NAME "${PROJECT_NAME}-${CONFIG}-${version}")
 set(CPACK_SOURCE_PACKAGE_FILE_NAME "${PROJECT_NAME}-${version}")
 set(CPACK_SOURCE_GENERATOR "ZIP;TBZ2")
+set(CPACK_PACKAGING_INSTALL_PREFIX "/usr")
+set(CPACK_SET_DESTDIR "OFF")
 if ("${CMAKE_SYSTEM}" MATCHES "Linux")
-  find_program(DPKG_PROGRAM dpkg)
-  if (EXISTS ${DPKG_PROGRAM})
-    list (APPEND CPACK_GENERATOR "DEB")
-  endif()
-endif()
-if (${OS} STREQUAL "posix")
-	set(CPACK_SET_DESTDIR "ON")
+	find_program(DPKG_PROGRAM dpkg)
+	if (EXISTS ${DPKG_PROGRAM})
+		list (APPEND CPACK_GENERATOR "DEB")
+	endif()
 endif()
 include(CPack)
 
-endif() # ros alternative endif
-
 # vim: set noet fenc=utf-8 ff=unix ft=cmake :
diff --git a/Makefile b/Makefile
index f80a24399c..b4a02f8d8c 100644
--- a/Makefile
+++ b/Makefile
@@ -111,20 +111,23 @@ else
 	BUILD_DIR_SUFFIX :=
 endif
 
+SRC_DIR := $(shell dirname $(realpath $(lastword $(MAKEFILE_LIST))))
+
 # Functions
 # --------------------------------------------------------------------
 # describe how to build a cmake config
 define cmake-build
-+@if [ $(PX4_CMAKE_GENERATOR) = "Ninja" ] && [ -e ./build_$@$(BUILD_DIR_SUFFIX)/Makefile ]; then rm -rf ./build_$@$(BUILD_DIR_SUFFIX); fi
-+@if [ ! -e ./build_$@$(BUILD_DIR_SUFFIX)/CMakeCache.txt ]; then Tools/check_submodules.sh && mkdir -p ./build_$@$(BUILD_DIR_SUFFIX) && cd ./build_$@$(BUILD_DIR_SUFFIX) && cmake .. -G$(PX4_CMAKE_GENERATOR) -DCONFIG=$(1) || (cd .. && rm -rf ./build_$@$(BUILD_DIR_SUFFIX)); fi
-+@Tools/check_submodules.sh
-+@(echo "PX4 CONFIG: $@$(BUILD_DIR_SUFFIX)" && cd ./build_$@$(BUILD_DIR_SUFFIX) && $(PX4_MAKE) $(PX4_MAKE_ARGS) $(ARGS))
++@$(eval BUILD_DIR = $(SRC_DIR)/build_$@$(BUILD_DIR_SUFFIX))
++@if [ $(PX4_CMAKE_GENERATOR) = "Ninja" ] && [ -e $(BUILD_DIR)/Makefile ]; then rm -rf $(BUILD_DIR); fi
++@if [ ! -e $(BUILD_DIR)/CMakeCache.txt ]; then mkdir -p $(BUILD_DIR) && cd $(BUILD_DIR) && cmake .. -G$(PX4_CMAKE_GENERATOR) -DCONFIG=$(1) || (cd .. && rm -rf $(BUILD_DIR)); fi
++@(echo "PX4 CONFIG: $(BUILD_DIR)" && cd $(BUILD_DIR) && $(PX4_MAKE) $(PX4_MAKE_ARGS) $(ARGS))
 endef
 
 define cmake-build-other
-+@if [ $(PX4_CMAKE_GENERATOR) = "Ninja" ] && [ -e ./build_$@/Makefile ]; then rm -rf ./build_$@; fi
-+@if [ ! -e ./build_$@/CMakeCache.txt ]; then Tools/check_submodules.sh && mkdir -p ./build_$@ && cd ./build_$@ && cmake $(2) -G$(PX4_CMAKE_GENERATOR) || (cd .. && rm -rf ./build_$@); fi
-+@(cd ./build_$@ && $(PX4_MAKE) $(PX4_MAKE_ARGS) $(ARGS))
++@$(eval BUILD_DIR = $(SRC_DIR)/build_$@$(BUILD_DIR_SUFFIX))
++@if [ $(PX4_CMAKE_GENERATOR) = "Ninja" ] && [ -e $(BUILD_DIR)/Makefile ]; then rm -rf $(BUILD_DIR); fi
++@if [ ! -e $(BUILD_DIR)/CMakeCache.txt ]; then mkdir -p $(BUILD_DIR) && cd $(BUILD_DIR) && cmake $(2) -G$(PX4_CMAKE_GENERATOR) -DCONFIG=$(1) || (cd .. && rm -rf $(BUILD_DIR)); fi
++@(cd $(BUILD_DIR) && $(PX4_MAKE) $(PX4_MAKE_ARGS) $(ARGS))
 endef
 
 # create empty targets to avoid msgs for targets passed to cmake
@@ -233,14 +236,8 @@ run_sitl_ros: sitl_deprecation
 # Other targets
 # --------------------------------------------------------------------
 
-.PHONY: gazebo_build uavcan_firmware check check_format unittest tests qgc_firmware package_firmware clean submodulesclean distclean
-.NOTPARALLEL: gazebo_build uavcan_firmware check check_format unittest tests qgc_firmware package_firmware clean submodulesclean distclean
-
-gazebo_build:
-	@mkdir -p build_gazebo
-	@if [ ! -e ./build_gazebo/CMakeCache.txt ];then cd build_gazebo && cmake -Wno-dev -G$(PX4_CMAKE_GENERATOR) ../Tools/sitl_gazebo; fi
-	@cd build_gazebo && $(PX4_MAKE) $(PX4_MAKE_ARGS)
-	@cd build_gazebo && $(PX4_MAKE) $(PX4_MAKE_ARGS) sdf
+.PHONY: uavcan_firmware check check_format unittest tests qgc_firmware package_firmware clean submodulesclean distclean
+.NOTPARALLEL: uavcan_firmware check check_format unittest tests qgc_firmware package_firmware clean submodulesclean distclean
 
 uavcan_firmware:
 ifeq ($(VECTORCONTROL),1)
@@ -302,11 +299,7 @@ unittest: posix_sitl_default
 	@(cd build_unittest && ctest -j2 --output-on-failure)
 
 run_tests_posix: posix_sitl_default
-	@mkdir -p build_posix_sitl_default/src/firmware/posix/rootfs/fs/microsd
-	@mkdir -p build_posix_sitl_default/src/firmware/posix/rootfs/eeprom
-	@touch build_posix_sitl_default/src/firmware/posix/rootfs/eeprom/parameters
-	@(cd build_posix_sitl_default/src/firmware/posix && ./px4 -d ../../../../posix-configs/SITL/init/rcS_tests | tee test_output)
-	@(cd build_posix_sitl_default/src/firmware/posix && grep --color=always "All tests passed" test_output)
+	@(cd build_posix_sitl_default/ && ctest -V)
 
 tests: check_unittest run_tests_posix
 
diff --git a/Tools/sitl_gazebo b/Tools/sitl_gazebo
index 00dcabb1c2..9486c331a7 160000
--- a/Tools/sitl_gazebo
+++ b/Tools/sitl_gazebo
@@ -1 +1 @@
-Subproject commit 00dcabb1c2c001b40713903da1a4a7f71d01ef27
+Subproject commit 9486c331a758fcadfaa314f6e9e44c36af5f243b
diff --git a/Tools/sitl_run.sh b/Tools/sitl_run.sh
index 6e467c31fd..8dfa2a62c6 100755
--- a/Tools/sitl_run.sh
+++ b/Tools/sitl_run.sh
@@ -1,22 +1,29 @@
 #!/bin/bash
 
-rc_script=$1
-debugger=$2
-program=$3
-model=$4
-build_path=$5
-curr_dir=`pwd`
+set -e
+
+echo args: $@
+
+sitl_bin=$1
+rc_script=$2
+debugger=$3
+program=$4
+model=$5
+src_path=$6
+build_path=$7
 
 echo SITL ARGS
+
+echo sitl_bin: $sitl_bin
 echo rc_script: $rc_script
 echo debugger: $debugger
 echo program: $program
 echo model: $model
+echo src_path: $src_path
 echo build_path: $build_path
 
-mkdir -p $build_path/src/firmware/posix/rootfs/fs/microsd
-mkdir -p $build_path/src/firmware/posix/rootfs/eeprom
-touch $build_path/src/firmware/posix/rootfs/eeprom/parameters
+working_dir=`pwd`
+sitl_bin=$build_path/src/firmware/posix/px4
 
 if [ "$chroot" == "1" ]
 then
@@ -35,32 +42,36 @@ fi
 
 if [ "$#" -lt 5 ]
 then
-	echo usage: sitl_run.sh rc_script debugger program model build_path
+	echo usage: sitl_run.sh rc_script debugger program model devel_path
 	echo ""
 	exit 1
 fi
 
+command_exists () {
+    type "$1" &> /dev/null ;
+}
+
 # kill process names that might stil
 # be running from last time
-pkill gazebo
-pkill px4
-jmavsim_pid=`jps | grep Simulator | cut -d" " -f1`
-if [ -n "$jmavsim_pid" ]
+pgrep gazebo && pkill gazebo
+pgrep px4 && pkill px4
+if command_exists jps
 then
-	kill $jmavsim_pid
+	jmavsim_pid=`jps | grep Simulator | cut -d" " -f1`
+	if [ -n "$jmavsim_pid" ]
+	then
+		kill $jmavsim_pid
+	fi
 fi
 
-set -e
-
-cd $build_path/..
-cp Tools/posix_lldbinit $build_path/src/firmware/posix/.lldbinit
-cp Tools/posix.gdbinit $build_path/src/firmware/posix/.gdbinit
+cp $src_path/Tools/posix_lldbinit $working_dir/.lldbinit
+cp $src_path/Tools/posix.gdbinit $working_dir/.gdbinit
 
 SIM_PID=0
 
 if [ "$program" == "jmavsim" ] && [ ! -n "$no_sim" ]
 then
-	cd Tools/jMAVSim
+	cd $src_path/Tools/jMAVSim
 	ant create_run_jar copy_res
 	cd out/production
 	java -Djava.ext.dirs= -jar jmavsim_run.jar -udp 127.0.0.1:14560 &
@@ -71,15 +82,9 @@ then
 	if [ -x "$(command -v gazebo)" ]
 	then
 		# Set the plugin path so Gazebo finds our model and sim
-		export GAZEBO_PLUGIN_PATH=$curr_dir/build_gazebo:${GAZEBO_PLUGIN_PATH}
-		# Set the model path so Gazebo finds the airframes
-		export GAZEBO_MODEL_PATH=${GAZEBO_MODEL_PATH}:$curr_dir/Tools/sitl_gazebo/models
-		# The next line would disable online model lookup, can be commented in, in case of unstable behaviour.
-		# export GAZEBO_MODEL_DATABASE_URI=""
-		export SITL_GAZEBO_PATH=$curr_dir/Tools/sitl_gazebo
-		make --no-print-directory gazebo_build
-
-		gzserver --verbose $curr_dir/Tools/sitl_gazebo/worlds/${model}.world &
+		source /usr/share/gazebo/setup.sh
+		source $src_path/integrationtests/setup_gazebo_ros.bash ${src_path} ${build_path}
+		gzserver --verbose worlds/${model}.world &
 		SIM_PID=`echo $!`
 
 		if [[ -n "$HEADLESS" ]]; then
@@ -98,17 +103,17 @@ then
 	# This is not a simulator, but a log file to replay
 
 	# Check if we need to creat a param file to allow user to change parameters
-	if ! [ -f "${build_path}/src/firmware/posix/rootfs/replay_params.txt" ]
+	if ! [ -f "$rootfs/replay_params.txt" ]
 		then
-		touch ${build_path}/src/firmware/posix/rootfs/replay_params.txt
+		touch $rootfs/replay_params.txt
 	fi
 fi
 
-cd $build_path/src/firmware/posix
+cd $working_dir
 
 if [ "$logfile" != "" ]
 then
-	cp $logfile rootfs/replay.px4log
+	cp $logfile $rootfs/replay.px4log
 fi
 
 # Do not exit on failure now from here on because we want the complete cleanup
@@ -117,18 +122,18 @@ set +e
 # Start Java simulator
 if [ "$debugger" == "lldb" ]
 then
-	lldb -- px4 ../../../../${rc_script}_${program}_${model}
+	lldb -- $sitl_bin $src_path $src_path/${rc_script}_${program}_${model}
 elif [ "$debugger" == "gdb" ]
 then
-	gdb --args px4 ../../../../${rc_script}_${program}_${model}
+	gdb --args $sitl_bin $src_path $src_path/${rc_script}_${program}_${model}
 elif [ "$debugger" == "ddd" ]
 then
-	ddd --debugger gdb --args px4 ../../../../${rc_script}_${program}_${model}
+	ddd --debugger gdb --args px4 $src_path $src_path/${rc_script}_${program}_${model}
 elif [ "$debugger" == "valgrind" ]
 then
-	valgrind ./px4 ../../../../${rc_script}_${program}_${model}
+	valgrind $sitl_bin $src_path $src_path/${rc_script}_${program}_${model}
 else
-	$sudo_enabled ./px4 $chroot_enabled ../../../../${rc_script}_${program}_${model}
+	$sudo_enabled $sitl_bin $chroot_enabled $src_path $src_path/${rc_script}_${program}_${model}
 fi
 
 if [ "$program" == "jmavsim" ]
diff --git a/cmake/common/px4_base.cmake b/cmake/common/px4_base.cmake
index ef05520375..88f1e57f98 100644
--- a/cmake/common/px4_base.cmake
+++ b/cmake/common/px4_base.cmake
@@ -136,14 +136,14 @@ function(px4_add_git_submodule)
 		REQUIRED TARGET PATH
 		ARGN ${ARGN})
 	string(REPLACE "/" "_" NAME ${PATH})
-	add_custom_command(OUTPUT ${CMAKE_BINARY_DIR}/git_init_${NAME}.stamp
-		WORKING_DIRECTORY ${CMAKE_SOURCE_DIR}
-		COMMAND touch ${CMAKE_BINARY_DIR}/git_init_${NAME}.stamp
-		DEPENDS ${CMAKE_SOURCE_DIR}/.gitmodules
+	add_custom_command(OUTPUT ${PX4_BINARY_DIR}/git_init_${NAME}.stamp
+		WORKING_DIRECTORY ${PX4_SOURCE_DIR}
+		COMMAND touch ${PX4_BINARY_DIR}/git_init_${NAME}.stamp
+		DEPENDS ${PX4_SOURCE_DIR}/.gitmodules
 		)
 	add_custom_target(${TARGET}
-		WORKING_DIRECTORY ${CMAKE_SOURCE_DIR}
-		DEPENDS ${CMAKE_BINARY_DIR}/git_init_${NAME}.stamp
+		WORKING_DIRECTORY ${PX4_SOURCE_DIR}
+		DEPENDS ${PX4_BINARY_DIR}/git_init_${NAME}.stamp
 		)
 endfunction()
 
@@ -371,7 +371,7 @@ function(px4_generate_messages)
 	endif()
 
 	# headers
-	set(msg_out_path ${CMAKE_BINARY_DIR}/src/modules/uORB/topics)
+	set(msg_out_path ${PX4_BINARY_DIR}/src/modules/uORB/topics)
 	set(msg_list)
 	foreach(msg_file ${MSG_FILES})
 		get_filename_component(msg ${msg_file} NAME_WE)
@@ -389,15 +389,15 @@ function(px4_generate_messages)
 			-d msg
 			-o ${msg_out_path}
 			-e msg/templates/uorb
-			-t ${CMAKE_BINARY_DIR}/topics_temporary_header
+			-t ${PX4_BINARY_DIR}/topics_temporary_header
 		DEPENDS ${DEPENDS} ${MSG_FILES}
-		WORKING_DIRECTORY ${CMAKE_SOURCE_DIR}
+		WORKING_DIRECTORY ${PX4_SOURCE_DIR}
 		COMMENT "Generating uORB topic headers"
 		VERBATIM
 		)
 
 	# !sources
-	set(msg_source_out_path	${CMAKE_BINARY_DIR}/topics_sources)
+	set(msg_source_out_path	${PX4_BINARY_DIR}/topics_sources)
 	set(msg_source_files_out ${msg_source_out_path}/uORBTopics.cpp)
 	foreach(msg ${msg_list})
 		list(APPEND msg_source_files_out ${msg_source_out_path}/${msg}.cpp)
@@ -410,9 +410,9 @@ function(px4_generate_messages)
 			-d msg
 			-o ${msg_source_out_path}
 			-e msg/templates/uorb
-			-t ${CMAKE_BINARY_DIR}/topics_temporary_sources
+			-t ${PX4_BINARY_DIR}/topics_temporary_sources
 		DEPENDS ${DEPENDS} ${MSG_FILES}
-		WORKING_DIRECTORY ${CMAKE_SOURCE_DIR}
+		WORKING_DIRECTORY ${PX4_SOURCE_DIR}
 		COMMENT "Generating uORB topic sources"
 		VERBATIM
 		)
@@ -427,7 +427,7 @@ function(px4_generate_messages)
 
 	# multi messages for target OS
 	set(msg_multi_out_path
-		${CMAKE_BINARY_DIR}/src/platforms/${OS}/px4_messages)
+		${PX4_BINARY_DIR}/src/platforms/${OS}/px4_messages)
 	set(msg_multi_files_out)
 	foreach(msg ${msg_list})
 		list(APPEND msg_multi_files_out ${msg_multi_out_path}/px4_${msg}.h)
@@ -440,10 +440,10 @@ function(px4_generate_messages)
 			-d msg
 			-o ${msg_multi_out_path}
 			-e msg/templates/px4/uorb
-			-t ${CMAKE_BINARY_DIR}/multi_topics_temporary/${OS}
+			-t ${PX4_BINARY_DIR}/multi_topics_temporary/${OS}
 			-p "px4_"
 		DEPENDS ${DEPENDS} ${MSG_FILES}
-		WORKING_DIRECTORY ${CMAKE_SOURCE_DIR}
+		WORKING_DIRECTORY ${PX4_SOURCE_DIR}
 		COMMENT "Generating uORB topic multi headers for ${OS}"
 		VERBATIM
 		)
@@ -504,9 +504,9 @@ function(px4_add_upload)
 	px4_join(OUT serial_ports LIST "${serial_ports}" GLUE ",")
 	add_custom_target(${OUT}
 		COMMAND ${PYTHON_EXECUTABLE}
-			${CMAKE_SOURCE_DIR}/Tools/px_uploader.py --port ${serial_ports} ${BUNDLE}
+			${PX4_SOURCE_DIR}/Tools/px_uploader.py --port ${serial_ports} ${BUNDLE}
 		DEPENDS ${BUNDLE}
-		WORKING_DIRECTORY ${CMAKE_BINARY_DIR}
+		WORKING_DIRECTORY ${PX4_BINARY_DIR}
 		COMMENT "uploading ${BUNDLE}"
 		VERBATIM
 		USES_TERMINAL
@@ -523,9 +523,9 @@ function(px4_add_adb_push)
 		ARGN ${ARGN})
 
 	add_custom_target(${OUT}
-		COMMAND ${CMAKE_SOURCE_DIR}/Tools/adb_upload.sh ${FILES} ${DEST}
+		COMMAND ${PX4_SOURCE_DIR}/Tools/adb_upload.sh ${FILES} ${DEST}
 		DEPENDS ${DEPENDS}
-		WORKING_DIRECTORY ${CMAKE_BINARY_DIR}
+		WORKING_DIRECTORY ${PX4_BINARY_DIR}
 		COMMENT "uploading ${BUNDLE}"
 		VERBATIM
 		USES_TERMINAL
@@ -541,9 +541,9 @@ function(px4_add_adb_push_to_bebop)
 		ARGN ${ARGN})
 
 	add_custom_target(${OUT}
-		COMMAND ${CMAKE_SOURCE_DIR}/Tools/adb_upload_to_bebop.sh ${FILES} ${DEST}
+		COMMAND ${PX4_SOURCE_DIR}/Tools/adb_upload_to_bebop.sh ${FILES} ${DEST}
 		DEPENDS ${DEPENDS}
-		WORKING_DIRECTORY ${CMAKE_BINARY_DIR}
+		WORKING_DIRECTORY ${PX4_BINARY_DIR}
 		COMMENT "uploading ${BUNDLE}"
 		VERBATIM
 		USES_TERMINAL
@@ -559,9 +559,9 @@ function(px4_add_scp_push)
 		ARGN ${ARGN})
 
 	add_custom_target(${OUT}
-		COMMAND ${CMAKE_SOURCE_DIR}/Tools/scp_upload.sh ${FILES} ${DEST}
+		COMMAND ${PX4_SOURCE_DIR}/Tools/scp_upload.sh ${FILES} ${DEST}
 		DEPENDS ${DEPENDS}
-		WORKING_DIRECTORY ${CMAKE_BINARY_DIR}
+		WORKING_DIRECTORY ${PX4_BINARY_DIR}
 		COMMENT "uploading ${BUNDLE}"
 		VERBATIM
 		USES_TERMINAL
@@ -761,21 +761,21 @@ function(px4_add_common_flags)
 		)
 
 	set(added_include_dirs
-		${CMAKE_SOURCE_DIR}/src
-		${CMAKE_BINARY_DIR}
-		${CMAKE_BINARY_DIR}/src
-		${CMAKE_SOURCE_DIR}/src/modules
-		${CMAKE_SOURCE_DIR}/src/include
-		${CMAKE_SOURCE_DIR}/src/lib
-		${CMAKE_SOURCE_DIR}/src/platforms
+		${PX4_SOURCE_DIR}/src
+		${PX4_BINARY_DIR}
+		${PX4_BINARY_DIR}/src
+		${PX4_SOURCE_DIR}/src/modules
+		${PX4_SOURCE_DIR}/src/include
+		${PX4_SOURCE_DIR}/src/lib
+		${PX4_SOURCE_DIR}/src/platforms
 		# TODO Build/versioning was in Makefile,
 		# do we need this, how does it work with cmake
-		${CMAKE_SOURCE_DIR}/src/drivers/boards/${BOARD}
-		${CMAKE_BINARY_DIR}
-		${CMAKE_BINARY_DIR}/src/modules/px4_messages
-		${CMAKE_BINARY_DIR}/src/modules
-		${CMAKE_SOURCE_DIR}/mavlink/include/mavlink
-		${CMAKE_SOURCE_DIR}/src/lib/DriverFramework/framework/include
+		${PX4_SOURCE_DIR}/src/drivers/boards/${BOARD}
+		${PX4_BINARY_DIR}
+		${PX4_BINARY_DIR}/src/modules/px4_messages
+		${PX4_BINARY_DIR}/src/modules
+		${PX4_SOURCE_DIR}/mavlink/include/mavlink
+		${PX4_SOURCE_DIR}/src/lib/DriverFramework/framework/include
 		)
 
 	list(APPEND added_include_dirs
@@ -858,19 +858,19 @@ function(px4_create_git_hash_header)
 		COMMAND git describe --always --tags
 		OUTPUT_VARIABLE git_tag
 		OUTPUT_STRIP_TRAILING_WHITESPACE
-		WORKING_DIRECTORY ${CMAKE_SOURCE_DIR}
+		WORKING_DIRECTORY ${PX4_SOURCE_DIR}
 		)
 	message(STATUS "GIT_TAG = ${git_tag}")
 	execute_process(
 		COMMAND git rev-parse --verify HEAD
 		OUTPUT_VARIABLE git_version
 		OUTPUT_STRIP_TRAILING_WHITESPACE
-		WORKING_DIRECTORY ${CMAKE_SOURCE_DIR}
+		WORKING_DIRECTORY ${PX4_SOURCE_DIR}
 		)
 	#message(STATUS "GIT_VERSION = ${git_version}")
 	set(git_version_short)
 	string(SUBSTRING ${git_version} 1 16 git_version_short)
-	configure_file(${CMAKE_SOURCE_DIR}/cmake/templates/build_git_version.h.in ${HEADER} @ONLY)
+	configure_file(${PX4_SOURCE_DIR}/cmake/templates/build_git_version.h.in ${HEADER} @ONLY)
 endfunction()
 
 #=============================================================================
@@ -897,12 +897,12 @@ function(px4_generate_parameters_xml)
 		ONE_VALUE OUT BOARD
 		REQUIRED OUT BOARD
 		ARGN ${ARGN})
-	set(path ${CMAKE_SOURCE_DIR}/src)
+	set(path ${PX4_SOURCE_DIR}/src)
 	file(GLOB_RECURSE param_src_files
-		${CMAKE_SOURCE_DIR}/src/*params.c
+		${PX4_SOURCE_DIR}/src/*params.c
 		)
 	add_custom_command(OUTPUT ${OUT}
-		COMMAND ${PYTHON_EXECUTABLE} ${CMAKE_SOURCE_DIR}/Tools/px_process_params.py
+		COMMAND ${PYTHON_EXECUTABLE} ${PX4_SOURCE_DIR}/Tools/px_process_params.py
 			-s ${path} --board CONFIG_ARCH_${BOARD} --xml --inject-xml
 		DEPENDS ${param_src_files}
 		)
@@ -944,7 +944,7 @@ function(px4_generate_parameters_source)
 		set(SCOPE "")
 	endif()
 	add_custom_command(OUTPUT ${generated_files}
-		COMMAND ${PYTHON_EXECUTABLE} ${CMAKE_SOURCE_DIR}/Tools/px_generate_params.py ${XML} ${SCOPE}
+		COMMAND ${PYTHON_EXECUTABLE} ${PX4_SOURCE_DIR}/Tools/px_generate_params.py ${XML} ${SCOPE}
 		DEPENDS ${XML} ${DEPS} ${SCOPE}
 		)
 	set(${OUT} ${generated_files} PARENT_SCOPE)
@@ -975,10 +975,10 @@ function(px4_generate_airframes_xml)
 		ONE_VALUE OUT BOARD
 		REQUIRED OUT BOARD
 		ARGN ${ARGN})
-	set(process_airframes ${CMAKE_SOURCE_DIR}/Tools/px_process_airframes.py)
+	set(process_airframes ${PX4_SOURCE_DIR}/Tools/px_process_airframes.py)
 	add_custom_command(OUTPUT ${OUT}
 		COMMAND ${PYTHON_EXECUTABLE} ${process_airframes}
-			-a ${CMAKE_SOURCE_DIR}/ROMFS/px4fmu_common/init.d
+			-a ${PX4_SOURCE_DIR}/ROMFS/px4fmu_common/init.d
 			--board CONFIG_ARCH_BOARD_${BOARD} --xml
 		)
 	set(${OUT} ${${OUT}} PARENT_SCOPE)
diff --git a/cmake/configs/nuttx_mindpx-v2_default.cmake b/cmake/configs/nuttx_mindpx-v2_default.cmake
index 41c9d32df8..32bbf050fd 100644
--- a/cmake/configs/nuttx_mindpx-v2_default.cmake
+++ b/cmake/configs/nuttx_mindpx-v2_default.cmake
@@ -1,6 +1,6 @@
 include(nuttx/px4_impl_nuttx)
 
-set(CMAKE_TOOLCHAIN_FILE ${CMAKE_SOURCE_DIR}/cmake/toolchains/Toolchain-arm-none-eabi.cmake)
+set(CMAKE_TOOLCHAIN_FILE ${PX4_SOURCE_DIR}/cmake/toolchains/Toolchain-arm-none-eabi.cmake)
 
 set(config_uavcan_num_ifaces 2)
 
diff --git a/cmake/configs/nuttx_px4-stm32f4discovery_default.cmake b/cmake/configs/nuttx_px4-stm32f4discovery_default.cmake
index c71db2fc31..377a90f335 100644
--- a/cmake/configs/nuttx_px4-stm32f4discovery_default.cmake
+++ b/cmake/configs/nuttx_px4-stm32f4discovery_default.cmake
@@ -1,6 +1,6 @@
 include(nuttx/px4_impl_nuttx)
 
-set(CMAKE_TOOLCHAIN_FILE ${CMAKE_SOURCE_DIR}/cmake/toolchains/Toolchain-arm-none-eabi.cmake)
+set(CMAKE_TOOLCHAIN_FILE ${PX4_SOURCE_DIR}/cmake/toolchains/Toolchain-arm-none-eabi.cmake)
 
 set(config_module_list
 	#
diff --git a/cmake/configs/nuttx_px4fmu-v1_default.cmake b/cmake/configs/nuttx_px4fmu-v1_default.cmake
index 75f8d054ed..2c0a5f7315 100644
--- a/cmake/configs/nuttx_px4fmu-v1_default.cmake
+++ b/cmake/configs/nuttx_px4fmu-v1_default.cmake
@@ -1,6 +1,6 @@
 include(nuttx/px4_impl_nuttx)
 
-set(CMAKE_TOOLCHAIN_FILE ${CMAKE_SOURCE_DIR}/cmake/toolchains/Toolchain-arm-none-eabi.cmake)
+set(CMAKE_TOOLCHAIN_FILE ${PX4_SOURCE_DIR}/cmake/toolchains/Toolchain-arm-none-eabi.cmake)
 
 set(config_module_list
 	#
diff --git a/cmake/configs/nuttx_px4fmu-v2_default.cmake b/cmake/configs/nuttx_px4fmu-v2_default.cmake
index c38d726702..13d02e6d28 100644
--- a/cmake/configs/nuttx_px4fmu-v2_default.cmake
+++ b/cmake/configs/nuttx_px4fmu-v2_default.cmake
@@ -1,6 +1,6 @@
 include(nuttx/px4_impl_nuttx)
 
-set(CMAKE_TOOLCHAIN_FILE ${CMAKE_SOURCE_DIR}/cmake/toolchains/Toolchain-arm-none-eabi.cmake)
+set(CMAKE_TOOLCHAIN_FILE ${PX4_SOURCE_DIR}/cmake/toolchains/Toolchain-arm-none-eabi.cmake)
 
 set(config_uavcan_num_ifaces 2)
 
diff --git a/cmake/configs/nuttx_px4fmu-v2_ekf2.cmake b/cmake/configs/nuttx_px4fmu-v2_ekf2.cmake
index f43b782045..e500752214 100644
--- a/cmake/configs/nuttx_px4fmu-v2_ekf2.cmake
+++ b/cmake/configs/nuttx_px4fmu-v2_ekf2.cmake
@@ -1,6 +1,6 @@
 include(nuttx/px4_impl_nuttx)
 
-set(CMAKE_TOOLCHAIN_FILE ${CMAKE_SOURCE_DIR}/cmake/toolchains/Toolchain-arm-none-eabi.cmake)
+set(CMAKE_TOOLCHAIN_FILE ${PX4_SOURCE_DIR}/cmake/toolchains/Toolchain-arm-none-eabi.cmake)
 
 set(config_uavcan_num_ifaces 2)
 
diff --git a/cmake/configs/nuttx_px4fmu-v2_test.cmake b/cmake/configs/nuttx_px4fmu-v2_test.cmake
index b72c5a8190..1839d8689d 100644
--- a/cmake/configs/nuttx_px4fmu-v2_test.cmake
+++ b/cmake/configs/nuttx_px4fmu-v2_test.cmake
@@ -1,6 +1,6 @@
 include(nuttx/px4_impl_nuttx)
 
-set(CMAKE_TOOLCHAIN_FILE ${CMAKE_SOURCE_DIR}/cmake/toolchains/Toolchain-arm-none-eabi.cmake)
+set(CMAKE_TOOLCHAIN_FILE ${PX4_SOURCE_DIR}/cmake/toolchains/Toolchain-arm-none-eabi.cmake)
 
 set(config_uavcan_num_ifaces 2)
 
diff --git a/cmake/configs/nuttx_px4fmu-v4_default.cmake b/cmake/configs/nuttx_px4fmu-v4_default.cmake
index 866e19dc0d..ad746ee426 100644
--- a/cmake/configs/nuttx_px4fmu-v4_default.cmake
+++ b/cmake/configs/nuttx_px4fmu-v4_default.cmake
@@ -1,6 +1,6 @@
 include(nuttx/px4_impl_nuttx)
 
-set(CMAKE_TOOLCHAIN_FILE ${CMAKE_SOURCE_DIR}/cmake/toolchains/Toolchain-arm-none-eabi.cmake)
+set(CMAKE_TOOLCHAIN_FILE ${PX4_SOURCE_DIR}/cmake/toolchains/Toolchain-arm-none-eabi.cmake)
 
 set(config_uavcan_num_ifaces 1)
 
diff --git a/cmake/configs/nuttx_tap-v1_default.cmake b/cmake/configs/nuttx_tap-v1_default.cmake
index 2dad824901..8a5b8187fb 100644
--- a/cmake/configs/nuttx_tap-v1_default.cmake
+++ b/cmake/configs/nuttx_tap-v1_default.cmake
@@ -1,6 +1,6 @@
 include(nuttx/px4_impl_nuttx)
 
-set(CMAKE_TOOLCHAIN_FILE ${CMAKE_SOURCE_DIR}/cmake/toolchains/Toolchain-arm-none-eabi.cmake)
+set(CMAKE_TOOLCHAIN_FILE ${PX4_SOURCE_DIR}/cmake/toolchains/Toolchain-arm-none-eabi.cmake)
 
 set(config_module_list
 	#
diff --git a/cmake/configs/posix_bebop_default.cmake b/cmake/configs/posix_bebop_default.cmake
index 5538ffe35a..72e945db0f 100644
--- a/cmake/configs/posix_bebop_default.cmake
+++ b/cmake/configs/posix_bebop_default.cmake
@@ -1,6 +1,6 @@
 include(posix/px4_impl_posix)
 
-set(CMAKE_TOOLCHAIN_FILE ${CMAKE_SOURCE_DIR}/cmake/toolchains/Toolchain-arm-linux-gnueabihf-raspbian.cmake)
+set(CMAKE_TOOLCHAIN_FILE ${PX4_SOURCE_DIR}/cmake/toolchains/Toolchain-arm-linux-gnueabihf-raspbian.cmake)
 
 add_definitions(
   -D__PX4_POSIX_BEBOP
diff --git a/cmake/configs/posix_eagle_default.cmake b/cmake/configs/posix_eagle_default.cmake
index 3a6dd22755..be81961425 100644
--- a/cmake/configs/posix_eagle_default.cmake
+++ b/cmake/configs/posix_eagle_default.cmake
@@ -4,9 +4,9 @@
 # on the Linux side of the Snapdragon.
 include(configs/posix_sdflight_default)
 
-set(CMAKE_TOOLCHAIN_FILE ${CMAKE_SOURCE_DIR}/cmake/cmake_hexagon/toolchain/Toolchain-arm-linux-gnueabihf.cmake)
+set(CMAKE_TOOLCHAIN_FILE ${PX4_SOURCE_DIR}/cmake/cmake_hexagon/toolchain/Toolchain-arm-linux-gnueabihf.cmake)
 
-set(CMAKE_MODULE_PATH ${CMAKE_MODULE_PATH} "${CMAKE_SOURCE_DIR}/cmake/cmake_hexagon")
+set(CMAKE_MODULE_PATH ${CMAKE_MODULE_PATH} "${PX4_SOURCE_DIR}/cmake/cmake_hexagon")
 
 set(CONFIG_SHMEM "1")
 
diff --git a/cmake/configs/posix_eagle_hil.cmake b/cmake/configs/posix_eagle_hil.cmake
index 909bdeecb1..d38da131bd 100644
--- a/cmake/configs/posix_eagle_hil.cmake
+++ b/cmake/configs/posix_eagle_hil.cmake
@@ -1,13 +1,13 @@
 include(posix/px4_impl_posix)
 
-set(CMAKE_MODULE_PATH ${CMAKE_MODULE_PATH} "${CMAKE_SOURCE_DIR}/cmake/cmake_hexagon")
+set(CMAKE_MODULE_PATH ${CMAKE_MODULE_PATH} "${PX4_SOURCE_DIR}/cmake/cmake_hexagon")
 
 # Use build stubs unless explicitly set not to
 if("${DSPAL_STUBS_ENABLE}" STREQUAL "")
 	set(DSPAL_STUBS_ENABLE "1")
 endif()
 
-set(CMAKE_TOOLCHAIN_FILE ${CMAKE_SOURCE_DIR}/cmake/toolchains/Toolchain-arm-linux-gnueabihf.cmake)
+set(CMAKE_TOOLCHAIN_FILE ${PX4_SOURCE_DIR}/cmake/toolchains/Toolchain-arm-linux-gnueabihf.cmake)
 
 set(config_generate_parameters_scope ALL)
 
diff --git a/cmake/configs/posix_eagle_legacy_driver_default.cmake b/cmake/configs/posix_eagle_legacy_driver_default.cmake
index ec172e7602..534d327f7e 100644
--- a/cmake/configs/posix_eagle_legacy_driver_default.cmake
+++ b/cmake/configs/posix_eagle_legacy_driver_default.cmake
@@ -1,8 +1,8 @@
 include(posix/px4_impl_posix)
 
-set(CMAKE_TOOLCHAIN_FILE ${CMAKE_SOURCE_DIR}/cmake/cmake_hexagon/toolchain/Toolchain-arm-linux-gnueabihf.cmake)
+set(CMAKE_TOOLCHAIN_FILE ${PX4_SOURCE_DIR}/cmake/cmake_hexagon/toolchain/Toolchain-arm-linux-gnueabihf.cmake)
 
-set(CMAKE_MODULE_PATH ${CMAKE_MODULE_PATH} "${CMAKE_SOURCE_DIR}/cmake/cmake_hexagon")
+set(CMAKE_MODULE_PATH ${CMAKE_MODULE_PATH} "${PX4_SOURCE_DIR}/cmake/cmake_hexagon")
 
 set(config_generate_parameters_scope ALL)
 
diff --git a/cmake/configs/posix_eagle_muorb.cmake b/cmake/configs/posix_eagle_muorb.cmake
index 95a5c59beb..f47c421dce 100644
--- a/cmake/configs/posix_eagle_muorb.cmake
+++ b/cmake/configs/posix_eagle_muorb.cmake
@@ -1,8 +1,8 @@
 include(posix/px4_impl_posix)
 
-set(CMAKE_TOOLCHAIN_FILE ${CMAKE_SOURCE_DIR}/cmake/toolchains/Toolchain-arm-linux-gnueabihf.cmake)
+set(CMAKE_TOOLCHAIN_FILE ${PX4_SOURCE_DIR}/cmake/toolchains/Toolchain-arm-linux-gnueabihf.cmake)
 
-set(CMAKE_MODULE_PATH ${CMAKE_MODULE_PATH} "${CMAKE_SOURCE_DIR}/cmake/cmake_hexagon")
+set(CMAKE_MODULE_PATH ${CMAKE_MODULE_PATH} "${PX4_SOURCE_DIR}/cmake/cmake_hexagon")
 
 set(config_generate_parameters_scope ALL)
 
diff --git a/cmake/configs/posix_rpi_cross.cmake b/cmake/configs/posix_rpi_cross.cmake
index 46243fc361..72896086c6 100644
--- a/cmake/configs/posix_rpi_cross.cmake
+++ b/cmake/configs/posix_rpi_cross.cmake
@@ -1,6 +1,6 @@
 include(configs/posix_rpi_common)
 
-set(CMAKE_TOOLCHAIN_FILE ${CMAKE_SOURCE_DIR}/cmake/toolchains/Toolchain-arm-linux-gnueabihf-raspbian.cmake)
+set(CMAKE_TOOLCHAIN_FILE ${PX4_SOURCE_DIR}/cmake/toolchains/Toolchain-arm-linux-gnueabihf-raspbian.cmake)
 
 set(CMAKE_PROGRAM_PATH
 	"${RPI_TOOLCHAIN_DIR}/gcc-linaro-arm-linux-gnueabihf-raspbian/bin"
diff --git a/cmake/configs/posix_rpi_native.cmake b/cmake/configs/posix_rpi_native.cmake
index e58f370282..b2a54c4f65 100644
--- a/cmake/configs/posix_rpi_native.cmake
+++ b/cmake/configs/posix_rpi_native.cmake
@@ -1,3 +1,3 @@
 include(configs/posix_rpi_common)
 
-set(CMAKE_TOOLCHAIN_FILE ${CMAKE_SOURCE_DIR}/cmake/toolchains/Toolchain-native.cmake)
+set(CMAKE_TOOLCHAIN_FILE ${PX4_SOURCE_DIR}/cmake/toolchains/Toolchain-native.cmake)
diff --git a/cmake/configs/posix_sdflight_default.cmake b/cmake/configs/posix_sdflight_default.cmake
index c43e51974c..fc66327184 100644
--- a/cmake/configs/posix_sdflight_default.cmake
+++ b/cmake/configs/posix_sdflight_default.cmake
@@ -1,8 +1,8 @@
 include(posix/px4_impl_posix)
 
-set(CMAKE_TOOLCHAIN_FILE ${CMAKE_SOURCE_DIR}/cmake/toolchains/Toolchain-arm-linux-gnueabihf.cmake)
+set(CMAKE_TOOLCHAIN_FILE ${PX4_SOURCE_DIR}/cmake/toolchains/Toolchain-arm-linux-gnueabihf.cmake)
 
-set(CMAKE_MODULE_PATH ${CMAKE_MODULE_PATH} "${CMAKE_SOURCE_DIR}/cmake/cmake_hexagon")
+set(CMAKE_MODULE_PATH ${CMAKE_MODULE_PATH} "${PX4_SOURCE_DIR}/cmake/cmake_hexagon")
 
 set(config_generate_parameters_scope ALL)
 
diff --git a/cmake/configs/posix_sitl_broadcast.cmake b/cmake/configs/posix_sitl_broadcast.cmake
index 9a9029f1c0..0c7bf4b0a2 100644
--- a/cmake/configs/posix_sitl_broadcast.cmake
+++ b/cmake/configs/posix_sitl_broadcast.cmake
@@ -1,6 +1,6 @@
 include(posix/px4_impl_posix)
 
-set(CMAKE_TOOLCHAIN_FILE ${CMAKE_SOURCE_DIR}/cmake/toolchains/Toolchain-native.cmake)
+set(CMAKE_TOOLCHAIN_FILE ${PX4_SOURCE_DIR}/cmake/toolchains/Toolchain-native.cmake)
 
 set(config_module_list
 	drivers/device
diff --git a/cmake/configs/posix_sitl_default.cmake b/cmake/configs/posix_sitl_default.cmake
index 6fa6d9c0b7..62caa65e72 100644
--- a/cmake/configs/posix_sitl_default.cmake
+++ b/cmake/configs/posix_sitl_default.cmake
@@ -1,6 +1,6 @@
 include(posix/px4_impl_posix)
 
-set(CMAKE_TOOLCHAIN_FILE ${CMAKE_SOURCE_DIR}/cmake/toolchains/Toolchain-native.cmake)
+set(CMAKE_TOOLCHAIN_FILE ${PX4_SOURCE_DIR}/cmake/toolchains/Toolchain-native.cmake)
 
 set(config_module_list
 	drivers/boards/sitl
diff --git a/cmake/configs/posix_sitl_replay.cmake b/cmake/configs/posix_sitl_replay.cmake
index 3c99cba7a7..a20a2d8cfe 100644
--- a/cmake/configs/posix_sitl_replay.cmake
+++ b/cmake/configs/posix_sitl_replay.cmake
@@ -1,6 +1,6 @@
 include(posix/px4_impl_posix)
 
-set(CMAKE_TOOLCHAIN_FILE ${CMAKE_SOURCE_DIR}/cmake/toolchains/Toolchain-native.cmake)
+set(CMAKE_TOOLCHAIN_FILE ${PX4_SOURCE_DIR}/cmake/toolchains/Toolchain-native.cmake)
 
 set(config_module_list
 	drivers/device
diff --git a/cmake/configs/qurt_eagle_hello.cmake b/cmake/configs/qurt_eagle_hello.cmake
index e1a66ea827..db16c17d2a 100644
--- a/cmake/configs/qurt_eagle_hello.cmake
+++ b/cmake/configs/qurt_eagle_hello.cmake
@@ -6,9 +6,9 @@ else()
 	set(HEXAGON_SDK_ROOT $ENV{HEXAGON_SDK_ROOT})
 endif()
 
-set(CMAKE_TOOLCHAIN_FILE ${CMAKE_SOURCE_DIR}/cmake/cmake_hexagon/toolchain/Toolchain-qurt.cmake)
+set(CMAKE_TOOLCHAIN_FILE ${PX4_SOURCE_DIR}/cmake/cmake_hexagon/toolchain/Toolchain-qurt.cmake)
 
-set(CMAKE_MODULE_PATH ${CMAKE_MODULE_PATH} "${CMAKE_SOURCE_DIR}/cmake/cmake_hexagon")
+set(CMAKE_MODULE_PATH ${CMAKE_MODULE_PATH} "${PX4_SOURCE_DIR}/cmake/cmake_hexagon")
 
 set(config_generate_parameters_scope ALL)
 
diff --git a/cmake/configs/qurt_eagle_hil.cmake b/cmake/configs/qurt_eagle_hil.cmake
index 469432818f..9eba5dea83 100644
--- a/cmake/configs/qurt_eagle_hil.cmake
+++ b/cmake/configs/qurt_eagle_hil.cmake
@@ -6,9 +6,9 @@ else()
 	set(HEXAGON_SDK_ROOT $ENV{HEXAGON_SDK_ROOT})
 endif()
 
-set(CMAKE_TOOLCHAIN_FILE ${CMAKE_SOURCE_DIR}/cmake/cmake_hexagon/toolchain/Toolchain-qurt.cmake)
+set(CMAKE_TOOLCHAIN_FILE ${PX4_SOURCE_DIR}/cmake/cmake_hexagon/toolchain/Toolchain-qurt.cmake)
 
-set(CMAKE_MODULE_PATH ${CMAKE_MODULE_PATH} "${CMAKE_SOURCE_DIR}/cmake/cmake_hexagon")
+set(CMAKE_MODULE_PATH ${CMAKE_MODULE_PATH} "${PX4_SOURCE_DIR}/cmake/cmake_hexagon")
 
 set(config_generate_parameters_scope ALL)
 
diff --git a/cmake/configs/qurt_eagle_legacy_driver_default.cmake b/cmake/configs/qurt_eagle_legacy_driver_default.cmake
index f0fdb2dee5..852fe55b1f 100644
--- a/cmake/configs/qurt_eagle_legacy_driver_default.cmake
+++ b/cmake/configs/qurt_eagle_legacy_driver_default.cmake
@@ -10,9 +10,9 @@ set(CONFIG_SHMEM "1")
 
 set(config_generate_parameters_scope ALL)
 
-set(CMAKE_TOOLCHAIN_FILE ${CMAKE_SOURCE_DIR}/cmake/cmake_hexagon/toolchain/Toolchain-qurt.cmake)
+set(CMAKE_TOOLCHAIN_FILE ${PX4_SOURCE_DIR}/cmake/cmake_hexagon/toolchain/Toolchain-qurt.cmake)
 
-set(CMAKE_MODULE_PATH ${CMAKE_MODULE_PATH} "${CMAKE_SOURCE_DIR}/cmake/cmake_hexagon")
+set(CMAKE_MODULE_PATH ${CMAKE_MODULE_PATH} "${PX4_SOURCE_DIR}/cmake/cmake_hexagon")
 
 add_definitions(
    -D__USING_SNAPDRAGON_LEGACY_DRIVER
diff --git a/cmake/configs/qurt_eagle_muorb.cmake b/cmake/configs/qurt_eagle_muorb.cmake
index 505ffd00cd..2af8ebdfb2 100644
--- a/cmake/configs/qurt_eagle_muorb.cmake
+++ b/cmake/configs/qurt_eagle_muorb.cmake
@@ -6,9 +6,9 @@ else()
 	set(HEXAGON_SDK_ROOT $ENV{HEXAGON_SDK_ROOT})
 endif()
 
-set(CMAKE_TOOLCHAIN_FILE ${CMAKE_SOURCE_DIR}/cmake/cmake_hexagon/toolchain/Toolchain-qurt.cmake)
+set(CMAKE_TOOLCHAIN_FILE ${PX4_SOURCE_DIR}/cmake/cmake_hexagon/toolchain/Toolchain-qurt.cmake)
 
-set(CMAKE_MODULE_PATH ${CMAKE_MODULE_PATH} "${CMAKE_SOURCE_DIR}/cmake/cmake_hexagon")
+set(CMAKE_MODULE_PATH ${CMAKE_MODULE_PATH} "${PX4_SOURCE_DIR}/cmake/cmake_hexagon")
 
 set(config_generate_parameters_scope ALL)
 
diff --git a/cmake/configs/qurt_eagle_test.cmake b/cmake/configs/qurt_eagle_test.cmake
index 9170b3da47..9131478577 100644
--- a/cmake/configs/qurt_eagle_test.cmake
+++ b/cmake/configs/qurt_eagle_test.cmake
@@ -6,8 +6,8 @@ else()
 	set(HEXAGON_SDK_ROOT $ENV{HEXAGON_SDK_ROOT})
 endif()
 
-set(CMAKE_TOOLCHAIN_FILE ${CMAKE_SOURCE_DIR}/cmake/cmake_hexagon/toolchain/Toolchain-qurt.cmake)
-set(CMAKE_MODULE_PATH ${CMAKE_MODULE_PATH} "${CMAKE_SOURCE_DIR}/cmake/cmake_hexagon")
+set(CMAKE_TOOLCHAIN_FILE ${PX4_SOURCE_DIR}/cmake/cmake_hexagon/toolchain/Toolchain-qurt.cmake)
+set(CMAKE_MODULE_PATH ${CMAKE_MODULE_PATH} "${PX4_SOURCE_DIR}/cmake/cmake_hexagon")
 
 set(config_generate_parameters_scope ALL)
 
diff --git a/cmake/configs/qurt_eagle_travis.cmake b/cmake/configs/qurt_eagle_travis.cmake
index 16aa788edc..505635b8bd 100644
--- a/cmake/configs/qurt_eagle_travis.cmake
+++ b/cmake/configs/qurt_eagle_travis.cmake
@@ -5,9 +5,9 @@ set(CONFIG_SHMEM "1")
 # Run a full link with build stubs to make sure qurt target isn't broken
 set(QURT_ENABLE_STUBS "1")
 
-set(CMAKE_TOOLCHAIN_FILE ${CMAKE_SOURCE_DIR}/cmake/cmake_hexagon/toolchain/Toolchain-qurt.cmake)
+set(CMAKE_TOOLCHAIN_FILE ${PX4_SOURCE_DIR}/cmake/cmake_hexagon/toolchain/Toolchain-qurt.cmake)
 
-set(CMAKE_MODULE_PATH ${CMAKE_MODULE_PATH} "${CMAKE_SOURCE_DIR}/cmake/cmake_hexagon")
+set(CMAKE_MODULE_PATH ${CMAKE_MODULE_PATH} "${PX4_SOURCE_DIR}/cmake/cmake_hexagon")
 include(hexagon_sdk)
 
 if ("$ENV{HEXAGON_SDK_ROOT}" STREQUAL "")
diff --git a/cmake/configs/qurt_sdflight_default.cmake b/cmake/configs/qurt_sdflight_default.cmake
index 195219f7af..1d30699014 100644
--- a/cmake/configs/qurt_sdflight_default.cmake
+++ b/cmake/configs/qurt_sdflight_default.cmake
@@ -10,9 +10,9 @@ set(CONFIG_SHMEM "1")
 
 set(config_generate_parameters_scope ALL)
 
-set(CMAKE_TOOLCHAIN_FILE ${CMAKE_SOURCE_DIR}/cmake/cmake_hexagon/toolchain/Toolchain-qurt.cmake)
+set(CMAKE_TOOLCHAIN_FILE ${PX4_SOURCE_DIR}/cmake/cmake_hexagon/toolchain/Toolchain-qurt.cmake)
 
-set(CMAKE_MODULE_PATH ${CMAKE_MODULE_PATH} "${CMAKE_SOURCE_DIR}/cmake/cmake_hexagon")
+set(CMAKE_MODULE_PATH ${CMAKE_MODULE_PATH} "${PX4_SOURCE_DIR}/cmake/cmake_hexagon")
 
 set(config_module_list
 	#
diff --git a/cmake/nuttx/px4_impl_nuttx.cmake b/cmake/nuttx/px4_impl_nuttx.cmake
index c969263be1..9a54b6277e 100644
--- a/cmake/nuttx/px4_impl_nuttx.cmake
+++ b/cmake/nuttx/px4_impl_nuttx.cmake
@@ -95,9 +95,9 @@ function(px4_nuttx_add_firmware)
 
 	add_custom_command(OUTPUT ${OUT}
 		COMMAND ${OBJCOPY} -O binary ${EXE} ${EXE}.bin
-		COMMAND ${PYTHON_EXECUTABLE} ${CMAKE_SOURCE_DIR}/Tools/px_mkfw.py
-			--prototype ${CMAKE_SOURCE_DIR}/Images/${BOARD}.prototype
-			--git_identity ${CMAKE_SOURCE_DIR}
+		COMMAND ${PYTHON_EXECUTABLE} ${PX4_SOURCE_DIR}/Tools/px_mkfw.py
+			--prototype ${PX4_SOURCE_DIR}/Images/${BOARD}.prototype
+			--git_identity ${PX4_SOURCE_DIR}
 			${extra_args}
 			--image ${EXE}.bin > ${OUT}
 		DEPENDS ${EXE}
@@ -148,7 +148,7 @@ function(px4_nuttx_generate_builtin_commands)
 			math(EXPR command_count "${command_count}+1")
 		endif()
 	endforeach()
-	configure_file(${CMAKE_SOURCE_DIR}/cmake/nuttx/builtin_commands.c.in
+	configure_file(${PX4_SOURCE_DIR}/cmake/nuttx/builtin_commands.c.in
 		${OUT})
 endfunction()
 
@@ -183,17 +183,17 @@ function(px4_nuttx_add_export)
 		REQUIRED OUT CONFIG THREADS
 		ARGN ${ARGN})
 
-	set(nuttx_src ${CMAKE_BINARY_DIR}/${CONFIG}/NuttX)
+	set(nuttx_src ${PX4_BINARY_DIR}/${CONFIG}/NuttX)
 
 	# patch
 	add_custom_target(__nuttx_patch_${CONFIG})
-	file(GLOB nuttx_patches RELATIVE ${CMAKE_SOURCE_DIR}
-	    ${CMAKE_SOURCE_DIR}/nuttx-patches/*.patch)
+	file(GLOB nuttx_patches RELATIVE ${PX4_SOURCE_DIR}
+	    ${PX4_SOURCE_DIR}/nuttx-patches/*.patch)
 	foreach(patch ${nuttx_patches})
 		string(REPLACE "/" "_" patch_name "${patch}-${CONFIG}")
 	    message(STATUS "nuttx-patch: ${patch}")
 		add_custom_command(OUTPUT nuttx_patch_${patch_name}.stamp
-			COMMAND ${PATCH} -p0 -N  < ${CMAKE_SOURCE_DIR}/${patch}
+			COMMAND ${PATCH} -p0 -N  < ${PX4_SOURCE_DIR}/${patch}
 			COMMAND ${TOUCH} nuttx_patch_${patch_name}.stamp
 			DEPENDS ${DEPENDS}
 			)
@@ -204,7 +204,7 @@ function(px4_nuttx_add_export)
 
 	# Read defconfig to see if CONFIG_ARMV7M_STACKCHECK is yes 
 	# note: CONFIG will be BOARD in the future evaluation of ${hw_stack_check_${CONFIG}
-	file(STRINGS "${CMAKE_SOURCE_DIR}/nuttx-configs/${CONFIG}/nsh/defconfig"
+	file(STRINGS "${PX4_SOURCE_DIR}/nuttx-configs/${CONFIG}/nsh/defconfig"
 		hw_stack_check_${CONFIG}
 		REGEX "CONFIG_ARMV7M_STACKCHECK=y"
 		)
@@ -213,29 +213,29 @@ function(px4_nuttx_add_export)
 	endif()
 
 	# copy and export
-	file(RELATIVE_PATH nuttx_cp_src ${CMAKE_BINARY_DIR} ${CMAKE_SOURCE_DIR}/NuttX)
-	file(GLOB_RECURSE config_files ${CMAKE_SOURCE_DIR}/nuttx-configs/${CONFIG}/*)
-	add_custom_command(OUTPUT ${CMAKE_BINARY_DIR}/${CONFIG}.export
+	file(RELATIVE_PATH nuttx_cp_src ${PX4_BINARY_DIR} ${PX4_SOURCE_DIR}/NuttX)
+	file(GLOB_RECURSE config_files ${PX4_SOURCE_DIR}/nuttx-configs/${CONFIG}/*)
+	add_custom_command(OUTPUT ${PX4_BINARY_DIR}/${CONFIG}.export
 		COMMAND ${MKDIR} -p ${nuttx_src}
 		COMMAND rsync -a --delete --exclude=.git ${nuttx_cp_src}/ ${CONFIG}/NuttX/
 		#COMMAND ${ECHO} Configuring NuttX for ${CONFIG}
 		COMMAND ${MAKE} --no-print-directory -C${nuttx_src}/nuttx -r --quiet distclean
-		COMMAND ${CP} -r ${CMAKE_SOURCE_DIR}/nuttx-configs/PX4_Warnings.mk ${nuttx_src}/nuttx/
-		COMMAND ${CP} -r ${CMAKE_SOURCE_DIR}/nuttx-configs/${CONFIG} ${nuttx_src}/nuttx/configs
+		COMMAND ${CP} -r ${PX4_SOURCE_DIR}/nuttx-configs/PX4_Warnings.mk ${nuttx_src}/nuttx/
+		COMMAND ${CP} -r ${PX4_SOURCE_DIR}/nuttx-configs/${CONFIG} ${nuttx_src}/nuttx/configs
 		COMMAND cd ${nuttx_src}/nuttx/tools && ./configure.sh ${CONFIG}/nsh && cd ..
 		#COMMAND ${ECHO} Exporting NuttX for ${CONFIG}
 		COMMAND ${MAKE} --no-print-directory --quiet -C ${nuttx_src}/nuttx -j${THREADS} -r CONFIG_ARCH_BOARD=${CONFIG} export > nuttx_build.log
-		COMMAND ${CP} -r ${nuttx_src}/nuttx/nuttx-export.zip ${CMAKE_BINARY_DIR}/${CONFIG}.export
+		COMMAND ${CP} -r ${nuttx_src}/nuttx/nuttx-export.zip ${PX4_BINARY_DIR}/${CONFIG}.export
 		DEPENDS ${config_files} ${DEPENDS}
-		WORKING_DIRECTORY ${CMAKE_BINARY_DIR}
+		WORKING_DIRECTORY ${PX4_BINARY_DIR}
 		COMMENT "Building NuttX for ${CONFIG}")
 
 	# extract
 	add_custom_command(OUTPUT nuttx_export_${CONFIG}.stamp
 		COMMAND ${RM} -rf ${nuttx_src}/nuttx-export
-		COMMAND ${UNZIP} -q ${CMAKE_BINARY_DIR}/${CONFIG}.export -d ${nuttx_src}
+		COMMAND ${UNZIP} -q ${PX4_BINARY_DIR}/${CONFIG}.export -d ${nuttx_src}
 		COMMAND ${TOUCH} nuttx_export_${CONFIG}.stamp
-		DEPENDS ${DEPENDS} ${CMAKE_BINARY_DIR}/${CONFIG}.export)
+		DEPENDS ${DEPENDS} ${PX4_BINARY_DIR}/${CONFIG}.export)
 
 	add_custom_target(${OUT}
 		DEPENDS nuttx_export_${CONFIG}.stamp)
@@ -308,16 +308,16 @@ function(px4_nuttx_add_romfs)
 		REQUIRED OUT ROOT
 		ARGN ${ARGN})
 
-	set(romfs_temp_dir ${CMAKE_BINARY_DIR}/tmp/${ROOT})
-	set(romfs_src_dir ${CMAKE_SOURCE_DIR}/${ROOT})
-	set(romfs_autostart ${CMAKE_SOURCE_DIR}/Tools/px_process_airframes.py)
-	set(romfs_pruner ${CMAKE_SOURCE_DIR}/Tools/px_romfs_pruner.py)
-	set(bin_to_obj ${CMAKE_SOURCE_DIR}/cmake/nuttx/bin_to_obj.py)
+	set(romfs_temp_dir ${PX4_BINARY_DIR}/tmp/${ROOT})
+	set(romfs_src_dir ${PX4_SOURCE_DIR}/${ROOT})
+	set(romfs_autostart ${PX4_SOURCE_DIR}/Tools/px_process_airframes.py)
+	set(romfs_pruner ${PX4_SOURCE_DIR}/Tools/px_romfs_pruner.py)
+	set(bin_to_obj ${PX4_SOURCE_DIR}/cmake/nuttx/bin_to_obj.py)
 	set(extras_dir ${CMAKE_CURRENT_BINARY_DIR}/extras)
 
 	file(GLOB_RECURSE romfs_src_files ${romfs_src_dir} ${romfs_src_dir}/*)
 
-	set(cmake_test ${CMAKE_SOURCE_DIR}/cmake/test/cmake_tester.py)
+	set(cmake_test ${PX4_SOURCE_DIR}/cmake/test/cmake_tester.py)
 
 	
 	set(extras)
@@ -346,7 +346,7 @@ function(px4_nuttx_add_romfs)
 		#COMMAND cmake -E remove_directory ${romfs_temp_dir}
 		COMMAND ${PYTHON_EXECUTABLE} ${bin_to_obj}
 			--ld ${LD} --c_flags ${CMAKE_C_FLAGS}
-			--include_path "${CMAKE_SOURCE_DIR}/src/include"
+			--include_path "${PX4_SOURCE_DIR}/src/include"
 			--c_compiler ${CMAKE_C_COMPILER}
 			--nm ${NM} --objcopy ${OBJCOPY}
 			--obj romfs.o
@@ -414,7 +414,7 @@ function(px4_os_add_flags)
 		LINK_DIRS ${LINK_DIRS}
 		DEFINITIONS ${DEFINITIONS})
 
-	set(nuttx_export_dir ${CMAKE_BINARY_DIR}/${BOARD}/NuttX/nuttx-export)
+	set(nuttx_export_dir ${PX4_BINARY_DIR}/${BOARD}/NuttX/nuttx-export)
 	set(added_include_dirs
 		${nuttx_export_dir}/include
 		${nuttx_export_dir}/include/cxx
diff --git a/cmake/posix/px4_impl_posix.cmake b/cmake/posix/px4_impl_posix.cmake
index d661bfa5eb..7786097c4c 100644
--- a/cmake/posix/px4_impl_posix.cmake
+++ b/cmake/posix/px4_impl_posix.cmake
@@ -49,7 +49,7 @@
 #
 
 include(common/px4_base)
-list(APPEND CMAKE_MODULE_PATH ${CMAKE_SOURCE_DIR}/cmake/posix)
+list(APPEND CMAKE_MODULE_PATH ${PX4_SOURCE_DIR}/cmake/posix)
 
 #=============================================================================
 #
@@ -101,7 +101,7 @@ function(px4_posix_generate_builtin_commands)
 			math(EXPR command_count "${command_count}+1")
 		endif()
 	endforeach()
-	configure_file(${CMAKE_SOURCE_DIR}/cmake/posix/apps.h_in
+	configure_file(${PX4_SOURCE_DIR}/cmake/posix/apps.h_in
 		${OUT})
 endfunction()
 
diff --git a/cmake/qurt/px4_impl_qurt.cmake b/cmake/qurt/px4_impl_qurt.cmake
index efb1135d7b..f5387c515e 100644
--- a/cmake/qurt/px4_impl_qurt.cmake
+++ b/cmake/qurt/px4_impl_qurt.cmake
@@ -50,7 +50,7 @@
 #
 
 include(common/px4_base)
-list(APPEND CMAKE_MODULE_PATH ${CMAKE_SOURCE_DIR}/cmake/qurt)
+list(APPEND CMAKE_MODULE_PATH ${PX4_SOURCE_DIR}/cmake/qurt)
 
 #=============================================================================
 #
@@ -95,7 +95,7 @@ function(px4_qurt_generate_builtin_commands)
 			math(EXPR command_count "${command_count}+1")
 		endif()
 	endforeach()
-	configure_file(${CMAKE_SOURCE_DIR}/cmake/qurt/apps.h_in ${OUT})
+	configure_file(${PX4_SOURCE_DIR}/cmake/qurt/apps.h_in ${OUT})
 endfunction()
 
 #=============================================================================
diff --git a/cmake/ros-CMakeLists.txt b/cmake/ros-CMakeLists.txt
deleted file mode 100644
index 67870b1e2c..0000000000
--- a/cmake/ros-CMakeLists.txt
+++ /dev/null
@@ -1,334 +0,0 @@
-cmake_minimum_required(VERSION 2.8.3)
-project(px4)
-set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11")
-add_definitions(-D__PX4_ROS)
-add_definitions(-D__EXPORT=)
-add_definitions(-DMAVLINK_DIALECT=common)
-
-## Find catkin macros and libraries
-## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
-## is used, also find other catkin packages
-find_package(catkin REQUIRED COMPONENTS
-  roscpp
-  rospy
-  std_msgs
-  geometry_msgs
-  message_generation
-  cmake_modules
-  gazebo_msgs
-  sensor_msgs
-  mav_msgs
-  libmavconn
-  tf
-)
-find_package(Eigen REQUIRED)
-
-## System dependencies are found with CMake's conventions
-# find_package(Boost REQUIRED COMPONENTS system)
-
-
-## Uncomment this if the package has a setup.py. This macro ensures
-## modules and global scripts declared therein get installed
-## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html
-# catkin_python_setup()
-
-################################################
-## Declare ROS messages, services and actions ##
-################################################
-
-## To declare and build messages, services or actions from within this
-## package, follow these steps:
-## * Let MSG_DEP_SET be the set of packages whose message types you use in
-##   your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...).
-## * In the file package.xml:
-##   * add a build_depend and a run_depend tag for each package in MSG_DEP_SET
-##   * If MSG_DEP_SET isn't empty the following dependencies might have been
-##     pulled in transitively but can be declared for certainty nonetheless:
-##     * add a build_depend tag for "message_generation"
-##     * add a run_depend tag for "message_runtime"
-## * In this file (CMakeLists.txt):
-##   * add "message_generation" and every package in MSG_DEP_SET to
-##     find_package(catkin REQUIRED COMPONENTS ...)
-##   * add "message_runtime" and every package in MSG_DEP_SET to
-##     catkin_package(CATKIN_DEPENDS ...)
-##   * uncomment the add_*_files sections below as needed
-##     and list every .msg/.srv/.action file to be processed
-##   * uncomment the generate_messages entry below
-##   * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...)
-
-## Generate messages in the 'msg' folder
-add_message_files(
-  FILES
-  actuator_armed.msg
-  actuator_controls.msg
-  commander_state.msg
-  control_state.msg
-  distance_sensor.msg
-  manual_control_setpoint.msg
-  mc_virtual_rates_setpoint.msg
-  offboard_control_mode.msg
-  parameter_update.msg
-  position_setpoint.msg
-  position_setpoint_triplet.msg
-  rc_channels.msg
-  ros/actuator_controls_0.msg
-  ros/actuator_controls_virtual_mc.msg
-  vehicle_attitude.msg
-  vehicle_attitude_setpoint.msg
-  vehicle_control_mode.msg
-  vehicle_force_setpoint.msg
-  vehicle_global_velocity_setpoint.msg
-  vehicle_local_position.msg
-  vehicle_local_position_setpoint.msg
-  vehicle_rates_setpoint.msg
-  vehicle_status.msg
-)
-
-## Generate services in the 'srv' folder
-# add_service_files(
-#   FILES
-#   Service1.srv
-#   Service2.srv
-# )
-
-## Generate actions in the 'action' folder
-# add_action_files(
-#   FILES
-#   Action1.action
-#   Action2.action
-# )
-
-## Generate added messages and services with any dependencies listed here
-generate_messages(
-  DEPENDENCIES
-  std_msgs
-  gazebo_msgs
-)
-
-###################################
-## catkin specific configuration ##
-###################################
-## The catkin_package macro generates cmake config files for your package
-## Declare things to be passed to dependent projects
-## INCLUDE_DIRS: uncomment this if you package contains header files
-## LIBRARIES: libraries you create in this project that dependent projects also need
-## CATKIN_DEPENDS: catkin_packages dependent projects also need
-## DEPENDS: system dependencies of this project that dependent projects also need
-catkin_package(
-  INCLUDE_DIRS src/include
-  LIBRARIES px4
-  CATKIN_DEPENDS message_runtime roscpp rospy std_msgs libmavconn
-  DEPENDS system_lib
-)
-
-###########
-## Build ##
-###########
-
-## Specify additional locations of header files
-## Your package locations should be listed before other locations
-include_directories(
-  ${catkin_INCLUDE_DIRS}
-  src/platforms
-  src/platforms/ros/px4_messages
-  src/include
-  src/modules
-  ${CMAKE_BINARY_DIR}/src/modules
-  src/
-  src/lib
-  src/lib/matrix
-  ${EIGEN_INCLUDE_DIRS}
-  integrationtests
-)
-
-## generate multiplatform wrapper headers
-## note that the message header files are generated as in any ROS project with generate_messages()
-set(MULTIPLATFORM_HEADER_DIR ${CMAKE_CURRENT_SOURCE_DIR}/src/platforms/ros/px4_messages)
-set(MULTIPLATFORM_TEMPLATE_DIR ${CMAKE_CURRENT_SOURCE_DIR}/msg/templates/px4/ros)
-set(TOPICHEADER_TEMP_DIR ${CMAKE_BINARY_DIR}/topics_temporary)
-set(MULTIPLATFORM_PREFIX px4_)
-add_custom_target(multiplatform_message_headers ALL ${PYTHON_EXECUTABLE} ${CMAKE_CURRENT_SOURCE_DIR}/Tools/px_generate_uorb_topic_files.py
-	--headers -d ${CMAKE_CURRENT_SOURCE_DIR}/msg -o ${MULTIPLATFORM_HEADER_DIR} -e ${MULTIPLATFORM_TEMPLATE_DIR}
-	-t ${TOPICHEADER_TEMP_DIR} -p ${MULTIPLATFORM_PREFIX})
-
-## Declare a cpp library
-add_library(px4
-  src/platforms/ros/px4_ros_impl.cpp
-  src/platforms/ros/perf_counter.cpp
-  src/platforms/ros/geo.cpp
-  src/lib/mathlib/math/Limits.cpp
-  src/modules/systemlib/circuit_breaker.cpp
-)
-add_dependencies(px4 ${PROJECT_NAME}_generate_messages_cpp multiplatform_message_headers)
-
-target_link_libraries(px4
-  ${catkin_LIBRARIES}
-)
-
-## Declare a test publisher
-add_executable(publisher
-	src/examples/publisher/publisher_main.cpp
-	src/examples/publisher/publisher_example.cpp)
-add_dependencies(publisher ${PROJECT_NAME}_generate_messages_cpp multiplatform_message_headers)
-target_link_libraries(publisher
-  ${catkin_LIBRARIES}
-  px4
-)
-
-## Declare a test subscriber
-add_executable(subscriber
-	src/examples/subscriber/subscriber_main.cpp
-	src/examples/subscriber/subscriber_example.cpp)
-add_dependencies(subscriber ${PROJECT_NAME}_generate_messages_cpp multiplatform_message_headers)
-target_link_libraries(subscriber
-  ${catkin_LIBRARIES}
-  px4
-)
-
-## MC Attitude Control
-add_executable(mc_att_control
-	src/modules/mc_att_control_multiplatform/mc_att_control_main.cpp
-	src/modules/mc_att_control_multiplatform/mc_att_control.cpp
-	src/modules/mc_att_control_multiplatform/mc_att_control_base.cpp)
-add_dependencies(mc_att_control ${PROJECT_NAME}_generate_messages_cpp_cpp)
-target_link_libraries(mc_att_control
-  ${catkin_LIBRARIES}
-  px4
-)
-
-## MC Position Control
-add_executable(mc_pos_control
-	src/modules/mc_pos_control_multiplatform/mc_pos_control_main.cpp
-	src/modules/mc_pos_control_multiplatform/mc_pos_control.cpp)
-add_dependencies(mc_pos_control ${PROJECT_NAME}_generate_messages_cpp_cpp)
-target_link_libraries(mc_pos_control
-  ${catkin_LIBRARIES}
-  px4
-)
-
-## Attitude Estimator dummy
-add_executable(attitude_estimator
-	src/platforms/ros/nodes/attitude_estimator/attitude_estimator.cpp)
-add_dependencies(attitude_estimator ${PROJECT_NAME}_generate_messages_cpp_cpp)
-target_link_libraries(attitude_estimator
-  ${catkin_LIBRARIES}
-  px4
-)
-
-## Position Estimator dummy
-add_executable(position_estimator
-	src/platforms/ros/nodes/position_estimator/position_estimator.cpp)
-add_dependencies(position_estimator ${PROJECT_NAME}_generate_messages_cpp_cpp)
-target_link_libraries(position_estimator
-  ${catkin_LIBRARIES}
-  px4
-)
-
-## Manual input
-add_executable(manual_input
-	src/platforms/ros/nodes/manual_input/manual_input.cpp)
-add_dependencies(manual_input ${PROJECT_NAME}_generate_messages_cpp_cpp)
-target_link_libraries(manual_input
-  ${catkin_LIBRARIES}
-  px4
-)
-
-## Multicopter Mixer dummy
-add_executable(mc_mixer
-  src/platforms/ros/nodes/mc_mixer/mc_mixer.cpp)
-add_dependencies(mc_mixer ${PROJECT_NAME}_generate_messages_cpp_cpp)
-target_link_libraries(mc_mixer
-  ${catkin_LIBRARIES}
-  px4
-)
-
-## Commander dummy
-add_executable(commander
-	src/platforms/ros/nodes/commander/commander.cpp)
-add_dependencies(commander ${PROJECT_NAME}_generate_messages_cpp_cpp)
-target_link_libraries(commander
-  ${catkin_LIBRARIES}
-  px4
-)
-
-## Mavlink dummy
-add_executable(mavlink
-	src/platforms/ros/nodes/mavlink/mavlink.cpp)
-add_dependencies(mavlink ${PROJECT_NAME}_generate_messages_cpp_cpp)
-target_link_libraries(mavlink
-  ${catkin_LIBRARIES}
-  px4
-)
-
-## Offboard Position Setpoint Demo
-add_executable(demo_offboard_position_setpoints
-	src/platforms/ros/nodes/demo_offboard_position_setpoints/demo_offboard_position_setpoints.cpp)
-add_dependencies(demo_offboard_position_setpoints ${PROJECT_NAME}_generate_messages_cpp_cpp)
-target_link_libraries(demo_offboard_position_setpoints
-  ${catkin_LIBRARIES}
-  px4
-)
-
-## Offboard Attitude Setpoint Demo
-add_executable(demo_offboard_attitude_setpoints
-	src/platforms/ros/nodes/demo_offboard_attitude_setpoints/demo_offboard_attitude_setpoints.cpp)
-add_dependencies(demo_offboard_attitude_setpoints ${PROJECT_NAME}_generate_messages_cpp_cpp)
-target_link_libraries(demo_offboard_attitude_setpoints
-  ${catkin_LIBRARIES}
-  px4
-)
-
-#############
-## Install ##
-#############
-
-# all install targets should use catkin DESTINATION variables
-# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html
-
-## Mark executable scripts (Python etc.) for installation
-## in contrast to setup.py, you can choose the destination
-# install(PROGRAMS
-#   scripts/my_python_script
-#   DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
-# )
-
-## Mark executables and/or libraries for installation
-install(TARGETS ${PROJECT_NAME}
-  ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
-  LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
-  RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
-)
-
-## Mark cpp header files for installation
-# install(DIRECTORY include/${PROJECT_NAME}/
-#   DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
-#   FILES_MATCHING PATTERN "*.h"
-#   PATTERN ".svn" EXCLUDE
-# )
-
-## Mark other files for installation (e.g. launch and bag files, etc.)
-# install(FILES
-#   # myfile1
-#   # myfile2
-#   DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
-# )
-
-#############
-## Testing ##
-#############
-
-## Add gtest based cpp test target and link libraries
-# catkin_add_gtest(${PROJECT_NAME}-test test/test_px4test.cpp)
-# if(TARGET ${PROJECT_NAME}-test)
-#   target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME})
-# endif()
-
-## Add folders to be run by python nosetests
-# catkin_add_nosetests(test)
-
-## Tests on multiplatform SITL (deprecated) have been moved to POSIX SITL
-#if(CATKIN_ENABLE_TESTING)
-#  find_package(rostest REQUIRED)
-#  add_rostest(integrationtests/demo_tests/direct_tests.launch)
-#  add_rostest(integrationtests/demo_tests/mavros_tests.launch)
-#endif()
diff --git a/integrationtests/run_tests.bash b/integrationtests/run_tests.bash
index 2a3a4c2a0d..61569a1a73 100755
--- a/integrationtests/run_tests.bash
+++ b/integrationtests/run_tests.bash
@@ -5,26 +5,27 @@
 # License: according to LICENSE.md in the root directory of the PX4 Firmware repository
 set -e
 
-if [ "$#" -lt 1 ]
+# handle cleaning command
+do_clean=true
+if [ "$1" = "-o" ]
 then
-	echo usage: run_tests.bash firmware_src_dir
-	echo ""
-	exit 1
+	echo not cleaning
+	do_clean=false
 fi
 
-SRC_DIR=$1
-JOB_DIR=$SRC_DIR/..
-BUILD=posix_sitl_default
-# TODO
-ROS_TEST_RESULT_DIR=/root/.ros/test_results/px4
-ROS_LOG_DIR=/root/.ros/log
-PX4_LOG_DIR=${SRC_DIR}/build_${BUILD}/src/firmware/posix/rootfs/fs/microsd/log
-TEST_RESULT_TARGET_DIR=$JOB_DIR/test_results
-# BAGS=/root/.ros
-# CHARTS=/root/.ros/charts
-# EXPORT_CHARTS=/sitl/testing/export_charts.py
+# determine the directory of the source given the directory of this script
+pushd `dirname $0` > /dev/null
+SCRIPTPATH=`pwd`
+popd > /dev/null
+ORIG_SRC=$(dirname $SCRIPTPATH)
 
-# source ROS env
+# set paths
+JOB_DIR=$(dirname $ORIG_SRC)
+CATKIN_DIR=$JOB_DIR/catkin
+BUILD_DIR=$CATKIN_DIR/build/px4
+SRC_DIR=${CATKIN_DIR}/src/px4
+
+echo setting up ROS paths
 if [ -f /opt/ros/indigo/setup.bash ]
 then
 	source /opt/ros/indigo/setup.bash
@@ -35,26 +36,57 @@ else
 	echo "could not find /opt/ros/{ros-distro}/setup.bash"
 	exit 1
 fi
-source $SRC_DIR/integrationtests/setup_gazebo_ros.bash $SRC_DIR
+export ROS_HOME=$JOB_DIR/.ros
+export ROS_LOG_DIR=$ROS_HOME/log
+export ROS_TEST_RESULT_DIR=$ROS_HOME/test_results/px4
 
-echo "deleting previous test results ($TEST_RESULT_TARGET_DIR)"
-if [ -d ${TEST_RESULT_TARGET_DIR} ]; then
-	rm -r ${TEST_RESULT_TARGET_DIR}
-fi
+PX4_LOG_DIR=$ROS_HOME/rootfs/fs/microsd/log
+TEST_RESULT_TARGET_DIR=$JOB_DIR/test_results
 
-# FIXME: Firmware compilation seems to CD into this directory (/root/Firmware)
-# when run from "run_container.bash". Why?
-if [ -d /root/Firmware ]; then
-	rm /root/Firmware
+# TODO
+# BAGS=$ROS_HOME
+# CHARTS=$ROS_HOME/charts
+# EXPORT_CHARTS=/sitl/testing/export_charts.py
+
+echo setting up gazebo paths
+source /usr/share/gazebo/setup.sh
+source $SCRIPTPATH/setup_gazebo_ros.bash ${SRC_DIR} ${BUILD_DIR}
+
+if $do_clean
+then
+	echo cleaning
+	rm -rf $CATKIN_DIR
+	rm -rf $ROS_HOME
+	rm -rf $TEST_RESULT_TARGET_DIR
+else
+	echo skipping clean step
 fi
-ln -s ${SRC_DIR} /root/Firmware
 
 echo "=====> compile ($SRC_DIR)"
-cd $SRC_DIR
-make ${BUILD}
-make --no-print-directory gazebo_build
+mkdir -p $ROS_HOME
+mkdir -p $CATKIN_DIR/src
+mkdir -p $TEST_RESULT_TARGET_DIR
+if ! [ -d $SRC_DIR ]
+then
+	ln -s $ORIG_SRC $SRC_DIR
+	ln -s $ORIG_SRC/Tools/sitl_gazebo ${CATKIN_DIR}/src/mavlink_sitl_gazebo
+fi
+cd $CATKIN_DIR
+catkin_make
+. ./devel/setup.bash
 echo "<====="
 
+# print paths to user
+echo -e "JOB_DIR\t\t: $JOB_DIR"
+echo -e "ROS_HOME\t: $ROS_HOME"
+echo -e "CATKIN_DIR\t: $CATKIN_DIR"
+echo -e "BUILD_DIR\t: $BUILD_DIR"
+echo -e "SRC_DIR\t\t: $SRC_DIR"
+echo -e "ROS_TEST_RESULT_DIR\t: $ROS_TEST_RESULT_DIR"
+echo -e "ROS_LOG_DIR\t\t: $ROS_LOG_DIR"
+echo -e "PX4_LOG_DIR\t\t: $PX4_LOG_DIR"
+echo -e "TEST_RESULT_TARGET_DIR\t: $TEST_RESULT_TARGET_DIR"
+
 # don't exit on error anymore from here on (because single tests or exports might fail)
 set +e
 echo "=====> run tests"
@@ -73,7 +105,6 @@ echo "=====> process test results"
 # done
 
 echo "copy build test results to job directory"
-mkdir -p ${TEST_RESULT_TARGET_DIR}
 cp -r $ROS_TEST_RESULT_DIR/* ${TEST_RESULT_TARGET_DIR}
 cp -r $ROS_LOG_DIR/* ${TEST_RESULT_TARGET_DIR}
 cp -r $PX4_LOG_DIR/* ${TEST_RESULT_TARGET_DIR}
diff --git a/integrationtests/setup_gazebo_ros.bash b/integrationtests/setup_gazebo_ros.bash
index 6c22bbe192..8cc4a5c58b 100755
--- a/integrationtests/setup_gazebo_ros.bash
+++ b/integrationtests/setup_gazebo_ros.bash
@@ -4,19 +4,24 @@
 #
 # License: according to LICENSE.md in the root directory of the PX4 Firmware repository
 
-if [ "$#" -lt 1 ]
+if [ "$#" != 2 ]
 then
-    echo usage: source setup_gazebo_ros.bash firmware_src_dir
+    echo usage: source setup_gazebo_ros.bash src_dir build_dir
     echo ""
     return 1
 fi
 
 SRC_DIR=$1
+BUILD_DIR=$2
 
 # setup Gazebo env and update package path
-export GAZEBO_MODEL_PATH=${GAZEBO_MODEL_PATH}:${SRC_DIR}/Tools/sitl_gazebo/models
-export GAZEBO_PLUGIN_PATH=${SRC_DIR}/build_gazebo:${GAZEBO_PLUGIN_PATH}
-export LD_LIBRARY_PATH=${LD_LIBRARY_PATH}:${SRC_DIR}/Tools/sitl_gazebo/Build/msgs/
-export ROS_PACKAGE_PATH=${ROS_PACKAGE_PATH}:${SRC_DIR}
+export GAZEBO_RESOURCE_PATH=${SRC_DIR}/Tools/sitl_gazebo:${GAZEBO_RESOURCE_PATH}
+export GAZEBO_MODEL_PATH=${SRC_DIR}/Tools/sitl_gazebo/models:${GAZEBO_MODEL_PATH}
+export GAZEBO_PLUGIN_PATH=${BUILD_DIR}/build_gazebo:${GAZEBO_PLUGIN_PATH}
+export LD_LIBRARY_PATH=${SRC_DIR}/Tools/sitl_gazebo/Build/msgs/:${BUILD_DIR}/build_gazebo:${LD_LIBRARY_PATH}:
 export GAZEBO_MODEL_DATABASE_URI=""
-export SITL_GAZEBO_PATH=$SRC_DIR/Tools/sitl_gazebo
+
+echo -e "GAZEBO_RESOURCE_PATH $GAZEBO_RESOURCE_PATH"
+echo -e "GAZEBO_MODEL_PATH $GAZEBO_MODEL_PATH"
+echo -e "GAZEBO_PLUGIN_PATH $GAZEBO_PLUGIN_PATH"
+echo -e "LD_LIBRARY_PATH $LD_LIBRARY_PATH"
diff --git a/launch/mavros_posix_sitl.launch b/launch/mavros_posix_sitl.launch
index 558f3bbadb..29793e10f8 100644
--- a/launch/mavros_posix_sitl.launch
+++ b/launch/mavros_posix_sitl.launch
@@ -1,18 +1,25 @@
 <launch>
+
     <!-- MAVROS posix SITL environment launch script -->
 
+    <arg name="vehicle" default="iris"/>
+    <arg name="world" default="$(find mavlink_sitl_gazebo)/worlds/empty.world"/>
+    <arg name="sdf" default="$(find mavlink_sitl_gazebo)/models/$(arg vehicle)/$(arg vehicle).sdf"/>
+    <arg name="rcS" default="$(find px4)/posix-configs/SITL/init/rcS_gazebo_$(arg vehicle)"/>
+
     <arg name="headless" default="false"/>
     <arg name="gui" default="true"/>
     <arg name="ns" default="/"/>
-    <arg name="world" default="iris"/>
     <arg name="build" default="posix_sitl_default"/>
     <arg name="fcu_url" default="udp://:14540@localhost:14557"/>
 
     <include file="$(find px4)/launch/posix_sitl.launch">
+        <arg name="world" value="$(arg world)"/>
+        <arg name="vehicle" value="$(arg vehicle)"/>
+        <arg name="rcS" value="$(arg rcS)"/>
         <arg name="headless" value="$(arg headless)"/>
         <arg name="gui" value="$(arg gui)"/>
         <arg name="ns" value="$(arg ns)"/>
-        <arg name="world" value="$(arg world)"/>
         <arg name="build" value="$(arg build)"/>
     </include>
 
@@ -22,3 +29,5 @@
         <arg name="fcu_url" value="$(arg fcu_url)"/>
     </include>
 </launch>
+
+<!-- vim: set et ft=xml fenc=utf-8 ff=unix sts=0 sw=4 ts=4 : -->
diff --git a/launch/mavros_posix_tests_iris.launch b/launch/mavros_posix_tests_iris.launch
index 235acda6f2..8112f6f1f6 100644
--- a/launch/mavros_posix_tests_iris.launch
+++ b/launch/mavros_posix_tests_iris.launch
@@ -9,6 +9,7 @@
         <arg name="ns" value="$(arg ns)"/>
         <arg name="headless" value="$(arg headless)"/>
         <arg name="gui" value="$(arg gui)"/>
+        <arg name="vehicle" value="iris"/>
     </include>
 
     <group ns="$(arg ns)">
diff --git a/launch/mavros_posix_tests_standard_vtol.launch b/launch/mavros_posix_tests_standard_vtol.launch
index d5aa9e285e..b20f4801c4 100644
--- a/launch/mavros_posix_tests_standard_vtol.launch
+++ b/launch/mavros_posix_tests_standard_vtol.launch
@@ -9,7 +9,7 @@
         <arg name="ns" value="$(arg ns)"/>
         <arg name="headless" value="$(arg headless)"/>
         <arg name="gui" value="$(arg gui)"/>
-        <arg name="world" value="standard_vtol"/>
+        <arg name="vehicle" value="standard_vtol"/>
     </include>
 
     <group ns="$(arg ns)">
diff --git a/launch/posix_sitl.launch b/launch/posix_sitl.launch
index 9895ee9d43..6c2d2da649 100644
--- a/launch/posix_sitl.launch
+++ b/launch/posix_sitl.launch
@@ -1,19 +1,31 @@
 <launch>
+
     <!-- Posix SITL environment launch script -->
 
+    <arg name="vehicle" default="iris"/>
+    <arg name="world" default="$(find mavlink_sitl_gazebo)/worlds/empty.world"/>
+    <arg name="sdf" default="$(find mavlink_sitl_gazebo)/models/$(arg vehicle)/$(arg vehicle).sdf"/>
+    <arg name="rcS" default="$(find px4)/posix-configs/SITL/init/rcS_gazebo_$(arg vehicle)"/>
+
     <arg name="headless" default="false"/>
     <arg name="gui" default="true"/>
     <arg name="ns" default="/"/>
-    <arg name="world" default="iris"/>
     <arg name="build" default="posix_sitl_default"/>
 
-    <node pkg="px4" type="sitl_run.sh" name="simulator" args="posix-configs/SITL/init/rcS none gazebo $(arg world) $(find px4)/build_$(arg build)">
-        <env name="no_sim" value="1" />
+    <node name="sitl" pkg="px4" type="px4" output="screen"
+        args="$(find px4) $(arg rcS)">
     </node>
 
     <include file="$(find gazebo_ros)/launch/empty_world.launch">
         <arg name="headless" value="$(arg headless)"/>
         <arg name="gui" value="$(arg gui)"/>
-        <arg name="world_name" value="$(find px4)/Tools/sitl_gazebo/worlds/$(arg world).world" />
+        <arg name="world_name" value="$(arg world)" />
     </include>
+
+    <node name="$(anon vehicle_spawn)" output="screen" pkg="gazebo_ros" type="spawn_model"
+        args="-sdf -file $(arg sdf) -model $(arg vehicle)"/>
+
+
 </launch>
+
+<!-- vim: set et ft=xml fenc=utf-8 ff=unix sts=0 sw=4 ts=4 : -->
diff --git a/posix-configs/SITL/README.md b/posix-configs/SITL/README.md
index 2588c4b09b..a98a6515fa 100644
--- a/posix-configs/SITL/README.md
+++ b/posix-configs/SITL/README.md
@@ -105,5 +105,5 @@ param set RC_MAP_POSCTL_SW 7
 param set RC_MAP_RETURN_SW 8
 param set MC_PITCHRATE_P 0.05
 param set MC_ROLLRATE_P 0.05
-mixer load /dev/pwm_output0 ../../ROMFS/px4fmu_common/mixers/quad_x.main.mix
+mixer load /dev/pwm_output0 ROMFS/px4fmu_common/mixers/quad_x.main.mix
 ```
diff --git a/posix-configs/SITL/init/rcS_gazebo_iris b/posix-configs/SITL/init/rcS_gazebo_iris
index d985929601..6b93b86c2e 100644
--- a/posix-configs/SITL/init/rcS_gazebo_iris
+++ b/posix-configs/SITL/init/rcS_gazebo_iris
@@ -60,7 +60,7 @@ navigator start
 ekf2 start
 mc_pos_control start
 mc_att_control start
-mixer load /dev/pwm_output0 ../../../../ROMFS/px4fmu_common/mixers/quad_w.main.mix
+mixer load /dev/pwm_output0 ROMFS/px4fmu_common/mixers/quad_w.main.mix
 mavlink start -u 14556 -r 4000000
 mavlink start -u 14557 -r 4000000 -m onboard -o 14540
 mavlink stream -r 50 -s POSITION_TARGET_LOCAL_NED -u 14556
diff --git a/posix-configs/SITL/init/rcS_gazebo_iris_opt_flow b/posix-configs/SITL/init/rcS_gazebo_iris_opt_flow
index 2be4900b81..44060e1b52 100644
--- a/posix-configs/SITL/init/rcS_gazebo_iris_opt_flow
+++ b/posix-configs/SITL/init/rcS_gazebo_iris_opt_flow
@@ -59,7 +59,7 @@ attitude_estimator_q start
 position_estimator_inav start
 mc_pos_control start
 mc_att_control start
-mixer load /dev/pwm_output0 ../../../../ROMFS/px4fmu_common/mixers/quad_w.main.mix
+mixer load /dev/pwm_output0 ROMFS/px4fmu_common/mixers/quad_w.main.mix
 mavlink start -u 14556 -r 2000000
 mavlink start -u 14557 -r 2000000 -m onboard -o 14540
 mavlink stream -r 80 -s POSITION_TARGET_LOCAL_NED -u 14556
diff --git a/posix-configs/SITL/init/rcS_gazebo_plane b/posix-configs/SITL/init/rcS_gazebo_plane
index b4f40ce2dd..bc837d2acd 100644
--- a/posix-configs/SITL/init/rcS_gazebo_plane
+++ b/posix-configs/SITL/init/rcS_gazebo_plane
@@ -47,7 +47,7 @@ ekf2 start
 fw_pos_control_l1 start
 fw_att_control start
 land_detector start fixedwing
-mixer load /dev/pwm_output0 ../../../../ROMFS/sitl/mixers/plane_sitl.main.mix
+mixer load /dev/pwm_output0 ROMFS/sitl/mixers/plane_sitl.main.mix
 mavlink start -u 14556 -r 2000000
 mavlink start -u 14557 -r 2000000 -m onboard -o 14540
 mavlink stream -r 80 -s POSITION_TARGET_LOCAL_NED -u 14556
diff --git a/posix-configs/SITL/init/rcS_gazebo_solo b/posix-configs/SITL/init/rcS_gazebo_solo
index 008261b6ec..8701609064 100644
--- a/posix-configs/SITL/init/rcS_gazebo_solo
+++ b/posix-configs/SITL/init/rcS_gazebo_solo
@@ -57,7 +57,7 @@ navigator start
 ekf2 start
 mc_pos_control start
 mc_att_control start
-mixer load /dev/pwm_output0 ../../../../ROMFS/px4fmu_common/mixers/quad_x.main.mix
+mixer load /dev/pwm_output0 ROMFS/px4fmu_common/mixers/quad_x.main.mix
 mavlink start -u 14556 -r 4000000
 mavlink start -u 14557 -r 4000000 -m onboard -o 14540
 mavlink stream -r 50 -s POSITION_TARGET_LOCAL_NED -u 14556
diff --git a/posix-configs/SITL/init/rcS_gazebo_standard_vtol b/posix-configs/SITL/init/rcS_gazebo_standard_vtol
index a1f448bb58..d90ddfa891 100644
--- a/posix-configs/SITL/init/rcS_gazebo_standard_vtol
+++ b/posix-configs/SITL/init/rcS_gazebo_standard_vtol
@@ -69,7 +69,7 @@ mc_pos_control start
 mc_att_control start
 fw_pos_control_l1 start
 fw_att_control start
-mixer load /dev/pwm_output0 ../../../../ROMFS/sitl/mixers/standard_vtol_sitl.main.mix
+mixer load /dev/pwm_output0 ROMFS/sitl/mixers/standard_vtol_sitl.main.mix
 mavlink start -u 14556 -r 2000000
 mavlink start -u 14557 -r 2000000 -m onboard -o 14540
 mavlink stream -r 80 -s POSITION_TARGET_LOCAL_NED -u 14556
diff --git a/posix-configs/SITL/init/rcS_gazebo_tailsitter b/posix-configs/SITL/init/rcS_gazebo_tailsitter
index 2ba72c5b8c..04d3a2b2f2 100644
--- a/posix-configs/SITL/init/rcS_gazebo_tailsitter
+++ b/posix-configs/SITL/init/rcS_gazebo_tailsitter
@@ -60,7 +60,7 @@ mc_pos_control start
 mc_att_control start
 fw_pos_control_l1 start
 fw_att_control start
-mixer load /dev/pwm_output0 ../../../../ROMFS/px4fmu_common/mixers/quad_x_vtol.main.mix
+mixer load /dev/pwm_output0 ROMFS/px4fmu_common/mixers/quad_x_vtol.main.mix
 mavlink start -u 14556 -r 2000000
 mavlink start -u 14557 -r 2000000 -m onboard -o 14540
 mavlink stream -r 80 -s POSITION_TARGET_LOCAL_NED -u 14556
diff --git a/posix-configs/SITL/init/rcS_gazebo_typhoon_h480 b/posix-configs/SITL/init/rcS_gazebo_typhoon_h480
index 87c620c750..8713c62f74 100644
--- a/posix-configs/SITL/init/rcS_gazebo_typhoon_h480
+++ b/posix-configs/SITL/init/rcS_gazebo_typhoon_h480
@@ -59,7 +59,7 @@ navigator start
 ekf2 start
 mc_pos_control start
 mc_att_control start
-mixer load /dev/pwm_output0 ../../../../ROMFS/px4fmu_common/mixers/hexa_x.main.mix
+mixer load /dev/pwm_output0 ROMFS/px4fmu_common/mixers/hexa_x.main.mix
 mavlink start -u 14556 -r 4000000
 mavlink start -u 14557 -r 4000000 -m onboard -o 14540
 mavlink stream -r 50 -s POSITION_TARGET_LOCAL_NED -u 14556
diff --git a/posix-configs/SITL/init/rcS_jmavsim_iris b/posix-configs/SITL/init/rcS_jmavsim_iris
index 26db22d8f0..64cf0a19a1 100644
--- a/posix-configs/SITL/init/rcS_jmavsim_iris
+++ b/posix-configs/SITL/init/rcS_jmavsim_iris
@@ -61,7 +61,7 @@ navigator start
 ekf2 start
 mc_pos_control start
 mc_att_control start
-mixer load /dev/pwm_output0 ../../../../ROMFS/px4fmu_common/mixers/quad_x.main.mix
+mixer load /dev/pwm_output0 ROMFS/px4fmu_common/mixers/quad_x.main.mix
 mavlink start -u 14556 -r 2000000
 mavlink start -u 14557 -r 2000000 -m onboard -o 14540
 mavlink stream -r 80 -s POSITION_TARGET_LOCAL_NED -u 14556
diff --git a/posix-configs/SITL/init/rcS_lpe_gazebo_iris b/posix-configs/SITL/init/rcS_lpe_gazebo_iris
index 9c84018e98..59b11d1c5e 100644
--- a/posix-configs/SITL/init/rcS_lpe_gazebo_iris
+++ b/posix-configs/SITL/init/rcS_lpe_gazebo_iris
@@ -60,7 +60,7 @@ attitude_estimator_q start
 local_position_estimator start
 mc_pos_control start
 mc_att_control start
-mixer load /dev/pwm_output0 /usr/share/px4/ROMFS/px4fmu_common/mixers/quad_w.main.mix
+mixer load /dev/pwm_output0 ROMFS/px4fmu_common/mixers/quad_w.main.mix
 mavlink start -u 14556 -r 4000000
 mavlink start -u 14557 -r 4000000 -m onboard -o 14540
 mavlink stream -r 50 -s POSITION_TARGET_LOCAL_NED -u 14556
diff --git a/posix-configs/SITL/init/rcS_lpe_gazebo_iris_opt_flow b/posix-configs/SITL/init/rcS_lpe_gazebo_iris_opt_flow
index c1a6a2eae5..759831595a 100644
--- a/posix-configs/SITL/init/rcS_lpe_gazebo_iris_opt_flow
+++ b/posix-configs/SITL/init/rcS_lpe_gazebo_iris_opt_flow
@@ -69,7 +69,7 @@ attitude_estimator_q start
 local_position_estimator start
 mc_pos_control start
 mc_att_control start
-mixer load /dev/pwm_output0 /usr/share/px4/ROMFS/px4fmu_common/mixers/quad_w.main.mix
+mixer load /dev/pwm_output0 ROMFS/px4fmu_common/mixers/quad_w.main.mix
 mavlink start -u 14556 -r 2000000
 mavlink start -u 14557 -r 2000000 -m onboard -o 14540
 mavlink stream -r 80 -s POSITION_TARGET_LOCAL_NED -u 14556
diff --git a/posix-configs/SITL/init/rcS_lpe_jmavsim_iris b/posix-configs/SITL/init/rcS_lpe_jmavsim_iris
index 106d032275..286cb13f9c 100644
--- a/posix-configs/SITL/init/rcS_lpe_jmavsim_iris
+++ b/posix-configs/SITL/init/rcS_lpe_jmavsim_iris
@@ -61,7 +61,7 @@ attitude_estimator_q start
 local_position_estimator start
 mc_pos_control start
 mc_att_control start
-mixer load /dev/pwm_output0 /usr/share/px4/ROMFS/px4fmu_common/mixers/quad_x.main.mix
+mixer load /dev/pwm_output0 ROMFS/px4fmu_common/mixers/quad_x.main.mix
 mavlink start -u 14556 -r 2000000
 mavlink start -u 14557 -r 2000000 -m onboard -o 14540
 mavlink stream -r 80 -s POSITION_TARGET_LOCAL_NED -u 14556
diff --git a/posix-configs/SITL/init/rcS_tests b/posix-configs/SITL/init/rcS_tests_none_iris
similarity index 100%
rename from posix-configs/SITL/init/rcS_tests
rename to posix-configs/SITL/init/rcS_tests_none_iris
diff --git a/posix-configs/SITL/init/rc_iris_ros b/posix-configs/SITL/init/rc_iris_ros
index ca1cf0961e..2aa41cdbc8 100644
--- a/posix-configs/SITL/init/rc_iris_ros
+++ b/posix-configs/SITL/init/rc_iris_ros
@@ -60,7 +60,7 @@ attitude_estimator_q start
 position_estimator_inav start
 mc_pos_control_m start
 mc_att_control_m start
-mixer load /dev/pwm_output0 ../../../../ROMFS/px4fmu_common/mixers/quad_x.main.mix
+mixer load /dev/pwm_output0 ROMFS/px4fmu_common/mixers/quad_x.main.mix
 mavlink start -u 14556 -r 2000000
 mavlink stream -r 80 -s POSITION_TARGET_LOCAL_NED -u 14556
 mavlink stream -r 80 -s LOCAL_POSITION_NED -u 14556
diff --git a/src/firmware/nuttx/CMakeLists.txt b/src/firmware/nuttx/CMakeLists.txt
index 53988b43cf..4d27d2758b 100644
--- a/src/firmware/nuttx/CMakeLists.txt
+++ b/src/firmware/nuttx/CMakeLists.txt
@@ -10,7 +10,7 @@ add_executable(firmware_nuttx
 	builtin_commands.c)
 
 
-set(nuttx_export_dir ${CMAKE_BINARY_DIR}/${BOARD}/NuttX/nuttx-export)
+set(nuttx_export_dir ${PX4_BINARY_DIR}/${BOARD}/NuttX/nuttx-export)
 
 set(link_libs
 	romfs apps nuttx m gcc
@@ -20,7 +20,7 @@ if(NOT ${BOARD} STREQUAL "sim")
 	list(APPEND link_libs nosys)
 	set(main_link_flags
 		"-T${nuttx_export_dir}/build/ld.script"
-		"-Wl,-Map=${CMAKE_BINARY_DIR}/${BOARD}/main.map"
+		"-Wl,-Map=${PX4_BINARY_DIR}/${BOARD}/main.map"
 		)
 	px4_join(OUT main_link_flags LIST ${main_link_flags} GLUE " ")
 	set_target_properties(firmware_nuttx PROPERTIES LINK_FLAGS ${main_link_flags})
@@ -43,12 +43,12 @@ add_custom_target(check_weak
 if(NOT ${BOARD} STREQUAL "sim")
 
 	if (config_io_board)
-		set(extras "${CMAKE_BINARY_DIR}/src/modules/px4iofirmware/${config_io_board}.bin")
+		set(extras "${PX4_BINARY_DIR}/src/modules/px4iofirmware/${config_io_board}.bin")
 	endif()
 	
 	set(romfs_dir "ROMFS/px4fmu_common")
 	if (${BOARD} STREQUAL "tap-v1")
-	set(romfs_dir "ROMFS/tap_common")
+		set(romfs_dir "ROMFS/tap_common")
 	endif()
 	if (${BOARD} STREQUAL "px4fmu-v2" AND ${LABEL} STREQUAL "test")
 	    set(romfs_dir "ROMFS/px4fmu_test")
@@ -68,8 +68,8 @@ if(NOT ${BOARD} STREQUAL "sim")
 	px4_nuttx_add_firmware(OUT ${fw_file}
 		BOARD ${BOARD}
 		EXE ${CMAKE_CURRENT_BINARY_DIR}/firmware_nuttx
-		PARAM_XML ${CMAKE_BINARY_DIR}/parameters.xml
-		AIRFRAMES_XML ${CMAKE_BINARY_DIR}/airframes.xml
+		PARAM_XML ${PX4_BINARY_DIR}/parameters.xml
+		AIRFRAMES_XML ${PX4_BINARY_DIR}/airframes.xml
 		)
 
 	configure_file(gdbinit.in .gdbinit)
@@ -94,21 +94,21 @@ if(NOT ${BOARD} STREQUAL "sim")
 
 	add_custom_target(debug_io
 		COMMAND ${GDB}
-			${CMAKE_BINARY_DIR}/src/modules/px4iofirmware/${config_io_board}
+			${PX4_BINARY_DIR}/src/modules/px4iofirmware/${config_io_board}
 		DEPENDS firmware_nuttx
 			${CMAKE_CURRENT_BINARY_DIR}/.gdbinit
 		)
 
 	add_custom_target(debug_io_tui
 		COMMAND ${GDBTUI}
-			${CMAKE_BINARY_DIR}/src/modules/px4iofirmware/${config_io_board}
+			${PX4_BINARY_DIR}/src/modules/px4iofirmware/${config_io_board}
 		DEPENDS firmware_nuttx
 			${CMAKE_CURRENT_BINARY_DIR}/.gdbinit
 		)
 
 	add_custom_target(debug_io_ddd
 		COMMAND ${DDD} --debugger ${GDB}
-			${CMAKE_BINARY_DIR}/src/modules/px4iofirmware/${config_io_board}
+			${PX4_BINARY_DIR}/src/modules/px4iofirmware/${config_io_board}
 		DEPENDS firmware_nuttx
 			${CMAKE_CURRENT_BINARY_DIR}/.gdbinit
 		)
diff --git a/src/firmware/nuttx/gdbinit.in b/src/firmware/nuttx/gdbinit.in
index e820673b21..fd1b1c451e 100644
--- a/src/firmware/nuttx/gdbinit.in
+++ b/src/firmware/nuttx/gdbinit.in
@@ -4,4 +4,4 @@ attach 1
 monitor vector_catch disable hard
 set mem inaccessible-by-default off
 set print pretty
-source ${CMAKE_SOURCE_DIR}/Debug/PX4
+source ${PX4_SOURCE_DIR}/Debug/PX4
diff --git a/src/firmware/posix/CMakeLists.txt b/src/firmware/posix/CMakeLists.txt
index 3e8e8b5227..58ab449b5a 100644
--- a/src/firmware/posix/CMakeLists.txt
+++ b/src/firmware/posix/CMakeLists.txt
@@ -18,7 +18,7 @@ if ("${BOARD}" STREQUAL "eagle" OR ("${BOARD}" STREQUAL "excelsior"))
 		APPS_DEST "/home/linaro"
 		SOURCES
 			px4muorb_stub.c
-			${CMAKE_SOURCE_DIR}/src/platforms/posix/main.cpp
+			${PX4_SOURCE_DIR}/src/platforms/posix/main.cpp
 			apps.h
 		LINK_LIBS
 			-Wl,--start-group
@@ -33,14 +33,14 @@ if ("${BOARD}" STREQUAL "eagle" OR ("${BOARD}" STREQUAL "excelsior"))
 			 OS ${OS}
 			 BOARD ${BOARD}
 			 FILES ${CMAKE_CURRENT_BINARY_DIR}/px4
-				 ${CMAKE_SOURCE_DIR}/posix-configs/eagle/flight/mainapp.config
+				 ${PX4_SOURCE_DIR}/posix-configs/eagle/flight/mainapp.config
 			 DEPENDS px4
 			 DEST /home/linaro)
 
 elseif ("${BOARD}" STREQUAL "rpi")
 
 	add_executable(px4
-		${CMAKE_SOURCE_DIR}/src/platforms/posix/main.cpp
+		${PX4_SOURCE_DIR}/src/platforms/posix/main.cpp
 		apps.h
 		)
 
@@ -57,7 +57,7 @@ elseif ("${BOARD}" STREQUAL "rpi")
 			 OS ${OS}
 			 BOARD ${BOARD}
 			 FILES ${CMAKE_CURRENT_BINARY_DIR}/px4
-				 ${CMAKE_SOURCE_DIR}/posix-configs/rpi/px4.config
+				 ${PX4_SOURCE_DIR}/posix-configs/rpi/px4.config
 			 DEPENDS px4
 			 DEST /home/pi)
 
@@ -67,7 +67,7 @@ elseif ("${BOARD}" STREQUAL "bebop")
 	set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -static")
 
 	add_executable(px4
-		${CMAKE_SOURCE_DIR}/src/platforms/posix/main.cpp
+		${PX4_SOURCE_DIR}/src/platforms/posix/main.cpp
 		apps.h
 		)
 
@@ -91,14 +91,14 @@ elseif ("${BOARD}" STREQUAL "bebop")
 			 OS ${OS}
 			 BOARD ${BOARD}
 			 FILES ${CMAKE_CURRENT_BINARY_DIR}/px4
-						 ${CMAKE_SOURCE_DIR}/posix-configs/bebop/px4.config
+						 ${PX4_SOURCE_DIR}/posix-configs/bebop/px4.config
 			 DEPENDS px4
 			 DEST /usr/bin)
 
 else()
 
 	add_executable(px4
-		${CMAKE_SOURCE_DIR}/src/platforms/posix/main.cpp
+		${PX4_SOURCE_DIR}/src/platforms/posix/main.cpp
 		apps.h
 		)
 	if (NOT APPLE)
@@ -118,17 +118,43 @@ else()
 	endif()
 endif()
 
+#=============================================================================
+# sitl run targets
+#
+
+set(SITL_WORKING_DIR ${PX4_BINARY_DIR}/tmp)
+file(MAKE_DIRECTORY ${SITL_WORKING_DIR})
+
 add_custom_target(run_config
-	COMMAND Tools/sitl_run.sh "${config_sitl_rcS}" "${config_sitl_debugger}"
-	"${config_sitl_viewer}" "${config_sitl_model}" "${CMAKE_BINARY_DIR}"
-	WORKING_DIRECTORY ${CMAKE_SOURCE_DIR}
+	COMMAND Tools/sitl_run.sh
+	"$<TARGET_FILE:px4>"
+	"${config_sitl_rcS}"
+	"${config_sitl_debugger}"
+	"${config_sitl_viewer}"
+	"${config_sitl_model}" 
+	"${PX4_SOURCE_DIR}"
+	"${PX4_BINARY_DIR}"
+	WORKING_DIRECTORY ${SITL_WORKING_DIR}
 	USES_TERMINAL
 	)
 add_dependencies(run_config px4)
 
-foreach(viewer none jmavsim gazebo replay)
-	foreach(debugger none gdb lldb ddd valgrind)
-		foreach(model none iris iris_opt_flow tailsitter standard_vtol plane solo typhoon_h480)
+# project to build sitl_gazebo if necessary
+ExternalProject_Add(sitl_gazebo
+	SOURCE_DIR ${PX4_SOURCE_DIR}/Tools/sitl_gazebo
+	CMAKE_ARGS -DCMAKE_INSTALL_PREFIX=${CMAKE_INSTALL_PREFIX}
+	BINARY_DIR ${PX4_BINARY_DIR}/build_gazebo
+	INSTALL_COMMAND ""
+	)
+set_target_properties(sitl_gazebo PROPERTIES EXCLUDE_FROM_ALL TRUE)
+
+# create targets for each viewer/model/debugger combination
+set(viewers none jmavsim gazebo replay)
+set(debuggers none gdb lldb ddd valgrind)
+set(models none iris iris_opt_flow tailsitter standard_vtol plane solo typhoon_h480)
+foreach(viewer ${viewers})
+	foreach(debugger ${debuggers})
+		foreach(model ${models})
 			if (debugger STREQUAL "none")
 				if (model STREQUAL "none")
 					set(_targ_name "${viewer}")
@@ -143,13 +169,20 @@ foreach(viewer none jmavsim gazebo replay)
 				endif()
 			endif()
 			add_custom_target(${_targ_name}
-				COMMAND Tools/sitl_run.sh "${config_sitl_rcS}"
-				"${debugger}"
-				"${viewer}" "${model}" "${CMAKE_BINARY_DIR}"
-				WORKING_DIRECTORY ${CMAKE_SOURCE_DIR}
+				COMMAND ${PX4_SOURCE_DIR}/Tools/sitl_run.sh
+				$<TARGET_FILE:px4>
+				${config_sitl_rcS}
+				${debugger}
+				${viewer}
+				${model}
+				${PX4_SOURCE_DIR}
+				${PX4_BINARY_DIR}
+				WORKING_DIRECTORY ${SITL_WORKING_DIR}
 				USES_TERMINAL
 				)
-			add_dependencies(${_targ_name} px4)
+			if (viewer STREQUAL "gazebo")
+				add_dependencies(${_targ_name} sitl_gazebo)
+			endif()
 		endforeach()
 	endforeach()
 endforeach()
@@ -158,8 +191,25 @@ endforeach()
 # install
 #
 
-install(TARGETS px4 DESTINATION bin)
+install(TARGETS px4 DESTINATION ${CMAKE_INSTALL_BINDIR})
 install(DIRECTORY ${PROJECT_SOURCE_DIR}/ROMFS DESTINATION ${CMAKE_INSTALL_DATADIR}/${PROJECT_NAME})
 install(DIRECTORY ${PROJECT_SOURCE_DIR}/posix-configs DESTINATION ${CMAKE_INSTALL_DATADIR}/${PROJECT_NAME})
 
+#=============================================================================
+# tests
+#
+
+add_test(NAME rcS_tests
+	COMMAND ${PX4_SOURCE_DIR}/Tools/sitl_run.sh
+		$<TARGET_FILE:px4>
+		posix-configs/SITL/init/rcS_tests
+		none
+		none
+		none
+		${PX4_SOURCE_DIR}
+		${PX4_BINARY_DIR}
+		WORKING_DIRECTORY ${SITL_WORKING_DIR})
+set_tests_properties(rcS_tests PROPERTIES
+	PASS_REGULAR_EXPRESSION "All tests passed")
+
 # vim: set noet ft=cmake fenc=utf-8 ff=unix :
diff --git a/src/firmware/qurt/CMakeLists.txt b/src/firmware/qurt/CMakeLists.txt
index 81be14afc5..2e30c6aa8d 100644
--- a/src/firmware/qurt/CMakeLists.txt
+++ b/src/firmware/qurt/CMakeLists.txt
@@ -1,4 +1,4 @@
-set(CMAKE_MODULE_PATH ${CMAKE_MODULE_PATH} "${CMAKE_SOURCE_DIR}/cmake/cmake_hexagon")
+set(CMAKE_MODULE_PATH ${CMAKE_MODULE_PATH} "${PX4_SOURCE_DIR}/cmake/cmake_hexagon")
 include(fastrpc)
 include(qurt_lib)
 
@@ -7,7 +7,7 @@ include_directories(${CMAKE_CURRENT_BINARY_DIR})
 set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -Wno-missing-prototypes")
 
 px4_qurt_generate_builtin_commands(
-	OUT ${CMAKE_BINARY_DIR}/apps.h
+	OUT ${PX4_BINARY_DIR}/apps.h
 	MODULE_LIST ${module_libraries})
 
 FASTRPC_STUB_GEN(px4muorb.idl)
@@ -20,8 +20,8 @@ if ("${QURT_ENABLE_STUBS}" STREQUAL "1")
                 ${FASTRPC_DSP_INCLUDES}
                 )
 	add_executable(px4
-		${CMAKE_BINARY_DIR}/src/firmware/qurt/px4muorb_skel.c
-		${CMAKE_BINARY_DIR}/apps.h)
+		${PX4_BINARY_DIR}/src/firmware/qurt/px4muorb_skel.c
+		${PX4_BINARY_DIR}/apps.h)
 
 	target_link_libraries(px4
 		-Wl,--start-group
@@ -40,7 +40,7 @@ else()
 	QURT_LIB(LIB_NAME px4
 		IDL_NAME px4muorb
 		SOURCES
-			${CMAKE_BINARY_DIR}/apps.h
+			${PX4_BINARY_DIR}/apps.h
 		LINK_LIBS
 			${module_libraries}
 			${target_libraries}
@@ -54,7 +54,7 @@ else()
 			 BOARD ${BOARD}
 			 FILES ${CMAKE_CURRENT_BINARY_DIR}/libpx4.so
 				 ${CMAKE_CURRENT_BINARY_DIR}/libpx4muorb_skel.so
-				 ${CMAKE_SOURCE_DIR}/posix-configs/eagle/flight/px4.config
+				 ${PX4_SOURCE_DIR}/posix-configs/eagle/flight/px4.config
 			 DEPENDS px4 px4muorb_skel
 			 DEST /usr/share/data/adsp)
 endif()
diff --git a/src/lib/rc/rc_tests/CMakeLists.txt b/src/lib/rc/rc_tests/CMakeLists.txt
index 42a5a1b1ba..8a5be79ec2 100644
--- a/src/lib/rc/rc_tests/CMakeLists.txt
+++ b/src/lib/rc/rc_tests/CMakeLists.txt
@@ -40,4 +40,5 @@ px4_add_module(
 	DEPENDS
 		platforms__common
 	)
+
 # vim: set noet ft=cmake fenc=utf-8 ff=unix : 
diff --git a/src/lib/rc/rc_tests/RCTest.cpp b/src/lib/rc/rc_tests/RCTest.cpp
index d7a834d5c8..14119835f5 100644
--- a/src/lib/rc/rc_tests/RCTest.cpp
+++ b/src/lib/rc/rc_tests/RCTest.cpp
@@ -16,7 +16,7 @@
 #if !defined(CONFIG_ARCH_BOARD_SITL)
 #define TEST_DATA_PATH "/fs/microsd"
 #else
-#define TEST_DATA_PATH "../../../../src/lib/rc/rc_tests/test_data/"
+#define TEST_DATA_PATH "./src/lib/rc/rc_tests/test_data/"
 #endif
 
 extern "C" __EXPORT int rc_tests_main(int argc, char *argv[]);
@@ -51,8 +51,8 @@ bool RCTest::dsmTest(void)
 	FILE *fp;
 	fp = fopen(filepath, "rt");
 
+	warnx("loading data from: %s", filepath);
 	ut_test(fp != nullptr);
-	//warnx("loading data from: %s", filepath);
 
 	float f;
 	unsigned x;
diff --git a/src/modules/muorb/krait/CMakeLists.txt b/src/modules/muorb/krait/CMakeLists.txt
index b9e054e3e7..ad05492026 100644
--- a/src/modules/muorb/krait/CMakeLists.txt
+++ b/src/modules/muorb/krait/CMakeLists.txt
@@ -30,10 +30,10 @@
 # POSSIBILITY OF SUCH DAMAGE.
 #
 ############################################################################
-set(CMAKE_MODULE_PATH ${CMAKE_MODULE_PATH} "${CMAKE_SOURCE_DIR}/cmake/cmake_hexagon")
+set(CMAKE_MODULE_PATH ${CMAKE_MODULE_PATH} "${PX4_SOURCE_DIR}/cmake/cmake_hexagon")
 include(hexagon_sdk)
 
-include_directories(${CMAKE_BINARY_DIR}/src/firmware/posix)
+include_directories(${PX4_BINARY_DIR}/src/firmware/posix)
 include_directories(${HEXAGON_SDK_INCLUDES})
 
 px4_add_module(
diff --git a/src/modules/param/CMakeLists.txt b/src/modules/param/CMakeLists.txt
index b3148baab1..3f64a9826c 100644
--- a/src/modules/param/CMakeLists.txt
+++ b/src/modules/param/CMakeLists.txt
@@ -34,8 +34,8 @@
 include_directories(${CMAKE_CURRENT_BINARY_DIR})
 
 px4_generate_parameters_source(OUT param_files
-	XML ${CMAKE_BINARY_DIR}/parameters.xml
-	SCOPE ${CMAKE_SOURCE_DIR}/cmake/configs/${OS}_${BOARD}_${LABEL}.cmake
+	XML ${PX4_BINARY_DIR}/parameters.xml
+	SCOPE ${PX4_SOURCE_DIR}/cmake/configs/${OS}_${BOARD}_${LABEL}.cmake
 	DEPS xml_gen
 	)
 
diff --git a/src/modules/px4iofirmware/CMakeLists.txt b/src/modules/px4iofirmware/CMakeLists.txt
index ab2778711c..67183999eb 100644
--- a/src/modules/px4iofirmware/CMakeLists.txt
+++ b/src/modules/px4iofirmware/CMakeLists.txt
@@ -63,7 +63,7 @@ px4_join(OUT CMAKE_CXX_FLAGS LIST "${cxx_flags}" GLUE " ")
 
 include_directories(
 	${include_dirs}
-	${CMAKE_BINARY_DIR}/src/modules/systemlib/mixer
+	${PX4_BINARY_DIR}/src/modules/systemlib/mixer
 	.
 	)
 link_directories(${link_dirs})
@@ -117,10 +117,10 @@ add_dependencies(${fw_io_name}
 	mixer_gen
 	)
 
-set(nuttx_export_dir ${CMAKE_BINARY_DIR}/${config_io_board}/NuttX/nuttx-export)
+set(nuttx_export_dir ${PX4_BINARY_DIR}/${config_io_board}/NuttX/nuttx-export)
 set(main_link_flags
 	"-T${nuttx_export_dir}/build/ld.script"
-	"-Wl,-Map=${CMAKE_BINARY_DIR}/${config_io_board}/main.map"
+	"-Wl,-Map=${PX4_BINARY_DIR}/${config_io_board}/main.map"
 	)
 px4_join(OUT main_link_flags LIST ${main_link_flags} GLUE " ")
 set_target_properties(${fw_io_name} PROPERTIES LINK_FLAGS ${main_link_flags})
diff --git a/src/modules/systemlib/CMakeLists.txt b/src/modules/systemlib/CMakeLists.txt
index 7277ee3ca1..91f405d605 100644
--- a/src/modules/systemlib/CMakeLists.txt
+++ b/src/modules/systemlib/CMakeLists.txt
@@ -32,7 +32,7 @@
 ############################################################################
 
 # for generated files
-include_directories(${CMAKE_BINARY_DIR}/src/modules/param)
+include_directories(${PX4_BINARY_DIR}/src/modules/param)
 
 set(SRCS
 	perf_counter.c
diff --git a/src/modules/uavcan/uavcan_servers.hpp b/src/modules/uavcan/uavcan_servers.hpp
index 17e94cd311..47131f2ec6 100644
--- a/src/modules/uavcan/uavcan_servers.hpp
+++ b/src/modules/uavcan/uavcan_servers.hpp
@@ -70,7 +70,7 @@
 /**
  * A UAVCAN Server Sub node.
  */
-class UavcanServers
+class __EXPORT UavcanServers
 {
 	static constexpr unsigned NumIfaces = 1;  // UAVCAN_STM32_NUM_IFACES
 
diff --git a/src/platforms/posix/main.cpp b/src/platforms/posix/main.cpp
index 646b94610f..ac0520c858 100644
--- a/src/platforms/posix/main.cpp
+++ b/src/platforms/posix/main.cpp
@@ -50,6 +50,7 @@
 #include "px4_middleware.h"
 #include "DriverFramework.hpp"
 #include <termios.h>
+#include <sys/stat.h>
 
 namespace px4
 {
@@ -62,6 +63,12 @@ typedef int (*px4_main_t)(int argc, char *argv[]);
 
 #define CMD_BUFF_SIZE	100
 
+#ifdef PATH_MAX
+const unsigned path_max_len = PATH_MAX;
+#else
+const unsigned path_max_len = 1024;
+#endif
+
 static bool _ExitFlag = false;
 
 static struct termios orig_term;
@@ -85,6 +92,96 @@ extern "C" {
 	}
 }
 
+static inline bool fileExists(const string &name)
+{
+	struct stat buffer;
+	return (stat(name.c_str(), &buffer) == 0);
+}
+
+static inline bool dirExists(const string &path)
+{
+	struct stat info;
+
+	if (stat(path.c_str(), &info) != 0) {
+		return false;
+
+	} else if (info.st_mode & S_IFDIR) {
+		return true;
+
+	} else {
+		return false;
+	}
+}
+
+static inline void touch(const string &name)
+{
+	fstream fs;
+	fs.open(name, ios::out);
+	fs.close();
+}
+
+static int mkpath(const char *path, mode_t mode);
+
+static int do_mkdir(const char *path, mode_t mode)
+{
+	struct stat             st;
+	int             status = 0;
+
+	if (stat(path, &st) != 0) {
+		/* Directory does not exist. EEXIST for race condition */
+		if (mkdir(path, mode) != 0 && errno != EEXIST) {
+			status = -1;
+		}
+
+	} else if (!S_ISDIR(st.st_mode)) {
+		errno = ENOTDIR;
+		status = -1;
+	}
+
+	return (status);
+}
+
+/**
+** mkpath - ensure all directories in path exist
+** Algorithm takes the pessimistic view and works top-down to ensure
+** each directory in path exists, rather than optimistically creating
+** the last element and working backwards.
+*/
+static int mkpath(const char *path, mode_t mode)
+{
+	char           *pp;
+	char           *sp;
+	int             status;
+	char           *copypath = strdup(path);
+
+	status = 0;
+	pp = copypath;
+
+	while (status == 0 && (sp = strchr(pp, '/')) != 0) {
+		if (sp != pp) {
+			/* Neither root nor double slash in path */
+			*sp = '\0';
+			status = do_mkdir(copypath, mode);
+			*sp = '/';
+		}
+
+		pp = sp + 1;
+	}
+
+	if (status == 0) {
+		status = do_mkdir(path, mode);
+	}
+
+	free(copypath);
+	return (status);
+}
+
+static string pwd()
+{
+	char temp[path_max_len];
+	return (getcwd(temp, path_max_len) ? string(temp) : string(""));
+}
+
 static void print_prompt()
 {
 	cout.flush();
@@ -134,12 +231,13 @@ static void run_cmd(const vector<string> &appargs, bool exit_on_fail, bool silen
 static void usage()
 {
 
-	cout << "./px4 [-d] [startup_config] -h" << std::endl;
+	cout << "./px4 [-d] data_directory startup_config [-h]" << endl;
 	cout << "   -d            - Optional flag to run the app in daemon mode and does not listen for user input." <<
-	     std::endl;
-	cout << "                   This is needed if px4 is intended to be run as a upstart job on linux" << std::endl;
-	cout << "<startup_config> - config file for starting/stopping px4 modules" << std::endl;
-	cout << "   -h            - help/usage information" << std::endl;
+	     endl;
+	cout << "                   This is needed if px4 is intended to be run as a upstart job on linux" << endl;
+	cout << "<data_directory> - directory where romfs and posix-configs are located" << endl;
+	cout << "<startup_config> - config file for starting/stopping px4 modules" << endl;
+	cout << "   -h            - help/usage information" << endl;
 }
 
 static void process_line(string &line, bool exit_on_fail)
@@ -202,52 +300,131 @@ int main(int argc, char **argv)
 	set_cpu_scaling();
 
 	int index = 1;
-	char *commands_file = nullptr;
+	string  commands_file = "";
+	int positional_arg_count = 0;
+	string data_path = "";
+	string node_name = "";
 
+	// parse arguments
 	while (index < argc) {
+		//cout << "arg: " << index << " : " << argv[index] << endl;
+
 		if (argv[index][0] == '-') {
 			// the arg starts with -
-			if (strcmp(argv[index], "-d") == 0) {
+			if (strncmp(argv[index], "-d", 2) == 0) {
 				daemon_mode = true;
 
-			} else if (strcmp(argv[index], "-h") == 0) {
+			} else if (strncmp(argv[index], "-h", 2) == 0) {
 				usage();
 				return 0;
 
-			} else if (strcmp(argv[index], "-c") == 0) {
+			} else if (strncmp(argv[index], "-c", 2) == 0) {
 				chroot_on = true;
 
 			} else {
-				PX4_WARN("Unknown/unhandled parameter: %s", argv[index]);
+				PX4_ERR("Unknown/unhandled parameter: %s", argv[index]);
 				return 1;
 			}
 
+		} else if (!strncmp(argv[index], "__", 2)) {
+			//cout << "ros argument" << endl;
+
+			// ros arguments
+			if (!strncmp(argv[index], "__name:=", 8)) {
+				string name_arg = argv[index];
+				node_name = name_arg.substr(8);
+				cout << "node name: " << node_name << endl;
+			}
+
 		} else {
-			// this is an argument that does not have '-' prefix; treat it like a file name
-			ifstream infile(argv[index]);
+			//cout << "positional argument" << endl;
 
-			if (infile.good()) {
-				infile.close();
+			positional_arg_count += 1;
+
+			if (positional_arg_count == 1) {
+				data_path = argv[index];
+				cout << "data path: " << data_path << endl;
+
+			} else if (positional_arg_count == 2) {
 				commands_file = argv[index];
-
-			} else {
-				PX4_WARN("Error opening file: %s", argv[index]);
-				return -1;
+				cout << "commands file: " << commands_file << endl;
 			}
 		}
 
 		++index;
 	}
 
+	if (positional_arg_count != 2) {
+		PX4_ERR("Error expected 2 position arguments, got %d", positional_arg_count);
+		usage();
+		return -1;
+	}
 
+	if (commands_file.size() < 1) {
+		PX4_ERR("Error commands file not specified");
+		return -1;
+	}
+
+	if (!fileExists(commands_file)) {
+		PX4_ERR("Error opening commands file, does not exist: %s", commands_file.c_str());
+		return -1;
+	}
+
+	// create sym-links
+	vector<string> path_sym_links;
+	path_sym_links.push_back("ROMFS");
+	path_sym_links.push_back("posix-configs");
+	path_sym_links.push_back("src");
+
+	for (int i = 0; i < path_sym_links.size(); i++) {
+		string path_sym_link = path_sym_links[i];
+		//cout << "path sym link: " << path_sym_link << endl;
+		string src_path = data_path + "/" + path_sym_link;
+		string dest_path =  pwd() + "/" +  path_sym_link;
+
+		PX4_DEBUG("Creating symlink %s -> %s", src_path.c_str(), dest_path.c_str());
+
+		if (dirExists(path_sym_link)) { continue; }
+
+		// create sym-links
+		int ret = symlink(src_path.c_str(), dest_path.c_str());
+
+		if (ret != 0) {
+			PX4_ERR("Error creating symlink %s -> %s",
+				src_path.c_str(), dest_path.c_str());
+			return ret;
+
+		} else {
+			PX4_DEBUG("Successfully created symlink %s -> %s",
+				  src_path.c_str(), dest_path.c_str());
+		}
+	}
+
+	// setup rootfs
+	const string eeprom_path = "./rootfs/eeprom/";
+	const string microsd_path = "./rootfs/fs/microsd/";
+
+	if (!fileExists(eeprom_path + "parameters")) {
+		cout << "creating new parameters file" << endl;
+		mkpath(eeprom_path.c_str(), S_IRUSR | S_IWUSR | S_IXUSR);
+		touch(eeprom_path + "parameters");
+	}
+
+	if (!fileExists(microsd_path + "dataman")) {
+		cout << "creating new dataman file" << endl;
+		mkpath(microsd_path.c_str(), S_IRUSR | S_IWUSR | S_IXUSR);
+		touch(microsd_path + "dataman");
+	}
+
+	// initialize
 	DriverFramework::Framework::initialize();
 	px4::init_once();
 
 	px4::init(argc, argv, "px4");
 
 	// if commandfile is present, process the commands from the file
-	if (commands_file != nullptr) {
-		ifstream infile(commands_file);
+	if (commands_file.size() != 0) {
+		ifstream infile(commands_file.c_str());
 
 		if (infile.is_open()) {
 			for (string line; getline(infile, line, '\n');) {
@@ -261,7 +438,7 @@ int main(int argc, char **argv)
 			}
 
 		} else {
-			PX4_WARN("Error opening file: %s", commands_file);
+			PX4_ERR("Error opening commands file: %s", commands_file.c_str());
 		}
 	}
 
@@ -270,12 +447,6 @@ int main(int argc, char **argv)
 		// this is not an attempt to secure the environment,
 		// rather, to replicate a deployed file system.
 
-#ifdef PATH_MAX
-		const unsigned path_max_len = PATH_MAX;
-#else
-		const unsigned path_max_len = 1024;
-#endif
-
 		char pwd_path[path_max_len];
 		const char *folderpath = "/rootfs/";
 
@@ -419,3 +590,5 @@ int main(int argc, char **argv)
 
 	return OK;
 }
+
+/* vim: set noet fenc=utf-8 ff=unix sts=0 sw=4 ts=4 : */
diff --git a/src/platforms/posix/px4_layer/CMakeLists.txt b/src/platforms/posix/px4_layer/CMakeLists.txt
index 344621c694..fa1eed4ffc 100644
--- a/src/platforms/posix/px4_layer/CMakeLists.txt
+++ b/src/platforms/posix/px4_layer/CMakeLists.txt
@@ -32,10 +32,10 @@
 ############################################################################
 
 if("${CONFIG_SHMEM}" STREQUAL "1")
-	set(CMAKE_MODULE_PATH ${CMAKE_MODULE_PATH} "${CMAKE_SOURCE_DIR}/cmake/cmake_hexagon")
+	set(CMAKE_MODULE_PATH ${CMAKE_MODULE_PATH} "${PX4_SOURCE_DIR}/cmake/cmake_hexagon")
 	include(hexagon_sdk)
 
-	include_directories(${CMAKE_BINARY_DIR}/src/firmware/posix)
+	include_directories(${PX4_BINARY_DIR}/src/firmware/posix)
 	include_directories(${HEXAGON_SDK_INCLUDES})
 	list(APPEND SHMEM_SRCS
 		shmem_posix.c
diff --git a/src/platforms/qurt/px4_layer/CMakeLists.txt b/src/platforms/qurt/px4_layer/CMakeLists.txt
index 38dbd8a46f..ed6e56790a 100644
--- a/src/platforms/qurt/px4_layer/CMakeLists.txt
+++ b/src/platforms/qurt/px4_layer/CMakeLists.txt
@@ -30,7 +30,7 @@
 # POSSIBILITY OF SUCH DAMAGE.
 #
 ############################################################################
-set(CMAKE_MODULE_PATH ${CMAKE_MODULE_PATH} "${CMAKE_SOURCE_DIR}/cmake/cmake_hexagon")
+set(CMAKE_MODULE_PATH ${CMAKE_MODULE_PATH} "${PX4_SOURCE_DIR}/cmake/cmake_hexagon")
 include(hexagon_sdk)
 
 include_directories(${HEXAGON_8074_INCLUDES})
diff --git a/src/systemcmds/tests/test_mixer.cpp b/src/systemcmds/tests/test_mixer.cpp
index 88c37d6e9c..da2cff5d5a 100644
--- a/src/systemcmds/tests/test_mixer.cpp
+++ b/src/systemcmds/tests/test_mixer.cpp
@@ -90,7 +90,7 @@ int test_mixer(int argc, char *argv[])
 #if !defined(CONFIG_ARCH_BOARD_SITL)
 	const char *filename = "/etc/mixers/IO_pass.mix";
 #else
-	const char *filename = "../../../../ROMFS/px4fmu_test/mixers/IO_pass.mix";
+	const char *filename = "ROMFS/px4fmu_test/mixers/IO_pass.mix";
 #endif
 
 	//PX4_INFO("loading: %s", filename);
@@ -400,7 +400,7 @@ int test_mixer(int argc, char *argv[])
 #if !defined(CONFIG_ARCH_BOARD_SITL)
 	filename = "/etc/mixers/quad_test.mix";
 #else
-	filename = "../../../../ROMFS/px4fmu_test/mixers/quad_test.mix";
+	filename = "ROMFS/px4fmu_test/mixers/quad_test.mix";
 #endif
 
 	load_mixer_file(filename, &buf[0], sizeof(buf));
diff --git a/src/systemcmds/topic_listener/CMakeLists.txt b/src/systemcmds/topic_listener/CMakeLists.txt
index 0984e546e3..bb955490bc 100644
--- a/src/systemcmds/topic_listener/CMakeLists.txt
+++ b/src/systemcmds/topic_listener/CMakeLists.txt
@@ -32,14 +32,14 @@
 ############################################################################
 
 add_custom_command(OUTPUT topic_listener.cpp
-                COMMAND ${PYTHON_EXECUTABLE} ${CMAKE_SOURCE_DIR}/Tools/generate_listener.py ${CMAKE_SOURCE_DIR} > topic_listener.cpp
+                COMMAND ${PYTHON_EXECUTABLE} ${PX4_SOURCE_DIR}/Tools/generate_listener.py ${PX4_SOURCE_DIR} > topic_listener.cpp
                 DEPENDS msg_gen
                 )
 
 add_custom_target(generate_topic_listener
 	DEPENDS
 		topic_listener.cpp
-		${CMAKE_SOURCE_DIR}/Tools/generate_listener.py)
+		${PX4_SOURCE_DIR}/Tools/generate_listener.py)
 
 px4_add_module(
 	MODULE systemcmds__topic_listener
diff --git a/unittests/CMakeLists.txt b/unittests/CMakeLists.txt
index 9df3adcce3..b6664b26e8 100644
--- a/unittests/CMakeLists.txt
+++ b/unittests/CMakeLists.txt
@@ -13,15 +13,19 @@ enable_testing()
 set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -Wall -Werror -std=gnu99 -g")
 set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall -Werror -std=c++11 -g -fno-exceptions -fno-rtti -fno-threadsafe-statics")
 
+if (NOT PX4_SOURCE_DIR)
+	set(PX4_SOURCE_DIR "${CMAKE_CURRENT_SOURCE_DIR}/..")
+endif()
 
-set(GTEST_DIR ${CMAKE_SOURCE_DIR}/googletest)
+
+set(GTEST_DIR ${PX4_SOURCE_DIR}/unittests/googletest)
 add_subdirectory(${GTEST_DIR})
 include_directories(${GTEST_DIR}/include)
 
-set(PX4_SRC ${CMAKE_SOURCE_DIR}/../src)
+set(PX4_SRC ${PX4_SOURCE_DIR}/src)
 set(PX4_SITL_BUILD ${PX4_SRC}/../build_posix_sitl_default)
 
-include_directories(${CMAKE_SOURCE_DIR})
+include_directories(${PX4_SOURCE_DIR})
 include_directories(${PX4_SITL_BUILD}/src)
 include_directories(${PX4_SITL_BUILD}/src/modules)
 include_directories(${PX4_SITL_BUILD}/src/modules/param)
@@ -94,7 +98,7 @@ target_include_directories(px4_platform PUBLIC ${PX4_SRC}/platforms/posix/work_q
 # check
 add_custom_target(check
 		COMMAND ${CMAKE_CTEST_COMMAND} -j2 --output-on-failure
-		WORKING_DIR ${CMAKE_BINARY_DIR}
+		WORKING_DIR ${PX4_BINARY_DIR}
 		USES_TERMINAL)
 
 # add_gtest
@@ -105,7 +109,7 @@ function(add_gtest)
 		else()
 			target_link_libraries(${test_name} gtest_main pthread rt px4_platform)
 		endif()
-		add_test(NAME ${test_name} COMMAND ${test_name} WORKING_DIRECTORY ${CMAKE_SOURCE_DIR})
+		add_test(NAME ${test_name} COMMAND ${test_name} WORKING_DIRECTORY ${PX4_SOURCE_DIR})
 		add_dependencies(check ${test_name})
 	endforeach()
 endfunction()

From 08f5ece306650814a72cf5302dfecb50271f49b5 Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Fri, 5 Aug 2016 11:13:17 +0200
Subject: [PATCH 1223/1230] Remove on arming reset, be less verbose in normal
 conditions output

---
 .../BlockLocalPositionEstimator.cpp           | 40 +++++++++++--------
 .../local_position_estimator/sensors/baro.cpp |  4 +-
 .../local_position_estimator/sensors/flow.cpp |  2 +-
 .../local_position_estimator/sensors/gps.cpp  | 12 +++---
 .../sensors/sonar.cpp                         |  2 +-
 .../sensors/vision.cpp                        |  2 +-
 6 files changed, 36 insertions(+), 26 deletions(-)

diff --git a/src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp b/src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp
index bdabd56b79..ede4309e58 100644
--- a/src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp
+++ b/src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp
@@ -254,28 +254,34 @@ void BlockLocalPositionEstimator::update()
 	}
 
 	// reset pos, vel, and terrain on arming
-	if (!_lastArmedState && armedState) {
 
-		// we just armed, we are at origin on the ground
-		_x(X_x) = 0;
-		_x(X_y) = 0;
+	// XXX this will be re-enabled for indoor use cases using a
+	// selection param, but is really not helping outdoors
+	// right now.
 
-		// reset flow integral
-		_flowX = 0;
-		_flowY = 0;
+	// if (!_lastArmedState && armedState) {
 
-		// we aren't moving, all velocities are zero
-		_x(X_vx) = 0;
-		_x(X_vy) = 0;
-		_x(X_vz) = 0;
+	// 	// we just armed, we are at origin on the ground
+	// 	_x(X_x) = 0;
+	// 	_x(X_y) = 0;
+	// 	// reset Z or not? _x(X_z) = 0;
 
-		// assume we are on the ground, so terrain alt is local alt
-		_x(X_tz) = _x(X_z);
+	// 	// reset flow integral
+	// 	_flowX = 0;
+	// 	_flowY = 0;
 
-		// reset lowpass filter as well
-		_xLowPass.setState(_x);
-		_aglLowPass.setState(0);
-	}
+	// 	// we aren't moving, all velocities are zero
+	// 	_x(X_vx) = 0;
+	// 	_x(X_vy) = 0;
+	// 	_x(X_vz) = 0;
+
+	// 	// assume we are on the ground, so terrain alt is local alt
+	// 	_x(X_tz) = _x(X_z);
+
+	// 	// reset lowpass filter as well
+	// 	_xLowPass.setState(_x);
+	// 	_aglLowPass.setState(0);
+	// }
 
 	_lastArmedState = armedState;
 
diff --git a/src/modules/local_position_estimator/sensors/baro.cpp b/src/modules/local_position_estimator/sensors/baro.cpp
index 1d0624a8c8..1f30fe98aa 100644
--- a/src/modules/local_position_estimator/sensors/baro.cpp
+++ b/src/modules/local_position_estimator/sensors/baro.cpp
@@ -76,8 +76,10 @@ void BlockLocalPositionEstimator::baroCorrect()
 
 	if (beta > BETA_TABLE[n_y_baro]) {
 		if (_baroFault < FAULT_MINOR) {
-			mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] baro fault, r %5.2f m, beta %5.2f",
+			if (beta > 2.0f * BETA_TABLE[n_y_baro]) {
+				mavlink_and_console_log_critical(&mavlink_log_pub, "[lpe] baro fault, r %5.2f m, beta %5.2f",
 						     double(r(0)), double(beta));
+			}
 			_baroFault = FAULT_MINOR;
 		}
 
diff --git a/src/modules/local_position_estimator/sensors/flow.cpp b/src/modules/local_position_estimator/sensors/flow.cpp
index 13b646d9e0..bdfcb3fb7e 100644
--- a/src/modules/local_position_estimator/sensors/flow.cpp
+++ b/src/modules/local_position_estimator/sensors/flow.cpp
@@ -180,7 +180,7 @@ void BlockLocalPositionEstimator::flowCheckTimeout()
 		if (_flowInitialized) {
 			_flowInitialized = false;
 			_flowQStats.reset();
-			mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] flow timeout ");
+			mavlink_and_console_log_critical(&mavlink_log_pub, "[lpe] flow timeout ");
 		}
 	}
 }
diff --git a/src/modules/local_position_estimator/sensors/gps.cpp b/src/modules/local_position_estimator/sensors/gps.cpp
index ffe3db2aab..4056c16bff 100644
--- a/src/modules/local_position_estimator/sensors/gps.cpp
+++ b/src/modules/local_position_estimator/sensors/gps.cpp
@@ -46,7 +46,7 @@ void BlockLocalPositionEstimator::gpsInit()
 		}
 
 		_gpsAltOrigin = _gpsStats.getMean()(2);
-		mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] gps init "
+		PX4_INFO("[lpe] gps init "
 					     "lat %6.2f lon %6.2f alt %5.1f m",
 					     gpsLatOrigin,
 					     gpsLonOrigin,
@@ -175,9 +175,11 @@ void BlockLocalPositionEstimator::gpsCorrect()
 
 	if (beta > BETA_TABLE[n_y_gps]) {
 		if (_gpsFault < FAULT_MINOR) {
-			mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] gps fault %3g %3g %3g %3g %3g %3g",
-						     double(r(0)*r(0) / S_I(0, 0)),  double(r(1)*r(1) / S_I(1, 1)), double(r(2)*r(2) / S_I(2, 2)),
-						     double(r(3)*r(3) / S_I(3, 3)),  double(r(4)*r(4) / S_I(4, 4)), double(r(5)*r(5) / S_I(5, 5)));
+			if (beta > 2.0f * BETA_TABLE[n_y_gps]) {
+				mavlink_and_console_log_critical(&mavlink_log_pub, "[lpe] gps fault %3g %3g %3g %3g %3g %3g",
+							     double(r(0)*r(0) / S_I(0, 0)),  double(r(1)*r(1) / S_I(1, 1)), double(r(2)*r(2) / S_I(2, 2)),
+							     double(r(3)*r(3) / S_I(3, 3)),  double(r(4)*r(4) / S_I(4, 4)), double(r(5)*r(5) / S_I(5, 5)));
+			}
 			_gpsFault = FAULT_MINOR;
 		}
 
@@ -202,7 +204,7 @@ void BlockLocalPositionEstimator::gpsCheckTimeout()
 		if (_gpsInitialized) {
 			_gpsInitialized = false;
 			_gpsStats.reset();
-			mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] GPS timeout ");
+			mavlink_and_console_log_critical(&mavlink_log_pub, "[lpe] GPS timeout ");
 		}
 	}
 }
diff --git a/src/modules/local_position_estimator/sensors/sonar.cpp b/src/modules/local_position_estimator/sensors/sonar.cpp
index d2ea5be9f5..fdb8d8f0a5 100644
--- a/src/modules/local_position_estimator/sensors/sonar.cpp
+++ b/src/modules/local_position_estimator/sensors/sonar.cpp
@@ -34,7 +34,7 @@ void BlockLocalPositionEstimator::sonarInit()
 			_sonarStats.reset();
 
 		} else {
-			mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] sonar init "
+			PX4_INFO("[lpe] sonar init "
 						     "mean %d cm std %d cm",
 						     int(100 * _sonarStats.getMean()(0)),
 						     int(100 * _sonarStats.getStdDev()(0)));
diff --git a/src/modules/local_position_estimator/sensors/vision.cpp b/src/modules/local_position_estimator/sensors/vision.cpp
index f0d9f8c0db..0832b22473 100644
--- a/src/modules/local_position_estimator/sensors/vision.cpp
+++ b/src/modules/local_position_estimator/sensors/vision.cpp
@@ -109,7 +109,7 @@ void BlockLocalPositionEstimator::visionCheckTimeout()
 		if (_visionInitialized) {
 			_visionInitialized = false;
 			_visionStats.reset();
-			mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] vision position timeout ");
+			mavlink_and_console_log_critical(&mavlink_log_pub, "[lpe] vision position timeout ");
 		}
 	}
 }

From 6ac79aa55f677553ba368e05502bec6533c7818d Mon Sep 17 00:00:00 2001
From: James Goppert <james.goppert@gmail.com>
Date: Fri, 5 Aug 2016 06:52:35 -0400
Subject: [PATCH 1224/1230] Formatting.

---
 .../local_position_estimator/sensors/baro.cpp       |  3 ++-
 .../local_position_estimator/sensors/gps.cpp        | 13 +++++++------
 .../local_position_estimator/sensors/sonar.cpp      |  6 +++---
 3 files changed, 12 insertions(+), 10 deletions(-)

diff --git a/src/modules/local_position_estimator/sensors/baro.cpp b/src/modules/local_position_estimator/sensors/baro.cpp
index 1f30fe98aa..c41bfed60c 100644
--- a/src/modules/local_position_estimator/sensors/baro.cpp
+++ b/src/modules/local_position_estimator/sensors/baro.cpp
@@ -78,8 +78,9 @@ void BlockLocalPositionEstimator::baroCorrect()
 		if (_baroFault < FAULT_MINOR) {
 			if (beta > 2.0f * BETA_TABLE[n_y_baro]) {
 				mavlink_and_console_log_critical(&mavlink_log_pub, "[lpe] baro fault, r %5.2f m, beta %5.2f",
-						     double(r(0)), double(beta));
+								 double(r(0)), double(beta));
 			}
+
 			_baroFault = FAULT_MINOR;
 		}
 
diff --git a/src/modules/local_position_estimator/sensors/gps.cpp b/src/modules/local_position_estimator/sensors/gps.cpp
index 4056c16bff..ff331072be 100644
--- a/src/modules/local_position_estimator/sensors/gps.cpp
+++ b/src/modules/local_position_estimator/sensors/gps.cpp
@@ -47,10 +47,10 @@ void BlockLocalPositionEstimator::gpsInit()
 
 		_gpsAltOrigin = _gpsStats.getMean()(2);
 		PX4_INFO("[lpe] gps init "
-					     "lat %6.2f lon %6.2f alt %5.1f m",
-					     gpsLatOrigin,
-					     gpsLonOrigin,
-					     double(_gpsAltOrigin));
+			 "lat %6.2f lon %6.2f alt %5.1f m",
+			 gpsLatOrigin,
+			 gpsLonOrigin,
+			 double(_gpsAltOrigin));
 		_gpsInitialized = true;
 		_gpsFault = FAULT_NONE;
 		_gpsStats.reset();
@@ -177,9 +177,10 @@ void BlockLocalPositionEstimator::gpsCorrect()
 		if (_gpsFault < FAULT_MINOR) {
 			if (beta > 2.0f * BETA_TABLE[n_y_gps]) {
 				mavlink_and_console_log_critical(&mavlink_log_pub, "[lpe] gps fault %3g %3g %3g %3g %3g %3g",
-							     double(r(0)*r(0) / S_I(0, 0)),  double(r(1)*r(1) / S_I(1, 1)), double(r(2)*r(2) / S_I(2, 2)),
-							     double(r(3)*r(3) / S_I(3, 3)),  double(r(4)*r(4) / S_I(4, 4)), double(r(5)*r(5) / S_I(5, 5)));
+								 double(r(0)*r(0) / S_I(0, 0)),  double(r(1)*r(1) / S_I(1, 1)), double(r(2)*r(2) / S_I(2, 2)),
+								 double(r(3)*r(3) / S_I(3, 3)),  double(r(4)*r(4) / S_I(4, 4)), double(r(5)*r(5) / S_I(5, 5)));
 			}
+
 			_gpsFault = FAULT_MINOR;
 		}
 
diff --git a/src/modules/local_position_estimator/sensors/sonar.cpp b/src/modules/local_position_estimator/sensors/sonar.cpp
index fdb8d8f0a5..c03015a53a 100644
--- a/src/modules/local_position_estimator/sensors/sonar.cpp
+++ b/src/modules/local_position_estimator/sensors/sonar.cpp
@@ -35,9 +35,9 @@ void BlockLocalPositionEstimator::sonarInit()
 
 		} else {
 			PX4_INFO("[lpe] sonar init "
-						     "mean %d cm std %d cm",
-						     int(100 * _sonarStats.getMean()(0)),
-						     int(100 * _sonarStats.getStdDev()(0)));
+				 "mean %d cm std %d cm",
+				 int(100 * _sonarStats.getMean()(0)),
+				 int(100 * _sonarStats.getStdDev()(0)));
 			_sonarInitialized = true;
 			_sonarFault = FAULT_NONE;
 		}

From bbe352537794f8c010c4af17812de28256ffa6a1 Mon Sep 17 00:00:00 2001
From: Mark Whitehorn <kd0aij@gmail.com>
Date: Fri, 5 Aug 2016 09:00:12 -0600
Subject: [PATCH 1225/1230] fix VTOL vehicle_status timestamp (#5252)

* fix VTOL vehicle_status timestamp

* run astyle
---
 src/modules/vtol_att_control/vtol_att_control_main.cpp | 3 ++-
 1 file changed, 2 insertions(+), 1 deletion(-)

diff --git a/src/modules/vtol_att_control/vtol_att_control_main.cpp b/src/modules/vtol_att_control/vtol_att_control_main.cpp
index 820f7cfa56..4d0083bf99 100644
--- a/src/modules/vtol_att_control/vtol_att_control_main.cpp
+++ b/src/modules/vtol_att_control/vtol_att_control_main.cpp
@@ -668,11 +668,12 @@ void VtolAttitudeControl::task_main()
 
 	while (!_task_should_exit) {
 		/*Advertise/Publish vtol vehicle status*/
+		_vtol_vehicle_status.timestamp = hrt_absolute_time();
+
 		if (_vtol_vehicle_status_pub != nullptr) {
 			orb_publish(ORB_ID(vtol_vehicle_status), _vtol_vehicle_status_pub, &_vtol_vehicle_status);
 
 		} else {
-			_vtol_vehicle_status.timestamp = hrt_absolute_time();
 			_vtol_vehicle_status_pub = orb_advertise(ORB_ID(vtol_vehicle_status), &_vtol_vehicle_status);
 		}
 

From 102f5b54d7edb11b7913514e1edd081eb4a0d642 Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Fri, 5 Aug 2016 21:29:49 +0200
Subject: [PATCH 1226/1230] Revert "Improvements to SITL to make paths more
 flexible. (#5181)"

This reverts commit 699b6a2cb340f4b6ec3979ae7748ff9c56cfdb89.
---
 CMakeLists.txt                                |  65 ++--
 Makefile                                      |  33 +-
 Tools/sitl_gazebo                             |   2 +-
 Tools/sitl_run.sh                             |  83 ++---
 cmake/common/px4_base.cmake                   |  90 ++---
 cmake/configs/nuttx_mindpx-v2_default.cmake   |   2 +-
 .../nuttx_px4-stm32f4discovery_default.cmake  |   2 +-
 cmake/configs/nuttx_px4fmu-v1_default.cmake   |   2 +-
 cmake/configs/nuttx_px4fmu-v2_default.cmake   |   2 +-
 cmake/configs/nuttx_px4fmu-v2_ekf2.cmake      |   2 +-
 cmake/configs/nuttx_px4fmu-v2_test.cmake      |   2 +-
 cmake/configs/nuttx_px4fmu-v4_default.cmake   |   2 +-
 cmake/configs/nuttx_tap-v1_default.cmake      |   2 +-
 cmake/configs/posix_bebop_default.cmake       |   2 +-
 cmake/configs/posix_eagle_default.cmake       |   4 +-
 cmake/configs/posix_eagle_hil.cmake           |   4 +-
 .../posix_eagle_legacy_driver_default.cmake   |   4 +-
 cmake/configs/posix_eagle_muorb.cmake         |   4 +-
 cmake/configs/posix_rpi_cross.cmake           |   2 +-
 cmake/configs/posix_rpi_native.cmake          |   2 +-
 cmake/configs/posix_sdflight_default.cmake    |   4 +-
 cmake/configs/posix_sitl_broadcast.cmake      |   2 +-
 cmake/configs/posix_sitl_default.cmake        |   2 +-
 cmake/configs/posix_sitl_replay.cmake         |   2 +-
 cmake/configs/qurt_eagle_hello.cmake          |   4 +-
 cmake/configs/qurt_eagle_hil.cmake            |   4 +-
 .../qurt_eagle_legacy_driver_default.cmake    |   4 +-
 cmake/configs/qurt_eagle_muorb.cmake          |   4 +-
 cmake/configs/qurt_eagle_test.cmake           |   4 +-
 cmake/configs/qurt_eagle_travis.cmake         |   4 +-
 cmake/configs/qurt_sdflight_default.cmake     |   4 +-
 cmake/nuttx/px4_impl_nuttx.cmake              |  52 +--
 cmake/posix/px4_impl_posix.cmake              |   4 +-
 cmake/qurt/px4_impl_qurt.cmake                |   4 +-
 cmake/ros-CMakeLists.txt                      | 334 ++++++++++++++++++
 integrationtests/run_tests.bash               |  93 ++---
 integrationtests/setup_gazebo_ros.bash        |  19 +-
 launch/mavros_posix_sitl.launch               |  13 +-
 launch/mavros_posix_tests_iris.launch         |   1 -
 .../mavros_posix_tests_standard_vtol.launch   |   2 +-
 launch/posix_sitl.launch                      |  20 +-
 posix-configs/SITL/README.md                  |   2 +-
 posix-configs/SITL/init/rcS_gazebo_iris       |   2 +-
 .../SITL/init/rcS_gazebo_iris_opt_flow        |   2 +-
 posix-configs/SITL/init/rcS_gazebo_plane      |   2 +-
 posix-configs/SITL/init/rcS_gazebo_solo       |   2 +-
 .../SITL/init/rcS_gazebo_standard_vtol        |   2 +-
 posix-configs/SITL/init/rcS_gazebo_tailsitter |   2 +-
 .../SITL/init/rcS_gazebo_typhoon_h480         |   2 +-
 posix-configs/SITL/init/rcS_jmavsim_iris      |   2 +-
 posix-configs/SITL/init/rcS_lpe_gazebo_iris   |   2 +-
 .../SITL/init/rcS_lpe_gazebo_iris_opt_flow    |   2 +-
 posix-configs/SITL/init/rcS_lpe_jmavsim_iris  |   2 +-
 .../init/{rcS_tests_none_iris => rcS_tests}   |   0
 posix-configs/SITL/init/rc_iris_ros           |   2 +-
 src/firmware/nuttx/CMakeLists.txt             |  18 +-
 src/firmware/nuttx/gdbinit.in                 |   2 +-
 src/firmware/posix/CMakeLists.txt             |  88 +----
 src/firmware/qurt/CMakeLists.txt              |  12 +-
 src/lib/rc/rc_tests/CMakeLists.txt            |   1 -
 src/lib/rc/rc_tests/RCTest.cpp                |   4 +-
 src/modules/muorb/krait/CMakeLists.txt        |   4 +-
 src/modules/param/CMakeLists.txt              |   4 +-
 src/modules/px4iofirmware/CMakeLists.txt      |   6 +-
 src/modules/systemlib/CMakeLists.txt          |   2 +-
 src/modules/uavcan/uavcan_servers.hpp         |   2 +-
 src/platforms/posix/main.cpp                  | 227 ++----------
 src/platforms/posix/px4_layer/CMakeLists.txt  |   4 +-
 src/platforms/qurt/px4_layer/CMakeLists.txt   |   2 +-
 src/systemcmds/tests/test_mixer.cpp           |   4 +-
 src/systemcmds/topic_listener/CMakeLists.txt  |   4 +-
 unittests/CMakeLists.txt                      |  14 +-
 72 files changed, 672 insertions(+), 643 deletions(-)
 create mode 100644 cmake/ros-CMakeLists.txt
 rename posix-configs/SITL/init/{rcS_tests_none_iris => rcS_tests} (100%)

diff --git a/CMakeLists.txt b/CMakeLists.txt
index 498b6ba7d8..9397b87c99 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -121,14 +121,6 @@
 # and leads to wrong toolchain detection
 cmake_minimum_required(VERSION 2.8 FATAL_ERROR)
 
-set(PX4_SOURCE_DIR "${CMAKE_CURRENT_SOURCE_DIR}")
-set(PX4_BINARY_DIR "${CMAKE_CURRENT_BINARY_DIR}")
-
-execute_process(
-	COMMAND Tools/check_submodules.sh
-	WORKING_DIRECTORY ${PX4_SOURCE_DIR}
-	)
-
 # Use clang
 #SET (CMAKE_C_COMPILER /usr/bin/clang-3.6)
 #SET (CMAKE_CXX_COMPILER /usr/bin/clang++-3.6)
@@ -163,9 +155,6 @@ message(STATUS "${target_name}")
 # Define GNU standard installation directories
 include(GNUInstallDirs)
 
-# Add support for external project building
-include(ExternalProject)
-
 # Setup install paths
 if(NOT CMAKE_INSTALL_PREFIX)
 	if (${OS} STREQUAL "posix")
@@ -174,8 +163,12 @@ if(NOT CMAKE_INSTALL_PREFIX)
 endif()
 message(STATUS "CMAKE_INSTALL_PREFIX: ${CMAKE_INSTALL_PREFIX}")
 
-list(APPEND CMAKE_MODULE_PATH "${PX4_SOURCE_DIR}/cmake")
-message(STATUS "cmake module path: ${CMAKE_MODULE_PATH}")
+# switch to ros CMake file if building ros
+if (${OS} STREQUAL "ros")
+	include("cmake/ros-CMakeLists.txt")
+else()  # otherwise use the rest of this file
+
+list(APPEND CMAKE_MODULE_PATH ${CMAKE_SOURCE_DIR}/cmake)
 set(config_module "configs/${CONFIG}")
 include(${config_module})
 
@@ -206,9 +199,6 @@ if(DEFINED config_df_driver_list)
 	message("DF Drivers: ${config_df_driver_list}")
 endif()
 
-# force static lib build
-set(BUILD_SHARED_LIBS OFF)
-
 #=============================================================================
 # project definition
 #
@@ -229,22 +219,8 @@ set(version "${version_major}.${version_minor}.${version_patch}")
 set(package-contact "px4users@googlegroups.com")
 
 #=============================================================================
-# find programs and packages
+# programs
 #
-
-# see if catkin was invoked to build this
-if (CATKIN_DEVEL_PREFIX)
-	message(STATUS "catkin ENABLED")
-	find_package(catkin REQUIRED)
-	if (catkin_FOUND)
-		catkin_package()
-	else()
-		message(FATAL_ERROR "catkin not found")
-	endif()
-else()
-	message(STATUS "catkin DISABLED")
-endif()
-
 find_package(PythonInterp REQUIRED)
 
 #=============================================================================
@@ -281,7 +257,7 @@ px4_add_git_submodule(TARGET git_matrix PATH "src/lib/matrix")
 px4_add_git_submodule(TARGET git_cmake_hexagon PATH "cmake/cmake_hexagon")
 
 add_custom_target(submodule_clean
-	WORKING_DIRECTORY ${PX4_SOURCE_DIR}
+	WORKING_DIRECTORY ${CMAKE_SOURCE_DIR}
 	COMMAND git submodule deinit -f .
 	COMMAND rm -rf .git/modules/*
 	)
@@ -291,12 +267,12 @@ add_custom_target(submodule_clean
 #
 add_custom_target(check_format
 	COMMAND Tools/check_code_style.sh
-	WORKING_DIRECTORY ${PX4_SOURCE_DIR}
+	WORKING_DIRECTORY ${CMAKE_SOURCE_DIR}
 	)
 
 add_custom_target(config
 	COMMAND cmake-gui .
-	WORKING_DIRECTORY ${PX4_BINARY_DIR}
+	WORKING_DIRECTORY ${CMAKE_BINARY_DIR}
 	)
 
 #=============================================================================
@@ -345,7 +321,7 @@ add_custom_target(xml_gen
 # external projects
 #
 
-set(ep_base ${PX4_BINARY_DIR}/external)
+set(ep_base ${CMAKE_BINARY_DIR}/external)
 set_property(DIRECTORY PROPERTY EP_BASE ${ep_base})
 
 # add external project install folders to build
@@ -374,7 +350,7 @@ foreach(module ${config_module_list})
 	if(external_module)
 		STRING(REGEX REPLACE "//" "/" EXT_MODULE ${module})
 		STRING(REGEX REPLACE "/" "__" EXT_MODULE_PREFIX ${EXT_MODULE})
-		add_subdirectory(${module} ${PX4_BINARY_DIR}/${EXT_MODULE_PREFIX})
+		add_subdirectory(${module} ${CMAKE_BINARY_DIR}/${EXT_MODULE_PREFIX})
 	else()
 		add_subdirectory(src/${module})
 	endif()
@@ -399,7 +375,7 @@ endif()
 #=============================================================================
 # generate git version
 #
-px4_create_git_hash_header(HEADER ${PX4_BINARY_DIR}/build_git_version.h)
+px4_create_git_hash_header(HEADER ${CMAKE_BINARY_DIR}/build_git_version.h)
 
 #=============================================================================
 # packaging
@@ -418,14 +394,17 @@ set(CPACK_GENERATOR "ZIP")
 set(CPACK_PACKAGE_FILE_NAME "${PROJECT_NAME}-${CONFIG}-${version}")
 set(CPACK_SOURCE_PACKAGE_FILE_NAME "${PROJECT_NAME}-${version}")
 set(CPACK_SOURCE_GENERATOR "ZIP;TBZ2")
-set(CPACK_PACKAGING_INSTALL_PREFIX "/usr")
-set(CPACK_SET_DESTDIR "OFF")
 if ("${CMAKE_SYSTEM}" MATCHES "Linux")
-	find_program(DPKG_PROGRAM dpkg)
-	if (EXISTS ${DPKG_PROGRAM})
-		list (APPEND CPACK_GENERATOR "DEB")
-	endif()
+  find_program(DPKG_PROGRAM dpkg)
+  if (EXISTS ${DPKG_PROGRAM})
+    list (APPEND CPACK_GENERATOR "DEB")
+  endif()
+endif()
+if (${OS} STREQUAL "posix")
+	set(CPACK_SET_DESTDIR "ON")
 endif()
 include(CPack)
 
+endif() # ros alternative endif
+
 # vim: set noet fenc=utf-8 ff=unix ft=cmake :
diff --git a/Makefile b/Makefile
index b4a02f8d8c..f80a24399c 100644
--- a/Makefile
+++ b/Makefile
@@ -111,23 +111,20 @@ else
 	BUILD_DIR_SUFFIX :=
 endif
 
-SRC_DIR := $(shell dirname $(realpath $(lastword $(MAKEFILE_LIST))))
-
 # Functions
 # --------------------------------------------------------------------
 # describe how to build a cmake config
 define cmake-build
-+@$(eval BUILD_DIR = $(SRC_DIR)/build_$@$(BUILD_DIR_SUFFIX))
-+@if [ $(PX4_CMAKE_GENERATOR) = "Ninja" ] && [ -e $(BUILD_DIR)/Makefile ]; then rm -rf $(BUILD_DIR); fi
-+@if [ ! -e $(BUILD_DIR)/CMakeCache.txt ]; then mkdir -p $(BUILD_DIR) && cd $(BUILD_DIR) && cmake .. -G$(PX4_CMAKE_GENERATOR) -DCONFIG=$(1) || (cd .. && rm -rf $(BUILD_DIR)); fi
-+@(echo "PX4 CONFIG: $(BUILD_DIR)" && cd $(BUILD_DIR) && $(PX4_MAKE) $(PX4_MAKE_ARGS) $(ARGS))
++@if [ $(PX4_CMAKE_GENERATOR) = "Ninja" ] && [ -e ./build_$@$(BUILD_DIR_SUFFIX)/Makefile ]; then rm -rf ./build_$@$(BUILD_DIR_SUFFIX); fi
++@if [ ! -e ./build_$@$(BUILD_DIR_SUFFIX)/CMakeCache.txt ]; then Tools/check_submodules.sh && mkdir -p ./build_$@$(BUILD_DIR_SUFFIX) && cd ./build_$@$(BUILD_DIR_SUFFIX) && cmake .. -G$(PX4_CMAKE_GENERATOR) -DCONFIG=$(1) || (cd .. && rm -rf ./build_$@$(BUILD_DIR_SUFFIX)); fi
++@Tools/check_submodules.sh
++@(echo "PX4 CONFIG: $@$(BUILD_DIR_SUFFIX)" && cd ./build_$@$(BUILD_DIR_SUFFIX) && $(PX4_MAKE) $(PX4_MAKE_ARGS) $(ARGS))
 endef
 
 define cmake-build-other
-+@$(eval BUILD_DIR = $(SRC_DIR)/build_$@$(BUILD_DIR_SUFFIX))
-+@if [ $(PX4_CMAKE_GENERATOR) = "Ninja" ] && [ -e $(BUILD_DIR)/Makefile ]; then rm -rf $(BUILD_DIR); fi
-+@if [ ! -e $(BUILD_DIR)/CMakeCache.txt ]; then mkdir -p $(BUILD_DIR) && cd $(BUILD_DIR) && cmake $(2) -G$(PX4_CMAKE_GENERATOR) -DCONFIG=$(1) || (cd .. && rm -rf $(BUILD_DIR)); fi
-+@(cd $(BUILD_DIR) && $(PX4_MAKE) $(PX4_MAKE_ARGS) $(ARGS))
++@if [ $(PX4_CMAKE_GENERATOR) = "Ninja" ] && [ -e ./build_$@/Makefile ]; then rm -rf ./build_$@; fi
++@if [ ! -e ./build_$@/CMakeCache.txt ]; then Tools/check_submodules.sh && mkdir -p ./build_$@ && cd ./build_$@ && cmake $(2) -G$(PX4_CMAKE_GENERATOR) || (cd .. && rm -rf ./build_$@); fi
++@(cd ./build_$@ && $(PX4_MAKE) $(PX4_MAKE_ARGS) $(ARGS))
 endef
 
 # create empty targets to avoid msgs for targets passed to cmake
@@ -236,8 +233,14 @@ run_sitl_ros: sitl_deprecation
 # Other targets
 # --------------------------------------------------------------------
 
-.PHONY: uavcan_firmware check check_format unittest tests qgc_firmware package_firmware clean submodulesclean distclean
-.NOTPARALLEL: uavcan_firmware check check_format unittest tests qgc_firmware package_firmware clean submodulesclean distclean
+.PHONY: gazebo_build uavcan_firmware check check_format unittest tests qgc_firmware package_firmware clean submodulesclean distclean
+.NOTPARALLEL: gazebo_build uavcan_firmware check check_format unittest tests qgc_firmware package_firmware clean submodulesclean distclean
+
+gazebo_build:
+	@mkdir -p build_gazebo
+	@if [ ! -e ./build_gazebo/CMakeCache.txt ];then cd build_gazebo && cmake -Wno-dev -G$(PX4_CMAKE_GENERATOR) ../Tools/sitl_gazebo; fi
+	@cd build_gazebo && $(PX4_MAKE) $(PX4_MAKE_ARGS)
+	@cd build_gazebo && $(PX4_MAKE) $(PX4_MAKE_ARGS) sdf
 
 uavcan_firmware:
 ifeq ($(VECTORCONTROL),1)
@@ -299,7 +302,11 @@ unittest: posix_sitl_default
 	@(cd build_unittest && ctest -j2 --output-on-failure)
 
 run_tests_posix: posix_sitl_default
-	@(cd build_posix_sitl_default/ && ctest -V)
+	@mkdir -p build_posix_sitl_default/src/firmware/posix/rootfs/fs/microsd
+	@mkdir -p build_posix_sitl_default/src/firmware/posix/rootfs/eeprom
+	@touch build_posix_sitl_default/src/firmware/posix/rootfs/eeprom/parameters
+	@(cd build_posix_sitl_default/src/firmware/posix && ./px4 -d ../../../../posix-configs/SITL/init/rcS_tests | tee test_output)
+	@(cd build_posix_sitl_default/src/firmware/posix && grep --color=always "All tests passed" test_output)
 
 tests: check_unittest run_tests_posix
 
diff --git a/Tools/sitl_gazebo b/Tools/sitl_gazebo
index 9486c331a7..00dcabb1c2 160000
--- a/Tools/sitl_gazebo
+++ b/Tools/sitl_gazebo
@@ -1 +1 @@
-Subproject commit 9486c331a758fcadfaa314f6e9e44c36af5f243b
+Subproject commit 00dcabb1c2c001b40713903da1a4a7f71d01ef27
diff --git a/Tools/sitl_run.sh b/Tools/sitl_run.sh
index 8dfa2a62c6..6e467c31fd 100755
--- a/Tools/sitl_run.sh
+++ b/Tools/sitl_run.sh
@@ -1,29 +1,22 @@
 #!/bin/bash
 
-set -e
-
-echo args: $@
-
-sitl_bin=$1
-rc_script=$2
-debugger=$3
-program=$4
-model=$5
-src_path=$6
-build_path=$7
+rc_script=$1
+debugger=$2
+program=$3
+model=$4
+build_path=$5
+curr_dir=`pwd`
 
 echo SITL ARGS
-
-echo sitl_bin: $sitl_bin
 echo rc_script: $rc_script
 echo debugger: $debugger
 echo program: $program
 echo model: $model
-echo src_path: $src_path
 echo build_path: $build_path
 
-working_dir=`pwd`
-sitl_bin=$build_path/src/firmware/posix/px4
+mkdir -p $build_path/src/firmware/posix/rootfs/fs/microsd
+mkdir -p $build_path/src/firmware/posix/rootfs/eeprom
+touch $build_path/src/firmware/posix/rootfs/eeprom/parameters
 
 if [ "$chroot" == "1" ]
 then
@@ -42,36 +35,32 @@ fi
 
 if [ "$#" -lt 5 ]
 then
-	echo usage: sitl_run.sh rc_script debugger program model devel_path
+	echo usage: sitl_run.sh rc_script debugger program model build_path
 	echo ""
 	exit 1
 fi
 
-command_exists () {
-    type "$1" &> /dev/null ;
-}
-
 # kill process names that might stil
 # be running from last time
-pgrep gazebo && pkill gazebo
-pgrep px4 && pkill px4
-if command_exists jps
+pkill gazebo
+pkill px4
+jmavsim_pid=`jps | grep Simulator | cut -d" " -f1`
+if [ -n "$jmavsim_pid" ]
 then
-	jmavsim_pid=`jps | grep Simulator | cut -d" " -f1`
-	if [ -n "$jmavsim_pid" ]
-	then
-		kill $jmavsim_pid
-	fi
+	kill $jmavsim_pid
 fi
 
-cp $src_path/Tools/posix_lldbinit $working_dir/.lldbinit
-cp $src_path/Tools/posix.gdbinit $working_dir/.gdbinit
+set -e
+
+cd $build_path/..
+cp Tools/posix_lldbinit $build_path/src/firmware/posix/.lldbinit
+cp Tools/posix.gdbinit $build_path/src/firmware/posix/.gdbinit
 
 SIM_PID=0
 
 if [ "$program" == "jmavsim" ] && [ ! -n "$no_sim" ]
 then
-	cd $src_path/Tools/jMAVSim
+	cd Tools/jMAVSim
 	ant create_run_jar copy_res
 	cd out/production
 	java -Djava.ext.dirs= -jar jmavsim_run.jar -udp 127.0.0.1:14560 &
@@ -82,9 +71,15 @@ then
 	if [ -x "$(command -v gazebo)" ]
 	then
 		# Set the plugin path so Gazebo finds our model and sim
-		source /usr/share/gazebo/setup.sh
-		source $src_path/integrationtests/setup_gazebo_ros.bash ${src_path} ${build_path}
-		gzserver --verbose worlds/${model}.world &
+		export GAZEBO_PLUGIN_PATH=$curr_dir/build_gazebo:${GAZEBO_PLUGIN_PATH}
+		# Set the model path so Gazebo finds the airframes
+		export GAZEBO_MODEL_PATH=${GAZEBO_MODEL_PATH}:$curr_dir/Tools/sitl_gazebo/models
+		# The next line would disable online model lookup, can be commented in, in case of unstable behaviour.
+		# export GAZEBO_MODEL_DATABASE_URI=""
+		export SITL_GAZEBO_PATH=$curr_dir/Tools/sitl_gazebo
+		make --no-print-directory gazebo_build
+
+		gzserver --verbose $curr_dir/Tools/sitl_gazebo/worlds/${model}.world &
 		SIM_PID=`echo $!`
 
 		if [[ -n "$HEADLESS" ]]; then
@@ -103,17 +98,17 @@ then
 	# This is not a simulator, but a log file to replay
 
 	# Check if we need to creat a param file to allow user to change parameters
-	if ! [ -f "$rootfs/replay_params.txt" ]
+	if ! [ -f "${build_path}/src/firmware/posix/rootfs/replay_params.txt" ]
 		then
-		touch $rootfs/replay_params.txt
+		touch ${build_path}/src/firmware/posix/rootfs/replay_params.txt
 	fi
 fi
 
-cd $working_dir
+cd $build_path/src/firmware/posix
 
 if [ "$logfile" != "" ]
 then
-	cp $logfile $rootfs/replay.px4log
+	cp $logfile rootfs/replay.px4log
 fi
 
 # Do not exit on failure now from here on because we want the complete cleanup
@@ -122,18 +117,18 @@ set +e
 # Start Java simulator
 if [ "$debugger" == "lldb" ]
 then
-	lldb -- $sitl_bin $src_path $src_path/${rc_script}_${program}_${model}
+	lldb -- px4 ../../../../${rc_script}_${program}_${model}
 elif [ "$debugger" == "gdb" ]
 then
-	gdb --args $sitl_bin $src_path $src_path/${rc_script}_${program}_${model}
+	gdb --args px4 ../../../../${rc_script}_${program}_${model}
 elif [ "$debugger" == "ddd" ]
 then
-	ddd --debugger gdb --args px4 $src_path $src_path/${rc_script}_${program}_${model}
+	ddd --debugger gdb --args px4 ../../../../${rc_script}_${program}_${model}
 elif [ "$debugger" == "valgrind" ]
 then
-	valgrind $sitl_bin $src_path $src_path/${rc_script}_${program}_${model}
+	valgrind ./px4 ../../../../${rc_script}_${program}_${model}
 else
-	$sudo_enabled $sitl_bin $chroot_enabled $src_path $src_path/${rc_script}_${program}_${model}
+	$sudo_enabled ./px4 $chroot_enabled ../../../../${rc_script}_${program}_${model}
 fi
 
 if [ "$program" == "jmavsim" ]
diff --git a/cmake/common/px4_base.cmake b/cmake/common/px4_base.cmake
index 88f1e57f98..ef05520375 100644
--- a/cmake/common/px4_base.cmake
+++ b/cmake/common/px4_base.cmake
@@ -136,14 +136,14 @@ function(px4_add_git_submodule)
 		REQUIRED TARGET PATH
 		ARGN ${ARGN})
 	string(REPLACE "/" "_" NAME ${PATH})
-	add_custom_command(OUTPUT ${PX4_BINARY_DIR}/git_init_${NAME}.stamp
-		WORKING_DIRECTORY ${PX4_SOURCE_DIR}
-		COMMAND touch ${PX4_BINARY_DIR}/git_init_${NAME}.stamp
-		DEPENDS ${PX4_SOURCE_DIR}/.gitmodules
+	add_custom_command(OUTPUT ${CMAKE_BINARY_DIR}/git_init_${NAME}.stamp
+		WORKING_DIRECTORY ${CMAKE_SOURCE_DIR}
+		COMMAND touch ${CMAKE_BINARY_DIR}/git_init_${NAME}.stamp
+		DEPENDS ${CMAKE_SOURCE_DIR}/.gitmodules
 		)
 	add_custom_target(${TARGET}
-		WORKING_DIRECTORY ${PX4_SOURCE_DIR}
-		DEPENDS ${PX4_BINARY_DIR}/git_init_${NAME}.stamp
+		WORKING_DIRECTORY ${CMAKE_SOURCE_DIR}
+		DEPENDS ${CMAKE_BINARY_DIR}/git_init_${NAME}.stamp
 		)
 endfunction()
 
@@ -371,7 +371,7 @@ function(px4_generate_messages)
 	endif()
 
 	# headers
-	set(msg_out_path ${PX4_BINARY_DIR}/src/modules/uORB/topics)
+	set(msg_out_path ${CMAKE_BINARY_DIR}/src/modules/uORB/topics)
 	set(msg_list)
 	foreach(msg_file ${MSG_FILES})
 		get_filename_component(msg ${msg_file} NAME_WE)
@@ -389,15 +389,15 @@ function(px4_generate_messages)
 			-d msg
 			-o ${msg_out_path}
 			-e msg/templates/uorb
-			-t ${PX4_BINARY_DIR}/topics_temporary_header
+			-t ${CMAKE_BINARY_DIR}/topics_temporary_header
 		DEPENDS ${DEPENDS} ${MSG_FILES}
-		WORKING_DIRECTORY ${PX4_SOURCE_DIR}
+		WORKING_DIRECTORY ${CMAKE_SOURCE_DIR}
 		COMMENT "Generating uORB topic headers"
 		VERBATIM
 		)
 
 	# !sources
-	set(msg_source_out_path	${PX4_BINARY_DIR}/topics_sources)
+	set(msg_source_out_path	${CMAKE_BINARY_DIR}/topics_sources)
 	set(msg_source_files_out ${msg_source_out_path}/uORBTopics.cpp)
 	foreach(msg ${msg_list})
 		list(APPEND msg_source_files_out ${msg_source_out_path}/${msg}.cpp)
@@ -410,9 +410,9 @@ function(px4_generate_messages)
 			-d msg
 			-o ${msg_source_out_path}
 			-e msg/templates/uorb
-			-t ${PX4_BINARY_DIR}/topics_temporary_sources
+			-t ${CMAKE_BINARY_DIR}/topics_temporary_sources
 		DEPENDS ${DEPENDS} ${MSG_FILES}
-		WORKING_DIRECTORY ${PX4_SOURCE_DIR}
+		WORKING_DIRECTORY ${CMAKE_SOURCE_DIR}
 		COMMENT "Generating uORB topic sources"
 		VERBATIM
 		)
@@ -427,7 +427,7 @@ function(px4_generate_messages)
 
 	# multi messages for target OS
 	set(msg_multi_out_path
-		${PX4_BINARY_DIR}/src/platforms/${OS}/px4_messages)
+		${CMAKE_BINARY_DIR}/src/platforms/${OS}/px4_messages)
 	set(msg_multi_files_out)
 	foreach(msg ${msg_list})
 		list(APPEND msg_multi_files_out ${msg_multi_out_path}/px4_${msg}.h)
@@ -440,10 +440,10 @@ function(px4_generate_messages)
 			-d msg
 			-o ${msg_multi_out_path}
 			-e msg/templates/px4/uorb
-			-t ${PX4_BINARY_DIR}/multi_topics_temporary/${OS}
+			-t ${CMAKE_BINARY_DIR}/multi_topics_temporary/${OS}
 			-p "px4_"
 		DEPENDS ${DEPENDS} ${MSG_FILES}
-		WORKING_DIRECTORY ${PX4_SOURCE_DIR}
+		WORKING_DIRECTORY ${CMAKE_SOURCE_DIR}
 		COMMENT "Generating uORB topic multi headers for ${OS}"
 		VERBATIM
 		)
@@ -504,9 +504,9 @@ function(px4_add_upload)
 	px4_join(OUT serial_ports LIST "${serial_ports}" GLUE ",")
 	add_custom_target(${OUT}
 		COMMAND ${PYTHON_EXECUTABLE}
-			${PX4_SOURCE_DIR}/Tools/px_uploader.py --port ${serial_ports} ${BUNDLE}
+			${CMAKE_SOURCE_DIR}/Tools/px_uploader.py --port ${serial_ports} ${BUNDLE}
 		DEPENDS ${BUNDLE}
-		WORKING_DIRECTORY ${PX4_BINARY_DIR}
+		WORKING_DIRECTORY ${CMAKE_BINARY_DIR}
 		COMMENT "uploading ${BUNDLE}"
 		VERBATIM
 		USES_TERMINAL
@@ -523,9 +523,9 @@ function(px4_add_adb_push)
 		ARGN ${ARGN})
 
 	add_custom_target(${OUT}
-		COMMAND ${PX4_SOURCE_DIR}/Tools/adb_upload.sh ${FILES} ${DEST}
+		COMMAND ${CMAKE_SOURCE_DIR}/Tools/adb_upload.sh ${FILES} ${DEST}
 		DEPENDS ${DEPENDS}
-		WORKING_DIRECTORY ${PX4_BINARY_DIR}
+		WORKING_DIRECTORY ${CMAKE_BINARY_DIR}
 		COMMENT "uploading ${BUNDLE}"
 		VERBATIM
 		USES_TERMINAL
@@ -541,9 +541,9 @@ function(px4_add_adb_push_to_bebop)
 		ARGN ${ARGN})
 
 	add_custom_target(${OUT}
-		COMMAND ${PX4_SOURCE_DIR}/Tools/adb_upload_to_bebop.sh ${FILES} ${DEST}
+		COMMAND ${CMAKE_SOURCE_DIR}/Tools/adb_upload_to_bebop.sh ${FILES} ${DEST}
 		DEPENDS ${DEPENDS}
-		WORKING_DIRECTORY ${PX4_BINARY_DIR}
+		WORKING_DIRECTORY ${CMAKE_BINARY_DIR}
 		COMMENT "uploading ${BUNDLE}"
 		VERBATIM
 		USES_TERMINAL
@@ -559,9 +559,9 @@ function(px4_add_scp_push)
 		ARGN ${ARGN})
 
 	add_custom_target(${OUT}
-		COMMAND ${PX4_SOURCE_DIR}/Tools/scp_upload.sh ${FILES} ${DEST}
+		COMMAND ${CMAKE_SOURCE_DIR}/Tools/scp_upload.sh ${FILES} ${DEST}
 		DEPENDS ${DEPENDS}
-		WORKING_DIRECTORY ${PX4_BINARY_DIR}
+		WORKING_DIRECTORY ${CMAKE_BINARY_DIR}
 		COMMENT "uploading ${BUNDLE}"
 		VERBATIM
 		USES_TERMINAL
@@ -761,21 +761,21 @@ function(px4_add_common_flags)
 		)
 
 	set(added_include_dirs
-		${PX4_SOURCE_DIR}/src
-		${PX4_BINARY_DIR}
-		${PX4_BINARY_DIR}/src
-		${PX4_SOURCE_DIR}/src/modules
-		${PX4_SOURCE_DIR}/src/include
-		${PX4_SOURCE_DIR}/src/lib
-		${PX4_SOURCE_DIR}/src/platforms
+		${CMAKE_SOURCE_DIR}/src
+		${CMAKE_BINARY_DIR}
+		${CMAKE_BINARY_DIR}/src
+		${CMAKE_SOURCE_DIR}/src/modules
+		${CMAKE_SOURCE_DIR}/src/include
+		${CMAKE_SOURCE_DIR}/src/lib
+		${CMAKE_SOURCE_DIR}/src/platforms
 		# TODO Build/versioning was in Makefile,
 		# do we need this, how does it work with cmake
-		${PX4_SOURCE_DIR}/src/drivers/boards/${BOARD}
-		${PX4_BINARY_DIR}
-		${PX4_BINARY_DIR}/src/modules/px4_messages
-		${PX4_BINARY_DIR}/src/modules
-		${PX4_SOURCE_DIR}/mavlink/include/mavlink
-		${PX4_SOURCE_DIR}/src/lib/DriverFramework/framework/include
+		${CMAKE_SOURCE_DIR}/src/drivers/boards/${BOARD}
+		${CMAKE_BINARY_DIR}
+		${CMAKE_BINARY_DIR}/src/modules/px4_messages
+		${CMAKE_BINARY_DIR}/src/modules
+		${CMAKE_SOURCE_DIR}/mavlink/include/mavlink
+		${CMAKE_SOURCE_DIR}/src/lib/DriverFramework/framework/include
 		)
 
 	list(APPEND added_include_dirs
@@ -858,19 +858,19 @@ function(px4_create_git_hash_header)
 		COMMAND git describe --always --tags
 		OUTPUT_VARIABLE git_tag
 		OUTPUT_STRIP_TRAILING_WHITESPACE
-		WORKING_DIRECTORY ${PX4_SOURCE_DIR}
+		WORKING_DIRECTORY ${CMAKE_SOURCE_DIR}
 		)
 	message(STATUS "GIT_TAG = ${git_tag}")
 	execute_process(
 		COMMAND git rev-parse --verify HEAD
 		OUTPUT_VARIABLE git_version
 		OUTPUT_STRIP_TRAILING_WHITESPACE
-		WORKING_DIRECTORY ${PX4_SOURCE_DIR}
+		WORKING_DIRECTORY ${CMAKE_SOURCE_DIR}
 		)
 	#message(STATUS "GIT_VERSION = ${git_version}")
 	set(git_version_short)
 	string(SUBSTRING ${git_version} 1 16 git_version_short)
-	configure_file(${PX4_SOURCE_DIR}/cmake/templates/build_git_version.h.in ${HEADER} @ONLY)
+	configure_file(${CMAKE_SOURCE_DIR}/cmake/templates/build_git_version.h.in ${HEADER} @ONLY)
 endfunction()
 
 #=============================================================================
@@ -897,12 +897,12 @@ function(px4_generate_parameters_xml)
 		ONE_VALUE OUT BOARD
 		REQUIRED OUT BOARD
 		ARGN ${ARGN})
-	set(path ${PX4_SOURCE_DIR}/src)
+	set(path ${CMAKE_SOURCE_DIR}/src)
 	file(GLOB_RECURSE param_src_files
-		${PX4_SOURCE_DIR}/src/*params.c
+		${CMAKE_SOURCE_DIR}/src/*params.c
 		)
 	add_custom_command(OUTPUT ${OUT}
-		COMMAND ${PYTHON_EXECUTABLE} ${PX4_SOURCE_DIR}/Tools/px_process_params.py
+		COMMAND ${PYTHON_EXECUTABLE} ${CMAKE_SOURCE_DIR}/Tools/px_process_params.py
 			-s ${path} --board CONFIG_ARCH_${BOARD} --xml --inject-xml
 		DEPENDS ${param_src_files}
 		)
@@ -944,7 +944,7 @@ function(px4_generate_parameters_source)
 		set(SCOPE "")
 	endif()
 	add_custom_command(OUTPUT ${generated_files}
-		COMMAND ${PYTHON_EXECUTABLE} ${PX4_SOURCE_DIR}/Tools/px_generate_params.py ${XML} ${SCOPE}
+		COMMAND ${PYTHON_EXECUTABLE} ${CMAKE_SOURCE_DIR}/Tools/px_generate_params.py ${XML} ${SCOPE}
 		DEPENDS ${XML} ${DEPS} ${SCOPE}
 		)
 	set(${OUT} ${generated_files} PARENT_SCOPE)
@@ -975,10 +975,10 @@ function(px4_generate_airframes_xml)
 		ONE_VALUE OUT BOARD
 		REQUIRED OUT BOARD
 		ARGN ${ARGN})
-	set(process_airframes ${PX4_SOURCE_DIR}/Tools/px_process_airframes.py)
+	set(process_airframes ${CMAKE_SOURCE_DIR}/Tools/px_process_airframes.py)
 	add_custom_command(OUTPUT ${OUT}
 		COMMAND ${PYTHON_EXECUTABLE} ${process_airframes}
-			-a ${PX4_SOURCE_DIR}/ROMFS/px4fmu_common/init.d
+			-a ${CMAKE_SOURCE_DIR}/ROMFS/px4fmu_common/init.d
 			--board CONFIG_ARCH_BOARD_${BOARD} --xml
 		)
 	set(${OUT} ${${OUT}} PARENT_SCOPE)
diff --git a/cmake/configs/nuttx_mindpx-v2_default.cmake b/cmake/configs/nuttx_mindpx-v2_default.cmake
index 32bbf050fd..41c9d32df8 100644
--- a/cmake/configs/nuttx_mindpx-v2_default.cmake
+++ b/cmake/configs/nuttx_mindpx-v2_default.cmake
@@ -1,6 +1,6 @@
 include(nuttx/px4_impl_nuttx)
 
-set(CMAKE_TOOLCHAIN_FILE ${PX4_SOURCE_DIR}/cmake/toolchains/Toolchain-arm-none-eabi.cmake)
+set(CMAKE_TOOLCHAIN_FILE ${CMAKE_SOURCE_DIR}/cmake/toolchains/Toolchain-arm-none-eabi.cmake)
 
 set(config_uavcan_num_ifaces 2)
 
diff --git a/cmake/configs/nuttx_px4-stm32f4discovery_default.cmake b/cmake/configs/nuttx_px4-stm32f4discovery_default.cmake
index 377a90f335..c71db2fc31 100644
--- a/cmake/configs/nuttx_px4-stm32f4discovery_default.cmake
+++ b/cmake/configs/nuttx_px4-stm32f4discovery_default.cmake
@@ -1,6 +1,6 @@
 include(nuttx/px4_impl_nuttx)
 
-set(CMAKE_TOOLCHAIN_FILE ${PX4_SOURCE_DIR}/cmake/toolchains/Toolchain-arm-none-eabi.cmake)
+set(CMAKE_TOOLCHAIN_FILE ${CMAKE_SOURCE_DIR}/cmake/toolchains/Toolchain-arm-none-eabi.cmake)
 
 set(config_module_list
 	#
diff --git a/cmake/configs/nuttx_px4fmu-v1_default.cmake b/cmake/configs/nuttx_px4fmu-v1_default.cmake
index 2c0a5f7315..75f8d054ed 100644
--- a/cmake/configs/nuttx_px4fmu-v1_default.cmake
+++ b/cmake/configs/nuttx_px4fmu-v1_default.cmake
@@ -1,6 +1,6 @@
 include(nuttx/px4_impl_nuttx)
 
-set(CMAKE_TOOLCHAIN_FILE ${PX4_SOURCE_DIR}/cmake/toolchains/Toolchain-arm-none-eabi.cmake)
+set(CMAKE_TOOLCHAIN_FILE ${CMAKE_SOURCE_DIR}/cmake/toolchains/Toolchain-arm-none-eabi.cmake)
 
 set(config_module_list
 	#
diff --git a/cmake/configs/nuttx_px4fmu-v2_default.cmake b/cmake/configs/nuttx_px4fmu-v2_default.cmake
index 13d02e6d28..c38d726702 100644
--- a/cmake/configs/nuttx_px4fmu-v2_default.cmake
+++ b/cmake/configs/nuttx_px4fmu-v2_default.cmake
@@ -1,6 +1,6 @@
 include(nuttx/px4_impl_nuttx)
 
-set(CMAKE_TOOLCHAIN_FILE ${PX4_SOURCE_DIR}/cmake/toolchains/Toolchain-arm-none-eabi.cmake)
+set(CMAKE_TOOLCHAIN_FILE ${CMAKE_SOURCE_DIR}/cmake/toolchains/Toolchain-arm-none-eabi.cmake)
 
 set(config_uavcan_num_ifaces 2)
 
diff --git a/cmake/configs/nuttx_px4fmu-v2_ekf2.cmake b/cmake/configs/nuttx_px4fmu-v2_ekf2.cmake
index e500752214..f43b782045 100644
--- a/cmake/configs/nuttx_px4fmu-v2_ekf2.cmake
+++ b/cmake/configs/nuttx_px4fmu-v2_ekf2.cmake
@@ -1,6 +1,6 @@
 include(nuttx/px4_impl_nuttx)
 
-set(CMAKE_TOOLCHAIN_FILE ${PX4_SOURCE_DIR}/cmake/toolchains/Toolchain-arm-none-eabi.cmake)
+set(CMAKE_TOOLCHAIN_FILE ${CMAKE_SOURCE_DIR}/cmake/toolchains/Toolchain-arm-none-eabi.cmake)
 
 set(config_uavcan_num_ifaces 2)
 
diff --git a/cmake/configs/nuttx_px4fmu-v2_test.cmake b/cmake/configs/nuttx_px4fmu-v2_test.cmake
index 1839d8689d..b72c5a8190 100644
--- a/cmake/configs/nuttx_px4fmu-v2_test.cmake
+++ b/cmake/configs/nuttx_px4fmu-v2_test.cmake
@@ -1,6 +1,6 @@
 include(nuttx/px4_impl_nuttx)
 
-set(CMAKE_TOOLCHAIN_FILE ${PX4_SOURCE_DIR}/cmake/toolchains/Toolchain-arm-none-eabi.cmake)
+set(CMAKE_TOOLCHAIN_FILE ${CMAKE_SOURCE_DIR}/cmake/toolchains/Toolchain-arm-none-eabi.cmake)
 
 set(config_uavcan_num_ifaces 2)
 
diff --git a/cmake/configs/nuttx_px4fmu-v4_default.cmake b/cmake/configs/nuttx_px4fmu-v4_default.cmake
index ad746ee426..866e19dc0d 100644
--- a/cmake/configs/nuttx_px4fmu-v4_default.cmake
+++ b/cmake/configs/nuttx_px4fmu-v4_default.cmake
@@ -1,6 +1,6 @@
 include(nuttx/px4_impl_nuttx)
 
-set(CMAKE_TOOLCHAIN_FILE ${PX4_SOURCE_DIR}/cmake/toolchains/Toolchain-arm-none-eabi.cmake)
+set(CMAKE_TOOLCHAIN_FILE ${CMAKE_SOURCE_DIR}/cmake/toolchains/Toolchain-arm-none-eabi.cmake)
 
 set(config_uavcan_num_ifaces 1)
 
diff --git a/cmake/configs/nuttx_tap-v1_default.cmake b/cmake/configs/nuttx_tap-v1_default.cmake
index 8a5b8187fb..2dad824901 100644
--- a/cmake/configs/nuttx_tap-v1_default.cmake
+++ b/cmake/configs/nuttx_tap-v1_default.cmake
@@ -1,6 +1,6 @@
 include(nuttx/px4_impl_nuttx)
 
-set(CMAKE_TOOLCHAIN_FILE ${PX4_SOURCE_DIR}/cmake/toolchains/Toolchain-arm-none-eabi.cmake)
+set(CMAKE_TOOLCHAIN_FILE ${CMAKE_SOURCE_DIR}/cmake/toolchains/Toolchain-arm-none-eabi.cmake)
 
 set(config_module_list
 	#
diff --git a/cmake/configs/posix_bebop_default.cmake b/cmake/configs/posix_bebop_default.cmake
index 72e945db0f..5538ffe35a 100644
--- a/cmake/configs/posix_bebop_default.cmake
+++ b/cmake/configs/posix_bebop_default.cmake
@@ -1,6 +1,6 @@
 include(posix/px4_impl_posix)
 
-set(CMAKE_TOOLCHAIN_FILE ${PX4_SOURCE_DIR}/cmake/toolchains/Toolchain-arm-linux-gnueabihf-raspbian.cmake)
+set(CMAKE_TOOLCHAIN_FILE ${CMAKE_SOURCE_DIR}/cmake/toolchains/Toolchain-arm-linux-gnueabihf-raspbian.cmake)
 
 add_definitions(
   -D__PX4_POSIX_BEBOP
diff --git a/cmake/configs/posix_eagle_default.cmake b/cmake/configs/posix_eagle_default.cmake
index be81961425..3a6dd22755 100644
--- a/cmake/configs/posix_eagle_default.cmake
+++ b/cmake/configs/posix_eagle_default.cmake
@@ -4,9 +4,9 @@
 # on the Linux side of the Snapdragon.
 include(configs/posix_sdflight_default)
 
-set(CMAKE_TOOLCHAIN_FILE ${PX4_SOURCE_DIR}/cmake/cmake_hexagon/toolchain/Toolchain-arm-linux-gnueabihf.cmake)
+set(CMAKE_TOOLCHAIN_FILE ${CMAKE_SOURCE_DIR}/cmake/cmake_hexagon/toolchain/Toolchain-arm-linux-gnueabihf.cmake)
 
-set(CMAKE_MODULE_PATH ${CMAKE_MODULE_PATH} "${PX4_SOURCE_DIR}/cmake/cmake_hexagon")
+set(CMAKE_MODULE_PATH ${CMAKE_MODULE_PATH} "${CMAKE_SOURCE_DIR}/cmake/cmake_hexagon")
 
 set(CONFIG_SHMEM "1")
 
diff --git a/cmake/configs/posix_eagle_hil.cmake b/cmake/configs/posix_eagle_hil.cmake
index d38da131bd..909bdeecb1 100644
--- a/cmake/configs/posix_eagle_hil.cmake
+++ b/cmake/configs/posix_eagle_hil.cmake
@@ -1,13 +1,13 @@
 include(posix/px4_impl_posix)
 
-set(CMAKE_MODULE_PATH ${CMAKE_MODULE_PATH} "${PX4_SOURCE_DIR}/cmake/cmake_hexagon")
+set(CMAKE_MODULE_PATH ${CMAKE_MODULE_PATH} "${CMAKE_SOURCE_DIR}/cmake/cmake_hexagon")
 
 # Use build stubs unless explicitly set not to
 if("${DSPAL_STUBS_ENABLE}" STREQUAL "")
 	set(DSPAL_STUBS_ENABLE "1")
 endif()
 
-set(CMAKE_TOOLCHAIN_FILE ${PX4_SOURCE_DIR}/cmake/toolchains/Toolchain-arm-linux-gnueabihf.cmake)
+set(CMAKE_TOOLCHAIN_FILE ${CMAKE_SOURCE_DIR}/cmake/toolchains/Toolchain-arm-linux-gnueabihf.cmake)
 
 set(config_generate_parameters_scope ALL)
 
diff --git a/cmake/configs/posix_eagle_legacy_driver_default.cmake b/cmake/configs/posix_eagle_legacy_driver_default.cmake
index 534d327f7e..ec172e7602 100644
--- a/cmake/configs/posix_eagle_legacy_driver_default.cmake
+++ b/cmake/configs/posix_eagle_legacy_driver_default.cmake
@@ -1,8 +1,8 @@
 include(posix/px4_impl_posix)
 
-set(CMAKE_TOOLCHAIN_FILE ${PX4_SOURCE_DIR}/cmake/cmake_hexagon/toolchain/Toolchain-arm-linux-gnueabihf.cmake)
+set(CMAKE_TOOLCHAIN_FILE ${CMAKE_SOURCE_DIR}/cmake/cmake_hexagon/toolchain/Toolchain-arm-linux-gnueabihf.cmake)
 
-set(CMAKE_MODULE_PATH ${CMAKE_MODULE_PATH} "${PX4_SOURCE_DIR}/cmake/cmake_hexagon")
+set(CMAKE_MODULE_PATH ${CMAKE_MODULE_PATH} "${CMAKE_SOURCE_DIR}/cmake/cmake_hexagon")
 
 set(config_generate_parameters_scope ALL)
 
diff --git a/cmake/configs/posix_eagle_muorb.cmake b/cmake/configs/posix_eagle_muorb.cmake
index f47c421dce..95a5c59beb 100644
--- a/cmake/configs/posix_eagle_muorb.cmake
+++ b/cmake/configs/posix_eagle_muorb.cmake
@@ -1,8 +1,8 @@
 include(posix/px4_impl_posix)
 
-set(CMAKE_TOOLCHAIN_FILE ${PX4_SOURCE_DIR}/cmake/toolchains/Toolchain-arm-linux-gnueabihf.cmake)
+set(CMAKE_TOOLCHAIN_FILE ${CMAKE_SOURCE_DIR}/cmake/toolchains/Toolchain-arm-linux-gnueabihf.cmake)
 
-set(CMAKE_MODULE_PATH ${CMAKE_MODULE_PATH} "${PX4_SOURCE_DIR}/cmake/cmake_hexagon")
+set(CMAKE_MODULE_PATH ${CMAKE_MODULE_PATH} "${CMAKE_SOURCE_DIR}/cmake/cmake_hexagon")
 
 set(config_generate_parameters_scope ALL)
 
diff --git a/cmake/configs/posix_rpi_cross.cmake b/cmake/configs/posix_rpi_cross.cmake
index 72896086c6..46243fc361 100644
--- a/cmake/configs/posix_rpi_cross.cmake
+++ b/cmake/configs/posix_rpi_cross.cmake
@@ -1,6 +1,6 @@
 include(configs/posix_rpi_common)
 
-set(CMAKE_TOOLCHAIN_FILE ${PX4_SOURCE_DIR}/cmake/toolchains/Toolchain-arm-linux-gnueabihf-raspbian.cmake)
+set(CMAKE_TOOLCHAIN_FILE ${CMAKE_SOURCE_DIR}/cmake/toolchains/Toolchain-arm-linux-gnueabihf-raspbian.cmake)
 
 set(CMAKE_PROGRAM_PATH
 	"${RPI_TOOLCHAIN_DIR}/gcc-linaro-arm-linux-gnueabihf-raspbian/bin"
diff --git a/cmake/configs/posix_rpi_native.cmake b/cmake/configs/posix_rpi_native.cmake
index b2a54c4f65..e58f370282 100644
--- a/cmake/configs/posix_rpi_native.cmake
+++ b/cmake/configs/posix_rpi_native.cmake
@@ -1,3 +1,3 @@
 include(configs/posix_rpi_common)
 
-set(CMAKE_TOOLCHAIN_FILE ${PX4_SOURCE_DIR}/cmake/toolchains/Toolchain-native.cmake)
+set(CMAKE_TOOLCHAIN_FILE ${CMAKE_SOURCE_DIR}/cmake/toolchains/Toolchain-native.cmake)
diff --git a/cmake/configs/posix_sdflight_default.cmake b/cmake/configs/posix_sdflight_default.cmake
index fc66327184..c43e51974c 100644
--- a/cmake/configs/posix_sdflight_default.cmake
+++ b/cmake/configs/posix_sdflight_default.cmake
@@ -1,8 +1,8 @@
 include(posix/px4_impl_posix)
 
-set(CMAKE_TOOLCHAIN_FILE ${PX4_SOURCE_DIR}/cmake/toolchains/Toolchain-arm-linux-gnueabihf.cmake)
+set(CMAKE_TOOLCHAIN_FILE ${CMAKE_SOURCE_DIR}/cmake/toolchains/Toolchain-arm-linux-gnueabihf.cmake)
 
-set(CMAKE_MODULE_PATH ${CMAKE_MODULE_PATH} "${PX4_SOURCE_DIR}/cmake/cmake_hexagon")
+set(CMAKE_MODULE_PATH ${CMAKE_MODULE_PATH} "${CMAKE_SOURCE_DIR}/cmake/cmake_hexagon")
 
 set(config_generate_parameters_scope ALL)
 
diff --git a/cmake/configs/posix_sitl_broadcast.cmake b/cmake/configs/posix_sitl_broadcast.cmake
index 0c7bf4b0a2..9a9029f1c0 100644
--- a/cmake/configs/posix_sitl_broadcast.cmake
+++ b/cmake/configs/posix_sitl_broadcast.cmake
@@ -1,6 +1,6 @@
 include(posix/px4_impl_posix)
 
-set(CMAKE_TOOLCHAIN_FILE ${PX4_SOURCE_DIR}/cmake/toolchains/Toolchain-native.cmake)
+set(CMAKE_TOOLCHAIN_FILE ${CMAKE_SOURCE_DIR}/cmake/toolchains/Toolchain-native.cmake)
 
 set(config_module_list
 	drivers/device
diff --git a/cmake/configs/posix_sitl_default.cmake b/cmake/configs/posix_sitl_default.cmake
index 62caa65e72..6fa6d9c0b7 100644
--- a/cmake/configs/posix_sitl_default.cmake
+++ b/cmake/configs/posix_sitl_default.cmake
@@ -1,6 +1,6 @@
 include(posix/px4_impl_posix)
 
-set(CMAKE_TOOLCHAIN_FILE ${PX4_SOURCE_DIR}/cmake/toolchains/Toolchain-native.cmake)
+set(CMAKE_TOOLCHAIN_FILE ${CMAKE_SOURCE_DIR}/cmake/toolchains/Toolchain-native.cmake)
 
 set(config_module_list
 	drivers/boards/sitl
diff --git a/cmake/configs/posix_sitl_replay.cmake b/cmake/configs/posix_sitl_replay.cmake
index a20a2d8cfe..3c99cba7a7 100644
--- a/cmake/configs/posix_sitl_replay.cmake
+++ b/cmake/configs/posix_sitl_replay.cmake
@@ -1,6 +1,6 @@
 include(posix/px4_impl_posix)
 
-set(CMAKE_TOOLCHAIN_FILE ${PX4_SOURCE_DIR}/cmake/toolchains/Toolchain-native.cmake)
+set(CMAKE_TOOLCHAIN_FILE ${CMAKE_SOURCE_DIR}/cmake/toolchains/Toolchain-native.cmake)
 
 set(config_module_list
 	drivers/device
diff --git a/cmake/configs/qurt_eagle_hello.cmake b/cmake/configs/qurt_eagle_hello.cmake
index db16c17d2a..e1a66ea827 100644
--- a/cmake/configs/qurt_eagle_hello.cmake
+++ b/cmake/configs/qurt_eagle_hello.cmake
@@ -6,9 +6,9 @@ else()
 	set(HEXAGON_SDK_ROOT $ENV{HEXAGON_SDK_ROOT})
 endif()
 
-set(CMAKE_TOOLCHAIN_FILE ${PX4_SOURCE_DIR}/cmake/cmake_hexagon/toolchain/Toolchain-qurt.cmake)
+set(CMAKE_TOOLCHAIN_FILE ${CMAKE_SOURCE_DIR}/cmake/cmake_hexagon/toolchain/Toolchain-qurt.cmake)
 
-set(CMAKE_MODULE_PATH ${CMAKE_MODULE_PATH} "${PX4_SOURCE_DIR}/cmake/cmake_hexagon")
+set(CMAKE_MODULE_PATH ${CMAKE_MODULE_PATH} "${CMAKE_SOURCE_DIR}/cmake/cmake_hexagon")
 
 set(config_generate_parameters_scope ALL)
 
diff --git a/cmake/configs/qurt_eagle_hil.cmake b/cmake/configs/qurt_eagle_hil.cmake
index 9eba5dea83..469432818f 100644
--- a/cmake/configs/qurt_eagle_hil.cmake
+++ b/cmake/configs/qurt_eagle_hil.cmake
@@ -6,9 +6,9 @@ else()
 	set(HEXAGON_SDK_ROOT $ENV{HEXAGON_SDK_ROOT})
 endif()
 
-set(CMAKE_TOOLCHAIN_FILE ${PX4_SOURCE_DIR}/cmake/cmake_hexagon/toolchain/Toolchain-qurt.cmake)
+set(CMAKE_TOOLCHAIN_FILE ${CMAKE_SOURCE_DIR}/cmake/cmake_hexagon/toolchain/Toolchain-qurt.cmake)
 
-set(CMAKE_MODULE_PATH ${CMAKE_MODULE_PATH} "${PX4_SOURCE_DIR}/cmake/cmake_hexagon")
+set(CMAKE_MODULE_PATH ${CMAKE_MODULE_PATH} "${CMAKE_SOURCE_DIR}/cmake/cmake_hexagon")
 
 set(config_generate_parameters_scope ALL)
 
diff --git a/cmake/configs/qurt_eagle_legacy_driver_default.cmake b/cmake/configs/qurt_eagle_legacy_driver_default.cmake
index 852fe55b1f..f0fdb2dee5 100644
--- a/cmake/configs/qurt_eagle_legacy_driver_default.cmake
+++ b/cmake/configs/qurt_eagle_legacy_driver_default.cmake
@@ -10,9 +10,9 @@ set(CONFIG_SHMEM "1")
 
 set(config_generate_parameters_scope ALL)
 
-set(CMAKE_TOOLCHAIN_FILE ${PX4_SOURCE_DIR}/cmake/cmake_hexagon/toolchain/Toolchain-qurt.cmake)
+set(CMAKE_TOOLCHAIN_FILE ${CMAKE_SOURCE_DIR}/cmake/cmake_hexagon/toolchain/Toolchain-qurt.cmake)
 
-set(CMAKE_MODULE_PATH ${CMAKE_MODULE_PATH} "${PX4_SOURCE_DIR}/cmake/cmake_hexagon")
+set(CMAKE_MODULE_PATH ${CMAKE_MODULE_PATH} "${CMAKE_SOURCE_DIR}/cmake/cmake_hexagon")
 
 add_definitions(
    -D__USING_SNAPDRAGON_LEGACY_DRIVER
diff --git a/cmake/configs/qurt_eagle_muorb.cmake b/cmake/configs/qurt_eagle_muorb.cmake
index 2af8ebdfb2..505ffd00cd 100644
--- a/cmake/configs/qurt_eagle_muorb.cmake
+++ b/cmake/configs/qurt_eagle_muorb.cmake
@@ -6,9 +6,9 @@ else()
 	set(HEXAGON_SDK_ROOT $ENV{HEXAGON_SDK_ROOT})
 endif()
 
-set(CMAKE_TOOLCHAIN_FILE ${PX4_SOURCE_DIR}/cmake/cmake_hexagon/toolchain/Toolchain-qurt.cmake)
+set(CMAKE_TOOLCHAIN_FILE ${CMAKE_SOURCE_DIR}/cmake/cmake_hexagon/toolchain/Toolchain-qurt.cmake)
 
-set(CMAKE_MODULE_PATH ${CMAKE_MODULE_PATH} "${PX4_SOURCE_DIR}/cmake/cmake_hexagon")
+set(CMAKE_MODULE_PATH ${CMAKE_MODULE_PATH} "${CMAKE_SOURCE_DIR}/cmake/cmake_hexagon")
 
 set(config_generate_parameters_scope ALL)
 
diff --git a/cmake/configs/qurt_eagle_test.cmake b/cmake/configs/qurt_eagle_test.cmake
index 9131478577..9170b3da47 100644
--- a/cmake/configs/qurt_eagle_test.cmake
+++ b/cmake/configs/qurt_eagle_test.cmake
@@ -6,8 +6,8 @@ else()
 	set(HEXAGON_SDK_ROOT $ENV{HEXAGON_SDK_ROOT})
 endif()
 
-set(CMAKE_TOOLCHAIN_FILE ${PX4_SOURCE_DIR}/cmake/cmake_hexagon/toolchain/Toolchain-qurt.cmake)
-set(CMAKE_MODULE_PATH ${CMAKE_MODULE_PATH} "${PX4_SOURCE_DIR}/cmake/cmake_hexagon")
+set(CMAKE_TOOLCHAIN_FILE ${CMAKE_SOURCE_DIR}/cmake/cmake_hexagon/toolchain/Toolchain-qurt.cmake)
+set(CMAKE_MODULE_PATH ${CMAKE_MODULE_PATH} "${CMAKE_SOURCE_DIR}/cmake/cmake_hexagon")
 
 set(config_generate_parameters_scope ALL)
 
diff --git a/cmake/configs/qurt_eagle_travis.cmake b/cmake/configs/qurt_eagle_travis.cmake
index 505635b8bd..16aa788edc 100644
--- a/cmake/configs/qurt_eagle_travis.cmake
+++ b/cmake/configs/qurt_eagle_travis.cmake
@@ -5,9 +5,9 @@ set(CONFIG_SHMEM "1")
 # Run a full link with build stubs to make sure qurt target isn't broken
 set(QURT_ENABLE_STUBS "1")
 
-set(CMAKE_TOOLCHAIN_FILE ${PX4_SOURCE_DIR}/cmake/cmake_hexagon/toolchain/Toolchain-qurt.cmake)
+set(CMAKE_TOOLCHAIN_FILE ${CMAKE_SOURCE_DIR}/cmake/cmake_hexagon/toolchain/Toolchain-qurt.cmake)
 
-set(CMAKE_MODULE_PATH ${CMAKE_MODULE_PATH} "${PX4_SOURCE_DIR}/cmake/cmake_hexagon")
+set(CMAKE_MODULE_PATH ${CMAKE_MODULE_PATH} "${CMAKE_SOURCE_DIR}/cmake/cmake_hexagon")
 include(hexagon_sdk)
 
 if ("$ENV{HEXAGON_SDK_ROOT}" STREQUAL "")
diff --git a/cmake/configs/qurt_sdflight_default.cmake b/cmake/configs/qurt_sdflight_default.cmake
index 1d30699014..195219f7af 100644
--- a/cmake/configs/qurt_sdflight_default.cmake
+++ b/cmake/configs/qurt_sdflight_default.cmake
@@ -10,9 +10,9 @@ set(CONFIG_SHMEM "1")
 
 set(config_generate_parameters_scope ALL)
 
-set(CMAKE_TOOLCHAIN_FILE ${PX4_SOURCE_DIR}/cmake/cmake_hexagon/toolchain/Toolchain-qurt.cmake)
+set(CMAKE_TOOLCHAIN_FILE ${CMAKE_SOURCE_DIR}/cmake/cmake_hexagon/toolchain/Toolchain-qurt.cmake)
 
-set(CMAKE_MODULE_PATH ${CMAKE_MODULE_PATH} "${PX4_SOURCE_DIR}/cmake/cmake_hexagon")
+set(CMAKE_MODULE_PATH ${CMAKE_MODULE_PATH} "${CMAKE_SOURCE_DIR}/cmake/cmake_hexagon")
 
 set(config_module_list
 	#
diff --git a/cmake/nuttx/px4_impl_nuttx.cmake b/cmake/nuttx/px4_impl_nuttx.cmake
index 9a54b6277e..c969263be1 100644
--- a/cmake/nuttx/px4_impl_nuttx.cmake
+++ b/cmake/nuttx/px4_impl_nuttx.cmake
@@ -95,9 +95,9 @@ function(px4_nuttx_add_firmware)
 
 	add_custom_command(OUTPUT ${OUT}
 		COMMAND ${OBJCOPY} -O binary ${EXE} ${EXE}.bin
-		COMMAND ${PYTHON_EXECUTABLE} ${PX4_SOURCE_DIR}/Tools/px_mkfw.py
-			--prototype ${PX4_SOURCE_DIR}/Images/${BOARD}.prototype
-			--git_identity ${PX4_SOURCE_DIR}
+		COMMAND ${PYTHON_EXECUTABLE} ${CMAKE_SOURCE_DIR}/Tools/px_mkfw.py
+			--prototype ${CMAKE_SOURCE_DIR}/Images/${BOARD}.prototype
+			--git_identity ${CMAKE_SOURCE_DIR}
 			${extra_args}
 			--image ${EXE}.bin > ${OUT}
 		DEPENDS ${EXE}
@@ -148,7 +148,7 @@ function(px4_nuttx_generate_builtin_commands)
 			math(EXPR command_count "${command_count}+1")
 		endif()
 	endforeach()
-	configure_file(${PX4_SOURCE_DIR}/cmake/nuttx/builtin_commands.c.in
+	configure_file(${CMAKE_SOURCE_DIR}/cmake/nuttx/builtin_commands.c.in
 		${OUT})
 endfunction()
 
@@ -183,17 +183,17 @@ function(px4_nuttx_add_export)
 		REQUIRED OUT CONFIG THREADS
 		ARGN ${ARGN})
 
-	set(nuttx_src ${PX4_BINARY_DIR}/${CONFIG}/NuttX)
+	set(nuttx_src ${CMAKE_BINARY_DIR}/${CONFIG}/NuttX)
 
 	# patch
 	add_custom_target(__nuttx_patch_${CONFIG})
-	file(GLOB nuttx_patches RELATIVE ${PX4_SOURCE_DIR}
-	    ${PX4_SOURCE_DIR}/nuttx-patches/*.patch)
+	file(GLOB nuttx_patches RELATIVE ${CMAKE_SOURCE_DIR}
+	    ${CMAKE_SOURCE_DIR}/nuttx-patches/*.patch)
 	foreach(patch ${nuttx_patches})
 		string(REPLACE "/" "_" patch_name "${patch}-${CONFIG}")
 	    message(STATUS "nuttx-patch: ${patch}")
 		add_custom_command(OUTPUT nuttx_patch_${patch_name}.stamp
-			COMMAND ${PATCH} -p0 -N  < ${PX4_SOURCE_DIR}/${patch}
+			COMMAND ${PATCH} -p0 -N  < ${CMAKE_SOURCE_DIR}/${patch}
 			COMMAND ${TOUCH} nuttx_patch_${patch_name}.stamp
 			DEPENDS ${DEPENDS}
 			)
@@ -204,7 +204,7 @@ function(px4_nuttx_add_export)
 
 	# Read defconfig to see if CONFIG_ARMV7M_STACKCHECK is yes 
 	# note: CONFIG will be BOARD in the future evaluation of ${hw_stack_check_${CONFIG}
-	file(STRINGS "${PX4_SOURCE_DIR}/nuttx-configs/${CONFIG}/nsh/defconfig"
+	file(STRINGS "${CMAKE_SOURCE_DIR}/nuttx-configs/${CONFIG}/nsh/defconfig"
 		hw_stack_check_${CONFIG}
 		REGEX "CONFIG_ARMV7M_STACKCHECK=y"
 		)
@@ -213,29 +213,29 @@ function(px4_nuttx_add_export)
 	endif()
 
 	# copy and export
-	file(RELATIVE_PATH nuttx_cp_src ${PX4_BINARY_DIR} ${PX4_SOURCE_DIR}/NuttX)
-	file(GLOB_RECURSE config_files ${PX4_SOURCE_DIR}/nuttx-configs/${CONFIG}/*)
-	add_custom_command(OUTPUT ${PX4_BINARY_DIR}/${CONFIG}.export
+	file(RELATIVE_PATH nuttx_cp_src ${CMAKE_BINARY_DIR} ${CMAKE_SOURCE_DIR}/NuttX)
+	file(GLOB_RECURSE config_files ${CMAKE_SOURCE_DIR}/nuttx-configs/${CONFIG}/*)
+	add_custom_command(OUTPUT ${CMAKE_BINARY_DIR}/${CONFIG}.export
 		COMMAND ${MKDIR} -p ${nuttx_src}
 		COMMAND rsync -a --delete --exclude=.git ${nuttx_cp_src}/ ${CONFIG}/NuttX/
 		#COMMAND ${ECHO} Configuring NuttX for ${CONFIG}
 		COMMAND ${MAKE} --no-print-directory -C${nuttx_src}/nuttx -r --quiet distclean
-		COMMAND ${CP} -r ${PX4_SOURCE_DIR}/nuttx-configs/PX4_Warnings.mk ${nuttx_src}/nuttx/
-		COMMAND ${CP} -r ${PX4_SOURCE_DIR}/nuttx-configs/${CONFIG} ${nuttx_src}/nuttx/configs
+		COMMAND ${CP} -r ${CMAKE_SOURCE_DIR}/nuttx-configs/PX4_Warnings.mk ${nuttx_src}/nuttx/
+		COMMAND ${CP} -r ${CMAKE_SOURCE_DIR}/nuttx-configs/${CONFIG} ${nuttx_src}/nuttx/configs
 		COMMAND cd ${nuttx_src}/nuttx/tools && ./configure.sh ${CONFIG}/nsh && cd ..
 		#COMMAND ${ECHO} Exporting NuttX for ${CONFIG}
 		COMMAND ${MAKE} --no-print-directory --quiet -C ${nuttx_src}/nuttx -j${THREADS} -r CONFIG_ARCH_BOARD=${CONFIG} export > nuttx_build.log
-		COMMAND ${CP} -r ${nuttx_src}/nuttx/nuttx-export.zip ${PX4_BINARY_DIR}/${CONFIG}.export
+		COMMAND ${CP} -r ${nuttx_src}/nuttx/nuttx-export.zip ${CMAKE_BINARY_DIR}/${CONFIG}.export
 		DEPENDS ${config_files} ${DEPENDS}
-		WORKING_DIRECTORY ${PX4_BINARY_DIR}
+		WORKING_DIRECTORY ${CMAKE_BINARY_DIR}
 		COMMENT "Building NuttX for ${CONFIG}")
 
 	# extract
 	add_custom_command(OUTPUT nuttx_export_${CONFIG}.stamp
 		COMMAND ${RM} -rf ${nuttx_src}/nuttx-export
-		COMMAND ${UNZIP} -q ${PX4_BINARY_DIR}/${CONFIG}.export -d ${nuttx_src}
+		COMMAND ${UNZIP} -q ${CMAKE_BINARY_DIR}/${CONFIG}.export -d ${nuttx_src}
 		COMMAND ${TOUCH} nuttx_export_${CONFIG}.stamp
-		DEPENDS ${DEPENDS} ${PX4_BINARY_DIR}/${CONFIG}.export)
+		DEPENDS ${DEPENDS} ${CMAKE_BINARY_DIR}/${CONFIG}.export)
 
 	add_custom_target(${OUT}
 		DEPENDS nuttx_export_${CONFIG}.stamp)
@@ -308,16 +308,16 @@ function(px4_nuttx_add_romfs)
 		REQUIRED OUT ROOT
 		ARGN ${ARGN})
 
-	set(romfs_temp_dir ${PX4_BINARY_DIR}/tmp/${ROOT})
-	set(romfs_src_dir ${PX4_SOURCE_DIR}/${ROOT})
-	set(romfs_autostart ${PX4_SOURCE_DIR}/Tools/px_process_airframes.py)
-	set(romfs_pruner ${PX4_SOURCE_DIR}/Tools/px_romfs_pruner.py)
-	set(bin_to_obj ${PX4_SOURCE_DIR}/cmake/nuttx/bin_to_obj.py)
+	set(romfs_temp_dir ${CMAKE_BINARY_DIR}/tmp/${ROOT})
+	set(romfs_src_dir ${CMAKE_SOURCE_DIR}/${ROOT})
+	set(romfs_autostart ${CMAKE_SOURCE_DIR}/Tools/px_process_airframes.py)
+	set(romfs_pruner ${CMAKE_SOURCE_DIR}/Tools/px_romfs_pruner.py)
+	set(bin_to_obj ${CMAKE_SOURCE_DIR}/cmake/nuttx/bin_to_obj.py)
 	set(extras_dir ${CMAKE_CURRENT_BINARY_DIR}/extras)
 
 	file(GLOB_RECURSE romfs_src_files ${romfs_src_dir} ${romfs_src_dir}/*)
 
-	set(cmake_test ${PX4_SOURCE_DIR}/cmake/test/cmake_tester.py)
+	set(cmake_test ${CMAKE_SOURCE_DIR}/cmake/test/cmake_tester.py)
 
 	
 	set(extras)
@@ -346,7 +346,7 @@ function(px4_nuttx_add_romfs)
 		#COMMAND cmake -E remove_directory ${romfs_temp_dir}
 		COMMAND ${PYTHON_EXECUTABLE} ${bin_to_obj}
 			--ld ${LD} --c_flags ${CMAKE_C_FLAGS}
-			--include_path "${PX4_SOURCE_DIR}/src/include"
+			--include_path "${CMAKE_SOURCE_DIR}/src/include"
 			--c_compiler ${CMAKE_C_COMPILER}
 			--nm ${NM} --objcopy ${OBJCOPY}
 			--obj romfs.o
@@ -414,7 +414,7 @@ function(px4_os_add_flags)
 		LINK_DIRS ${LINK_DIRS}
 		DEFINITIONS ${DEFINITIONS})
 
-	set(nuttx_export_dir ${PX4_BINARY_DIR}/${BOARD}/NuttX/nuttx-export)
+	set(nuttx_export_dir ${CMAKE_BINARY_DIR}/${BOARD}/NuttX/nuttx-export)
 	set(added_include_dirs
 		${nuttx_export_dir}/include
 		${nuttx_export_dir}/include/cxx
diff --git a/cmake/posix/px4_impl_posix.cmake b/cmake/posix/px4_impl_posix.cmake
index 7786097c4c..d661bfa5eb 100644
--- a/cmake/posix/px4_impl_posix.cmake
+++ b/cmake/posix/px4_impl_posix.cmake
@@ -49,7 +49,7 @@
 #
 
 include(common/px4_base)
-list(APPEND CMAKE_MODULE_PATH ${PX4_SOURCE_DIR}/cmake/posix)
+list(APPEND CMAKE_MODULE_PATH ${CMAKE_SOURCE_DIR}/cmake/posix)
 
 #=============================================================================
 #
@@ -101,7 +101,7 @@ function(px4_posix_generate_builtin_commands)
 			math(EXPR command_count "${command_count}+1")
 		endif()
 	endforeach()
-	configure_file(${PX4_SOURCE_DIR}/cmake/posix/apps.h_in
+	configure_file(${CMAKE_SOURCE_DIR}/cmake/posix/apps.h_in
 		${OUT})
 endfunction()
 
diff --git a/cmake/qurt/px4_impl_qurt.cmake b/cmake/qurt/px4_impl_qurt.cmake
index f5387c515e..efb1135d7b 100644
--- a/cmake/qurt/px4_impl_qurt.cmake
+++ b/cmake/qurt/px4_impl_qurt.cmake
@@ -50,7 +50,7 @@
 #
 
 include(common/px4_base)
-list(APPEND CMAKE_MODULE_PATH ${PX4_SOURCE_DIR}/cmake/qurt)
+list(APPEND CMAKE_MODULE_PATH ${CMAKE_SOURCE_DIR}/cmake/qurt)
 
 #=============================================================================
 #
@@ -95,7 +95,7 @@ function(px4_qurt_generate_builtin_commands)
 			math(EXPR command_count "${command_count}+1")
 		endif()
 	endforeach()
-	configure_file(${PX4_SOURCE_DIR}/cmake/qurt/apps.h_in ${OUT})
+	configure_file(${CMAKE_SOURCE_DIR}/cmake/qurt/apps.h_in ${OUT})
 endfunction()
 
 #=============================================================================
diff --git a/cmake/ros-CMakeLists.txt b/cmake/ros-CMakeLists.txt
new file mode 100644
index 0000000000..67870b1e2c
--- /dev/null
+++ b/cmake/ros-CMakeLists.txt
@@ -0,0 +1,334 @@
+cmake_minimum_required(VERSION 2.8.3)
+project(px4)
+set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11")
+add_definitions(-D__PX4_ROS)
+add_definitions(-D__EXPORT=)
+add_definitions(-DMAVLINK_DIALECT=common)
+
+## Find catkin macros and libraries
+## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
+## is used, also find other catkin packages
+find_package(catkin REQUIRED COMPONENTS
+  roscpp
+  rospy
+  std_msgs
+  geometry_msgs
+  message_generation
+  cmake_modules
+  gazebo_msgs
+  sensor_msgs
+  mav_msgs
+  libmavconn
+  tf
+)
+find_package(Eigen REQUIRED)
+
+## System dependencies are found with CMake's conventions
+# find_package(Boost REQUIRED COMPONENTS system)
+
+
+## Uncomment this if the package has a setup.py. This macro ensures
+## modules and global scripts declared therein get installed
+## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html
+# catkin_python_setup()
+
+################################################
+## Declare ROS messages, services and actions ##
+################################################
+
+## To declare and build messages, services or actions from within this
+## package, follow these steps:
+## * Let MSG_DEP_SET be the set of packages whose message types you use in
+##   your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...).
+## * In the file package.xml:
+##   * add a build_depend and a run_depend tag for each package in MSG_DEP_SET
+##   * If MSG_DEP_SET isn't empty the following dependencies might have been
+##     pulled in transitively but can be declared for certainty nonetheless:
+##     * add a build_depend tag for "message_generation"
+##     * add a run_depend tag for "message_runtime"
+## * In this file (CMakeLists.txt):
+##   * add "message_generation" and every package in MSG_DEP_SET to
+##     find_package(catkin REQUIRED COMPONENTS ...)
+##   * add "message_runtime" and every package in MSG_DEP_SET to
+##     catkin_package(CATKIN_DEPENDS ...)
+##   * uncomment the add_*_files sections below as needed
+##     and list every .msg/.srv/.action file to be processed
+##   * uncomment the generate_messages entry below
+##   * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...)
+
+## Generate messages in the 'msg' folder
+add_message_files(
+  FILES
+  actuator_armed.msg
+  actuator_controls.msg
+  commander_state.msg
+  control_state.msg
+  distance_sensor.msg
+  manual_control_setpoint.msg
+  mc_virtual_rates_setpoint.msg
+  offboard_control_mode.msg
+  parameter_update.msg
+  position_setpoint.msg
+  position_setpoint_triplet.msg
+  rc_channels.msg
+  ros/actuator_controls_0.msg
+  ros/actuator_controls_virtual_mc.msg
+  vehicle_attitude.msg
+  vehicle_attitude_setpoint.msg
+  vehicle_control_mode.msg
+  vehicle_force_setpoint.msg
+  vehicle_global_velocity_setpoint.msg
+  vehicle_local_position.msg
+  vehicle_local_position_setpoint.msg
+  vehicle_rates_setpoint.msg
+  vehicle_status.msg
+)
+
+## Generate services in the 'srv' folder
+# add_service_files(
+#   FILES
+#   Service1.srv
+#   Service2.srv
+# )
+
+## Generate actions in the 'action' folder
+# add_action_files(
+#   FILES
+#   Action1.action
+#   Action2.action
+# )
+
+## Generate added messages and services with any dependencies listed here
+generate_messages(
+  DEPENDENCIES
+  std_msgs
+  gazebo_msgs
+)
+
+###################################
+## catkin specific configuration ##
+###################################
+## The catkin_package macro generates cmake config files for your package
+## Declare things to be passed to dependent projects
+## INCLUDE_DIRS: uncomment this if you package contains header files
+## LIBRARIES: libraries you create in this project that dependent projects also need
+## CATKIN_DEPENDS: catkin_packages dependent projects also need
+## DEPENDS: system dependencies of this project that dependent projects also need
+catkin_package(
+  INCLUDE_DIRS src/include
+  LIBRARIES px4
+  CATKIN_DEPENDS message_runtime roscpp rospy std_msgs libmavconn
+  DEPENDS system_lib
+)
+
+###########
+## Build ##
+###########
+
+## Specify additional locations of header files
+## Your package locations should be listed before other locations
+include_directories(
+  ${catkin_INCLUDE_DIRS}
+  src/platforms
+  src/platforms/ros/px4_messages
+  src/include
+  src/modules
+  ${CMAKE_BINARY_DIR}/src/modules
+  src/
+  src/lib
+  src/lib/matrix
+  ${EIGEN_INCLUDE_DIRS}
+  integrationtests
+)
+
+## generate multiplatform wrapper headers
+## note that the message header files are generated as in any ROS project with generate_messages()
+set(MULTIPLATFORM_HEADER_DIR ${CMAKE_CURRENT_SOURCE_DIR}/src/platforms/ros/px4_messages)
+set(MULTIPLATFORM_TEMPLATE_DIR ${CMAKE_CURRENT_SOURCE_DIR}/msg/templates/px4/ros)
+set(TOPICHEADER_TEMP_DIR ${CMAKE_BINARY_DIR}/topics_temporary)
+set(MULTIPLATFORM_PREFIX px4_)
+add_custom_target(multiplatform_message_headers ALL ${PYTHON_EXECUTABLE} ${CMAKE_CURRENT_SOURCE_DIR}/Tools/px_generate_uorb_topic_files.py
+	--headers -d ${CMAKE_CURRENT_SOURCE_DIR}/msg -o ${MULTIPLATFORM_HEADER_DIR} -e ${MULTIPLATFORM_TEMPLATE_DIR}
+	-t ${TOPICHEADER_TEMP_DIR} -p ${MULTIPLATFORM_PREFIX})
+
+## Declare a cpp library
+add_library(px4
+  src/platforms/ros/px4_ros_impl.cpp
+  src/platforms/ros/perf_counter.cpp
+  src/platforms/ros/geo.cpp
+  src/lib/mathlib/math/Limits.cpp
+  src/modules/systemlib/circuit_breaker.cpp
+)
+add_dependencies(px4 ${PROJECT_NAME}_generate_messages_cpp multiplatform_message_headers)
+
+target_link_libraries(px4
+  ${catkin_LIBRARIES}
+)
+
+## Declare a test publisher
+add_executable(publisher
+	src/examples/publisher/publisher_main.cpp
+	src/examples/publisher/publisher_example.cpp)
+add_dependencies(publisher ${PROJECT_NAME}_generate_messages_cpp multiplatform_message_headers)
+target_link_libraries(publisher
+  ${catkin_LIBRARIES}
+  px4
+)
+
+## Declare a test subscriber
+add_executable(subscriber
+	src/examples/subscriber/subscriber_main.cpp
+	src/examples/subscriber/subscriber_example.cpp)
+add_dependencies(subscriber ${PROJECT_NAME}_generate_messages_cpp multiplatform_message_headers)
+target_link_libraries(subscriber
+  ${catkin_LIBRARIES}
+  px4
+)
+
+## MC Attitude Control
+add_executable(mc_att_control
+	src/modules/mc_att_control_multiplatform/mc_att_control_main.cpp
+	src/modules/mc_att_control_multiplatform/mc_att_control.cpp
+	src/modules/mc_att_control_multiplatform/mc_att_control_base.cpp)
+add_dependencies(mc_att_control ${PROJECT_NAME}_generate_messages_cpp_cpp)
+target_link_libraries(mc_att_control
+  ${catkin_LIBRARIES}
+  px4
+)
+
+## MC Position Control
+add_executable(mc_pos_control
+	src/modules/mc_pos_control_multiplatform/mc_pos_control_main.cpp
+	src/modules/mc_pos_control_multiplatform/mc_pos_control.cpp)
+add_dependencies(mc_pos_control ${PROJECT_NAME}_generate_messages_cpp_cpp)
+target_link_libraries(mc_pos_control
+  ${catkin_LIBRARIES}
+  px4
+)
+
+## Attitude Estimator dummy
+add_executable(attitude_estimator
+	src/platforms/ros/nodes/attitude_estimator/attitude_estimator.cpp)
+add_dependencies(attitude_estimator ${PROJECT_NAME}_generate_messages_cpp_cpp)
+target_link_libraries(attitude_estimator
+  ${catkin_LIBRARIES}
+  px4
+)
+
+## Position Estimator dummy
+add_executable(position_estimator
+	src/platforms/ros/nodes/position_estimator/position_estimator.cpp)
+add_dependencies(position_estimator ${PROJECT_NAME}_generate_messages_cpp_cpp)
+target_link_libraries(position_estimator
+  ${catkin_LIBRARIES}
+  px4
+)
+
+## Manual input
+add_executable(manual_input
+	src/platforms/ros/nodes/manual_input/manual_input.cpp)
+add_dependencies(manual_input ${PROJECT_NAME}_generate_messages_cpp_cpp)
+target_link_libraries(manual_input
+  ${catkin_LIBRARIES}
+  px4
+)
+
+## Multicopter Mixer dummy
+add_executable(mc_mixer
+  src/platforms/ros/nodes/mc_mixer/mc_mixer.cpp)
+add_dependencies(mc_mixer ${PROJECT_NAME}_generate_messages_cpp_cpp)
+target_link_libraries(mc_mixer
+  ${catkin_LIBRARIES}
+  px4
+)
+
+## Commander dummy
+add_executable(commander
+	src/platforms/ros/nodes/commander/commander.cpp)
+add_dependencies(commander ${PROJECT_NAME}_generate_messages_cpp_cpp)
+target_link_libraries(commander
+  ${catkin_LIBRARIES}
+  px4
+)
+
+## Mavlink dummy
+add_executable(mavlink
+	src/platforms/ros/nodes/mavlink/mavlink.cpp)
+add_dependencies(mavlink ${PROJECT_NAME}_generate_messages_cpp_cpp)
+target_link_libraries(mavlink
+  ${catkin_LIBRARIES}
+  px4
+)
+
+## Offboard Position Setpoint Demo
+add_executable(demo_offboard_position_setpoints
+	src/platforms/ros/nodes/demo_offboard_position_setpoints/demo_offboard_position_setpoints.cpp)
+add_dependencies(demo_offboard_position_setpoints ${PROJECT_NAME}_generate_messages_cpp_cpp)
+target_link_libraries(demo_offboard_position_setpoints
+  ${catkin_LIBRARIES}
+  px4
+)
+
+## Offboard Attitude Setpoint Demo
+add_executable(demo_offboard_attitude_setpoints
+	src/platforms/ros/nodes/demo_offboard_attitude_setpoints/demo_offboard_attitude_setpoints.cpp)
+add_dependencies(demo_offboard_attitude_setpoints ${PROJECT_NAME}_generate_messages_cpp_cpp)
+target_link_libraries(demo_offboard_attitude_setpoints
+  ${catkin_LIBRARIES}
+  px4
+)
+
+#############
+## Install ##
+#############
+
+# all install targets should use catkin DESTINATION variables
+# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html
+
+## Mark executable scripts (Python etc.) for installation
+## in contrast to setup.py, you can choose the destination
+# install(PROGRAMS
+#   scripts/my_python_script
+#   DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+# )
+
+## Mark executables and/or libraries for installation
+install(TARGETS ${PROJECT_NAME}
+  ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+  LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+  RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+)
+
+## Mark cpp header files for installation
+# install(DIRECTORY include/${PROJECT_NAME}/
+#   DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
+#   FILES_MATCHING PATTERN "*.h"
+#   PATTERN ".svn" EXCLUDE
+# )
+
+## Mark other files for installation (e.g. launch and bag files, etc.)
+# install(FILES
+#   # myfile1
+#   # myfile2
+#   DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
+# )
+
+#############
+## Testing ##
+#############
+
+## Add gtest based cpp test target and link libraries
+# catkin_add_gtest(${PROJECT_NAME}-test test/test_px4test.cpp)
+# if(TARGET ${PROJECT_NAME}-test)
+#   target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME})
+# endif()
+
+## Add folders to be run by python nosetests
+# catkin_add_nosetests(test)
+
+## Tests on multiplatform SITL (deprecated) have been moved to POSIX SITL
+#if(CATKIN_ENABLE_TESTING)
+#  find_package(rostest REQUIRED)
+#  add_rostest(integrationtests/demo_tests/direct_tests.launch)
+#  add_rostest(integrationtests/demo_tests/mavros_tests.launch)
+#endif()
diff --git a/integrationtests/run_tests.bash b/integrationtests/run_tests.bash
index 61569a1a73..2a3a4c2a0d 100755
--- a/integrationtests/run_tests.bash
+++ b/integrationtests/run_tests.bash
@@ -5,27 +5,26 @@
 # License: according to LICENSE.md in the root directory of the PX4 Firmware repository
 set -e
 
-# handle cleaning command
-do_clean=true
-if [ "$1" = "-o" ]
+if [ "$#" -lt 1 ]
 then
-	echo not cleaning
-	do_clean=false
+	echo usage: run_tests.bash firmware_src_dir
+	echo ""
+	exit 1
 fi
 
-# determine the directory of the source given the directory of this script
-pushd `dirname $0` > /dev/null
-SCRIPTPATH=`pwd`
-popd > /dev/null
-ORIG_SRC=$(dirname $SCRIPTPATH)
+SRC_DIR=$1
+JOB_DIR=$SRC_DIR/..
+BUILD=posix_sitl_default
+# TODO
+ROS_TEST_RESULT_DIR=/root/.ros/test_results/px4
+ROS_LOG_DIR=/root/.ros/log
+PX4_LOG_DIR=${SRC_DIR}/build_${BUILD}/src/firmware/posix/rootfs/fs/microsd/log
+TEST_RESULT_TARGET_DIR=$JOB_DIR/test_results
+# BAGS=/root/.ros
+# CHARTS=/root/.ros/charts
+# EXPORT_CHARTS=/sitl/testing/export_charts.py
 
-# set paths
-JOB_DIR=$(dirname $ORIG_SRC)
-CATKIN_DIR=$JOB_DIR/catkin
-BUILD_DIR=$CATKIN_DIR/build/px4
-SRC_DIR=${CATKIN_DIR}/src/px4
-
-echo setting up ROS paths
+# source ROS env
 if [ -f /opt/ros/indigo/setup.bash ]
 then
 	source /opt/ros/indigo/setup.bash
@@ -36,57 +35,26 @@ else
 	echo "could not find /opt/ros/{ros-distro}/setup.bash"
 	exit 1
 fi
-export ROS_HOME=$JOB_DIR/.ros
-export ROS_LOG_DIR=$ROS_HOME/log
-export ROS_TEST_RESULT_DIR=$ROS_HOME/test_results/px4
+source $SRC_DIR/integrationtests/setup_gazebo_ros.bash $SRC_DIR
 
-PX4_LOG_DIR=$ROS_HOME/rootfs/fs/microsd/log
-TEST_RESULT_TARGET_DIR=$JOB_DIR/test_results
-
-# TODO
-# BAGS=$ROS_HOME
-# CHARTS=$ROS_HOME/charts
-# EXPORT_CHARTS=/sitl/testing/export_charts.py
-
-echo setting up gazebo paths
-source /usr/share/gazebo/setup.sh
-source $SCRIPTPATH/setup_gazebo_ros.bash ${SRC_DIR} ${BUILD_DIR}
-
-if $do_clean
-then
-	echo cleaning
-	rm -rf $CATKIN_DIR
-	rm -rf $ROS_HOME
-	rm -rf $TEST_RESULT_TARGET_DIR
-else
-	echo skipping clean step
+echo "deleting previous test results ($TEST_RESULT_TARGET_DIR)"
+if [ -d ${TEST_RESULT_TARGET_DIR} ]; then
+	rm -r ${TEST_RESULT_TARGET_DIR}
 fi
 
+# FIXME: Firmware compilation seems to CD into this directory (/root/Firmware)
+# when run from "run_container.bash". Why?
+if [ -d /root/Firmware ]; then
+	rm /root/Firmware
+fi
+ln -s ${SRC_DIR} /root/Firmware
+
 echo "=====> compile ($SRC_DIR)"
-mkdir -p $ROS_HOME
-mkdir -p $CATKIN_DIR/src
-mkdir -p $TEST_RESULT_TARGET_DIR
-if ! [ -d $SRC_DIR ]
-then
-	ln -s $ORIG_SRC $SRC_DIR
-	ln -s $ORIG_SRC/Tools/sitl_gazebo ${CATKIN_DIR}/src/mavlink_sitl_gazebo
-fi
-cd $CATKIN_DIR
-catkin_make
-. ./devel/setup.bash
+cd $SRC_DIR
+make ${BUILD}
+make --no-print-directory gazebo_build
 echo "<====="
 
-# print paths to user
-echo -e "JOB_DIR\t\t: $JOB_DIR"
-echo -e "ROS_HOME\t: $ROS_HOME"
-echo -e "CATKIN_DIR\t: $CATKIN_DIR"
-echo -e "BUILD_DIR\t: $BUILD_DIR"
-echo -e "SRC_DIR\t\t: $SRC_DIR"
-echo -e "ROS_TEST_RESULT_DIR\t: $ROS_TEST_RESULT_DIR"
-echo -e "ROS_LOG_DIR\t\t: $ROS_LOG_DIR"
-echo -e "PX4_LOG_DIR\t\t: $PX4_LOG_DIR"
-echo -e "TEST_RESULT_TARGET_DIR\t: $TEST_RESULT_TARGET_DIR"
-
 # don't exit on error anymore from here on (because single tests or exports might fail)
 set +e
 echo "=====> run tests"
@@ -105,6 +73,7 @@ echo "=====> process test results"
 # done
 
 echo "copy build test results to job directory"
+mkdir -p ${TEST_RESULT_TARGET_DIR}
 cp -r $ROS_TEST_RESULT_DIR/* ${TEST_RESULT_TARGET_DIR}
 cp -r $ROS_LOG_DIR/* ${TEST_RESULT_TARGET_DIR}
 cp -r $PX4_LOG_DIR/* ${TEST_RESULT_TARGET_DIR}
diff --git a/integrationtests/setup_gazebo_ros.bash b/integrationtests/setup_gazebo_ros.bash
index 8cc4a5c58b..6c22bbe192 100755
--- a/integrationtests/setup_gazebo_ros.bash
+++ b/integrationtests/setup_gazebo_ros.bash
@@ -4,24 +4,19 @@
 #
 # License: according to LICENSE.md in the root directory of the PX4 Firmware repository
 
-if [ "$#" != 2 ]
+if [ "$#" -lt 1 ]
 then
-    echo usage: source setup_gazebo_ros.bash src_dir build_dir
+    echo usage: source setup_gazebo_ros.bash firmware_src_dir
     echo ""
     return 1
 fi
 
 SRC_DIR=$1
-BUILD_DIR=$2
 
 # setup Gazebo env and update package path
-export GAZEBO_RESOURCE_PATH=${SRC_DIR}/Tools/sitl_gazebo:${GAZEBO_RESOURCE_PATH}
-export GAZEBO_MODEL_PATH=${SRC_DIR}/Tools/sitl_gazebo/models:${GAZEBO_MODEL_PATH}
-export GAZEBO_PLUGIN_PATH=${BUILD_DIR}/build_gazebo:${GAZEBO_PLUGIN_PATH}
-export LD_LIBRARY_PATH=${SRC_DIR}/Tools/sitl_gazebo/Build/msgs/:${BUILD_DIR}/build_gazebo:${LD_LIBRARY_PATH}:
+export GAZEBO_MODEL_PATH=${GAZEBO_MODEL_PATH}:${SRC_DIR}/Tools/sitl_gazebo/models
+export GAZEBO_PLUGIN_PATH=${SRC_DIR}/build_gazebo:${GAZEBO_PLUGIN_PATH}
+export LD_LIBRARY_PATH=${LD_LIBRARY_PATH}:${SRC_DIR}/Tools/sitl_gazebo/Build/msgs/
+export ROS_PACKAGE_PATH=${ROS_PACKAGE_PATH}:${SRC_DIR}
 export GAZEBO_MODEL_DATABASE_URI=""
-
-echo -e "GAZEBO_RESOURCE_PATH $GAZEBO_RESOURCE_PATH"
-echo -e "GAZEBO_MODEL_PATH $GAZEBO_MODEL_PATH"
-echo -e "GAZEBO_PLUGIN_PATH $GAZEBO_PLUGIN_PATH"
-echo -e "LD_LIBRARY_PATH $LD_LIBRARY_PATH"
+export SITL_GAZEBO_PATH=$SRC_DIR/Tools/sitl_gazebo
diff --git a/launch/mavros_posix_sitl.launch b/launch/mavros_posix_sitl.launch
index 29793e10f8..558f3bbadb 100644
--- a/launch/mavros_posix_sitl.launch
+++ b/launch/mavros_posix_sitl.launch
@@ -1,25 +1,18 @@
 <launch>
-
     <!-- MAVROS posix SITL environment launch script -->
 
-    <arg name="vehicle" default="iris"/>
-    <arg name="world" default="$(find mavlink_sitl_gazebo)/worlds/empty.world"/>
-    <arg name="sdf" default="$(find mavlink_sitl_gazebo)/models/$(arg vehicle)/$(arg vehicle).sdf"/>
-    <arg name="rcS" default="$(find px4)/posix-configs/SITL/init/rcS_gazebo_$(arg vehicle)"/>
-
     <arg name="headless" default="false"/>
     <arg name="gui" default="true"/>
     <arg name="ns" default="/"/>
+    <arg name="world" default="iris"/>
     <arg name="build" default="posix_sitl_default"/>
     <arg name="fcu_url" default="udp://:14540@localhost:14557"/>
 
     <include file="$(find px4)/launch/posix_sitl.launch">
-        <arg name="world" value="$(arg world)"/>
-        <arg name="vehicle" value="$(arg vehicle)"/>
-        <arg name="rcS" value="$(arg rcS)"/>
         <arg name="headless" value="$(arg headless)"/>
         <arg name="gui" value="$(arg gui)"/>
         <arg name="ns" value="$(arg ns)"/>
+        <arg name="world" value="$(arg world)"/>
         <arg name="build" value="$(arg build)"/>
     </include>
 
@@ -29,5 +22,3 @@
         <arg name="fcu_url" value="$(arg fcu_url)"/>
     </include>
 </launch>
-
-<!-- vim: set et ft=xml fenc=utf-8 ff=unix sts=0 sw=4 ts=4 : -->
diff --git a/launch/mavros_posix_tests_iris.launch b/launch/mavros_posix_tests_iris.launch
index 8112f6f1f6..235acda6f2 100644
--- a/launch/mavros_posix_tests_iris.launch
+++ b/launch/mavros_posix_tests_iris.launch
@@ -9,7 +9,6 @@
         <arg name="ns" value="$(arg ns)"/>
         <arg name="headless" value="$(arg headless)"/>
         <arg name="gui" value="$(arg gui)"/>
-        <arg name="vehicle" value="iris"/>
     </include>
 
     <group ns="$(arg ns)">
diff --git a/launch/mavros_posix_tests_standard_vtol.launch b/launch/mavros_posix_tests_standard_vtol.launch
index b20f4801c4..d5aa9e285e 100644
--- a/launch/mavros_posix_tests_standard_vtol.launch
+++ b/launch/mavros_posix_tests_standard_vtol.launch
@@ -9,7 +9,7 @@
         <arg name="ns" value="$(arg ns)"/>
         <arg name="headless" value="$(arg headless)"/>
         <arg name="gui" value="$(arg gui)"/>
-        <arg name="vehicle" value="standard_vtol"/>
+        <arg name="world" value="standard_vtol"/>
     </include>
 
     <group ns="$(arg ns)">
diff --git a/launch/posix_sitl.launch b/launch/posix_sitl.launch
index 6c2d2da649..9895ee9d43 100644
--- a/launch/posix_sitl.launch
+++ b/launch/posix_sitl.launch
@@ -1,31 +1,19 @@
 <launch>
-
     <!-- Posix SITL environment launch script -->
 
-    <arg name="vehicle" default="iris"/>
-    <arg name="world" default="$(find mavlink_sitl_gazebo)/worlds/empty.world"/>
-    <arg name="sdf" default="$(find mavlink_sitl_gazebo)/models/$(arg vehicle)/$(arg vehicle).sdf"/>
-    <arg name="rcS" default="$(find px4)/posix-configs/SITL/init/rcS_gazebo_$(arg vehicle)"/>
-
     <arg name="headless" default="false"/>
     <arg name="gui" default="true"/>
     <arg name="ns" default="/"/>
+    <arg name="world" default="iris"/>
     <arg name="build" default="posix_sitl_default"/>
 
-    <node name="sitl" pkg="px4" type="px4" output="screen"
-        args="$(find px4) $(arg rcS)">
+    <node pkg="px4" type="sitl_run.sh" name="simulator" args="posix-configs/SITL/init/rcS none gazebo $(arg world) $(find px4)/build_$(arg build)">
+        <env name="no_sim" value="1" />
     </node>
 
     <include file="$(find gazebo_ros)/launch/empty_world.launch">
         <arg name="headless" value="$(arg headless)"/>
         <arg name="gui" value="$(arg gui)"/>
-        <arg name="world_name" value="$(arg world)" />
+        <arg name="world_name" value="$(find px4)/Tools/sitl_gazebo/worlds/$(arg world).world" />
     </include>
-
-    <node name="$(anon vehicle_spawn)" output="screen" pkg="gazebo_ros" type="spawn_model"
-        args="-sdf -file $(arg sdf) -model $(arg vehicle)"/>
-
-
 </launch>
-
-<!-- vim: set et ft=xml fenc=utf-8 ff=unix sts=0 sw=4 ts=4 : -->
diff --git a/posix-configs/SITL/README.md b/posix-configs/SITL/README.md
index a98a6515fa..2588c4b09b 100644
--- a/posix-configs/SITL/README.md
+++ b/posix-configs/SITL/README.md
@@ -105,5 +105,5 @@ param set RC_MAP_POSCTL_SW 7
 param set RC_MAP_RETURN_SW 8
 param set MC_PITCHRATE_P 0.05
 param set MC_ROLLRATE_P 0.05
-mixer load /dev/pwm_output0 ROMFS/px4fmu_common/mixers/quad_x.main.mix
+mixer load /dev/pwm_output0 ../../ROMFS/px4fmu_common/mixers/quad_x.main.mix
 ```
diff --git a/posix-configs/SITL/init/rcS_gazebo_iris b/posix-configs/SITL/init/rcS_gazebo_iris
index 6b93b86c2e..d985929601 100644
--- a/posix-configs/SITL/init/rcS_gazebo_iris
+++ b/posix-configs/SITL/init/rcS_gazebo_iris
@@ -60,7 +60,7 @@ navigator start
 ekf2 start
 mc_pos_control start
 mc_att_control start
-mixer load /dev/pwm_output0 ROMFS/px4fmu_common/mixers/quad_w.main.mix
+mixer load /dev/pwm_output0 ../../../../ROMFS/px4fmu_common/mixers/quad_w.main.mix
 mavlink start -u 14556 -r 4000000
 mavlink start -u 14557 -r 4000000 -m onboard -o 14540
 mavlink stream -r 50 -s POSITION_TARGET_LOCAL_NED -u 14556
diff --git a/posix-configs/SITL/init/rcS_gazebo_iris_opt_flow b/posix-configs/SITL/init/rcS_gazebo_iris_opt_flow
index 44060e1b52..2be4900b81 100644
--- a/posix-configs/SITL/init/rcS_gazebo_iris_opt_flow
+++ b/posix-configs/SITL/init/rcS_gazebo_iris_opt_flow
@@ -59,7 +59,7 @@ attitude_estimator_q start
 position_estimator_inav start
 mc_pos_control start
 mc_att_control start
-mixer load /dev/pwm_output0 ROMFS/px4fmu_common/mixers/quad_w.main.mix
+mixer load /dev/pwm_output0 ../../../../ROMFS/px4fmu_common/mixers/quad_w.main.mix
 mavlink start -u 14556 -r 2000000
 mavlink start -u 14557 -r 2000000 -m onboard -o 14540
 mavlink stream -r 80 -s POSITION_TARGET_LOCAL_NED -u 14556
diff --git a/posix-configs/SITL/init/rcS_gazebo_plane b/posix-configs/SITL/init/rcS_gazebo_plane
index bc837d2acd..b4f40ce2dd 100644
--- a/posix-configs/SITL/init/rcS_gazebo_plane
+++ b/posix-configs/SITL/init/rcS_gazebo_plane
@@ -47,7 +47,7 @@ ekf2 start
 fw_pos_control_l1 start
 fw_att_control start
 land_detector start fixedwing
-mixer load /dev/pwm_output0 ROMFS/sitl/mixers/plane_sitl.main.mix
+mixer load /dev/pwm_output0 ../../../../ROMFS/sitl/mixers/plane_sitl.main.mix
 mavlink start -u 14556 -r 2000000
 mavlink start -u 14557 -r 2000000 -m onboard -o 14540
 mavlink stream -r 80 -s POSITION_TARGET_LOCAL_NED -u 14556
diff --git a/posix-configs/SITL/init/rcS_gazebo_solo b/posix-configs/SITL/init/rcS_gazebo_solo
index 8701609064..008261b6ec 100644
--- a/posix-configs/SITL/init/rcS_gazebo_solo
+++ b/posix-configs/SITL/init/rcS_gazebo_solo
@@ -57,7 +57,7 @@ navigator start
 ekf2 start
 mc_pos_control start
 mc_att_control start
-mixer load /dev/pwm_output0 ROMFS/px4fmu_common/mixers/quad_x.main.mix
+mixer load /dev/pwm_output0 ../../../../ROMFS/px4fmu_common/mixers/quad_x.main.mix
 mavlink start -u 14556 -r 4000000
 mavlink start -u 14557 -r 4000000 -m onboard -o 14540
 mavlink stream -r 50 -s POSITION_TARGET_LOCAL_NED -u 14556
diff --git a/posix-configs/SITL/init/rcS_gazebo_standard_vtol b/posix-configs/SITL/init/rcS_gazebo_standard_vtol
index d90ddfa891..a1f448bb58 100644
--- a/posix-configs/SITL/init/rcS_gazebo_standard_vtol
+++ b/posix-configs/SITL/init/rcS_gazebo_standard_vtol
@@ -69,7 +69,7 @@ mc_pos_control start
 mc_att_control start
 fw_pos_control_l1 start
 fw_att_control start
-mixer load /dev/pwm_output0 ROMFS/sitl/mixers/standard_vtol_sitl.main.mix
+mixer load /dev/pwm_output0 ../../../../ROMFS/sitl/mixers/standard_vtol_sitl.main.mix
 mavlink start -u 14556 -r 2000000
 mavlink start -u 14557 -r 2000000 -m onboard -o 14540
 mavlink stream -r 80 -s POSITION_TARGET_LOCAL_NED -u 14556
diff --git a/posix-configs/SITL/init/rcS_gazebo_tailsitter b/posix-configs/SITL/init/rcS_gazebo_tailsitter
index 04d3a2b2f2..2ba72c5b8c 100644
--- a/posix-configs/SITL/init/rcS_gazebo_tailsitter
+++ b/posix-configs/SITL/init/rcS_gazebo_tailsitter
@@ -60,7 +60,7 @@ mc_pos_control start
 mc_att_control start
 fw_pos_control_l1 start
 fw_att_control start
-mixer load /dev/pwm_output0 ROMFS/px4fmu_common/mixers/quad_x_vtol.main.mix
+mixer load /dev/pwm_output0 ../../../../ROMFS/px4fmu_common/mixers/quad_x_vtol.main.mix
 mavlink start -u 14556 -r 2000000
 mavlink start -u 14557 -r 2000000 -m onboard -o 14540
 mavlink stream -r 80 -s POSITION_TARGET_LOCAL_NED -u 14556
diff --git a/posix-configs/SITL/init/rcS_gazebo_typhoon_h480 b/posix-configs/SITL/init/rcS_gazebo_typhoon_h480
index 8713c62f74..87c620c750 100644
--- a/posix-configs/SITL/init/rcS_gazebo_typhoon_h480
+++ b/posix-configs/SITL/init/rcS_gazebo_typhoon_h480
@@ -59,7 +59,7 @@ navigator start
 ekf2 start
 mc_pos_control start
 mc_att_control start
-mixer load /dev/pwm_output0 ROMFS/px4fmu_common/mixers/hexa_x.main.mix
+mixer load /dev/pwm_output0 ../../../../ROMFS/px4fmu_common/mixers/hexa_x.main.mix
 mavlink start -u 14556 -r 4000000
 mavlink start -u 14557 -r 4000000 -m onboard -o 14540
 mavlink stream -r 50 -s POSITION_TARGET_LOCAL_NED -u 14556
diff --git a/posix-configs/SITL/init/rcS_jmavsim_iris b/posix-configs/SITL/init/rcS_jmavsim_iris
index 64cf0a19a1..26db22d8f0 100644
--- a/posix-configs/SITL/init/rcS_jmavsim_iris
+++ b/posix-configs/SITL/init/rcS_jmavsim_iris
@@ -61,7 +61,7 @@ navigator start
 ekf2 start
 mc_pos_control start
 mc_att_control start
-mixer load /dev/pwm_output0 ROMFS/px4fmu_common/mixers/quad_x.main.mix
+mixer load /dev/pwm_output0 ../../../../ROMFS/px4fmu_common/mixers/quad_x.main.mix
 mavlink start -u 14556 -r 2000000
 mavlink start -u 14557 -r 2000000 -m onboard -o 14540
 mavlink stream -r 80 -s POSITION_TARGET_LOCAL_NED -u 14556
diff --git a/posix-configs/SITL/init/rcS_lpe_gazebo_iris b/posix-configs/SITL/init/rcS_lpe_gazebo_iris
index 59b11d1c5e..9c84018e98 100644
--- a/posix-configs/SITL/init/rcS_lpe_gazebo_iris
+++ b/posix-configs/SITL/init/rcS_lpe_gazebo_iris
@@ -60,7 +60,7 @@ attitude_estimator_q start
 local_position_estimator start
 mc_pos_control start
 mc_att_control start
-mixer load /dev/pwm_output0 ROMFS/px4fmu_common/mixers/quad_w.main.mix
+mixer load /dev/pwm_output0 /usr/share/px4/ROMFS/px4fmu_common/mixers/quad_w.main.mix
 mavlink start -u 14556 -r 4000000
 mavlink start -u 14557 -r 4000000 -m onboard -o 14540
 mavlink stream -r 50 -s POSITION_TARGET_LOCAL_NED -u 14556
diff --git a/posix-configs/SITL/init/rcS_lpe_gazebo_iris_opt_flow b/posix-configs/SITL/init/rcS_lpe_gazebo_iris_opt_flow
index 759831595a..c1a6a2eae5 100644
--- a/posix-configs/SITL/init/rcS_lpe_gazebo_iris_opt_flow
+++ b/posix-configs/SITL/init/rcS_lpe_gazebo_iris_opt_flow
@@ -69,7 +69,7 @@ attitude_estimator_q start
 local_position_estimator start
 mc_pos_control start
 mc_att_control start
-mixer load /dev/pwm_output0 ROMFS/px4fmu_common/mixers/quad_w.main.mix
+mixer load /dev/pwm_output0 /usr/share/px4/ROMFS/px4fmu_common/mixers/quad_w.main.mix
 mavlink start -u 14556 -r 2000000
 mavlink start -u 14557 -r 2000000 -m onboard -o 14540
 mavlink stream -r 80 -s POSITION_TARGET_LOCAL_NED -u 14556
diff --git a/posix-configs/SITL/init/rcS_lpe_jmavsim_iris b/posix-configs/SITL/init/rcS_lpe_jmavsim_iris
index 286cb13f9c..106d032275 100644
--- a/posix-configs/SITL/init/rcS_lpe_jmavsim_iris
+++ b/posix-configs/SITL/init/rcS_lpe_jmavsim_iris
@@ -61,7 +61,7 @@ attitude_estimator_q start
 local_position_estimator start
 mc_pos_control start
 mc_att_control start
-mixer load /dev/pwm_output0 ROMFS/px4fmu_common/mixers/quad_x.main.mix
+mixer load /dev/pwm_output0 /usr/share/px4/ROMFS/px4fmu_common/mixers/quad_x.main.mix
 mavlink start -u 14556 -r 2000000
 mavlink start -u 14557 -r 2000000 -m onboard -o 14540
 mavlink stream -r 80 -s POSITION_TARGET_LOCAL_NED -u 14556
diff --git a/posix-configs/SITL/init/rcS_tests_none_iris b/posix-configs/SITL/init/rcS_tests
similarity index 100%
rename from posix-configs/SITL/init/rcS_tests_none_iris
rename to posix-configs/SITL/init/rcS_tests
diff --git a/posix-configs/SITL/init/rc_iris_ros b/posix-configs/SITL/init/rc_iris_ros
index 2aa41cdbc8..ca1cf0961e 100644
--- a/posix-configs/SITL/init/rc_iris_ros
+++ b/posix-configs/SITL/init/rc_iris_ros
@@ -60,7 +60,7 @@ attitude_estimator_q start
 position_estimator_inav start
 mc_pos_control_m start
 mc_att_control_m start
-mixer load /dev/pwm_output0 ROMFS/px4fmu_common/mixers/quad_x.main.mix
+mixer load /dev/pwm_output0 ../../../../ROMFS/px4fmu_common/mixers/quad_x.main.mix
 mavlink start -u 14556 -r 2000000
 mavlink stream -r 80 -s POSITION_TARGET_LOCAL_NED -u 14556
 mavlink stream -r 80 -s LOCAL_POSITION_NED -u 14556
diff --git a/src/firmware/nuttx/CMakeLists.txt b/src/firmware/nuttx/CMakeLists.txt
index 4d27d2758b..53988b43cf 100644
--- a/src/firmware/nuttx/CMakeLists.txt
+++ b/src/firmware/nuttx/CMakeLists.txt
@@ -10,7 +10,7 @@ add_executable(firmware_nuttx
 	builtin_commands.c)
 
 
-set(nuttx_export_dir ${PX4_BINARY_DIR}/${BOARD}/NuttX/nuttx-export)
+set(nuttx_export_dir ${CMAKE_BINARY_DIR}/${BOARD}/NuttX/nuttx-export)
 
 set(link_libs
 	romfs apps nuttx m gcc
@@ -20,7 +20,7 @@ if(NOT ${BOARD} STREQUAL "sim")
 	list(APPEND link_libs nosys)
 	set(main_link_flags
 		"-T${nuttx_export_dir}/build/ld.script"
-		"-Wl,-Map=${PX4_BINARY_DIR}/${BOARD}/main.map"
+		"-Wl,-Map=${CMAKE_BINARY_DIR}/${BOARD}/main.map"
 		)
 	px4_join(OUT main_link_flags LIST ${main_link_flags} GLUE " ")
 	set_target_properties(firmware_nuttx PROPERTIES LINK_FLAGS ${main_link_flags})
@@ -43,12 +43,12 @@ add_custom_target(check_weak
 if(NOT ${BOARD} STREQUAL "sim")
 
 	if (config_io_board)
-		set(extras "${PX4_BINARY_DIR}/src/modules/px4iofirmware/${config_io_board}.bin")
+		set(extras "${CMAKE_BINARY_DIR}/src/modules/px4iofirmware/${config_io_board}.bin")
 	endif()
 	
 	set(romfs_dir "ROMFS/px4fmu_common")
 	if (${BOARD} STREQUAL "tap-v1")
-		set(romfs_dir "ROMFS/tap_common")
+	set(romfs_dir "ROMFS/tap_common")
 	endif()
 	if (${BOARD} STREQUAL "px4fmu-v2" AND ${LABEL} STREQUAL "test")
 	    set(romfs_dir "ROMFS/px4fmu_test")
@@ -68,8 +68,8 @@ if(NOT ${BOARD} STREQUAL "sim")
 	px4_nuttx_add_firmware(OUT ${fw_file}
 		BOARD ${BOARD}
 		EXE ${CMAKE_CURRENT_BINARY_DIR}/firmware_nuttx
-		PARAM_XML ${PX4_BINARY_DIR}/parameters.xml
-		AIRFRAMES_XML ${PX4_BINARY_DIR}/airframes.xml
+		PARAM_XML ${CMAKE_BINARY_DIR}/parameters.xml
+		AIRFRAMES_XML ${CMAKE_BINARY_DIR}/airframes.xml
 		)
 
 	configure_file(gdbinit.in .gdbinit)
@@ -94,21 +94,21 @@ if(NOT ${BOARD} STREQUAL "sim")
 
 	add_custom_target(debug_io
 		COMMAND ${GDB}
-			${PX4_BINARY_DIR}/src/modules/px4iofirmware/${config_io_board}
+			${CMAKE_BINARY_DIR}/src/modules/px4iofirmware/${config_io_board}
 		DEPENDS firmware_nuttx
 			${CMAKE_CURRENT_BINARY_DIR}/.gdbinit
 		)
 
 	add_custom_target(debug_io_tui
 		COMMAND ${GDBTUI}
-			${PX4_BINARY_DIR}/src/modules/px4iofirmware/${config_io_board}
+			${CMAKE_BINARY_DIR}/src/modules/px4iofirmware/${config_io_board}
 		DEPENDS firmware_nuttx
 			${CMAKE_CURRENT_BINARY_DIR}/.gdbinit
 		)
 
 	add_custom_target(debug_io_ddd
 		COMMAND ${DDD} --debugger ${GDB}
-			${PX4_BINARY_DIR}/src/modules/px4iofirmware/${config_io_board}
+			${CMAKE_BINARY_DIR}/src/modules/px4iofirmware/${config_io_board}
 		DEPENDS firmware_nuttx
 			${CMAKE_CURRENT_BINARY_DIR}/.gdbinit
 		)
diff --git a/src/firmware/nuttx/gdbinit.in b/src/firmware/nuttx/gdbinit.in
index fd1b1c451e..e820673b21 100644
--- a/src/firmware/nuttx/gdbinit.in
+++ b/src/firmware/nuttx/gdbinit.in
@@ -4,4 +4,4 @@ attach 1
 monitor vector_catch disable hard
 set mem inaccessible-by-default off
 set print pretty
-source ${PX4_SOURCE_DIR}/Debug/PX4
+source ${CMAKE_SOURCE_DIR}/Debug/PX4
diff --git a/src/firmware/posix/CMakeLists.txt b/src/firmware/posix/CMakeLists.txt
index 58ab449b5a..3e8e8b5227 100644
--- a/src/firmware/posix/CMakeLists.txt
+++ b/src/firmware/posix/CMakeLists.txt
@@ -18,7 +18,7 @@ if ("${BOARD}" STREQUAL "eagle" OR ("${BOARD}" STREQUAL "excelsior"))
 		APPS_DEST "/home/linaro"
 		SOURCES
 			px4muorb_stub.c
-			${PX4_SOURCE_DIR}/src/platforms/posix/main.cpp
+			${CMAKE_SOURCE_DIR}/src/platforms/posix/main.cpp
 			apps.h
 		LINK_LIBS
 			-Wl,--start-group
@@ -33,14 +33,14 @@ if ("${BOARD}" STREQUAL "eagle" OR ("${BOARD}" STREQUAL "excelsior"))
 			 OS ${OS}
 			 BOARD ${BOARD}
 			 FILES ${CMAKE_CURRENT_BINARY_DIR}/px4
-				 ${PX4_SOURCE_DIR}/posix-configs/eagle/flight/mainapp.config
+				 ${CMAKE_SOURCE_DIR}/posix-configs/eagle/flight/mainapp.config
 			 DEPENDS px4
 			 DEST /home/linaro)
 
 elseif ("${BOARD}" STREQUAL "rpi")
 
 	add_executable(px4
-		${PX4_SOURCE_DIR}/src/platforms/posix/main.cpp
+		${CMAKE_SOURCE_DIR}/src/platforms/posix/main.cpp
 		apps.h
 		)
 
@@ -57,7 +57,7 @@ elseif ("${BOARD}" STREQUAL "rpi")
 			 OS ${OS}
 			 BOARD ${BOARD}
 			 FILES ${CMAKE_CURRENT_BINARY_DIR}/px4
-				 ${PX4_SOURCE_DIR}/posix-configs/rpi/px4.config
+				 ${CMAKE_SOURCE_DIR}/posix-configs/rpi/px4.config
 			 DEPENDS px4
 			 DEST /home/pi)
 
@@ -67,7 +67,7 @@ elseif ("${BOARD}" STREQUAL "bebop")
 	set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -static")
 
 	add_executable(px4
-		${PX4_SOURCE_DIR}/src/platforms/posix/main.cpp
+		${CMAKE_SOURCE_DIR}/src/platforms/posix/main.cpp
 		apps.h
 		)
 
@@ -91,14 +91,14 @@ elseif ("${BOARD}" STREQUAL "bebop")
 			 OS ${OS}
 			 BOARD ${BOARD}
 			 FILES ${CMAKE_CURRENT_BINARY_DIR}/px4
-						 ${PX4_SOURCE_DIR}/posix-configs/bebop/px4.config
+						 ${CMAKE_SOURCE_DIR}/posix-configs/bebop/px4.config
 			 DEPENDS px4
 			 DEST /usr/bin)
 
 else()
 
 	add_executable(px4
-		${PX4_SOURCE_DIR}/src/platforms/posix/main.cpp
+		${CMAKE_SOURCE_DIR}/src/platforms/posix/main.cpp
 		apps.h
 		)
 	if (NOT APPLE)
@@ -118,43 +118,17 @@ else()
 	endif()
 endif()
 
-#=============================================================================
-# sitl run targets
-#
-
-set(SITL_WORKING_DIR ${PX4_BINARY_DIR}/tmp)
-file(MAKE_DIRECTORY ${SITL_WORKING_DIR})
-
 add_custom_target(run_config
-	COMMAND Tools/sitl_run.sh
-	"$<TARGET_FILE:px4>"
-	"${config_sitl_rcS}"
-	"${config_sitl_debugger}"
-	"${config_sitl_viewer}"
-	"${config_sitl_model}" 
-	"${PX4_SOURCE_DIR}"
-	"${PX4_BINARY_DIR}"
-	WORKING_DIRECTORY ${SITL_WORKING_DIR}
+	COMMAND Tools/sitl_run.sh "${config_sitl_rcS}" "${config_sitl_debugger}"
+	"${config_sitl_viewer}" "${config_sitl_model}" "${CMAKE_BINARY_DIR}"
+	WORKING_DIRECTORY ${CMAKE_SOURCE_DIR}
 	USES_TERMINAL
 	)
 add_dependencies(run_config px4)
 
-# project to build sitl_gazebo if necessary
-ExternalProject_Add(sitl_gazebo
-	SOURCE_DIR ${PX4_SOURCE_DIR}/Tools/sitl_gazebo
-	CMAKE_ARGS -DCMAKE_INSTALL_PREFIX=${CMAKE_INSTALL_PREFIX}
-	BINARY_DIR ${PX4_BINARY_DIR}/build_gazebo
-	INSTALL_COMMAND ""
-	)
-set_target_properties(sitl_gazebo PROPERTIES EXCLUDE_FROM_ALL TRUE)
-
-# create targets for each viewer/model/debugger combination
-set(viewers none jmavsim gazebo replay)
-set(debuggers none gdb lldb ddd valgrind)
-set(models none iris iris_opt_flow tailsitter standard_vtol plane solo typhoon_h480)
-foreach(viewer ${viewers})
-	foreach(debugger ${debuggers})
-		foreach(model ${models})
+foreach(viewer none jmavsim gazebo replay)
+	foreach(debugger none gdb lldb ddd valgrind)
+		foreach(model none iris iris_opt_flow tailsitter standard_vtol plane solo typhoon_h480)
 			if (debugger STREQUAL "none")
 				if (model STREQUAL "none")
 					set(_targ_name "${viewer}")
@@ -169,20 +143,13 @@ foreach(viewer ${viewers})
 				endif()
 			endif()
 			add_custom_target(${_targ_name}
-				COMMAND ${PX4_SOURCE_DIR}/Tools/sitl_run.sh
-				$<TARGET_FILE:px4>
-				${config_sitl_rcS}
-				${debugger}
-				${viewer}
-				${model}
-				${PX4_SOURCE_DIR}
-				${PX4_BINARY_DIR}
-				WORKING_DIRECTORY ${SITL_WORKING_DIR}
+				COMMAND Tools/sitl_run.sh "${config_sitl_rcS}"
+				"${debugger}"
+				"${viewer}" "${model}" "${CMAKE_BINARY_DIR}"
+				WORKING_DIRECTORY ${CMAKE_SOURCE_DIR}
 				USES_TERMINAL
 				)
-			if (viewer STREQUAL "gazebo")
-				add_dependencies(${_targ_name} sitl_gazebo)
-			endif()
+			add_dependencies(${_targ_name} px4)
 		endforeach()
 	endforeach()
 endforeach()
@@ -191,25 +158,8 @@ endforeach()
 # install
 #
 
-install(TARGETS px4 DESTINATION ${CMAKE_INSTALL_BINDIR})
+install(TARGETS px4 DESTINATION bin)
 install(DIRECTORY ${PROJECT_SOURCE_DIR}/ROMFS DESTINATION ${CMAKE_INSTALL_DATADIR}/${PROJECT_NAME})
 install(DIRECTORY ${PROJECT_SOURCE_DIR}/posix-configs DESTINATION ${CMAKE_INSTALL_DATADIR}/${PROJECT_NAME})
 
-#=============================================================================
-# tests
-#
-
-add_test(NAME rcS_tests
-	COMMAND ${PX4_SOURCE_DIR}/Tools/sitl_run.sh
-		$<TARGET_FILE:px4>
-		posix-configs/SITL/init/rcS_tests
-		none
-		none
-		none
-		${PX4_SOURCE_DIR}
-		${PX4_BINARY_DIR}
-		WORKING_DIRECTORY ${SITL_WORKING_DIR})
-set_tests_properties(rcS_tests PROPERTIES
-	PASS_REGULAR_EXPRESSION "All tests passed")
-
 # vim: set noet ft=cmake fenc=utf-8 ff=unix :
diff --git a/src/firmware/qurt/CMakeLists.txt b/src/firmware/qurt/CMakeLists.txt
index 2e30c6aa8d..81be14afc5 100644
--- a/src/firmware/qurt/CMakeLists.txt
+++ b/src/firmware/qurt/CMakeLists.txt
@@ -1,4 +1,4 @@
-set(CMAKE_MODULE_PATH ${CMAKE_MODULE_PATH} "${PX4_SOURCE_DIR}/cmake/cmake_hexagon")
+set(CMAKE_MODULE_PATH ${CMAKE_MODULE_PATH} "${CMAKE_SOURCE_DIR}/cmake/cmake_hexagon")
 include(fastrpc)
 include(qurt_lib)
 
@@ -7,7 +7,7 @@ include_directories(${CMAKE_CURRENT_BINARY_DIR})
 set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -Wno-missing-prototypes")
 
 px4_qurt_generate_builtin_commands(
-	OUT ${PX4_BINARY_DIR}/apps.h
+	OUT ${CMAKE_BINARY_DIR}/apps.h
 	MODULE_LIST ${module_libraries})
 
 FASTRPC_STUB_GEN(px4muorb.idl)
@@ -20,8 +20,8 @@ if ("${QURT_ENABLE_STUBS}" STREQUAL "1")
                 ${FASTRPC_DSP_INCLUDES}
                 )
 	add_executable(px4
-		${PX4_BINARY_DIR}/src/firmware/qurt/px4muorb_skel.c
-		${PX4_BINARY_DIR}/apps.h)
+		${CMAKE_BINARY_DIR}/src/firmware/qurt/px4muorb_skel.c
+		${CMAKE_BINARY_DIR}/apps.h)
 
 	target_link_libraries(px4
 		-Wl,--start-group
@@ -40,7 +40,7 @@ else()
 	QURT_LIB(LIB_NAME px4
 		IDL_NAME px4muorb
 		SOURCES
-			${PX4_BINARY_DIR}/apps.h
+			${CMAKE_BINARY_DIR}/apps.h
 		LINK_LIBS
 			${module_libraries}
 			${target_libraries}
@@ -54,7 +54,7 @@ else()
 			 BOARD ${BOARD}
 			 FILES ${CMAKE_CURRENT_BINARY_DIR}/libpx4.so
 				 ${CMAKE_CURRENT_BINARY_DIR}/libpx4muorb_skel.so
-				 ${PX4_SOURCE_DIR}/posix-configs/eagle/flight/px4.config
+				 ${CMAKE_SOURCE_DIR}/posix-configs/eagle/flight/px4.config
 			 DEPENDS px4 px4muorb_skel
 			 DEST /usr/share/data/adsp)
 endif()
diff --git a/src/lib/rc/rc_tests/CMakeLists.txt b/src/lib/rc/rc_tests/CMakeLists.txt
index 8a5be79ec2..42a5a1b1ba 100644
--- a/src/lib/rc/rc_tests/CMakeLists.txt
+++ b/src/lib/rc/rc_tests/CMakeLists.txt
@@ -40,5 +40,4 @@ px4_add_module(
 	DEPENDS
 		platforms__common
 	)
-
 # vim: set noet ft=cmake fenc=utf-8 ff=unix : 
diff --git a/src/lib/rc/rc_tests/RCTest.cpp b/src/lib/rc/rc_tests/RCTest.cpp
index 14119835f5..d7a834d5c8 100644
--- a/src/lib/rc/rc_tests/RCTest.cpp
+++ b/src/lib/rc/rc_tests/RCTest.cpp
@@ -16,7 +16,7 @@
 #if !defined(CONFIG_ARCH_BOARD_SITL)
 #define TEST_DATA_PATH "/fs/microsd"
 #else
-#define TEST_DATA_PATH "./src/lib/rc/rc_tests/test_data/"
+#define TEST_DATA_PATH "../../../../src/lib/rc/rc_tests/test_data/"
 #endif
 
 extern "C" __EXPORT int rc_tests_main(int argc, char *argv[]);
@@ -51,8 +51,8 @@ bool RCTest::dsmTest(void)
 	FILE *fp;
 	fp = fopen(filepath, "rt");
 
-	warnx("loading data from: %s", filepath);
 	ut_test(fp != nullptr);
+	//warnx("loading data from: %s", filepath);
 
 	float f;
 	unsigned x;
diff --git a/src/modules/muorb/krait/CMakeLists.txt b/src/modules/muorb/krait/CMakeLists.txt
index ad05492026..b9e054e3e7 100644
--- a/src/modules/muorb/krait/CMakeLists.txt
+++ b/src/modules/muorb/krait/CMakeLists.txt
@@ -30,10 +30,10 @@
 # POSSIBILITY OF SUCH DAMAGE.
 #
 ############################################################################
-set(CMAKE_MODULE_PATH ${CMAKE_MODULE_PATH} "${PX4_SOURCE_DIR}/cmake/cmake_hexagon")
+set(CMAKE_MODULE_PATH ${CMAKE_MODULE_PATH} "${CMAKE_SOURCE_DIR}/cmake/cmake_hexagon")
 include(hexagon_sdk)
 
-include_directories(${PX4_BINARY_DIR}/src/firmware/posix)
+include_directories(${CMAKE_BINARY_DIR}/src/firmware/posix)
 include_directories(${HEXAGON_SDK_INCLUDES})
 
 px4_add_module(
diff --git a/src/modules/param/CMakeLists.txt b/src/modules/param/CMakeLists.txt
index 3f64a9826c..b3148baab1 100644
--- a/src/modules/param/CMakeLists.txt
+++ b/src/modules/param/CMakeLists.txt
@@ -34,8 +34,8 @@
 include_directories(${CMAKE_CURRENT_BINARY_DIR})
 
 px4_generate_parameters_source(OUT param_files
-	XML ${PX4_BINARY_DIR}/parameters.xml
-	SCOPE ${PX4_SOURCE_DIR}/cmake/configs/${OS}_${BOARD}_${LABEL}.cmake
+	XML ${CMAKE_BINARY_DIR}/parameters.xml
+	SCOPE ${CMAKE_SOURCE_DIR}/cmake/configs/${OS}_${BOARD}_${LABEL}.cmake
 	DEPS xml_gen
 	)
 
diff --git a/src/modules/px4iofirmware/CMakeLists.txt b/src/modules/px4iofirmware/CMakeLists.txt
index 67183999eb..ab2778711c 100644
--- a/src/modules/px4iofirmware/CMakeLists.txt
+++ b/src/modules/px4iofirmware/CMakeLists.txt
@@ -63,7 +63,7 @@ px4_join(OUT CMAKE_CXX_FLAGS LIST "${cxx_flags}" GLUE " ")
 
 include_directories(
 	${include_dirs}
-	${PX4_BINARY_DIR}/src/modules/systemlib/mixer
+	${CMAKE_BINARY_DIR}/src/modules/systemlib/mixer
 	.
 	)
 link_directories(${link_dirs})
@@ -117,10 +117,10 @@ add_dependencies(${fw_io_name}
 	mixer_gen
 	)
 
-set(nuttx_export_dir ${PX4_BINARY_DIR}/${config_io_board}/NuttX/nuttx-export)
+set(nuttx_export_dir ${CMAKE_BINARY_DIR}/${config_io_board}/NuttX/nuttx-export)
 set(main_link_flags
 	"-T${nuttx_export_dir}/build/ld.script"
-	"-Wl,-Map=${PX4_BINARY_DIR}/${config_io_board}/main.map"
+	"-Wl,-Map=${CMAKE_BINARY_DIR}/${config_io_board}/main.map"
 	)
 px4_join(OUT main_link_flags LIST ${main_link_flags} GLUE " ")
 set_target_properties(${fw_io_name} PROPERTIES LINK_FLAGS ${main_link_flags})
diff --git a/src/modules/systemlib/CMakeLists.txt b/src/modules/systemlib/CMakeLists.txt
index 91f405d605..7277ee3ca1 100644
--- a/src/modules/systemlib/CMakeLists.txt
+++ b/src/modules/systemlib/CMakeLists.txt
@@ -32,7 +32,7 @@
 ############################################################################
 
 # for generated files
-include_directories(${PX4_BINARY_DIR}/src/modules/param)
+include_directories(${CMAKE_BINARY_DIR}/src/modules/param)
 
 set(SRCS
 	perf_counter.c
diff --git a/src/modules/uavcan/uavcan_servers.hpp b/src/modules/uavcan/uavcan_servers.hpp
index 47131f2ec6..17e94cd311 100644
--- a/src/modules/uavcan/uavcan_servers.hpp
+++ b/src/modules/uavcan/uavcan_servers.hpp
@@ -70,7 +70,7 @@
 /**
  * A UAVCAN Server Sub node.
  */
-class __EXPORT UavcanServers
+class UavcanServers
 {
 	static constexpr unsigned NumIfaces = 1;  // UAVCAN_STM32_NUM_IFACES
 
diff --git a/src/platforms/posix/main.cpp b/src/platforms/posix/main.cpp
index ac0520c858..646b94610f 100644
--- a/src/platforms/posix/main.cpp
+++ b/src/platforms/posix/main.cpp
@@ -50,7 +50,6 @@
 #include "px4_middleware.h"
 #include "DriverFramework.hpp"
 #include <termios.h>
-#include <sys/stat.h>
 
 namespace px4
 {
@@ -63,12 +62,6 @@ typedef int (*px4_main_t)(int argc, char *argv[]);
 
 #define CMD_BUFF_SIZE	100
 
-#ifdef PATH_MAX
-const unsigned path_max_len = PATH_MAX;
-#else
-const unsigned path_max_len = 1024;
-#endif
-
 static bool _ExitFlag = false;
 
 static struct termios orig_term;
@@ -92,96 +85,6 @@ extern "C" {
 	}
 }
 
-static inline bool fileExists(const string &name)
-{
-	struct stat buffer;
-	return (stat(name.c_str(), &buffer) == 0);
-}
-
-static inline bool dirExists(const string &path)
-{
-	struct stat info;
-
-	if (stat(path.c_str(), &info) != 0) {
-		return false;
-
-	} else if (info.st_mode & S_IFDIR) {
-		return true;
-
-	} else {
-		return false;
-	}
-}
-
-static inline void touch(const string &name)
-{
-	fstream fs;
-	fs.open(name, ios::out);
-	fs.close();
-}
-
-static int mkpath(const char *path, mode_t mode);
-
-static int do_mkdir(const char *path, mode_t mode)
-{
-	struct stat             st;
-	int             status = 0;
-
-	if (stat(path, &st) != 0) {
-		/* Directory does not exist. EEXIST for race condition */
-		if (mkdir(path, mode) != 0 && errno != EEXIST) {
-			status = -1;
-		}
-
-	} else if (!S_ISDIR(st.st_mode)) {
-		errno = ENOTDIR;
-		status = -1;
-	}
-
-	return (status);
-}
-
-/**
-** mkpath - ensure all directories in path exist
-** Algorithm takes the pessimistic view and works top-down to ensure
-** each directory in path exists, rather than optimistically creating
-** the last element and working backwards.
-*/
-static int mkpath(const char *path, mode_t mode)
-{
-	char           *pp;
-	char           *sp;
-	int             status;
-	char           *copypath = strdup(path);
-
-	status = 0;
-	pp = copypath;
-
-	while (status == 0 && (sp = strchr(pp, '/')) != 0) {
-		if (sp != pp) {
-			/* Neither root nor double slash in path */
-			*sp = '\0';
-			status = do_mkdir(copypath, mode);
-			*sp = '/';
-		}
-
-		pp = sp + 1;
-	}
-
-	if (status == 0) {
-		status = do_mkdir(path, mode);
-	}
-
-	free(copypath);
-	return (status);
-}
-
-static string pwd()
-{
-	char temp[path_max_len];
-	return (getcwd(temp, path_max_len) ? string(temp) : string(""));
-}
-
 static void print_prompt()
 {
 	cout.flush();
@@ -231,13 +134,12 @@ static void run_cmd(const vector<string> &appargs, bool exit_on_fail, bool silen
 static void usage()
 {
 
-	cout << "./px4 [-d] data_directory startup_config [-h]" << endl;
+	cout << "./px4 [-d] [startup_config] -h" << std::endl;
 	cout << "   -d            - Optional flag to run the app in daemon mode and does not listen for user input." <<
-	     endl;
-	cout << "                   This is needed if px4 is intended to be run as a upstart job on linux" << endl;
-	cout << "<data_directory> - directory where romfs and posix-configs are located" << endl;
-	cout << "<startup_config> - config file for starting/stopping px4 modules" << endl;
-	cout << "   -h            - help/usage information" << endl;
+	     std::endl;
+	cout << "                   This is needed if px4 is intended to be run as a upstart job on linux" << std::endl;
+	cout << "<startup_config> - config file for starting/stopping px4 modules" << std::endl;
+	cout << "   -h            - help/usage information" << std::endl;
 }
 
 static void process_line(string &line, bool exit_on_fail)
@@ -300,131 +202,52 @@ int main(int argc, char **argv)
 	set_cpu_scaling();
 
 	int index = 1;
-	string  commands_file = "";
-	int positional_arg_count = 0;
-	string data_path = "";
-	string node_name = "";
+	char *commands_file = nullptr;
 
-	// parse arguments
 	while (index < argc) {
-		//cout << "arg: " << index << " : " << argv[index] << endl;
-
 		if (argv[index][0] == '-') {
 			// the arg starts with -
-			if (strncmp(argv[index], "-d", 2) == 0) {
+			if (strcmp(argv[index], "-d") == 0) {
 				daemon_mode = true;
 
-			} else if (strncmp(argv[index], "-h", 2) == 0) {
+			} else if (strcmp(argv[index], "-h") == 0) {
 				usage();
 				return 0;
 
-			} else if (strncmp(argv[index], "-c", 2) == 0) {
+			} else if (strcmp(argv[index], "-c") == 0) {
 				chroot_on = true;
 
 			} else {
-				PX4_ERR("Unknown/unhandled parameter: %s", argv[index]);
+				PX4_WARN("Unknown/unhandled parameter: %s", argv[index]);
 				return 1;
 			}
 
-		} else if (!strncmp(argv[index], "__", 2)) {
-			//cout << "ros argument" << endl;
-
-			// ros arguments
-			if (!strncmp(argv[index], "__name:=", 8)) {
-				string name_arg = argv[index];
-				node_name = name_arg.substr(8);
-				cout << "node name: " << node_name << endl;
-			}
-
 		} else {
-			//cout << "positional argument" << endl;
+			// this is an argument that does not have '-' prefix; treat it like a file name
+			ifstream infile(argv[index]);
 
-			positional_arg_count += 1;
-
-			if (positional_arg_count == 1) {
-				data_path = argv[index];
-				cout << "data path: " << data_path << endl;
-
-			} else if (positional_arg_count == 2) {
+			if (infile.good()) {
+				infile.close();
 				commands_file = argv[index];
-				cout << "commands file: " << commands_file << endl;
+
+			} else {
+				PX4_WARN("Error opening file: %s", argv[index]);
+				return -1;
 			}
 		}
 
 		++index;
 	}
 
-	if (positional_arg_count != 2) {
-		PX4_ERR("Error expected 2 position arguments, got %d", positional_arg_count);
-		usage();
-		return -1;
-	}
 
-	if (commands_file.size() < 1) {
-		PX4_ERR("Error commands file not specified");
-		return -1;
-	}
-
-	if (!fileExists(commands_file)) {
-		PX4_ERR("Error opening commands file, does not exist: %s", commands_file.c_str());
-		return -1;
-	}
-
-	// create sym-links
-	vector<string> path_sym_links;
-	path_sym_links.push_back("ROMFS");
-	path_sym_links.push_back("posix-configs");
-	path_sym_links.push_back("src");
-
-	for (int i = 0; i < path_sym_links.size(); i++) {
-		string path_sym_link = path_sym_links[i];
-		//cout << "path sym link: " << path_sym_link << endl;
-		string src_path = data_path + "/" + path_sym_link;
-		string dest_path =  pwd() + "/" +  path_sym_link;
-
-		PX4_DEBUG("Creating symlink %s -> %s", src_path.c_str(), dest_path.c_str());
-
-		if (dirExists(path_sym_link)) { continue; }
-
-		// create sym-links
-		int ret = symlink(src_path.c_str(), dest_path.c_str());
-
-		if (ret != 0) {
-			PX4_ERR("Error creating symlink %s -> %s",
-				src_path.c_str(), dest_path.c_str());
-			return ret;
-
-		} else {
-			PX4_DEBUG("Successfully created symlink %s -> %s",
-				  src_path.c_str(), dest_path.c_str());
-		}
-	}
-
-	// setup rootfs
-	const string eeprom_path = "./rootfs/eeprom/";
-	const string microsd_path = "./rootfs/fs/microsd/";
-
-	if (!fileExists(eeprom_path + "parameters")) {
-		cout << "creating new parameters file" << endl;
-		mkpath(eeprom_path.c_str(), S_IRUSR | S_IWUSR | S_IXUSR);
-		touch(eeprom_path + "parameters");
-	}
-
-	if (!fileExists(microsd_path + "dataman")) {
-		cout << "creating new dataman file" << endl;
-		mkpath(microsd_path.c_str(), S_IRUSR | S_IWUSR | S_IXUSR);
-		touch(microsd_path + "dataman");
-	}
-
-	// initialize
 	DriverFramework::Framework::initialize();
 	px4::init_once();
 
 	px4::init(argc, argv, "px4");
 
 	// if commandfile is present, process the commands from the file
-	if (commands_file.size() != 0) {
-		ifstream infile(commands_file.c_str());
+	if (commands_file != nullptr) {
+		ifstream infile(commands_file);
 
 		if (infile.is_open()) {
 			for (string line; getline(infile, line, '\n');) {
@@ -438,7 +261,7 @@ int main(int argc, char **argv)
 			}
 
 		} else {
-			PX4_ERR("Error opening commands file: %s", commands_file.c_str());
+			PX4_WARN("Error opening file: %s", commands_file);
 		}
 	}
 
@@ -447,6 +270,12 @@ int main(int argc, char **argv)
 		// this is not an attempt to secure the environment,
 		// rather, to replicate a deployed file system.
 
+#ifdef PATH_MAX
+		const unsigned path_max_len = PATH_MAX;
+#else
+		const unsigned path_max_len = 1024;
+#endif
+
 		char pwd_path[path_max_len];
 		const char *folderpath = "/rootfs/";
 
@@ -590,5 +419,3 @@ int main(int argc, char **argv)
 
 	return OK;
 }
-
-/* vim: set noet fenc=utf-8 ff=unix sts=0 sw=4 ts=4 : */
diff --git a/src/platforms/posix/px4_layer/CMakeLists.txt b/src/platforms/posix/px4_layer/CMakeLists.txt
index fa1eed4ffc..344621c694 100644
--- a/src/platforms/posix/px4_layer/CMakeLists.txt
+++ b/src/platforms/posix/px4_layer/CMakeLists.txt
@@ -32,10 +32,10 @@
 ############################################################################
 
 if("${CONFIG_SHMEM}" STREQUAL "1")
-	set(CMAKE_MODULE_PATH ${CMAKE_MODULE_PATH} "${PX4_SOURCE_DIR}/cmake/cmake_hexagon")
+	set(CMAKE_MODULE_PATH ${CMAKE_MODULE_PATH} "${CMAKE_SOURCE_DIR}/cmake/cmake_hexagon")
 	include(hexagon_sdk)
 
-	include_directories(${PX4_BINARY_DIR}/src/firmware/posix)
+	include_directories(${CMAKE_BINARY_DIR}/src/firmware/posix)
 	include_directories(${HEXAGON_SDK_INCLUDES})
 	list(APPEND SHMEM_SRCS
 		shmem_posix.c
diff --git a/src/platforms/qurt/px4_layer/CMakeLists.txt b/src/platforms/qurt/px4_layer/CMakeLists.txt
index ed6e56790a..38dbd8a46f 100644
--- a/src/platforms/qurt/px4_layer/CMakeLists.txt
+++ b/src/platforms/qurt/px4_layer/CMakeLists.txt
@@ -30,7 +30,7 @@
 # POSSIBILITY OF SUCH DAMAGE.
 #
 ############################################################################
-set(CMAKE_MODULE_PATH ${CMAKE_MODULE_PATH} "${PX4_SOURCE_DIR}/cmake/cmake_hexagon")
+set(CMAKE_MODULE_PATH ${CMAKE_MODULE_PATH} "${CMAKE_SOURCE_DIR}/cmake/cmake_hexagon")
 include(hexagon_sdk)
 
 include_directories(${HEXAGON_8074_INCLUDES})
diff --git a/src/systemcmds/tests/test_mixer.cpp b/src/systemcmds/tests/test_mixer.cpp
index da2cff5d5a..88c37d6e9c 100644
--- a/src/systemcmds/tests/test_mixer.cpp
+++ b/src/systemcmds/tests/test_mixer.cpp
@@ -90,7 +90,7 @@ int test_mixer(int argc, char *argv[])
 #if !defined(CONFIG_ARCH_BOARD_SITL)
 	const char *filename = "/etc/mixers/IO_pass.mix";
 #else
-	const char *filename = "ROMFS/px4fmu_test/mixers/IO_pass.mix";
+	const char *filename = "../../../../ROMFS/px4fmu_test/mixers/IO_pass.mix";
 #endif
 
 	//PX4_INFO("loading: %s", filename);
@@ -400,7 +400,7 @@ int test_mixer(int argc, char *argv[])
 #if !defined(CONFIG_ARCH_BOARD_SITL)
 	filename = "/etc/mixers/quad_test.mix";
 #else
-	filename = "ROMFS/px4fmu_test/mixers/quad_test.mix";
+	filename = "../../../../ROMFS/px4fmu_test/mixers/quad_test.mix";
 #endif
 
 	load_mixer_file(filename, &buf[0], sizeof(buf));
diff --git a/src/systemcmds/topic_listener/CMakeLists.txt b/src/systemcmds/topic_listener/CMakeLists.txt
index bb955490bc..0984e546e3 100644
--- a/src/systemcmds/topic_listener/CMakeLists.txt
+++ b/src/systemcmds/topic_listener/CMakeLists.txt
@@ -32,14 +32,14 @@
 ############################################################################
 
 add_custom_command(OUTPUT topic_listener.cpp
-                COMMAND ${PYTHON_EXECUTABLE} ${PX4_SOURCE_DIR}/Tools/generate_listener.py ${PX4_SOURCE_DIR} > topic_listener.cpp
+                COMMAND ${PYTHON_EXECUTABLE} ${CMAKE_SOURCE_DIR}/Tools/generate_listener.py ${CMAKE_SOURCE_DIR} > topic_listener.cpp
                 DEPENDS msg_gen
                 )
 
 add_custom_target(generate_topic_listener
 	DEPENDS
 		topic_listener.cpp
-		${PX4_SOURCE_DIR}/Tools/generate_listener.py)
+		${CMAKE_SOURCE_DIR}/Tools/generate_listener.py)
 
 px4_add_module(
 	MODULE systemcmds__topic_listener
diff --git a/unittests/CMakeLists.txt b/unittests/CMakeLists.txt
index b6664b26e8..9df3adcce3 100644
--- a/unittests/CMakeLists.txt
+++ b/unittests/CMakeLists.txt
@@ -13,19 +13,15 @@ enable_testing()
 set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -Wall -Werror -std=gnu99 -g")
 set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall -Werror -std=c++11 -g -fno-exceptions -fno-rtti -fno-threadsafe-statics")
 
-if (NOT PX4_SOURCE_DIR)
-	set(PX4_SOURCE_DIR "${CMAKE_CURRENT_SOURCE_DIR}/..")
-endif()
 
-
-set(GTEST_DIR ${PX4_SOURCE_DIR}/unittests/googletest)
+set(GTEST_DIR ${CMAKE_SOURCE_DIR}/googletest)
 add_subdirectory(${GTEST_DIR})
 include_directories(${GTEST_DIR}/include)
 
-set(PX4_SRC ${PX4_SOURCE_DIR}/src)
+set(PX4_SRC ${CMAKE_SOURCE_DIR}/../src)
 set(PX4_SITL_BUILD ${PX4_SRC}/../build_posix_sitl_default)
 
-include_directories(${PX4_SOURCE_DIR})
+include_directories(${CMAKE_SOURCE_DIR})
 include_directories(${PX4_SITL_BUILD}/src)
 include_directories(${PX4_SITL_BUILD}/src/modules)
 include_directories(${PX4_SITL_BUILD}/src/modules/param)
@@ -98,7 +94,7 @@ target_include_directories(px4_platform PUBLIC ${PX4_SRC}/platforms/posix/work_q
 # check
 add_custom_target(check
 		COMMAND ${CMAKE_CTEST_COMMAND} -j2 --output-on-failure
-		WORKING_DIR ${PX4_BINARY_DIR}
+		WORKING_DIR ${CMAKE_BINARY_DIR}
 		USES_TERMINAL)
 
 # add_gtest
@@ -109,7 +105,7 @@ function(add_gtest)
 		else()
 			target_link_libraries(${test_name} gtest_main pthread rt px4_platform)
 		endif()
-		add_test(NAME ${test_name} COMMAND ${test_name} WORKING_DIRECTORY ${PX4_SOURCE_DIR})
+		add_test(NAME ${test_name} COMMAND ${test_name} WORKING_DIRECTORY ${CMAKE_SOURCE_DIR})
 		add_dependencies(check ${test_name})
 	endforeach()
 endfunction()

From 37230f4c5d19db993d04bf155d54f15c3617f1e5 Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Fri, 5 Aug 2016 22:10:34 +0200
Subject: [PATCH 1227/1230] Revert "navigator: mission check refactor"

This reverts commit a956429c4ccd80dfd8ae8f20fdfa8d777dfa221d.
---
 src/modules/navigator/mission.cpp | 59 +++++++++++++++++--------------
 src/modules/navigator/mission.h   |  3 +-
 2 files changed, 34 insertions(+), 28 deletions(-)

diff --git a/src/modules/navigator/mission.cpp b/src/modules/navigator/mission.cpp
index 1cd019d214..e9369aacaa 100644
--- a/src/modules/navigator/mission.cpp
+++ b/src/modules/navigator/mission.cpp
@@ -81,6 +81,7 @@ Mission::Mission(Navigator *navigator, const char *name) :
 	_need_takeoff(true),
 	_mission_type(MISSION_TYPE_NONE),
 	_inited(false),
+	_home_inited(false),
 	_need_mission_reset(false),
 	_missionFeasibilityChecker(),
 	_min_current_sp_distance_xy(FLT_MAX),
@@ -99,11 +100,6 @@ Mission::~Mission()
 void
 Mission::on_inactive()
 {
-	/* Without home a mission can't be valid yet anyway, let's wait. */
-	if (!_navigator->home_position_valid()) {
-		return;
-	}
-
 	if (_inited) {
 		/* check if missions have changed so that feedback to ground station is given */
 		bool onboard_updated = false;
@@ -142,12 +138,11 @@ Mission::on_inactive()
 			_current_offboard_mission_index = mission_state.current_seq;
 		}
 
-		/* On init let's check the mission, maybe there is already one available. */
-		check_mission_valid();
-
 		_inited = true;
 	}
 
+	check_mission_valid();
+
 	/* require takeoff after non-loiter or landing */
 	if (!_navigator->get_can_loiter_at_sp() || _navigator->get_land_detected()->landed) {
 		_need_takeoff = true;
@@ -279,14 +274,24 @@ Mission::update_offboard_mission()
 			/* otherwise, just leave it */
 		}
 
-		check_mission_valid();
+		/* Check mission feasibility, for now do not handle the return value,
+		 * however warnings are issued to the gcs via mavlink from inside the MissionFeasiblityChecker */
+		dm_item_t dm_current = DM_KEY_WAYPOINTS_OFFBOARD(_offboard_mission.dataman_id);
 
-		failed = !_navigator->get_mission_result()->valid;
+		failed = !_missionFeasibilityChecker.checkMissionFeasible(_navigator->get_mavlink_log_pub(), (_navigator->get_vstatus()->is_rotary_wing || _navigator->get_vstatus()->is_vtol),
+				dm_current, (size_t) _offboard_mission.count, _navigator->get_geofence(),
+				_navigator->get_home_position()->alt, _navigator->home_position_valid(),
+				_navigator->get_global_position()->lat, _navigator->get_global_position()->lon,
+				_param_dist_1wp.get(), _navigator->get_mission_result()->warning, _navigator->get_default_acceptance_radius(),
+				_navigator->get_land_detected()->landed);
 
+		_navigator->get_mission_result()->valid = !failed;
 		if (!failed) {
 			/* reset mission failure if we have an updated valid mission */
 			_navigator->get_mission_result()->mission_failure = false;
 		}
+		_navigator->increment_mission_instance_count();
+		_navigator->set_mission_result_updated();
 
 	} else {
 		PX4_WARN("offboard mission update failed, handle: %d", _navigator->get_offboard_mission_sub());
@@ -1126,28 +1131,28 @@ Mission::set_mission_finished()
 	_navigator->set_mission_result_updated();
 }
 
-void
+bool
 Mission::check_mission_valid()
 {
+	/* check if the home position became valid in the meantime */
+	if (!_home_inited && _navigator->home_position_valid()) {
 
-	dm_item_t dm_current = DM_KEY_WAYPOINTS_OFFBOARD(_offboard_mission.dataman_id);
+		dm_item_t dm_current = DM_KEY_WAYPOINTS_OFFBOARD(_offboard_mission.dataman_id);
 
-	_navigator->get_mission_result()->valid =
-		_missionFeasibilityChecker.checkMissionFeasible(
-			_navigator->get_mavlink_log_pub(),
-			(_navigator->get_vstatus()->is_rotary_wing || _navigator->get_vstatus()->is_vtol),
-			dm_current, (size_t) _offboard_mission.count, _navigator->get_geofence(),
-			_navigator->get_home_position()->alt,
-			_navigator->home_position_valid(),
-			_navigator->get_global_position()->lat,
-			_navigator->get_global_position()->lon,
-			_param_dist_1wp.get(),
-			_navigator->get_mission_result()->warning,
-			_navigator->get_default_acceptance_radius(),
-			_navigator->get_land_detected()->landed);
+		_navigator->get_mission_result()->valid = _missionFeasibilityChecker.checkMissionFeasible(_navigator->get_mavlink_log_pub(), (_navigator->get_vstatus()->is_rotary_wing || _navigator->get_vstatus()->is_vtol),
+				dm_current, (size_t) _offboard_mission.count, _navigator->get_geofence(),
+				_navigator->get_home_position()->alt, _navigator->home_position_valid(),
+				_navigator->get_global_position()->lat, _navigator->get_global_position()->lon,
+				_param_dist_1wp.get(), _navigator->get_mission_result()->warning, _navigator->get_default_acceptance_radius(),
+				_navigator->get_land_detected()->landed);
 
-	_navigator->increment_mission_instance_count();
-	_navigator->set_mission_result_updated();
+		_navigator->increment_mission_instance_count();
+		_navigator->set_mission_result_updated();
+
+		_home_inited = true;
+	}
+
+	return _navigator->get_mission_result()->valid;
 }
 
 void
diff --git a/src/modules/navigator/mission.h b/src/modules/navigator/mission.h
index cd43327550..2bcfa85508 100644
--- a/src/modules/navigator/mission.h
+++ b/src/modules/navigator/mission.h
@@ -211,7 +211,7 @@ private:
 	/**
 	 * Check wether a mission is ready to go
 	 */
-	void check_mission_valid();
+	bool check_mission_valid();
 
 	/**
 	 * Reset offboard mission
@@ -244,6 +244,7 @@ private:
 	} _mission_type;
 
 	bool _inited;
+	bool _home_inited;
 	bool _need_mission_reset;
 
 	MissionFeasibilityChecker _missionFeasibilityChecker; /**< class that checks if a mission is feasible */

From 278b607f62f0b08efbfbba6a68a1373630158204 Mon Sep 17 00:00:00 2001
From: Roman <bapstroman@gmail.com>
Date: Fri, 5 Aug 2016 14:23:48 +0200
Subject: [PATCH 1228/1230] Update ECL library

---
 src/lib/ecl | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/src/lib/ecl b/src/lib/ecl
index 168d9add76..df0f8fed45 160000
--- a/src/lib/ecl
+++ b/src/lib/ecl
@@ -1 +1 @@
-Subproject commit 168d9add76f94c910d1930b58abb455bf2739c8a
+Subproject commit df0f8fed45b94b44f18595abafbd5f6fd1de4275

From bcd5f98cb11f456b2d945a1f718a5319e9acd2e2 Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Sat, 6 Aug 2016 10:34:20 +0200
Subject: [PATCH 1229/1230] Navigator: Fix mission feedback and wording

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

diff --git a/src/modules/navigator/mission.cpp b/src/modules/navigator/mission.cpp
index e9369aacaa..a9e0b96c05 100644
--- a/src/modules/navigator/mission.cpp
+++ b/src/modules/navigator/mission.cpp
@@ -372,7 +372,7 @@ Mission::set_mission_items()
 	if (_param_onboard_enabled.get() && prepare_mission_items(true, &_mission_item, &mission_item_next_position, &has_next_position_item)) {
 		/* if mission type changed, notify */
 		if (_mission_type != MISSION_TYPE_ONBOARD) {
-			mavlink_log_critical(_navigator->get_mavlink_log_pub(), "onboard mission now running");
+			mavlink_log_info(_navigator->get_mavlink_log_pub(), "Executing internal mission");
 			user_feedback_done = true;
 		}
 		_mission_type = MISSION_TYPE_ONBOARD;
@@ -381,7 +381,7 @@ Mission::set_mission_items()
 	} else if (prepare_mission_items(false, &_mission_item, &mission_item_next_position, &has_next_position_item)) {
 		/* if mission type changed, notify */
 		if (_mission_type != MISSION_TYPE_OFFBOARD) {
-			mavlink_log_info(_navigator->get_mavlink_log_pub(), "offboard mission now running");
+			mavlink_log_info(_navigator->get_mavlink_log_pub(), "Executing mission");
 			user_feedback_done = true;
 		}
 		_mission_type = MISSION_TYPE_OFFBOARD;
@@ -390,10 +390,10 @@ Mission::set_mission_items()
 		if (_mission_type != MISSION_TYPE_NONE) {
 
 			if (_navigator->get_land_detected()->landed) {
-				mavlink_log_critical(_navigator->get_mavlink_log_pub(), "mission finished, landed");
+				mavlink_log_info(_navigator->get_mavlink_log_pub(), "mission finished, landed");
 			} else {
 				/* https://en.wikipedia.org/wiki/Loiter_(aeronautics) */
-				mavlink_log_critical(_navigator->get_mavlink_log_pub(), "mission finished, loitering");
+				mavlink_log_info(_navigator->get_mavlink_log_pub(), "mission finished, loitering");
 
 				/* use last setpoint for loiter */
 				_navigator->set_can_loiter_at_sp(true);

From c76c8fda044e91fb966504131239d83862831a0b Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Sat, 6 Aug 2016 11:50:08 +0200
Subject: [PATCH 1230/1230] MC pos control: Drop default position controller
 gain

---
 src/modules/mc_pos_control/mc_pos_control_params.c | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/src/modules/mc_pos_control/mc_pos_control_params.c b/src/modules/mc_pos_control/mc_pos_control_params.c
index 2d30adc1cf..5e817addad 100644
--- a/src/modules/mc_pos_control/mc_pos_control_params.c
+++ b/src/modules/mc_pos_control/mc_pos_control_params.c
@@ -246,7 +246,7 @@ PARAM_DEFINE_FLOAT(MPC_Z_FF, 0.5f);
  * @decimal 2
  * @group Multicopter Position Control
  */
-PARAM_DEFINE_FLOAT(MPC_XY_P, 1.25f);
+PARAM_DEFINE_FLOAT(MPC_XY_P, 0.95f);
 
 /**
  * Proportional gain for horizontal velocity error