Merge pull request #5848 from patrickelectric/mission_tkoff

MissionController: Add takeoff only with supported vehicles

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

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;
 

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;
         }