diff --git a/src/FirmwarePlugin/PX4/PX4FirmwarePlugin.cc b/src/FirmwarePlugin/PX4/PX4FirmwarePlugin.cc
index 6ade5c3449091ca44980cefab6448c8e6c30f4c6..731f7505675865b6dbf8b50a47d2e066630189d6 100644
--- a/src/FirmwarePlugin/PX4/PX4FirmwarePlugin.cc
+++ b/src/FirmwarePlugin/PX4/PX4FirmwarePlugin.cc
@@ -191,5 +191,5 @@ void PX4FirmwarePlugin::adjustMavlinkMessage(mavlink_message_t* message)
 
 bool PX4FirmwarePlugin::isCapable(FirmwareCapabilities capabilities)
 {
-    return capabilities && (MavCmdPreflightStorageCapability | SetFlightModeCapability);
+    return (capabilities & (MavCmdPreflightStorageCapability | SetFlightModeCapability)) == capabilities;
 }
diff --git a/src/ui/toolbar/MainToolBar.qml b/src/ui/toolbar/MainToolBar.qml
index ef1b6da797b8e2dd3e0d0a746dc1022123b4a450..3699dbbaebd712fe62496e6d2efe87175c1f0855 100644
--- a/src/ui/toolbar/MainToolBar.qml
+++ b/src/ui/toolbar/MainToolBar.qml
@@ -542,7 +542,7 @@ Rectangle {
                 anchors.verticalCenter: parent.verticalCenter
                 text:                   activeVehicle.flightMode
 
-                menu: flightModesMenu
+                menu: activeVehicle.flightModeSetAvailable ? flightModesMenu : null
 
                 Menu {
                     id: flightModesMenu
@@ -561,17 +561,19 @@ Rectangle {
                 property var flightModesMenuItems: []
 
                 function updateFlightModesMenu() {
-                    // Remove old menu items
-                    for (var i=0; i<flightModesMenuItems.length; i++) {
-                        flightModesMenu.removeItem(flightModesMenuItems[i])
-                    }
-                    flightModesMenuItems.length = 0
-
-                    // Add new items
-                    for (var i=0; i<activeVehicle.flightModes.length; i++) {
-                        var menuItem = flightModeMenuItemComponent.createObject(null, { "text": activeVehicle.flightModes[i] })
-                        flightModesMenuItems.push(menuItem)
-                        flightModesMenu.insertItem(i, menuItem)
+                    if (activeVehicle.flightModeSetAvailable) {
+                        // Remove old menu items
+                        for (var i=0; i<flightModesMenuItems.length; i++) {
+                            flightModesMenu.removeItem(flightModesMenuItems[i])
+                        }
+                        flightModesMenuItems.length = 0
+
+                            // Add new items
+                            for (var i=0; i<activeVehicle.flightModes.length; i++) {
+                                var menuItem = flightModeMenuItemComponent.createObject(null, { "text": activeVehicle.flightModes[i] })
+                                flightModesMenuItems.push(menuItem)
+                                flightModesMenu.insertItem(i, menuItem)
+                            }
                     }
                 }