Merge pull request #5044 from DonLakeFlyer/SurveyGimbal

Plan: Survey gimbal

src/FirmwarePlugin/FirmwarePlugin.cc

@@ -475,3 +475,12 @@ void FirmwarePlugin::missionFlightSpeedInfo(Vehicle* vehicle, double& hoverSpeed
     hoverSpeed = appSettings->offlineEditingHoverSpeed()->rawValue().toDouble();
     cruiseSpeed = appSettings->offlineEditingCruiseSpeed()->rawValue().toDouble();
 }
+
+bool FirmwarePlugin::hasGimbal(Vehicle* vehicle, bool& rollSupported, bool& pitchSupported, bool& yawSupported)
+{
+    Q_UNUSED(vehicle);
+    rollSupported = false;
+    pitchSupported = false;
+    yawSupported = false;
+    return false;
+}

src/FirmwarePlugin/FirmwarePlugin.h

@@ -287,9 +287,16 @@ public:
     ///     @param[out] cruiseSpeed Flight speed for vehicle flying in fixed wing forward flight mode. 0 for none, or not available.
     virtual void missionFlightSpeedInfo(Vehicle* vehicle, double& hoverSpeed, double& cruiseSpeed);
 
-    // Returns the parameter which control auto-dismar. Assume == 0 means no auto disarm
+    // Returns the parameter which control auto-disarm. Assume == 0 means no auto disarm
     virtual QString autoDisarmParameter(Vehicle* vehicle);
 
+    /// Used to determine whether a vehicle has a gimbal.
+    ///     @param[out] rollSupported Gimbal supports roll
+    ///     @param[out] pitchSupported Gimbal supports pitch
+    ///     @param[out] yawSupported Gimbal supports yaw
+    /// @return true: vehicle has gimbal, false: gimbal support unknown
+    virtual bool hasGimbal(Vehicle* vehicle, bool& rollSupported, bool& pitchSupported, bool& yawSupported);
+
     // FIXME: Hack workaround for non pluginize FollowMe support
     static const char* px4FollowMeFlightMode;
 

src/MissionManager/MissionController.cc

@@ -288,6 +288,19 @@ int MissionController::insertComplexMissionItem(QString itemName, QGeoCoordinate
     if (itemName == _surveyMissionItemName) {
         newItem = new SurveyMissionItem(_controllerVehicle, _visualItems);
         newItem->setCoordinate(mapCenterCoordinate);
+        // If the vehicle is known to have a gimbal then we automatically point the gimbal straight down if not already set
+        bool rollSupported = false;
+        bool pitchSupported = false;
+        bool yawSupported = false;
+        if (_controllerVehicle->firmwarePlugin()->hasGimbal(_controllerVehicle, rollSupported, pitchSupported, yawSupported) && pitchSupported) {
+            MissionSettingsItem* settingsItem = _visualItems->value<MissionSettingsItem*>(0);
+            // If the user already specified a gimbal angle leave it alone
+            CameraSection* cameraSection = settingsItem->cameraSection();
+            if (!cameraSection->specifyGimbal()) {
+                cameraSection->setSpecifyGimbal(true);
+                cameraSection->gimbalYaw()->setRawValue(-90.0);
+            }
+        }
     } else if (itemName == _fwLandingMissionItemName) {
         newItem = new FixedWingLandingComplexItem(_controllerVehicle, _visualItems);
     } else {
@@ -306,11 +319,42 @@ int MissionController::insertComplexMissionItem(QString itemName, QGeoCoordinate
 
 void MissionController::removeMissionItem(int index)
 {
+    if (index <= 0 || index >= _visualItems->count()) {
+        qWarning() << "MissionController::removeMissionItem called with bad index - count:index" << _visualItems->count() << index;
+        return;
+    }
+
+    bool surveyRemoved = _visualItems->value<SurveyMissionItem*>(index);
     VisualMissionItem* item = qobject_cast<VisualMissionItem*>(_visualItems->removeAt(index));
 
     _deinitVisualItem(item);
     item->deleteLater();
 
+    if (surveyRemoved) {
+        // Determine if the mission still has another survey in it
+        bool foundSurvey = false;
+        for (int i=1; i<_visualItems->count(); i++) {
+            if (_visualItems->value<SurveyMissionItem*>(i)) {
+                foundSurvey = true;
+                break;
+            }
+        }
+
+        // If there is no longer a survey item in the mission remove the gimbal pitch command
+        if (!foundSurvey) {
+            bool rollSupported = false;
+            bool pitchSupported = false;
+            bool yawSupported = false;
+            if (_controllerVehicle->firmwarePlugin()->hasGimbal(_controllerVehicle, rollSupported, pitchSupported, yawSupported) && pitchSupported) {
+                MissionSettingsItem* settingsItem = _visualItems->value<MissionSettingsItem*>(0);
+                CameraSection* cameraSection = settingsItem->cameraSection();
+                if (cameraSection->specifyGimbal() && cameraSection->gimbalYaw()->rawValue().toDouble() == -90.0 && cameraSection->gimbalPitch()->rawValue().toDouble() == 0.0) {
+                    cameraSection->setSpecifyGimbal(false);
+                }
+            }
+        }
+    }
+
     _recalcAll();
     setDirty(true);
 }

src/MissionManager/PlanMasterController.cc

@@ -119,9 +119,13 @@ void PlanMasterController::_activeVehicleChanged(Vehicle* activeVehicle)
 
 void PlanMasterController::loadFromVehicle(void)
 {
-    _loadGeoFence = true;
-    _missionController.loadFromVehicle();
-    setDirty(false);
+    if (!offline()) {
+        _loadGeoFence = true;
+        _missionController.loadFromVehicle();
+        setDirty(false);
+    } else {
+        qWarning() << "PlanMasterController::sendToVehicle called while offline";
+    }
 }
 
 
@@ -154,9 +158,13 @@ void PlanMasterController::_loadSendGeoFenceCompelte(void)
 
 void PlanMasterController::sendToVehicle(void)
 {
-    _sendGeoFence = true;
-    _missionController.sendToVehicle();
-    setDirty(false);
+    if (!offline()) {
+        _sendGeoFence = true;
+        _missionController.sendToVehicle();
+        setDirty(false);
+    } else {
+        qWarning() << "PlanMasterController::sendToVehicle called while offline";
+    }
 }
 
 void PlanMasterController::loadFromFile(const QString& filename)
@@ -264,14 +272,20 @@ void PlanMasterController::saveToFile(const QString& filename)
 
 void PlanMasterController::removeAll(void)
 {
-
+    _missionController.removeAll();
+    _geoFenceController.removeAll();
+    _rallyPointController.removeAll();
 }
 
 void PlanMasterController::removeAllFromVehicle(void)
 {
-    _missionController.removeAllFromVehicle();
-    _geoFenceController.removeAllFromVehicle();
-    _rallyPointController.removeAllFromVehicle();
+    if (!offline()) {
+        _missionController.removeAllFromVehicle();
+        _geoFenceController.removeAllFromVehicle();
+        _rallyPointController.removeAllFromVehicle();
+    } else {
+        qWarning() << "PlanMasterController::removeAllFromVehicle called while offline";
+    }
 }
 
 bool PlanMasterController::containsItems(void) const

src/PlanView/PlanToolBar.qml

@@ -33,9 +33,9 @@ Rectangle {
     property bool   missionDirty:               _controllerValid ? planMasterController.missionController.dirty : false
 
     property bool   _controllerValid:           planMasterController != undefined
-    property var    _activeVehicle:             QGroundControl.multiVehicleManager.activeVehicle
-    property var    _controllerDirty:           planMasterController ? planMasterController.dirty : false
-    property var    _controllerSyncInProgress:  planMasterController ? planMasterController.syncInProgress : false
+    property bool   _controllerOffline:         _controllerValid ? planMasterController.offline : true
+    property var    _controllerDirty:           _controllerValid ? planMasterController.dirty : false
+    property var    _controllerSyncInProgress:  _controllerValid ? planMasterController.syncInProgress : false
 
     property bool   _statusValid:               currentMissionItem != undefined
     property bool   _missionValid:              missionItems != undefined
@@ -274,7 +274,7 @@ Rectangle {
         anchors.verticalCenter: parent.verticalCenter
         text:                   _controllerDirty ? qsTr("Upload Required") : qsTr("Upload")
         enabled:                !_controllerSyncInProgress
-        visible:                !planMasterController.offline
+        visible:                !_controllerOffline
         onClicked:              planMasterController.upload()
 
         PropertyAnimation on opacity {

src/PlanView/PlanView.qml

@@ -664,9 +664,14 @@ QGCView {
     Component {
         id: removeAllPromptDialog
         QGCViewMessage {
-            message: qsTr("Are you sure you want to remove all items? This will also remove all items from the vehicle.")
+            message: qsTr("Are you sure you want to remove all items? ") +
+                     (_planMasterController.offline ? "" : qsTr("This will also remove all items from the vehicle."))
             function accept() {
-                masterController.removeAllFromVehicle()
+                if (_planMasterController.offline) {
+                    masterController.removeAll()
+                } else {
+                    masterController.removeAllFromVehicle()
+                }
                 hideDialog()
             }
         }