From 310f18ff187d7049a1d79c6ef85154704220026d Mon Sep 17 00:00:00 2001 From: DonLakeFlyer Date: Sat, 22 Apr 2017 09:26:23 -0700 Subject: [PATCH] Support for automatic gimbal setting on Survey --- src/FirmwarePlugin/FirmwarePlugin.cc | 9 +++++ src/FirmwarePlugin/FirmwarePlugin.h | 9 ++++- src/MissionManager/MissionController.cc | 44 ++++++++++++++++++++++ src/MissionManager/PlanMasterController.cc | 34 ++++++++++++----- 4 files changed, 85 insertions(+), 11 deletions(-) diff --git a/src/FirmwarePlugin/FirmwarePlugin.cc b/src/FirmwarePlugin/FirmwarePlugin.cc index 9dc62344c..5ed512200 100644 --- a/src/FirmwarePlugin/FirmwarePlugin.cc +++ b/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; +} diff --git a/src/FirmwarePlugin/FirmwarePlugin.h b/src/FirmwarePlugin/FirmwarePlugin.h index 5c6a6c686..022317d57 100644 --- a/src/FirmwarePlugin/FirmwarePlugin.h +++ b/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; diff --git a/src/MissionManager/MissionController.cc b/src/MissionManager/MissionController.cc index 5dbb4f475..b5bac4e93 100644 --- a/src/MissionManager/MissionController.cc +++ b/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) && yawSupported) { + MissionSettingsItem* settingsItem = _visualItems->value(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(index); VisualMissionItem* item = qobject_cast(_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(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) && yawSupported) { + MissionSettingsItem* settingsItem = _visualItems->value(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); } diff --git a/src/MissionManager/PlanMasterController.cc b/src/MissionManager/PlanMasterController.cc index c404805dd..6156259a2 100644 --- a/src/MissionManager/PlanMasterController.cc +++ b/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 -- 2.22.0