diff --git a/android/AndroidManifest.xml b/android/AndroidManifest.xml index b95c5ae59130dd74800f2bbfd3d385cded32260e..7c0efd00c1b7cc0b9ff0b874995b89c57e736219 100644 --- a/android/AndroidManifest.xml +++ b/android/AndroidManifest.xml @@ -1,6 +1,6 @@ - + diff --git a/src/FlightDisplay/GuidedActionsController.qml b/src/FlightDisplay/GuidedActionsController.qml index 111b8e4f65b71205cf4105a2c601e8f7c77d0899..f111c6fb018012f3612bbaa633519ff526a75533 100644 --- a/src/FlightDisplay/GuidedActionsController.qml +++ b/src/FlightDisplay/GuidedActionsController.qml @@ -122,8 +122,8 @@ Item { property bool _hideOrbit: !QGroundControl.corePlugin.options.guidedBarShowOrbit property bool _vehicleWasFlying: false - //Handy code for debugging state problems /* + //Handy code for debugging state problems property bool __guidedModeSupported: _activeVehicle ? _activeVehicle.guidedModeSupported : false property bool __pauseVehicleSupported: _activeVehicle ? _activeVehicle.pauseVehicleSupported : false property bool __flightMode: _flightMode @@ -140,6 +140,23 @@ Item { on__FlightModeChanged: _outputState() on__GuidedModeSupportedChanged: _outputState() on__PauseVehicleSupportedChanged: _outputState() + + on_CurrentMissionIndexChanged: console.log("_currentMissionIndex", _currentMissionIndex) + on_ResumeMissionIndexChanged: console.log("_resumeMissionIndex", _resumeMissionIndex) + onShowResumeMissionChanged: { + console.log("showResumeMission", showResumeMission) + _outputState() + } + onShowStartMissionChanged: { + console.log("showStartMission", showStartMission) + _outputState() + } + onShowContinueMissionChanged: { + console.log("showContinueMission", showContinueMission) + _outputState() + } + + // End of hack */ on_VehicleFlyingChanged: { @@ -150,11 +167,8 @@ Item { _vehicleWasFlying = true } } - property var _actionData - on_CurrentMissionIndexChanged: console.log("_currentMissionIndex", _currentMissionIndex) - on_FlightModeChanged: { _vehiclePaused = _flightMode === _activeVehicle.pauseFlightMode _vehicleInRTLMode = _flightMode === _activeVehicle.rtlFlightMode diff --git a/src/MissionManager/CameraSection.FactMetaData.json b/src/MissionManager/CameraSection.FactMetaData.json index b35c376096b6184b60535353f71383be0081b0dd..c5a0b6bf7a4bf1c4187041e89b95b13c3db66559 100644 --- a/src/MissionManager/CameraSection.FactMetaData.json +++ b/src/MissionManager/CameraSection.FactMetaData.json @@ -49,7 +49,7 @@ "name": "CameraMode", "shortDescription": "Specify whether the camera should switch to photo or video mode", "type": "uint32", - "enumStrings": "Take photos,Record video", + "enumStrings": "Photo,Video", "enumValues": "0,1", "defaultValue": 0 } diff --git a/src/MissionManager/CameraSection.cc b/src/MissionManager/CameraSection.cc index 4ff082d5e2dc57c747a6cf7161906a8d6b3258b7..12b3875693d5597a0a02dbb86bfe4791909f160a 100644 --- a/src/MissionManager/CameraSection.cc +++ b/src/MissionManager/CameraSection.cc @@ -67,6 +67,7 @@ CameraSection::CameraSection(Vehicle* vehicle, QObject* parent) connect(this, &CameraSection::specifyGimbalChanged, this, &CameraSection::_setDirty); connect(this, &CameraSection::specifyCameraModeChanged, this, &CameraSection::_setDirty); + connect(this, &CameraSection::specifyGimbalChanged, this, &CameraSection::_updateSpecifiedGimbalYaw); connect(&_gimbalYawFact, &Fact::valueChanged, this, &CameraSection::_updateSpecifiedGimbalYaw); } diff --git a/src/MissionManager/CameraSection.h b/src/MissionManager/CameraSection.h index e0538473a98a5b9b7e15114d80fb1b3e67950f53..daedda5e33b8b5bec78db7ed047f9d1418cd7a5c 100644 --- a/src/MissionManager/CameraSection.h +++ b/src/MissionManager/CameraSection.h @@ -63,6 +63,7 @@ public: void setSpecifyGimbal (bool specifyGimbal); void setSpecifyCameraMode (bool specifyCameraMode); + ///< Signals specifiedGimbalYawChanged ///< @return The gimbal yaw specified by this item, NaN if not specified double specifiedGimbalYaw(void) const; diff --git a/src/MissionManager/MissionManager.cc b/src/MissionManager/MissionManager.cc index 1a2d0115608b8b85a2a8ae9d476d835279c243ba..ab54a7da11981d3f0d41bf89c6ebe1e1c2722bbd 100644 --- a/src/MissionManager/MissionManager.cc +++ b/src/MissionManager/MissionManager.cc @@ -90,9 +90,24 @@ void MissionManager::generateResumeMission(int resumeIndex) } } - resumeIndex = qMin(resumeIndex, _missionItems.count() - 1); + // Be anal about crap input + resumeIndex = qMax(0, qMin(resumeIndex, _missionItems.count() - 1)); + + // Adjust resume index to be a location based command + const MissionCommandUIInfo* uiInfo = qgcApp()->toolbox()->missionCommandTree()->getUIInfo(_vehicle, _missionItems[resumeIndex]->command()); + if (!uiInfo || uiInfo->isStandaloneCoordinate() || !uiInfo->specifiesCoordinate()) { + // We have to back up to the last command which the vehicle flies through + while (--resumeIndex > 0) { + uiInfo = qgcApp()->toolbox()->missionCommandTree()->getUIInfo(_vehicle, _missionItems[resumeIndex]->command()); + if (uiInfo && (uiInfo->specifiesCoordinate() && !uiInfo->isStandaloneCoordinate())) { + // Found it + break; + } + + } + } + resumeIndex = qMax(0, resumeIndex); - int seqNum = 0; QList resumeMission; QList includedResumeCommands; @@ -110,114 +125,85 @@ void MissionManager::generateResumeMission(int resumeIndex) << MAV_CMD_IMAGE_STOP_CAPTURE << MAV_CMD_VIDEO_START_CAPTURE << MAV_CMD_VIDEO_STOP_CAPTURE - << MAV_CMD_DO_CHANGE_SPEED; - if (_vehicle->fixedWing() && _vehicle->px4Firmware()) { - includedResumeCommands << MAV_CMD_NAV_TAKEOFF; - } + << MAV_CMD_DO_CHANGE_SPEED + << MAV_CMD_SET_CAMERA_MODE + << MAV_CMD_NAV_TAKEOFF; bool addHomePosition = _vehicle->firmwarePlugin()->sendHomePositionToVehicle(); - int setCurrentIndex = addHomePosition ? 1 : 0; - int resumeCommandCount = 0; + int prefixCommandCount = 0; for (int i=0; i<_missionItems.count(); i++) { MissionItem* oldItem = _missionItems[i]; if ((i == 0 && addHomePosition) || i >= resumeIndex || includedResumeCommands.contains(oldItem->command())) { if (i < resumeIndex) { - resumeCommandCount++; + prefixCommandCount++; } MissionItem* newItem = new MissionItem(*oldItem, this); - newItem->setIsCurrentItem( i == setCurrentIndex); - newItem->setSequenceNumber(seqNum++); + newItem->setIsCurrentItem(false); resumeMission.append(newItem); } } + prefixCommandCount = qMax(0, qMin(prefixCommandCount, resumeMission.count())); // Anal prevention against crashes // De-dup and remove no-ops from the commands which were added to the front of the mission bool foundROI = false; - bool foundCamTrigDist = false; - QList imageStartCameraIds; - QList imageStopCameraIds; - QList videoStartCameraIds; - QList videoStopCameraIds; - while (resumeIndex >= 0) { - MissionItem* resumeItem = resumeMission[resumeIndex]; + bool foundCameraSetMode = false; + bool foundCameraStartStop = false; + prefixCommandCount--; // Change from count to array index + while (prefixCommandCount >= 0) { + MissionItem* resumeItem = resumeMission[prefixCommandCount]; switch (resumeItem->command()) { + case MAV_CMD_SET_CAMERA_MODE: + // Only keep the last one + if (foundCameraSetMode) { + resumeMission.removeAt(prefixCommandCount); + } + foundCameraSetMode = true; + break; case MAV_CMD_DO_SET_ROI: // Only keep the last one if (foundROI) { - resumeMission.removeAt(resumeIndex); + resumeMission.removeAt(prefixCommandCount); } foundROI = true; break; case MAV_CMD_DO_SET_CAM_TRIGG_DIST: - // Only keep the last one - if (foundCamTrigDist) { - resumeMission.removeAt(resumeIndex); + case MAV_CMD_IMAGE_STOP_CAPTURE: + case MAV_CMD_VIDEO_START_CAPTURE: + case MAV_CMD_VIDEO_STOP_CAPTURE: + // Only keep the first of these commands that are found + if (foundCameraStartStop) { + resumeMission.removeAt(prefixCommandCount); } - foundCamTrigDist = true; + foundCameraStartStop = true; break; case MAV_CMD_IMAGE_START_CAPTURE: - { - // FIXME: Handle single image capture - int cameraId = resumeItem->param1(); - - if (resumeItem->param3() == 1) { - // This is an individual image capture command, remove it - resumeMission.removeAt(resumeIndex); + if (resumeItem->param3() != 0) { + // Remove commands which do not trigger by time + resumeMission.removeAt(prefixCommandCount); break; } - // If we already found an image stop, then all image start/stop commands are useless - // De-dup repeated image start commands - // Otherwise keep only the last image start - if (imageStopCameraIds.contains(cameraId) || imageStartCameraIds.contains(cameraId)) { - resumeMission.removeAt(resumeIndex); - } - if (!imageStopCameraIds.contains(cameraId)) { - imageStopCameraIds.append(cameraId); - } - } - break; - case MAV_CMD_IMAGE_STOP_CAPTURE: - { - int cameraId = resumeItem->param1(); - // Image stop only matters to kill all previous image starts - if (!imageStopCameraIds.contains(cameraId)) { - imageStopCameraIds.append(cameraId); - } - resumeMission.removeAt(resumeIndex); - } - break; - case MAV_CMD_VIDEO_START_CAPTURE: - { - int cameraId = resumeItem->param1(); - // If we've already found a video stop, then all video start/stop commands are useless - // De-dup repeated video start commands - // Otherwise keep only the last video start - if (videoStopCameraIds.contains(cameraId) || videoStopCameraIds.contains(cameraId)) { - resumeMission.removeAt(resumeIndex); - } - if (!videoStopCameraIds.contains(cameraId)) { - videoStopCameraIds.append(cameraId); - } - } - break; - case MAV_CMD_VIDEO_STOP_CAPTURE: - { - int cameraId = resumeItem->param1(); - // Video stop only matters to kill all previous video starts - if (!videoStopCameraIds.contains(cameraId)) { - videoStopCameraIds.append(cameraId); + if (foundCameraStartStop) { + // Only keep the first of these commands that are found + resumeMission.removeAt(prefixCommandCount); } - resumeMission.removeAt(resumeIndex); - } + foundCameraStartStop = true; break; default: break; } - resumeIndex--; + prefixCommandCount--; } + // Adjust sequence numbers and current item + int seqNum = 0; + for (int i=0; isetSequenceNumber(seqNum++); + } + int setCurrentIndex = addHomePosition ? 1 : 0; + resumeMission[setCurrentIndex]->setIsCurrentItem(true); + // Send to vehicle _clearAndDeleteWriteMissionItems(); for (int i=0; iflightSpeed(), &Fact::rawValueChanged, this, &SimpleMissionItem::specifiedFlightSpeedChanged); + connect(_speedSection, &SpeedSection::dirtyChanged, this, &SimpleMissionItem::_sectionDirtyChanged); + connect(_speedSection, &SpeedSection::itemCountChanged, this, &SimpleMissionItem::_updateLastSequenceNumber); + connect(_speedSection, &SpeedSection::specifiedFlightSpeedChanged, this, &SimpleMissionItem::specifiedFlightSpeedChanged); emit cameraSectionChanged(_cameraSection); emit speedSectionChanged(_speedSection); diff --git a/src/MissionManager/SpeedSection.cc b/src/MissionManager/SpeedSection.cc index 7979690ebb87834ecd6a8570f8ad4d4b9370d0fa..6f465a3dca0ab9a9b5afccf64a72f988d353480d 100644 --- a/src/MissionManager/SpeedSection.cc +++ b/src/MissionManager/SpeedSection.cc @@ -38,8 +38,11 @@ SpeedSection::SpeedSection(Vehicle* vehicle, QObject* parent) _flightSpeedFact.setMetaData(_metaDataMap[_flightSpeedName]); _flightSpeedFact.setRawValue(flightSpeed); - connect(this, &SpeedSection::specifyFlightSpeedChanged, this, &SpeedSection::settingsSpecifiedChanged); - connect(&_flightSpeedFact, &Fact::valueChanged, this, &SpeedSection::_setDirty); + connect(this, &SpeedSection::specifyFlightSpeedChanged, this, &SpeedSection::settingsSpecifiedChanged); + connect(&_flightSpeedFact, &Fact::valueChanged, this, &SpeedSection::_setDirty); + + connect(this, &SpeedSection::specifyFlightSpeedChanged, this, &SpeedSection::_updateSpecifiedFlightSpeed); + connect(&_flightSpeedFact, &Fact::valueChanged, this, &SpeedSection::_updateSpecifiedFlightSpeed); } bool SpeedSection::settingsSpecified(void) const @@ -134,3 +137,15 @@ bool SpeedSection::scanForSection(QmlObjectListModel* visualItems, int scanIndex return false; } + + +double SpeedSection::specifiedFlightSpeed(void) const +{ + return _specifyFlightSpeed ? _flightSpeedFact.rawValue().toDouble() : std::numeric_limits::quiet_NaN(); +} + +void SpeedSection::_updateSpecifiedFlightSpeed(void) +{ + emit specifiedFlightSpeedChanged(specifiedFlightSpeed()); +} + diff --git a/src/MissionManager/SpeedSection.h b/src/MissionManager/SpeedSection.h index 83069d5c7d175372b4be7587381a63e32063099f..1b37b8f9f21facd47d15497cb03489639c44901b 100644 --- a/src/MissionManager/SpeedSection.h +++ b/src/MissionManager/SpeedSection.h @@ -27,6 +27,10 @@ public: Fact* flightSpeed (void) { return &_flightSpeedFact; } void setSpecifyFlightSpeed (bool specifyFlightSpeed); + ///< Signals specifiedFlightSpeedChanged + ///< @return The flight speed specified by this item, NaN if not specified + double specifiedFlightSpeed(void) const; + // Overrides from Section bool available (void) const override { return _available; } bool dirty (void) const override { return _dirty; } @@ -38,10 +42,12 @@ public: bool settingsSpecified (void) const override; signals: - void specifyFlightSpeedChanged(bool specifyFlightSpeed); + void specifyFlightSpeedChanged (bool specifyFlightSpeed); + void specifiedFlightSpeedChanged (double flightSpeed); private slots: - void _setDirty(void); + void _setDirty (void); + void _updateSpecifiedFlightSpeed(void); private: bool _available; diff --git a/src/MissionManager/SurveyMissionItem.cc b/src/MissionManager/SurveyMissionItem.cc index eb28e7ec78f02e00bafa01f7435642a3ea858df1..94520eefee252dd9d2e5d5b371e7ca56e5b3859b 100644 --- a/src/MissionManager/SurveyMissionItem.cc +++ b/src/MissionManager/SurveyMissionItem.cc @@ -747,7 +747,9 @@ void SurveyMissionItem::_generateGrid(void) _setSurveyDistance(surveyDistance); if (cameraShots == 0 && _triggerCamera()) { - cameraShots = (int)ceil(surveyDistance / _triggerDistance()); + cameraShots = (int)floor(surveyDistance / _triggerDistance()); + // Take into account immediate camera trigger at waypoint entry + cameraShots++; } _setCameraShots(cameraShots); @@ -1065,7 +1067,9 @@ int SurveyMissionItem::_gridGenerator(const QList& polygonPoints, QLis // Calc camera shots here if there are no images in turnaround if (_triggerCamera() && !_imagesEverywhere()) { for (int i=0; i -1 || activeMapType.name.indexOf("Hybrid") > -1 - plugin: Plugin { name: "QGroundControl" } - MapRectangle { id: mapBoundary border.width: 2