diff --git a/src/FirmwarePlugin/APM/ArduSubFirmwarePlugin.cc b/src/FirmwarePlugin/APM/ArduSubFirmwarePlugin.cc
index b06b53990a2157a8d29fee02c76589ddf0be3758..6fd925901f36c444882d75b6158c59412eaf6ee6 100644
--- a/src/FirmwarePlugin/APM/ArduSubFirmwarePlugin.cc
+++ b/src/FirmwarePlugin/APM/ArduSubFirmwarePlugin.cc
@@ -116,6 +116,38 @@ ArduSubFirmwarePlugin::ArduSubFirmwarePlugin(void):
     _nameToFactGroupMap.insert("APMSubInfo", &_infoFactGroup);
 }
 
+QList<MAV_CMD> ArduSubFirmwarePlugin::supportedMissionCommands(void)
+{
+    QList<MAV_CMD> list;
+
+    list << MAV_CMD_NAV_WAYPOINT
+         << MAV_CMD_NAV_RETURN_TO_LAUNCH
+         << MAV_CMD_NAV_LAND
+         << MAV_CMD_NAV_CONTINUE_AND_CHANGE_ALT
+         << MAV_CMD_NAV_SPLINE_WAYPOINT
+         << MAV_CMD_NAV_GUIDED_ENABLE
+         << MAV_CMD_NAV_DELAY
+         << MAV_CMD_CONDITION_DELAY << MAV_CMD_CONDITION_DISTANCE << MAV_CMD_CONDITION_YAW
+         << MAV_CMD_DO_SET_MODE
+         << MAV_CMD_DO_JUMP
+         << MAV_CMD_DO_CHANGE_SPEED
+         << MAV_CMD_DO_SET_HOME
+         << MAV_CMD_DO_SET_RELAY << MAV_CMD_DO_REPEAT_RELAY
+         << MAV_CMD_DO_SET_SERVO << MAV_CMD_DO_REPEAT_SERVO
+         << MAV_CMD_DO_LAND_START
+         << MAV_CMD_DO_SET_ROI
+         << MAV_CMD_DO_DIGICAM_CONFIGURE << MAV_CMD_DO_DIGICAM_CONTROL
+         << MAV_CMD_DO_MOUNT_CONTROL
+         << MAV_CMD_DO_SET_CAM_TRIGG_DIST
+         << MAV_CMD_DO_FENCE_ENABLE
+         << MAV_CMD_DO_INVERTED_FLIGHT
+         << MAV_CMD_DO_GRIPPER
+         << MAV_CMD_DO_GUIDED_LIMITS
+         << MAV_CMD_DO_AUTOTUNE_ENABLE;
+
+    return list;
+}
+
 int ArduSubFirmwarePlugin::remapParamNameHigestMinorVersionNumber(int majorVersionNumber) const
 {
     // Remapping supports up to 3.5
diff --git a/src/FirmwarePlugin/APM/ArduSubFirmwarePlugin.h b/src/FirmwarePlugin/APM/ArduSubFirmwarePlugin.h
index c28ca691049e1303e490a88018ecb3edda7a76d4..7191a640a2ee7a8d6e3acb36c34886391db7a7cc 100644
--- a/src/FirmwarePlugin/APM/ArduSubFirmwarePlugin.h
+++ b/src/FirmwarePlugin/APM/ArduSubFirmwarePlugin.h
@@ -100,6 +100,8 @@ class ArduSubFirmwarePlugin : public APMFirmwarePlugin
 public:
     ArduSubFirmwarePlugin(void);
 
+    QList<MAV_CMD> supportedMissionCommands(void);
+
     // Overrides from FirmwarePlugin
     int manualControlReservedButtonCount(void) final;
 
diff --git a/src/MissionManager/MissionController.cc b/src/MissionManager/MissionController.cc
index d8c28a8f23049ab86dae0828108a1aa5b855708d..4d96c86a5891b1dadf38b36ee405d3a45b77b317 100644
--- a/src/MissionManager/MissionController.cc
+++ b/src/MissionManager/MissionController.cc
@@ -340,7 +340,10 @@ int MissionController::insertSimpleMissionItem(QGeoCoordinate coordinate, int i)
     newItem->setCommand(MavlinkQmlSingleton::MAV_CMD_NAV_WAYPOINT);
     _initVisualItem(newItem);
     if (_visualItems->count() == 1) {
-        newItem->setCommand(_controllerVehicle->vtol() ? MavlinkQmlSingleton::MAV_CMD_NAV_VTOL_TAKEOFF : MavlinkQmlSingleton::MAV_CMD_NAV_TAKEOFF);
+        MavlinkQmlSingleton::Qml_MAV_CMD takeoffCmd = _controllerVehicle->vtol() ? MavlinkQmlSingleton::MAV_CMD_NAV_VTOL_TAKEOFF : MavlinkQmlSingleton::MAV_CMD_NAV_TAKEOFF;
+        if (_controllerVehicle->firmwarePlugin()->supportedMissionCommands().contains((MAV_CMD)takeoffCmd)) {
+            newItem->setCommand(takeoffCmd);
+        }
     }
     newItem->setDefaultsForCommand();
     if ((MAV_CMD)newItem->command() == MAV_CMD_NAV_WAYPOINT) {
@@ -1033,8 +1036,9 @@ void MissionController::_recalcWaypointLines(void)
         // If we still haven't found the first coordinate item and we hit a takeoff command, link back to home
         if (firstCoordinateItem &&
                 item->isSimpleItem() &&
-                (qobject_cast<SimpleMissionItem*>(item)->command() == MavlinkQmlSingleton::MAV_CMD_NAV_TAKEOFF ||
-                 qobject_cast<SimpleMissionItem*>(item)->command() == MavlinkQmlSingleton::MAV_CMD_NAV_VTOL_TAKEOFF)) {
+                (!_controllerVehicle->firmwarePlugin()->supportedMissionCommands().contains(MAV_CMD_NAV_TAKEOFF) ||
+                    qobject_cast<SimpleMissionItem*>(item)->command() == MavlinkQmlSingleton::MAV_CMD_NAV_TAKEOFF ||
+                    qobject_cast<SimpleMissionItem*>(item)->command() == MavlinkQmlSingleton::MAV_CMD_NAV_VTOL_TAKEOFF)) {
             linkStartToHome = true;
         }