diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp
index ca1b2fcd59..b6844a002d 100644
--- a/src/modules/commander/commander.cpp
+++ b/src/modules/commander/commander.cpp
@@ -675,14 +675,20 @@ bool handle_command(struct vehicle_status_s *status_local, const struct safety_s
 		// doing something sensible with the coordinates. Its designed
 		// to not require navigator and command to receive / process
 		// the data at the exact same time.
-		transition_result_t main_ret = main_state_transition(status_local, commander_state_s::MAIN_STATE_AUTO_LOITER, main_state_prev, &status_flags, &internal_state);
 
-		if ((main_ret != TRANSITION_DENIED)) {
-			cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED;
+		// Check if a mode switch had been requested
+		if ((((uint8_t)cmd->param1) & 1) > 0) {
+			transition_result_t main_ret = main_state_transition(status_local, commander_state_s::MAIN_STATE_AUTO_LOITER, main_state_prev, &status_flags, &internal_state);
 
+			if ((main_ret != TRANSITION_DENIED)) {
+				cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED;
+
+			} else {
+				cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED;
+				mavlink_log_critical(&mavlink_log_pub, "Rejecting reposition command");
+			}
 		} else {
-			cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED;
-			mavlink_log_critical(&mavlink_log_pub, "Rejecting reposition command");
+			cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED;
 		}
 	}
 	break;
diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp
index b3d04cd69c..fec9634893 100644
--- a/src/modules/navigator/navigator_main.cpp
+++ b/src/modules/navigator/navigator_main.cpp
@@ -419,25 +419,25 @@ Navigator::task_main()
 				rep->previous.alt = get_global_position()->alt;
 
 				// Go on and check which changes had been requested
-				if (isfinite(cmd.param4)) {
+				if (PX4_ISFINITE(cmd.param4)) {
 					rep->current.yaw = cmd.param4;
 				} else {
 					rep->current.yaw = NAN;
 				}
 
-				if (isfinite(cmd.param5)) {
+				if (PX4_ISFINITE(cmd.param5)) {
 					rep->current.lat = cmd.param5 / (double)1e7;
 				} else {
 					rep->current.lat = get_global_position()->lat;
 				}
 
-				if (isfinite(cmd.param6)) {
+				if (PX4_ISFINITE(cmd.param6)) {
 					rep->current.lon = cmd.param6 / (double)1e7;
 				} else {
 					rep->current.lon = get_global_position()->lon;
 				}
 
-				if (isfinite(cmd.param7)) {
+				if (PX4_ISFINITE(cmd.param7)) {
 					rep->current.alt = cmd.param7;
 				} else {
 					rep->current.alt = get_global_position()->alt;