diff --git a/src/MissionManager/MissionCommandTree.cc b/src/MissionManager/MissionCommandTree.cc
index 2c6e4fed84283c50cffa4d7e760bd528d853eb3a..e6828007b3267cc136a5d20df6a32a398f5adc33 100644
--- a/src/MissionManager/MissionCommandTree.cc
+++ b/src/MissionManager/MissionCommandTree.cc
@@ -101,6 +101,11 @@ void MissionCommandTree::_collapseHierarchy(Vehicle*
     _baseVehicleInfo(vehicle, baseFirmwareType, baseVehicleType);
 
     foreach (MAV_CMD command, cmdList->commandIds()) {
+        // Only add supported command to tree (MAV_CMD_NAV_LAST is used for planned home position)
+        if (!qgcApp()->runningUnitTests() && !vehicle->firmwarePlugin()->supportedMissionCommands().contains(command) && command != MAV_CMD_NAV_LAST) {
+            continue;
+        }
+
         MissionCommandUIInfo* uiInfo = cmdList->getUIInfo(command);
         if (uiInfo) {
             if (collapsedTree.contains(command)) {