Merge remote-tracking branch 'upstream/master' into dev_ros

Conflicts:
	.gitignore
	src/lib/uavcan

27 files changed, 477 insertions, 161 deletions

diff --git a/src/drivers/mpu6000/mpu6000.cpp b/src/drivers/mpu6000/mpu6000.cpp
index a95e041a1..c9c27892f 100644
--- a/src/drivers/mpu6000/mpu6000.cpp
+++ b/src/drivers/mpu6000/mpu6000.cpp
@@ -194,6 +194,8 @@ public:
 	 */
 	void			print_info();
 
+	void			print_registers();
+
 protected:
 	virtual int		probe();
 
@@ -1414,6 +1416,21 @@ MPU6000::print_info()
 	_gyro_reports->print_info("gyro queue");
 }
 
+void
+MPU6000::print_registers()
+{
+	printf("MPU6000 registers\n");
+	for (uint8_t reg=MPUREG_PRODUCT_ID; reg<=108; reg++) {
+		uint8_t v = read_reg(reg);
+		printf("%02x:%02x ",(unsigned)reg, (unsigned)v);
+		if ((reg - (MPUREG_PRODUCT_ID-1)) % 13 == 0) {
+			printf("\n");
+		}
+	}
+	printf("\n");
+}
+
+
 MPU6000_gyro::MPU6000_gyro(MPU6000 *parent, const char *path) :
 	CDev("MPU6000_gyro", path),
 	_parent(parent),
@@ -1479,6 +1496,7 @@ void	start(bool, enum Rotation);
 void	test(bool);
 void	reset(bool);
 void	info(bool);
+void	regdump(bool);
 void	usage();
 
 /**
@@ -1654,10 +1672,26 @@ info(bool external_bus)
 	exit(0);
 }
 
+/**
+ * Dump the register information
+ */
+void
+regdump(bool external_bus)
+{
+	MPU6000 **g_dev_ptr = external_bus?&g_dev_ext:&g_dev_int;
+	if (*g_dev_ptr == nullptr)
+		errx(1, "driver not running");
+
+	printf("regdump @ %p\n", *g_dev_ptr);
+	(*g_dev_ptr)->print_registers();
+
+	exit(0);
+}
+
 void
 usage()
 {
-	warnx("missing command: try 'start', 'info', 'test', 'reset'");
+	warnx("missing command: try 'start', 'info', 'test', 'reset', 'regdump'");
 	warnx("options:");
 	warnx("    -X    (external bus)");
 	warnx("    -R rotation");
@@ -1714,5 +1748,11 @@ mpu6000_main(int argc, char *argv[])
 	if (!strcmp(verb, "info"))
 		mpu6000::info(external_bus);
 
-	errx(1, "unrecognized command, try 'start', 'test', 'reset' or 'info'");
+	/*
+	 * Print register information.
+	 */
+	if (!strcmp(verb, "regdump"))
+		mpu6000::regdump(external_bus);
+
+	errx(1, "unrecognized command, try 'start', 'test', 'reset', 'info' or 'regdump'");
 }
diff --git a/src/drivers/stm32/drv_hrt.c b/src/drivers/stm32/drv_hrt.c
index 281f918d7..603c2eb9d 100644
--- a/src/drivers/stm32/drv_hrt.c
+++ b/src/drivers/stm32/drv_hrt.c
@@ -253,9 +253,11 @@ static uint16_t			latency_baseline;
 static uint16_t			latency_actual;
 
 /* latency histogram */
-#define LATENCY_BUCKET_COUNT	8
-static const uint16_t		latency_buckets[LATENCY_BUCKET_COUNT] = { 1, 2, 5, 10, 20, 50, 100, 1000 };
-static uint32_t			latency_counters[LATENCY_BUCKET_COUNT + 1];
+#define LATENCY_BUCKET_COUNT 8
+__EXPORT const uint16_t latency_bucket_count = LATENCY_BUCKET_COUNT;
+__EXPORT const uint16_t	latency_buckets[LATENCY_BUCKET_COUNT] = { 1, 2, 5, 10, 20, 50, 100, 1000 };
+__EXPORT uint32_t		latency_counters[LATENCY_BUCKET_COUNT + 1];
+
 
 /* timer-specific functions */
 static void		hrt_tim_init(void);
diff --git a/src/lib/uavcan b/src/lib/uavcan
new file mode 160000
+Subproject 6a193648985adab9665e31a9460be04bc91d896
diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp
index 744323c01..9262f9e81 100644
--- a/src/modules/commander/commander.cpp
+++ b/src/modules/commander/commander.cpp
@@ -81,6 +81,7 @@
 #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>
 
@@ -926,6 +927,11 @@ int commander_thread_main(int argc, char *argv[])
 	struct mission_result_s mission_result;
 	memset(&mission_result, 0, sizeof(mission_result));
 
+	/* Subscribe to geofence result topic */
+	int geofence_result_sub = orb_subscribe(ORB_ID(geofence_result));
+	struct geofence_result_s geofence_result;
+	memset(&geofence_result, 0, sizeof(geofence_result));
+
 	/* Subscribe to manual control data */
 	int sp_man_sub = orb_subscribe(ORB_ID(manual_control_setpoint));
 	struct manual_control_setpoint_s sp_man;
@@ -1545,27 +1551,34 @@ int commander_thread_main(int argc, char *argv[])
 
 		if (updated) {
 			orb_copy(ORB_ID(mission_result), mission_result_sub, &mission_result);
+		}
 
-			/* Check for geofence violation */
-			if (armed.armed && (mission_result.geofence_violated || mission_result.flight_termination)) {
-				//XXX: make this configurable to select different actions (e.g. navigation modes)
-				/* this will only trigger if geofence is activated via param and a geofence file is present, also there is a circuit breaker to disable the actual flight termination in the px4io driver */
-				armed.force_failsafe = true;
-				status_changed = true;
-				static bool flight_termination_printed = false;
-
-				if (!flight_termination_printed) {
-					warnx("Flight termination because of navigator request or geofence");
-					mavlink_log_critical(mavlink_fd, "GF violation: flight termination");
-					flight_termination_printed = true;
-				}
+		/* start geofence result check */
+		orb_check(geofence_result_sub, &updated);
 
-				if (counter % (1000000 / COMMANDER_MONITORING_INTERVAL) == 0) {
-					mavlink_log_critical(mavlink_fd, "GF violation: flight termination");
-				}
-			} // no reset is done here on purpose, on geofence violation we want to stay in flighttermination
+		if (updated) {
+			orb_copy(ORB_ID(geofence_result), geofence_result_sub, &geofence_result);
 		}
 
+		/* Check for geofence violation */
+		if (armed.armed && (geofence_result.geofence_violated || mission_result.flight_termination)) {
+			//XXX: make this configurable to select different actions (e.g. navigation modes)
+			/* this will only trigger if geofence is activated via param and a geofence file is present, also there is a circuit breaker to disable the actual flight termination in the px4io driver */
+			armed.force_failsafe = true;
+			status_changed = true;
+			static bool flight_termination_printed = false;
+
+			if (!flight_termination_printed) {
+				warnx("Flight termination because of navigator request or geofence");
+				mavlink_log_critical(mavlink_fd, "GF violation: flight termination");
+				flight_termination_printed = true;
+			}
+
+			if (counter % (1000000 / COMMANDER_MONITORING_INTERVAL) == 0) {
+				mavlink_log_critical(mavlink_fd, "GF violation: flight termination");
+			}
+		} // no reset is done here on purpose, on geofence violation we want to stay in flighttermination
+
 		/* RC input check */
 		if (!status.rc_input_blocked && sp_man.timestamp != 0 &&
 		    hrt_absolute_time() < sp_man.timestamp + (uint64_t)(rc_loss_timeout * 1e6f)) {
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 3668f9211..74249c9c5 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
@@ -202,9 +202,11 @@ private:
 	ECL_L1_Pos_Controller				_l1_control;
 	TECS						_tecs;
 	fwPosctrl::mTecs				_mTecs;
-	bool						_was_pos_control_mode;
-	bool						_was_velocity_control_mode;
-	bool						_was_alt_control_mode;
+	enum FW_POSCTRL_MODE {
+		FW_POSCTRL_MODE_AUTO,
+		FW_POSCTRL_MODE_POSITION,
+		FW_POSCTRL_MODE_OTHER
+	} _control_mode_current;			///< used to check the mode in the last control loop iteration. Use to check if the last iteration was in the same mode.
 
 	struct {
 		float l1_period;
@@ -473,7 +475,7 @@ FixedwingPositionControl::FixedwingPositionControl() :
 	_global_pos_valid(false),
 	_l1_control(),
 	_mTecs(),
-	_was_pos_control_mode(false)
+	_control_mode_current(FW_POSCTRL_MODE_OTHER)
 {
 	_nav_capabilities.turn_distance = 0.0f;
 
@@ -910,22 +912,19 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
 	/* no throttle limit as default */
 	float throttle_max = 1.0f;
 
-	/* AUTONOMOUS FLIGHT */
+	if (_control_mode.flag_control_auto_enabled &&
+			pos_sp_triplet.current.valid) {
+		/* AUTONOMOUS FLIGHT */
 
-	// XXX this should only execute if auto AND safety off (actuators active),
-	// else integrators should be constantly reset.
-	if (pos_sp_triplet.current.valid) {
-
-		if (!_was_pos_control_mode) {
+		/* Reset integrators if switching to this mode from a other mode in which posctl was not active */
+		if (_control_mode_current == FW_POSCTRL_MODE_OTHER) {
 			/* reset integrators */
 			if (_mTecs.getEnabled()) {
 				_mTecs.resetIntegrators();
 				_mTecs.resetDerivatives(_airspeed.true_airspeed_m_s);
 			}
 		}
-
-		_was_pos_control_mode = true;
-		_was_velocity_control_mode = false;
+		_control_mode_current = FW_POSCTRL_MODE_AUTO;
 
 		/* reset hold altitude */
 		_hold_alt = _global_pos.alt;
@@ -1210,15 +1209,15 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
 					tecs_update_pitch_throttle(_pos_sp_triplet.current.alt,
 							calculate_target_airspeed(_parameters.airspeed_trim),
 							eas2tas,
-								math::radians(_parameters.pitch_limit_min),
-								math::radians(_parameters.pitch_limit_max),
-								_parameters.throttle_min,
-								takeoff_throttle,
-								_parameters.throttle_cruise,
-								false,
-								math::radians(_parameters.pitch_limit_min),
-								_global_pos.alt,
-								ground_speed);
+							math::radians(_parameters.pitch_limit_min),
+							math::radians(_parameters.pitch_limit_max),
+							_parameters.throttle_min,
+							takeoff_throttle,
+							_parameters.throttle_cruise,
+							false,
+							math::radians(_parameters.pitch_limit_min),
+							_global_pos.alt,
+							ground_speed);
 				}
 			} else {
 				/* Tell the attitude controller to stop integrating while we are waiting
@@ -1250,53 +1249,63 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
 			_att_sp.roll_reset_integral = true;
 		}
 
-	} else if (_control_mode.flag_control_velocity_enabled) {
+	} else if (_control_mode.flag_control_velocity_enabled &&
+			_control_mode.flag_control_altitude_enabled) {
+		/* POSITION CONTROL: pitch stick moves altitude setpoint, throttle stick sets airspeed */
+
 		const float deadBand = (60.0f/1000.0f);
 		const float factor = 1.0f - deadBand;
-		if (!_was_velocity_control_mode) {
+		if (_control_mode_current != FW_POSCTRL_MODE_POSITION) {
+			/* Need to init because last loop iteration was in a different mode */
 			_hold_alt = _global_pos.alt;
-			_was_alt_control_mode = false;
 		}
-		_was_velocity_control_mode = true;
-		_was_pos_control_mode = false;
-		// Get demanded airspeed
+		/* Reset integrators if switching to this mode from a other mode in which posctl was not active */
+		if (_control_mode_current == FW_POSCTRL_MODE_OTHER) {
+			/* reset integrators */
+			if (_mTecs.getEnabled()) {
+				_mTecs.resetIntegrators();
+				_mTecs.resetDerivatives(_airspeed.true_airspeed_m_s);
+			}
+		}
+		_control_mode_current = FW_POSCTRL_MODE_POSITION;
+
+		/* Get demanded airspeed */
 		float altctrl_airspeed = _parameters.airspeed_min +
 					  (_parameters.airspeed_max - _parameters.airspeed_min) *
 					  _manual.z;
 
-		// Get demanded vertical velocity from pitch control
-		float pitch = 0.0f;
+		/* Get demanded vertical velocity from pitch control */
+		static bool was_in_deadband = false;
 		if (_manual.x > deadBand) {
-			pitch = (_manual.x - deadBand) / factor;
-		} else if (_manual.x < - deadBand) {
-			pitch = (_manual.x + deadBand) / factor;
-		}
-		if (pitch > 0.0f) {
+			float pitch = (_manual.x - deadBand) / factor;
 			_hold_alt -= (_parameters.max_climb_rate * dt) * pitch;
-			_was_alt_control_mode = false;
-		} else if (pitch < 0.0f) {
+			was_in_deadband = false;
+		} else if (_manual.x < - deadBand) {
+			float pitch = (_manual.x + deadBand) / factor;
 			_hold_alt -= (_parameters.max_sink_rate * dt) * pitch;
-			_was_alt_control_mode = false;
-		} else if (!_was_alt_control_mode) {
+			was_in_deadband = false;
+		} else if (!was_in_deadband) {
+			 /* store altitude at which manual.x was inside deadBand
+			  * The aircraft should immediately try to fly at this altitude
+			  * as this is what the pilot expects when he moves the stick to the center */
 			_hold_alt = _global_pos.alt;
-			_was_alt_control_mode = true;
+			was_in_deadband = true;
 		}
 		tecs_update_pitch_throttle(_hold_alt,
-									altctrl_airspeed,
-									eas2tas,
-									math::radians(_parameters.pitch_limit_min),
-									math::radians(_parameters.pitch_limit_max),
-									_parameters.throttle_min,
-									_parameters.throttle_max,
-									_parameters.throttle_cruise,
-									false,
-									math::radians(_parameters.pitch_limit_min),
-									_global_pos.alt,
-									ground_speed,
-									TECS_MODE_NORMAL);
+				altctrl_airspeed,
+				eas2tas,
+				math::radians(_parameters.pitch_limit_min),
+				math::radians(_parameters.pitch_limit_max),
+				_parameters.throttle_min,
+				_parameters.throttle_max,
+				_parameters.throttle_cruise,
+				false,
+				math::radians(_parameters.pitch_limit_min),
+				_global_pos.alt,
+				ground_speed,
+				TECS_MODE_NORMAL);
 	} else {
-		_was_velocity_control_mode = false;
-		_was_pos_control_mode = false;
+		_control_mode_current = FW_POSCTRL_MODE_OTHER;
 
 		/** MANUAL FLIGHT **/
 
@@ -1313,10 +1322,12 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
 		}
 	}
 
+	/* Copy thrust output for publication */
 	if (_vehicle_status.engine_failure || _vehicle_status.engine_failure_cmd) {
 		/* Set thrust to 0 to minimize damage */
 		_att_sp.thrust = 0.0f;
-	} else if (pos_sp_triplet.current.type == SETPOINT_TYPE_TAKEOFF &&
+	} else if (_control_mode_current ==  FW_POSCTRL_MODE_AUTO && // launchdetector only available in auto
+			pos_sp_triplet.current.type == SETPOINT_TYPE_TAKEOFF &&
 			launch_detection_state != LAUNCHDETECTION_RES_DETECTED_ENABLEMOTORS) {
 		 /* making sure again that the correct thrust is used,
 		 * without depending on library calls for safety reasons */
@@ -1329,8 +1340,9 @@ 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 (!(pos_sp_triplet.current.type == SETPOINT_TYPE_TAKEOFF &&
-			launch_detection_state == LAUNCHDETECTION_RES_NONE)) {
+	if (!(_control_mode_current ==  FW_POSCTRL_MODE_AUTO &&
+				pos_sp_triplet.current.type == SETPOINT_TYPE_TAKEOFF &&
+				launch_detection_state == LAUNCHDETECTION_RES_NONE)) {
 		_att_sp.pitch_body = _mTecs.getEnabled() ? _mTecs.getPitchSetpoint() : _tecs.get_pitch_demand();
 	}
 
diff --git a/src/modules/mavlink/mavlink_mission.cpp b/src/modules/mavlink/mavlink_mission.cpp
index a3c127cdc..859d380fe 100644
--- a/src/modules/mavlink/mavlink_mission.cpp
+++ b/src/modules/mavlink/mavlink_mission.cpp
@@ -292,9 +292,6 @@ MavlinkMissionManager::send_mission_item_reached(uint16_t seq)
 void
 MavlinkMissionManager::send(const hrt_abstime now)
 {
-	/* update interval for slow rate limiter */
-	_slow_rate_limiter.set_interval(_interval * 10 / _mavlink->get_rate_mult());
-
 	bool updated = false;
 	orb_check(_mission_result_sub, &updated);
 
@@ -312,6 +309,12 @@ MavlinkMissionManager::send(const hrt_abstime now)
 
 		send_mission_current(_current_seq);
 
+		if (mission_result.item_do_jump_changed) {
+			/* send a mission item again if the remaining DO_JUMPs has changed */
+			send_mission_item(_transfer_partner_sysid, _transfer_partner_compid,
+					  (uint16_t)mission_result.item_changed_index);
+		}
+
 	} else {
 		if (_slow_rate_limiter.check(now)) {
 			send_mission_current(_current_seq);
@@ -811,7 +814,7 @@ MavlinkMissionManager::format_mavlink_mission_item(const struct mission_item_s *
 
 	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;
+		mavlink_mission_item->param2 = mission_item->do_jump_repeat_count - mission_item->do_jump_current_count;
 		break;
 
 	default:
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 4ea5fdfb6..bf65d2805 100644
--- a/src/modules/mc_pos_control/mc_pos_control_main.cpp
+++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp
@@ -653,8 +653,6 @@ MulticopterPositionControl::control_offboard(float dt)
 			/* control position */
 			_pos_sp(0) = _pos_sp_triplet.current.x;
 			_pos_sp(1) = _pos_sp_triplet.current.y;
-			_pos_sp(2) = _pos_sp_triplet.current.z;
-
 		} else if (_control_mode.flag_control_velocity_enabled && _pos_sp_triplet.current.velocity_valid) {
 			/* control velocity */
 			/* reset position setpoint to current position if needed */
@@ -671,7 +669,10 @@ MulticopterPositionControl::control_offboard(float dt)
 			_att_sp.yaw_body = _att_sp.yaw_body + _pos_sp_triplet.current.yawspeed * dt;
 		}
 
-		if (_control_mode.flag_control_altitude_enabled) {
+		if (_control_mode.flag_control_altitude_enabled && _pos_sp_triplet.current.position_valid) {
+			/* Control altitude */
+			_pos_sp(2) = _pos_sp_triplet.current.z;
+		} else if (_control_mode.flag_control_climb_rate_enabled && _pos_sp_triplet.current.velocity_valid) {
 			/* reset alt setpoint to current altitude if needed */
 			reset_alt_sp();
 
diff --git a/src/modules/navigator/datalinkloss.cpp b/src/modules/navigator/datalinkloss.cpp
index e789fd10d..87a6e023a 100644
--- a/src/modules/navigator/datalinkloss.cpp
+++ b/src/modules/navigator/datalinkloss.cpp
@@ -155,7 +155,7 @@ DataLinkLoss::set_dll_item()
 	case DLL_STATE_TERMINATE: {
 		/* Request flight termination from the commander */
 		_navigator->get_mission_result()->flight_termination = true;
-		_navigator->publish_mission_result();
+		_navigator->set_mission_result_updated();
 		reset_mission_item_reached();
 		warnx("not switched to manual: request flight termination");
 		pos_sp_triplet->previous.valid = false;
@@ -188,7 +188,7 @@ DataLinkLoss::advance_dll()
 					_navigator->get_vstatus()->data_link_lost_counter, _param_numberdatalinklosses.get());
 			mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: too many DL losses, fly to airfield home");
 			_navigator->get_mission_result()->stay_in_failsafe = true;
-			_navigator->publish_mission_result();
+			_navigator->set_mission_result_updated();
 			reset_mission_item_reached();
 			_dll_state = DLL_STATE_FLYTOAIRFIELDHOMEWP;
 		} else {
@@ -209,7 +209,7 @@ DataLinkLoss::advance_dll()
 			mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: fly to airfield home");
 		_dll_state = DLL_STATE_FLYTOAIRFIELDHOMEWP;
 		_navigator->get_mission_result()->stay_in_failsafe = true;
-		_navigator->publish_mission_result();
+		_navigator->set_mission_result_updated();
 		reset_mission_item_reached();
 		break;
 	case DLL_STATE_FLYTOAIRFIELDHOMEWP:
@@ -217,7 +217,7 @@ DataLinkLoss::advance_dll()
 		warnx("time is up, state should have been changed manually by now");
 		mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: no manual control, terminating");
 		_navigator->get_mission_result()->stay_in_failsafe = true;
-		_navigator->publish_mission_result();
+		_navigator->set_mission_result_updated();
 		reset_mission_item_reached();
 		break;
 	case DLL_STATE_TERMINATE:
diff --git a/src/modules/navigator/geofence.cpp b/src/modules/navigator/geofence.cpp
index 0f431ded2..4482fb36b 100644
--- a/src/modules/navigator/geofence.cpp
+++ b/src/modules/navigator/geofence.cpp
@@ -279,8 +279,14 @@ Geofence::loadFromFile(const char *filename)
 		while((textStart < sizeof(line)/sizeof(char)) && isspace(line[textStart])) textStart++;
 
 		/* if the line starts with #, skip */
-		if (line[textStart] == commentChar)
+		if (line[textStart] == commentChar) {
 			continue;
+		}
+
+		/* if there is only a linefeed, skip it */
+		if (line[0] == '\n') {
+			continue;
+		}
 
 		if (gotVertical) {
 			/* Parse the line as a geofence point */
@@ -291,8 +297,10 @@ Geofence::loadFromFile(const char *filename)
 				/* Handle degree minute second format */
 				float lat_d, lat_m, lat_s, lon_d, lon_m, lon_s;
 
-				if (sscanf(line, "DMS %f %f %f %f %f %f", &lat_d, &lat_m, &lat_s, &lon_d, &lon_m, &lon_s) != 6)
+				if (sscanf(line, "DMS %f %f %f %f %f %f", &lat_d, &lat_m, &lat_s, &lon_d, &lon_m, &lon_s) != 6) {
+					warnx("Scanf to parse DMS geofence vertex failed.");
 					return ERROR;
+				}
 
 //				warnx("Geofence DMS: %.5f %.5f %.5f ; %.5f %.5f %.5f", (double)lat_d, (double)lat_m, (double)lat_s, (double)lon_d, (double)lon_m, (double)lon_s);
 
@@ -301,9 +309,10 @@ Geofence::loadFromFile(const char *filename)
 
 			} else {
 				/* Handle decimal degree format */
-
-				if (sscanf(line, "%f %f", &(vertex.lat), &(vertex.lon)) != 2)
+				if (sscanf(line, "%f %f", &(vertex.lat), &(vertex.lon)) != 2) {
+					warnx("Scanf to parse geofence vertex failed.");
 					return ERROR;
+				}
 			}
 
 			if (dm_write(DM_KEY_FENCE_POINTS, pointCounter, DM_PERSIST_POWER_ON_RESET, &vertex, sizeof(vertex)) != sizeof(vertex))
diff --git a/src/modules/navigator/gpsfailure.cpp b/src/modules/navigator/gpsfailure.cpp
index cd55f60b0..e370796c0 100644
--- a/src/modules/navigator/gpsfailure.cpp
+++ b/src/modules/navigator/gpsfailure.cpp
@@ -141,7 +141,7 @@ GpsFailure::set_gpsf_item()
 	case GPSF_STATE_TERMINATE: {
 		/* Request flight termination from the commander */
 		_navigator->get_mission_result()->flight_termination = true;
-		_navigator->publish_mission_result();
+		_navigator->set_mission_result_updated();
 		warnx("gps fail: request flight termination");
 	}
 	default:
diff --git a/src/modules/navigator/mission.cpp b/src/modules/navigator/mission.cpp
index 0765e9b7c..9b0a092da 100644
--- a/src/modules/navigator/mission.cpp
+++ b/src/modules/navigator/mission.cpp
@@ -38,6 +38,7 @@
  * @author Julian Oes <julian@oes.ch>
  * @author Thomas Gubler <thomasgubler@gmail.com>
  * @author Anton Babushkin <anton.babushkin@me.com>
+ * @author Ban Siesta <bansiesta@gmail.com>
  */
 
 #include <sys/types.h>
@@ -149,18 +150,12 @@ Mission::on_active()
 
 	/* lets check if we reached the current mission item */
 	if (_mission_type != MISSION_TYPE_NONE && is_mission_item_reached()) {
+		set_mission_item_reached();
 		if (_mission_item.autocontinue) {
 			/* switch to next waypoint if 'autocontinue' flag set */
 			advance_mission();
 			set_mission_items();
 
-		} else {
-			/* else just report that item reached */
-			if (_mission_type == MISSION_TYPE_OFFBOARD) {
-				if (!(_navigator->get_mission_result()->seq_reached == _current_offboard_mission_index && _navigator->get_mission_result()->reached)) {
-					set_mission_item_reached();
-				}
-			}
 		}
 
 	} else if (_mission_type != MISSION_TYPE_NONE &&_param_altmode.get() == MISSION_ALTMODE_FOH) {
@@ -395,7 +390,6 @@ Mission::set_mission_items()
 		/* reuse setpoint for LOITER only if it's not IDLE */
 		_navigator->set_can_loiter_at_sp(pos_sp_triplet->current.type == SETPOINT_TYPE_LOITER);
 
-		reset_mission_item_reached();
 		set_mission_finished();
 
 		_navigator->set_position_setpoint_triplet_updated();
@@ -636,6 +630,8 @@ Mission::read_mission_item(bool onboard, bool is_current, struct mission_item_s
 								     "ERROR DO JUMP waypoint could not be written");
 						return false;
 					}
+					report_do_jump_mission_changed(*mission_index_ptr,
+								       mission_item_tmp.do_jump_repeat_count);
 				}
 				/* set new mission item index and repeat
 				* we don't have to validate here, if it's invalid, we should realize this later .*/
@@ -707,22 +703,31 @@ Mission::save_offboard_mission_state()
 }
 
 void
+Mission::report_do_jump_mission_changed(int index, int do_jumps_remaining)
+{
+	/* inform about the change */
+	_navigator->get_mission_result()->item_do_jump_changed = true;
+	_navigator->get_mission_result()->item_changed_index = index;
+	_navigator->get_mission_result()->item_do_jump_remaining = do_jumps_remaining;
+	_navigator->set_mission_result_updated();
+}
+
+void
 Mission::set_mission_item_reached()
 {
 	_navigator->get_mission_result()->reached = true;
 	_navigator->get_mission_result()->seq_reached = _current_offboard_mission_index;
-	_navigator->publish_mission_result();
+	_navigator->set_mission_result_updated();
 	reset_mission_item_reached();
 }
 
 void
 Mission::set_current_offboard_mission_item()
 {
-	warnx("current offboard mission index: %d", _current_offboard_mission_index);
 	_navigator->get_mission_result()->reached = false;
 	_navigator->get_mission_result()->finished = false;
 	_navigator->get_mission_result()->seq_current = _current_offboard_mission_index;
-	_navigator->publish_mission_result();
+	_navigator->set_mission_result_updated();
 
 	save_offboard_mission_state();
 }
@@ -731,5 +736,5 @@ void
 Mission::set_mission_finished()
 {
 	_navigator->get_mission_result()->finished = true;
-	_navigator->publish_mission_result();
+	_navigator->set_mission_result_updated();
 }
diff --git a/src/modules/navigator/mission.h b/src/modules/navigator/mission.h
index ea7cc0927..a8a644b0f 100644
--- a/src/modules/navigator/mission.h
+++ b/src/modules/navigator/mission.h
@@ -38,6 +38,7 @@
  * @author Julian Oes <julian@oes.ch>
  * @author Thomas Gubler <thomasgubler@gmail.com>
  * @author Anton Babushkin <anton.babushkin@me.com>
+ * @author Ban Siesta <bansiesta@gmail.com>
  */
 
 #ifndef NAVIGATOR_MISSION_H
@@ -131,6 +132,11 @@ private:
 	void save_offboard_mission_state();
 
 	/**
+	 * Inform about a changed mission item after a DO_JUMP
+	 */
+	void report_do_jump_mission_changed(int index, int do_jumps_remaining);
+
+	/**
 	 * Set a mission item as reached
 	 */
 	void set_mission_item_reached();
diff --git a/src/modules/navigator/navigator.h b/src/modules/navigator/navigator.h
index 9cd609955..d9d911d9c 100644
--- a/src/modules/navigator/navigator.h
+++ b/src/modules/navigator/navigator.h
@@ -54,6 +54,7 @@
 #include <uORB/topics/vehicle_gps_position.h>
 #include <uORB/topics/parameter_update.h>
 #include <uORB/topics/mission_result.h>
+#include <uORB/topics/geofence_result.h>
 #include <uORB/topics/vehicle_attitude_setpoint.h>
 
 #include "navigator_mode.h"
@@ -107,9 +108,9 @@ public:
 	void		load_fence_from_file(const char *filename);
 
 	/**
-	 * Publish the mission result so commander and mavlink know what is going on
+	 * Publish the geofence result
 	 */
-	void publish_mission_result();
+	void publish_geofence_result();
 
 	/**
 	 * Publish the attitude sp, only to be used in very special modes when position control is deactivated
@@ -122,6 +123,7 @@ public:
 	 */
 	void		set_can_loiter_at_sp(bool can_loiter) { _can_loiter_at_sp = can_loiter; }
 	void		set_position_setpoint_triplet_updated() { _pos_sp_triplet_updated = true; }
+	void		set_mission_result_updated() { _mission_result_updated = true; }
 
 	/**
 	 * Getters
@@ -134,6 +136,7 @@ public:
 	struct home_position_s*		    get_home_position() { return &_home_pos; }
 	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; }
 	struct vehicle_attitude_setpoint_s* get_att_sp() { return &_att_sp; }
 
 	int		get_onboard_mission_sub() { return _onboard_mission_sub; }
@@ -164,6 +167,7 @@ private:
 
 	orb_advert_t	_pos_sp_triplet_pub;		/**< publish position setpoint triplet */
 	orb_advert_t	_mission_result_pub;
+	orb_advert_t	_geofence_result_pub;
 	orb_advert_t	_att_sp_pub;			/**< publish att sp
 							  used only in very special failsafe modes
 							  when pos control is deactivated */
@@ -179,7 +183,8 @@ private:
 	position_setpoint_triplet_s			_pos_sp_triplet;	/**< triplet of position setpoints */
 
 	mission_result_s				_mission_result;
-	vehicle_attitude_setpoint_s					_att_sp;
+	geofence_result_s				_geofence_result;
+	vehicle_attitude_setpoint_s			_att_sp;
 
 	bool 		_mission_item_valid;		/**< flags if the current mission item is valid */
 
@@ -206,6 +211,7 @@ private:
 	bool		_can_loiter_at_sp;			/**< flags if current position SP can be used to loiter */
 	bool		_pos_sp_triplet_updated;		/**< flags if position SP triplet needs to be published */
 	bool 		_pos_sp_triplet_published_invalid_once;	/**< flags if position SP triplet has been published once to UORB */
+	bool		_mission_result_updated;		/**< flags if mission result has seen an update */
 
 	control::BlockParamFloat _param_loiter_radius;	/**< loiter radius for fixedwing */
 	control::BlockParamFloat _param_acceptance_radius;	/**< acceptance for takeoff */
@@ -271,6 +277,12 @@ private:
 	 */
 	void		publish_position_setpoint_triplet();
 
+
+	/**
+	 * Publish the mission result so commander and mavlink know what is going on
+	 */
+	void		publish_mission_result();
+
 	/* this class has ptr data members, so it should not be copied,
 	 * consequently the copy constructors are private.
 	 */
diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp
index df620e5e7..3f7670ec4 100644
--- a/src/modules/navigator/navigator_main.cpp
+++ b/src/modules/navigator/navigator_main.cpp
@@ -110,6 +110,7 @@ Navigator::Navigator() :
 	_param_update_sub(-1),
 	_pos_sp_triplet_pub(-1),
 	_mission_result_pub(-1),
+	_geofence_result_pub(-1),
 	_att_sp_pub(-1),
 	_vstatus{},
 	_control_mode{},
@@ -138,6 +139,7 @@ Navigator::Navigator() :
 	_can_loiter_at_sp(false),
 	_pos_sp_triplet_updated(false),
 	_pos_sp_triplet_published_invalid_once(false),
+	_mission_result_updated(false),
 	_param_loiter_radius(this, "LOITER_RAD"),
 	_param_acceptance_radius(this, "ACC_RAD"),
 	_param_datalinkloss_obc(this, "DLL_OBC"),
@@ -398,8 +400,8 @@ Navigator::task_main()
 			have_geofence_position_data = false;
 			if (!inside) {
 				/* inform other apps via the mission result */
-				_mission_result.geofence_violated = true;
-				publish_mission_result();
+				_geofence_result.geofence_violated = true;
+				publish_geofence_result();
 
 				/* Issue a warning about the geofence violation once */
 				if (!_geofence_violation_warning_sent) {
@@ -408,8 +410,8 @@ Navigator::task_main()
 				}
 			} else {
 				/* inform other apps via the mission result */
-				_mission_result.geofence_violated = false;
-				publish_mission_result();
+				_geofence_result.geofence_violated = false;
+				publish_geofence_result();
 				/* Reset the _geofence_violation_warning_sent field */
 				_geofence_violation_warning_sent = false;
 			}
@@ -490,6 +492,11 @@ Navigator::task_main()
 			_pos_sp_triplet_updated = false;
 		}
 
+		if (_mission_result_updated) {
+			publish_mission_result();
+			_mission_result_updated = false;
+		}
+
 		perf_end(_loop_perf);
 	}
 	warnx("exiting.");
@@ -639,6 +646,28 @@ Navigator::publish_mission_result()
 		/* advertise and publish */
 		_mission_result_pub = orb_advertise(ORB_ID(mission_result), &_mission_result);
 	}
+
+	/* reset some of the flags */
+	_mission_result.seq_reached = false;
+	_mission_result.seq_current = 0;
+	_mission_result.item_do_jump_changed = false;
+	_mission_result.item_changed_index = 0;
+	_mission_result.item_do_jump_remaining = 0;
+}
+
+void
+Navigator::publish_geofence_result()
+{
+
+	/* lazily publish the geofence result only once available */
+	if (_geofence_result_pub > 0) {
+		/* publish mission result */
+		orb_publish(ORB_ID(geofence_result), _geofence_result_pub, &_geofence_result);
+
+	} else {
+		/* advertise and publish */
+		_geofence_result_pub = orb_advertise(ORB_ID(geofence_result), &_geofence_result);
+	}
 }
 
 void
diff --git a/src/modules/navigator/navigator_mode.cpp b/src/modules/navigator/navigator_mode.cpp
index 3807c5ea8..2f322031c 100644
--- a/src/modules/navigator/navigator_mode.cpp
+++ b/src/modules/navigator/navigator_mode.cpp
@@ -65,7 +65,7 @@ NavigatorMode::run(bool active) {
 			_first_run = false;
 			/* Reset stay in failsafe flag */
 			_navigator->get_mission_result()->stay_in_failsafe = false;
-			_navigator->publish_mission_result();
+			_navigator->set_mission_result_updated();
 			on_activation();
 
 		} else {
diff --git a/src/modules/navigator/rcloss.cpp b/src/modules/navigator/rcloss.cpp
index 42392e739..a7cde6325 100644
--- a/src/modules/navigator/rcloss.cpp
+++ b/src/modules/navigator/rcloss.cpp
@@ -128,7 +128,7 @@ RCLoss::set_rcl_item()
 	case RCL_STATE_TERMINATE: {
 		/* Request flight termination from the commander */
 		_navigator->get_mission_result()->flight_termination = true;
-		_navigator->publish_mission_result();
+		_navigator->set_mission_result_updated();
 		warnx("rc not recovered: request flight termination");
 		pos_sp_triplet->previous.valid = false;
 		pos_sp_triplet->current.valid = false;
@@ -162,7 +162,7 @@ RCLoss::advance_rcl()
 			mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: rc loss, terminating");
 			_rcl_state = RCL_STATE_TERMINATE;
 			_navigator->get_mission_result()->stay_in_failsafe = true;
-			_navigator->publish_mission_result();
+			_navigator->set_mission_result_updated();
 			reset_mission_item_reached();
 		}
 		break;
@@ -171,7 +171,7 @@ RCLoss::advance_rcl()
 		warnx("time is up, no RC regain, terminating");
 		mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: RC not regained, terminating");
 		_navigator->get_mission_result()->stay_in_failsafe = true;
-		_navigator->publish_mission_result();
+		_navigator->set_mission_result_updated();
 		reset_mission_item_reached();
 		break;
 	case RCL_STATE_TERMINATE:
diff --git a/src/modules/position_estimator_inav/position_estimator_inav_main.c b/src/modules/position_estimator_inav/position_estimator_inav_main.c
index de6357d31..4bdb4d539 100644
--- a/src/modules/position_estimator_inav/position_estimator_inav_main.c
+++ b/src/modules/position_estimator_inav/position_estimator_inav_main.c
@@ -1053,10 +1053,6 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
 				landed = false;
 				landed_time = 0;
 			}
-			/* reset xy velocity estimates when landed */
-			x_est[1] = 0.0f;
-			y_est[1] = 0.0f;
-
 		} else {
 			if (alt_disp2 < land_disp2 && thrust < params.land_thr) {
 				if (landed_time == 0) {
diff --git a/src/modules/px4iofirmware/protocol.h b/src/modules/px4iofirmware/protocol.h
index c7e9ae3eb..bd777428f 100644
--- a/src/modules/px4iofirmware/protocol.h
+++ b/src/modules/px4iofirmware/protocol.h
@@ -33,6 +33,8 @@
 
 #pragma once
 
+#include <inttypes.h>
+
 /**
  * @file protocol.h
  *
diff --git a/src/modules/systemlib/perf_counter.c b/src/modules/systemlib/perf_counter.c
index d6d8284d2..f9e90652d 100644
--- a/src/modules/systemlib/perf_counter.c
+++ b/src/modules/systemlib/perf_counter.c
@@ -41,7 +41,7 @@
 #include <stdio.h>
 #include <sys/queue.h>
 #include <drivers/drv_hrt.h>
-
+#include <math.h>
 #include "perf_counter.h"
 
 /**
@@ -84,7 +84,8 @@ struct perf_ctr_interval {
 	uint64_t		time_last;
 	uint64_t		time_least;
 	uint64_t		time_most;
-
+	float			mean;
+	float			M2;
 };
 
 /**
@@ -109,6 +110,7 @@ perf_alloc(enum perf_counter_type type, const char *name)
 
 	case PC_INTERVAL:
 		ctr = (perf_counter_t)calloc(sizeof(struct perf_ctr_interval), 1);
+
 		break;
 
 	default:
@@ -156,15 +158,23 @@ perf_count(perf_counter_t handle)
 		case 1:
 			pci->time_least = now - pci->time_last;
 			pci->time_most = now - pci->time_last;
+			pci->mean = pci->time_least / 1e6f;
+			pci->M2 = 0;
 			break;
 		default: {
-			hrt_abstime interval = now - pci->time_last;
-			if (interval < pci->time_least)
-				pci->time_least = interval;
-			if (interval > pci->time_most)
-				pci->time_most = interval;
-			break;
-		}
+				hrt_abstime interval = now - pci->time_last;
+				if (interval < pci->time_least)
+					pci->time_least = interval;
+				if (interval > pci->time_most)
+					pci->time_most = interval;
+				// maintain mean and variance of interval in seconds
+				// Knuth/Welford recursive mean and variance of update intervals (via Wikipedia)
+				float dt = interval / 1e6f;
+				float delta_intvl = dt - pci->mean;
+				pci->mean += delta_intvl / pci->event_count;
+				pci->M2 += delta_intvl * (dt - pci->mean);
+				break;
+			}
 		}
 		pci->time_last = now;
 		pci->event_count++;
@@ -313,13 +323,16 @@ perf_print_counter_fd(int fd, perf_counter_t handle)
 
 	case PC_INTERVAL: {
 		struct perf_ctr_interval *pci = (struct perf_ctr_interval *)handle;
+		float rms = sqrtf(pci->M2 / (pci->event_count-1));
 
-		dprintf(fd, "%s: %llu events, %lluus avg, min %lluus max %lluus\n",
+		dprintf(fd, "%s: %llu events, %lluus avg, min %lluus max %lluus %5.3f msec mean %5.3f msec rms\n",
 		       handle->name,
 		       pci->event_count,
 		       (pci->time_last - pci->time_first) / pci->event_count,
 		       pci->time_least,
-		       pci->time_most);
+		       pci->time_most,
+			   (double)(1000 * pci->mean),
+			   (double)(1000 * rms));
 		break;
 	}
 
@@ -365,6 +378,21 @@ perf_print_all(int fd)
 	}
 }
 
+extern const uint16_t latency_bucket_count;
+extern uint32_t latency_counters[];
+extern const uint16_t latency_buckets[];
+
+void
+perf_print_latency(int fd)
+{
+	dprintf(fd, "bucket : events\n");
+	for (int i = 0; i < latency_bucket_count; i++) {
+		printf("  %4i : %i\n", latency_buckets[i], latency_counters[i]);
+	}
+	// print the overflow bucket value
+	dprintf(fd, " >%4i : %i\n", latency_buckets[latency_bucket_count-1], latency_counters[latency_bucket_count]);
+}
+
 void
 perf_reset_all(void)
 {
@@ -374,4 +402,7 @@ perf_reset_all(void)
 		perf_reset(handle);
 		handle = (perf_counter_t)sq_next(&handle->link);
 	}
+	for (int i = 0; i <= latency_bucket_count; i++) {
+		latency_counters[i] = 0;
+	}
 }
diff --git a/src/modules/systemlib/perf_counter.h b/src/modules/systemlib/perf_counter.h
index a9dfb13f8..49590c086 100644
--- a/src/modules/systemlib/perf_counter.h
+++ b/src/modules/systemlib/perf_counter.h
@@ -95,7 +95,7 @@ __EXPORT extern void		perf_begin(perf_counter_t handle);
  * End a performance event.
  *
  * This call applies to counters that operate over ranges of time; PC_ELAPSED etc.
- * If a call is made without a corresopnding perf_begin call, or if perf_cancel
+ * If a call is made without a corresponding perf_begin call, or if perf_cancel
  * has been called subsequently, no change is made to the counter.
  *
  * @param handle		The handle returned from perf_alloc.
@@ -144,6 +144,13 @@ __EXPORT extern void		perf_print_counter_fd(int fd, perf_counter_t handle);
 __EXPORT extern void		perf_print_all(int fd);
 
 /**
+ * Print hrt latency counters.
+ *
+ * @param fd			File descriptor to print to - e.g. 0 for stdout
+ */
+__EXPORT extern void		perf_print_latency(int fd);
+
+/**
  * Reset all of the performance counters.
  */
 __EXPORT extern void		perf_reset_all(void);
diff --git a/src/modules/uORB/objects_common.cpp b/src/modules/uORB/objects_common.cpp
index 1141431cc..3d5755a46 100644
--- a/src/modules/uORB/objects_common.cpp
+++ b/src/modules/uORB/objects_common.cpp
@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (C) 2012, 2013 PX4 Development Team. 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
@@ -148,6 +148,9 @@ 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, unsigned);
 
diff --git a/src/modules/uORB/topics/geofence_result.h b/src/modules/uORB/topics/geofence_result.h
new file mode 100644
index 000000000..b07e04499
--- /dev/null
+++ b/src/modules/uORB/topics/geofence_result.h
@@ -0,0 +1,65 @@
+/****************************************************************************
+ *
+ *   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 geofence_result.h
+ * Status of the plance concerning the geofence
+ *
+ * @author Ban Siesta <bansiesta@gmail.com>
+ */
+
+#ifndef TOPIC_GEOFENCE_RESULT_H
+#define TOPIC_GEOFENCE_RESULT_H
+
+#include <stdint.h>
+#include <stdbool.h>
+#include "../uORB.h"
+
+/**
+ * @addtogroup topics
+ * @{
+ */
+
+struct geofence_result_s
+{
+	bool geofence_violated;		/**< true if the geofence is violated */
+};
+
+/**
+ * @}
+ */
+
+/* register this as object request broker structure */
+ORB_DECLARE(geofence_result);
+
+#endif
diff --git a/src/modules/uORB/topics/mission_result.h b/src/modules/uORB/topics/mission_result.h
index c7d25d1f0..2ddc529a3 100644
--- a/src/modules/uORB/topics/mission_result.h
+++ b/src/modules/uORB/topics/mission_result.h
@@ -1,9 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
- *   Author: @author Thomas Gubler <thomasgubler@student.ethz.ch>
- *           @author Julian Oes <joes@student.ethz.ch>
- *           @author Lorenz Meier <lm@inf.ethz.ch>
+ *   Copyright (C) 2012-2014 PX4 Development Team. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -37,6 +34,11 @@
 /**
  * @file mission_result.h
  * Mission results that navigator needs to pass on to commander and mavlink.
+ *
+ * @author Thomas Gubler <thomasgubler@student.ethz.ch>
+ * @author Julian Oes <joes@student.ethz.ch>
+ * @author Lorenz Meier <lm@inf.ethz.ch>
+ * @author Ban Siesta <bansiesta@gmail.com>
  */
 
 #ifndef TOPIC_MISSION_RESULT_H
@@ -58,8 +60,10 @@ struct mission_result_s
 	bool reached;			/**< true if mission has been reached					 */
 	bool finished;			/**< true if mission has been completed					 */
 	bool stay_in_failsafe;		/**< true if the commander should not switch out of the failsafe mode*/
-	bool geofence_violated;		/**< true if the geofence is violated */
 	bool flight_termination;	/**< true if the navigator demands a flight termination from the commander app */
+	bool item_do_jump_changed;	/**< true if the number of do jumps remaining has changed */
+	unsigned item_changed_index;	/**< indicate which item has changed */
+	unsigned item_do_jump_remaining;/**< set to the number of do jumps remaining for that item */
 };
 
 /**
diff --git a/src/modules/uavcan/module.mk b/src/modules/uavcan/module.mk
index f92bc754f..e5d30f6c4 100644
--- a/src/modules/uavcan/module.mk
+++ b/src/modules/uavcan/module.mk
@@ -57,7 +57,7 @@ SRCS += sensors/sensor_bridge.cpp   \
 #
 # libuavcan
 #
-include $(UAVCAN_DIR)/libuavcan/include.mk
+include $(PX4_LIB_DIR)/uavcan/libuavcan/include.mk
 SRCS += $(LIBUAVCAN_SRC)
 INCLUDE_DIRS += $(LIBUAVCAN_INC)
 # Since actual compiler mode is C++11, the library will default to UAVCAN_CPP11, but it will fail to compile
@@ -67,7 +67,7 @@ override EXTRADEFINES := $(EXTRADEFINES) -DUAVCAN_CPP_VERSION=UAVCAN_CPP03 -DUAV
 #
 # libuavcan drivers for STM32
 #
-include $(UAVCAN_DIR)/libuavcan_drivers/stm32/driver/include.mk
+include $(PX4_LIB_DIR)/uavcan/libuavcan_drivers/stm32/driver/include.mk
 SRCS += $(LIBUAVCAN_STM32_SRC)
 INCLUDE_DIRS += $(LIBUAVCAN_STM32_INC)
 override EXTRADEFINES := $(EXTRADEFINES) -DUAVCAN_STM32_NUTTX -DUAVCAN_STM32_NUM_IFACES=2
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 24800cce8..9a80562f3 100644
--- a/src/modules/vtol_att_control/vtol_att_control_main.cpp
+++ b/src/modules/vtol_att_control/vtol_att_control_main.cpp
@@ -74,11 +74,8 @@
 #include <lib/geo/geo.h>
 
 #include "drivers/drv_pwm_output.h"
-#include <nuttx/fs/nxffs.h>
 #include <nuttx/fs/ioctl.h>
 
-#include <nuttx/mtd.h>
-
 #include <fcntl.h>
 
 
@@ -238,11 +235,11 @@ VtolAttitudeControl::VtolAttitudeControl() :
 	_params.idle_pwm_mc = PWM_LOWEST_MIN;
 	_params.vtol_motor_count = 0;
 
-	_params_handles.idle_pwm_mc = param_find("IDLE_PWM_MC");
-	_params_handles.vtol_motor_count = param_find("VTOL_MOT_COUNT");
-	_params_handles.mc_airspeed_min = param_find("VTOL_MC_AIRSPEED_MIN");
-	_params_handles.mc_airspeed_max = param_find("VTOL_MC_AIRSPEED_MAX");
-	_params_handles.mc_airspeed_trim = param_find("VTOL_MC_AIRSPEED_TRIM");
+	_params_handles.idle_pwm_mc = param_find("VT_IDLE_PWM_MC");
+	_params_handles.vtol_motor_count = param_find("VT_MOT_COUNT");
+	_params_handles.mc_airspeed_min = param_find("VT_MC_ARSPD_MIN");
+	_params_handles.mc_airspeed_max = param_find("VT_MC_ARSPD_MAX");
+	_params_handles.mc_airspeed_trim = param_find("VT_MC_ARSPD_TRIM");
 
 	/* fetch initial parameter values */
 	parameters_update();
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 44157acba..e21bccb0c 100644
--- a/src/modules/vtol_att_control/vtol_att_control_params.c
+++ b/src/modules/vtol_att_control/vtol_att_control_params.c
@@ -1,13 +1,88 @@
+/****************************************************************************
+ *
+ *   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 vtol_att_control_params.c
+ * Parameters for vtol attitude controller.
+ *
+ * @author Roman Bapst <bapstr@ethz.ch>
+ */
+
 #include <systemlib/param/param.h>
 
-// number of engines
-PARAM_DEFINE_INT32(VTOL_MOT_COUNT,0);
-// idle pwm in multicopter mode
-PARAM_DEFINE_INT32(IDLE_PWM_MC,900);
-// min airspeed in multicopter mode
-PARAM_DEFINE_FLOAT(VTOL_MC_AIRSPEED_MIN,2);
-// max airspeed in multicopter mode
-PARAM_DEFINE_FLOAT(VTOL_MC_AIRSPEED_MAX,30);
-// trim airspeed in multicopter mode
-PARAM_DEFINE_FLOAT(VTOL_MC_AIRSPEED_TRIM,10);
+/**
+ * VTOL number of engines
+ *
+ * @min 1.0
+ * @group VTOL Attitude Control
+ */
+PARAM_DEFINE_INT32(VT_MOT_COUNT,0);
+
+/**
+ * Idle speed of VTOL when in multicopter mode
+ *
+ * @min 900
+ * @group VTOL Attitude Control
+ */
+PARAM_DEFINE_INT32(VT_IDLE_PWM_MC,900);
+
+/**
+ * Minimum airspeed in multicopter mode
+ *
+ * This is the minimum speed of the air flowing over the control surfaces.
+ *
+ * @min 0.0
+ * @group VTOL Attitude Control
+ */
+PARAM_DEFINE_FLOAT(VT_MC_ARSPD_MIN,2.0f);
+
+/**
+ * Maximum airspeed in multicopter mode
+ *
+ * This is the maximum speed of the air flowing over the control surfaces.
+ *
+ * @min 0.0
+ * @group VTOL Attitude Control
+ */
+PARAM_DEFINE_FLOAT(VT_MC_ARSPD_MAX,30.0f);
+
+/**
+ * Trim airspeed when in multicopter mode
+ *
+ * This is the airflow over the control surfaces for which no airspeed scaling is applied in multicopter mode.
+ *
+ * @min 0.0
+ * @group VTOL Attitude Control
+ */
+PARAM_DEFINE_FLOAT(VT_MC_ARSPD_TRIM,10.0f);
 
diff --git a/src/systemcmds/perf/perf.c b/src/systemcmds/perf/perf.c
index b08a2e3b7..a788dfc11 100644
--- a/src/systemcmds/perf/perf.c
+++ b/src/systemcmds/perf/perf.c
@@ -68,8 +68,12 @@ int perf_main(int argc, char *argv[])
 		if (strcmp(argv[1], "reset") == 0) {
 			perf_reset_all();
 			return 0;
+		} else if (strcmp(argv[1], "latency") == 0) {
+			perf_print_latency(0 /* stdout */);
+			fflush(stdout);
+			return 0;
 		}
-		printf("Usage: perf <reset>\n");
+		printf("Usage: perf [reset | latency]\n");
 		return -1;
 	}