From e224441ac14b4ed384d77995b7dbdbc047cee6a5 Mon Sep 17 00:00:00 2001
From: Mark Whitehorn <kd0aij@gmail.com>
Date: Sat, 6 Jun 2015 10:34:01 -0600
Subject: [PATCH 01/25] special treatment and warning message for HIL platform
 arming

---
 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 64fd972ba4..c6f5eaa7b5 100644
--- a/src/modules/commander/commander.cpp
+++ b/src/modules/commander/commander.cpp
@@ -437,6 +437,12 @@ transition_result_t arm_disarm(bool arm, const int mavlink_fd_local, const char
 {
 	transition_result_t arming_res = TRANSITION_NOT_CHANGED;
 
+	// For HIL platforms, require that simulated sensors are connected
+	if (autostart_id < 2000 && status.hil_state != vehicle_status_s::HIL_STATE_ON) {
+		mavlink_and_console_log_critical(mavlink_fd_local, "HIL platform: Connect to simulator before arming");
+		return TRANSITION_DENIED;
+	}
+
 	// Transition the armed state. By passing mavlink_fd to arming_state_transition it will
 	// output appropriate error messages if the state cannot transition.
 	arming_res = arming_state_transition(&status, &safety, arm ? vehicle_status_s::ARMING_STATE_ARMED : vehicle_status_s::ARMING_STATE_STANDBY, &armed,

From 71da3976abf593c7d515b8aa78b49e7f8553a51e Mon Sep 17 00:00:00 2001
From: Mark Whitehorn <kd0aij@gmail.com>
Date: Sat, 6 Jun 2015 15:03:03 -0600
Subject: [PATCH 02/25] add HIL autostart ID range macros and remove warnx

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

diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp
index c6f5eaa7b5..8091a73ece 100644
--- a/src/modules/commander/commander.cpp
+++ b/src/modules/commander/commander.cpp
@@ -147,6 +147,9 @@ static constexpr uint8_t COMMANDER_MAX_GPS_NOISE = 60;		/**< Maximum percentage
 
 #define INAIR_RESTART_HOLDOFF_INTERVAL	2000000
 
+#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. | */
@@ -438,7 +441,8 @@ transition_result_t arm_disarm(bool arm, const int mavlink_fd_local, const char
 	transition_result_t arming_res = TRANSITION_NOT_CHANGED;
 
 	// For HIL platforms, require that simulated sensors are connected
-	if (autostart_id < 2000 && status.hil_state != vehicle_status_s::HIL_STATE_ON) {
+	if (autostart_id >= HIL_ID_MIN && autostart_id <= HIL_ID_MAX
+			&& status.hil_state != vehicle_status_s::HIL_STATE_ON) {
 		mavlink_and_console_log_critical(mavlink_fd_local, "HIL platform: Connect to simulator before arming");
 		return TRANSITION_DENIED;
 	}
@@ -1162,7 +1166,11 @@ int commander_thread_main(int argc, char *argv[])
 
 	// Run preflight check
 	param_get(_param_autostart_id, &autostart_id);
-	if (autostart_id > 1999) {
+	if (autostart_id >= HIL_ID_MIN && autostart_id <= HIL_ID_MAX) {
+		// HIL configuration selected: real sensors will be disabled
+		status.condition_system_sensors_initialized = false;
+		set_tune_override(TONE_STARTUP_TUNE); //normal boot tune
+	} else {
 		status.condition_system_sensors_initialized = Commander::preflightCheck(mavlink_fd, true, true, true, true, checkAirspeed, !status.rc_input_mode, !status.circuit_breaker_engaged_gpsfailure_check);
 		if (!status.condition_system_sensors_initialized) {
 			set_tune_override(TONE_GPS_WARNING_TUNE); //sensor fail tune
@@ -1170,11 +1178,6 @@ int commander_thread_main(int argc, char *argv[])
 		else {
 			set_tune_override(TONE_STARTUP_TUNE); //normal boot tune
 		}
-	} else {
-		// HIL configuration selected: real sensors will be disabled
-		warnx("HIL configuration; autostart_id: %d, onboard IMU sensors disabled", autostart_id);
-		status.condition_system_sensors_initialized = false;
-		set_tune_override(TONE_STARTUP_TUNE); //normal boot tune
 	}
 
 	commander_boot_timestamp = hrt_absolute_time();
@@ -1345,14 +1348,14 @@ int commander_thread_main(int argc, char *argv[])
 					}
 
 					/* provide RC and sensor status feedback to the user */
-					if (autostart_id > 1999) {
-						(void)Commander::preflightCheck(mavlink_fd, true, true, true, true, chAirspeed,
-								!(status.rc_input_mode == vehicle_status_s::RC_IN_MODE_OFF), !status.circuit_breaker_engaged_gpsfailure_check);
-					} else {
+					if (autostart_id >= HIL_ID_MIN && autostart_id <= HIL_ID_MAX) {
 						/* HIL configuration: check only RC input */
-						warnx("new telemetry link connected: checking RC status");
 						(void)Commander::preflightCheck(mavlink_fd, false, false, false, false, false,
 								!(status.rc_input_mode == vehicle_status_s::RC_IN_MODE_OFF), false);
+					} else {
+						/* check sensors also */
+						(void)Commander::preflightCheck(mavlink_fd, true, true, true, true, chAirspeed,
+								!(status.rc_input_mode == vehicle_status_s::RC_IN_MODE_OFF), !status.circuit_breaker_engaged_gpsfailure_check);
 					}
 				}
 

From 7e48c66c221fc281b37dfb0db188440f89823ce6 Mon Sep 17 00:00:00 2001
From: Mark Whitehorn <kd0aij@gmail.com>
Date: Wed, 10 Jun 2015 11:05:56 -0600
Subject: [PATCH 03/25] add is_hil_setup()

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

diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp
index 8091a73ece..becf585452 100644
--- a/src/modules/commander/commander.cpp
+++ b/src/modules/commander/commander.cpp
@@ -256,6 +256,15 @@ void *commander_low_prio_loop(void *arg);
 
 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) {
+	return (id >= HIL_ID_MIN) && (id <= HIL_ID_MAX);
+}
+
 
 int commander_main(int argc, char *argv[])
 {
@@ -441,8 +450,7 @@ transition_result_t arm_disarm(bool arm, const int mavlink_fd_local, const char
 	transition_result_t arming_res = TRANSITION_NOT_CHANGED;
 
 	// For HIL platforms, require that simulated sensors are connected
-	if (autostart_id >= HIL_ID_MIN && autostart_id <= HIL_ID_MAX
-			&& status.hil_state != vehicle_status_s::HIL_STATE_ON) {
+	if (is_hil_setup(autostart_id) && status.hil_state != vehicle_status_s::HIL_STATE_ON) {
 		mavlink_and_console_log_critical(mavlink_fd_local, "HIL platform: Connect to simulator before arming");
 		return TRANSITION_DENIED;
 	}
@@ -1166,7 +1174,7 @@ int commander_thread_main(int argc, char *argv[])
 
 	// Run preflight check
 	param_get(_param_autostart_id, &autostart_id);
-	if (autostart_id >= HIL_ID_MIN && autostart_id <= HIL_ID_MAX) {
+	if (is_hil_setup(autostart_id)) {
 		// HIL configuration selected: real sensors will be disabled
 		status.condition_system_sensors_initialized = false;
 		set_tune_override(TONE_STARTUP_TUNE); //normal boot tune
@@ -1348,7 +1356,7 @@ int commander_thread_main(int argc, char *argv[])
 					}
 
 					/* provide RC and sensor status feedback to the user */
-					if (autostart_id >= HIL_ID_MIN && autostart_id <= HIL_ID_MAX) {
+					if (is_hil_setup(autostart_id)) {
 						/* HIL configuration: check only RC input */
 						(void)Commander::preflightCheck(mavlink_fd, false, false, false, false, false,
 								!(status.rc_input_mode == vehicle_status_s::RC_IN_MODE_OFF), false);

From e8a9c200561fea02f24a97883e6c617d07b251ce Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Thu, 11 Jun 2015 12:30:05 +0200
Subject: [PATCH 04/25] EKF: Ensure we start with zero local altitude and zero
 GPS offset. Since the filter is not publishing any data at this point this is
 not relevant in operation, but might be important later if we publish a
 separate altitude estimate topic

---
 .../ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp   | 7 ++++++-
 1 file changed, 6 insertions(+), 1 deletion(-)

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 74d6dbadf0..50410eac7d 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
@@ -631,7 +631,10 @@ void AttitudePositionEstimatorEKF::task_main()
 				// gps sample rate is 5Hz and altitude is assumed accurate when averaged over 30 seconds
 				// maintain heavily filtered values for both baro and gps altitude
 				// Assume the filtered output should be identical for both sensors
-				_baro_gps_offset = _baro_alt_filt - _gps_alt_filt;
+
+				if (_gpsIsGood) {
+					_baro_gps_offset = _baro_alt_filt - _gps_alt_filt;
+				}
 //				if (hrt_elapsed_time(&_last_debug_print) >= 5e6) {
 //					_last_debug_print = hrt_absolute_time();
 //					perf_print_counter(_perf_baro);
@@ -658,6 +661,8 @@ void AttitudePositionEstimatorEKF::task_main()
 
 					_ekf->InitialiseFilter(initVelNED, 0.0, 0.0, 0.0f, 0.0f);
 
+					_filter_ref_offset = -_baro.altitude;
+
 					warnx("filter ref off: baro_alt: %8.4f", (double)_filter_ref_offset);
 
 				} else {

From 086123fe8419f9382d0c0e2d80e1fe7a27a17ae6 Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Thu, 11 Jun 2015 12:40:39 +0200
Subject: [PATCH 05/25] Fix RC failsafe handling when landed

---
 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 92f73ca21f..7a379612da 100644
--- a/src/modules/commander/state_machine_helper.cpp
+++ b/src/modules/commander/state_machine_helper.cpp
@@ -494,7 +494,7 @@ bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_en
 	case vehicle_status_s::MAIN_STATE_ALTCTL:
 	case vehicle_status_s::MAIN_STATE_POSCTL:
 		/* require RC for all manual modes */
-		if ((status->rc_signal_lost || status->rc_signal_lost_cmd) && armed) {
+		if ((status->rc_signal_lost || status->rc_signal_lost_cmd) && armed && !status->condition_landed) {
 			status->failsafe = true;
 
 			if (status->condition_global_position_valid && status->condition_home_position_valid) {

From 25c23dbf4cb58d4e8c94a749801a1129ef7cdf34 Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Thu, 11 Jun 2015 13:19:36 +0200
Subject: [PATCH 06/25] back out payload mixer from FX79 default config

---
 ROMFS/px4fmu_common/mixers/FX79.main.mix | 16 ----------------
 1 file changed, 16 deletions(-)

diff --git a/ROMFS/px4fmu_common/mixers/FX79.main.mix b/ROMFS/px4fmu_common/mixers/FX79.main.mix
index b8879af9ee..3968a2b08d 100755
--- a/ROMFS/px4fmu_common/mixers/FX79.main.mix
+++ b/ROMFS/px4fmu_common/mixers/FX79.main.mix
@@ -51,19 +51,3 @@ range.  Inputs below zero are treated as zero.
 M: 1
 O:      10000  10000      0 -10000  10000
 S: 0 3      0  20000 -10000 -10000  10000
-
-Inputs to the mixer come from channel group 2 (payload), channels 0
-(bay servo 1), 1 (bay servo 2) and 3 (drop release).
------------------------------------------------------
-
-M: 1
-O:      10000  10000      0 -10000  10000
-S: 2 0  10000  10000      0 -10000  10000
-
-M: 1
-O:      10000  10000      0 -10000  10000
-S: 2 1  10000  10000      0 -10000  10000
-
-M: 1
-O:      10000  10000      0 -10000  10000
-S: 2 2 -10000 -10000      0 -10000  10000

From f0f3ffaec1b012fd95d220a9379ae07ed30053e9 Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Thu, 11 Jun 2015 13:31:58 +0200
Subject: [PATCH 07/25] IO firmware: Do not apply trim values a second time

---
 src/modules/px4iofirmware/mixer.cpp | 7 -------
 1 file changed, 7 deletions(-)

diff --git a/src/modules/px4iofirmware/mixer.cpp b/src/modules/px4iofirmware/mixer.cpp
index 596064f15f..9fadec57c0 100644
--- a/src/modules/px4iofirmware/mixer.cpp
+++ b/src/modules/px4iofirmware/mixer.cpp
@@ -311,13 +311,6 @@ mixer_callback(uintptr_t handle,
 	case MIX_FMU:
 		if (control_index < PX4IO_CONTROL_CHANNELS && control_group < PX4IO_CONTROL_GROUPS ) {
 			control = REG_TO_FLOAT(r_page_controls[CONTROL_PAGE_INDEX(control_group, control_index)]);
-			if (control_group == 0 && control_index == 0) {
-				control += REG_TO_FLOAT(r_setup_trim_roll);
-			} else if (control_group == 0 && control_index == 1) {
-				control += REG_TO_FLOAT(r_setup_trim_pitch);
-			} else if (control_group == 0 && control_index == 2) {
-				control += REG_TO_FLOAT(r_setup_trim_yaw);
-			}
 			break;
 		}
 		return -1;

From 3cc2b7ed1285489b4cda8360d720738e0d146e0b Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Thu, 11 Jun 2015 14:51:34 +0200
Subject: [PATCH 08/25] EKF: Add small gyro failover hysteresis

---
 .../ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp    | 6 ++++--
 1 file changed, 4 insertions(+), 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 50410eac7d..b9897ffcfc 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
@@ -72,6 +72,8 @@ static constexpr float rc = 10.0f;	// RC time constant of 1st order LPF in secon
 static constexpr uint64_t FILTER_INIT_DELAY = 1 * 1000 * 1000; 	///< units: microseconds
 static constexpr float POS_RESET_THRESHOLD = 5.0f; 				///< Seconds before we signal a total GPS failure
 static constexpr unsigned MAG_SWITCH_HYSTERESIS = 10;	///< Ignore the first few mag failures (which amounts to a few milliseconds)
+static constexpr unsigned GYRO_SWITCH_HYSTERESIS = 5;	///< Ignore the first few gyro failures (which amounts to a few milliseconds)
+static constexpr unsigned ACCEL_SWITCH_HYSTERESIS = 5;	///< Ignore the first few accel failures (which amounts to a few milliseconds)
 
 /**
  * estimator app start / stop handling function
@@ -1224,7 +1226,7 @@ void AttitudePositionEstimatorEKF::pollData()
 	if (isfinite(_sensor_combined.gyro_rad_s[0]) &&
 	    isfinite(_sensor_combined.gyro_rad_s[1]) &&
 	    isfinite(_sensor_combined.gyro_rad_s[2]) &&
-	    (_sensor_combined.gyro_errcount <= _sensor_combined.gyro1_errcount)) {
+	    (_sensor_combined.gyro_errcount <= (_sensor_combined.gyro1_errcount + GYRO_SWITCH_HYSTERESIS))) {
 
 		_ekf->angRate.x = _sensor_combined.gyro_rad_s[0];
 		_ekf->angRate.y = _sensor_combined.gyro_rad_s[1];
@@ -1263,7 +1265,7 @@ void AttitudePositionEstimatorEKF::pollData()
 		int last_accel_main = _accel_main;
 
 		/* fail over to the 2nd accel if we know the first is down */
-		if (_sensor_combined.accelerometer_errcount <= _sensor_combined.accelerometer1_errcount) {
+		if (_sensor_combined.accelerometer_errcount <= (_sensor_combined.accelerometer1_errcount + ACCEL_SWITCH_HYSTERESIS)) {
 			_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];

From 000434be15c9ac8a93fa0e675f790464d8b5d23e Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Thu, 11 Jun 2015 14:51:49 +0200
Subject: [PATCH 09/25] IO mixer: Limit outputs to proper range

---
 src/modules/px4iofirmware/mixer.cpp | 7 +++++++
 1 file changed, 7 insertions(+)

diff --git a/src/modules/px4iofirmware/mixer.cpp b/src/modules/px4iofirmware/mixer.cpp
index 9fadec57c0..5e6a3a585a 100644
--- a/src/modules/px4iofirmware/mixer.cpp
+++ b/src/modules/px4iofirmware/mixer.cpp
@@ -353,6 +353,13 @@ mixer_callback(uintptr_t handle,
 		return -1;
 	}
 
+	/* limit output */
+	if (control > 1.0f) {
+		control = 1.0f;
+	} else if (control < -1.0f) {
+		control = -1.0f;
+	}
+
 	return 0;
 }
 

From 9155e8a7fe2a7611c6b1ed136b5691475546a65c Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Thu, 11 Jun 2015 16:35:10 +0200
Subject: [PATCH 10/25] FX79: Increase travel

---
 ROMFS/px4fmu_common/mixers/FX79.main.mix | 8 ++++----
 1 file changed, 4 insertions(+), 4 deletions(-)

diff --git a/ROMFS/px4fmu_common/mixers/FX79.main.mix b/ROMFS/px4fmu_common/mixers/FX79.main.mix
index 3968a2b08d..3223eff133 100755
--- a/ROMFS/px4fmu_common/mixers/FX79.main.mix
+++ b/ROMFS/px4fmu_common/mixers/FX79.main.mix
@@ -27,13 +27,13 @@ for the elevons.
 
 M: 2
 O:      10000  10000      0 -10000  10000
-S: 0 0   7500   7500      0 -10000  10000
-S: 0 1   8000   8000      0 -10000  10000
+S: 0 0   8500   8500      0 -10000  10000
+S: 0 1   9500   9500      0 -10000  10000
 
 M: 2
 O:      10000  10000      0 -10000  10000
-S: 0 0   7500   7500      0 -10000  10000
-S: 0 1  -8000  -8000      0 -10000  10000
+S: 0 0   8500   8500      0 -10000  10000
+S: 0 1  -9500  -9500      0 -10000  10000
 
 Output 2
 --------

From f2b81ce69a88683289ead9de33ce83efc3831a1f Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Thu, 11 Jun 2015 12:22:47 +0200
Subject: [PATCH 11/25] commander: Only update home position if not armed
 already

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

diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp
index becf585452..7c15992f40 100644
--- a/src/modules/commander/commander.cpp
+++ b/src/modules/commander/commander.cpp
@@ -499,10 +499,14 @@ bool handle_command(struct vehicle_status_s *status_local, const struct safety_s
 			transition_result_t hil_ret = hil_state_transition(new_hil_state, status_pub, status_local, mavlink_fd);
 
 			// Transition the arming state
-			arming_ret = arm_disarm(base_mode & MAV_MODE_FLAG_SAFETY_ARMED, mavlink_fd, "set mode command");
+			bool cmd_arm = base_mode & MAV_MODE_FLAG_SAFETY_ARMED;
+
+			arming_ret = arm_disarm(cmd_arm, mavlink_fd, "set mode command");
 
 			/* update home position on arming if at least 2s from commander start spent to avoid setting home on in-air restart */
-			if (arming_ret == TRANSITION_CHANGED && hrt_absolute_time() > commander_boot_timestamp + INAIR_RESTART_HOLDOFF_INTERVAL) {
+			if (cmd_arm && (arming_ret == TRANSITION_CHANGED) &&
+				(hrt_absolute_time() > (commander_boot_timestamp + INAIR_RESTART_HOLDOFF_INTERVAL))) {
+
 				commander_set_home_position(*home_pub, *home, *local_pos, *global_pos);
 			}
 
@@ -2041,11 +2045,11 @@ int commander_thread_main(int argc, char *argv[])
 			}
 		}
 
-		//Get current timestamp
+		/* Get current timestamp */
 		const hrt_abstime now = hrt_absolute_time();
 
-		//First time home position update
-		if (!status.condition_home_position_valid) {
+		/* First time home position update - but only if disarmed */
+		if (!status.condition_home_position_valid && !armed.armed) {
 			commander_set_home_position(home_pub, _home, local_position, global_position);
 		}
 

From bc48634101dff97b7e4cd2d5e3008613a1c0df6d Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Thu, 11 Jun 2015 12:24:26 +0200
Subject: [PATCH 12/25] Navigator: Reject missions with relative altitude if no
 home was set before arming

---
 src/modules/navigator/mission.cpp             |  2 +-
 .../navigator/mission_feasibility_checker.cpp | 23 ++++++++++++-------
 .../navigator/mission_feasibility_checker.h   |  8 +++----
 src/modules/navigator/navigator.h             |  1 +
 4 files changed, 21 insertions(+), 13 deletions(-)

diff --git a/src/modules/navigator/mission.cpp b/src/modules/navigator/mission.cpp
index 98b01c7578..4eaeca1d8e 100644
--- a/src/modules/navigator/mission.cpp
+++ b/src/modules/navigator/mission.cpp
@@ -232,7 +232,7 @@ Mission::update_offboard_mission()
 
 		failed = !_missionFeasiblityChecker.checkMissionFeasible(_navigator->get_vstatus()->is_rotary_wing,
 				dm_current, (size_t) _offboard_mission.count, _navigator->get_geofence(),
-				_navigator->get_home_position()->alt);
+				_navigator->get_home_position()->alt, _navigator->home_position_valid());
 
 	} else {
 		warnx("offboard mission update failed");
diff --git a/src/modules/navigator/mission_feasibility_checker.cpp b/src/modules/navigator/mission_feasibility_checker.cpp
index d9297f25bf..19baa79fcd 100644
--- a/src/modules/navigator/mission_feasibility_checker.cpp
+++ b/src/modules/navigator/mission_feasibility_checker.cpp
@@ -63,7 +63,7 @@ MissionFeasibilityChecker::MissionFeasibilityChecker() : _mavlink_fd(-1), _capab
 }
 
 
-bool MissionFeasibilityChecker::checkMissionFeasible(bool isRotarywing, dm_item_t dm_current, size_t nMissionItems, Geofence &geofence, float home_alt)
+bool MissionFeasibilityChecker::checkMissionFeasible(bool isRotarywing, dm_item_t dm_current, size_t nMissionItems, Geofence &geofence, float home_alt, bool home_valid)
 {
 	bool failed = false;
 	/* Init if not done yet */
@@ -80,25 +80,25 @@ bool MissionFeasibilityChecker::checkMissionFeasible(bool isRotarywing, dm_item_
 
 
 	if (isRotarywing)
-		failed |= !checkMissionFeasibleRotarywing(dm_current, nMissionItems, geofence, home_alt);
+		failed |= !checkMissionFeasibleRotarywing(dm_current, nMissionItems, geofence, home_alt, home_valid);
 	else
-		failed |= !checkMissionFeasibleFixedwing(dm_current, nMissionItems, geofence, home_alt);
+		failed |= !checkMissionFeasibleFixedwing(dm_current, nMissionItems, geofence, home_alt, home_valid);
 
 	return !failed;
 }
 
-bool MissionFeasibilityChecker::checkMissionFeasibleRotarywing(dm_item_t dm_current, size_t nMissionItems, Geofence &geofence, float home_alt)
+bool MissionFeasibilityChecker::checkMissionFeasibleRotarywing(dm_item_t dm_current, size_t nMissionItems, Geofence &geofence, float home_alt, bool home_valid)
 {
 
 	/* Perform checks and issue feedback to the user for all checks */
 	bool resGeofence = checkGeofence(dm_current, nMissionItems, geofence);
-	bool resHomeAltitude = checkHomePositionAltitude(dm_current, nMissionItems, home_alt);
+	bool resHomeAltitude = checkHomePositionAltitude(dm_current, nMissionItems, home_alt, home_valid);
 
 	/* Mission is only marked as feasible if all checks return true */
 	return (resGeofence && resHomeAltitude);
 }
 
-bool MissionFeasibilityChecker::checkMissionFeasibleFixedwing(dm_item_t dm_current, size_t nMissionItems, Geofence &geofence, float home_alt)
+bool MissionFeasibilityChecker::checkMissionFeasibleFixedwing(dm_item_t dm_current, size_t nMissionItems, Geofence &geofence, float home_alt, bool home_valid)
 {
 	/* Update fixed wing navigation capabilites */
 	updateNavigationCapabilities();
@@ -107,7 +107,7 @@ bool MissionFeasibilityChecker::checkMissionFeasibleFixedwing(dm_item_t dm_curre
 	/* Perform checks and issue feedback to the user for all checks */
 	bool resLanding = checkFixedWingLanding(dm_current, nMissionItems);
 	bool resGeofence = checkGeofence(dm_current, nMissionItems, geofence);
-	bool resHomeAltitude = checkHomePositionAltitude(dm_current, nMissionItems, home_alt);
+	bool resHomeAltitude = checkHomePositionAltitude(dm_current, nMissionItems, home_alt, home_valid);
 
 	/* Mission is only marked as feasible if all checks return true */
 	return (resLanding && resGeofence && resHomeAltitude);
@@ -136,7 +136,7 @@ bool MissionFeasibilityChecker::checkGeofence(dm_item_t dm_current, size_t nMiss
 	return true;
 }
 
-bool MissionFeasibilityChecker::checkHomePositionAltitude(dm_item_t dm_current, size_t nMissionItems, float home_alt, bool throw_error)
+bool MissionFeasibilityChecker::checkHomePositionAltitude(dm_item_t dm_current, size_t nMissionItems, float home_alt, bool home_valid, bool throw_error)
 {
 	/* Check if all all waypoints are above the home altitude, only return false if bool throw_error = true */
 	for (size_t i = 0; i < nMissionItems; i++) {
@@ -152,6 +152,13 @@ bool MissionFeasibilityChecker::checkHomePositionAltitude(dm_item_t dm_current,
 			}
 		}
 
+		if (missionitem.altitude_is_relative && !home_valid) {
+			if (throw_error) {
+				mavlink_log_critical(_mavlink_fd, "Rejecting Mission: No home pos, WP %d uses rel alt", i);
+				return false;
+			}
+		}
+
 		/* calculate the global waypoint altitude */
 		float wp_alt = (missionitem.altitude_is_relative) ? missionitem.altitude + home_alt : missionitem.altitude;
 
diff --git a/src/modules/navigator/mission_feasibility_checker.h b/src/modules/navigator/mission_feasibility_checker.h
index 94b6b29226..964bd6097d 100644
--- a/src/modules/navigator/mission_feasibility_checker.h
+++ b/src/modules/navigator/mission_feasibility_checker.h
@@ -61,16 +61,16 @@ private:
 
 	/* Checks for all airframes */
 	bool checkGeofence(dm_item_t dm_current, size_t nMissionItems, Geofence &geofence);
-	bool checkHomePositionAltitude(dm_item_t dm_current, size_t nMissionItems, float home_alt, bool throw_error = false);
+	bool checkHomePositionAltitude(dm_item_t dm_current, size_t nMissionItems, float home_alt, bool home_valid, bool throw_error = false);
 	bool checkMissionItemValidity(dm_item_t dm_current, size_t nMissionItems);
 
 	/* Checks specific to fixedwing airframes */
-	bool checkMissionFeasibleFixedwing(dm_item_t dm_current, size_t nMissionItems, Geofence &geofence, float home_alt);
+	bool checkMissionFeasibleFixedwing(dm_item_t dm_current, size_t nMissionItems, Geofence &geofence, float home_alt, bool home_valid);
 	bool checkFixedWingLanding(dm_item_t dm_current, size_t nMissionItems);
 	void updateNavigationCapabilities();
 
 	/* Checks specific to rotarywing airframes */
-	bool checkMissionFeasibleRotarywing(dm_item_t dm_current, size_t nMissionItems, Geofence &geofence, float home_alt);
+	bool checkMissionFeasibleRotarywing(dm_item_t dm_current, size_t nMissionItems, Geofence &geofence, float home_alt, bool home_valid);
 public:
 
 	MissionFeasibilityChecker();
@@ -80,7 +80,7 @@ public:
 	 * Returns true if mission is feasible and false otherwise
 	 */
 	bool checkMissionFeasible(bool isRotarywing, dm_item_t dm_current, size_t nMissionItems, Geofence &geofence,
-				  float home_alt);
+				  float home_alt, bool home_valid);
 
 };
 
diff --git a/src/modules/navigator/navigator.h b/src/modules/navigator/navigator.h
index 95e3f9f964..782a297fbb 100644
--- a/src/modules/navigator/navigator.h
+++ b/src/modules/navigator/navigator.h
@@ -134,6 +134,7 @@ public:
 	struct vehicle_gps_position_s*	    get_gps_position() { return &_gps_pos; }
 	struct sensor_combined_s*	    get_sensor_combined() { return &_sensor_combined; }
 	struct home_position_s*		    get_home_position() { return &_home_pos; }
+	bool				    home_position_valid() { return _home_position_set; }
 	struct position_setpoint_triplet_s* get_position_setpoint_triplet() { return &_pos_sp_triplet; }
 	struct mission_result_s*	    get_mission_result() { return &_mission_result; }
 	struct geofence_result_s*		    get_geofence_result() { return &_geofence_result; }

From a66b1b9d04c6c200879576e2f4d9609e229606c0 Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Thu, 11 Jun 2015 17:02:11 +0200
Subject: [PATCH 13/25] Improve feedback when auto mode is rejected due to a
 non suitable mission

---
 src/modules/navigator/mission.cpp             |  4 +--
 .../navigator/mission_feasibility_checker.cpp | 25 +++++++++----------
 .../navigator/mission_feasibility_checker.h   |  2 +-
 src/modules/navigator/navigator_main.cpp      |  5 +++-
 4 files changed, 19 insertions(+), 17 deletions(-)

diff --git a/src/modules/navigator/mission.cpp b/src/modules/navigator/mission.cpp
index 4eaeca1d8e..13fcd5daca 100644
--- a/src/modules/navigator/mission.cpp
+++ b/src/modules/navigator/mission.cpp
@@ -230,7 +230,7 @@ Mission::update_offboard_mission()
 		 * 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 = !_missionFeasiblityChecker.checkMissionFeasible(_navigator->get_vstatus()->is_rotary_wing,
+		failed = !_missionFeasiblityChecker.checkMissionFeasible(_navigator->get_mavlink_fd(), _navigator->get_vstatus()->is_rotary_wing,
 				dm_current, (size_t) _offboard_mission.count, _navigator->get_geofence(),
 				_navigator->get_home_position()->alt, _navigator->home_position_valid());
 
@@ -374,7 +374,7 @@ Mission::set_mission_items()
 	} else if (home_dist_ok && read_mission_item(false, true, &_mission_item)) {
 		/* if mission type changed, notify */
 		if (_mission_type != MISSION_TYPE_OFFBOARD) {
-			mavlink_log_critical(_navigator->get_mavlink_fd(), "offboard mission now running");
+			mavlink_log_info(_navigator->get_mavlink_fd(), "offboard mission now running");
 		}
 		_mission_type = MISSION_TYPE_OFFBOARD;
 	} else {
diff --git a/src/modules/navigator/mission_feasibility_checker.cpp b/src/modules/navigator/mission_feasibility_checker.cpp
index 19baa79fcd..9d1dc7c7e6 100644
--- a/src/modules/navigator/mission_feasibility_checker.cpp
+++ b/src/modules/navigator/mission_feasibility_checker.cpp
@@ -63,26 +63,27 @@ MissionFeasibilityChecker::MissionFeasibilityChecker() : _mavlink_fd(-1), _capab
 }
 
 
-bool MissionFeasibilityChecker::checkMissionFeasible(bool isRotarywing, dm_item_t dm_current, size_t nMissionItems, Geofence &geofence, float home_alt, bool home_valid)
+bool MissionFeasibilityChecker::checkMissionFeasible(int mavlink_fd, bool isRotarywing, dm_item_t dm_current, size_t nMissionItems, Geofence &geofence, float home_alt, bool home_valid)
 {
 	bool failed = false;
 	/* Init if not done yet */
 	init();
 
-	/* Open mavlink fd */
-	if (_mavlink_fd < 0) {
-		/* try to open the mavlink log device every once in a while */
-		_mavlink_fd = open(MAVLINK_LOG_DEVICE, 0);
-	}
+	_mavlink_fd = mavlink_fd;
 
 	// check if all mission item commands are supported
 	failed |= !checkMissionItemValidity(dm_current, nMissionItems);
 
 
-	if (isRotarywing)
+	if (isRotarywing) {
 		failed |= !checkMissionFeasibleRotarywing(dm_current, nMissionItems, geofence, home_alt, home_valid);
-	else
+	} else {
 		failed |= !checkMissionFeasibleFixedwing(dm_current, nMissionItems, geofence, home_alt, home_valid);
+	}
+
+	if (!failed) {
+		mavlink_log_info(_mavlink_fd, "Mission checked and ready.");
+	}
 
 	return !failed;
 }
@@ -152,11 +153,10 @@ bool MissionFeasibilityChecker::checkHomePositionAltitude(dm_item_t dm_current,
 			}
 		}
 
+		/* always reject relative alt without home set */
 		if (missionitem.altitude_is_relative && !home_valid) {
-			if (throw_error) {
-				mavlink_log_critical(_mavlink_fd, "Rejecting Mission: No home pos, WP %d uses rel alt", i);
-				return false;
-			}
+			mavlink_log_critical(_mavlink_fd, "Rejecting Mission: No home pos, WP %d uses rel alt", i);
+			return false;
 		}
 
 		/* calculate the global waypoint altitude */
@@ -204,7 +204,6 @@ bool MissionFeasibilityChecker::checkMissionItemValidity(dm_item_t dm_current, s
 			return false;
 		}
 	}
-	mavlink_log_info(_mavlink_fd, "Mission ready.");
 	return true;
 }
 
diff --git a/src/modules/navigator/mission_feasibility_checker.h b/src/modules/navigator/mission_feasibility_checker.h
index 964bd6097d..9c9511be3d 100644
--- a/src/modules/navigator/mission_feasibility_checker.h
+++ b/src/modules/navigator/mission_feasibility_checker.h
@@ -79,7 +79,7 @@ public:
 	/*
 	 * Returns true if mission is feasible and false otherwise
 	 */
-	bool checkMissionFeasible(bool isRotarywing, dm_item_t dm_current, size_t nMissionItems, Geofence &geofence,
+	bool checkMissionFeasible(int mavlink_fd, bool isRotarywing, dm_item_t dm_current, size_t nMissionItems, Geofence &geofence,
 				  float home_alt, bool home_valid);
 
 };
diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp
index 97baa1235f..8cfce50879 100644
--- a/src/modules/navigator/navigator_main.cpp
+++ b/src/modules/navigator/navigator_main.cpp
@@ -209,7 +209,10 @@ Navigator::home_position_update()
 
 	if (updated) {
 		orb_copy(ORB_ID(home_position), _home_pos_sub, &_home_pos);
-		_home_position_set = true;
+
+		if (_home_pos.timestamp > 0) {
+			_home_position_set = true;
+		}
 	}
 }
 

From e94bc7960c027d8e806e026b3c25d79c378a6fe7 Mon Sep 17 00:00:00 2001
From: Pavel Kirienko <pavel.kirienko@gmail.com>
Date: Wed, 10 Jun 2015 21:20:57 +0300
Subject: [PATCH 14/25] libuavcan submodule updated

---
 .gitmodules | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/.gitmodules b/.gitmodules
index 6a4f815d42..b05de95995 100644
--- a/.gitmodules
+++ b/.gitmodules
@@ -4,9 +4,9 @@
 [submodule "NuttX"]
 	path = NuttX
 	url = git://github.com/PX4/NuttX.git
-[submodule "uavcan"]
+[submodule "libuavcan"]
 	path = src/lib/uavcan
-	url = git://github.com/pavel-kirienko/uavcan.git
+	url = git://github.com/UAVCAN/libuavcan.git
 [submodule "Tools/genmsg"]
 	path = Tools/genmsg
 	url = https://github.com/ros/genmsg.git

From 891829d3a770bb98d7fd40ae1b0f7b39a188bbda Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Thu, 11 Jun 2015 19:25:17 +0200
Subject: [PATCH 15/25] Land detector: Protect fixed wing logic from
 false-positives due to bad input data

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

diff --git a/src/modules/land_detector/FixedwingLandDetector.cpp b/src/modules/land_detector/FixedwingLandDetector.cpp
index 6e8393617e..5f7ded9cb2 100644
--- a/src/modules/land_detector/FixedwingLandDetector.cpp
+++ b/src/modules/land_detector/FixedwingLandDetector.cpp
@@ -85,9 +85,16 @@ bool FixedwingLandDetector::update()
 	bool landDetected = false;
 
 	if (hrt_elapsed_time(&_vehicleLocalPosition.timestamp) < 500 * 1000) {
-		_velocity_xy_filtered = 0.95f * _velocity_xy_filtered + 0.05f * sqrtf(_vehicleLocalPosition.vx *
+		float val = 0.95f * _velocity_xy_filtered + 0.05f * sqrtf(_vehicleLocalPosition.vx *
 					_vehicleLocalPosition.vx + _vehicleLocalPosition.vy * _vehicleLocalPosition.vy);
-		_velocity_z_filtered = 0.95f * _velocity_z_filtered + 0.05f * fabsf(_vehicleLocalPosition.vz);
+		if (isfinite(val)) {
+			_velocity_xy_filtered = val;
+		}
+		val = 0.95f * _velocity_z_filtered + 0.05f * fabsf(_vehicleLocalPosition.vz);
+
+		if (isfinite(val)) {
+			_velocity_z_filtered = val;
+		}
 	}
 
 	if (hrt_elapsed_time(&_airspeed.timestamp) < 500 * 1000) {

From 7540aa6b87bc8d958f0818ac2ff8a43e7942f2ca Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Thu, 11 Jun 2015 21:05:38 +0200
Subject: [PATCH 16/25] Navigator: Make logic using previous and current
 altitudes consistent

---
 src/modules/navigator/mission.cpp | 21 ++++++++++++++-------
 src/modules/navigator/mission.h   |  2 ++
 2 files changed, 16 insertions(+), 7 deletions(-)

diff --git a/src/modules/navigator/mission.cpp b/src/modules/navigator/mission.cpp
index 13fcd5daca..1d47167a04 100644
--- a/src/modules/navigator/mission.cpp
+++ b/src/modules/navigator/mission.cpp
@@ -271,6 +271,16 @@ Mission::advance_mission()
 	}
 }
 
+int
+Mission::get_absolute_altitude_for_item(struct mission_item_s &mission_item)
+{
+	if (_mission_item.altitude_is_relative) {
+		return _mission_item.altitude + _navigator->get_home_position()->alt;
+	} else {
+		return _mission_item.altitude;
+	}
+}
+
 bool
 Mission::check_dist_1wp()
 {
@@ -354,7 +364,7 @@ Mission::set_mission_items()
 
 	/* Copy previous mission item altitude (can be extended to a copy of the full mission item if needed) */
 	if (pos_sp_triplet->previous.valid) {
-		_mission_item_previous_alt = _mission_item.altitude;
+		_mission_item_previous_alt = get_absolute_altitude_for_item(_mission_item);
 	}
 
 	/* get home distance state */
@@ -440,10 +450,7 @@ Mission::set_mission_items()
 		mission_item_to_position_setpoint(&_mission_item, &pos_sp_triplet->next);
 
 		/* calculate takeoff altitude */
-		float takeoff_alt = _mission_item.altitude;
-		if (_mission_item.altitude_is_relative) {
-			takeoff_alt += _navigator->get_home_position()->alt;
-		}
+		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 */
 		if (_navigator->get_vstatus()->condition_landed) {
@@ -609,14 +616,14 @@ Mission::altitude_sp_foh_update()
 	/* if the minimal distance is smaller then the acceptance radius, we should be at waypoint alt
 	 * navigator will soon switch to the next waypoint item (if there is one) as soon as we reach this altitude */
 	if (_min_current_sp_distance_xy < _navigator->get_acceptance_radius(_mission_item.acceptance_radius)) {
-		pos_sp_triplet->current.alt = _mission_item.altitude;
+		pos_sp_triplet->current.alt = get_absolute_altitude_for_item(_mission_item);
 	} else {
 		/* update the altitude sp of the 'current' item in the sp triplet, but do not update the altitude sp
 		* of the mission item as it is used to check if the mission item is reached
 		* The setpoint is set linearly and such that the system reaches the current altitude at the acceptance
 		* radius around the current waypoint
 		**/
-		float delta_alt = (_mission_item.altitude - _mission_item_previous_alt);
+		float delta_alt = (get_absolute_altitude_for_item(_mission_item) - _mission_item_previous_alt);
 		float grad = -delta_alt/(_distance_current_previous - _navigator->get_acceptance_radius(_mission_item.acceptance_radius));
 		float a = _mission_item_previous_alt - grad * _distance_current_previous;
 		pos_sp_triplet->current.alt = a + grad * _min_current_sp_distance_xy;
diff --git a/src/modules/navigator/mission.h b/src/modules/navigator/mission.h
index e9f78e8fdc..bc9a2c6c82 100644
--- a/src/modules/navigator/mission.h
+++ b/src/modules/navigator/mission.h
@@ -132,6 +132,8 @@ private:
 	 */
 	void altitude_sp_foh_reset();
 
+	int get_absolute_altitude_for_item(struct mission_item_s &mission_item);
+
 	/**
 	 * Read current or next mission item from the dataman and watch out for DO_JUMPS
 	 * @return true if successful

From 315683124d10cc34673d9993f2340ae0f0378b20 Mon Sep 17 00:00:00 2001
From: Andreas Antener <antener_a@gmx.ch>
Date: Fri, 12 Jun 2015 08:47:46 +0200
Subject: [PATCH 17/25] fix posctl th param meta

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

diff --git a/src/modules/sensors/sensor_params.c b/src/modules/sensors/sensor_params.c
index 1bebea2060..7b3932638b 100644
--- a/src/modules/sensors/sensor_params.c
+++ b/src/modules/sensors/sensor_params.c
@@ -1285,7 +1285,7 @@ PARAM_DEFINE_FLOAT(RC_AUTO_TH, 0.75f);
  *
  * @min -1
  * @max 1
- *
+ * @group Radio Switches
  *
  */
 PARAM_DEFINE_FLOAT(RC_POSCTL_TH, 0.5f);

From 863a6e88119384f4aa5776f5c9e264e06697632b Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Fri, 12 Jun 2015 09:27:15 +0200
Subject: [PATCH 18/25] HIL driver: Fix boot order race

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

diff --git a/src/drivers/hil/hil.cpp b/src/drivers/hil/hil.cpp
index 961ec47246..a7c2e83e34 100644
--- a/src/drivers/hil/hil.cpp
+++ b/src/drivers/hil/hil.cpp
@@ -331,10 +331,9 @@ HIL::task_main()
 	/* advertise the mixed control outputs */
 	actuator_outputs_s outputs;
 	memset(&outputs, 0, sizeof(outputs));
-	/* advertise the mixed control outputs */
-	int dummy;
-	_t_outputs = orb_advertise_multi(ORB_ID(actuator_outputs),
-				   &outputs, &dummy, ORB_PRIO_LOW);
+	/* advertise the mixed control outputs, insist on the first group output */
+	_t_outputs = orb_advertise(ORB_ID(actuator_outputs),
+				   &outputs);
 
 	pollfd fds[2];
 	fds[0].fd = _t_actuators;

From 57279856788d3e879ee76ae827791e25be3d8f3d Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Fri, 12 Jun 2015 09:31:10 +0200
Subject: [PATCH 19/25] Comment out unused pca8574 driver in boot

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

diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS
index 13b35313e7..8a93737ef8 100644
--- a/ROMFS/px4fmu_common/init.d/rcS
+++ b/ROMFS/px4fmu_common/init.d/rcS
@@ -101,9 +101,10 @@ then
 		fi
 	fi
 
-	if pca8574 start
-	then
-	fi
+	# Currently unused, but might be useful down the road
+	#if pca8574 start
+	#then
+	#fi
 
 	#
 	# Set default values

From 267fb408b1f5cfc27235b70f5dc8457e135622b7 Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Fri, 12 Jun 2015 11:08:30 +0200
Subject: [PATCH 20/25] Update MC thrust limit default param value and add
 explanation

---
 src/modules/mc_pos_control/mc_pos_control_params.c | 7 +++++--
 1 file changed, 5 insertions(+), 2 deletions(-)

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 fc73b31f59..ade43ffb91 100644
--- a/src/modules/mc_pos_control/mc_pos_control_params.c
+++ b/src/modules/mc_pos_control/mc_pos_control_params.c
@@ -55,13 +55,16 @@ PARAM_DEFINE_FLOAT(MPC_THR_MIN, 0.1f);
 /**
  * Maximum thrust
  *
- * Limit max allowed thrust.
+ * Limit max allowed thrust. Setting a value of one can put
+ * the system into actuator saturation as no spread between
+ * the motors is possible any more. A value of 0.8 - 0.9
+ * is recommended.
  *
  * @min 0.0
  * @max 1.0
  * @group Multicopter Position Control
  */
-PARAM_DEFINE_FLOAT(MPC_THR_MAX, 1.0f);
+PARAM_DEFINE_FLOAT(MPC_THR_MAX, 0.9f);
 
 /**
  * Proportional gain for vertical position error

From 7e44a234118eb8b940f2323f10a6023c1ce5f4a5 Mon Sep 17 00:00:00 2001
From: tumbili <bapstr@ethz.ch>
Date: Fri, 12 Jun 2015 13:50:02 +0200
Subject: [PATCH 21/25] fix: take current trim values into account when doing
 trim calibration

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

diff --git a/src/modules/commander/rc_calibration.cpp b/src/modules/commander/rc_calibration.cpp
index 08261c9890..c9a8343b66 100644
--- a/src/modules/commander/rc_calibration.cpp
+++ b/src/modules/commander/rc_calibration.cpp
@@ -68,16 +68,26 @@ int do_trim_calibration(int mavlink_fd)
 
 	orb_copy(ORB_ID(manual_control_setpoint), sub_man, &sp);
 
-	/* set parameters */
-	float p = sp.y;
+	/* load trim values which are active */
+	float roll_trim_active;
+	param_get(param_find("TRIM_ROLL"), &roll_trim_active);
+	float pitch_trim_active;
+	param_get(param_find("TRIM_PITCH"), &pitch_trim_active);
+	float yaw_trim_active;
+	param_get(param_find("TRIM_YAW"), &yaw_trim_active);
+
+	/* 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;
 	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;
+	p = -sp.x + pitch_trim_active;
 	int p2r = param_set(param_find("TRIM_PITCH"), &p);
-	p = sp.r;
+	p = sp.r + yaw_trim_active;
 	int p3r = param_set(param_find("TRIM_YAW"), &p);
 
 	/* store to permanent storage */

From 540ffa7861c0583ed6b13bfa1fdfab04193fd74b Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Fri, 12 Jun 2015 14:02:47 +0200
Subject: [PATCH 22/25] Let user know we are loitering now

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

diff --git a/src/modules/navigator/mission.cpp b/src/modules/navigator/mission.cpp
index 1d47167a04..fe876ee8b1 100644
--- a/src/modules/navigator/mission.cpp
+++ b/src/modules/navigator/mission.cpp
@@ -390,7 +390,8 @@ Mission::set_mission_items()
 	} else {
 		/* no mission available or mission finished, switch to loiter */
 		if (_mission_type != MISSION_TYPE_NONE) {
-			mavlink_log_critical(_navigator->get_mavlink_fd(), "mission finished");
+			/* https://en.wikipedia.org/wiki/Loiter_(aeronautics) */
+			mavlink_log_critical(_navigator->get_mavlink_fd(), "mission finished, loitering");
 
 			/* use last setpoint for loiter */
 			_navigator->set_can_loiter_at_sp(true);
@@ -398,8 +399,9 @@ Mission::set_mission_items()
 		} else if (!user_feedback_done) {
 			/* only tell users that we got no mission if there has not been any
 			 * better, more specific feedback yet
+			 * https://en.wikipedia.org/wiki/Loiter_(aeronautics)
 			 */
-			mavlink_log_critical(_navigator->get_mavlink_fd(), "no valid mission available");
+			mavlink_log_critical(_navigator->get_mavlink_fd(), "no valid mission available, loitering");
 		}
 		_mission_type = MISSION_TYPE_NONE;
 

From 428611119fe7b4cd27695e9435edea4ff0582f23 Mon Sep 17 00:00:00 2001
From: David Sidrane <david_s5@nscdg.com>
Date: Fri, 12 Jun 2015 06:49:15 -1000
Subject: [PATCH 23/25] Merged GIT version changes

---
 Makefile                            | 51 +++++++++++++++++++++++++----
 makefiles/setup.mk                  |  4 ++-
 src/modules/systemlib/git_version.h |  2 ++
 src/modules/systemlib/module.mk     |  4 +--
 src/systemcmds/ver/ver.c            |  3 ++
 5 files changed, 55 insertions(+), 9 deletions(-)

diff --git a/Makefile b/Makefile
index 47fe326b92..aedf595aef 100644
--- a/Makefile
+++ b/Makefile
@@ -51,10 +51,6 @@ endif
 
 GIT_DESC_SHORT := $(shell echo $(GIT_DESC) | cut -c1-16)
 
-$(shell echo "#include <systemlib/git_version.h>" > $(BUILD_DIR)git_version.c)
-$(shell echo "const char* px4_git_version = \"$(GIT_DESC)\";" >> $(BUILD_DIR)git_version.c)
-$(shell echo "const uint64_t px4_git_version_binary = 0x$(GIT_DESC_SHORT);" >> $(BUILD_DIR)git_version.c)
-
 #
 # Canned firmware configurations that we (know how to) build.
 #
@@ -129,7 +125,7 @@ $(STAGED_FIRMWARES): $(IMAGE_DIR)%.px4: $(BUILD_DIR)%.build/firmware.px4
 .PHONY: $(FIRMWARES)
 $(BUILD_DIR)%.build/firmware.px4: config   = $(patsubst $(BUILD_DIR)%.build/firmware.px4,%,$@)
 $(BUILD_DIR)%.build/firmware.px4: work_dir = $(BUILD_DIR)$(config).build/
-$(FIRMWARES): $(BUILD_DIR)%.build/firmware.px4:	generateuorbtopicheaders checksubmodules
+$(FIRMWARES): $(BUILD_DIR)%.build/firmware.px4:	checkgitversion generateuorbtopicheaders checksubmodules
 	@$(ECHO) %%%%
 	@$(ECHO) %%%% Building $(config) in $(work_dir)
 	@$(ECHO) %%%%
@@ -216,11 +212,53 @@ menuconfig:
 	@$(ECHO) ""
 endif
 
-$(NUTTX_SRC): checksubmodules
+$(NUTTX_SRC): checkgitversion checksubmodules
 
 $(UAVCAN_DIR):
 	$(Q) (./Tools/check_submodules.sh)
 
+
+ifeq ($(PX4_TARGET_OS),nuttx)
+# TODO
+# Move the above nuttx specific rules into $(PX4_BASE)makefiles/firmware_nuttx.mk
+endif
+ifeq ($(PX4_TARGET_OS),posix)
+include $(PX4_BASE)makefiles/firmware_posix.mk
+endif
+ifeq ($(PX4_TARGET_OS),posix-arm)
+include $(PX4_BASE)makefiles/firmware_posix.mk
+endif
+ifeq ($(PX4_TARGET_OS),qurt)
+include $(PX4_BASE)makefiles/firmware_qurt.mk
+endif
+
+#
+# Versioning
+#
+
+GIT_VER_FILE = $(PX4_VERSIONING_DIR).build_git_ver
+GIT_HEADER_FILE = $(PX4_VERSIONING_DIR)build_git_version.h
+
+$(GIT_VER_FILE) :
+	$(Q) if [ ! -f $(GIT_VER_FILE) ]; then \
+		$(MKDIR) -p $(PX4_VERSIONING_DIR); \
+		$(ECHO) "" > $(GIT_VER_FILE); \
+	fi
+
+.PHONY: checkgitversion
+checkgitversion: $(GIT_VER_FILE)
+	$(Q) if [ "$(GIT_DESC)" != "$(shell cat $(GIT_VER_FILE))" ]; then \
+		$(ECHO) "/* Auto Magically Generated file */" > $(GIT_HEADER_FILE); \
+		$(ECHO) "/* Do not edit! */" >> $(GIT_HEADER_FILE); \
+		$(ECHO) "#define PX4_GIT_VERSION_STR  \"$(GIT_DESC)\"" >> $(GIT_HEADER_FILE); \
+		$(ECHO) "#define PX4_GIT_VERSION_BINARY 0x$(GIT_DESC_SHORT)" >> $(GIT_HEADER_FILE); \
+		$(ECHO) $(GIT_DESC) > $(GIT_VER_FILE); \
+	fi
+
+#
+# Submodule Checks
+#
+
 .PHONY: checksubmodules
 checksubmodules:
 	$(Q) ($(PX4_BASE)/Tools/check_submodules.sh)
@@ -280,6 +318,7 @@ check_format:
 clean:
 	@echo > /dev/null
 	$(Q) $(RMDIR) $(BUILD_DIR)*.build
+	$(Q) $(RMDIR) $(PX4_VERSIONING_DIR)
 	$(Q) $(REMOVE) $(IMAGE_DIR)*.px4
 
 .PHONY:	distclean
diff --git a/makefiles/setup.mk b/makefiles/setup.mk
index 351b9f1b71..ead85e7fc9 100644
--- a/makefiles/setup.mk
+++ b/makefiles/setup.mk
@@ -55,6 +55,7 @@ export ROMFS_SRC	 = $(abspath $(PX4_BASE)/ROMFS)/
 export IMAGE_DIR	 = $(abspath $(PX4_BASE)/Images)/
 export BUILD_DIR	 = $(abspath $(PX4_BASE)/Build)/
 export ARCHIVE_DIR	 = $(abspath $(PX4_BASE)/Archives)/
+export PX4_VERSIONING_DIR = $(BUILD_DIR)versioning/
 
 #
 # Default include paths
@@ -63,7 +64,8 @@ export INCLUDE_DIRS	:= $(PX4_MODULE_SRC) \
 			   $(PX4_MODULE_SRC)/modules/ \
 			   $(PX4_INCLUDE_DIR) \
 			   $(PX4_LIB_DIR) \
-			   $(PX4_PLATFORMS_DIR)
+			   $(PX4_PLATFORMS_DIR) \
+			   $(PX4_VERSIONING_DIR)
 
 #
 # Tools
diff --git a/src/modules/systemlib/git_version.h b/src/modules/systemlib/git_version.h
index 9b8f9e860e..5f8d411088 100644
--- a/src/modules/systemlib/git_version.h
+++ b/src/modules/systemlib/git_version.h
@@ -41,6 +41,8 @@
 
 #include <stdint.h>
 
+#include "build_git_version.h"
+
 __BEGIN_DECLS
 
 __EXPORT extern const char* px4_git_version;
diff --git a/src/modules/systemlib/module.mk b/src/modules/systemlib/module.mk
index ff409fa473..81e7d87536 100644
--- a/src/modules/systemlib/module.mk
+++ b/src/modules/systemlib/module.mk
@@ -55,8 +55,8 @@ SRCS		 = err.c \
 		   pwm_limit/pwm_limit.c \
 		   circuit_breaker.cpp \
 		   circuit_breaker_params.c \
-		   mcu_version.c \
-		   $(BUILD_DIR)git_version.c
+		   mcu_version.c
+		   circuit_breaker_params.c
 
 MAXOPTIMIZATION	 = -Os
 
diff --git a/src/systemcmds/ver/ver.c b/src/systemcmds/ver/ver.c
index a248dfb79c..caac93024a 100644
--- a/src/systemcmds/ver/ver.c
+++ b/src/systemcmds/ver/ver.c
@@ -57,6 +57,9 @@ static const char sz_ver_all_str[] 	= "all";
 static const char mcu_ver_str[]		= "mcu";
 static const char mcu_uid_str[]		= "uid";
 
+const char* px4_git_version = PX4_GIT_VERSION_STR;
+const uint64_t px4_git_version_binary = PX4_GIT_VERSION_BINARY;
+
 static void usage(const char *reason)
 {
 	if (reason != NULL) {

From 9585bb4a3c35f9b02574a61c5ece2e6952f15aa2 Mon Sep 17 00:00:00 2001
From: David Sidrane <david_s5@nscdg.com>
Date: Fri, 12 Jun 2015 07:54:51 -1000
Subject: [PATCH 24/25] Missing slash

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

diff --git a/src/modules/systemlib/module.mk b/src/modules/systemlib/module.mk
index 81e7d87536..f80d8009ae 100644
--- a/src/modules/systemlib/module.mk
+++ b/src/modules/systemlib/module.mk
@@ -55,7 +55,7 @@ SRCS		 = err.c \
 		   pwm_limit/pwm_limit.c \
 		   circuit_breaker.cpp \
 		   circuit_breaker_params.c \
-		   mcu_version.c
+		   mcu_version.c \
 		   circuit_breaker_params.c
 
 MAXOPTIMIZATION	 = -Os

From a0176474c7740a4a04c2eb91eba57d8ce64027a1 Mon Sep 17 00:00:00 2001
From: Elikos default <info@elikos.polymtl.ca>
Date: Thu, 11 Jun 2015 20:28:47 -0400
Subject: [PATCH 25/25] fix NaN yaw breaking attitude setpoints when going back
 into posctl from offboard

---
 src/modules/mavlink/mavlink_receiver.cpp    | 4 ++--
 src/platforms/ros/nodes/mavlink/mavlink.cpp | 4 ++--
 2 files changed, 4 insertions(+), 4 deletions(-)

diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp
index 7ede93ec2d..1a6494ad93 100644
--- a/src/modules/mavlink/mavlink_receiver.cpp
+++ b/src/modules/mavlink/mavlink_receiver.cpp
@@ -621,7 +621,7 @@ MavlinkReceiver::handle_message_set_position_target_local_ned(mavlink_message_t
 					}
 
 					/* set the yaw sp value */
-					if (!offboard_control_mode.ignore_attitude) {
+					if (!offboard_control_mode.ignore_attitude && !isnan(set_position_target_local_ned.yaw)) {
 						pos_sp_triplet.current.yaw_valid = true;
 						pos_sp_triplet.current.yaw = set_position_target_local_ned.yaw;
 
@@ -630,7 +630,7 @@ MavlinkReceiver::handle_message_set_position_target_local_ned(mavlink_message_t
 					}
 
 					/* set the yawrate sp value */
-					if (!offboard_control_mode.ignore_bodyrate) {
+					if (!offboard_control_mode.ignore_bodyrate && !isnan(set_position_target_local_ned.yaw)) {
 						pos_sp_triplet.current.yawspeed_valid = true;
 						pos_sp_triplet.current.yawspeed = set_position_target_local_ned.yaw_rate;
 
diff --git a/src/platforms/ros/nodes/mavlink/mavlink.cpp b/src/platforms/ros/nodes/mavlink/mavlink.cpp
index 2758979a29..c92dd0843d 100644
--- a/src/platforms/ros/nodes/mavlink/mavlink.cpp
+++ b/src/platforms/ros/nodes/mavlink/mavlink.cpp
@@ -273,7 +273,7 @@ void Mavlink::handle_msg_set_position_target_local_ned(const mavlink_message_t *
 		}
 
 		/* set the yaw sp value */
-		if (!offboard_control_mode.ignore_attitude) {
+		if (!offboard_control_mode.ignore_attitude && !isnan(set_position_target_local_ned.yaw)) {
 			pos_sp_triplet.current.yaw_valid = true;
 			pos_sp_triplet.current.yaw = set_position_target_local_ned.yaw;
 
@@ -282,7 +282,7 @@ void Mavlink::handle_msg_set_position_target_local_ned(const mavlink_message_t *
 		}
 
 		/* set the yawrate sp value */
-		if (!offboard_control_mode.ignore_bodyrate) {
+		if (!offboard_control_mode.ignore_bodyrate && !isnan(set_position_target_local_ned.yaw)) {
 			pos_sp_triplet.current.yawspeed_valid = true;
 			pos_sp_triplet.current.yawspeed = set_position_target_local_ned.yaw_rate;