diff --git a/ROMFS/px4fmu_common/init.d/airframes/4020_holybro_px4vision_v1_5 b/ROMFS/px4fmu_common/init.d/airframes/4020_holybro_px4vision_v1_5
index 0a9289f34d24..ff1e78c566d7 100644
--- a/ROMFS/px4fmu_common/init.d/airframes/4020_holybro_px4vision_v1_5
+++ b/ROMFS/px4fmu_common/init.d/airframes/4020_holybro_px4vision_v1_5
@@ -11,6 +11,10 @@
 
 . ${R}etc/init.d/rc.mc_defaults
 
+# System parameters
+# use FMU motor outputs for less delay in the rate control loop
+param set-default SYS_USE_IO 0
+
 # Commander Parameters
 param set-default COM_OBS_AVOID 1
 param set-default COM_DISARM_LAND 0.5
diff --git a/boards/px4/fmu-v2/init/rc.board_sensors b/boards/px4/fmu-v2/init/rc.board_sensors
index f9a0c7217a71..c0232b3d9e11 100644
--- a/boards/px4/fmu-v2/init/rc.board_sensors
+++ b/boards/px4/fmu-v2/init/rc.board_sensors
@@ -12,7 +12,7 @@ board_adc start
 hmc5883 -T -I -R 4 start
 
 # Internal SPI (auto detect ms5611 or ms5607)
-if ! ms5611 -T 5607 -s -b 1 start
+if ! ms5611 -T 5607 -s -b 1 -q start
 then
 	ms5611 -s -b 1 start
 fi
@@ -24,13 +24,13 @@ if ver hwtypecmp V30
 then
 	# Check for Pixhawk 2.0 cube (isolated IMU on SPI4)
 	#  mpu6000 on v2.0, mpu9250 on v2.1
-	if mpu6000 -s -b 4 -R 10 start
+	if mpu6000 -s -b 4 -R 10 -q start
 	then
 		set BOARD_FMUV3 20
 	else
 		# Check for Pixhawk 2.1 cube
 		# isolated/external mpu9250 (SPI4)
-		if mpu9250 -s -b 4 -R 10 start
+		if mpu9250 -s -b 4 -R 10 -q start
 		then
 			set BOARD_FMUV3 21
 		fi
diff --git a/boards/px4/fmu-v6c/src/board_config.h b/boards/px4/fmu-v6c/src/board_config.h
index dec040aca493..58a40389f40c 100644
--- a/boards/px4/fmu-v6c/src/board_config.h
+++ b/boards/px4/fmu-v6c/src/board_config.h
@@ -145,8 +145,10 @@
 
 #define BOARD_NUM_SPI_CFG_HW_VERSIONS 2 // Rev 0, 10 Sensor sets
 //                 Base/FMUM
-#define V6C00   HW_VER_REV(0x0,0x0) // FMUV6C,                 Rev 0
-#define V6C10   HW_VER_REV(0x1,0x0) // NO PX4IO,               Rev 0
+#define V6C00   HW_VER_REV(0x0,0x0) // FMUV6C,                 Rev 0  I2C4 External but with Internal devices
+#define V6C01   HW_VER_REV(0x0,0x1) // FMUV6C,                 Rev 1  I2C4 Internal I2C2 External
+#define V6C10   HW_VER_REV(0x1,0x0) // NO PX4IO,               Rev 0  I2C4 External but with Internal devices
+#define V6C11   HW_VER_REV(0x1,0x1) // NO PX4IO,               Rev 1  I2C4 Internal I2C2 External
 
 
 /* HEATER
diff --git a/boards/px4/fmu-v6c/src/manifest.c b/boards/px4/fmu-v6c/src/manifest.c
index 04d2386163d0..33b65b4f787a 100644
--- a/boards/px4/fmu-v6c/src/manifest.c
+++ b/boards/px4/fmu-v6c/src/manifest.c
@@ -97,8 +97,10 @@ static const px4_hw_mft_item_t hw_mft_list_v0610[] = {
 
 static px4_hw_mft_list_entry_t mft_lists[] = {
 //  ver_rev
-	{V6C00, hw_mft_list_v0600, arraySize(hw_mft_list_v0600)},
-	{V6C10, hw_mft_list_v0610, arraySize(hw_mft_list_v0610)}, // No PX4IO
+	{V6C00, hw_mft_list_v0600, arraySize(hw_mft_list_v0600)}, // Rev 0
+	{V6C01, hw_mft_list_v0600, arraySize(hw_mft_list_v0600)}, // Rev 1
+	{V6C10, hw_mft_list_v0610, arraySize(hw_mft_list_v0610)}, // Rev 0 No PX4IO
+	{V6C11, hw_mft_list_v0610, arraySize(hw_mft_list_v0610)}, // Rev 1 No PX4IO
 };
 
 /************************************************************************************
diff --git a/boards/px4/fmu-v6x/default.px4board b/boards/px4/fmu-v6x/default.px4board
index 3a61af2e07fb..eaadb8898ee9 100644
--- a/boards/px4/fmu-v6x/default.px4board
+++ b/boards/px4/fmu-v6x/default.px4board
@@ -75,6 +75,7 @@ CONFIG_MODULES_VTOL_ATT_CONTROL=y
 CONFIG_SYSTEMCMDS_ACTUATOR_TEST=y
 CONFIG_SYSTEMCMDS_BL_UPDATE=y
 CONFIG_SYSTEMCMDS_DMESG=y
+CONFIG_SYSTEMCMDS_GPIO=y
 CONFIG_SYSTEMCMDS_HARDFAULT_LOG=y
 CONFIG_SYSTEMCMDS_I2CDETECT=y
 CONFIG_SYSTEMCMDS_LED_CONTROL=y
diff --git a/src/modules/ekf2/EKF/estimator_interface.cpp b/src/modules/ekf2/EKF/estimator_interface.cpp
index fb6ccb71db51..d126c09a5226 100644
--- a/src/modules/ekf2/EKF/estimator_interface.cpp
+++ b/src/modules/ekf2/EKF/estimator_interface.cpp
@@ -528,6 +528,17 @@ bool EstimatorInterface::isHorizontalAidingActive() const
 	return getNumberOfActiveHorizontalAidingSources() > 0;
 }
 
+bool EstimatorInterface::isVerticalVelocityAidingActive() const
+{
+	return getNumberOfActiveVerticalVelocityAidingSources() > 0;
+}
+
+int EstimatorInterface::getNumberOfActiveVerticalVelocityAidingSources() const
+{
+	return int(_control_status.flags.gps)
+	       + int(_control_status.flags.ev_vel);
+}
+
 void EstimatorInterface::printBufferAllocationFailed(const char *buffer_name)
 {
 	if (buffer_name) {
diff --git a/src/modules/ekf2/EKF/estimator_interface.h b/src/modules/ekf2/EKF/estimator_interface.h
index 328c5fd537d3..f98d3394d90e 100644
--- a/src/modules/ekf2/EKF/estimator_interface.h
+++ b/src/modules/ekf2/EKF/estimator_interface.h
@@ -181,6 +181,9 @@ class EstimatorInterface
 
 	int getNumberOfActiveHorizontalAidingSources() const;
 
+	bool isVerticalVelocityAidingActive() const;
+	int getNumberOfActiveVerticalVelocityAidingSources() const;
+
 	const matrix::Quatf &getQuaternion() const { return _output_new.quat_nominal; }
 
 	// get the velocity of the body frame origin in local NED earth frame
diff --git a/src/modules/ekf2/EKF/zero_velocity_update.cpp b/src/modules/ekf2/EKF/zero_velocity_update.cpp
index 3a750292e80c..fc5dc145d4e3 100644
--- a/src/modules/ekf2/EKF/zero_velocity_update.cpp
+++ b/src/modules/ekf2/EKF/zero_velocity_update.cpp
@@ -45,13 +45,16 @@ void Ekf::controlZeroVelocityUpdate()
 
 	if (zero_velocity_update_data_ready) {
 		const bool continuing_conditions_passing = _control_status.flags.vehicle_at_rest
-				&& _control_status_prev.flags.vehicle_at_rest;
+				&& _control_status_prev.flags.vehicle_at_rest
+				&& !isVerticalVelocityAidingActive(); // otherwise the filter is "too rigid" to follow a position drift
 
 		if (continuing_conditions_passing) {
 			Vector3f vel_obs{0, 0, 0};
 			Vector3f innovation = _state.vel - vel_obs;
 
-			const float obs_var = sq(0.001f);
+			// Set a low variance initially for faster leveling and higher
+			// later to let the states follow the measurements
+			const float obs_var = _control_status.flags.tilt_align ? sq(0.2f) : sq(0.001f);
 			Vector3f innov_var{
 				P(4, 4) + obs_var,
 				P(5, 5) + obs_var,
diff --git a/src/modules/navigator/mission_block.cpp b/src/modules/navigator/mission_block.cpp
index 03be49bcc562..6c9f046a999d 100644
--- a/src/modules/navigator/mission_block.cpp
+++ b/src/modules/navigator/mission_block.cpp
@@ -59,14 +59,7 @@ using matrix::wrap_pi;
 MissionBlock::MissionBlock(Navigator *navigator) :
 	NavigatorMode(navigator)
 {
-	_mission_item.lat = (double)NAN;
-	_mission_item.lon = (double)NAN;
-	_mission_item.yaw = NAN;
-	_mission_item.loiter_radius = _navigator->get_loiter_radius();
-	_mission_item.acceptance_radius = _navigator->get_acceptance_radius();
-	_mission_item.time_inside = 0.0f;
-	_mission_item.autocontinue = true;
-	_mission_item.origin = ORIGIN_ONBOARD;
+
 }
 
 bool
@@ -607,7 +600,7 @@ MissionBlock::mission_item_to_position_setpoint(const mission_item_s &item, posi
 			    _navigator->get_loiter_radius();
 	sp->loiter_direction = (item.loiter_radius > 0) ? 1 : -1;
 
-	if (item.acceptance_radius > 0.0f && PX4_ISFINITE(item.acceptance_radius)) {
+	if (item.acceptance_radius > 0.001f && PX4_ISFINITE(item.acceptance_radius)) {
 		// if the mission item has a specified acceptance radius, overwrite the default one from parameters
 		sp->acceptance_radius = item.acceptance_radius;
 
@@ -736,6 +729,7 @@ MissionBlock::set_takeoff_item(struct mission_item_s *item, float abs_altitude)
 	item->altitude = abs_altitude;
 	item->altitude_is_relative = false;
 
+	item->acceptance_radius = _navigator->get_acceptance_radius();
 	item->loiter_radius = _navigator->get_loiter_radius();
 	item->autocontinue = false;
 	item->origin = ORIGIN_ONBOARD;
@@ -838,3 +832,16 @@ MissionBlock::get_absolute_altitude_for_item(const mission_item_s &mission_item)
 		return mission_item.altitude;
 	}
 }
+
+void
+MissionBlock::initialize()
+{
+	_mission_item.lat = (double)NAN;
+	_mission_item.lon = (double)NAN;
+	_mission_item.yaw = NAN;
+	_mission_item.loiter_radius = _navigator->get_loiter_radius();
+	_mission_item.acceptance_radius = _navigator->get_acceptance_radius();
+	_mission_item.time_inside = 0.0f;
+	_mission_item.autocontinue = true;
+	_mission_item.origin = ORIGIN_ONBOARD;
+}
diff --git a/src/modules/navigator/mission_block.h b/src/modules/navigator/mission_block.h
index a6e0045a7b74..2a4ddd14875f 100644
--- a/src/modules/navigator/mission_block.h
+++ b/src/modules/navigator/mission_block.h
@@ -67,6 +67,8 @@ class MissionBlock : public NavigatorMode
 	MissionBlock(const MissionBlock &) = delete;
 	MissionBlock &operator=(const MissionBlock &) = delete;
 
+	void initialize() override;
+
 	/**
 	 * Check if the mission item contains a navigation position
 	 *
diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp
index 1be889b9d8ec..c17d38e24b99 100644
--- a/src/modules/navigator/navigator_main.cpp
+++ b/src/modules/navigator/navigator_main.cpp
@@ -92,6 +92,13 @@ Navigator::Navigator() :
 	_navigation_mode_array[7] = &_vtol_takeoff;
 	_navigation_mode_array[8] = &_follow_target;
 
+	/* iterate through navigation modes and initialize _mission_item for each */
+	for (unsigned int i = 0; i < NAVIGATOR_MODE_ARRAY_SIZE; i++) {
+		if (_navigation_mode_array[i]) {
+			_navigation_mode_array[i]->initialize();
+		}
+	}
+
 	_handle_back_trans_dec_mss = param_find("VT_B_DEC_MSS");
 	_handle_reverse_delay = param_find("VT_B_REV_DEL");
 
diff --git a/src/modules/navigator/navigator_mode.h b/src/modules/navigator/navigator_mode.h
index a3a48a920a6e..3aa6f255f684 100644
--- a/src/modules/navigator/navigator_mode.h
+++ b/src/modules/navigator/navigator_mode.h
@@ -49,7 +49,8 @@ class NavigatorMode
 	NavigatorMode(Navigator *navigator);
 	virtual ~NavigatorMode() = default;
 	NavigatorMode(const NavigatorMode &) = delete;
-	NavigatorMode operator=(const NavigatorMode &) = delete;
+	NavigatorMode &operator=(const NavigatorMode &) = delete;
+	virtual void initialize() = 0;
 
 	void run(bool active);
 
diff --git a/src/modules/sensors/vehicle_magnetometer/VehicleMagnetometer.cpp b/src/modules/sensors/vehicle_magnetometer/VehicleMagnetometer.cpp
index 37db3b7a215b..c67ead82f8be 100644
--- a/src/modules/sensors/vehicle_magnetometer/VehicleMagnetometer.cpp
+++ b/src/modules/sensors/vehicle_magnetometer/VehicleMagnetometer.cpp
@@ -261,9 +261,8 @@ void VehicleMagnetometer::UpdateMagCalibration()
 
 							_mag_cal[i].device_id = estimator_sensor_bias.mag_device_id;
 
-							// readd estimated bias that was removed before publishing vehicle_magnetometer
-							_mag_cal[i].offset = _calibration[mag_index].BiasCorrectedSensorOffset(bias) +
-									     _calibration_estimator_bias[mag_index];
+							// readd estimated bias that was removed before publishing vehicle_magnetometer (_calibration_estimator_bias)
+							_mag_cal[i].offset = _calibration[mag_index].BiasCorrectedSensorOffset(bias + _calibration_estimator_bias[mag_index]);
 
 							_mag_cal[i].variance = bias_variance;