diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp
index bc4f2feec7..fd1826a0c0 100644
--- a/src/modules/commander/commander.cpp
+++ b/src/modules/commander/commander.cpp
@@ -559,6 +559,7 @@ bool handle_command(struct vehicle_status_s *status_local, const struct safety_s
 	case vehicle_command_s::VEHICLE_CMD_DO_SET_MODE: {
 			uint8_t base_mode = (uint8_t)cmd->param1;
 			uint8_t custom_main_mode = (uint8_t)cmd->param2;
+			uint8_t custom_sub_mode = (uint8_t)cmd->param3;
 
 			transition_result_t arming_ret = TRANSITION_NOT_CHANGED;
 
@@ -596,7 +597,22 @@ bool handle_command(struct vehicle_status_s *status_local, const struct safety_s
 
 				} else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_AUTO) {
 					/* AUTO */
-					main_ret = main_state_transition(status_local, vehicle_status_s::MAIN_STATE_AUTO_MISSION);
+					switch(custom_sub_mode) {
+					case PX4_CUSTOM_SUB_MODE_AUTO_LOITER:
+						main_ret = main_state_transition(status_local, vehicle_status_s::MAIN_STATE_AUTO_LOITER);
+						break;
+					case PX4_CUSTOM_SUB_MODE_AUTO_MISSION:
+						main_ret = main_state_transition(status_local, vehicle_status_s::MAIN_STATE_AUTO_MISSION);
+						break;
+					case PX4_CUSTOM_SUB_MODE_AUTO_RTL:
+						main_ret = main_state_transition(status_local, vehicle_status_s::MAIN_STATE_AUTO_RTL);
+						break;
+
+					default:
+						main_ret = TRANSITION_DENIED;
+						mavlink_log_critical(mavlink_fd, "Unsupported auto command");
+						break;
+					}
 
 				} else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_ACRO) {
 					/* ACRO */
diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp
index 3ac659608a..508a1d8c95 100644
--- a/src/modules/mavlink/mavlink_receiver.cpp
+++ b/src/modules/mavlink/mavlink_receiver.cpp
@@ -539,7 +539,7 @@ MavlinkReceiver::handle_message_set_mode(mavlink_message_t *msg)
 	/* copy the content of mavlink_command_long_t cmd_mavlink into command_t cmd */
 	vcmd.param1 = new_mode.base_mode;
 	vcmd.param2 = custom_mode.main_mode;
-	vcmd.param3 = 0;
+	vcmd.param3 = custom_mode.sub_mode;
 	vcmd.param4 = 0;
 	vcmd.param5 = 0;
 	vcmd.param6 = 0;