diff --git a/src/lib/modes/standard_modes.hpp b/src/lib/modes/standard_modes.hpp
index df27f92735..68d849f09a 100644
--- a/src/lib/modes/standard_modes.hpp
+++ b/src/lib/modes/standard_modes.hpp
@@ -57,7 +57,7 @@ enum class StandardMode : uint8_t {
 /**
  * @return Get MAVLink standard mode from nav_state
  */
-static inline StandardMode getStandardModeFromNavState(uint8_t nav_state)
+static inline StandardMode getStandardModeFromNavState(uint8_t nav_state, uint8_t vehicle_type, bool is_vtol)
 {
 	switch (nav_state) {
 	case vehicle_status_s::NAVIGATION_STATE_AUTO_RTL: return StandardMode::RETURN_HOME;
@@ -67,7 +67,33 @@ static inline StandardMode getStandardModeFromNavState(uint8_t nav_state)
 	case vehicle_status_s::NAVIGATION_STATE_AUTO_LAND: return StandardMode::LAND;
 
 	case vehicle_status_s::NAVIGATION_STATE_AUTO_TAKEOFF: return StandardMode::TAKEOFF;
-		// Note: all other standard modes do not directly map, or are vehicle-type specific
+
+	case vehicle_status_s::NAVIGATION_STATE_ALTCTL:
+		if (is_vtol || vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING
+		    || vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING) {
+			return StandardMode::ALTITUDE_HOLD;
+		}
+
+		break;
+
+	case vehicle_status_s::NAVIGATION_STATE_POSCTL:
+		if (!is_vtol && vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) {
+			return StandardMode::POSITION_HOLD;
+		}
+
+		if (!is_vtol && vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING) {
+			return StandardMode::CRUISE;
+		}
+
+		break;
+
+	case vehicle_status_s::NAVIGATION_STATE_ORBIT:
+		if (is_vtol || vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING
+		    || vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING) {
+			return StandardMode::ORBIT;
+		}
+
+		break;
 	}
 
 	return StandardMode::NON_STANDARD;
@@ -76,7 +102,7 @@ static inline StandardMode getStandardModeFromNavState(uint8_t nav_state)
 /**
  * @return Get nav_state from a standard mode, or vehicle_status_s::NAVIGATION_STATE_MAX if not supported
  */
-static inline uint8_t getNavStateFromStandardMode(StandardMode mode)
+static inline uint8_t getNavStateFromStandardMode(StandardMode mode, uint8_t vehicle_type, bool is_vtol)
 {
 	switch (mode) {
 	case StandardMode::RETURN_HOME: return vehicle_status_s::NAVIGATION_STATE_AUTO_RTL;
@@ -87,6 +113,39 @@ static inline uint8_t getNavStateFromStandardMode(StandardMode mode)
 
 	case StandardMode::TAKEOFF: return vehicle_status_s::NAVIGATION_STATE_AUTO_TAKEOFF;
 
+	case StandardMode::ALTITUDE_HOLD:
+		if (is_vtol || vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING
+		    || vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING) {
+			return vehicle_status_s::NAVIGATION_STATE_ALTCTL;
+		}
+
+		break;
+
+	case StandardMode::POSITION_HOLD:
+		if (!is_vtol && vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) {
+			return vehicle_status_s::NAVIGATION_STATE_POSCTL;
+		}
+
+		break;
+
+	case StandardMode::CRUISE:
+		if (!is_vtol && vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING) {
+			return vehicle_status_s::NAVIGATION_STATE_POSCTL;
+		}
+
+		break;
+
+	case StandardMode::ORBIT:
+		if (vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) {
+			return vehicle_status_s::NAVIGATION_STATE_ORBIT;
+		}
+
+		if (vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING) {
+			return vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER;
+		}
+
+		break;
+
 	default: break;
 	}
 
diff --git a/src/modules/commander/Commander.cpp b/src/modules/commander/Commander.cpp
index 8e41b2ac56..5fc8ee4d8f 100644
--- a/src/modules/commander/Commander.cpp
+++ b/src/modules/commander/Commander.cpp
@@ -1450,7 +1450,8 @@ Commander::handle_command(const vehicle_command_s &cmd)
 
 	case vehicle_command_s::VEHICLE_CMD_DO_SET_STANDARD_MODE: {
 			mode_util::StandardMode standard_mode = (mode_util::StandardMode) roundf(cmd.param1);
-			uint8_t nav_state = mode_util::getNavStateFromStandardMode(standard_mode);
+			uint8_t nav_state = mode_util::getNavStateFromStandardMode(standard_mode, _vehicle_status.vehicle_type,
+					    _vehicle_status.is_vtol);
 
 			if (nav_state == vehicle_status_s::NAVIGATION_STATE_MAX) {
 				answer_command(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_FAILED);
diff --git a/src/modules/mavlink/streams/AVAILABLE_MODES.hpp b/src/modules/mavlink/streams/AVAILABLE_MODES.hpp
index d19aeb4613..ae077c8239 100644
--- a/src/modules/mavlink/streams/AVAILABLE_MODES.hpp
+++ b/src/modules/mavlink/streams/AVAILABLE_MODES.hpp
@@ -86,7 +86,8 @@ private:
 		const bool cannot_be_selected = (vehicle_status.can_set_nav_states_mask & (1u << nav_state)) == 0;
 
 		// Set the mode name if not a standard mode
-		available_modes.standard_mode = (uint8_t)mode_util::getStandardModeFromNavState(nav_state);
+		available_modes.standard_mode = (uint8_t)mode_util::getStandardModeFromNavState(nav_state, vehicle_status.vehicle_type,
+						vehicle_status.is_vtol);
 
 		if (mode_util::isAdvanced(nav_state)) {
 			available_modes.properties |= MAV_MODE_PROPERTY_ADVANCED;
diff --git a/src/modules/mavlink/streams/CURRENT_MODE.hpp b/src/modules/mavlink/streams/CURRENT_MODE.hpp
index 5527801670..2be1c47c4b 100644
--- a/src/modules/mavlink/streams/CURRENT_MODE.hpp
+++ b/src/modules/mavlink/streams/CURRENT_MODE.hpp
@@ -66,7 +66,8 @@ private:
 			mavlink_current_mode_t current_mode{};
 			current_mode.custom_mode = get_px4_custom_mode(vehicle_status.nav_state).data;
 			current_mode.intended_custom_mode = get_px4_custom_mode(vehicle_status.nav_state_user_intention).data;
-			current_mode.standard_mode = (uint8_t) mode_util::getStandardModeFromNavState(vehicle_status.nav_state);
+			current_mode.standard_mode = (uint8_t) mode_util::getStandardModeFromNavState(vehicle_status.nav_state,
+						     vehicle_status.vehicle_type, vehicle_status.is_vtol);
 			mavlink_msg_current_mode_send_struct(_mavlink->get_channel(), &current_mode);
 			return true;
 		}