diff --git a/src/main/fc/fc_msp.c b/src/main/fc/fc_msp.c
index 53b59f5d490..cf8038252ec 100644
--- a/src/main/fc/fc_msp.c
+++ b/src/main/fc/fc_msp.c
@@ -4218,14 +4218,15 @@ bool mspFCProcessInOutCommand(uint16_t cmdMSP, sbuf_t *dst, sbuf_t *src, mspResu
         break;
 #endif
 
-#ifdef USE_BARO
+#ifdef USE_GPS
     case MSP2_INAV_SET_ALT_TARGET:
         if (dataSize != (sizeof(int32_t) + sizeof(uint8_t))) {
             *ret = MSP_RESULT_ERROR;
             break;
         }
-
-        if (navigationSetAltitudeTargetWithDatum((geoAltitudeDatumFlag_e)sbufReadU8(src), (int32_t)sbufReadU32(src))) {
+        uint8_t setAltDatum = (geoAltitudeDatumFlag_e)sbufReadU8(src);
+        int32_t setNewAlt = sbufReadU32(src);
+        if (navigationSetAltitudeTargetWithDatum(setAltDatum, setNewAlt)) {
             *ret = MSP_RESULT_ACK;
             break;
         }
diff --git a/src/main/navigation/navigation.c b/src/main/navigation/navigation.c
index a3752233149..89bf5421ab0 100644
--- a/src/main/navigation/navigation.c
+++ b/src/main/navigation/navigation.c
@@ -5204,7 +5204,14 @@ bool navigationIsControllingAltitude(void) {
 bool navigationSetAltitudeTargetWithDatum(geoAltitudeDatumFlag_e datumFlag, int32_t targetAltitudeCm)
 {
     const navigationFSMStateFlags_t stateFlags = navGetCurrentStateFlags();
-    if (!(stateFlags & NAV_CTL_ALT) || (stateFlags & NAV_CTL_LAND) || navigationIsExecutingAnEmergencyLanding() || posControl.flags.estAltStatus == EST_NONE) {
+    if (!(stateFlags & NAV_CTL_ALT) ||
+        (stateFlags & NAV_CTL_LAND) ||
+        navigationIsExecutingAnEmergencyLanding() ||
+        posControl.flags.estAltStatus == EST_NONE ||
+        (stateFlags & NAV_MIXERAT) ||
+        FLIGHT_MODE(NAV_FW_AUTOLAND) ||
+        FLIGHT_MODE(NAV_SEND_TO) ||
+        ((stateFlags & NAV_AUTO_RTH) && posControl.navState != NAV_STATE_RTH_HEAD_HOME)) {
         return false;
     }
 
diff --git a/src/main/telemetry/mavlink.c b/src/main/telemetry/mavlink.c
index 60f6aed67c3..0e6abe146b8 100644
--- a/src/main/telemetry/mavlink.c
+++ b/src/main/telemetry/mavlink.c
@@ -1215,7 +1215,7 @@ static bool handleIncoming_COMMAND_INT(void)
                 mavlinkSendMessage();
             }
         } else {
-#ifdef USE_BARO
+#ifdef USE_GPS
             if (msg.command == MAV_CMD_DO_CHANGE_ALTITUDE) {
                 const float altitudeMeters = msg.param1;
                 const uint8_t frame = (uint8_t)msg.frame;