diff --git a/posix-configs/SITL/init/rcS_gazebo_standard_vtol b/posix-configs/SITL/init/rcS_gazebo_standard_vtol
index 14dcdfc207..c5282f6395 100644
--- a/posix-configs/SITL/init/rcS_gazebo_standard_vtol
+++ b/posix-configs/SITL/init/rcS_gazebo_standard_vtol
@@ -30,6 +30,7 @@ param set MPC_XY_FF 0.1
 param set MPC_Z_VEL_MAX 1.0
 param set SENS_BOARD_ROT 8
 param set COM_RC_IN_MODE 1
+param set NAV_ACC_RAD 3.0
 rgbledsim start
 tone_alarm start
 gyrosim start
diff --git a/src/modules/navigator/mission.cpp b/src/modules/navigator/mission.cpp
index 627db6aa09..fb6a300f94 100644
--- a/src/modules/navigator/mission.cpp
+++ b/src/modules/navigator/mission.cpp
@@ -432,7 +432,7 @@ Mission::set_mission_items()
 
 			float takeoff_alt = calculate_takeoff_altitude(&_mission_item);
 
-			mavlink_log_info(_navigator->get_mavlink_fd(), "takeoff to %.1f meters above home", (double)(takeoff_alt - _navigator->get_home_position()->alt));
+			mavlink_log_critical(_navigator->get_mavlink_fd(), "takeoff to %.1f meters above home", (double)(takeoff_alt - _navigator->get_home_position()->alt));
 
 			_mission_item.nav_cmd = NAV_CMD_TAKEOFF;
 			_mission_item.lat = _navigator->get_global_position()->lat;
diff --git a/src/modules/navigator/mission_block.cpp b/src/modules/navigator/mission_block.cpp
index 6e32af0fd4..4d4effd6cc 100644
--- a/src/modules/navigator/mission_block.cpp
+++ b/src/modules/navigator/mission_block.cpp
@@ -105,11 +105,18 @@ MissionBlock::is_mission_item_reached()
 		case NAV_CMD_LOITER_UNLIMITED:
 			return false;
 
-		case NAV_CMD_DO_DIGICAM_CONTROL: /* fallthrough */
+		case NAV_CMD_DO_DIGICAM_CONTROL:
+			return true;
+
 		case NAV_CMD_DO_VTOL_TRANSITION:
-			// XXX: we should wait on command ACK or status change instead
-			// currently we just wait so the command can be processed
-			if (hrt_absolute_time() - _action_start > 1000) {
+			/*
+			 * We wait half a second to give the transition command time to propagate.
+			 * As soon as the timeout is over or when we're in transition mode let the mission continue.
+			 */
+			if (hrt_absolute_time() - _action_start > 500000 ||
+					_navigator->get_vstatus()->in_transition_mode) {
+				_action_start = 0;
+
 				return true;
 
 			} else {