diff --git a/.travis.yml b/.travis.yml index c4b61d6884abc652808b73b510f37c8ea8573aa9..a4a2e7f69791d37ad837641d09c47ad907808b5f 100644 --- a/.travis.yml +++ b/.travis.yml @@ -241,6 +241,7 @@ deploy: # deploy tagged installers to s3 latest/ - provider: s3 + edge: true # Use V2 provider to work around V1 bug access_key_id: AKIAIVORNALE7NHD3T6Q secret_access_key: secure: BsLXeXUPsCJdX4tawrDnO8OFK5Hk4kzlDTiyH93En6TbjUargVAWDMcHVj7TUhr7+3Tao1W1zb0G4SJe9kHv+jrky0yE72KvoG3YAON0VXWKizxBAKkgHE2RxSTNAwDeKbi2G6YJfNDescBBfX7zEohShdXglQu7CGaUQKRaiI4= @@ -283,6 +284,7 @@ deploy: on: tags: true condition: $CONFIG = installer + condition: $SPEC != macx-clang # GitHub OSX deploy broken due to travis problem notifications: webhooks: diff --git a/CMakeLists.txt b/CMakeLists.txt index 5f60bf299db45b6f09de038a8de850f6c72ed476..e2f00e718c229f91827475b1616416eaae0efd15 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -88,6 +88,7 @@ pkg_check_modules(GST gstreamer-1.0>=1.14 gstreamer-video-1.0>=1.14 gstreamer-gl-1.0>=1.14 + egl ) if (GST_FOUND) diff --git a/ChangeLog.md b/ChangeLog.md index 805720edf15c3167096058a667ecefd140d0bd5a..096f20600ac409a7e364dd1b7cda16f3e9d3a8f4 100644 --- a/ChangeLog.md +++ b/ChangeLog.md @@ -8,12 +8,28 @@ Note: This file only contains high level features or important fixes. ## 4.0 -### 4.0.1 - Not yet release +### 4.0.3 - Not yet released + + +### 4.0.3 - Not yet released + +* Plan: Add setting for takeoff item not required +* Plan: Takeoff item must be added prior to allowing other item types to enable + +### 4.0.2 - Stable + +* Fix Mavlink V2 protocol negotation based on capability bits +* Fix waiting for AUTOPILOT_VERSION response to get capability bits +* ArduPilot: Above two fixes make fence/rally support enabling more reliable + +### 4.0.1 * Fix ArduPilot current mission item tracking in Fly view * Fix ADSB vehicle display +* Fix map positioning bug in Plan view +* Fix Windows 0xcc000007b startup error causes by incorrect VC runtimes being installed. -### 4.0.0 - Stable +### 4.0.0 * Added ROI option during manual flight. * Windows: Move builds to 64 bit, Qt 5.12.5 diff --git a/QGCSetup.pri b/QGCSetup.pri index e6546f6af289e052acb87135e03e157a30595dac..9ecb1435eeafa118a03a7184e1ef5a33bc0066b5 100644 --- a/QGCSetup.pri +++ b/QGCSetup.pri @@ -79,8 +79,6 @@ WindowsBuild { # Copy Visual Studio DLLs # Note that this is only done for release because the debugging versions of these DLLs cannot be redistributed. QMAKE_POST_LINK += $$escape_expand(\\n) $$QMAKE_COPY \"$$BASEDIR\\deploy\\msvcp140.dll\" \"$$DESTDIR_WIN\" - QMAKE_POST_LINK += $$escape_expand(\\n) $$QMAKE_COPY \"$$BASEDIR\\deploy\\msvcp140_1.dll\" \"$$DESTDIR_WIN\" - QMAKE_POST_LINK += $$escape_expand(\\n) $$QMAKE_COPY \"$$BASEDIR\\deploy\\msvcp140_2.dll\" \"$$DESTDIR_WIN\" QMAKE_POST_LINK += $$escape_expand(\\n) $$QMAKE_COPY \"$$BASEDIR\\deploy\\vcruntime140.dll\" \"$$DESTDIR_WIN\" } diff --git a/deploy/msvcp140.dll b/deploy/msvcp140.dll index 6ca27d5150e0faaf4a32ee3224332cdea5b61829..98313d4c125307dbc5784b5a329c7d113183d3c8 100644 Binary files a/deploy/msvcp140.dll and b/deploy/msvcp140.dll differ diff --git a/deploy/msvcp140_1.dll b/deploy/msvcp140_1.dll deleted file mode 100644 index 64f090daf6d5d06dcc2991c6469ffe23964b59c3..0000000000000000000000000000000000000000 Binary files a/deploy/msvcp140_1.dll and /dev/null differ diff --git a/deploy/msvcp140_2.dll b/deploy/msvcp140_2.dll deleted file mode 100644 index 682fd794200adce7f9a6184ea4cfaf019702af1f..0000000000000000000000000000000000000000 Binary files a/deploy/msvcp140_2.dll and /dev/null differ diff --git a/deploy/qgroundcontrol_installer.nsi b/deploy/qgroundcontrol_installer.nsi index e69741bceefa3da8313b232f0f6721cbf2971fa1..b3a8a3c83e9d92ae96814a74afd27c726379fdfb 100644 --- a/deploy/qgroundcontrol_installer.nsi +++ b/deploy/qgroundcontrol_installer.nsi @@ -71,8 +71,7 @@ check64BitUninstall: StrCmp $R0 "" doInstall doUninstall: - DetailPrint "Uninstalling previous version..." - ExecWait "$R0 /S -LEAVE_DATA=1" + DetailPrint "Uninstalling previous version..." ExecWait "$R0 /S -LEAVE_DATA=1 _?=$INSTDIR" IntCmp $0 0 doInstall MessageBox MB_OK|MB_ICONEXCLAMATION \ diff --git a/deploy/vcruntime140.dll b/deploy/vcruntime140.dll index 3e49417f82c57023b2fd59defeb87e8b2ffea620..34a0e72504776f90526966db1e3e9d382674d613 100644 Binary files a/deploy/vcruntime140.dll and b/deploy/vcruntime140.dll differ diff --git a/src/AutoPilotPlugins/PX4/AirframeFactMetaData.xml b/src/AutoPilotPlugins/PX4/AirframeFactMetaData.xml index a51fa522ca4ea376e4bf98fc5c4f1219ad8a12cf..d7a74dfc11058dfb7a148804a8e3e8e6975803a2 100644 --- a/src/AutoPilotPlugins/PX4/AirframeFactMetaData.xml +++ b/src/AutoPilotPlugins/PX4/AirframeFactMetaData.xml @@ -726,6 +726,14 @@ Speed of left wheels Speed of right wheels + + Rover + Katrin Moritz + Rover + Steering servo + Speed of left wheels + Speed of right wheels + diff --git a/src/AutoPilotPlugins/PX4/PX4SimpleFlightModes.qml b/src/AutoPilotPlugins/PX4/PX4SimpleFlightModes.qml index cef4a531ebea1192a35b7d96ef55988b7f35e540..b55ad337dd64ce1f672ff1d7954161b53d5db422 100644 --- a/src/AutoPilotPlugins/PX4/PX4SimpleFlightModes.qml +++ b/src/AutoPilotPlugins/PX4/PX4SimpleFlightModes.qml @@ -102,7 +102,7 @@ Item { QGCLabel { Layout.fillWidth: true text: qsTr("Flight Mode %1").arg(modelData + 1) - color: controller.activeFlightMode == index ? "yellow" : qgcPal.text + color: (controller.activeFlightMode - 1) == index ? "yellow" : qgcPal.text } } diff --git a/src/Camera/QGCCameraControl.cc b/src/Camera/QGCCameraControl.cc index 1fd5ce07112181464e92543e033561fa3b233048..f46f974d5c46c3db191fc8a91b79b4034b9b1634 100644 --- a/src/Camera/QGCCameraControl.cc +++ b/src/Camera/QGCCameraControl.cc @@ -388,11 +388,11 @@ QGCCameraControl::takePhoto() _setPhotoStatus(PHOTO_CAPTURE_IN_PROGRESS); _captureInfoRetries = 0; //-- Capture local image as well - if(qgcApp()->toolbox()->videoManager()->videoReceiver()) { + if(qgcApp()->toolbox()->videoManager()) { QString photoPath = qgcApp()->toolbox()->settingsManager()->appSettings()->savePath()->rawValue().toString() + QStringLiteral("/Photo"); QDir().mkpath(photoPath); photoPath += + "/" + QDateTime::currentDateTime().toString("yyyy-MM-dd_hh.mm.ss.zzz") + ".jpg"; - qgcApp()->toolbox()->videoManager()->videoReceiver()->grabImage(photoPath); + qgcApp()->toolbox()->videoManager()->grabImage(photoPath); } return true; } @@ -1542,11 +1542,11 @@ QGCCameraControl::handleCaptureStatus(const mavlink_camera_capture_status_t& cap //-- Time Lapse if(photoStatus() == PHOTO_CAPTURE_INTERVAL_IDLE || photoStatus() == PHOTO_CAPTURE_INTERVAL_IN_PROGRESS) { //-- Capture local image as well - if(qgcApp()->toolbox()->videoManager()->videoReceiver()) { + if(qgcApp()->toolbox()->videoManager()) { QString photoPath = qgcApp()->toolbox()->settingsManager()->appSettings()->savePath()->rawValue().toString() + QStringLiteral("/Photo"); QDir().mkpath(photoPath); photoPath += + "/" + QDateTime::currentDateTime().toString("yyyy-MM-dd_hh.mm.ss.zzz") + ".jpg"; - qgcApp()->toolbox()->videoManager()->videoReceiver()->grabImage(photoPath); + qgcApp()->toolbox()->videoManager()->grabImage(photoPath); } } } diff --git a/src/FirmwarePlugin/PX4/PX4FirmwarePlugin.cc b/src/FirmwarePlugin/PX4/PX4FirmwarePlugin.cc index c5cffe94656544fb704a4e8e2a3101047a5ef6b4..0d967c4cc6398b609b327e02e1e3fb62866cb2f8 100644 --- a/src/FirmwarePlugin/PX4/PX4FirmwarePlugin.cc +++ b/src/FirmwarePlugin/PX4/PX4FirmwarePlugin.cc @@ -23,6 +23,8 @@ #include "RadioComponentController.h" #include "QGCCameraManager.h" #include "QGCFileDownload.h" +#include "SettingsManager.h" +#include "PlanViewSettings.h" #include @@ -279,7 +281,7 @@ void PX4FirmwarePlugin::getParameterMetaDataVersionInfo(const QString& metaDataF QList PX4FirmwarePlugin::supportedMissionCommands(void) { - return { + QList cmds = { MAV_CMD_NAV_WAYPOINT, MAV_CMD_NAV_LOITER_UNLIM, MAV_CMD_NAV_LOITER_TIME, MAV_CMD_NAV_LOITER_TO_ALT, MAV_CMD_NAV_LAND, MAV_CMD_NAV_TAKEOFF, MAV_CMD_NAV_RETURN_TO_LAUNCH, @@ -298,6 +300,12 @@ QList PX4FirmwarePlugin::supportedMissionCommands(void) MAV_CMD_NAV_DELAY, MAV_CMD_CONDITION_YAW, }; + + if (qgcApp()->toolbox()->settingsManager()->planViewSettings()->useConditionGate()->rawValue().toBool()) { + cmds.append(MAV_CMD_CONDITION_GATE); + } + + return cmds; } QString PX4FirmwarePlugin::missionCommandOverrides(MAV_TYPE vehicleType) const @@ -474,14 +482,13 @@ void PX4FirmwarePlugin::guidedModeChangeAltitude(Vehicle* vehicle, double altitu void PX4FirmwarePlugin::startMission(Vehicle* vehicle) { - if (_setFlightModeAndValidate(vehicle, missionFlightMode())) { if (!_armVehicleAndValidate(vehicle)) { qgcApp()->showMessage(tr("Unable to start mission: Vehicle rejected arming.")); return; } } else { - qgcApp()->showMessage(tr("Unable to start mission: Vehicle not ready.")); + qgcApp()->showMessage(tr("Unable to start mission: Vehicle not changing to %1 flight mode.").arg(missionFlightMode())); } } diff --git a/src/FirmwarePlugin/PX4/PX4ParameterFactMetaData.xml b/src/FirmwarePlugin/PX4/PX4ParameterFactMetaData.xml index cd6ae42e322c8773af99658e80b7929fbb83df87..38a44775f69dd9e0867a8a11faacac3029a81da5 100644 --- a/src/FirmwarePlugin/PX4/PX4ParameterFactMetaData.xml +++ b/src/FirmwarePlugin/PX4/PX4ParameterFactMetaData.xml @@ -1422,57 +1422,6 @@ See COM_OBL_ACT and COM_OBL_RC_ACT to configure action 1800000000 deg * 1e7 - - Airfield home wait time - The amount of time in seconds the system should wait at the airfield home waypoint - 0.0 - 3600.0 - s - 0 - 1 - - - Skip comms hold wp - If set to 1 the system will skip the comms hold wp on data link loss and will directly fly to airfield home - - - Comms hold alt - Altitude of comms hold waypoint - -50 - 30000 - m - 1 - 0.5 - - - Comms hold Lat - Latitude of comms hold waypoint - -900000000 - 900000000 - deg * 1e7 - - - Comms hold Lon - Longitude of comms hold waypoint - -1800000000 - 1800000000 - deg * 1e7 - - - Comms hold wait time - The amount of time in seconds the system should wait at the comms hold waypoint - 0.0 - 3600.0 - s - 0 - 1 - - - Number of allowed Datalink timeouts - After more than this number of data link timeouts the aircraft returns home directly - 0 - 1000 - @@ -2360,6 +2309,10 @@ This is the ratio of static pressure error to dynamic pressure generated by a wi Airspeed disabled + + Enable airspeed scaling + This enables a logic that automatically adjusts the output of the rate controller to take into account the real torque produced by an aerodynamic control surface given the current deviation from the trim airspeed (FW_AIRSPD_TRIM). Enable when using aerodynamic control surfaces (e.g.: plane) Disable when using rotor wings (e.g.: autogyro) + Whether to scale throttle by battery power level This compensates for voltage drop of the battery over time by attempting to normalize performance across the operating range of the battery. The fixed wing should constantly behave as if it was fully charged with reduced max thrust at lower battery percentages. i.e. if cruise speed is at 0.5 throttle at 100% battery, it will still be 0.5 at 60% battery. @@ -4254,13 +4207,12 @@ Used to calculate increased terrain random walk nosie due to movement Set data link loss failsafe mode - The data link loss failsafe will only be entered after a timeout, set by COM_DL_LOSS_T in seconds. Once the timeout occurs the selected action will be executed. Setting this parameter to 4 will enable CASA Outback Challenge rules, which are only recommended to participants of that competition. + The data link loss failsafe will only be entered after a timeout, set by COM_DL_LOSS_T in seconds. Once the timeout occurs the selected action will be executed. Disabled Hold mode Return mode Land mode - Data Link Auto Recovery (CASA Outback Challenge rules) Terminate Lockdown @@ -4305,25 +4257,16 @@ Used to calculate increased terrain random walk nosie due to movement Set RC loss failsafe mode - The RC loss failsafe will only be entered after a timeout, set by COM_RC_LOSS_T in seconds. If RC input checks have been disabled by setting the COM_RC_IN_MODE param it will not be triggered. Setting this parameter to 4 will enable CASA Outback Challenge rules, which are only recommended to participants of that competition. + The RC loss failsafe will only be entered after a timeout, set by COM_RC_LOSS_T in seconds. If RC input checks have been disabled by setting the COM_RC_IN_MODE param it will not be triggered. Disabled Hold mode Return mode Land mode - RC Auto Recovery (CASA Outback Challenge rules) Terminate Lockdown - - RC Loss Loiter Time (CASA Outback Challenge rules) - The amount of time in seconds the system should loiter at current position before termination. Only applies if NAV_RCL_ACT is set to 2 (CASA Outback Challenge rules). Set to -1 to make the system skip loitering. - -1.0 - s - 1 - 0.1 - Set traffic avoidance mode Enabling this will allow the system to respond to transponder data from e.g. ADSB transponders @@ -4838,6 +4781,10 @@ Setting this parameter to 0 disables the filter m/s 2 + + Hover thrust source selector + Set false to use the fixed parameter MPC_THR_HOVER (EXPERIMENTAL) Set true to use the value computed by the hover thrust estimator + Low pass filter cut freq. for numerical velocity derivative 0.0 @@ -5964,48 +5911,6 @@ default 1.5 turns per second 1.0 - - - Ground drag property - This parameter encodes the ground drag coefficient and the corresponding decrease in wind speed from the plane altitude to ground altitude. - 0.001 - 0.1 - - - Payload drag coefficient of the dropped object - The drag coefficient (cd) is the typical drag constant for air. It is in general object specific, but the closest primitive shape to the actual object should give good results: http://en.wikipedia.org/wiki/Drag_coefficient - 0.08 - 1.5 - - - Payload mass - A typical small toy ball: 0.025 kg OBC water bottle: 0.6 kg - 0.001 - 5.0 - kg - - - Payload front surface area - A typical small toy ball: (0.045 * 0.045) / 4.0 * pi = 0.001590 m^2 OBC water bottle: (0.063 * 0.063) / 4.0 * pi = 0.003117 m^2 - 0.001 - 0.5 - m^2 - - - Drop precision - If the system is closer than this distance on passing over the drop position, it will release the payload. This is a safeguard to prevent a drop out of the required accuracy. - 1.0 - 80.0 - m - - - Plane turn radius - The planes known minimal turn radius - use a higher value to make the plane maneuver more distant from the actual drop position. This is to ensure the wings are level during the drop. - 30.0 - 500.0 - m - - BlinkM LED diff --git a/src/FlightDisplay/FlightDisplayView.qml b/src/FlightDisplay/FlightDisplayView.qml index 9bcbf6918e49f174885293783af5ac1412e0f2a1..92644b07cb6f1f4328d7b836726de725318a5e45 100644 --- a/src/FlightDisplay/FlightDisplayView.qml +++ b/src/FlightDisplay/FlightDisplayView.qml @@ -778,17 +778,17 @@ Item { //-- Checklist GUI Popup { id: checklistDropPanel - x: Math.round((mainWindow.width - width) * 0.5) - y: Math.round((mainWindow.height - height) * 0.5) + x: toolStrip.x + toolStrip.width + (ScreenTools.defaultFontPixelWidth * 2) + y: toolStrip.y height: checkList.height width: checkList.width modal: true focus: true closePolicy: Popup.CloseOnEscape | Popup.CloseOnPressOutside background: Rectangle { - anchors.fill: parent - color: Qt.rgba(0,0,0,0) - clip: true + anchors.fill: parent + color: Qt.rgba(0,0,0,0) + clip: true } Loader { diff --git a/src/FlightDisplay/FlightDisplayViewVideo.qml b/src/FlightDisplay/FlightDisplayViewVideo.qml index 393886a9e22eb2ba9cc426d391ed1081355cbe38..16c4a2afdfbef88da568d2b676c9d0d3a00730e7 100644 --- a/src/FlightDisplay/FlightDisplayViewVideo.qml +++ b/src/FlightDisplay/FlightDisplayViewVideo.qml @@ -40,7 +40,7 @@ Item { id: noVideo anchors.fill: parent color: Qt.rgba(0,0,0,0.75) - visible: !(_videoReceiver && _videoReceiver.videoRunning) + visible: !(_videoReceiver && _videoReceiver.decoding) QGCLabel { text: QGroundControl.settingsManager.videoSettings.streamEnabled.rawValue ? qsTr("WAITING FOR VIDEO") : qsTr("VIDEO DISABLED") font.family: ScreenTools.demiboldFontFamily @@ -58,7 +58,7 @@ Item { Rectangle { anchors.fill: parent color: "black" - visible: _videoReceiver && _videoReceiver.videoRunning + visible: _videoReceiver && _videoReceiver.decoding function getWidth() { //-- Fit Width or Stretch if(_fitMode === 0 || _fitMode === 2) { @@ -86,7 +86,7 @@ Item { target: _videoReceiver onImageFileChanged: { videoContent.grabToImage(function(result) { - if (!result.saveToFile(_videoReceiver.imageFile)) { + if (!result.saveToFile(QGroundControl.videoManager.imageFile)) { console.error('Error capturing video frame'); } }); @@ -130,7 +130,7 @@ Item { height: parent.getHeight() width: parent.getWidth() anchors.centerIn: parent - visible: _videoReceiver && _videoReceiver.videoRunning + visible: _videoReceiver && _videoReceiver.decoding sourceComponent: videoBackgroundComponent property bool videoDisabled: QGroundControl.settingsManager.videoSettings.videoSource.rawValue === QGroundControl.settingsManager.videoSettings.disabledVideoSource diff --git a/src/FlightDisplay/PreFlightSensorsHealthCheck.qml b/src/FlightDisplay/PreFlightSensorsHealthCheck.qml index 1d04ad1bce0522a66b8ee70ea1808a5280c53a35..e5e4a2a37886890c4ed13b8b0cd9b32226b88533 100644 --- a/src/FlightDisplay/PreFlightSensorsHealthCheck.qml +++ b/src/FlightDisplay/PreFlightSensorsHealthCheck.qml @@ -17,7 +17,7 @@ PreFlightCheckButton { name: qsTr("Sensors") telemetryFailure: _unhealthySensors & _allCheckedSensors - property int _unhealthySensors: activeVehicle ? activeVehicle.sensorsUnhealthyBits : 0 + property int _unhealthySensors: activeVehicle ? activeVehicle.sensorsUnhealthyBits : 1 property int _allCheckedSensors: Vehicle.SysStatusSensor3dMag | Vehicle.SysStatusSensor3dAccel | Vehicle.SysStatusSensor3dGyro | diff --git a/src/FlightDisplay/VTOLChecklist.qml b/src/FlightDisplay/VTOLChecklist.qml index 40aa5ace41f5658bc6a7385709cb64d1c453f2cc..f0235e399cc6e353a2e5053dbc351c0da6c728e0 100644 --- a/src/FlightDisplay/VTOLChecklist.qml +++ b/src/FlightDisplay/VTOLChecklist.qml @@ -17,8 +17,6 @@ import QGroundControl.Controls 1.0 import QGroundControl.FlightDisplay 1.0 import QGroundControl.Vehicle 1.0 -import CustomQuickInterface 1.0 - Item { property var model: listModel PreFlightCheckModel { @@ -26,31 +24,6 @@ Item { PreFlightCheckGroup { name: qsTr("VTOL Initial Checks") - // Standard check list items (group 0) - Available from the start - Rectangle { - width: ScreenTools.defaultFontPixelWidth * 40 - height: testFlight.height + ScreenTools.defaultFontPixelHeight - color: qgcPal.button - property bool passed: true - function reset() { - if(activeVehicle) { - CustomQuickInterface.testFlight = false - activeVehicle.checkListState = Vehicle.CheckListNotSetup - } - } - QGCCheckBox { - id: testFlight - text: "Test Flight" - enabled: !CustomQuickInterface.debugBuild - checked: CustomQuickInterface.testFlight - anchors.centerIn: parent - onClicked: CustomQuickInterface.testFlight = checked - Component.onCompleted: { - CustomQuickInterface.testFlight = false - } - } - } - PreFlightCheckButton { name: qsTr("Hardware") manualText: qsTr("Props mounted? Wings secured? Tail secured?") diff --git a/src/FlightMap/Widgets/MapFitFunctions.qml b/src/FlightMap/Widgets/MapFitFunctions.qml index b8197994f988a8b5afd1be00bff0c015b17059b5..ac8bfa8cf88227304e3b294cbe60a4f154b80670 100644 --- a/src/FlightMap/Widgets/MapFitFunctions.qml +++ b/src/FlightMap/Widgets/MapFitFunctions.qml @@ -41,7 +41,7 @@ Item { /// Normalize longitude to range: 0 to 360, W to E function normalizeLon(lon) { - return lon + 180.0 + return lon + 180.0 } /// Fits the visible region of the map to inclues all of the specified coordinates. If no coordinates @@ -64,12 +64,12 @@ Item { for (var i = 1; i < coordList.length; i++) { var lat = coordList[i].latitude var lon = coordList[i].longitude - if (isNaN(lat) || lat == 0 || isNan(lon) || lon == 0) { + if (isNaN(lat) || lat == 0 || isNaN(lon) || lon == 0) { // Be careful of invalid coords which can happen when items are not yet complete continue } lat = normalizeLat(lat) - lon = normalizeLon(lat) + lon = normalizeLon(lon) north = Math.max(north, lat) south = Math.min(south, lat) east = Math.max(east, lon) @@ -77,8 +77,8 @@ Item { } // Expand the coordinate bounding rect to make room for the tools around the edge of the map - var latDegreesPerPixel = (north - south) / mapFitViewport.width - var lonDegreesPerPixel = (east - west) / mapFitViewport.height + var latDegreesPerPixel = (north - south) / mapFitViewport.height + var lonDegreesPerPixel = (east - west) / mapFitViewport.width north = Math.min(north + (mapFitViewport.y * latDegreesPerPixel), 180) south = Math.max(south - ((map.height - mapFitViewport.bottom) * latDegreesPerPixel), 0) west = Math.max(west - (mapFitViewport.x * lonDegreesPerPixel), 0) @@ -94,7 +94,6 @@ Item { var topLeftCoord = QtPositioning.coordinate(north - 90.0, west - 180.0) var bottomRightCoord = QtPositioning.coordinate(south - 90.0, east - 180.0) map.setVisibleRegion(QtPositioning.rectangle(topLeftCoord, bottomRightCoord)) - } function addMissionItemCoordsForFit(coordList) { diff --git a/src/FlightMap/Widgets/VideoPageWidget.qml b/src/FlightMap/Widgets/VideoPageWidget.qml index f835e896c1ad7296b074c485cf42a5bf02874bc1..651b27a092602b896f8508d98fe64f096503598c 100644 --- a/src/FlightMap/Widgets/VideoPageWidget.qml +++ b/src/FlightMap/Widgets/VideoPageWidget.qml @@ -33,7 +33,7 @@ Item { property bool _communicationLost: activeVehicle ? activeVehicle.connectionLost : false property var _videoReceiver: QGroundControl.videoManager.videoReceiver property bool _recordingVideo: _videoReceiver && _videoReceiver.recording - property bool _videoRunning: _videoReceiver && _videoReceiver.videoRunning + property bool _decodingVideo: _videoReceiver && _videoReceiver.decoding property bool _streamingEnabled: QGroundControl.settingsManager.videoSettings.streamConfigured property var _dynamicCameras: activeVehicle ? activeVehicle.dynamicCameras : null property int _curCameraIndex: _dynamicCameras ? _dynamicCameras.currentCamera : 0 @@ -136,7 +136,7 @@ Item { anchors.bottom: parent.bottom width: height radius: _recordingVideo ? 0 : height - color: (_videoRunning && _streamingEnabled) ? "red" : "gray" + color: (_decodingVideo && _streamingEnabled) ? "red" : "gray" SequentialAnimation on opacity { running: _recordingVideo loops: Animation.Infinite @@ -157,14 +157,14 @@ Item { } MouseArea { anchors.fill: parent - enabled: _videoRunning && _streamingEnabled + enabled: _decodingVideo && _streamingEnabled onClicked: { if (_recordingVideo) { - _videoReceiver.stopRecording() + QGroundControl.videoManager.stopRecording() // reset blinking animation recordBtnBackground.opacity = 1 } else { - _videoReceiver.startRecording(videoFileName.text) + QGroundControl.videoManager.startRecording(videoFileName.text) } } } diff --git a/src/MissionManager/CameraSectionTest.cc b/src/MissionManager/CameraSectionTest.cc index 6f9497a902b2f52b86e235766a21dfced1166394..f6dfb63493f978686f152b99dc00c2d5c380c490 100644 --- a/src/MissionManager/CameraSectionTest.cc +++ b/src/MissionManager/CameraSectionTest.cc @@ -592,7 +592,7 @@ void CameraSectionTest::_testScanForGimbalSection(void) // Check for a scan success - SimpleMissionItem* newValidGimbalItem = new SimpleMissionItem(_masterController, false /* flyView */, this); + SimpleMissionItem* newValidGimbalItem = new SimpleMissionItem(_masterController, false /* flyView */, false /* forLoad */, this); newValidGimbalItem->missionItem() = _validGimbalItem->missionItem(); visualItems.append(newValidGimbalItem); scanIndex = 0; @@ -676,7 +676,7 @@ void CameraSectionTest::_testScanForCameraModeSection(void) // Check for a scan success - SimpleMissionItem* newValidCameraModeItem = new SimpleMissionItem(_masterController, false /* flyView */, this); + SimpleMissionItem* newValidCameraModeItem = new SimpleMissionItem(_masterController, false /* flyView */, false /* forLoad */, this); newValidCameraModeItem->missionItem() = _validCameraPhotoModeItem->missionItem(); visualItems.append(newValidCameraModeItem); scanIndex = 0; @@ -735,7 +735,7 @@ void CameraSectionTest::_testScanForPhotoIntervalTimeSection(void) Mission Param #3 Number of images to capture total - 0 for unlimited capture */ - SimpleMissionItem* newValidTimeItem = new SimpleMissionItem(_masterController, false /* flyView */, this); + SimpleMissionItem* newValidTimeItem = new SimpleMissionItem(_masterController, false /* flyView */, false /* forLoad */, this); newValidTimeItem->missionItem() = _validTimeItem->missionItem(); visualItems.append(newValidTimeItem); QCOMPARE(_cameraSection->scanForSection(&visualItems, scanIndex), true); @@ -776,7 +776,7 @@ void CameraSectionTest::_testScanForPhotoIntervalDistanceSection(void) Mission Param #7 Empty */ - SimpleMissionItem* newValidDistanceItem = new SimpleMissionItem(_masterController, false /* flyView */, this); + SimpleMissionItem* newValidDistanceItem = new SimpleMissionItem(_masterController, false /* flyView */, false /* forLoad */, this); newValidDistanceItem->missionItem() = _validDistanceItem->missionItem(); visualItems.append(newValidDistanceItem); QCOMPARE(_cameraSection->scanForSection(&visualItems, scanIndex), true); @@ -862,7 +862,7 @@ void CameraSectionTest::_testScanForStartVideoSection(void) Mission Param #3 Reserved */ - SimpleMissionItem* newValidStartVideoItem = new SimpleMissionItem(_masterController, false /* flyView */, this); + SimpleMissionItem* newValidStartVideoItem = new SimpleMissionItem(_masterController, false /* flyView */, false /* forLoad */, this); newValidStartVideoItem->missionItem() = _validStartVideoItem->missionItem(); visualItems.append(newValidStartVideoItem); QCOMPARE(_cameraSection->scanForSection(&visualItems, scanIndex), true); @@ -898,7 +898,7 @@ void CameraSectionTest::_testScanForStopVideoSection(void) Mission Param #1 Reserved (Set to 0) */ - SimpleMissionItem* newValidStopVideoItem = new SimpleMissionItem(_masterController, false /* flyView */, this); + SimpleMissionItem* newValidStopVideoItem = new SimpleMissionItem(_masterController, false /* flyView */, false /* forLoad */, this); newValidStopVideoItem->missionItem() = _validStopVideoItem->missionItem(); visualItems.append(newValidStopVideoItem); QCOMPARE(_cameraSection->scanForSection(&visualItems, scanIndex), true); @@ -936,8 +936,8 @@ void CameraSectionTest::_testScanForStopPhotoSection(void) _commonScanTest(_cameraSection); - SimpleMissionItem* newValidStopDistanceItem = new SimpleMissionItem(_masterController, false /* flyView */, this); - SimpleMissionItem* newValidStopTimeItem = new SimpleMissionItem(_masterController, false /* flyView */, this); + SimpleMissionItem* newValidStopDistanceItem = new SimpleMissionItem(_masterController, false /* flyView */, false /* forLoad */, this); + SimpleMissionItem* newValidStopTimeItem = new SimpleMissionItem(_masterController, false /* flyView */, false /* forLoad */, this); newValidStopDistanceItem->missionItem() = _validStopDistanceItem->missionItem(); newValidStopTimeItem->missionItem() = _validStopTimeItem->missionItem(); visualItems.append(newValidStopDistanceItem); @@ -950,8 +950,8 @@ void CameraSectionTest::_testScanForStopPhotoSection(void) // Out of order commands - SimpleMissionItem validStopDistanceItem(_masterController, false /* flyView */, nullptr); - SimpleMissionItem validStopTimeItem(_masterController, false /* flyView */, nullptr); + SimpleMissionItem validStopDistanceItem(_masterController, false /* flyView */, false /* forLoad */, nullptr); + SimpleMissionItem validStopTimeItem(_masterController, false /* flyView */, false /* forLoad */, nullptr); validStopDistanceItem.missionItem() = _validStopDistanceItem->missionItem(); validStopTimeItem.missionItem() = _validStopTimeItem->missionItem(); visualItems.append(&validStopTimeItem); @@ -979,7 +979,7 @@ void CameraSectionTest::_testScanForTakePhotoSection(void) Mission Param #4 0 Unused sequence id */ - SimpleMissionItem* newValidTakePhotoItem = new SimpleMissionItem(_masterController, false /* flyView */, this); + SimpleMissionItem* newValidTakePhotoItem = new SimpleMissionItem(_masterController, false /* flyView */, false /* forLoad */, this); newValidTakePhotoItem->missionItem() = _validTakePhotoItem->missionItem(); visualItems.append(newValidTakePhotoItem); QCOMPARE(_cameraSection->scanForSection(&visualItems, scanIndex), true); @@ -1051,9 +1051,9 @@ void CameraSectionTest::_testScanForMultipleItems(void) // Camera action followed by gimbal/mode for (SimpleMissionItem* actionItem: rgActionItems) { for (SimpleMissionItem* cameraItem: rgCameraItems) { - SimpleMissionItem* item1 = new SimpleMissionItem(_masterController, false /* flyView */, this); + SimpleMissionItem* item1 = new SimpleMissionItem(_masterController, false /* flyView */, false /* forLoad */, this); item1->missionItem() = actionItem->missionItem(); - SimpleMissionItem* item2 = new SimpleMissionItem(_masterController, false /* flyView */, this); + SimpleMissionItem* item2 = new SimpleMissionItem(_masterController, false /* flyView */, false /* forLoad */, this); item2->missionItem() = cameraItem->missionItem(); visualItems.append(item1); visualItems.append(item2); @@ -1072,9 +1072,9 @@ void CameraSectionTest::_testScanForMultipleItems(void) // Gimbal/Mode followed by camera action for (SimpleMissionItem* actionItem: rgCameraItems) { for (SimpleMissionItem* cameraItem: rgActionItems) { - SimpleMissionItem* item1 = new SimpleMissionItem(_masterController, false /* flyView */, this); + SimpleMissionItem* item1 = new SimpleMissionItem(_masterController, false /* flyView */, false /* forLoad */, this); item1->missionItem() = actionItem->missionItem(); - SimpleMissionItem* item2 = new SimpleMissionItem(_masterController, false /* flyView */, this); + SimpleMissionItem* item2 = new SimpleMissionItem(_masterController, false /* flyView */, false /* forLoad */, this); item2->missionItem() = cameraItem->missionItem(); visualItems.append(item1); visualItems.append(item2); diff --git a/src/MissionManager/CorridorScanComplexItem.cc b/src/MissionManager/CorridorScanComplexItem.cc index 4ad06458552afc2b3a55591995780bcc3abc51d5..091e2b2fad379abf7cdaf4ae394721088bb543a9 100644 --- a/src/MissionManager/CorridorScanComplexItem.cc +++ b/src/MissionManager/CorridorScanComplexItem.cc @@ -217,7 +217,7 @@ void CorridorScanComplexItem::_buildAndAppendMissionItems(QList& i firstOverallPoint = false; // Possibly add trigger start/stop to survey area entrance/exit - if (triggerCamera() && transectCoordInfo.coordType == TransectStyleComplexItem::CoordTypeSurveyEdge) { + if (triggerCamera() && (transectCoordInfo.coordType == TransectStyleComplexItem::CoordTypeSurveyEntry || transectCoordInfo.coordType == TransectStyleComplexItem::CoordTypeSurveyExit)) { if (transectEntry) { // Start of transect, always start triggering. We do this even if we are taking images everywhere. // This allows a restart of the mission in mid-air without losing images from the entire mission. @@ -370,9 +370,9 @@ void CorridorScanComplexItem::_rebuildTransectsPhase1(void) TransectStyleComplexItem::CoordInfo_t coordInfo = { transectCoords[j], CoordTypeInterior }; transect.append(coordInfo); } - TransectStyleComplexItem::CoordInfo_t coordInfo = { transectCoords.first(), CoordTypeSurveyEdge }; + TransectStyleComplexItem::CoordInfo_t coordInfo = { transectCoords.first(), CoordTypeSurveyEntry }; transect.prepend(coordInfo); - coordInfo = { transectCoords.last(), CoordTypeSurveyEdge }; + coordInfo = { transectCoords.last(), CoordTypeSurveyExit }; transect.append(coordInfo); // Extend the transect ends for turnaround diff --git a/src/MissionManager/FWLandingPatternTest.cc b/src/MissionManager/FWLandingPatternTest.cc index 4ff6f3aa66b2dc82a7a704a7410802df2bee8cbd..ee969f0ffa203b7ae78642cec4d57316932c56ca 100644 --- a/src/MissionManager/FWLandingPatternTest.cc +++ b/src/MissionManager/FWLandingPatternTest.cc @@ -89,7 +89,7 @@ void FWLandingPatternTest::_testAppendSectionItems(void) QmlObjectListModel* simpleItems = new QmlObjectListModel(this); for (MissionItem* item: rgMissionItems) { - SimpleMissionItem* simpleItem = new SimpleMissionItem(_masterController, false /* flyView */, simpleItems); + SimpleMissionItem* simpleItem = new SimpleMissionItem(_masterController, false /* flyView */, false /* forLoad */, simpleItems); simpleItem->missionItem() = *item; simpleItems->append(simpleItem); } @@ -110,7 +110,7 @@ void FWLandingPatternTest::_testAppendSectionItems(void) _fwItem->appendMissionItems(rgMissionItems, this); simpleItems = new QmlObjectListModel(this); for (MissionItem* item: rgMissionItems) { - SimpleMissionItem* simpleItem = new SimpleMissionItem(_masterController, false /* flyView */, simpleItems); + SimpleMissionItem* simpleItem = new SimpleMissionItem(_masterController, false /* flyView */, false /* forLoad */, simpleItems); simpleItem->missionItem() = *item; simpleItems->append(simpleItem); } diff --git a/src/MissionManager/MavCmdInfoCommon.json b/src/MissionManager/MavCmdInfoCommon.json index 8ada5854f4e59566837ec249d83d4a72e716d618..c84b524c3775ae05a2776c5862e268daab1be311 100644 --- a/src/MissionManager/MavCmdInfoCommon.json +++ b/src/MissionManager/MavCmdInfoCommon.json @@ -858,6 +858,20 @@ "default": 25, "units": "m", "decimalPlaces": 2 + }, + "param2": { + "label": "Shutter", + "default": 0, + "units": "msecs", + "decimalPlaces": 0 + }, + "param3": { + "label": "Trigger", + "default": 25, + "enumStrings": "No Trigger,Once Immediately", + "enumValues": "0,1", + "default": 0, + "decimalPlaces": 0 } }, { @@ -1063,6 +1077,22 @@ "enumValues": "3,4" } }, + { + "id": 4501, + "rawName": "MAV_CMD_CONDITION_GATE", + "friendlyName": "Condition Gate", + "description": "Delay mission state machine until gate has been reached.", + "specifiesCoordinate": true, + "friendlyEdit": true, + "category": "Conditionals", + "param2": { + "label": "Ignore Alt", + "enumStrings": "False,True", + "enumValues": "0,1", + "default": 1, + "decimalPlaces": 0 + } + }, { "id": 30001, "rawName": "MAV_CMD_PAYLOAD_PREPARE_DEPLOY", "friendlyName": "Payload prepare deploy" }, { "id": 30002, "rawName": "MAV_CMD_PAYLOAD_CONTROL_DEPLOY", "friendlyName": "Payload control deploy" } ] diff --git a/src/MissionManager/MissionController.cc b/src/MissionManager/MissionController.cc index 5ab4302f09c4c80a74505c3426f24b92a0243626..5809910349cc7e15f499b1ef17e2bb6e675d1190 100644 --- a/src/MissionManager/MissionController.cc +++ b/src/MissionManager/MissionController.cc @@ -31,6 +31,7 @@ #include "KML.h" #include "QGCCorePlugin.h" #include "TakeoffMissionItem.h" +#include "PlanViewSettings.h" #define UPDATE_TIMEOUT 5000 ///< How often we check for bounding box changes @@ -62,12 +63,15 @@ MissionController::MissionController(PlanMasterController* masterController, QOb , _controllerVehicle (masterController->controllerVehicle()) , _managerVehicle (masterController->managerVehicle()) , _missionManager (masterController->managerVehicle()->missionManager()) + , _planViewSettings (qgcApp()->toolbox()->settingsManager()->planViewSettings()) , _appSettings (qgcApp()->toolbox()->settingsManager()->appSettings()) { _resetMissionFlightStatus(); _updateTimer.setSingleShot(true); connect(&_updateTimer, &QTimer::timeout, this, &MissionController::_updateTimeout); + + connect(_planViewSettings->takeoffItemNotRequired(), &Fact::rawValueChanged, this, &MissionController::_takeoffItemNotRequiredChanged); } MissionController::~MissionController() @@ -337,7 +341,7 @@ int MissionController::_nextSequenceNumber(void) VisualMissionItem* MissionController::_insertSimpleMissionItemWorker(QGeoCoordinate coordinate, MAV_CMD command, int visualItemIndex, bool makeCurrentItem) { int sequenceNumber = _nextSequenceNumber(); - SimpleMissionItem * newItem = new SimpleMissionItem(_masterController, _flyView, this); + SimpleMissionItem * newItem = new SimpleMissionItem(_masterController, _flyView, false /* forLoad */, this); newItem->setSequenceNumber(sequenceNumber); newItem->setCoordinate(coordinate); newItem->setCommand(command); @@ -651,7 +655,7 @@ bool MissionController::_loadJsonMissionFileV1(const QJsonObject& json, QmlObjec MissionSettingsItem* settingsItem = _addMissionSettings(visualItems); if (json.contains(_jsonPlannedHomePositionKey)) { - SimpleMissionItem* item = new SimpleMissionItem(_masterController, _flyView, visualItems); + SimpleMissionItem* item = new SimpleMissionItem(_masterController, _flyView, true /* forLoad */, visualItems); if (item->load(json[_jsonPlannedHomePositionKey].toObject(), 0, errorString)) { settingsItem->setInitialHomePositionFromUser(item->coordinate()); item->deleteLater(); @@ -687,11 +691,11 @@ bool MissionController::_loadJsonMissionFileV1(const QJsonObject& json, QmlObjec } const QJsonObject itemObject = itemValue.toObject(); - SimpleMissionItem* item = new SimpleMissionItem(_masterController, _flyView, visualItems); + SimpleMissionItem* item = new SimpleMissionItem(_masterController, _flyView, true /* forLoad */, visualItems); if (item->load(itemObject, itemObject["id"].toInt(), errorString)) { if (TakeoffMissionItem::isTakeoffCommand(item->mavCommand())) { // This needs to be a TakeoffMissionItem - TakeoffMissionItem* takeoffItem = new TakeoffMissionItem(_masterController, _flyView, settingsItem, visualItems); + TakeoffMissionItem* takeoffItem = new TakeoffMissionItem(_masterController, _flyView, settingsItem, true /* forLoad */, visualItems); takeoffItem->load(itemObject, itemObject["id"].toInt(), errorString); item->deleteLater(); item = takeoffItem; @@ -775,11 +779,11 @@ bool MissionController::_loadJsonMissionFileV2(const QJsonObject& json, QmlObjec QString itemType = itemObject[VisualMissionItem::jsonTypeKey].toString(); if (itemType == VisualMissionItem::jsonTypeSimpleItemValue) { - SimpleMissionItem* simpleItem = new SimpleMissionItem(_masterController, _flyView, visualItems); + SimpleMissionItem* simpleItem = new SimpleMissionItem(_masterController, _flyView, true /* forLoad */, visualItems); if (simpleItem->load(itemObject, nextSequenceNumber, errorString)) { if (TakeoffMissionItem::isTakeoffCommand(static_cast(simpleItem->command()))) { // This needs to be a TakeoffMissionItem - TakeoffMissionItem* takeoffItem = new TakeoffMissionItem(_masterController, _flyView, settingsItem, this); + TakeoffMissionItem* takeoffItem = new TakeoffMissionItem(_masterController, _flyView, settingsItem, true /* forLoad */, this); takeoffItem->load(itemObject, nextSequenceNumber, errorString); simpleItem->deleteLater(); simpleItem = takeoffItem; @@ -919,14 +923,14 @@ bool MissionController::_loadTextMissionFile(QTextStream& stream, QmlObjectListM MissionSettingsItem* settingsItem = _addMissionSettings(visualItems); while (!stream.atEnd()) { - SimpleMissionItem* item = new SimpleMissionItem(_masterController, _flyView, visualItems); + SimpleMissionItem* item = new SimpleMissionItem(_masterController, _flyView, true /* forLoad */, visualItems); if (item->load(stream)) { if (firstItem && plannedHomePositionInFile) { settingsItem->setInitialHomePositionFromUser(item->coordinate()); } else { if (TakeoffMissionItem::isTakeoffCommand(static_cast(item->command()))) { // This needs to be a TakeoffMissionItem - TakeoffMissionItem* takeoffItem = new TakeoffMissionItem(_masterController, _flyView, settingsItem, visualItems); + TakeoffMissionItem* takeoffItem = new TakeoffMissionItem(_masterController, _flyView, settingsItem, true /* forLoad */, visualItems); takeoffItem->load(stream); item->deleteLater(); item = takeoffItem; @@ -2283,6 +2287,7 @@ void MissionController::setCurrentPlanViewSeqNum(int sequenceNumber, bool force) _currentPlanViewItem = nullptr; _currentPlanViewSeqNum = -1; _currentPlanViewVIIndex = -1; + _onlyInsertTakeoffValid = !_planViewSettings->takeoffItemNotRequired()->rawValue().toBool() && _visualItems->count() == 1; // First item must be takeoff _isInsertTakeoffValid = true; _isInsertLandValid = true; _isROIActive = false; @@ -2412,10 +2417,15 @@ void MissionController::setCurrentPlanViewSeqNum(int sequenceNumber, bool force) } } + // These are not valid when only takeoff is allowed + _isInsertLandValid = _isInsertLandValid && !_onlyInsertTakeoffValid; + _flyThroughCommandsAllowed = _flyThroughCommandsAllowed && !_onlyInsertTakeoffValid; + emit currentPlanViewSeqNumChanged(); emit currentPlanViewVIIndexChanged(); emit currentPlanViewItemChanged(); emit splitSegmentChanged(); + emit onlyInsertTakeoffValidChanged(); emit isInsertTakeoffValidChanged(); emit isInsertLandValidChanged(); emit isROIActiveChanged(); @@ -2513,3 +2523,9 @@ bool MissionController::isEmpty(void) const { return _visualItems->count() <= 1; } + +void MissionController::_takeoffItemNotRequiredChanged(void) +{ + // Force a recalc of allowed bits + setCurrentPlanViewSeqNum(_currentPlanViewSeqNum, true /* force */); +} diff --git a/src/MissionManager/MissionController.h b/src/MissionManager/MissionController.h index dc128f188a6fdad8a5a4dcf0df09da21be2e3231..2274408d6f3c9f86e38e9a09acc1a7625f5bd0c2 100644 --- a/src/MissionManager/MissionController.h +++ b/src/MissionManager/MissionController.h @@ -28,6 +28,7 @@ class SimpleMissionItem; class ComplexMissionItem; class MissionSettingsItem; class QDomDocument; +class PlanViewSettings; Q_DECLARE_LOGGING_CATEGORY(MissionControllerLog) @@ -95,6 +96,7 @@ public: Q_PROPERTY(QString surveyComplexItemName READ surveyComplexItemName CONSTANT) Q_PROPERTY(QString corridorScanComplexItemName READ corridorScanComplexItemName CONSTANT) Q_PROPERTY(QString structureScanComplexItemName READ structureScanComplexItemName CONSTANT) + Q_PROPERTY(bool onlyInsertTakeoffValid MEMBER _onlyInsertTakeoffValid NOTIFY onlyInsertTakeoffValidChanged) Q_PROPERTY(bool isInsertTakeoffValid MEMBER _isInsertTakeoffValid NOTIFY isInsertTakeoffValidChanged) Q_PROPERTY(bool isInsertLandValid MEMBER _isInsertLandValid NOTIFY isInsertLandValidChanged) Q_PROPERTY(bool isROIActive MEMBER _isROIActive NOTIFY isROIActiveChanged) @@ -263,6 +265,7 @@ signals: void currentPlanViewItemChanged (void); void missionBoundingCubeChanged (void); void missionItemCountChanged (int missionItemCount); + void onlyInsertTakeoffValidChanged (void); void isInsertTakeoffValidChanged (void); void isInsertLandValidChanged (void); void isROIActiveChanged (void); @@ -286,6 +289,7 @@ private slots: void _complexBoundingBoxChanged (void); void _recalcAll (void); void _managerVehicleChanged (Vehicle* managerVehicle); + void _takeoffItemNotRequiredChanged (void); private: void _init(void); @@ -334,6 +338,7 @@ private: int _missionItemCount = 0; QmlObjectListModel* _visualItems = nullptr; MissionSettingsItem* _settingsItem = nullptr; + PlanViewSettings* _planViewSettings = nullptr; QmlObjectListModel _waypointLines; QVariantList _waypointPath; QmlObjectListModel _directionArrows; @@ -353,10 +358,11 @@ private: QGeoCoordinate _takeoffCoordinate; QGeoCoordinate _previousCoordinate; CoordinateVector* _splitSegment = nullptr; + bool _onlyInsertTakeoffValid = true; bool _isInsertTakeoffValid = true; - bool _isInsertLandValid = true; + bool _isInsertLandValid = false; bool _isROIActive = false; - bool _flyThroughCommandsAllowed = true; + bool _flyThroughCommandsAllowed = false; bool _isROIBeginCurrentItem = false; static const char* _settingsGroup; diff --git a/src/MissionManager/MissionItemTest.cc b/src/MissionManager/MissionItemTest.cc index 33946aef87e9dc3bd43a830493a9cb6cee944f4a..e2df716a21666becdf2a51b15481984dd8d41e91 100644 --- a/src/MissionManager/MissionItemTest.cc +++ b/src/MissionManager/MissionItemTest.cc @@ -278,7 +278,7 @@ void MissionItemTest::_testSimpleLoadFromStream(void) { // We specifically test SimpleMissionItem loading as well since it has additional // signalling which can affect values. - SimpleMissionItem simpleMissionItem(_masterController, false /* flyView */, nullptr); + SimpleMissionItem simpleMissionItem(_masterController, false /* flyView */, false /* forLoad */, nullptr); QString testString("10\t0\t3\t80\t10\t20\t30\t40\t-10\t-20\t-30\t1\r\n"); QTextStream testStream(&testString, QIODevice::ReadOnly); @@ -448,7 +448,7 @@ void MissionItemTest::_testSimpleLoadFromJson(void) // We specifically test SimpleMissionItem loading as well since it has additional // signalling which can affect values. - SimpleMissionItem simpleMissionItem(_masterController, false /* flyView */, nullptr); + SimpleMissionItem simpleMissionItem(_masterController, false /* flyView */, false /* forLoad */, nullptr); QString errorString; QJsonArray coordinateArray; QJsonObject jsonObject; diff --git a/src/MissionManager/MissionManagerTest.cc b/src/MissionManager/MissionManagerTest.cc index fefdaa87f21b5ed3967444965f328feca22878a9..01c1f606da1bad14ecc07b951f4961da205a5c08 100644 --- a/src/MissionManager/MissionManagerTest.cc +++ b/src/MissionManager/MissionManagerTest.cc @@ -27,9 +27,9 @@ MissionManagerTest::MissionManagerTest(void) } -void MissionManagerTest::_writeItems(MockLinkMissionItemHandler::FailureMode_t failureMode, bool shouldFail) +void MissionManagerTest::_writeItems(MockLinkMissionItemHandler::FailureMode_t failureMode, MAV_MISSION_RESULT failureAckResult, bool shouldFail) { - _mockLink->setMissionItemFailureMode(failureMode); + _mockLink->setMissionItemFailureMode(failureMode, failureAckResult); // Setup our test case data QList missionItems; @@ -115,11 +115,11 @@ void MissionManagerTest::_writeItems(MockLinkMissionItemHandler::FailureMode_t f _multiSpyMissionManager->clearAllSignals(); } -void MissionManagerTest::_roundTripItems(MockLinkMissionItemHandler::FailureMode_t failureMode, bool shouldFail) +void MissionManagerTest::_roundTripItems(MockLinkMissionItemHandler::FailureMode_t failureMode, MAV_MISSION_RESULT failureAckResult, bool shouldFail) { - _writeItems(MockLinkMissionItemHandler::FailNone, false); + _writeItems(MockLinkMissionItemHandler::FailNone, failureAckResult, false); - _mockLink->setMissionItemFailureMode(failureMode); + _mockLink->setMissionItemFailureMode(failureMode, failureAckResult); // Read the items back from the vehicle _missionManager->loadFromVehicle(); @@ -253,8 +253,8 @@ void MissionManagerTest::_testWriteFailureHandlingWorker(void) for (size_t i=0; ifailureText; - _writeItems(pCase->failureMode, pCase->shouldFail); + qDebug() << "TEST CASE _testWriteFailureHandlingWorker" << pCase->failureText; + _writeItems(pCase->failureMode, MAV_MISSION_ERROR, pCase->shouldFail); _mockLink->resetMissionItemHandler(); } } @@ -298,8 +298,8 @@ void MissionManagerTest::_testReadFailureHandlingWorker(void) for (size_t i=0; ifailureText; - _roundTripItems(pCase->failureMode, pCase->shouldFail); + qDebug() << "TEST CASE _testReadFailureHandlingWorker" << pCase->failureText; + _roundTripItems(pCase->failureMode, MAV_MISSION_ERROR, pCase->shouldFail); _mockLink->resetMissionItemHandler(); _multiSpyMissionManager->clearAllSignals(); } @@ -329,3 +329,34 @@ void MissionManagerTest::_testReadFailureHandlingPX4(void) _initForFirmwareType(MAV_AUTOPILOT_PX4); _testReadFailureHandlingWorker(); } + +void MissionManagerTest::_testErrorAckFailureStrings(void) +{ + _initForFirmwareType(MAV_AUTOPILOT_PX4); + + typedef struct { + const char* ackResultStr; + MAV_MISSION_RESULT ackResult; + } ErrorStringTestCase_t; + + static const ErrorStringTestCase_t rgTestCases[] = { + { "MAV_MISSION_UNSUPPORTED_FRAME", MAV_MISSION_UNSUPPORTED_FRAME }, + { "MAV_MISSION_UNSUPPORTED", MAV_MISSION_UNSUPPORTED }, + { "MAV_MISSION_INVALID_PARAM1", MAV_MISSION_INVALID_PARAM1 }, + { "MAV_MISSION_INVALID_PARAM2", MAV_MISSION_INVALID_PARAM2 }, + { "MAV_MISSION_INVALID_PARAM3", MAV_MISSION_INVALID_PARAM3 }, + { "MAV_MISSION_INVALID_PARAM4", MAV_MISSION_INVALID_PARAM4 }, + { "MAV_MISSION_INVALID_PARAM5_X", MAV_MISSION_INVALID_PARAM5_X }, + { "MAV_MISSION_INVALID_PARAM6_Y", MAV_MISSION_INVALID_PARAM6_Y }, + { "MAV_MISSION_INVALID_PARAM7", MAV_MISSION_INVALID_PARAM7 }, + { "MAV_MISSION_INVALID_SEQUENCE", MAV_MISSION_INVALID_SEQUENCE }, + }; + + for (size_t i=0; iackResultStr; + _writeItems(MockLinkMissionItemHandler::FailWriteRequest1ErrorAck, pCase->ackResult, true /* shouldFail */); + _mockLink->resetMissionItemHandler(); + } + +} diff --git a/src/MissionManager/MissionManagerTest.h b/src/MissionManager/MissionManagerTest.h index 7d3b4d5926077e7aa302c522b5c9a6c1db858dae..0d3d893f5cfb75fd60859b71574f57d9f36772d9 100644 --- a/src/MissionManager/MissionManagerTest.h +++ b/src/MissionManager/MissionManagerTest.h @@ -31,10 +31,11 @@ private slots: void _testWriteFailureHandlingAPM(void); void _testReadFailureHandlingPX4(void); void _testReadFailureHandlingAPM(void); + void _testErrorAckFailureStrings(void); private: - void _roundTripItems(MockLinkMissionItemHandler::FailureMode_t failureMode, bool shouldFail); - void _writeItems(MockLinkMissionItemHandler::FailureMode_t failureMode, bool shouldFail); + void _roundTripItems(MockLinkMissionItemHandler::FailureMode_t failureMode, MAV_MISSION_RESULT failureAckResult, bool shouldFail); + void _writeItems(MockLinkMissionItemHandler::FailureMode_t failureMode, MAV_MISSION_RESULT failureAckResult, bool shouldFail); void _testWriteFailureHandlingWorker(void); void _testReadFailureHandlingWorker(void); diff --git a/src/MissionManager/PlanManager.cc b/src/MissionManager/PlanManager.cc index 8231f90ae32596d91c0bce62fb294082a910133d..3ab585b6f5b4a3b37de152490eef289507c572c9 100644 --- a/src/MissionManager/PlanManager.cc +++ b/src/MissionManager/PlanManager.cc @@ -23,6 +23,7 @@ QGC_LOGGING_CATEGORY(PlanManagerLog, "PlanManagerLog") PlanManager::PlanManager(Vehicle* vehicle, MAV_MISSION_TYPE planType) : _vehicle (vehicle) + , _missionCommandTree (qgcApp()->toolbox()->missionCommandTree()) , _planType (planType) , _dedicatedLink (nullptr) , _ackTimeoutTimer (nullptr) @@ -178,7 +179,7 @@ void PlanManager::_ackTimeout(void) case AckMissionCount: // MISSION_COUNT message expected if (_retryCount > _maxRetryCount) { - _sendError(VehicleError, tr("Mission request list failed, maximum retries exceeded.")); + _sendError(MaxRetryExceeded, tr("Mission request list failed, maximum retries exceeded.")); _finishTransaction(false); } else { _retryCount++; @@ -189,7 +190,7 @@ void PlanManager::_ackTimeout(void) case AckMissionItem: // MISSION_ITEM expected if (_retryCount > _maxRetryCount) { - _sendError(VehicleError, tr("Mission read failed, maximum retries exceeded.")); + _sendError(MaxRetryExceeded, tr("Mission read failed, maximum retries exceeded.")); _finishTransaction(false); } else { _retryCount++; @@ -201,12 +202,12 @@ void PlanManager::_ackTimeout(void) // MISSION_REQUEST is expected, or MISSION_ACK to end sequence if (_itemIndicesToWrite.count() == 0) { // Vehicle did not send final MISSION_ACK at end of sequence - _sendError(VehicleError, tr("Mission write failed, vehicle failed to send final ack.")); + _sendError(ProtocolError, tr("Mission write failed, vehicle failed to send final ack.")); _finishTransaction(false); } else if (_itemIndicesToWrite[0] == 0) { // Vehicle did not respond to MISSION_COUNT, try again if (_retryCount > _maxRetryCount) { - _sendError(VehicleError, tr("Mission write mission count failed, maximum retries exceeded.")); + _sendError(MaxRetryExceeded, tr("Mission write mission count failed, maximum retries exceeded.")); _finishTransaction(false); } else { _retryCount++; @@ -215,7 +216,7 @@ void PlanManager::_ackTimeout(void) } } else { // Vehicle did not request all items from ground station - _sendError(AckTimeoutError, tr("Vehicle did not request all items from ground station: %1").arg(_ackTypeToString(_expectedAck))); + _sendError(ProtocolError, tr("Vehicle did not request all items from ground station: %1").arg(_ackTypeToString(_expectedAck))); _expectedAck = AckNone; _finishTransaction(false); } @@ -223,7 +224,7 @@ void PlanManager::_ackTimeout(void) case AckMissionClearAll: // MISSION_ACK expected if (_retryCount > _maxRetryCount) { - _sendError(VehicleError, tr("Mission remove all, maximum retries exceeded.")); + _sendError(MaxRetryExceeded, tr("Mission remove all, maximum retries exceeded.")); _finishTransaction(false); } else { _retryCount++; @@ -341,7 +342,7 @@ void PlanManager::_handleMissionCount(const mavlink_message_t& message) void PlanManager::_requestNextMissionItem(void) { if (_itemIndicesToRead.count() == 0) { - _sendError(InternalError, "Internal Error: Call to Vehicle _requestNextMissionItem with no more indices to read"); + _sendError(InternalError, tr("Internal Error: Call to Vehicle _requestNextMissionItem with no more indices to read")); return; } @@ -623,47 +624,51 @@ void PlanManager::_handleMissionAck(const mavlink_message_t& message) switch (savedExpectedAck) { case AckNone: // State machine is idle. Vehicle is confused. - qCDebug(PlanManagerLog) << QStringLiteral("Vehicle sent unexpected MISSION_ACK message, error: %1").arg(_missionResultToString((MAV_MISSION_RESULT)missionAck.type)); + qCDebug(PlanManagerLog) << QStringLiteral("Vehicle sent unexpected MISSION_ACK message, error: %1").arg(_missionResultToString((MAV_MISSION_RESULT)missionAck.type)); break; case AckMissionCount: // MISSION_COUNT message expected - _sendError(VehicleError, tr("Vehicle returned error: %1.").arg(_missionResultToString((MAV_MISSION_RESULT)missionAck.type))); + // FIXME: Protocol error + _sendError(VehicleAckError, _missionResultToString((MAV_MISSION_RESULT)missionAck.type)); _finishTransaction(false); break; case AckMissionItem: // MISSION_ITEM expected - _sendError(VehicleError, tr("Vehicle returned error: %1.").arg(_missionResultToString((MAV_MISSION_RESULT)missionAck.type))); + // FIXME: Protocol error + _sendError(VehicleAckError, _missionResultToString((MAV_MISSION_RESULT)missionAck.type)); _finishTransaction(false); break; case AckMissionRequest: - // MISSION_REQUEST is expected, or MISSION_ACK to end sequence + // MISSION_REQUEST is expected, or MAV_MISSION_ACCEPTED to end sequence if (missionAck.type == MAV_MISSION_ACCEPTED) { if (_itemIndicesToWrite.count() == 0) { qCDebug(PlanManagerLog) << QStringLiteral("_handleMissionAck write sequence complete %1").arg(_planTypeString()); _finishTransaction(true); } else { - _sendError(MissingRequestsError, tr("Vehicle did not request all items during write sequence, missed count %1.").arg(_itemIndicesToWrite.count())); + // FIXME: Protocol error + _sendError(VehicleAckError, _missionResultToString((MAV_MISSION_RESULT)missionAck.type)); _finishTransaction(false); } } else { - _sendError(VehicleError, tr("Vehicle returned error: %1.").arg(_missionResultToString((MAV_MISSION_RESULT)missionAck.type))); + _sendError(VehicleAckError, _missionResultToString((MAV_MISSION_RESULT)missionAck.type)); _finishTransaction(false); } break; case AckMissionClearAll: - // MISSION_ACK expected + // MAV_MISSION_ACCEPTED expected if (missionAck.type != MAV_MISSION_ACCEPTED) { - _sendError(VehicleError, tr("Vehicle returned error: %1. Vehicle remove all failed.").arg(_missionResultToString((MAV_MISSION_RESULT)missionAck.type))); + _sendError(VehicleAckError, tr("Vehicle remove all failed. Error: %1").arg(_missionResultToString((MAV_MISSION_RESULT)missionAck.type))); } _finishTransaction(missionAck.type == MAV_MISSION_ACCEPTED); break; case AckGuidedItem: - // MISSION_REQUEST is expected, or MISSION_ACK to end sequence + // MISSION_REQUEST is expected, or MAV_MISSION_ACCEPTED to end sequence if (missionAck.type == MAV_MISSION_ACCEPTED) { qCDebug(PlanManagerLog) << QStringLiteral("_handleMissionAck %1 guided mode item accepted").arg(_planTypeString()); _finishTransaction(true, true /* apmGuidedItemWrite */); } else { - _sendError(VehicleError, tr("Vehicle returned error: %1. %2Vehicle did not accept guided item.").arg(_missionResultToString((MAV_MISSION_RESULT)missionAck.type))); + // FIXME: Protocol error + _sendError(VehicleAckError, tr("Vehicle returned error: %1. %2Vehicle did not accept guided item.").arg(_missionResultToString((MAV_MISSION_RESULT)missionAck.type))); _finishTransaction(false, true /* apmGuidedItemWrite */); } break; @@ -702,7 +707,7 @@ void PlanManager::_mavlinkMessageReceived(const mavlink_message_t& message) void PlanManager::_sendError(ErrorCode_t errorCode, const QString& errorMsg) { - qCDebug(PlanManagerLog) << QStringLiteral("Sending %1 error").arg(_planTypeString()) << errorCode << errorMsg; + qCDebug(PlanManagerLog) << QStringLiteral("Sending error - _planTypeString(%1) errorCode(%2) errorMsg(%4)").arg(_planTypeString()).arg(errorCode).arg(errorMsg); emit error(errorCode, errorMsg); } @@ -728,104 +733,115 @@ QString PlanManager::_ackTypeToString(AckType_t ackType) QString PlanManager::_lastMissionReqestString(MAV_MISSION_RESULT result) { + QString prefix; + QString postfix; + if (_lastMissionRequest >= 0 && _lastMissionRequest < _writeMissionItems.count()) { MissionItem* item = _writeMissionItems[_lastMissionRequest]; + prefix = tr("Item #%1 Command: %2").arg(_lastMissionRequest).arg(_missionCommandTree->friendlyName(item->command())); + switch (result) { case MAV_MISSION_UNSUPPORTED_FRAME: - return QString(". Frame: %1").arg(item->frame()); + postfix = tr("Frame: %1").arg(item->frame()); + break; case MAV_MISSION_UNSUPPORTED: - { - const MissionCommandUIInfo* uiInfo = qgcApp()->toolbox()->missionCommandTree()->getUIInfo(_vehicle, item->command()); - QString friendlyName; - QString rawName; - if (uiInfo) { - friendlyName = uiInfo->friendlyName(); - rawName = uiInfo->rawName(); - } - return QString(". Command: (%1, %2, %3)").arg(friendlyName).arg(rawName).arg(item->command()); - } + // All we need is the prefix + break; case MAV_MISSION_INVALID_PARAM1: - return QString(". Param1: %1").arg(item->param1()); + postfix = tr("Value: %1").arg(item->param1()); + break; case MAV_MISSION_INVALID_PARAM2: - return QString(". Param2: %1").arg(item->param2()); + postfix = tr("Value: %1").arg(item->param2()); + break; case MAV_MISSION_INVALID_PARAM3: - return QString(". Param3: %1").arg(item->param3()); + postfix = tr("Value: %1").arg(item->param3()); + break; case MAV_MISSION_INVALID_PARAM4: - return QString(". Param4: %1").arg(item->param4()); + postfix = tr("Value: %1").arg(item->param4()); + break; case MAV_MISSION_INVALID_PARAM5_X: - return QString(". Param5: %1").arg(item->param5()); + postfix = tr("Value: %1").arg(item->param5()); + break; case MAV_MISSION_INVALID_PARAM6_Y: - return QString(". Param6: %1").arg(item->param6()); + postfix = tr("Value: %1").arg(item->param6()); + break; case MAV_MISSION_INVALID_PARAM7: - return QString(". Param7: %1").arg(item->param7()); + postfix = tr("Value: %1").arg(item->param7()); + break; case MAV_MISSION_INVALID_SEQUENCE: - return QString(". Sequence: %1").arg(item->sequenceNumber()); + // All we need is the prefix + break; default: break; } } - return QString(); + return prefix + (postfix.isEmpty() ? QStringLiteral("") : QStringLiteral(" ")) + postfix; } QString PlanManager::_missionResultToString(MAV_MISSION_RESULT result) { - QString resultString; - QString lastRequestString = _lastMissionReqestString(result); + QString error; switch (result) { case MAV_MISSION_ACCEPTED: - resultString = tr("Mission accepted (MAV_MISSION_ACCEPTED)"); + error = tr("Mission accepted."); break; case MAV_MISSION_ERROR: - resultString = tr("Unspecified error (MAV_MISSION_ERROR)"); + error = tr("Unspecified error."); break; case MAV_MISSION_UNSUPPORTED_FRAME: - resultString = tr("Coordinate frame is not supported (MAV_MISSION_UNSUPPORTED_FRAME)"); + error = tr("Coordinate frame is not supported."); break; case MAV_MISSION_UNSUPPORTED: - resultString = tr("Command is not supported (MAV_MISSION_UNSUPPORTED)"); + error = tr("Command is not supported."); break; case MAV_MISSION_NO_SPACE: - resultString = tr("Mission item exceeds storage space (MAV_MISSION_NO_SPACE)"); + error = tr("Mission item exceeds storage space."); break; case MAV_MISSION_INVALID: - resultString = tr("One of the parameters has an invalid value (MAV_MISSION_INVALID)"); + error = tr("One of the parameters has an invalid value."); break; case MAV_MISSION_INVALID_PARAM1: - resultString = tr("Param1 has an invalid value (MAV_MISSION_INVALID_PARAM1)"); + error = tr("Param 1 invalid value."); break; case MAV_MISSION_INVALID_PARAM2: - resultString = tr("Param2 has an invalid value (MAV_MISSION_INVALID_PARAM2)"); + error = tr("Param 2 invalid value."); break; case MAV_MISSION_INVALID_PARAM3: - resultString = tr("Param3 has an invalid value (MAV_MISSION_INVALID_PARAM3)"); + error = tr("Param 3 invalid value."); break; case MAV_MISSION_INVALID_PARAM4: - resultString = tr("Param4 has an invalid value (MAV_MISSION_INVALID_PARAM4)"); + error = tr("Param 4 invalid value."); break; case MAV_MISSION_INVALID_PARAM5_X: - resultString = tr("X/Param5 has an invalid value (MAV_MISSION_INVALID_PARAM5_X)"); + error = tr("Param 5 invalid value."); break; case MAV_MISSION_INVALID_PARAM6_Y: - resultString = tr("Y/Param6 has an invalid value (MAV_MISSION_INVALID_PARAM6_Y)"); + error = tr("Param 6 invalid value."); break; case MAV_MISSION_INVALID_PARAM7: - resultString = tr("Param7 has an invalid value (MAV_MISSION_INVALID_PARAM7)"); + error = tr("Param 7 invalid value."); break; case MAV_MISSION_INVALID_SEQUENCE: - resultString = tr("Received mission item out of sequence (MAV_MISSION_INVALID_SEQUENCE)"); + error = tr("Received mission item out of sequence."); break; case MAV_MISSION_DENIED: - resultString = tr("Not accepting any mission commands (MAV_MISSION_DENIED)"); + error = tr("Not accepting any mission commands."); break; default: - qWarning(PlanManagerLog) << QStringLiteral("Fell off end of switch statement %1").arg(_planTypeString()); - resultString = tr("QGC Internal Error"); + qWarning(PlanManagerLog) << QStringLiteral("Fell off end of switch statement %1 %2").arg(_planTypeString()).arg(result); + error = tr("Unknown error: %1.").arg(result); + break; + } + + QString lastRequestString = _lastMissionReqestString(result); + if (!lastRequestString.isEmpty()) { + error += QStringLiteral(" ") + lastRequestString; } - return resultString + lastRequestString; + return error; } void PlanManager::_finishTransaction(bool success, bool apmGuidedItemWrite) diff --git a/src/MissionManager/PlanManager.h b/src/MissionManager/PlanManager.h index 88fabe9357b61cdcb5eaf4b4bf9f7e9fad64ce21..49144887f9e34d0435726e98deeac85002ef5a13 100644 --- a/src/MissionManager/PlanManager.h +++ b/src/MissionManager/PlanManager.h @@ -20,6 +20,7 @@ #include "LinkInterface.h" class Vehicle; +class MissionCommandTree; Q_DECLARE_LOGGING_CATEGORY(PlanManagerLog) @@ -60,10 +61,10 @@ public: typedef enum { InternalError, AckTimeoutError, ///< Timed out waiting for response from vehicle - ProtocolOrderError, ///< Incorrect protocol sequence from vehicle + ProtocolError, ///< Incorrect protocol sequence from vehicle RequestRangeError, ///< Vehicle requested item out of range ItemMismatchError, ///< Vehicle returned item with seq # different than requested - VehicleError, ///< Vehicle returned error + VehicleAckError, ///< Vehicle returned error in ack MissingRequestsError, ///< Vehicle did not request all items during write sequence MaxRetryExceeded, ///< Retry failed MissionTypeMismatch, ///< MAV_MISSION_TYPE does not match _planType @@ -134,11 +135,12 @@ protected: QString _planTypeString(void); protected: - Vehicle* _vehicle; + Vehicle* _vehicle = nullptr; + MissionCommandTree* _missionCommandTree = nullptr; MAV_MISSION_TYPE _planType; - LinkInterface* _dedicatedLink; + LinkInterface* _dedicatedLink = nullptr; - QTimer* _ackTimeoutTimer; + QTimer* _ackTimeoutTimer = nullptr; AckType_t _expectedAck; int _retryCount; diff --git a/src/MissionManager/SimpleMissionItem.cc b/src/MissionManager/SimpleMissionItem.cc index 16fd42259eb55eae535e578991a3ed1641d5fc67..b4308bdae2c143d8d1596a1ee905733220b0805b 100644 --- a/src/MissionManager/SimpleMissionItem.cc +++ b/src/MissionManager/SimpleMissionItem.cc @@ -52,7 +52,7 @@ static const struct EnumInfo_s _rgMavFrameInfo[] = { { "MAV_FRAME_GLOBAL_TERRAIN_ALT_INT", MAV_FRAME_GLOBAL_TERRAIN_ALT_INT }, }; -SimpleMissionItem::SimpleMissionItem(PlanMasterController* masterController, bool flyView, QObject* parent) +SimpleMissionItem::SimpleMissionItem(PlanMasterController* masterController, bool flyView, bool forLoad, QObject* parent) : VisualMissionItem (masterController, flyView, parent) , _commandTree (qgcApp()->toolbox()->missionCommandTree()) , _supportedCommandFact (0, "Command:", FactMetaData::valueTypeUint32) @@ -69,13 +69,15 @@ SimpleMissionItem::SimpleMissionItem(PlanMasterController* masterController, boo _editorQml = QStringLiteral("qrc:/qml/SimpleItemEditor.qml"); _setupMetaData(); - _connectSignals(); - _updateOptionalSections(); - - _setDefaultsForCommand(); - _rebuildFacts(); - setDirty(false); + if (!forLoad) { + // We are are going to load the SimpleItem right after this then don't connnect up signalling until after load is done + _connectSignals(); + _updateOptionalSections(); + _setDefaultsForCommand(); + _rebuildFacts(); + setDirty(false); + } } SimpleMissionItem::SimpleMissionItem(PlanMasterController* masterController, bool flyView, const MissionItem& missionItem, QObject* parent) @@ -280,7 +282,10 @@ bool SimpleMissionItem::load(QTextStream &loadStream) _altitudeFact.setRawValue(_missionItem._param7Fact.rawValue()); _amslAltAboveTerrainFact.setRawValue(qQNaN()); } + _connectSignals(); _updateOptionalSections(); + _rebuildFacts(); + setDirty(false); } return success; @@ -313,7 +318,10 @@ bool SimpleMissionItem::load(const QJsonObject& json, int sequenceNumber, QStrin } } + _connectSignals(); _updateOptionalSections(); + _rebuildFacts(); + setDirty(false); return true; } diff --git a/src/MissionManager/SimpleMissionItem.h b/src/MissionManager/SimpleMissionItem.h index 3ca54977d9fe674c71d3466b10c95df76c6ccc7b..491e0d35b832f904872d03a8bda4f2a2f61274be 100644 --- a/src/MissionManager/SimpleMissionItem.h +++ b/src/MissionManager/SimpleMissionItem.h @@ -24,9 +24,8 @@ class SimpleMissionItem : public VisualMissionItem Q_OBJECT public: - SimpleMissionItem(PlanMasterController* masterController, bool flyView, QObject* parent); + SimpleMissionItem(PlanMasterController* masterController, bool flyView, bool forLoad, QObject* parent); SimpleMissionItem(PlanMasterController* masterController, bool flyView, const MissionItem& missionItem, QObject* parent); - //SimpleMissionItem(const SimpleMissionItem& other, bool flyView, QObject* parent); ~SimpleMissionItem(); diff --git a/src/MissionManager/SimpleMissionItemTest.cc b/src/MissionManager/SimpleMissionItemTest.cc index caa8c5204ba045c3353e5ded11ba2583c25fd7ac..44aa0b509f9564eeb904598b44f58c9492103779 100644 --- a/src/MissionManager/SimpleMissionItemTest.cc +++ b/src/MissionManager/SimpleMissionItemTest.cc @@ -166,7 +166,7 @@ void SimpleMissionItemTest::_testEditorFacts(void) void SimpleMissionItemTest::_testDefaultValues(void) { - SimpleMissionItem item(_masterController, false /* flyView */, nullptr); + SimpleMissionItem item(_masterController, false /* flyView */, false /* forLoad */, nullptr); item.missionItem().setCommand(MAV_CMD_NAV_WAYPOINT); item.missionItem().setFrame(MAV_FRAME_GLOBAL_RELATIVE_ALT); diff --git a/src/MissionManager/SurveyComplexItem.cc b/src/MissionManager/SurveyComplexItem.cc index 82815ce9668d34fdd524b4337d61768222cc0b4c..3ee340f136baa4ec2632d2d2ce1481ced72f3946 100644 --- a/src/MissionManager/SurveyComplexItem.cc +++ b/src/MissionManager/SurveyComplexItem.cc @@ -79,6 +79,11 @@ SurveyComplexItem::SurveyComplexItem(PlanMasterController* masterController, boo _turnAroundDistanceFact.setRawValue(10); } + if (_controllerVehicle && !(_controllerVehicle->fixedWing() || _controllerVehicle->vtol())) { + // Only fixed wing flight paths support alternate transects + _flyAlternateTransectsFact.setRawValue(false); + } + // We override the altitude to the mission default if (_cameraCalc.isManualCamera() || !_cameraCalc.valueSetIsDistance()->rawValue().toBool()) { _cameraCalc.distanceToSurface()->setRawValue(qgcApp()->toolbox()->settingsManager()->appSettings()->defaultMissionItemAltitude()->rawValue()); @@ -648,119 +653,152 @@ bool SurveyComplexItem::_nextTransectCoord(const QList& transect return true; } +void SurveyComplexItem::_appendWaypoint(QList& items, QObject* missionItemParent, int& seqNum, MAV_FRAME mavFrame, float holdTime, const QGeoCoordinate& coordinate) +{ + MissionItem* item = new MissionItem(seqNum++, + MAV_CMD_NAV_WAYPOINT, + mavFrame, + holdTime, + 0.0, // No acceptance radius specified + 0.0, // Pass through waypoint + std::numeric_limits::quiet_NaN(), // Yaw unchanged + coordinate.latitude(), + coordinate.longitude(), + coordinate.altitude(), + true, // autoContinue + false, // isCurrentItem + missionItemParent); + items.append(item); +} + +void SurveyComplexItem::_appendSinglePhotoCapture(QList& items, QObject* missionItemParent, int& seqNum) +{ + MissionItem* item = new MissionItem(seqNum++, + MAV_CMD_IMAGE_START_CAPTURE, + MAV_FRAME_MISSION, + 0, // Reserved (Set to 0) + 0, // Interval (none) + 1, // Take 1 photo + qQNaN(), qQNaN(), qQNaN(), qQNaN(), // param 4-7 reserved + true, // autoContinue + false, // isCurrentItem + missionItemParent); + items.append(item); +} + +void SurveyComplexItem::_appendConditionGate(QList& items, QObject* missionItemParent, int& seqNum, MAV_FRAME mavFrame, const QGeoCoordinate& coordinate) +{ + MissionItem* item = new MissionItem(seqNum++, + MAV_CMD_CONDITION_GATE, + mavFrame, + 0, // Gate is orthogonal to path + 0, // Ignore altitude + 0, 0, // Param 3-4 ignored + coordinate.latitude(), + coordinate.longitude(), + 0, // No altitude + true, // autoContinue + false, // isCurrentItem + missionItemParent); + items.append(item); +} + +void SurveyComplexItem::_appendCameraTriggerDistance(QList& items, QObject* missionItemParent, int& seqNum, float triggerDistance) +{ + MissionItem* item = new MissionItem(seqNum++, + MAV_CMD_DO_SET_CAM_TRIGG_DIST, + MAV_FRAME_MISSION, + triggerDistance, + 0, // shutter integration (ignore) + 1, // 1 - trigger one image immediately, both and entry and exit to get full coverage + 0, 0, 0, 0, // param 4-7 unused + true, // autoContinue + false, // isCurrentItem + missionItemParent); + items.append(item); +} + +void SurveyComplexItem::_appendCameraTriggerDistanceUpdatePoint(QList& items, QObject* missionItemParent, int& seqNum, MAV_FRAME mavFrame, const QGeoCoordinate& coordinate, bool useConditionGate, float triggerDistance) +{ + if (useConditionGate) { + _appendConditionGate(items, missionItemParent, seqNum, mavFrame, coordinate); + } else { + _appendWaypoint(items, missionItemParent, seqNum, mavFrame, 0 /* holdTime */, coordinate); + } + _appendCameraTriggerDistance(items, missionItemParent, seqNum, triggerDistance); +} + void SurveyComplexItem::_buildAndAppendMissionItems(QList& items, QObject* missionItemParent) { qCDebug(SurveyComplexItemLog) << "_buildAndAppendMissionItems"; // Now build the mission items from the transect points - MissionItem* item; int seqNum = _sequenceNumber; - bool imagesEverywhere = _cameraTriggerInTurnAroundFact.rawValue().toBool(); - bool addTriggerAtBeginning = !hoverAndCaptureEnabled() && imagesEverywhere; - bool firstOverallPoint = true; + bool imagesInTurnaround = _cameraTriggerInTurnAroundFact.rawValue().toBool() && turnAroundDistance()->rawValue().toDouble() != 0; + bool addTriggerAtBeginningEnd = !hoverAndCaptureEnabled() && imagesInTurnaround && triggerCamera(); + bool useConditionGate = _controllerVehicle->firmwarePlugin()->supportedMissionCommands().contains(MAV_CMD_CONDITION_GATE) && + triggerCamera() && + !hoverAndCaptureEnabled(); MAV_FRAME mavFrame = followTerrain() || !_cameraCalc.distanceToSurfaceRelative() ? MAV_FRAME_GLOBAL : MAV_FRAME_GLOBAL_RELATIVE_ALT; + // Note: The code below is written to be understable as oppose to being compact and/or remove duplicate code + int transectIndex = 0; for (const QList& transect: _transects) { - bool transectEntry = true; - + bool entryTurnaround = true; for (const CoordInfo_t& transectCoordInfo: transect) { - item = new MissionItem(seqNum++, - MAV_CMD_NAV_WAYPOINT, - mavFrame, - hoverAndCaptureEnabled() ? - _hoverAndCaptureDelaySeconds : 0, // Hold time (delay for hover and capture to settle vehicle before image is taken) - 0.0, // No acceptance radius specified - 0.0, // Pass through waypoint - std::numeric_limits::quiet_NaN(), // Yaw unchanged - transectCoordInfo.coord.latitude(), - transectCoordInfo.coord.longitude(), - transectCoordInfo.coord.altitude(), - true, // autoContinue - false, // isCurrentItem - missionItemParent); - items.append(item); - if (hoverAndCaptureEnabled()) { - item = new MissionItem(seqNum++, - MAV_CMD_IMAGE_START_CAPTURE, - MAV_FRAME_MISSION, - 0, // Reserved (Set to 0) - 0, // Interval (none) - 1, // Take 1 photo - qQNaN(), qQNaN(), qQNaN(), qQNaN(), // param 4-7 reserved - true, // autoContinue - false, // isCurrentItem - missionItemParent); - items.append(item); - } - - if (firstOverallPoint && addTriggerAtBeginning) { - // Start triggering - addTriggerAtBeginning = false; - item = new MissionItem(seqNum++, - MAV_CMD_DO_SET_CAM_TRIGG_DIST, - MAV_FRAME_MISSION, - triggerDistance(), // trigger distance - 0, // shutter integration (ignore) - 1, // trigger immediately when starting - 0, 0, 0, 0, // param 4-7 unused - true, // autoContinue - false, // isCurrentItem - missionItemParent); - items.append(item); + switch (transectCoordInfo.coordType) { + case CoordTypeTurnaround: + { + bool firstEntryTurnaround = transectIndex == 0 && entryTurnaround; + bool lastExitTurnaround = transectIndex == _transects.count() - 1 && !entryTurnaround; + if (addTriggerAtBeginningEnd && (firstEntryTurnaround || lastExitTurnaround)) { + _appendCameraTriggerDistanceUpdatePoint(items, missionItemParent, seqNum, mavFrame, transectCoordInfo.coord, useConditionGate, firstEntryTurnaround ? triggerDistance() : 0); + } else { + _appendWaypoint(items, missionItemParent, seqNum, mavFrame, 0 /* holdTime */, transectCoordInfo.coord); + } + entryTurnaround = false; } - firstOverallPoint = false; - - // Possibly add trigger start/stop to survey area entrance/exit - if (triggerCamera() && !hoverAndCaptureEnabled() && transectCoordInfo.coordType == TransectStyleComplexItem::CoordTypeSurveyEdge) { - if (transectEntry) { - // Start of transect, always start triggering. We do this even if we are taking images everywhere. - // This allows a restart of the mission in mid-air without losing images from the entire mission. - // At most you may lose part of a transect. - item = new MissionItem(seqNum++, - MAV_CMD_DO_SET_CAM_TRIGG_DIST, - MAV_FRAME_MISSION, - triggerDistance(), // trigger distance - 0, // shutter integration (ignore) - 1, // trigger immediately when starting - 0, 0, 0, 0, // param 4-7 unused - true, // autoContinue - false, // isCurrentItem - missionItemParent); - items.append(item); - transectEntry = false; - } else if (!imagesEverywhere && !transectEntry){ - // End of transect, stop triggering - item = new MissionItem(seqNum++, - MAV_CMD_DO_SET_CAM_TRIGG_DIST, - MAV_FRAME_MISSION, - 0, // stop triggering - 0, // shutter integration (ignore) - 0, // trigger immediately when starting - 0, 0, 0, 0, // param 4-7 unused - true, // autoContinue - false, // isCurrentItem - missionItemParent); - items.append(item); + break; + case CoordTypeInterior: + case CoordTypeInteriorTerrainAdded: + _appendWaypoint(items, missionItemParent, seqNum, mavFrame, 0 /* holdTime */, transectCoordInfo.coord); + break; + case CoordTypeInteriorHoverTrigger: + _appendWaypoint(items, missionItemParent, seqNum, mavFrame, _hoverAndCaptureDelaySeconds, transectCoordInfo.coord); + _appendSinglePhotoCapture(items, missionItemParent, seqNum); + break; + case CoordTypeSurveyEntry: + if (triggerCamera()) { + if (hoverAndCaptureEnabled()) { + _appendWaypoint(items, missionItemParent, seqNum, mavFrame, _hoverAndCaptureDelaySeconds, transectCoordInfo.coord); + _appendSinglePhotoCapture(items, missionItemParent, seqNum); + } else { + // We always add a trigger start to survey entry. Even for imagesInTurnaround = true. This allows you to resume a mission and refly a transect + _appendCameraTriggerDistanceUpdatePoint(items, missionItemParent, seqNum, mavFrame, transectCoordInfo.coord, useConditionGate, triggerDistance()); + } + } else { + _appendWaypoint(items, missionItemParent, seqNum, mavFrame, 0 /* holdTime */, transectCoordInfo.coord); + } + break; + case CoordTypeSurveyExit: + if (triggerCamera()) { + if (hoverAndCaptureEnabled()) { + _appendWaypoint(items, missionItemParent, seqNum, mavFrame, _hoverAndCaptureDelaySeconds, transectCoordInfo.coord); + _appendSinglePhotoCapture(items, missionItemParent, seqNum); + } else if (imagesInTurnaround) { + _appendWaypoint(items, missionItemParent, seqNum, mavFrame, 0 /* holdTime */, transectCoordInfo.coord); + } else { + _appendCameraTriggerDistanceUpdatePoint(items, missionItemParent, seqNum, mavFrame, transectCoordInfo.coord, useConditionGate, 0 /* triggerDistance */); + } + } else { + _appendWaypoint(items, missionItemParent, seqNum, mavFrame, 0 /* holdTime */, transectCoordInfo.coord); } + break; } } - } - - if (triggerCamera() && !hoverAndCaptureEnabled() && imagesEverywhere) { - // Stop triggering - MissionItem* item = new MissionItem(seqNum++, - MAV_CMD_DO_SET_CAM_TRIGG_DIST, - MAV_FRAME_MISSION, - 0, // stop triggering - 0, // shutter integration (ignore) - 0, // trigger immediately when starting - 0, 0, 0, 0, // param 4-7 unused - true, // autoContinue - false, // isCurrentItem - missionItemParent); - items.append(item); + transectIndex++; } } @@ -971,9 +1009,9 @@ void SurveyComplexItem::_rebuildTransectsPhase1WorkerSinglePolygon(bool refly) QList coordInfoTransect; TransectStyleComplexItem::CoordInfo_t coordInfo; - coordInfo = { transect[0], CoordTypeSurveyEdge }; + coordInfo = { transect[0], CoordTypeSurveyEntry }; coordInfoTransect.append(coordInfo); - coordInfo = { transect[1], CoordTypeSurveyEdge }; + coordInfo = { transect[1], CoordTypeSurveyExit }; coordInfoTransect.append(coordInfo); // For hover and capture we need points for each camera location within the transect @@ -1380,9 +1418,9 @@ void SurveyComplexItem::_rebuildTransectsFromPolygon(bool refly, const QPolygonF QList coordInfoTransect; TransectStyleComplexItem::CoordInfo_t coordInfo; - coordInfo = { transect[0], CoordTypeSurveyEdge }; + coordInfo = { transect[0], CoordTypeSurveyEntry }; coordInfoTransect.append(coordInfo); - coordInfo = { transect[1], CoordTypeSurveyEdge }; + coordInfo = { transect[1], CoordTypeSurveyExit }; coordInfoTransect.append(coordInfo); // For hover and capture we need points for each camera location within the transect diff --git a/src/MissionManager/SurveyComplexItem.h b/src/MissionManager/SurveyComplexItem.h index b2d59aece64e40a36c10815b8ead9ab831d3b17f..7b152fdfd34d72b3b4f905753d26c5cf8267c3fb 100644 --- a/src/MissionManager/SurveyComplexItem.h +++ b/src/MissionManager/SurveyComplexItem.h @@ -98,7 +98,6 @@ private: void _intersectLinesWithRect(const QList& lineList, const QRectF& boundRect, QList& resultLines); void _intersectLinesWithPolygon(const QList& lineList, const QPolygonF& polygon, QList& resultLines); void _adjustLineDirection(const QList& lineList, QList& resultLines); - int _appendWaypointToMission(QList& items, int seqNum, QGeoCoordinate& coord, CameraTriggerCode cameraTrigger, QObject* missionItemParent); bool _nextTransectCoord(const QList& transectPoints, int pointIndex, QGeoCoordinate& coord); bool _appendMissionItemsWorker(QList& items, QObject* missionItemParent, int& seqNum, bool hasRefly, bool buildRefly); void _optimizeTransectsForShortestDistance(const QGeoCoordinate& distanceCoord, QList>& transects); @@ -130,6 +129,11 @@ private: // return true if vertex a can see vertex b bool _VertexCanSeeOther(const QPolygonF& polygon, const QPointF* vertexA, const QPointF* vertexB); bool _VertexIsReflex(const QPolygonF& polygon, const QPointF* vertex); + void _appendWaypoint(QList& items, QObject* missionItemParent, int& seqNum, MAV_FRAME mavFrame, float holdTime, const QGeoCoordinate& coordinate); + void _appendSinglePhotoCapture(QList& items, QObject* missionItemParent, int& seqNum); + void _appendConditionGate(QList& items, QObject* missionItemParent, int& seqNum, MAV_FRAME mavFrame, const QGeoCoordinate& coordinate); + void _appendCameraTriggerDistance(QList& items, QObject* missionItemParent, int& seqNum, float triggerDistance); + void _appendCameraTriggerDistanceUpdatePoint(QList& items, QObject* missionItemParent, int& seqNum, MAV_FRAME mavFrame, const QGeoCoordinate& coordinate, bool useConditionGate, float triggerDistance); QMap _metaDataMap; diff --git a/src/MissionManager/SurveyComplexItemTest.cc b/src/MissionManager/SurveyComplexItemTest.cc index f812d7bcb5689d8f4584f76f66da2eba419db998..91dc49ddc399e3f247e0f9ee92dad11de5928c34 100644 --- a/src/MissionManager/SurveyComplexItemTest.cc +++ b/src/MissionManager/SurveyComplexItemTest.cc @@ -9,11 +9,16 @@ #include "SurveyComplexItemTest.h" #include "QGCApplication.h" +#include "JsonHelper.h" SurveyComplexItemTest::SurveyComplexItemTest(void) { - _polyPoints << QGeoCoordinate(47.633550640000003, -122.08982199) << QGeoCoordinate(47.634129020000003, -122.08887249) << - QGeoCoordinate(47.633619320000001, -122.08811074) << QGeoCoordinate(47.633189139999999, -122.08900124); + // We use a 100m by 100m square test polygon + const double edgeDistance = 100; + _polyVertices.append(QGeoCoordinate(47.633550640000003, -122.08982199)); + _polyVertices.append(_polyVertices[0].atDistanceAndAzimuth(edgeDistance, 90)); + _polyVertices.append(_polyVertices[1].atDistanceAndAzimuth(edgeDistance, 180)); + _polyVertices.append(_polyVertices[2].atDistanceAndAzimuth(edgeDistance, -90.0)); } void SurveyComplexItemTest::init(void) @@ -27,12 +32,26 @@ void SurveyComplexItemTest::init(void) _rgSurveySignals[surveyRefly90DegreesChangedIndex] = SIGNAL(refly90DegreesChanged(bool)); _rgSurveySignals[surveyDirtyChangedIndex] = SIGNAL(dirtyChanged(bool)); + _planViewSettings = qgcApp()->toolbox()->settingsManager()->planViewSettings(); _masterController = new PlanMasterController(this); _controllerVehicle = _masterController->controllerVehicle(); _surveyItem = new SurveyComplexItem(_masterController, false /* flyView */, QString() /* kmlFile */, this /* parent */); - _surveyItem->turnAroundDistance()->setRawValue(0); // Unit test written for no turnaround distance - _surveyItem->setDirty(false); _mapPolygon = _surveyItem->surveyAreaPolygon(); + _mapPolygon->appendVertices(_polyVertices); + + QVERIFY(_surveyItem->cameraCalc()->isManualCamera()); + + // Set grid spacing to match expected transect count + double polyWidthDistance = _polyVertices[0].distanceTo(_polyVertices[1]); + double polyHeightDistance = _polyVertices[0].distanceTo(_polyVertices[3]); + _surveyItem->cameraCalc()->adjustedFootprintSide()->setRawValue((polyWidthDistance * 0.5) - 1.0); + _surveyItem->cameraCalc()->adjustedFootprintFrontal()->setRawValue(polyHeightDistance * 0.25); + + _surveyItem->gridAngle()->setRawValue(0); + int expectedTransectCount = _expectedTransectCount; + QCOMPARE(_surveyItem->_transectCount(), expectedTransectCount); + + _surveyItem->setDirty(false); // It's important to check that the right signals are emitted at the right time since that drives ui change. // It's also important to check that things are not being over-signalled when they should not be, since that can lead @@ -107,10 +126,7 @@ double SurveyComplexItemTest::_clampGridAngle180(double gridAngle) void SurveyComplexItemTest::_setPolygon(void) { - for (int i=0; i<_polyPoints.count(); i++) { - QGeoCoordinate& vertex = _polyPoints[i]; - _mapPolygon->appendVertex(vertex); - } + _mapPolygon->appendVertices(_polyVertices); } void SurveyComplexItemTest::_testGridAngle(void) @@ -184,3 +200,142 @@ void SurveyComplexItemTest::_testItemCount(void) QCOMPARE(items.count() - 1, _surveyItem->lastSequenceNumber()); items.clear(); } + +QList SurveyComplexItemTest::_createExpectedCommands(bool hasTurnaround, bool useConditionGate) +{ + static const QList singleFullTransect = { + MAV_CMD_NAV_WAYPOINT, // Turnaround + MAV_CMD_CONDITION_GATE, // Survey area entry edge + MAV_CMD_DO_SET_CAM_TRIGG_DIST, + MAV_CMD_CONDITION_GATE, // Survey area exit edge + MAV_CMD_DO_SET_CAM_TRIGG_DIST, + MAV_CMD_NAV_WAYPOINT, + }; + + QList singleTransect = singleFullTransect; + QList expectedCommands; + + if (!useConditionGate) { + for (MAV_CMD& cmd : singleTransect) { + cmd = cmd == MAV_CMD_CONDITION_GATE ? MAV_CMD_NAV_WAYPOINT : cmd; + } + } + + if (!hasTurnaround) { + singleTransect.takeFirst(); + singleTransect.takeLast(); + } + + for (int i=0; i<_expectedTransectCount; i++) { + expectedCommands.append(singleTransect); + } + + return expectedCommands; +} + +void SurveyComplexItemTest::_testItemGenerationWorker(bool imagesInTurnaround, bool hasTurnaround, bool useConditionGate, const QList& expectedCommands) +{ + qDebug() << QStringLiteral("_testItemGenerationWorker imagesInTuraround:%1 turnaround:%2 gate:%3").arg(imagesInTurnaround).arg(hasTurnaround).arg(useConditionGate); + + _surveyItem->turnAroundDistance()->setRawValue(hasTurnaround ? 50 : 0); + _surveyItem->cameraTriggerInTurnAround()->setRawValue(imagesInTurnaround); + _planViewSettings->useConditionGate()->setRawValue(useConditionGate); + + QList items; + _surveyItem->appendMissionItems(items, this); +#if 0 + // Handy for debugging failures + for (const MissionItem* item : items) { + qDebug() << "Cmd" << item->command(); + } +#endif + QCOMPARE(items.count(), expectedCommands.count()); + for (int i=0; icommand(); + int expectedCommand = expectedCommands[i]; +#if 0 + // Handy for debugging failures + qDebug() << "Index" << i; +#endif + QCOMPARE(actualCommand, expectedCommand); + } +} + +void SurveyComplexItemTest::_testItemGeneration(void) +{ + // Test all the combinations of: cameraTriggerInTurnAround: false, hasTurnAround: *, useConditionGate: * + + typedef struct { + bool hasTurnaround; + bool useConditionGate; + } TestCase_t; + + static const TestCase_t rgTestCases[] = { + { false, false }, + { false, true }, + { true, false }, + { true, true }, + }; + + for (const TestCase_t& testCase : rgTestCases) { + _testItemGenerationWorker(false /* imagesInTurnaround */, testCase.hasTurnaround, testCase.useConditionGate, _createExpectedCommands(testCase.hasTurnaround, testCase.useConditionGate)); + } + + // Test cameraTriggerInTurnAround = true cases + + QList imagesInTurnaroundCommands = { + // Transect 1 + MAV_CMD_CONDITION_GATE, // First turaround + MAV_CMD_DO_SET_CAM_TRIGG_DIST, + MAV_CMD_CONDITION_GATE, // Survey entry + MAV_CMD_DO_SET_CAM_TRIGG_DIST, // Survey entry also has trigger start + MAV_CMD_NAV_WAYPOINT, // Survey exit + MAV_CMD_NAV_WAYPOINT, // Turnaround + // Transect 2 + MAV_CMD_NAV_WAYPOINT, // Turnaround + MAV_CMD_CONDITION_GATE, // Survey entry + MAV_CMD_DO_SET_CAM_TRIGG_DIST, // Survey entry also has trigger start + MAV_CMD_NAV_WAYPOINT, // Survey exit + MAV_CMD_CONDITION_GATE, // Final turnaround + MAV_CMD_DO_SET_CAM_TRIGG_DIST, + }; + + _testItemGenerationWorker(true /* imagesInTurnaround */, true /* hasTurnaround */, true /* useConditionGate */, imagesInTurnaroundCommands); + + // Switch to non CONDITION_GATE usage + for (MAV_CMD& cmd : imagesInTurnaroundCommands) { + cmd = cmd == MAV_CMD_CONDITION_GATE ? MAV_CMD_NAV_WAYPOINT : cmd; + } + _testItemGenerationWorker(true /* imagesInTurnaround */, true /* hasTurnaround */, false /* useConditionGate */, imagesInTurnaroundCommands); +} + +void SurveyComplexItemTest::_testHoverCaptureItemGeneration(void) +{ + static const QList singleFullTransect = { + MAV_CMD_NAV_WAYPOINT, // Turnaround + MAV_CMD_NAV_WAYPOINT, // Survey area entry edge + MAV_CMD_IMAGE_START_CAPTURE, + MAV_CMD_NAV_WAYPOINT, // Interior trigger + MAV_CMD_IMAGE_START_CAPTURE, + MAV_CMD_NAV_WAYPOINT, // Interior trigger + MAV_CMD_IMAGE_START_CAPTURE, + MAV_CMD_NAV_WAYPOINT, // Survey area exit edge + MAV_CMD_IMAGE_START_CAPTURE, + MAV_CMD_NAV_WAYPOINT, // Turnaround + }; + + QList expectedCommands; + for (int i=0; i<_expectedTransectCount; i++) { + expectedCommands.append(singleFullTransect); + } + + // Set trigger distance to generates two interior capture points + double polyHeightDistance = _polyVertices[0].distanceTo(_polyVertices[3]); + double triggerDistance = (polyHeightDistance / 3.0) + 1.0; + _surveyItem->cameraCalc()->adjustedFootprintFrontal()->setRawValue(triggerDistance); + + qDebug() << "_testHoverCaptureItemGeneration"; + _surveyItem->hoverAndCapture()->setRawValue(true); + _testItemGenerationWorker(false /* imagesInTurnaround */, true /* hasTurnaround */, true /* useConditionGate */, expectedCommands); + _testItemGenerationWorker(false /* imagesInTurnaround */, true /* hasTurnaround */, false /* useConditionGate */, expectedCommands); +} diff --git a/src/MissionManager/SurveyComplexItemTest.h b/src/MissionManager/SurveyComplexItemTest.h index b2a3728e041ac0b8c98262b7ca8309ed07a77f3a..6204d30cbee8bd50fa520ca12280b3fac45bebc1 100644 --- a/src/MissionManager/SurveyComplexItemTest.h +++ b/src/MissionManager/SurveyComplexItemTest.h @@ -14,6 +14,7 @@ #include "MultiSignalSpy.h" #include "SurveyComplexItem.h" #include "PlanMasterController.h" +#include "PlanViewSettings.h" #include @@ -30,15 +31,18 @@ protected: void cleanup(void) final; private slots: + void _testHoverCaptureItemGeneration(void); + +private: void _testDirty(void); void _testGridAngle(void); void _testEntryLocation(void); void _testItemCount(void); - -private: - + void _testItemGeneration(void); double _clampGridAngle180(double gridAngle); void _setPolygon(void); + QList _createExpectedCommands(bool hasTurnaround, bool useConditionGate); + void _testItemGenerationWorker(bool imagesInTurnaround, bool hasTurnaround, bool useConditionGate, const QList& expectedCommands); // SurveyComplexItem signals @@ -69,5 +73,8 @@ private: MultiSignalSpy* _multiSpy = nullptr; SurveyComplexItem* _surveyItem = nullptr; QGCMapPolygon* _mapPolygon = nullptr; - QList _polyPoints; + PlanViewSettings* _planViewSettings = nullptr; + QList _polyVertices; + + static const int _expectedTransectCount = 2; }; diff --git a/src/MissionManager/TakeoffMissionItem.cc b/src/MissionManager/TakeoffMissionItem.cc index 08400bf9e17852df3289b7d72f4f114fec2bae9a..335f1b3a474c8d9189da668ba1a08dc5cc27a42e 100644 --- a/src/MissionManager/TakeoffMissionItem.cc +++ b/src/MissionManager/TakeoffMissionItem.cc @@ -20,26 +20,27 @@ #include "SettingsManager.h" #include "PlanMasterController.h" -TakeoffMissionItem::TakeoffMissionItem(PlanMasterController* masterController, bool flyView, MissionSettingsItem* settingsItem, QObject* parent) - : SimpleMissionItem (masterController, flyView, parent) +TakeoffMissionItem::TakeoffMissionItem(PlanMasterController* masterController, bool flyView, MissionSettingsItem* settingsItem, bool forLoad, QObject* parent) + : SimpleMissionItem (masterController, flyView, forLoad, parent) , _settingsItem (settingsItem) { - _init(); + _init(forLoad); } TakeoffMissionItem::TakeoffMissionItem(MAV_CMD takeoffCmd, PlanMasterController* masterController, bool flyView, MissionSettingsItem* settingsItem, QObject* parent) - : SimpleMissionItem (masterController, flyView, parent) + : SimpleMissionItem (masterController, flyView, false /* forLoad */, parent) , _settingsItem (settingsItem) { setCommand(takeoffCmd); - _init(); + _init(false /* forLoad */); } TakeoffMissionItem::TakeoffMissionItem(const MissionItem& missionItem, PlanMasterController* masterController, bool flyView, MissionSettingsItem* settingsItem, QObject* parent) : SimpleMissionItem (masterController, flyView, missionItem, parent) , _settingsItem (settingsItem) { - _init(); + _init(false /* forLoad */); + _wizardMode = false; } TakeoffMissionItem::~TakeoffMissionItem() @@ -47,7 +48,7 @@ TakeoffMissionItem::~TakeoffMissionItem() } -void TakeoffMissionItem::_init(void) +void TakeoffMissionItem::_init(bool forLoad) { _editorQml = QStringLiteral("qrc:/qml/SimpleItemEditor.qml"); @@ -69,10 +70,15 @@ void TakeoffMissionItem::_init(void) } } + if (forLoad) { + // Load routines will set the rest up after load + return; + } + _initLaunchTakeoffAtSameLocation(); if (homePosition.isValid() && coordinate().isValid()) { - // Item already full specified, most likely from mission load from storage + // Item already fully specified, most likely from mission load from storage _wizardMode = false; } else { if (_launchTakeoffAtSameLocation && homePosition.isValid()) { @@ -140,6 +146,7 @@ bool TakeoffMissionItem::load(QTextStream &loadStream) if (success) { _initLaunchTakeoffAtSameLocation(); } + _wizardMode = false; // Always be off for loaded items return success; } @@ -149,6 +156,7 @@ bool TakeoffMissionItem::load(const QJsonObject& json, int sequenceNumber, QStri if (success) { _initLaunchTakeoffAtSameLocation(); } + _wizardMode = false; // Always be off for loaded items return success; } diff --git a/src/MissionManager/TakeoffMissionItem.h b/src/MissionManager/TakeoffMissionItem.h index 13a1cb65440c13f55a7fc397b9c033567393c69d..6a1114fcd66d9a3002d521b7e30fb8d66422e1fd 100644 --- a/src/MissionManager/TakeoffMissionItem.h +++ b/src/MissionManager/TakeoffMissionItem.h @@ -21,7 +21,7 @@ class TakeoffMissionItem : public SimpleMissionItem Q_OBJECT public: - TakeoffMissionItem(PlanMasterController* masterController, bool flyView, MissionSettingsItem* settingsItem, QObject* parent); + TakeoffMissionItem(PlanMasterController* masterController, bool flyView, MissionSettingsItem* settingsItem, bool forLoad, QObject* parent); TakeoffMissionItem(MAV_CMD takeoffCmd, PlanMasterController* masterController, bool flyView, MissionSettingsItem* settingsItem, QObject* parent); TakeoffMissionItem(const MissionItem& missionItem, PlanMasterController* masterController, bool flyView, MissionSettingsItem* settingsItem, QObject* parent); @@ -57,7 +57,7 @@ signals: void launchTakeoffAtSameLocationChanged (bool launchTakeoffAtSameLocation); private: - void _init(void); + void _init(bool forLoad); void _initLaunchTakeoffAtSameLocation(void); MissionSettingsItem* _settingsItem; diff --git a/src/MissionManager/TransectStyleComplexItem.cc b/src/MissionManager/TransectStyleComplexItem.cc index 4fbe9a3dd16df76255a9ae476ad2997f291847d6..89c0a768e5bb3eb8df76334936f79ed231a9cb4a 100644 --- a/src/MissionManager/TransectStyleComplexItem.cc +++ b/src/MissionManager/TransectStyleComplexItem.cc @@ -498,7 +498,19 @@ void TransectStyleComplexItem::_polyPathTerrainData(bool success, const QListrawValue().toBool(); } bool triggerCamera (void) const { return triggerDistance() != 0; } + // Used internally only by unit tests + int _transectCount(void) { return _transects.count(); } + // Overrides from ComplexMissionItem int lastSequenceNumber (void) const final; @@ -159,8 +162,9 @@ protected: CoordTypeInterior, ///< Interior waypoint for flight path only CoordTypeInteriorHoverTrigger, ///< Interior waypoint for hover and capture trigger CoordTypeInteriorTerrainAdded, ///< Interior waypoint added for terrain - CoordTypeSurveyEdge, ///< Waypoint at edge of survey polygon - CoordTypeTurnaround ///< Waypoint outside of survey polygon for turnaround + CoordTypeSurveyEntry, ///< Waypoint at entry edge of survey polygon + CoordTypeSurveyExit, ///< Waypoint at exit edge of survey polygon + CoordTypeTurnaround, ///< First turnaround waypoint }; typedef struct { diff --git a/src/MissionManager/UnitTest/MP 19.prj b/src/MissionManager/UnitTest/MP 19.prj new file mode 100644 index 0000000000000000000000000000000000000000..a30c00a55de19be195abf9e942f6cff93bf0a825 --- /dev/null +++ b/src/MissionManager/UnitTest/MP 19.prj @@ -0,0 +1 @@ +GEOGCS["GCS_WGS_1984",DATUM["D_WGS_1984",SPHEROID["WGS_1984",6378137,298.257223563]],PRIMEM["Greenwich",0],UNIT["Degree",0.017453292519943295]] \ No newline at end of file diff --git a/src/MissionManager/UnitTest/MP 19.shp b/src/MissionManager/UnitTest/MP 19.shp new file mode 100644 index 0000000000000000000000000000000000000000..fdb99b041675a81a45765dfe1ba438d93eb3c9a0 Binary files /dev/null and b/src/MissionManager/UnitTest/MP 19.shp differ diff --git a/src/MissionManager/UnitTest/MP 19.shx b/src/MissionManager/UnitTest/MP 19.shx new file mode 100644 index 0000000000000000000000000000000000000000..1094b553681921f96961aedd3fb4ea2f6133a565 Binary files /dev/null and b/src/MissionManager/UnitTest/MP 19.shx differ diff --git a/src/MissionManager/UnitTest/MP Bonus.prj b/src/MissionManager/UnitTest/MP Bonus.prj new file mode 100644 index 0000000000000000000000000000000000000000..a30c00a55de19be195abf9e942f6cff93bf0a825 --- /dev/null +++ b/src/MissionManager/UnitTest/MP Bonus.prj @@ -0,0 +1 @@ +GEOGCS["GCS_WGS_1984",DATUM["D_WGS_1984",SPHEROID["WGS_1984",6378137,298.257223563]],PRIMEM["Greenwich",0],UNIT["Degree",0.017453292519943295]] \ No newline at end of file diff --git a/src/MissionManager/UnitTest/MP Bonus.shp b/src/MissionManager/UnitTest/MP Bonus.shp new file mode 100644 index 0000000000000000000000000000000000000000..38c575f34f554a14b15debdb941e16a912c4f643 Binary files /dev/null and b/src/MissionManager/UnitTest/MP Bonus.shp differ diff --git a/src/MissionManager/UnitTest/MP Bonus.shx b/src/MissionManager/UnitTest/MP Bonus.shx new file mode 100644 index 0000000000000000000000000000000000000000..8dd9412638b55d9824540f3b3827aa39ce846af7 Binary files /dev/null and b/src/MissionManager/UnitTest/MP Bonus.shx differ diff --git a/src/MissionManager/UnitTest/Sarah's Farm.prj b/src/MissionManager/UnitTest/Sarah's Farm.prj new file mode 100644 index 0000000000000000000000000000000000000000..a30c00a55de19be195abf9e942f6cff93bf0a825 --- /dev/null +++ b/src/MissionManager/UnitTest/Sarah's Farm.prj @@ -0,0 +1 @@ +GEOGCS["GCS_WGS_1984",DATUM["D_WGS_1984",SPHEROID["WGS_1984",6378137,298.257223563]],PRIMEM["Greenwich",0],UNIT["Degree",0.017453292519943295]] \ No newline at end of file diff --git a/src/MissionManager/UnitTest/Sarah's Farm.shp b/src/MissionManager/UnitTest/Sarah's Farm.shp new file mode 100644 index 0000000000000000000000000000000000000000..cb0985bb0409bdb744912ae39d1a69fa1fa9724c Binary files /dev/null and b/src/MissionManager/UnitTest/Sarah's Farm.shp differ diff --git a/src/MissionManager/UnitTest/Sarah's Farm.shx b/src/MissionManager/UnitTest/Sarah's Farm.shx new file mode 100644 index 0000000000000000000000000000000000000000..f82d459c668466a4675e6c53b45cb2b078e04ac8 Binary files /dev/null and b/src/MissionManager/UnitTest/Sarah's Farm.shx differ diff --git a/src/PlanView/PlanView.qml b/src/PlanView/PlanView.qml index 0d9e5eda54dca78f04c7eaa757755b7acb976e3a..399cea59e400d97e5c8cdaa541194babd2d6b5c3 100644 --- a/src/PlanView/PlanView.qml +++ b/src/PlanView/PlanView.qml @@ -617,7 +617,7 @@ Item { { name: _missionController.isROIActive ? qsTr("Cancel ROI") : qsTr("ROI"), iconSource: "/qmlimages/MapAddMission.svg", - buttonEnabled: true, + buttonEnabled: !_missionController.onlyInsertTakeoffValid, buttonVisible: _isMissionLayer && _planMasterController.controllerVehicle.roiModeSupported, toggle: !_missionController.isROIActive }, diff --git a/src/QGCApplication.h b/src/QGCApplication.h index 9ae1ef12f39409fe5b1ac4c63755f32aaf21b3ba..08a11daad15cb9c747a2f8b99559ae0b89cb59d2 100644 --- a/src/QGCApplication.h +++ b/src/QGCApplication.h @@ -71,7 +71,7 @@ public: void reportMissingParameter(int componentId, const QString& name); /// Show a non-modal message to the user - void showMessage(const QString& message); + Q_SLOT void showMessage(const QString& message); /// @return true: Fake ui into showing mobile interface bool fakeMobile(void) const { return _fakeMobile; } diff --git a/src/QmlControls/QGCFileDialog.qml b/src/QmlControls/QGCFileDialog.qml index fedb66e15db2108296d13f3d8602b48bfb9591cc..06fe48e4a274c56d19ec3585f8e4cde266f9f4ea 100644 --- a/src/QmlControls/QGCFileDialog.qml +++ b/src/QmlControls/QGCFileDialog.qml @@ -117,12 +117,12 @@ Item { onClicked: { hideDialog() - _root.acceptedForLoad(controller.fullyQualifiedFilename(folder, modelData, fileExtension)) + _root.acceptedForLoad(controller.fullyQualifiedFilename(folder, modelData, _rgExtensions)) } onHamburgerClicked: { highlight = true - hamburgerMenu.fileToDelete = controller.fullyQualifiedFilename(folder, modelData, fileExtension) + hamburgerMenu.fileToDelete = controller.fullyQualifiedFilename(folder, modelData, _rgExtensions) hamburgerMenu.popup() } @@ -162,12 +162,12 @@ Item { return } if (!replaceMessage.visible) { - if (controller.fileExists(controller.fullyQualifiedFilename(folder, filenameTextField.text, fileExtension))) { + if (controller.fileExists(controller.fullyQualifiedFilename(folder, filenameTextField.text, _rgExtensions))) { replaceMessage.visible = true return } } - _root.acceptedForSave(controller.fullyQualifiedFilename(folder, filenameTextField.text, fileExtension)) + _root.acceptedForSave(controller.fullyQualifiedFilename(folder, filenameTextField.text, _rgExtensions)) hideDialog() } @@ -230,12 +230,12 @@ Item { onClicked: { hideDialog() - _root.acceptedForSave(controller.fullyQualifiedFilename(folder, modelData, fileExtension)) + _root.acceptedForSave(controller.fullyQualifiedFilename(folder, modelData, _rgExtensions)) } onHamburgerClicked: { highlight = true - hamburgerMenu.fileToDelete = controller.fullyQualifiedFilename(folder, modelData, fileExtension) + hamburgerMenu.fileToDelete = controller.fullyQualifiedFilename(folder, modelData, _rgExtensions) hamburgerMenu.popup() } diff --git a/src/QmlControls/QGCFileDialogController.cc b/src/QmlControls/QGCFileDialogController.cc index f5528df80d52cf4ce90c49eab7340a35ce442059..375fbd32aa564b9186942d4f54367964a45c877e 100644 --- a/src/QmlControls/QGCFileDialogController.cc +++ b/src/QmlControls/QGCFileDialogController.cc @@ -38,16 +38,24 @@ QStringList QGCFileDialogController::getFiles(const QString& directoryPath, cons return files; } -QString QGCFileDialogController::filenameWithExtension(const QString& filename, const QString& fileExtension) +QString QGCFileDialogController::filenameWithExtension(const QString& filename, const QStringList& rgFileExtensions) { QString filenameWithExtension(filename); - QString correctExtension = QString(".%1").arg(fileExtension); - if (!filenameWithExtension.endsWith(correctExtension)) { - filenameWithExtension += correctExtension; + bool matchFound = false; + for (const QString& extension : rgFileExtensions) { + QString dotExtension = QString(".%1").arg(extension); + matchFound = filenameWithExtension.endsWith(dotExtension); + if (matchFound) { + break; + } } - return filenameWithExtension; + if (!matchFound) { + filenameWithExtension += rgFileExtensions[0]; + } + +return filenameWithExtension; } bool QGCFileDialogController::fileExists(const QString& filename) @@ -55,9 +63,9 @@ bool QGCFileDialogController::fileExists(const QString& filename) return QFile(filename).exists(); } -QString QGCFileDialogController::fullyQualifiedFilename(const QString& directoryPath, const QString& filename, const QString& fileExtension) +QString QGCFileDialogController::fullyQualifiedFilename(const QString& directoryPath, const QString& filename, const QStringList& rgFileExtensions) { - return directoryPath + QStringLiteral("/") + filenameWithExtension(filename, fileExtension); + return directoryPath + QStringLiteral("/") + filenameWithExtension(filename, rgFileExtensions); } void QGCFileDialogController::deleteFile(const QString& filename) diff --git a/src/QmlControls/QGCFileDialogController.h b/src/QmlControls/QGCFileDialogController.h index 209a483cd376c24544f91ad36091fda65cc0aaa6..4e80503a06cd1cbda317dfa7d03a70dfe1d8cb8e 100644 --- a/src/QmlControls/QGCFileDialogController.h +++ b/src/QmlControls/QGCFileDialogController.h @@ -27,10 +27,10 @@ public: Q_INVOKABLE QStringList getFiles(const QString& directoryPath, const QStringList& fileExtensions); /// Returns the specified file name with the extension added it needed - Q_INVOKABLE QString filenameWithExtension(const QString& filename, const QString& fileExtension); + Q_INVOKABLE QString filenameWithExtension(const QString& filename, const QStringList& rgFileExtensions); /// Returns the fully qualified file name from the specified parts - Q_INVOKABLE QString fullyQualifiedFilename(const QString& directoryPath, const QString& filename, const QString& fileExtension); + Q_INVOKABLE QString fullyQualifiedFilename(const QString& directoryPath, const QString& filename, const QStringList& rgFileExtensions); /// Check for file existence of specified fully qualified file name Q_INVOKABLE bool fileExists(const QString& filename); diff --git a/src/QmlControls/ToolStrip.qml b/src/QmlControls/ToolStrip.qml index 067928fb44a7dd4925660b621684b6024580901e..9e10e79dc383e9d3678b793bbeb0f8cf9c770864 100644 --- a/src/QmlControls/ToolStrip.qml +++ b/src/QmlControls/ToolStrip.qml @@ -30,8 +30,10 @@ Rectangle { function simulateClick(buttonIndex) { buttonIndex = buttonIndex + 1 // skip over title - toolStripColumn.children[buttonIndex].checked = true - toolStripColumn.children[buttonIndex].clicked() + if (!toolStripColumn.children[buttonIndex].checked) { + toolStripColumn.children[buttonIndex].checked = true + toolStripColumn.children[buttonIndex].clicked() + } } // Ensure we don't get narrower than content diff --git a/src/Settings/PlanView.SettingsGroup.json b/src/Settings/PlanView.SettingsGroup.json index eb1921d9d94c202513305533517669f02b0250eb..dcb198bc95f712442b53fdf7721a4586c52beeca 100644 --- a/src/Settings/PlanView.SettingsGroup.json +++ b/src/Settings/PlanView.SettingsGroup.json @@ -15,6 +15,18 @@ "name": "showMissionItemStatus", "shortDescription": "Show/Hide the mission item status display", "type": "bool", + "defaultValue": true +}, +{ + "name": "takeoffItemNotRequired", + "shortDescription": "Allow missions to not require a takeoff item", + "type": "bool", "defaultValue": false +}, +{ + "name": "useConditionGate", + "shortDescription": "Use MAV_CMD_CONDITION_GATE for pattern generation", + "type": "bool", + "defaultValue": false } ] diff --git a/src/Settings/PlanViewSettings.cc b/src/Settings/PlanViewSettings.cc index 8f82dc044f3aedd9453277162ef7aeab58e8e128..9af8227d7b04641f8fea04693fb906a3ab870211 100644 --- a/src/Settings/PlanViewSettings.cc +++ b/src/Settings/PlanViewSettings.cc @@ -20,3 +20,5 @@ DECLARE_SETTINGGROUP(PlanView, "PlanView") DECLARE_SETTINGSFACT(PlanViewSettings, displayPresetsTabFirst) DECLARE_SETTINGSFACT(PlanViewSettings, aboveTerrainWarning) DECLARE_SETTINGSFACT(PlanViewSettings, showMissionItemStatus) +DECLARE_SETTINGSFACT(PlanViewSettings, useConditionGate) +DECLARE_SETTINGSFACT(PlanViewSettings, takeoffItemNotRequired) diff --git a/src/Settings/PlanViewSettings.h b/src/Settings/PlanViewSettings.h index 2fbc3bdc03fa0d37be8f96f4d2f8e6ed915ddb2a..c9bed13462743bcb2956e6f1ac861d09b3f6e134 100644 --- a/src/Settings/PlanViewSettings.h +++ b/src/Settings/PlanViewSettings.h @@ -23,4 +23,6 @@ public: DEFINE_SETTINGFACT(displayPresetsTabFirst) DEFINE_SETTINGFACT(aboveTerrainWarning) DEFINE_SETTINGFACT(showMissionItemStatus) + DEFINE_SETTINGFACT(useConditionGate) + DEFINE_SETTINGFACT(takeoffItemNotRequired) }; diff --git a/src/Settings/VideoSettings.cc b/src/Settings/VideoSettings.cc index 08fb41dc64ea694c4fe1240fa5c8e223cfa0fafb..7e2689c05314444d01a0591492e591c031299c92 100644 --- a/src/Settings/VideoSettings.cc +++ b/src/Settings/VideoSettings.cc @@ -167,5 +167,5 @@ bool VideoSettings::streamConfigured(void) void VideoSettings::_configChanged(QVariant) { - emit streamConfiguredChanged(); + emit streamConfiguredChanged(streamConfigured()); } diff --git a/src/Settings/VideoSettings.h b/src/Settings/VideoSettings.h index e8864f251d6fd41f3604da6a4537d83734f14451..38da7691681a97bf1838c4097674384d1ace3698 100644 --- a/src/Settings/VideoSettings.h +++ b/src/Settings/VideoSettings.h @@ -60,7 +60,7 @@ public: static const char* videoSourceMPEGTS; signals: - void streamConfiguredChanged (); + void streamConfiguredChanged (bool configured); private slots: void _configChanged (QVariant value); diff --git a/src/Terrain/TerrainQuery.cc b/src/Terrain/TerrainQuery.cc index bb5a44712fbf3f9146dff7e4c7f0c67578d490b3..4bdb4513fe6c2aa81849e5bb9967e2dd9dec547f 100644 --- a/src/Terrain/TerrainQuery.cc +++ b/src/Terrain/TerrainQuery.cc @@ -249,7 +249,7 @@ void TerrainAirMapQuery::_parseCarpetData(const QJsonValue& carpetJson) QJsonObject statsObject = jsonObject["stats"].toObject(); double minHeight = statsObject["min"].toDouble(); - double maxHeight = statsObject["min"].toDouble(); + double maxHeight = statsObject["max"].toDouble(); QList> carpet; if (!_carpetStatsOnly) { diff --git a/src/Vehicle/CMakeLists.txt b/src/Vehicle/CMakeLists.txt index f87853841e05eb00c8a2e97c6019ef42b7d44c5f..85b9cd115eb9b74530c7a22a06dcbfb04547c624 100644 --- a/src/Vehicle/CMakeLists.txt +++ b/src/Vehicle/CMakeLists.txt @@ -16,6 +16,10 @@ add_library(Vehicle MultiVehicleManager.h TrajectoryPoints.cc TrajectoryPoints.h + TerrainFactGroup.cc + TerrainFactGroup.h + TerrainProtocolHandler.cc + TerrainProtocolHandler.h Vehicle.cc Vehicle.h VehicleObjectAvoidance.cc diff --git a/src/Vehicle/Vehicle.cc b/src/Vehicle/Vehicle.cc index b863f3a268b4f377a0282c48dfd009973f30ef5f..9bd2e9111d45cff4f8205ba896a1522d0735114a 100644 --- a/src/Vehicle/Vehicle.cc +++ b/src/Vehicle/Vehicle.cc @@ -148,12 +148,6 @@ Vehicle::Vehicle(LinkInterface* link, , _telemetryTXBuffer(0) , _telemetryLNoise(0) , _telemetryRNoise(0) - , _mavlinkProtocolRequestComplete(false) - , _maxProtoVersion(0) - , _vehicleCapabilitiesKnown(false) - , _capabilityBits(firmwareType == MAV_AUTOPILOT_ARDUPILOTMEGA ? - MAV_PROTOCOL_CAPABILITY_MISSION_INT | MAV_PROTOCOL_CAPABILITY_COMMAND_INT | MAV_PROTOCOL_CAPABILITY_TERRAIN | MAV_PROTOCOL_CAPABILITY_MISSION_FENCE | MAV_PROTOCOL_CAPABILITY_MISSION_RALLY : // Hack workound for ArduPilot startup problems - 0) , _highLatencyLink(false) , _receivingAttitudeQuaternion(false) , _cameras(nullptr) @@ -355,7 +349,7 @@ Vehicle::Vehicle(MAV_AUTOPILOT firmwareType, , _defaultHoverSpeed(_settingsManager->appSettings()->offlineEditingHoverSpeed()->rawValue().toDouble()) , _mavlinkProtocolRequestComplete(true) , _maxProtoVersion(200) - , _vehicleCapabilitiesKnown(true) + , _capabilityBitsKnown(true) , _capabilityBits(MAV_PROTOCOL_CAPABILITY_MISSION_FENCE | MAV_PROTOCOL_CAPABILITY_MISSION_RALLY) , _highLatencyLink(false) , _receivingAttitudeQuaternion(false) @@ -712,6 +706,34 @@ void Vehicle::_mavlinkMessageReceived(LinkInterface* link, mavlink_message_t mes return; } + if (!_capabilityBitsKnown && message.msgid == MAVLINK_MSG_ID_HEARTBEAT && message.compid == MAV_COMP_ID_AUTOPILOT1) { + // We want to try to get capabilities as fast as possible so we retry on heartbeats + bool foundRequest = false; + for (MavCommandQueueEntry_t& queuedCommand : _mavCommandQueue) { + if (queuedCommand.command == MAV_CMD_REQUEST_AUTOPILOT_CAPABILITIES) { + foundRequest = true; + break; + } + } + if (!foundRequest) { + _capabilitiesRetryCount++; + if (_capabilitiesRetryCount == 1) { + _capabilitiesRetryElapsed.start(); + } else if (_capabilitiesRetryElapsed.elapsed() > 10000){ + qCDebug(VehicleLog) << "Giving up on getting AUTOPILOT_VERSION after 10 seconds"; + qgcApp()->showMessage(QStringLiteral("Vehicle failed to send AUTOPILOT_VERSION after waiting/retrying for 10 seconds")); + _handleUnsupportedRequestAutopilotCapabilities(); + } else { + // Vehicle never sent us AUTOPILOT_VERSION response. Try again. + qCDebug(VehicleLog) << QStringLiteral("Sending MAV_CMD_REQUEST_AUTOPILOT_CAPABILITIES again due to no response with AUTOPILOT_VERSION - _capabilitiesRetryCount(%1) elapsed(%2)").arg(_capabilitiesRetryCount).arg(_capabilitiesRetryElapsed.elapsed()); + sendMavCommand(MAV_COMP_ID_ALL, + MAV_CMD_REQUEST_AUTOPILOT_CAPABILITIES, + true, // Show error on failure + 1); // Request firmware version + } + } + } + switch (message.msgid) { case MAVLINK_MSG_ID_HOME_POSITION: _handleHomePosition(message); @@ -1308,7 +1330,7 @@ void Vehicle::_handleAltitude(mavlink_message_t& message) void Vehicle::_setCapabilities(uint64_t capabilityBits) { _capabilityBits = capabilityBits; - _vehicleCapabilitiesKnown = true; + _capabilityBitsKnown = true; emit capabilitiesKnownChanged(true); emit capabilityBitsChanged(_capabilityBits); @@ -1321,6 +1343,8 @@ void Vehicle::_setCapabilities(uint64_t capabilityBits) qCDebug(VehicleLog) << QString("Vehicle %1 GeoFence").arg(_capabilityBits & MAV_PROTOCOL_CAPABILITY_MISSION_FENCE ? supports : doesNotSupport); qCDebug(VehicleLog) << QString("Vehicle %1 RallyPoints").arg(_capabilityBits & MAV_PROTOCOL_CAPABILITY_MISSION_RALLY ? supports : doesNotSupport); qCDebug(VehicleLog) << QString("Vehicle %1 Terrain").arg(_capabilityBits & MAV_PROTOCOL_CAPABILITY_TERRAIN ? supports : doesNotSupport); + + _setMaxProtoVersionFromBothSources(); } void Vehicle::_handleAutopilotVersion(LinkInterface *link, mavlink_message_t& message) @@ -1383,8 +1407,9 @@ void Vehicle::_handleProtocolVersion(LinkInterface *link, mavlink_message_t& mes qCDebug(VehicleLog) << "_handleProtocolVersion" << protoVersion.max_version; + _mavlinkProtocolRequestMaxProtoVersion = protoVersion.max_version; _mavlinkProtocolRequestComplete = true; - _setMaxProtoVersion(protoVersion.max_version); + _setMaxProtoVersionFromBothSources(); _startPlanRequest(); } @@ -1398,6 +1423,19 @@ void Vehicle::_setMaxProtoVersion(unsigned version) { } } +void Vehicle::_setMaxProtoVersionFromBothSources() +{ + if (_mavlinkProtocolRequestComplete && _capabilityBitsKnown) { + if (_mavlinkProtocolRequestMaxProtoVersion != 0) { + qCDebug(VehicleLog) << "_setMaxProtoVersionFromBothSources using protocol version message"; + _setMaxProtoVersion(_mavlinkProtocolRequestMaxProtoVersion); + } else { + qCDebug(VehicleLog) << "_setMaxProtoVersionFromBothSources using capability bits"; + _setMaxProtoVersion(capabilityBits() & MAV_PROTOCOL_CAPABILITY_MAVLINK2 ? 200 : 100); + } + } +} + QString Vehicle::vehicleUIDStr() { QString uid; @@ -2572,19 +2610,19 @@ void Vehicle::sendMessageMultiple(mavlink_message_t message) void Vehicle::_missionManagerError(int errorCode, const QString& errorMsg) { Q_UNUSED(errorCode); - qgcApp()->showMessage(tr("Mission transfer failed. Retry transfer. Error: %1").arg(errorMsg)); + qgcApp()->showMessage(tr("Mission transfer failed. Error: %1").arg(errorMsg)); } void Vehicle::_geoFenceManagerError(int errorCode, const QString& errorMsg) { Q_UNUSED(errorCode); - qgcApp()->showMessage(tr("GeoFence transfer failed. Retry transfer. Error: %1").arg(errorMsg)); + qgcApp()->showMessage(tr("GeoFence transfer failed. Error: %1").arg(errorMsg)); } void Vehicle::_rallyPointManagerError(int errorCode, const QString& errorMsg) { Q_UNUSED(errorCode); - qgcApp()->showMessage(tr("Rally Point transfer failed. Retry transfer. Error: %1").arg(errorMsg)); + qgcApp()->showMessage(tr("Rally Point transfer failed. Error: %1").arg(errorMsg)); } void Vehicle::_clearCameraTriggerPoints() @@ -2621,7 +2659,7 @@ void Vehicle::_startPlanRequest() // - Parameter download is complete // - We know the vehicle capabilities // - We know the max mavlink protocol version - if (_parameterManager->parametersReady() && _vehicleCapabilitiesKnown && _mavlinkProtocolRequestComplete) { + if (_parameterManager->parametersReady() && _capabilityBitsKnown && _mavlinkProtocolRequestComplete) { qCDebug(VehicleLog) << "_startPlanRequest"; _missionManagerInitialRequestSent = true; if (_settingsManager->appSettings()->autoLoadMissions()->rawValue().toBool()) { @@ -2640,7 +2678,7 @@ void Vehicle::_startPlanRequest() } else { if (!_parameterManager->parametersReady()) { qCDebug(VehicleLog) << "Delaying _startPlanRequest due to parameters not ready"; - } else if (!_vehicleCapabilitiesKnown) { + } else if (!_capabilityBitsKnown) { qCDebug(VehicleLog) << "Delaying _startPlanRequest due to vehicle capabilities not known"; } else if (!_mavlinkProtocolRequestComplete) { qCDebug(VehicleLog) << "Delaying _startPlanRequest due to mavlink protocol request not complete"; @@ -3405,19 +3443,9 @@ void Vehicle::_handleUnsupportedRequestProtocolVersion() // We end up here if either the vehicle does not support the MAV_CMD_REQUEST_PROTOCOL_VERSION command or if // we never received an Ack back for the command. - // If the link isn't already running Mavlink V2 fall back to the capability bits to determine whether to use Mavlink V2 or not. - if (_maxProtoVersion == 0) { - if (capabilitiesKnown()) { - unsigned vehicleMaxProtoVersion = capabilityBits() & MAV_PROTOCOL_CAPABILITY_MAVLINK2 ? 200 : 100; - qCDebug(VehicleLog) << QStringLiteral("Setting _maxProtoVersion to %1 based on capabilitity bits since not yet set.").arg(vehicleMaxProtoVersion); - _setMaxProtoVersion(vehicleMaxProtoVersion); - } else { - qCDebug(VehicleLog) << "Waiting for capabilities to be known to set _maxProtoVersion"; - } - } else { - qCDebug(VehicleLog) << "Leaving _maxProtoVersion at current value" << _maxProtoVersion; - } + // _mavlinkProtocolRequestMaxProtoVersion stays at 0 to indicate unknown _mavlinkProtocolRequestComplete = true; + _setMaxProtoVersionFromBothSources(); // Determining protocol version is one of the triggers to possibly start downloading the plan _startPlanRequest(); @@ -3428,8 +3456,9 @@ void Vehicle::_protocolVersionTimeOut() // The PROTOCOL_VERSION message didn't make it through the pipe from Vehicle->QGC. // This means although the vehicle may support mavlink 2, the pipe does not. qCDebug(VehicleLog) << QStringLiteral("Setting _maxProtoVersion to 100 due to timeout on receiving PROTOCOL_VERSION message."); - _setMaxProtoVersion(100); + _mavlinkProtocolRequestMaxProtoVersion = 100; _mavlinkProtocolRequestComplete = true; + _setMaxProtoVersionFromBothSources(); _startPlanRequest(); } diff --git a/src/Vehicle/Vehicle.h b/src/Vehicle/Vehicle.h index e4f9191da5a8e61ce831fdab7747f30717c8bd3a..eb0d39b8f4f44922e75ce4c3a92b9614f465ebdb 100644 --- a/src/Vehicle/Vehicle.h +++ b/src/Vehicle/Vehicle.h @@ -12,6 +12,7 @@ #include #include #include +#include #include "FactGroup.h" #include "LinkInterface.h" @@ -1082,7 +1083,7 @@ public: const QVariantList& toolBarIndicators (); const QVariantList& staticCameraList () const; - bool capabilitiesKnown () const { return _vehicleCapabilitiesKnown; } + bool capabilitiesKnown () const { return _capabilityBitsKnown; } uint64_t capabilityBits () const { return _capabilityBits; } // Change signalled by capabilityBitsChanged QGCCameraManager* dynamicCameras () { return _cameras; } @@ -1100,7 +1101,8 @@ public: void _setFlying(bool flying); void _setLanding(bool landing); void _setHomePosition(QGeoCoordinate& homeCoord); - void _setMaxProtoVersion (unsigned version); + void _setMaxProtoVersion(unsigned version); + void _setMaxProtoVersionFromBothSources(); /// Vehicle is about to be deleted void prepareDelete(); @@ -1410,9 +1412,10 @@ private: uint32_t _telemetryTXBuffer; int _telemetryLNoise; int _telemetryRNoise; - bool _mavlinkProtocolRequestComplete; - unsigned _maxProtoVersion; - bool _vehicleCapabilitiesKnown; + bool _mavlinkProtocolRequestComplete = false; + unsigned _mavlinkProtocolRequestMaxProtoVersion = 0; + unsigned _maxProtoVersion = 0; + bool _capabilityBitsKnown = false; uint64_t _capabilityBits; bool _highLatencyLink; bool _receivingAttitudeQuaternion; @@ -1432,8 +1435,10 @@ private: QList _mavCommandQueue; QTimer _mavCommandAckTimer; int _mavCommandRetryCount; - static const int _mavCommandMaxRetryCount = 3; - static const int _mavCommandAckTimeoutMSecs = 3000; + int _capabilitiesRetryCount = 0; + QTime _capabilitiesRetryElapsed; + static const int _mavCommandMaxRetryCount = 3; + static const int _mavCommandAckTimeoutMSecs = 3000; static const int _mavCommandAckTimeoutMSecsHighLatency = 120000; QString _prearmError; diff --git a/src/VehicleSetup/FirmwareUpgrade.qml b/src/VehicleSetup/FirmwareUpgrade.qml index 178380924e179f71471dc129b4a4d998399b3a23..094b30b2fb39060ff6d8fa8f6d720bee69342745 100644 --- a/src/VehicleSetup/FirmwareUpgrade.qml +++ b/src/VehicleSetup/FirmwareUpgrade.qml @@ -383,8 +383,7 @@ SetupPage { anchors.right: parent.right visible: !px4Flow && apmFlightStack.checked && !controller.downloadingFirmwareList && controller.apmFirmwareNames.length !== 0 model: controller.apmFirmwareNames - - onModelChanged: console.log("model", model) + onModelChanged: currentIndex = controller.apmFirmwareNamesBestIndex } QGCLabel { diff --git a/src/VehicleSetup/FirmwareUpgradeController.cc b/src/VehicleSetup/FirmwareUpgradeController.cc index ae5ec5d012b428f06f4ff4bbad76e4399c67a5b0..e04fad68cb43ef7b6a185322eb1383d5291136cb 100644 --- a/src/VehicleSetup/FirmwareUpgradeController.cc +++ b/src/VehicleSetup/FirmwareUpgradeController.cc @@ -730,52 +730,47 @@ void FirmwareUpgradeController::_buildAPMFirmwareNames(void) QString boardDescription = _foundBoardInfo.description(); quint16 boardVID = _foundBoardInfo.vendorIdentifier(); quint16 boardPID = _foundBoardInfo.productIdentifier(); - -#if 0 - // This is left in for debugging manifest problems - boardDescription = "KakuteF7"; - boardVID = 1155; - boardPID = 22336; -#endif + uint32_t rawBoardId = _bootloaderBoardID == Bootloader::boardIDPX4FMUV3 ? Bootloader::boardIDPX4FMUV2 : _bootloaderBoardID; qCDebug(FirmwareUpgradeLog) << QStringLiteral("_buildAPMFirmwareNames description(%1) vid(%2/0x%3) pid(%4/0x%5)").arg(boardDescription).arg(boardVID).arg(boardVID, 1, 16).arg(boardPID).arg(boardPID, 1, 16); _apmFirmwareNames.clear(); + _apmFirmwareNamesBestIndex = -1; _apmFirmwareUrls.clear(); QString apmDescriptionSuffix("-BL"); + QString boardDescriptionPrefix; bool bootloaderMatch = boardDescription.endsWith(apmDescriptionSuffix); + int currentIndex = 0; for (const ManifestFirmwareInfo_t& firmwareInfo: _rgManifestFirmwareInfo) { bool match = false; - if (firmwareInfo.firmwareBuildType == _selectedFirmwareBuildType && firmwareInfo.chibios == chibios && firmwareInfo.vehicleType == vehicleType) { - if (bootloaderMatch) { - if (firmwareInfo.rgBootloaderPortString.contains(boardDescription)) { - qCDebug(FirmwareUpgradeLog) << "Bootloader match:" << firmwareInfo.friendlyName << boardDescription << firmwareInfo.rgBootloaderPortString << firmwareInfo.url << firmwareInfo.vehicleType; - match = true; - } + if (firmwareInfo.firmwareBuildType == _selectedFirmwareBuildType && firmwareInfo.chibios == chibios && firmwareInfo.vehicleType == vehicleType && firmwareInfo.boardId == rawBoardId) { + if (firmwareInfo.fmuv2 && _bootloaderBoardID == Bootloader::boardIDPX4FMUV3) { + qCDebug(FirmwareUpgradeLog) << "Skipping fmuv2 manifest entry for fmuv3 board:" << firmwareInfo.friendlyName << boardDescription << firmwareInfo.rgBootloaderPortString << firmwareInfo.url << firmwareInfo.vehicleType; } else { - if (firmwareInfo.rgVID.contains(boardVID) && firmwareInfo.rgPID.contains(boardPID)) { - qCDebug(FirmwareUpgradeLog) << "VID/PID match:" << firmwareInfo.friendlyName << boardVID << boardPID << _bootloaderBoardID << firmwareInfo.url << firmwareInfo.vehicleType; - match = true; + qCDebug(FirmwareUpgradeLog) << "Board id match:" << firmwareInfo.friendlyName << boardDescription << firmwareInfo.rgBootloaderPortString << firmwareInfo.url << firmwareInfo.vehicleType; + match = true; + if (bootloaderMatch && _apmFirmwareNamesBestIndex == -1 && firmwareInfo.rgBootloaderPortString.contains(boardDescription)) { + _apmFirmwareNamesBestIndex = currentIndex; + qCDebug(FirmwareUpgradeLog) << "Bootloader best match:" << firmwareInfo.friendlyName << boardDescription << firmwareInfo.rgBootloaderPortString << firmwareInfo.url << firmwareInfo.vehicleType; } } } - // Do a final filter on fmuv2/fmuv3 - if (match && _bootloaderBoardID == Bootloader::boardIDPX4FMUV3) { - match = !firmwareInfo.fmuv2; - } - if (match) { _apmFirmwareNames.append(firmwareInfo.friendlyName); _apmFirmwareUrls.append(firmwareInfo.url); + currentIndex++; } } - if (_apmFirmwareNames.count() > 1) { - _apmFirmwareNames.prepend(tr("Choose board type")); - _apmFirmwareUrls.prepend(QString()); + if (_apmFirmwareNamesBestIndex == -1) { + _apmFirmwareNamesBestIndex++; + if (_apmFirmwareNames.count() > 1) { + _apmFirmwareNames.prepend(tr("Choose board type")); + _apmFirmwareUrls.prepend(QString()); + } } emit apmFirmwareNamesChanged(); diff --git a/src/VehicleSetup/FirmwareUpgradeController.h b/src/VehicleSetup/FirmwareUpgradeController.h index 5a375a9ae4a8b10bfd1374863a9009638bab64dc..88f20f00af6b25dfa6c76167da346d7a2e396c91 100644 --- a/src/VehicleSetup/FirmwareUpgradeController.h +++ b/src/VehicleSetup/FirmwareUpgradeController.h @@ -94,6 +94,7 @@ public: Q_PROPERTY(bool px4FlowBoard READ px4FlowBoard NOTIFY boardFound) Q_PROPERTY(FirmwareBuildType_t selectedFirmwareBuildType READ selectedFirmwareBuildType WRITE setSelectedFirmwareBuildType NOTIFY selectedFirmwareBuildTypeChanged) Q_PROPERTY(QStringList apmFirmwareNames MEMBER _apmFirmwareNames NOTIFY apmFirmwareNamesChanged) + Q_PROPERTY(int apmFirmwareNamesBestIndex MEMBER _apmFirmwareNamesBestIndex NOTIFY apmFirmwareNamesChanged) Q_PROPERTY(QStringList apmFirmwareUrls MEMBER _apmFirmwareUrls NOTIFY apmFirmwareNamesChanged) Q_PROPERTY(QString px4StableVersion READ px4StableVersion NOTIFY px4StableVersionChanged) Q_PROPERTY(QString px4BetaVersion READ px4BetaVersion NOTIFY px4BetaVersionChanged) @@ -308,6 +309,7 @@ private: QMap _manifestMavFirmwareVersionTypeToFirmwareBuildTypeMap; QMap _manifestMavTypeToFirmwareVehicleTypeMap; QStringList _apmFirmwareNames; + int _apmFirmwareNamesBestIndex = 0; QStringList _apmFirmwareUrls; Fact* _apmChibiOSSetting; Fact* _apmVehicleTypeSetting; diff --git a/src/VideoStreaming/SubtitleWriter.cc b/src/VideoStreaming/SubtitleWriter.cc index 9a2a4908a171f50a1f6a96461de0e1cc5fd636bc..4e64e147fe494a29a24e644bd8f1d1edcb2423c4 100644 --- a/src/VideoStreaming/SubtitleWriter.cc +++ b/src/VideoStreaming/SubtitleWriter.cc @@ -15,9 +15,6 @@ */ #include "SubtitleWriter.h" -#include "SettingsManager.h" -#include "VideoReceiver.h" -#include "VideoManager.h" #include "QGCApplication.h" #include "QGCCorePlugin.h" #include @@ -31,48 +28,11 @@ const int SubtitleWriter::_sampleRate = 1; // Sample rate in Hz for getting tele SubtitleWriter::SubtitleWriter(QObject* parent) : QObject(parent) { + connect(&_timer, &QTimer::timeout, this, &SubtitleWriter::_captureTelemetry); } -void SubtitleWriter::setVideoReceiver(VideoReceiver* videoReceiver) +void SubtitleWriter::startCapturingTelemetry(const QString& videoFile) { - if(!videoReceiver) { - qCWarning(SubtitleWriterLog) << "Invalid VideoReceiver pointer! Aborting subtitle capture!"; - return; - } - _videoReceiver = videoReceiver; - -#if defined(QGC_GST_STREAMING) - // Only start writing subtitles once the recording pipeline actually starts - connect(_videoReceiver, &VideoReceiver::gotFirstRecordingKeyFrame, this, &SubtitleWriter::_startCapturingTelemetry); - - // Captures recordingChanged() signals to stop writing subtitles - connect(_videoReceiver, &VideoReceiver::recordingChanged, this, &SubtitleWriter::_onVideoRecordingChanged); -#endif - - // Timer for telemetry capture and writing to file - connect(&_timer, &QTimer::timeout, this, &SubtitleWriter::_captureTelemetry); -} - -void SubtitleWriter::_onVideoRecordingChanged() -{ -#if defined(QGC_GST_STREAMING) - // Stop capturing data if recording stopped - if(!_videoReceiver->recording()) { - qCDebug(SubtitleWriterLog) << "Stopping writing"; - _timer.stop(); - _file.close(); - } -#endif -} - -void SubtitleWriter::_startCapturingTelemetry() -{ - if(!_videoReceiver) { - qCWarning(SubtitleWriterLog) << "Invalid VideoReceiver pointer! Aborting subtitle capture!"; - _timer.stop(); - return; - } - // Get the facts displayed in the values widget and capture them, removing the "Vehicle." prefix. QSettings settings; settings.beginGroup("ValuesWidget"); @@ -81,8 +41,8 @@ void SubtitleWriter::_startCapturingTelemetry() _startTime = QDateTime::currentDateTime(); - QFileInfo videoFile(_videoReceiver->videoFile()); - QString subtitleFilePath = QStringLiteral("%1/%2.ass").arg(videoFile.path(), videoFile.completeBaseName()); + QFileInfo videoFileInfo(videoFile); + QString subtitleFilePath = QStringLiteral("%1/%2.ass").arg(videoFileInfo.path(), videoFileInfo.completeBaseName()); qCDebug(SubtitleWriterLog) << "Writing overlay to file:" << subtitleFilePath; _file.setFileName(subtitleFilePath); @@ -118,14 +78,17 @@ void SubtitleWriter::_startCapturingTelemetry() _timer.start(1000/_sampleRate); } -void SubtitleWriter::_captureTelemetry() +void SubtitleWriter::stopCapturingTelemetry() { - if(!_videoReceiver) { - qCWarning(SubtitleWriterLog) << "Invalid VideoReceiver pointer! Aborting subtitle capture!"; - _timer.stop(); - return; - } +#if defined(QGC_GST_STREAMING) + qCDebug(SubtitleWriterLog) << "Stopping writing"; + _timer.stop(); + _file.close(); +#endif +} +void SubtitleWriter::_captureTelemetry() +{ static const float nRows = 3; // number of rows used for displaying data static const int offsetFactor = 700; // Used to simulate a larger resolution and reduce the borders in the layout diff --git a/src/VideoStreaming/SubtitleWriter.h b/src/VideoStreaming/SubtitleWriter.h index 9d4047ff1fa56e8fdf0085b11169cf7b266335ed..000c3976520f0291613cbb859b27dfba74424d6a 100644 --- a/src/VideoStreaming/SubtitleWriter.h +++ b/src/VideoStreaming/SubtitleWriter.h @@ -17,7 +17,6 @@ #pragma once #include "QGCLoggingCategory.h" -#include "VideoReceiver.h" #include #include #include @@ -33,25 +32,19 @@ public: explicit SubtitleWriter(QObject* parent = nullptr); ~SubtitleWriter() = default; - void setVideoReceiver(VideoReceiver* videoReceiver); + // starts capturing vehicle telemetry. + void startCapturingTelemetry(const QString& videoFile); + void stopCapturingTelemetry(); private slots: - // Fires with every "videoRecordingChanged() signal, stops capturing telemetry if video stopped." - void _onVideoRecordingChanged(); - // Captures a snapshot of telemetry data from vehicle into the subtitles file. void _captureTelemetry(); - // starts capturing vehicle telemetry. - void _startCapturingTelemetry(); - private: QTimer _timer; QStringList _values; QDateTime _startTime; QFile _file; - VideoReceiver* _videoReceiver; - static const int _sampleRate; }; diff --git a/src/VideoStreaming/VideoManager.cc b/src/VideoStreaming/VideoManager.cc index 7fd2ef1368e948d1a6ac57c6898e1c5d9af2ce91..705efa24392dd784aff25e9b6871d4542a73b3c5 100644 --- a/src/VideoStreaming/VideoManager.cc +++ b/src/VideoStreaming/VideoManager.cc @@ -30,6 +30,12 @@ QGC_LOGGING_CATEGORY(VideoManagerLog, "VideoManagerLog") +static const char* kFileExtension[VideoReceiver::FILE_FORMAT_MAX - VideoReceiver::FILE_FORMAT_MIN] = { + "mkv", + "mov", + "mp4" +}; + //----------------------------------------------------------------------------- VideoManager::VideoManager(QGCApplication* app, QGCToolbox* toolbox) : QGCTool(app, toolbox) @@ -43,6 +49,17 @@ VideoManager::~VideoManager() _videoReceiver = nullptr; delete _thermalVideoReceiver; _thermalVideoReceiver = nullptr; +#if defined(QGC_GST_STREAMING) + if (_thermalVideoSink != nullptr) { + gst_object_unref(_thermalVideoSink); + _thermalVideoSink = nullptr; + } + + if (_videoSink != nullptr) { + gst_object_unref(_videoSink); + _videoSink = nullptr; + } +#endif } //----------------------------------------------------------------------------- @@ -53,6 +70,8 @@ VideoManager::setToolbox(QGCToolbox *toolbox) QQmlEngine::setObjectOwnership(this, QQmlEngine::CppOwnership); qmlRegisterUncreatableType ("QGroundControl.VideoManager", 1, 0, "VideoManager", "Reference only"); qmlRegisterUncreatableType("QGroundControl", 1, 0, "VideoReceiver","Reference only"); + + // TODO: Those connections should be Per Video, not per VideoManager. _videoSettings = toolbox->settingsManager()->videoSettings(); QString videoSource = _videoSettings->videoSource()->rawValue().toString(); connect(_videoSettings->videoSource(), &Fact::rawValueChanged, this, &VideoManager::_videoSourceChanged); @@ -73,10 +92,21 @@ VideoManager::setToolbox(QGCToolbox *toolbox) qCDebug(VideoManagerLog) << "New Video Source:" << videoSource; _videoReceiver = toolbox->corePlugin()->createVideoReceiver(this); _thermalVideoReceiver = toolbox->corePlugin()->createVideoReceiver(this); + + connect(_videoReceiver, &VideoReceiver::timeout, this, &VideoManager::_restartVideo); + connect(_videoReceiver, &VideoReceiver::streamingChanged, this, &VideoManager::_streamingChanged); + connect(_videoReceiver, &VideoReceiver::recordingStarted, this, &VideoManager::_recordingStarted); + connect(_videoReceiver, &VideoReceiver::recordingChanged, this, &VideoManager::_recordingChanged); + connect(_videoReceiver, &VideoReceiver::screenshotComplete, this, &VideoManager::_screenshotComplete); + + // FIXME: AV: I believe _thermalVideoReceiver should be handled just like _videoReceiver in terms of event + // and I expect that it will be changed during multiple video stream activity + connect(_thermalVideoReceiver, &VideoReceiver::timeout, this, &VideoManager::_restartVideo); + connect(_thermalVideoReceiver, &VideoReceiver::streamingChanged, this, &VideoManager::_streamingChanged); + _updateSettings(); if(isGStreamer()) { startVideo(); - _subtitleWriter.setVideoReceiver(_videoReceiver); } else { stopVideo(); } @@ -84,22 +114,159 @@ VideoManager::setToolbox(QGCToolbox *toolbox) #endif } +void VideoManager::_cleanupOldVideos() +{ +#if defined(QGC_GST_STREAMING) + //-- Only perform cleanup if storage limit is enabled + if(!_videoSettings->enableStorageLimit()->rawValue().toBool()) { + return; + } + QString savePath = qgcApp()->toolbox()->settingsManager()->appSettings()->videoSavePath(); + QDir videoDir = QDir(savePath); + videoDir.setFilter(QDir::Files | QDir::Readable | QDir::NoSymLinks | QDir::Writable); + videoDir.setSorting(QDir::Time); + + QStringList nameFilters; + + for(size_t i = 0; i < sizeof(kFileExtension) / sizeof(kFileExtension[0]); i += 1) { + nameFilters << QString("*.") + kFileExtension[i]; + } + + videoDir.setNameFilters(nameFilters); + //-- get the list of videos stored + QFileInfoList vidList = videoDir.entryInfoList(); + if(!vidList.isEmpty()) { + uint64_t total = 0; + //-- Settings are stored using MB + uint64_t maxSize = _videoSettings->maxVideoSize()->rawValue().toUInt() * 1024 * 1024; + //-- Compute total used storage + for(int i = 0; i < vidList.size(); i++) { + total += vidList[i].size(); + } + //-- Remove old movies until max size is satisfied. + while(total >= maxSize && !vidList.isEmpty()) { + total -= vidList.last().size(); + qCDebug(VideoReceiverLog) << "Removing old video file:" << vidList.last().filePath(); + QFile file (vidList.last().filePath()); + file.remove(); + vidList.removeLast(); + } + } +#endif +} + //----------------------------------------------------------------------------- void VideoManager::startVideo() { - if(_videoReceiver) _videoReceiver->start(); - if(_thermalVideoReceiver) _thermalVideoReceiver->start(); + if (qgcApp()->runningUnitTests()) { + return; + } + + if(!_videoSettings->streamEnabled()->rawValue().toBool() || !_videoSettings->streamConfigured()) { + qCDebug(VideoReceiverLog) << "Stream not enabled/configured"; + return; + } + +#if defined(QGC_GST_STREAMING) + const unsigned timeout = _videoSettings->rtspTimeout()->rawValue().toUInt(); + + if(_videoReceiver != nullptr) { + _videoReceiver->start(_videoUri, timeout); + if (_videoSink != nullptr) { + _videoReceiver->startDecoding(_videoSink); + } + } + + if(_thermalVideoReceiver != nullptr) { + _thermalVideoReceiver->start(_thermalVideoUri, timeout); + if (_thermalVideoSink != nullptr) { + _thermalVideoReceiver->startDecoding(_thermalVideoSink); + } + } +#endif } //----------------------------------------------------------------------------- void VideoManager::stopVideo() { + if (qgcApp()->runningUnitTests()) { + return; + } + if(_videoReceiver) _videoReceiver->stop(); if(_thermalVideoReceiver) _thermalVideoReceiver->stop(); } +void +VideoManager::startRecording(const QString& videoFile) +{ + if (qgcApp()->runningUnitTests()) { + return; + } + + if (!_videoReceiver) { + qgcApp()->showMessage(tr("Video receiver is not ready.")); + return; + } + + const VideoReceiver::FILE_FORMAT fileFormat = static_cast(_videoSettings->recordingFormat()->rawValue().toInt()); + + if(fileFormat < VideoReceiver::FILE_FORMAT_MIN || fileFormat >= VideoReceiver::FILE_FORMAT_MAX) { + qgcApp()->showMessage(tr("Invalid video format defined.")); + return; + } + + //-- Disk usage maintenance + _cleanupOldVideos(); + + QString savePath = qgcApp()->toolbox()->settingsManager()->appSettings()->videoSavePath(); + + if(savePath.isEmpty()) { + qgcApp()->showMessage(tr("Unabled to record video. Video save path must be specified in Settings.")); + return; + } + + _videoFile = savePath + "/" + + (videoFile.isEmpty() ? QDateTime::currentDateTime().toString("yyyy-MM-dd_hh.mm.ss") : videoFile) + + "." + kFileExtension[fileFormat - VideoReceiver::FILE_FORMAT_MIN]; + + _videoReceiver->startRecording(_videoFile, fileFormat); +} + +void +VideoManager::stopRecording() +{ + if (qgcApp()->runningUnitTests()) { + return; + } + + if (!_videoReceiver) { + return; + } + + _videoReceiver->stopRecording(); +} + +void +VideoManager::grabImage(const QString& imageFile) +{ + if (qgcApp()->runningUnitTests()) { + return; + } + + if (!_videoReceiver) { + return; + } + + _imageFile = imageFile; + + emit imageFileChanged(); + + _videoReceiver->takeScreenshot(_imageFile); +} + //----------------------------------------------------------------------------- double VideoManager::aspectRatio() { @@ -110,6 +277,7 @@ double VideoManager::aspectRatio() return pInfo->aspectRatio(); } } + // FIXME: AV: use _videoReceiver->videoSize() to calculate AR (if AR is not specified in the settings?) return _videoSettings->aspectRatio()->rawValue().toDouble(); } @@ -163,6 +331,13 @@ VideoManager::hasThermal() return false; } +//----------------------------------------------------------------------------- +QString +VideoManager::imageFile() +{ + return _imageFile; +} + //----------------------------------------------------------------------------- bool VideoManager::autoStreamConfigured() @@ -204,28 +379,28 @@ VideoManager::_videoSourceChanged() emit hasVideoChanged(); emit isGStreamerChanged(); emit isAutoStreamChanged(); - restartVideo(); + _restartVideo(); } //----------------------------------------------------------------------------- void VideoManager::_udpPortChanged() { - restartVideo(); + _restartVideo(); } //----------------------------------------------------------------------------- void VideoManager::_rtspUrlChanged() { - restartVideo(); + _restartVideo(); } //----------------------------------------------------------------------------- void VideoManager::_tcpUrlChanged() { - restartVideo(); + _restartVideo(); } //----------------------------------------------------------------------------- @@ -290,7 +465,7 @@ VideoManager::_makeVideoSink(gpointer widget) if ((sink = gst_element_factory_make("qgcvideosinkbin", nullptr)) != nullptr) { g_object_set(sink, "widget", widget, NULL); } else { - qCritical() << "VideoManager::_makeVideoSink() failed. Error with gst_element_factory_make('qgcvideosinkbin')"; + qCritical() << "gst_element_factory_make('qgcvideosinkbin') failed"; } return sink; @@ -305,24 +480,32 @@ VideoManager::_initVideo() QQuickItem* root = qgcApp()->mainRootWindow(); if (root == nullptr) { - qCDebug(VideoManagerLog) << "VideoManager::_makeVideoSink() failed. No root window"; + qCDebug(VideoManagerLog) << "mainRootWindow() failed. No root window"; return; } QQuickItem* widget = root->findChild("videoContent"); - if (widget != nullptr) { - _videoReceiver->setVideoSink(_makeVideoSink(widget)); + if (widget != nullptr && _videoReceiver != nullptr) { + if ((_videoSink = _makeVideoSink(widget)) != nullptr) { + _videoReceiver->startDecoding(_videoSink); + } else { + qCDebug(VideoManagerLog) << "_makeVideoSink() failed"; + } } else { - qCDebug(VideoManagerLog) << "VideoManager::_makeVideoSink() failed. 'videoContent' widget not found"; + qCDebug(VideoManagerLog) << "video receiver disabled"; } widget = root->findChild("thermalVideo"); - if (widget != nullptr) { - _thermalVideoReceiver->setVideoSink(_makeVideoSink(widget)); + if (widget != nullptr && _thermalVideoReceiver != nullptr) { + if ((_thermalVideoSink = _makeVideoSink(widget)) != nullptr) { + _thermalVideoReceiver->startDecoding(_thermalVideoSink); + } else { + qCDebug(VideoManagerLog) << "_makeVideoSink() failed"; + } } else { - qCDebug(VideoManagerLog) << "VideoManager::_makeVideoSink() failed. 'thermalVideo' widget not found"; + qCDebug(VideoManagerLog) << "thermal video receiver disabled"; } #endif } @@ -340,23 +523,23 @@ VideoManager::_updateSettings() qCDebug(VideoManagerLog) << "Configure primary stream: " << pInfo->uri(); switch(pInfo->type()) { case VIDEO_STREAM_TYPE_RTSP: - _videoReceiver->setUri(pInfo->uri()); + _setVideoUri(pInfo->uri()); _toolbox->settingsManager()->videoSettings()->videoSource()->setRawValue(VideoSettings::videoSourceRTSP); break; case VIDEO_STREAM_TYPE_TCP_MPEG: - _videoReceiver->setUri(pInfo->uri()); + _setVideoUri(pInfo->uri()); _toolbox->settingsManager()->videoSettings()->videoSource()->setRawValue(VideoSettings::videoSourceTCP); break; case VIDEO_STREAM_TYPE_RTPUDP: - _videoReceiver->setUri(QStringLiteral("udp://0.0.0.0:%1").arg(pInfo->uri())); + _setVideoUri(QStringLiteral("udp://0.0.0.0:%1").arg(pInfo->uri())); _toolbox->settingsManager()->videoSettings()->videoSource()->setRawValue(VideoSettings::videoSourceUDPH264); break; case VIDEO_STREAM_TYPE_MPEG_TS_H264: - _videoReceiver->setUri(QStringLiteral("mpegts://0.0.0.0:%1").arg(pInfo->uri())); + _setVideoUri(QStringLiteral("mpegts://0.0.0.0:%1").arg(pInfo->uri())); _toolbox->settingsManager()->videoSettings()->videoSource()->setRawValue(VideoSettings::videoSourceMPEGTS); break; default: - _videoReceiver->setUri(pInfo->uri()); + _setVideoUri(pInfo->uri()); break; } //-- Thermal stream (if any) @@ -366,16 +549,16 @@ VideoManager::_updateSettings() switch(pTinfo->type()) { case VIDEO_STREAM_TYPE_RTSP: case VIDEO_STREAM_TYPE_TCP_MPEG: - _thermalVideoReceiver->setUri(pTinfo->uri()); + _setThermalVideoUri(pTinfo->uri()); break; case VIDEO_STREAM_TYPE_RTPUDP: - _thermalVideoReceiver->setUri(QStringLiteral("udp://0.0.0.0:%1").arg(pTinfo->uri())); + _setThermalVideoUri(QStringLiteral("udp://0.0.0.0:%1").arg(pTinfo->uri())); break; case VIDEO_STREAM_TYPE_MPEG_TS_H264: - _thermalVideoReceiver->setUri(QStringLiteral("mpegts://0.0.0.0:%1").arg(pTinfo->uri())); + _setThermalVideoUri(QStringLiteral("mpegts://0.0.0.0:%1").arg(pTinfo->uri())); break; default: - _thermalVideoReceiver->setUri(pTinfo->uri()); + _setThermalVideoUri(pTinfo->uri()); break; } } @@ -384,20 +567,62 @@ VideoManager::_updateSettings() } QString source = _videoSettings->videoSource()->rawValue().toString(); if (source == VideoSettings::videoSourceUDPH264) - _videoReceiver->setUri(QStringLiteral("udp://0.0.0.0:%1").arg(_videoSettings->udpPort()->rawValue().toInt())); + _setVideoUri(QStringLiteral("udp://0.0.0.0:%1").arg(_videoSettings->udpPort()->rawValue().toInt())); else if (source == VideoSettings::videoSourceUDPH265) - _videoReceiver->setUri(QStringLiteral("udp265://0.0.0.0:%1").arg(_videoSettings->udpPort()->rawValue().toInt())); + _setVideoUri(QStringLiteral("udp265://0.0.0.0:%1").arg(_videoSettings->udpPort()->rawValue().toInt())); else if (source == VideoSettings::videoSourceMPEGTS) - _videoReceiver->setUri(QStringLiteral("mpegts://0.0.0.0:%1").arg(_videoSettings->udpPort()->rawValue().toInt())); + _setVideoUri(QStringLiteral("mpegts://0.0.0.0:%1").arg(_videoSettings->udpPort()->rawValue().toInt())); else if (source == VideoSettings::videoSourceRTSP) - _videoReceiver->setUri(_videoSettings->rtspUrl()->rawValue().toString()); + _setVideoUri(_videoSettings->rtspUrl()->rawValue().toString()); else if (source == VideoSettings::videoSourceTCP) - _videoReceiver->setUri(QStringLiteral("tcp://%1").arg(_videoSettings->tcpUrl()->rawValue().toString())); + _setVideoUri(QStringLiteral("tcp://%1").arg(_videoSettings->tcpUrl()->rawValue().toString())); +} + +void +VideoManager::_setVideoUri(const QString& uri) +{ +#if defined(QGC_GST_TAISYNC_ENABLED) && (defined(__android__) || defined(__ios__)) + //-- Taisync on iOS or Android sends a raw h.264 stream + if (isTaisync()) { + _videoUri = QString("tsusb://0.0.0.0:%1").arg(TAISYNC_VIDEO_UDP_PORT); + return; + } +#endif + _videoUri = uri; +} + +void +VideoManager::_setThermalVideoUri(const QString& uri) +{ +#if defined(QGC_GST_TAISYNC_ENABLED) && (defined(__android__) || defined(__ios__)) + //-- Taisync on iOS or Android sends a raw h.264 stream + if (isTaisync()) { + // FIXME: AV: TAISYNC_VIDEO_UDP_PORT is used by video stream, thermal stream should go via its own proxy + _thermalVideoUri = QString("tsusb://0.0.0.0:%1").arg(TAISYNC_VIDEO_UDP_PORT); + return; + } +#endif + _thermalVideoUri = uri; +} + +//----------------------------------------------------------------------------- +void +VideoManager::_streamingChanged() +{ +#if defined(QGC_GST_STREAMING) + // FIXME: AV: we need VideoReceiver::running() to avoid restarting if one of streams is not active + // but since VideoManager is going to be relpaced by Video Model during multiple video streaming development activity + // I'll leave it as is for week or two + if ((_videoReceiver && !_videoReceiver->streaming()) + /*|| (_thermalVideoReceiver && !_thermalVideoReceiver->streaming())*/) { + _restartVideo(); + } +#endif } //----------------------------------------------------------------------------- void -VideoManager::restartVideo() +VideoManager::_restartVideo() { #if defined(QGC_GST_STREAMING) qCDebug(VideoManagerLog) << "Restart video streaming"; @@ -408,6 +633,28 @@ VideoManager::restartVideo() #endif } +//----------------------------------------------------------------------------- +void +VideoManager::_recordingStarted() +{ + _subtitleWriter.startCapturingTelemetry(_videoFile); +} + +//----------------------------------------------------------------------------- +void +VideoManager::_recordingChanged() +{ + if (_videoReceiver && !_videoReceiver->recording()) { + _subtitleWriter.stopCapturingTelemetry(); + } +} + +//---------------------------------------------------------------------------------------- +void +VideoManager::_screenshotComplete() +{ +} + //---------------------------------------------------------------------------------------- void VideoManager::_setActiveVehicle(Vehicle* vehicle) @@ -419,14 +666,14 @@ VideoManager::_setActiveVehicle(Vehicle* vehicle) if(pCamera) { pCamera->stopStream(); } - disconnect(_activeVehicle->dynamicCameras(), &QGCCameraManager::streamChanged, this, &VideoManager::restartVideo); + disconnect(_activeVehicle->dynamicCameras(), &QGCCameraManager::streamChanged, this, &VideoManager::_restartVideo); } } _activeVehicle = vehicle; if(_activeVehicle) { connect(_activeVehicle, &Vehicle::connectionLostChanged, this, &VideoManager::_connectionLostChanged); if(_activeVehicle->dynamicCameras()) { - connect(_activeVehicle->dynamicCameras(), &QGCCameraManager::streamChanged, this, &VideoManager::restartVideo); + connect(_activeVehicle->dynamicCameras(), &QGCCameraManager::streamChanged, this, &VideoManager::_restartVideo); QGCCameraControl* pCamera = _activeVehicle->dynamicCameras()->currentCameraInstance(); if(pCamera) { pCamera->resumeStream(); @@ -437,7 +684,7 @@ VideoManager::_setActiveVehicle(Vehicle* vehicle) setfullScreen(false); } emit autoStreamConfiguredChanged(); - restartVideo(); + _restartVideo(); } //---------------------------------------------------------------------------------------- diff --git a/src/VideoStreaming/VideoManager.h b/src/VideoStreaming/VideoManager.h index f53f28223992f202e1fdb2d8b8c45bf7f6a1ee31..70c5d8a79d1dbc9f37117aa56e5130b3f98787c5 100644 --- a/src/VideoStreaming/VideoManager.h +++ b/src/VideoStreaming/VideoManager.h @@ -50,6 +50,7 @@ public: Q_PROPERTY(double thermalHfov READ thermalHfov NOTIFY aspectRatioChanged) Q_PROPERTY(bool autoStreamConfigured READ autoStreamConfigured NOTIFY autoStreamConfiguredChanged) Q_PROPERTY(bool hasThermal READ hasThermal NOTIFY aspectRatioChanged) + Q_PROPERTY(QString imageFile READ imageFile NOTIFY imageFileChanged) virtual bool hasVideo (); virtual bool isGStreamer (); @@ -62,7 +63,8 @@ public: virtual double thermalHfov (); virtual bool autoStreamConfigured(); virtual bool hasThermal (); - virtual void restartVideo (); + virtual QString imageFile (); + virtual VideoReceiver* videoReceiver () { return _videoReceiver; } virtual VideoReceiver* thermalVideoReceiver () { return _thermalVideoReceiver; } @@ -82,6 +84,11 @@ public: Q_INVOKABLE void startVideo (); Q_INVOKABLE void stopVideo (); + Q_INVOKABLE void startRecording (const QString& videoFile = QString()); + Q_INVOKABLE void stopRecording (); + + Q_INVOKABLE void grabImage(const QString& imageFile); + signals: void hasVideoChanged (); void isGStreamerChanged (); @@ -91,6 +98,7 @@ signals: void isTaisyncChanged (); void aspectRatioChanged (); void autoStreamConfiguredChanged(); + void imageFileChanged (); protected slots: void _videoSourceChanged (); @@ -110,13 +118,29 @@ protected: #endif void _initVideo (); void _updateSettings (); + void _setVideoUri (const QString& uri); + void _setThermalVideoUri (const QString& uri); + void _cleanupOldVideos (); + void _restartVideo (); + void _streamingChanged (); + void _recordingStarted (); + void _recordingChanged (); + void _screenshotComplete (); protected: + QString _videoFile; + QString _imageFile; SubtitleWriter _subtitleWriter; bool _isTaisync = false; VideoReceiver* _videoReceiver = nullptr; VideoReceiver* _thermalVideoReceiver = nullptr; +#if defined(QGC_GST_STREAMING) + GstElement* _videoSink = nullptr; + GstElement* _thermalVideoSink = nullptr; +#endif VideoSettings* _videoSettings = nullptr; + QString _videoUri; + QString _thermalVideoUri; QString _videoSourceID; bool _fullScreen = false; Vehicle* _activeVehicle = nullptr; diff --git a/src/VideoStreaming/VideoReceiver.cc b/src/VideoStreaming/VideoReceiver.cc index 36b3ef9b0b48737a3c890792dfdfab97bd76102b..da89ec18008c8770c78ba465c7575f221a55f31c 100644 --- a/src/VideoStreaming/VideoReceiver.cc +++ b/src/VideoStreaming/VideoReceiver.cc @@ -15,279 +15,570 @@ */ #include "VideoReceiver.h" -#include "SettingsManager.h" -#include "QGCApplication.h" -#include "VideoManager.h" -#ifdef QGC_GST_TAISYNC_ENABLED -#include "TaisyncHandler.h" -#endif + #include #include -#include #include #include QGC_LOGGING_CATEGORY(VideoReceiverLog, "VideoReceiverLog") -#if defined(QGC_GST_STREAMING) - -static const char* kVideoExtensions[] = -{ - "mkv", - "mov", - "mp4" -}; - -static const char* kVideoMuxes[] = -{ - "matroskamux", - "qtmux", - "mp4mux" -}; - -#define NUM_MUXES (sizeof(kVideoMuxes) / sizeof(char*)) - -#endif - +//----------------------------------------------------------------------------- +// Our pipeline look like this: +// +// +-->queue-->_decoderValve[-->_decoder-->_videoSink] +// | +// _source-->_tee +// | +// +-->queue-->_recorderValve[-->_fileSink] +// VideoReceiver::VideoReceiver(QObject* parent) - : QObject(parent) + : QThread(parent) #if defined(QGC_GST_STREAMING) - , _running(false) - , _recording(false) - , _streaming(false) - , _starting(false) - , _stopping(false) - , _stop(true) - , _sink(nullptr) + , _removingDecoder(false) + , _removingRecorder(false) + , _source(nullptr) , _tee(nullptr) - , _pipeline(nullptr) + , _decoderValve(nullptr) + , _recorderValve(nullptr) + , _decoder(nullptr) , _videoSink(nullptr) - , _lastFrameId(G_MAXUINT64) - , _lastFrameTime(0) - , _restart_time_ms(1389) + , _fileSink(nullptr) + , _pipeline(nullptr) + , _lastSourceFrameTime(0) + , _lastVideoFrameTime(0) + , _resetVideoSink(true) + , _videoSinkProbeId(0) , _udpReconnect_us(5000000) + , _shutdown(false) #endif - , _videoRunning(false) - , _showFullScreen(false) - , _videoSettings(nullptr) + , _streaming(false) + , _decoding(false) + , _recording(false) { - // FIXME: AV: temporal workaround to allow for Qt::QueuedConnection for gstreamer signals. Need to evaluate proper solution - perhaps QtGst will be helpful - moveToThread(qgcApp()->thread()); - _videoSettings = qgcApp()->toolbox()->settingsManager()->videoSettings(); #if defined(QGC_GST_STREAMING) - _restart_timer.setSingleShot(true); - connect(&_restart_timer, &QTimer::timeout, this, &VideoReceiver::_restart_timeout); - connect(this, &VideoReceiver::msgErrorReceived, this, &VideoReceiver::_handleError, Qt::QueuedConnection); - connect(this, &VideoReceiver::msgEOSReceived, this, &VideoReceiver::_handleEOS, Qt::QueuedConnection); - connect(this, &VideoReceiver::msgStateChangedReceived, this, &VideoReceiver::_handleStateChanged, Qt::QueuedConnection); - connect(&_frameTimer, &QTimer::timeout, this, &VideoReceiver::_updateTimer); - _frameTimer.start(1000); + QThread::start(); + connect(&_watchdogTimer, &QTimer::timeout, this, &VideoReceiver::_watchdog); + _watchdogTimer.start(1000); #endif } -VideoReceiver::~VideoReceiver() +VideoReceiver::~VideoReceiver(void) { #if defined(QGC_GST_STREAMING) stop(); - setVideoSink(nullptr); + _post([this](){ + _shutdown = true; + }); + QThread::wait(); #endif } -//----------------------------------------------------------------------------- void -VideoReceiver::grabImage(QString imageFile) +VideoReceiver::start(const QString& uri, unsigned timeout) { - _imageFile = imageFile; - emit imageFileChanged(); -} - -//----------------------------------------------------------------------------- #if defined(QGC_GST_STREAMING) -static void -newPadCB(GstElement* element, GstPad* pad, gpointer data) -{ - gchar* name = gst_pad_get_name(pad); - //g_print("A new pad %s was created\n", name); - GstCaps* p_caps = gst_pad_get_pad_template_caps (pad); - gchar* description = gst_caps_to_string(p_caps); - qCDebug(VideoReceiverLog) << p_caps << ", " << description; - g_free(description); - GstElement* sink = GST_ELEMENT(data); - if(gst_element_link_pads(element, name, sink, "sink") == false) - qCCritical(VideoReceiverLog) << "Failed to link elements\n"; - g_free(name); + if (!_isOurThread()) { + _post([this, uri, timeout]() { + start(uri, timeout); + }); + return; + } + + if(_pipeline) { + qCDebug(VideoReceiverLog) << "Already running!"; + return; + } + + if (uri.isEmpty()) { + qCDebug(VideoReceiverLog) << "Failed because URI is not specified"; + return; + } + + qCDebug(VideoReceiverLog) << "Starting"; + + _timeout = timeout; + + bool running = false; + bool pipelineUp = false; + + GstElement* decoderQueue = nullptr; + GstElement* recorderQueue = nullptr; + + do { + if((_tee = gst_element_factory_make("tee", nullptr)) == nullptr) { + qCCritical(VideoReceiverLog) << "gst_element_factory_make('tee') failed"; + break; + } + + GstPad* pad; + + if ((pad = gst_element_get_static_pad(_tee, "sink")) == nullptr) { + qCCritical(VideoReceiverLog) << "gst_element_get_static_pad() failed"; + break; + } + + _lastSourceFrameTime = 0; + + gst_pad_add_probe(pad, GST_PAD_PROBE_TYPE_BUFFER, _teeProbe, this, nullptr); + gst_object_unref(pad); + pad = nullptr; + + if((decoderQueue = gst_element_factory_make("queue", nullptr)) == nullptr) { + qCCritical(VideoReceiverLog) << "gst_element_factory_make('queue') failed"; + break; + } + + if((_decoderValve = gst_element_factory_make("valve", nullptr)) == nullptr) { + qCCritical(VideoReceiverLog) << "gst_element_factory_make('valve') failed"; + break; + } + + g_object_set(_decoderValve, "drop", TRUE, nullptr); + + if((recorderQueue = gst_element_factory_make("queue", nullptr)) == nullptr) { + qCCritical(VideoReceiverLog) << "gst_element_factory_make('queue') failed"; + break; + } + + if((_recorderValve = gst_element_factory_make("valve", nullptr)) == nullptr) { + qCCritical(VideoReceiverLog) << "gst_element_factory_make('valve') failed"; + break; + } + + g_object_set(_recorderValve, "drop", TRUE, nullptr); + + if ((_pipeline = gst_pipeline_new("receiver")) == nullptr) { + qCCritical(VideoReceiverLog) << "gst_pipeline_new() failed"; + break; + } + + g_object_set(_pipeline, "message-forward", TRUE, nullptr); + + if ((_source = _makeSource(uri)) == nullptr) { + qCCritical(VideoReceiverLog) << "_makeSource() failed"; + break; + } + + g_signal_connect(_source, "pad-added", G_CALLBACK(_onNewPad), this); + + gst_bin_add_many(GST_BIN(_pipeline), _source, _tee, decoderQueue, _decoderValve, recorderQueue, _recorderValve, nullptr); + + pipelineUp = true; + + if(!gst_element_link_many(_tee, decoderQueue, _decoderValve, nullptr)) { + qCCritical(VideoReceiverLog) << "Unable to link decoder queue"; + break; + } + + if(!gst_element_link_many(_tee, recorderQueue, _recorderValve, nullptr)) { + qCCritical(VideoReceiverLog) << "Unable to link recorder queue"; + break; + } + + GstBus* bus = nullptr; + + if ((bus = gst_pipeline_get_bus(GST_PIPELINE(_pipeline))) != nullptr) { + gst_bus_enable_sync_message_emission(bus); + g_signal_connect(bus, "sync-message", G_CALLBACK(_onBusMessage), this); + gst_object_unref(bus); + bus = nullptr; + } + + GST_DEBUG_BIN_TO_DOT_FILE(GST_BIN(_pipeline), GST_DEBUG_GRAPH_SHOW_ALL, "pipeline-initial"); + running = gst_element_set_state(_pipeline, GST_STATE_PLAYING) != GST_STATE_CHANGE_FAILURE; + } while(0); + + if (!running) { + qCCritical(VideoReceiverLog) << "Failed"; + + // In newer versions, the pipeline will clean up all references that are added to it + if (_pipeline != nullptr) { + gst_element_set_state(_pipeline, GST_STATE_NULL); + gst_object_unref(_pipeline); + _pipeline = nullptr; + } + + // If we failed before adding items to the pipeline, then clean up + if (!pipelineUp) { + if (_recorderValve != nullptr) { + gst_object_unref(_recorderValve); + _recorderValve = nullptr; + } + + if (recorderQueue != nullptr) { + gst_object_unref(recorderQueue); + recorderQueue = nullptr; + } + + if (_decoderValve != nullptr) { + gst_object_unref(_decoderValve); + _decoderValve = nullptr; + } + + if (decoderQueue != nullptr) { + gst_object_unref(decoderQueue); + decoderQueue = nullptr; + } + + if (_tee != nullptr) { + gst_object_unref(_tee); + _tee = nullptr; + } + + if (_source != nullptr) { + gst_object_unref(_source); + _source = nullptr; + } + } + } else { + GST_DEBUG_BIN_TO_DOT_FILE(GST_BIN(_pipeline), GST_DEBUG_GRAPH_SHOW_ALL, "pipeline-started"); + qCDebug(VideoReceiverLog) << "Started"; + } +#else + Q_UNUSED(uri); + Q_UNUSED(timeout); +#endif } -static gboolean -autoplugQueryCaps(GstElement* bin, GstPad* pad, GstElement* element, GstQuery* query, gpointer data) +void +VideoReceiver::stop(void) { - GstElement* glupload = (GstElement* )data; +#if defined(QGC_GST_STREAMING) + if (!_isOurThread()) { + _post([this]() { + stop(); + }); + return; + } - GstPad* sinkpad = gst_element_get_static_pad(glupload, "sink"); + qCDebug(VideoReceiverLog) << "Stopping"; - if (!sinkpad) { - qCCritical(VideoReceiverLog) << "No sink pad found"; - return FALSE; - } + if (_pipeline != nullptr) { + GstBus* bus; - GstCaps* filter; + if ((bus = gst_pipeline_get_bus(GST_PIPELINE(_pipeline))) != nullptr) { + gst_bus_disable_sync_message_emission(bus); - gst_query_parse_caps(query, &filter); + gst_element_send_event(_pipeline, gst_event_new_eos()); - GstCaps* sinkcaps = gst_pad_query_caps(sinkpad, filter); + GstMessage* msg; - gst_query_set_caps_result(query, sinkcaps); + if((msg = gst_bus_timed_pop_filtered(bus, GST_CLOCK_TIME_NONE, (GstMessageType)(GST_MESSAGE_EOS|GST_MESSAGE_ERROR))) != nullptr) { + if(GST_MESSAGE_TYPE(msg) == GST_MESSAGE_ERROR) { + qCCritical(VideoReceiverLog) << "Error stopping pipeline!"; + } else if(GST_MESSAGE_TYPE(msg) == GST_MESSAGE_EOS) { + qCDebug(VideoReceiverLog) << "End of stream received!"; + } - const gboolean ret = !gst_caps_is_empty(sinkcaps); + gst_message_unref(msg); + msg = nullptr; + } else { + qCCritical(VideoReceiverLog) << "gst_bus_timed_pop_filtered() failed"; + } - gst_caps_unref(sinkcaps); - sinkcaps = nullptr; + gst_object_unref(bus); + bus = nullptr; + } else { + qCCritical(VideoReceiverLog) << "gst_pipeline_get_bus() failed"; + } - gst_object_unref(sinkpad); - sinkpad = nullptr; + gst_element_set_state(_pipeline, GST_STATE_NULL); - return ret; + // FIXME: check if branch is connected and remove all elements from branch + if (_fileSink != nullptr) { + _shutdownRecordingBranch(); + } + + if (_videoSink != nullptr) { + _shutdownDecodingBranch(); + } + + GST_DEBUG_BIN_TO_DOT_FILE(GST_BIN(_pipeline), GST_DEBUG_GRAPH_SHOW_ALL, "pipeline-stopped"); + + gst_object_unref(_pipeline); + _pipeline = nullptr; + + _recorderValve = nullptr; + _decoderValve = nullptr; + _tee = nullptr; + _source = nullptr; + + _lastSourceFrameTime = 0; + + if (_streaming) { + _streaming = false; + emit streamingChanged(); + qCDebug(VideoReceiverLog) << "Streaming stopped"; + } + } + + qCDebug(VideoReceiverLog) << "Stopped"; +#endif } -static gboolean -autoplugQueryContext(GstElement* bin, GstPad* pad, GstElement* element, GstQuery* query, gpointer data) +void +VideoReceiver::startDecoding(VideoSink* videoSink) { - GstElement* glsink = (GstElement* )data; +#if defined(QGC_GST_STREAMING) + if (!_isOurThread()) { + gst_object_ref(videoSink); + _post([this, videoSink]() { + startDecoding(videoSink); + gst_object_unref(videoSink); + }); + return; + } - GstPad* sinkpad = gst_element_get_static_pad(glsink, "sink"); + qCDebug(VideoReceiverLog) << "Starting decoding"; - if (!sinkpad){ - qCCritical(VideoReceiverLog) << "No sink pad found"; - return FALSE; + if (_pipeline == nullptr) { + if (_videoSink != nullptr) { + gst_object_unref(_videoSink); + _videoSink = nullptr; + } } - const gboolean ret = gst_pad_query(sinkpad, query); + if(_videoSink != nullptr || _decoding) { + qCDebug(VideoReceiverLog) << "Already decoding!"; + return; + } - gst_object_unref(sinkpad); - sinkpad = nullptr; + GstPad* pad; - return ret; + if ((pad = gst_element_get_static_pad(videoSink, "sink")) == nullptr) { + qCCritical(VideoReceiverLog) << "Unable to find sink pad of video sink"; + return; + } + + _lastVideoFrameTime = 0; + _resetVideoSink = true; + + _videoSinkProbeId = gst_pad_add_probe(pad, GST_PAD_PROBE_TYPE_BUFFER, _videoSinkProbe, this, nullptr); + gst_object_unref(pad); + pad = nullptr; + + _videoSink = videoSink; + gst_object_ref(_videoSink); + + _removingDecoder = false; + + if (!_streaming) { + return; + } + + if (!_addDecoder(_decoderValve)) { + qCCritical(VideoReceiverLog) << "_addDecoder() failed"; + return; + } + + g_object_set(_decoderValve, "drop", FALSE, nullptr); + + qCDebug(VideoReceiverLog) << "Decoding started"; +#else + Q_UNUSED(videoSink) +#endif } -static gboolean -autoplugQueryCB(GstElement* bin, GstPad* pad, GstElement* element, GstQuery* query, gpointer data) +void +VideoReceiver::stopDecoding(void) { - gboolean ret; +#if defined(QGC_GST_STREAMING) + if (!_isOurThread()) { + _post([this]() { + stopDecoding(); + }); + return; + } - switch (GST_QUERY_TYPE (query)) { - case GST_QUERY_CAPS: - ret = autoplugQueryCaps(bin, pad, element, query, data); - break; - case GST_QUERY_CONTEXT: - ret = autoplugQueryContext(bin, pad, element, query, data); - break; - default: - ret = FALSE; - break; + qCDebug(VideoReceiverLog) << "Stopping decoding"; + + // exit immediately if we are not decoding + if (_pipeline == nullptr || !_decoding) { + qCDebug(VideoReceiverLog) << "Not decoding!"; + return; } - return ret; + g_object_set(_decoderValve, "drop", TRUE, nullptr); + + _removingDecoder = true; + + _unlinkBranch(_decoderValve); +#endif } -//----------------------------------------------------------------------------- -static void -_wrapWithGhostPad(GstElement* element, GstPad* pad, gpointer data) +void +VideoReceiver::startRecording(const QString& videoFile, FILE_FORMAT format) { - gchar* name = gst_pad_get_name(pad); +#if defined(QGC_GST_STREAMING) + if (!_isOurThread()) { + _post([this, videoFile, format]() { + startRecording(videoFile, format); + }); + return; + } - GstPad* ghostpad = gst_ghost_pad_new(name, pad); + qCDebug(VideoReceiverLog) << "Starting recording"; - g_free(name); + // exit immediately if we are already recording + if (_pipeline == nullptr || _recording) { + qCDebug(VideoReceiverLog) << "Already recording!"; + return; + } - gst_pad_set_active(ghostpad, TRUE); + qCDebug(VideoReceiverLog) << "New video file:" << videoFile; - if (!gst_element_add_pad(GST_ELEMENT_PARENT(element), ghostpad)) { - qCCritical(VideoReceiverLog) << "Failed to add ghost pad to source"; + if ((_fileSink = _makeFileSink(videoFile, format)) == nullptr) { + qCCritical(VideoReceiverLog) << "_makeFileSink() failed"; + return; } -} -static void -_linkPadWithOptionalBuffer(GstElement* element, GstPad* pad, gpointer data) -{ - gboolean isRtpPad = FALSE; + _removingRecorder = false; - GstCaps* filter = gst_caps_from_string("application/x-rtp"); + gst_object_ref(_fileSink); - if (filter != nullptr) { - GstCaps* caps = gst_pad_query_caps(pad, nullptr); + gst_bin_add(GST_BIN(_pipeline), _fileSink); - if (caps != nullptr) { - if (!gst_caps_is_any(caps) && gst_caps_can_intersect(caps, filter)) { - isRtpPad = TRUE; - } - gst_caps_unref(caps); - caps = nullptr; - } + if (!gst_element_link(_recorderValve, _fileSink)) { + qCCritical(VideoReceiverLog) << "Failed to link valve and file sink"; + return; + } - gst_caps_unref(filter); - filter = nullptr; + gst_element_sync_state_with_parent(_fileSink); + + GST_DEBUG_BIN_TO_DOT_FILE(GST_BIN(_pipeline), GST_DEBUG_GRAPH_SHOW_ALL, "pipeline-with-filesink"); + + // Install a probe on the recording branch to drop buffers until we hit our first keyframe + // When we hit our first keyframe, we can offset the timestamps appropriately according to the first keyframe time + // This will ensure the first frame is a keyframe at t=0, and decoding can begin immediately on playback + GstPad* probepad; + + if ((probepad = gst_element_get_static_pad(_recorderValve, "src")) == nullptr) { + qCCritical(VideoReceiverLog) << "gst_element_get_static_pad() failed"; + return; } - if (isRtpPad) { - GstElement* buffer; + gst_pad_add_probe(probepad, GST_PAD_PROBE_TYPE_BUFFER, _keyframeWatch, this, nullptr); // to drop the buffers until key frame is received + gst_object_unref(probepad); + probepad = nullptr; - if ((buffer = gst_element_factory_make("rtpjitterbuffer", nullptr)) != nullptr) { - gst_bin_add(GST_BIN(GST_ELEMENT_PARENT(element)), buffer); + g_object_set(_recorderValve, "drop", FALSE, nullptr); - gst_element_sync_state_with_parent(buffer); + _recording = true; - GstPad* sinkpad = gst_element_get_static_pad(buffer, "sink"); + emit recordingChanged(); - if (sinkpad != nullptr) { - const GstPadLinkReturn ret = gst_pad_link(pad, sinkpad); + qCDebug(VideoReceiverLog) << "Recording started"; +#else + Q_UNUSED(videoFile) + Q_UNUSED(format) +#endif +} - gst_object_unref(sinkpad); - sinkpad = nullptr; +//----------------------------------------------------------------------------- +void +VideoReceiver::stopRecording(void) +{ +#if defined(QGC_GST_STREAMING) + if (!_isOurThread()) { + _post([this]() { + stopRecording(); + }); + return; + } - if (ret == GST_PAD_LINK_OK) { - pad = gst_element_get_static_pad(buffer, "src"); - element = buffer; - } else { - qCDebug(VideoReceiverLog) << "Partially failed - gst_pad_link()"; - } - } else { - qCDebug(VideoReceiverLog) << "Partially failed - gst_element_get_static_pad()"; - } - } else { - qCDebug(VideoReceiverLog) << "Partially failed - gst_element_factory_make('rtpjitterbuffer')"; - } + qCDebug(VideoReceiverLog) << "Stopping recording"; + + // exit immediately if we are not recording + if (_pipeline == nullptr || !_recording) { + qCDebug(VideoReceiverLog) << "Not recording!"; + return; } - newPadCB(element, pad, data); + g_object_set(_recorderValve, "drop", TRUE, nullptr); + + _removingRecorder = true; + + _unlinkBranch(_recorderValve); +#endif } -static gboolean -_padProbe(GstElement* element, GstPad* pad, gpointer user_data) +void +VideoReceiver::takeScreenshot(const QString& imageFile) { - int* probeRes = (int*)user_data; +#if defined(QGC_GST_STREAMING) + if (!_isOurThread()) { + _post([this, imageFile]() { + takeScreenshot(imageFile); + }); + return; + } - *probeRes |= 1; + // FIXME: AV: record screenshot here + emit screenshotComplete(); +#else + Q_UNUSED(imageFile); +#endif +} - GstCaps* filter = gst_caps_from_string("application/x-rtp"); +#if defined(QGC_GST_STREAMING) +const char* VideoReceiver::_kFileMux[FILE_FORMAT_MAX - FILE_FORMAT_MIN] = { + "matroskamux", + "qtmux", + "mp4mux" +}; - if (filter != nullptr) { - GstCaps* caps = gst_pad_query_caps(pad, nullptr); +void +VideoReceiver::_watchdog(void) +{ + _post([this](){ + if(_pipeline == nullptr) { + return; + } - if (caps != nullptr) { - if (!gst_caps_is_any(caps) && gst_caps_can_intersect(caps, filter)) { - *probeRes |= 2; + const qint64 now = QDateTime::currentSecsSinceEpoch(); + + if (_lastSourceFrameTime == 0) { + _lastSourceFrameTime = now; + } + + if (now - _lastSourceFrameTime > _timeout) { + emit timeout(); + } + + if (_decoding && !_removingDecoder) { + if (_lastVideoFrameTime == 0) { + _lastVideoFrameTime = now; } - gst_caps_unref(caps); - caps = nullptr; + if (now - _lastVideoFrameTime > _timeout * 2) { + emit timeout(); + } } + }); +} - gst_caps_unref(filter); - filter = nullptr; +void +VideoReceiver::_handleEOS(void) +{ + if(_pipeline == nullptr) { + qCWarning(VideoReceiverLog) << "We should not be here"; + return; } - return TRUE; + if (!_streaming) { + stop(); + } else { + if(_decoding && _removingDecoder) { + _shutdownDecodingBranch(); + } else if(_recording && _removingRecorder) { + _shutdownRecordingBranch(); + } else { + qCWarning(VideoReceiverLog) << "Unexpected EOS!"; + stop(); + } + } } GstElement* @@ -307,6 +598,7 @@ VideoReceiver::_makeSource(const QString& uri) GstElement* source = nullptr; GstElement* buffer = nullptr; + GstElement* tsdemux = nullptr; GstElement* parser = nullptr; GstElement* bin = nullptr; GstElement* srcbin = nullptr; @@ -333,7 +625,7 @@ VideoReceiver::_makeSource(const QString& uri) qCCritical(VideoReceiverLog) << "gst_caps_from_string() failed"; break; } - } else if (isUdp264) { + } else if (isUdp265) { if ((caps = gst_caps_from_string("application/x-rtp, media=(string)video, clock-rate=(int)90000, encoding-name=(string)H265")) == nullptr) { qCCritical(VideoReceiverLog) << "gst_caps_from_string() failed"; break; @@ -355,26 +647,37 @@ VideoReceiver::_makeSource(const QString& uri) break; } + if ((bin = gst_bin_new("sourcebin")) == nullptr) { + qCCritical(VideoReceiverLog) << "gst_bin_new('sourcebin') failed"; + break; + } + + if ((parser = gst_element_factory_make("parsebin", "parser")) == nullptr) { + qCCritical(VideoReceiverLog) << "gst_element_factory_make('parsebin') failed"; + break; + } + + gst_bin_add_many(GST_BIN(bin), source, parser, nullptr); + // FIXME: AV: Android does not determine MPEG2-TS via parsebin - have to explicitly state which demux to use + // FIXME: AV: tsdemux handling is a bit ugly - let's try to find elegant solution for that later if (isTcpMPEGTS || isUdpMPEGTS) { - if ((parser = gst_element_factory_make("tsdemux", "parser")) == nullptr) { - qCritical(VideoReceiverLog) << "gst_element_factory_make('tsdemux') failed"; + if ((tsdemux = gst_element_factory_make("tsdemux", nullptr)) == nullptr) { + qCCritical(VideoReceiverLog) << "gst_element_factory_make('tsdemux') failed"; break; } - } else { - if ((parser = gst_element_factory_make("parsebin", "parser")) == nullptr) { - qCritical() << "VideoReceiver::_makeSource() failed. Error with gst_element_factory_make('parsebin')"; + + gst_bin_add(GST_BIN(bin), tsdemux); + + if (!gst_element_link(source, tsdemux)) { + qCCritical(VideoReceiverLog) << "gst_element_link() failed"; break; } - } - if ((bin = gst_bin_new("sourcebin")) == nullptr) { - qCCritical(VideoReceiverLog) << "gst_bin_new('sourcebin') failed"; - break; + source = tsdemux; + tsdemux = nullptr; } - gst_bin_add_many(GST_BIN(bin), source, parser, nullptr); - int probeRes = 0; gst_element_foreach_src_pad(source, _padProbe, &probeRes); @@ -402,318 +705,456 @@ VideoReceiver::_makeSource(const QString& uri) g_signal_connect(source, "pad-added", G_CALLBACK(_linkPadWithOptionalBuffer), parser); } - g_signal_connect(parser, "pad-added", G_CALLBACK(_wrapWithGhostPad), nullptr); - - source = buffer = parser = nullptr; - - srcbin = bin; + g_signal_connect(parser, "pad-added", G_CALLBACK(_wrapWithGhostPad), nullptr); + + source = tsdemux = buffer = parser = nullptr; + + srcbin = bin; + bin = nullptr; + } while(0); + + if (bin != nullptr) { + gst_object_unref(bin); + bin = nullptr; + } + + if (parser != nullptr) { + gst_object_unref(parser); + parser = nullptr; + } + + if (tsdemux != nullptr) { + gst_object_unref(tsdemux); + tsdemux = nullptr; + } + + if (buffer != nullptr) { + gst_object_unref(buffer); + buffer = nullptr; + } + + if (source != nullptr) { + gst_object_unref(source); + source = nullptr; + } + + return srcbin; +} + +GstElement* +VideoReceiver::_makeDecoder(GstCaps* caps, GstElement* videoSink) +{ + GstElement* decoder = nullptr; + + do { + if ((decoder = gst_element_factory_make("decodebin", nullptr)) == nullptr) { + qCCritical(VideoReceiverLog) << "gst_element_factory_make('decodebin') failed"; + break; + } + + g_signal_connect(decoder, "autoplug-query", G_CALLBACK(_autoplugQuery), videoSink); + } while(0); + + return decoder; +} + +GstElement* +VideoReceiver::_makeFileSink(const QString& videoFile, FILE_FORMAT format) +{ + GstElement* fileSink = nullptr; + GstElement* mux = nullptr; + GstElement* sink = nullptr; + GstElement* bin = nullptr; + bool releaseElements = true; + + do{ + if (format < FILE_FORMAT_MIN || format >= FILE_FORMAT_MAX) { + qCCritical(VideoReceiverLog) << "Unsupported file format"; + break; + } + + if ((mux = gst_element_factory_make(_kFileMux[format - FILE_FORMAT_MIN], nullptr)) == nullptr) { + qCCritical(VideoReceiverLog) << "gst_element_factory_make('" << _kFileMux[format - FILE_FORMAT_MIN] << "') failed"; + break; + } + + if ((sink = gst_element_factory_make("filesink", nullptr)) == nullptr) { + qCCritical(VideoReceiverLog) << "gst_element_factory_make('filesink') failed"; + break; + } + + g_object_set(static_cast(sink), "location", qPrintable(videoFile), nullptr); + + if ((bin = gst_bin_new("sinkbin")) == nullptr) { + qCCritical(VideoReceiverLog) << "gst_bin_new('sinkbin') failed"; + break; + } + + GstPadTemplate* padTemplate; + + if ((padTemplate = gst_element_class_get_pad_template(GST_ELEMENT_GET_CLASS(mux), "video_%u")) == nullptr) { + qCCritical(VideoReceiverLog) << "gst_element_class_get_pad_template(mux) failed"; + break; + } + + // FIXME: AV: pad handling is potentially leaking (and other similar places too!) + GstPad* pad; + + if ((pad = gst_element_request_pad(mux, padTemplate, nullptr, nullptr)) == nullptr) { + qCCritical(VideoReceiverLog) << "gst_element_request_pad(mux) failed"; + break; + } + + gst_bin_add_many(GST_BIN(bin), mux, sink, nullptr); + + releaseElements = false; + + GstPad* ghostpad = gst_ghost_pad_new("sink", pad); + + gst_element_add_pad(bin, ghostpad); + + gst_object_unref(pad); + pad = nullptr; + + if (!gst_element_link(mux, sink)) { + qCCritical(VideoReceiverLog) << "gst_element_link() failed"; + break; + } + + fileSink = bin; bin = nullptr; } while(0); + if (releaseElements) { + if (sink != nullptr) { + gst_object_unref(sink); + sink = nullptr; + } + + if (mux != nullptr) { + gst_object_unref(mux); + mux = nullptr; + } + } + if (bin != nullptr) { gst_object_unref(bin); bin = nullptr; } - if (parser != nullptr) { - gst_object_unref(parser); - parser = nullptr; + return fileSink; +} + +void +VideoReceiver::_onNewSourcePad(GstPad* pad) +{ + // FIXME: check for caps - if this is not video stream (and preferably - one of these which we have to support) then simply skip it + if(!gst_element_link(_source, _tee)) { + qCCritical(VideoReceiverLog) << "Unable to link source"; + return; } - if (buffer != nullptr) { - gst_object_unref(buffer); - buffer = nullptr; + if (!_streaming) { + _streaming = true; + qCDebug(VideoReceiverLog) << "Streaming started"; + emit streamingChanged(); } - if (source != nullptr) { - gst_object_unref(source); - source = nullptr; + gst_pad_add_probe(pad, GST_PAD_PROBE_TYPE_EVENT_DOWNSTREAM, _eosProbe, this, nullptr); + + if (_videoSink == nullptr) { + return; } - return srcbin; + GST_DEBUG_BIN_TO_DOT_FILE(GST_BIN(_pipeline), GST_DEBUG_GRAPH_SHOW_ALL, "pipeline-with-new-source-pad"); + + if (!_addDecoder(_decoderValve)) { + qCCritical(VideoReceiverLog) << "_addDecoder() failed"; + return; + } + + g_object_set(_decoderValve, "drop", FALSE, nullptr); + + qCDebug(VideoReceiverLog) << "Decoding started"; } -//----------------------------------------------------------------------------- void -VideoReceiver::_restart_timeout() +VideoReceiver::_onNewDecoderPad(GstPad* pad) { - qgcApp()->toolbox()->videoManager()->restartVideo(); + GST_DEBUG_BIN_TO_DOT_FILE(GST_BIN(_pipeline), GST_DEBUG_GRAPH_SHOW_ALL, "pipeline-with-new-decoder-pad"); + + if (!_addVideoSink(pad)) { + qCCritical(VideoReceiverLog) << "_addVideoSink() failed"; + } } -#endif -//----------------------------------------------------------------------------- -// When we finish our pipeline will look like this: -// -// +-->queue-->decoder-->_videosink -// | -// datasource-->demux-->parser-->tee -// ^ -// | -// +-Here we will later link elements for recording -void -VideoReceiver::start() +bool +VideoReceiver::_addDecoder(GstElement* src) { - qCDebug(VideoReceiverLog) << "Starting " << _uri; - if(qgcApp()->runningUnitTests()) { - return; - } - if(!_videoSettings->streamEnabled()->rawValue().toBool() || - !_videoSettings->streamConfigured()) { - qCDebug(VideoReceiverLog) << "Stream not enabled/configured"; - return; - } + GstPad* srcpad; -#if defined(QGC_GST_STREAMING) - _stop = false; + if ((srcpad = gst_element_get_static_pad(src, "src")) == nullptr) { + qCCritical(VideoReceiverLog) << "gst_element_get_static_pad() failed"; + return false; + } - QString uri = _uri; + GstCaps* caps; -#if defined(QGC_GST_TAISYNC_ENABLED) && (defined(__android__) || defined(__ios__)) - //-- Taisync on iOS or Android sends a raw h.264 stream - if (qgcApp()->toolbox()->videoManager()->isTaisync()) { - uri = QString("tsusb://0.0.0.0:%1").arg(TAISYNC_VIDEO_UDP_PORT); + if ((caps = gst_pad_query_caps(srcpad, nullptr)) == nullptr) { + qCCritical(VideoReceiverLog) << "gst_pad_query_caps() failed"; + gst_object_unref(srcpad); + srcpad = nullptr; + return false; } -#endif - if (uri.isEmpty()) { - qCDebug(VideoReceiverLog) << "Failed because URI is not specified"; - return; - } + gst_object_unref(srcpad); + srcpad = nullptr; - if (_videoSink == nullptr) { - qCWarning(VideoReceiverLog) << "Failed because video sink is not set"; - return; - } - if(_running) { - qCDebug(VideoReceiverLog) << "Already running!"; - return; + if ((_decoder = _makeDecoder(caps, _videoSink)) == nullptr) { + qCCritical(VideoReceiverLog) << "_makeDecoder() failed"; + gst_caps_unref(caps); + caps = nullptr; + return false; } - _starting = true; + gst_object_ref(_decoder); - _lastFrameId = G_MAXUINT64; - _lastFrameTime = 0; + gst_caps_unref(caps); + caps = nullptr; - bool running = false; - bool pipelineUp = false; + // FIXME: AV: check if srcpad exists - if it does then no need to wait for new pad + // int probeRes = 0; + // gst_element_foreach_src_pad(source, _padProbe, &probeRes); + g_signal_connect(_decoder, "pad-added", G_CALLBACK(_onNewPad), this); - GstElement* source = nullptr; - GstElement* queue = nullptr; - GstElement* decoder = nullptr; + gst_bin_add(GST_BIN(_pipeline), _decoder); - do { - if ((_pipeline = gst_pipeline_new("receiver")) == nullptr) { - qCCritical(VideoReceiverLog) << "gst_pipeline_new() failed"; - break; - } + gst_element_sync_state_with_parent(_decoder); - g_object_set(_pipeline, "message-forward", TRUE, nullptr); + GST_DEBUG_BIN_TO_DOT_FILE(GST_BIN(_pipeline), GST_DEBUG_GRAPH_SHOW_ALL, "pipeline-with-decoder"); - if ((source = _makeSource(uri)) == nullptr) { - qCCritical(VideoReceiverLog) << "_makeSource() failed"; - break; - } + if (!gst_element_link(src, _decoder)) { + qCCritical(VideoReceiverLog) << "Unable to link decoder"; + return false; + } - if((_tee = gst_element_factory_make("tee", nullptr)) == nullptr) { - qCCritical(VideoReceiverLog) << "gst_element_factory_make('tee') failed"; - break; - } + return true; +} - if((queue = gst_element_factory_make("queue", nullptr)) == nullptr) { - // TODO: We may want to add queue2 max-size-buffers=1 to get lower latency - // We should compare gstreamer scripts to QGroundControl to determine the need - qCCritical(VideoReceiverLog) << "gst_element_factory_make('queue') failed"; - break; - } +bool +VideoReceiver::_addVideoSink(GstPad* pad) +{ + GstCaps* caps = gst_pad_query_caps(pad, nullptr); - if ((decoder = gst_element_factory_make("decodebin", "decoder")) == nullptr) { - qCCritical(VideoReceiverLog) << "gst_element_factory_make('decodebin') failed"; - break; + gst_object_ref(_videoSink); // gst_bin_add() will steal one reference + + gst_bin_add(GST_BIN(_pipeline), _videoSink); + + if(!gst_element_link(_decoder, _videoSink)) { + gst_bin_remove(GST_BIN(_pipeline), _videoSink); + qCCritical(VideoReceiverLog) << "Unable to link video sink"; + if (caps != nullptr) { + gst_caps_unref(caps); + caps = nullptr; } + return false; + } - gst_bin_add_many(GST_BIN(_pipeline), source, _tee, queue, decoder, _videoSink, nullptr); + gst_element_sync_state_with_parent(_videoSink); - pipelineUp = true; + GST_DEBUG_BIN_TO_DOT_FILE(GST_BIN(_pipeline), GST_DEBUG_GRAPH_SHOW_ALL, "pipeline-with-videosink"); - g_signal_connect(source, "pad-added", G_CALLBACK(newPadCB), _tee); + if (caps != nullptr) { + GstStructure* s = gst_caps_get_structure(caps, 0); - if(!gst_element_link_many(_tee, queue, decoder, nullptr)) { - qCCritical(VideoReceiverLog) << "Unable to receiver pipeline."; - break; + if (s != nullptr) { + gint width, height; + gst_structure_get_int(s, "width", &width); + gst_structure_get_int(s, "height", &height); + _setVideoSize(QSize(width, height)); } - g_signal_connect(decoder, "pad-added", G_CALLBACK(newPadCB), _videoSink); - g_signal_connect(decoder, "autoplug-query", G_CALLBACK(autoplugQueryCB), _videoSink); + gst_caps_unref(caps); + caps = nullptr; + } else { + _setVideoSize(QSize(0, 0)); + } - source = queue = decoder = nullptr; + _decoding = true; + emit decodingChanged(); - GstBus* bus = nullptr; + return true; +} - if ((bus = gst_pipeline_get_bus(GST_PIPELINE(_pipeline))) != nullptr) { - gst_bus_enable_sync_message_emission(bus); - g_signal_connect(bus, "sync-message", G_CALLBACK(_onBusMessage), this); - gst_object_unref(bus); - bus = nullptr; - } +void +VideoReceiver::_noteTeeFrame(void) +{ + _lastSourceFrameTime = QDateTime::currentSecsSinceEpoch(); +} - GST_DEBUG_BIN_TO_DOT_FILE(GST_BIN(_pipeline), GST_DEBUG_GRAPH_SHOW_ALL, "pipeline-paused"); - running = gst_element_set_state(_pipeline, GST_STATE_PLAYING) != GST_STATE_CHANGE_FAILURE; +void +VideoReceiver::_noteVideoSinkFrame(void) +{ + _lastVideoFrameTime = QDateTime::currentSecsSinceEpoch(); +} - } while(0); +void +VideoReceiver::_noteEndOfStream(void) +{ + if (_streaming) { + _streaming = false; + } +} - if (!running) { - qCCritical(VideoReceiverLog) << "Failed"; +// -Unlink the branch from the src pad +// -Send an EOS event at the beginning of that branch +void +VideoReceiver::_unlinkBranch(GstElement* from) +{ + GstPad* src; - // In newer versions, the pipeline will clean up all references that are added to it - if (_pipeline != nullptr) { - gst_bin_remove(GST_BIN(_pipeline), _videoSink); - gst_object_unref(_pipeline); - _pipeline = nullptr; - } + if ((src = gst_element_get_static_pad(from, "src")) == nullptr) { + qCCritical(VideoReceiverLog) << "gst_element_get_static_pad() failed"; + return; + } - // If we failed before adding items to the pipeline, then clean up - if (!pipelineUp) { - if (decoder != nullptr) { - gst_object_unref(decoder); - decoder = nullptr; - } + GstPad* sink; - if (queue != nullptr) { - gst_object_unref(queue); - queue = nullptr; - } + if ((sink = gst_pad_get_peer(src)) == nullptr) { + gst_object_unref(src); + src = nullptr; + qCCritical(VideoReceiverLog) << "gst_pad_get_peer() failed"; + return; + } - if (source != nullptr) { - gst_object_unref(source); - source = nullptr; - } + if (!gst_pad_unlink(src, sink)) { + gst_object_unref(src); + src = nullptr; + gst_object_unref(sink); + sink = nullptr; + qCCritical(VideoReceiverLog) << "gst_pad_unlink() failed"; + return; + } - if (_tee != nullptr) { - gst_object_unref(_tee); - _tee = nullptr; - } + gst_object_unref(src); + src = nullptr; - } + // Send EOS at the beginning of the branch + const gboolean ret = gst_pad_send_event(sink, gst_event_new_eos()); - _running = false; + gst_object_unref(sink); + sink = nullptr; + + if (ret) { + qCDebug(VideoReceiverLog) << "Branch EOS was sent"; } else { - GST_DEBUG_BIN_TO_DOT_FILE(GST_BIN(_pipeline), GST_DEBUG_GRAPH_SHOW_ALL, "pipeline-playing"); - _running = true; - qCDebug(VideoReceiverLog) << "Running"; + qCCritical(VideoReceiverLog) << "Branch EOS was NOT sent"; } - _starting = false; -#endif } -//----------------------------------------------------------------------------- void -VideoReceiver::stop() +VideoReceiver::_shutdownDecodingBranch(void) { - if(qgcApp() && qgcApp()->runningUnitTests()) { - return; + if (_decoder != nullptr) { + GstObject* parent; + + if ((parent = gst_element_get_parent(_decoder)) != nullptr) { + gst_bin_remove(GST_BIN(_pipeline), _decoder); + gst_element_set_state(_decoder, GST_STATE_NULL); + gst_object_unref(parent); + parent = nullptr; + } + + gst_object_unref(_decoder); + _decoder = nullptr; } -#if defined(QGC_GST_STREAMING) - _stop = true; - qCDebug(VideoReceiverLog) << "Stopping"; - if(!_streaming) { - _shutdownPipeline(); - } else if (_pipeline != nullptr && !_stopping) { - qCDebug(VideoReceiverLog) << "Stopping _pipeline"; - GstBus* bus = gst_pipeline_get_bus(GST_PIPELINE(_pipeline)); - gst_bus_disable_sync_message_emission(bus); - gst_element_send_event(_pipeline, gst_event_new_eos()); - _stopping = true; - GstMessage* message = gst_bus_timed_pop_filtered(bus, GST_CLOCK_TIME_NONE, (GstMessageType)(GST_MESSAGE_EOS|GST_MESSAGE_ERROR)); - gst_object_unref(bus); - if(GST_MESSAGE_TYPE(message) == GST_MESSAGE_ERROR) { - _shutdownPipeline(); - qCCritical(VideoReceiverLog) << "Error stopping pipeline!"; - } else if(GST_MESSAGE_TYPE(message) == GST_MESSAGE_EOS) { - _handleEOS(); + + if (_videoSinkProbeId != 0) { + GstPad* sinkpad; + if ((sinkpad = gst_element_get_static_pad(_videoSink, "sink")) != nullptr) { + gst_pad_remove_probe(sinkpad, _videoSinkProbeId); + gst_object_unref(sinkpad); + sinkpad = nullptr; } - gst_message_unref(message); + _videoSinkProbeId = 0; } -#endif + + _lastVideoFrameTime = 0; + + GstObject* parent; + + if ((parent = gst_element_get_parent(_videoSink)) != nullptr) { + gst_bin_remove(GST_BIN(_pipeline), _videoSink); + gst_element_set_state(_videoSink, GST_STATE_NULL); + gst_object_unref(parent); + parent = nullptr; + } + + gst_object_unref(_videoSink); + _videoSink = nullptr; + + _removingDecoder = false; + + if (_decoding) { + _decoding = false; + emit decodingChanged(); + qCDebug(VideoReceiverLog) << "Decoding stopped"; + } + + GST_DEBUG_BIN_TO_DOT_FILE(GST_BIN(_pipeline), GST_DEBUG_GRAPH_SHOW_ALL, "pipeline-decoding-stopped"); } -//----------------------------------------------------------------------------- void -VideoReceiver::setUri(const QString & uri) +VideoReceiver::_shutdownRecordingBranch(void) { - _uri = uri; -} + gst_bin_remove(GST_BIN(_pipeline), _fileSink); + gst_element_set_state(_fileSink, GST_STATE_NULL); + gst_object_unref(_fileSink); + _fileSink = nullptr; -//----------------------------------------------------------------------------- -#if defined(QGC_GST_STREAMING) -void -VideoReceiver::_shutdownPipeline() { - if(!_pipeline) { - qCDebug(VideoReceiverLog) << "No pipeline"; - return; + _removingRecorder = false; + + if (_recording) { + _recording = false; + emit recordingChanged(); + qCDebug(VideoReceiverLog) << "Recording stopped"; } - GstBus* bus = nullptr; - if ((bus = gst_pipeline_get_bus(GST_PIPELINE(_pipeline))) != nullptr) { - gst_bus_disable_sync_message_emission(bus); - gst_object_unref(bus); - bus = nullptr; - } - gst_element_set_state(_pipeline, GST_STATE_NULL); - gst_bin_remove(GST_BIN(_pipeline), _videoSink); - _tee = nullptr; - gst_object_unref(_pipeline); - _pipeline = nullptr; - delete _sink; - _sink = nullptr; - _streaming = false; - _recording = false; - _stopping = false; - _running = false; - emit recordingChanged(); + + GST_DEBUG_BIN_TO_DOT_FILE(GST_BIN(_pipeline), GST_DEBUG_GRAPH_SHOW_ALL, "pipeline-recording-stopped"); } -#endif -//----------------------------------------------------------------------------- -#if defined(QGC_GST_STREAMING) -void -VideoReceiver::_handleError() { - qCDebug(VideoReceiverLog) << "Gstreamer error!"; - stop(); - _restart_timer.start(_restart_time_ms); +bool +VideoReceiver::_isOurThread(void) +{ + return QThread::currentThread() == (QThread*)this; } -#endif -//----------------------------------------------------------------------------- -#if defined(QGC_GST_STREAMING) void -VideoReceiver::_handleEOS() { - if(_stopping) { - if(_recording && _sink->removing) { - _shutdownRecordingBranch(); - } - _shutdownPipeline(); - qCDebug(VideoReceiverLog) << "Stopped"; - } else if(_recording && _sink->removing) { - _shutdownRecordingBranch(); - } else { - qCWarning(VideoReceiverLog) << "Unexpected EOS!"; - _handleError(); - } +VideoReceiver::_post(Task t) +{ + QMutexLocker lock(&_taskQueueSync); + _taskQueue.enqueue(t); + _taskQueueUpdate.wakeOne(); } -#endif -//----------------------------------------------------------------------------- -#if defined(QGC_GST_STREAMING) void -VideoReceiver::_handleStateChanged() { - if(_pipeline) { - _streaming = GST_STATE(_pipeline) == GST_STATE_PLAYING; - //qCDebug(VideoReceiverLog) << "State changed, _streaming:" << _streaming; +VideoReceiver::run(void) +{ + while(!_shutdown) { + _taskQueueSync.lock(); + + while (_taskQueue.isEmpty()) { + _taskQueueUpdate.wait(&_taskQueueSync); + } + + Task t = _taskQueue.dequeue(); + + _taskQueueSync.unlock(); + + t(); } } -#endif -//----------------------------------------------------------------------------- -#if defined(QGC_GST_STREAMING) gboolean VideoReceiver::_onBusMessage(GstBus* bus, GstMessage* msg, gpointer data) { @@ -721,38 +1162,60 @@ VideoReceiver::_onBusMessage(GstBus* bus, GstMessage* msg, gpointer data) Q_ASSERT(msg != nullptr && data != nullptr); VideoReceiver* pThis = (VideoReceiver*)data; - switch(GST_MESSAGE_TYPE(msg)) { - case(GST_MESSAGE_ERROR): { - gchar* debug; - GError* error; - gst_message_parse_error(msg, &error, &debug); - g_free(debug); - qCCritical(VideoReceiverLog) << error->message; - g_error_free(error); - pThis->msgErrorReceived(); - } - break; - case(GST_MESSAGE_EOS): - pThis->msgEOSReceived(); + switch (GST_MESSAGE_TYPE(msg)) { + case GST_MESSAGE_ERROR: + do { + gchar* debug; + GError* error; + + gst_message_parse_error(msg, &error, &debug); + + if (debug != nullptr) { + g_free(debug); + debug = nullptr; + } + + if (error != nullptr) { + qCCritical(VideoReceiverLog) << error->message; + g_error_free(error); + error = nullptr; + } + + pThis->_post([pThis](){ + pThis->stop(); + }); + } while(0); break; - case(GST_MESSAGE_STATE_CHANGED): - pThis->msgStateChangedReceived(); + case GST_MESSAGE_EOS: + pThis->_post([pThis](){ + pThis->_handleEOS(); + }); break; - case(GST_MESSAGE_ELEMENT): { - const GstStructure *s = gst_message_get_structure (msg); - - if (gst_structure_has_name (s, "GstBinForwarded")) { - GstMessage *forward_msg = NULL; - gst_structure_get (s, "message", GST_TYPE_MESSAGE, &forward_msg, NULL); - if (forward_msg != nullptr) { - if (GST_MESSAGE_TYPE(forward_msg) == GST_MESSAGE_EOS) { - pThis->msgEOSReceived(); - } - gst_message_unref(forward_msg); - forward_msg = nullptr; + case GST_MESSAGE_ELEMENT: + do { + const GstStructure* s = gst_message_get_structure (msg); + + if (!gst_structure_has_name (s, "GstBinForwarded")) { + break; } - } - } + + GstMessage* forward_msg = nullptr; + + gst_structure_get(s, "message", GST_TYPE_MESSAGE, &forward_msg, NULL); + + if (forward_msg == nullptr) { + break; + } + + if (GST_MESSAGE_TYPE(forward_msg) == GST_MESSAGE_EOS) { + pThis->_post([pThis](){ + pThis->_handleEOS(); + }); + } + + gst_message_unref(forward_msg); + forward_msg = nullptr; + } while(0); break; default: break; @@ -760,439 +1223,309 @@ VideoReceiver::_onBusMessage(GstBus* bus, GstMessage* msg, gpointer data) return TRUE; } -#endif -//----------------------------------------------------------------------------- -#if defined(QGC_GST_STREAMING) void -VideoReceiver::_cleanupOldVideos() +VideoReceiver::_onNewPad(GstElement* element, GstPad* pad, gpointer data) { - //-- Only perform cleanup if storage limit is enabled - if(_videoSettings->enableStorageLimit()->rawValue().toBool()) { - QString savePath = qgcApp()->toolbox()->settingsManager()->appSettings()->videoSavePath(); - QDir videoDir = QDir(savePath); - videoDir.setFilter(QDir::Files | QDir::Readable | QDir::NoSymLinks | QDir::Writable); - videoDir.setSorting(QDir::Time); - //-- All the movie extensions we support - QStringList nameFilters; - for(uint32_t i = 0; i < NUM_MUXES; i++) { - nameFilters << QString("*.") + QString(kVideoExtensions[i]); - } - videoDir.setNameFilters(nameFilters); - //-- get the list of videos stored - QFileInfoList vidList = videoDir.entryInfoList(); - if(!vidList.isEmpty()) { - uint64_t total = 0; - //-- Settings are stored using MB - uint64_t maxSize = (_videoSettings->maxVideoSize()->rawValue().toUInt() * 1024 * 1024); - //-- Compute total used storage - for(int i = 0; i < vidList.size(); i++) { - total += vidList[i].size(); - } - //-- Remove old movies until max size is satisfied. - while(total >= maxSize && !vidList.isEmpty()) { - total -= vidList.last().size(); - qCDebug(VideoReceiverLog) << "Removing old video file:" << vidList.last().filePath(); - QFile file (vidList.last().filePath()); - file.remove(); - vidList.removeLast(); - } - } + VideoReceiver* self = static_cast(data); + + if (element == self->_source) { + self->_onNewSourcePad(pad); + } else if (element == self->_decoder) { + self->_onNewDecoderPad(pad); + } else { + qCDebug(VideoReceiverLog) << "Unexpected call!"; } } -#endif -//----------------------------------------------------------------------------- -#if defined(QGC_GST_STREAMING) void -VideoReceiver::setVideoSink(GstElement* videoSink) +VideoReceiver::_wrapWithGhostPad(GstElement* element, GstPad* pad, gpointer data) { - if(_pipeline != nullptr) { - qCDebug(VideoReceiverLog) << "Video receiver pipeline is active, video sink change is not possible"; - return; - } - - if (_videoSink != nullptr) { - gst_object_unref(_videoSink); - _videoSink = nullptr; - } + gchar* name; - if (videoSink != nullptr) { - _videoSink = videoSink; - gst_object_ref(_videoSink); - - GstPad* pad = gst_element_get_static_pad(_videoSink, "sink"); - - if (pad != nullptr) { - gst_pad_add_probe(pad, (GstPadProbeType)(GST_PAD_PROBE_TYPE_BUFFER), _videoSinkProbe, this, nullptr); - gst_object_unref(pad); - pad = nullptr; - } else { - qCCritical(VideoReceiverLog) << "Unable to find sink pad of video sink"; - } + if ((name = gst_pad_get_name(pad)) == nullptr) { + qCCritical(VideoReceiverLog) << "gst_pad_get_name() failed"; + return; } -} -#endif -//----------------------------------------------------------------------------- -// When we finish our pipeline will look like this: -// -// +-->queue-->decoder-->_videosink -// | -// source-->tee -// | -// | +---------_sink----------+ -// | | | -// we are adding these elements-> +->teepad-->queue-->_filesink | -// | | -// +------------------------+ -#if defined(QGC_GST_STREAMING) -GstElement* -VideoReceiver::_makeFileSink(const QString& videoFile, unsigned format) -{ - GstElement* fileSink = nullptr; - GstElement* mux = nullptr; - GstElement* sink = nullptr; - GstElement* bin = nullptr; - bool releaseElements = true; + GstPad* ghostpad; - do{ - if ((mux = gst_element_factory_make(kVideoMuxes[format], nullptr)) == nullptr) { - qCCritical(VideoReceiverLog) << "gst_element_factory_make('" << kVideoMuxes[format] << "') failed"; - break; - } + if ((ghostpad = gst_ghost_pad_new(name, pad)) == nullptr) { + qCCritical(VideoReceiverLog) << "gst_ghost_pad_new() failed"; + g_free(name); + name = nullptr; + return; + } - if ((sink = gst_element_factory_make("filesink", nullptr)) == nullptr) { - qCCritical(VideoReceiverLog) << "gst_element_factory_make('filesink') failed"; - break; - } + g_free(name); + name = nullptr; - g_object_set(static_cast(sink), "location", qPrintable(videoFile), nullptr); + gst_pad_set_active(ghostpad, TRUE); - if ((bin = gst_bin_new("sinkbin")) == nullptr) { - qCCritical(VideoReceiverLog) << "gst_bin_new('sinkbin') failed"; - break; - } + if (!gst_element_add_pad(GST_ELEMENT_PARENT(element), ghostpad)) { + qCCritical(VideoReceiverLog) << "gst_element_add_pad() failed"; + } +} - GstPadTemplate* padTemplate; +void +VideoReceiver::_linkPadWithOptionalBuffer(GstElement* element, GstPad* pad, gpointer data) +{ + bool isRtpPad = false; - if ((padTemplate = gst_element_class_get_pad_template(GST_ELEMENT_GET_CLASS(mux), "video_%u")) == nullptr) { - qCCritical(VideoReceiverLog) << "gst_element_class_get_pad_template(mux) failed"; - break; - } + GstCaps* filter; - // FIXME: AV: pad handling is potentially leaking (and other similar places too!) - GstPad* pad; + if ((filter = gst_caps_from_string("application/x-rtp")) != nullptr) { + GstCaps* caps = gst_pad_query_caps(pad, nullptr); - if ((pad = gst_element_request_pad(mux, padTemplate, nullptr, nullptr)) == nullptr) { - qCCritical(VideoReceiverLog) << "gst_element_request_pad(mux) failed"; - break; + if (caps != nullptr) { + if (!gst_caps_is_any(caps) && gst_caps_can_intersect(caps, filter)) { + isRtpPad = true; + } + gst_caps_unref(caps); + caps = nullptr; } - gst_bin_add_many(GST_BIN(bin), mux, sink, nullptr); + gst_caps_unref(filter); + filter = nullptr; + } - releaseElements = false; + if (isRtpPad) { + GstElement* buffer; - GstPad* ghostpad = gst_ghost_pad_new("sink", pad); + if ((buffer = gst_element_factory_make("rtpjitterbuffer", nullptr)) != nullptr) { + gst_bin_add(GST_BIN(GST_ELEMENT_PARENT(element)), buffer); - gst_element_add_pad(bin, ghostpad); + gst_element_sync_state_with_parent(buffer); - gst_object_unref(pad); - pad = nullptr; + GstPad* sinkpad = gst_element_get_static_pad(buffer, "sink"); - if (!gst_element_link(mux, sink)) { - qCCritical(VideoReceiverLog) << "gst_element_link() failed"; - break; - } + if (sinkpad != nullptr) { + const GstPadLinkReturn ret = gst_pad_link(pad, sinkpad); - fileSink = bin; - bin = nullptr; - } while(0); + gst_object_unref(sinkpad); + sinkpad = nullptr; - if (releaseElements) { - if (sink != nullptr) { - gst_object_unref(sink); - sink = nullptr; + if (ret == GST_PAD_LINK_OK) { + pad = gst_element_get_static_pad(buffer, "src"); + element = buffer; + } else { + qCDebug(VideoReceiverLog) << "Partially failed - gst_pad_link()"; + } + } else { + qCDebug(VideoReceiverLog) << "Partially failed - gst_element_get_static_pad()"; + } + } else { + qCDebug(VideoReceiverLog) << "Partially failed - gst_element_factory_make('rtpjitterbuffer')"; } + } - if (mux != nullptr) { - gst_object_unref(mux); - mux = nullptr; + gchar* name; + + if ((name = gst_pad_get_name(pad)) != nullptr) { + if(gst_element_link_pads(element, name, GST_ELEMENT(data), "sink") == false) { + qCCritical(VideoReceiverLog) << "gst_element_link_pads() failed"; } - } - if (bin != nullptr) { - gst_object_unref(bin); - bin = nullptr; + g_free(name); + name = nullptr; + } else { + qCCritical(VideoReceiverLog) << "gst_pad_get_name() failed"; } - - return fileSink; } -#endif -void -VideoReceiver::startRecording(const QString &videoFile) +gboolean +VideoReceiver::_padProbe(GstElement* element, GstPad* pad, gpointer user_data) { -#if defined(QGC_GST_STREAMING) + int* probeRes = (int*)user_data; - qCDebug(VideoReceiverLog) << "Starting recording"; - // exit immediately if we are already recording - if(_pipeline == nullptr || _recording) { - qCDebug(VideoReceiverLog) << "Already recording!"; - return; - } + *probeRes |= 1; - uint32_t muxIdx = _videoSettings->recordingFormat()->rawValue().toUInt(); - if(muxIdx >= NUM_MUXES) { - qgcApp()->showMessage(tr("Invalid video format defined.")); - return; - } + GstCaps* filter = gst_caps_from_string("application/x-rtp"); + + if (filter != nullptr) { + GstCaps* caps = gst_pad_query_caps(pad, nullptr); - //-- Disk usage maintenance - _cleanupOldVideos(); + if (caps != nullptr) { + if (!gst_caps_is_any(caps) && gst_caps_can_intersect(caps, filter)) { + *probeRes |= 2; + } - QString savePath = qgcApp()->toolbox()->settingsManager()->appSettings()->videoSavePath(); - if(savePath.isEmpty()) { - qgcApp()->showMessage(tr("Unabled to record video. Video save path must be specified in Settings.")); - return; - } + gst_caps_unref(caps); + caps = nullptr; + } - _videoFile = savePath + "/" - + (videoFile.isEmpty() ? QDateTime::currentDateTime().toString("yyyy-MM-dd_hh.mm.ss") : videoFile) - + "." + kVideoExtensions[muxIdx]; + gst_caps_unref(filter); + filter = nullptr; + } - qCDebug(VideoReceiverLog) << "New video file:" << _videoFile; + return TRUE; +} - emit videoFileChanged(); +gboolean +VideoReceiver::_autoplugQueryCaps(GstElement* bin, GstPad* pad, GstElement* element, GstQuery* query, gpointer data) +{ + GstElement* glupload = (GstElement* )data; - _sink = new Sink(); - _sink->teepad = gst_element_get_request_pad(_tee, "src_%u"); - _sink->queue = gst_element_factory_make("queue", nullptr); - _sink->filesink = _makeFileSink(_videoFile, muxIdx); - _sink->removing = false; + GstPad* sinkpad; - if(!_sink->teepad || !_sink->queue || !_sink->filesink) { - qCCritical(VideoReceiverLog) << "Failed to make _sink elements"; - return; + if ((sinkpad = gst_element_get_static_pad(glupload, "sink")) == nullptr) { + qCCritical(VideoReceiverLog) << "No sink pad found"; + return FALSE; } - gst_object_ref(_sink->queue); - gst_object_ref(_sink->filesink); + GstCaps* filter; - gst_bin_add(GST_BIN(_pipeline), _sink->queue); + gst_query_parse_caps(query, &filter); - gst_element_sync_state_with_parent(_sink->queue); + GstCaps* sinkcaps = gst_pad_query_caps(sinkpad, filter); - // Install a probe on the recording branch to drop buffers until we hit our first keyframe - // When we hit our first keyframe, we can offset the timestamps appropriately according to the first keyframe time - // This will ensure the first frame is a keyframe at t=0, and decoding can begin immediately on playback - // Once we have this valid frame, we attach the filesink. - // Attaching it here would cause the filesink to fail to preroll and to stall the pipeline for a few seconds. - GstPad* probepad = gst_element_get_static_pad(_sink->queue, "src"); - gst_pad_add_probe(probepad, GST_PAD_PROBE_TYPE_BUFFER, _keyframeWatch, this, nullptr); // to drop the buffer - gst_object_unref(probepad); + gst_query_set_caps_result(query, sinkcaps); - // Link the recording branch to the pipeline - GstPad* sinkpad = gst_element_get_static_pad(_sink->queue, "sink"); - gst_pad_link(_sink->teepad, sinkpad); - gst_object_unref(sinkpad); + const gboolean ret = !gst_caps_is_empty(sinkcaps); -// // Add the filesink once we have a valid I-frame -// gst_bin_add(GST_BIN(_pipeline), _sink->filesink); -// if (!gst_element_link(_sink->queue, _sink->filesink)) { -// qCritical() << "Failed to link queue and file sink"; -// } -// gst_element_sync_state_with_parent(_sink->filesink); + gst_caps_unref(sinkcaps); + sinkcaps = nullptr; - GST_DEBUG_BIN_TO_DOT_FILE(GST_BIN(_pipeline), GST_DEBUG_GRAPH_SHOW_ALL, "pipeline-recording"); + gst_object_unref(sinkpad); + sinkpad = nullptr; - _recording = true; - emit recordingChanged(); - qCDebug(VideoReceiverLog) << "Recording started"; -#else - Q_UNUSED(videoFile) -#endif + return ret; } -//----------------------------------------------------------------------------- -void -VideoReceiver::stopRecording(void) +gboolean +VideoReceiver::_autoplugQueryContext(GstElement* bin, GstPad* pad, GstElement* element, GstQuery* query, gpointer data) { -#if defined(QGC_GST_STREAMING) - qCDebug(VideoReceiverLog) << "Stopping recording"; - // exit immediately if we are not recording - if(_pipeline == nullptr || !_recording) { - qCDebug(VideoReceiverLog) << "Not recording!"; - return; - } - // Wait for data block before unlinking - gst_pad_add_probe(_sink->teepad, GST_PAD_PROBE_TYPE_IDLE, _unlinkCallBack, this, nullptr); -#endif -} + GstElement* glsink = (GstElement* )data; -//----------------------------------------------------------------------------- -// This is only installed on the transient _pipelineStopRec in order -// to finalize a video file. It is not used for the main _pipeline. -// -EOS has appeared on the bus of the temporary pipeline -// -At this point all of the recoring elements have been flushed, and the video file has been finalized -// -Now we can remove the temporary pipeline and its elements -#if defined(QGC_GST_STREAMING) -void -VideoReceiver::_shutdownRecordingBranch() -{ - gst_bin_remove(GST_BIN(_pipeline), _sink->queue); - gst_bin_remove(GST_BIN(_pipeline), _sink->filesink); + GstPad* sinkpad; - gst_element_set_state(_sink->queue, GST_STATE_NULL); - gst_element_set_state(_sink->filesink, GST_STATE_NULL); + if ((sinkpad = gst_element_get_static_pad(glsink, "sink")) == nullptr){ + qCCritical(VideoReceiverLog) << "No sink pad found"; + return FALSE; + } - gst_object_unref(_sink->queue); - gst_object_unref(_sink->filesink); + const gboolean ret = gst_pad_query(sinkpad, query); - delete _sink; - _sink = nullptr; - _recording = false; + gst_object_unref(sinkpad); + sinkpad = nullptr; - emit recordingChanged(); - qCDebug(VideoReceiverLog) << "Recording stopped"; + return ret; } -#endif -//----------------------------------------------------------------------------- -// -Unlink the recording branch from the tee in the main _pipeline -// -Create a second temporary pipeline, and place the recording branch elements into that pipeline -// -Setup watch and handler for EOS event on the temporary pipeline's bus -// -Send an EOS event at the beginning of that pipeline -#if defined(QGC_GST_STREAMING) -void -VideoReceiver::_unlinkRecordingBranch(GstPadProbeInfo* info) +gboolean +VideoReceiver::_autoplugQuery(GstElement* bin, GstPad* pad, GstElement* element, GstQuery* query, gpointer data) { - Q_UNUSED(info) - // Send EOS at the beginning of the pipeline - GstPad* sinkpad = gst_element_get_static_pad(_sink->queue, "sink"); - gst_pad_unlink(_sink->teepad, sinkpad); - gst_pad_send_event(sinkpad, gst_event_new_eos()); - gst_object_unref(sinkpad); - qCDebug(VideoReceiverLog) << "Recording EOS was sent"; - // Give tee its pad back - gst_element_release_request_pad(_tee, _sink->teepad); - gst_object_unref(_sink->teepad); + gboolean ret; + + switch (GST_QUERY_TYPE(query)) { + case GST_QUERY_CAPS: + ret = _autoplugQueryCaps(bin, pad, element, query, data); + break; + case GST_QUERY_CONTEXT: + ret = _autoplugQueryContext(bin, pad, element, query, data); + break; + default: + ret = FALSE; + break; + } + + return ret; } -#endif -//----------------------------------------------------------------------------- -#if defined(QGC_GST_STREAMING) GstPadProbeReturn -VideoReceiver::_unlinkCallBack(GstPad* pad, GstPadProbeInfo* info, gpointer user_data) +VideoReceiver::_teeProbe(GstPad* pad, GstPadProbeInfo* info, gpointer user_data) { Q_UNUSED(pad); - if(info != nullptr && user_data != nullptr) { + Q_UNUSED(info) + + if(user_data != nullptr) { VideoReceiver* pThis = static_cast(user_data); - // We will only act once - if(g_atomic_int_compare_and_exchange(&pThis->_sink->removing, FALSE, TRUE)) { - pThis->_unlinkRecordingBranch(info); - } + pThis->_noteTeeFrame(); } - return GST_PAD_PROBE_REMOVE; + + return GST_PAD_PROBE_OK; } -#endif -//----------------------------------------------------------------------------- -#if defined(QGC_GST_STREAMING) GstPadProbeReturn VideoReceiver::_videoSinkProbe(GstPad* pad, GstPadProbeInfo* info, gpointer user_data) { - Q_UNUSED(pad); - if(info != nullptr && user_data != nullptr) { + if(user_data != nullptr) { VideoReceiver* pThis = static_cast(user_data); + + if (pThis->_resetVideoSink) { + pThis->_resetVideoSink = false; + +// FIXME: AV: this makes MPEG2-TS playing smooth but breaks RTSP +// gst_pad_send_event(pad, gst_event_new_flush_start()); +// gst_pad_send_event(pad, gst_event_new_flush_stop(TRUE)); + +// GstBuffer* buf; + +// if ((buf = gst_pad_probe_info_get_buffer(info)) != nullptr) { +// GstSegment* seg; + +// if ((seg = gst_segment_new()) != nullptr) { +// gst_segment_init(seg, GST_FORMAT_TIME); + +// seg->start = buf->pts; + +// gst_pad_send_event(pad, gst_event_new_segment(seg)); + +// gst_segment_free(seg); +// seg = nullptr; +// } + +// gst_pad_set_offset(pad, -static_cast(buf->pts)); +// } + } + pThis->_noteVideoSinkFrame(); } return GST_PAD_PROBE_OK; } -#endif - -//----------------------------------------------------------------------------- -#if defined(QGC_GST_STREAMING) -void -VideoReceiver::_noteVideoSinkFrame() -{ - _lastFrameTime = QDateTime::currentSecsSinceEpoch(); -} -#endif -//----------------------------------------------------------------------------- -#if defined(QGC_GST_STREAMING) GstPadProbeReturn -VideoReceiver::_keyframeWatch(GstPad* pad, GstPadProbeInfo* info, gpointer user_data) +VideoReceiver::_eosProbe(GstPad* pad, GstPadProbeInfo* info, gpointer user_data) { Q_UNUSED(pad); - if(info != nullptr && user_data != nullptr) { - GstBuffer* buf = gst_pad_probe_info_get_buffer(info); - if(GST_BUFFER_FLAG_IS_SET(buf, GST_BUFFER_FLAG_DELTA_UNIT)) { // wait for a keyframe - return GST_PAD_PROBE_DROP; - } else { - VideoReceiver* pThis = static_cast(user_data); - - // set media file '0' offset to current timeline position - we don't want to touch other elements in the graph, except these which are downstream! - gst_pad_set_offset(pad, -buf->pts); + Q_ASSERT(user_data != nullptr); - // Add the filesink once we have a valid I-frame - gst_bin_add(GST_BIN(pThis->_pipeline), pThis->_sink->filesink); - if (!gst_element_link(pThis->_sink->queue, pThis->_sink->filesink)) { - qCCritical(VideoReceiverLog) << "Failed to link queue and file sink"; - } - gst_element_sync_state_with_parent(pThis->_sink->filesink); + if(info != nullptr) { + GstEvent* event = gst_pad_probe_info_get_event(info); - qCDebug(VideoReceiverLog) << "Got keyframe, stop dropping buffers"; - pThis->gotFirstRecordingKeyFrame(); + if (GST_EVENT_TYPE(event) == GST_EVENT_EOS) { + VideoReceiver* pThis = static_cast(user_data); + pThis->_noteEndOfStream(); } } - return GST_PAD_PROBE_REMOVE; + return GST_PAD_PROBE_OK; } -#endif -//----------------------------------------------------------------------------- -void -VideoReceiver::_updateTimer() +GstPadProbeReturn +VideoReceiver::_keyframeWatch(GstPad* pad, GstPadProbeInfo* info, gpointer user_data) { -#if defined(QGC_GST_STREAMING) - if(_stopping || _starting) { - return; + if (info == nullptr || user_data == nullptr) { + qCCritical(VideoReceiverLog) << "Invalid arguments"; + return GST_PAD_PROBE_DROP; } - if(_streaming) { - if(!_videoRunning) { - _videoRunning = true; - emit videoRunningChanged(); - } - } else { - if(_videoRunning) { - _videoRunning = false; - emit videoRunningChanged(); - } + GstBuffer* buf = gst_pad_probe_info_get_buffer(info); + + if (GST_BUFFER_FLAG_IS_SET(buf, GST_BUFFER_FLAG_DELTA_UNIT)) { // wait for a keyframe + return GST_PAD_PROBE_DROP; } - if(_videoRunning) { - uint32_t timeout = 1; - if(qgcApp()->toolbox() && qgcApp()->toolbox()->settingsManager()) { - timeout = _videoSettings->rtspTimeout()->rawValue().toUInt(); - } + // set media file '0' offset to current timeline position - we don't want to touch other elements in the graph, except these which are downstream! + gst_pad_set_offset(pad, -static_cast(buf->pts)); - const qint64 now = QDateTime::currentSecsSinceEpoch(); + VideoReceiver* pThis = static_cast(user_data); - if(now - _lastFrameTime > timeout) { - stop(); - // We want to start it back again with _updateTimer - _stop = false; - } - } else { - // FIXME: AV: if pipeline is _running but not _streaming for some time then we need to restart - if(!_stop && !_running && !_uri.isEmpty() && _videoSettings->streamEnabled()->rawValue().toBool()) { - start(); - } - } -#endif -} + qCDebug(VideoReceiverLog) << "Got keyframe, stop dropping buffers"; + pThis->recordingStarted(); + + return GST_PAD_PROBE_REMOVE; +} +#endif diff --git a/src/VideoStreaming/VideoReceiver.h b/src/VideoStreaming/VideoReceiver.h index 7f6e73a889e33675f73d612e56dbdca39126d86e..d30e902bdacaa8f5fda5bc08e58eaabd5264c5a7 100644 --- a/src/VideoStreaming/VideoReceiver.h +++ b/src/VideoStreaming/VideoReceiver.h @@ -17,132 +17,159 @@ #include "QGCLoggingCategory.h" #include +#include #include #include +#include +#include +#include +#include #if defined(QGC_GST_STREAMING) #include +typedef GstElement VideoSink; +#else +typedef void VideoSink; #endif Q_DECLARE_LOGGING_CATEGORY(VideoReceiverLog) -class VideoSettings; - -class VideoReceiver : public QObject +class VideoReceiver : public QThread { Q_OBJECT -public: - -#if defined(QGC_GST_STREAMING) - Q_PROPERTY(bool recording READ recording NOTIFY recordingChanged) -#endif - Q_PROPERTY(bool videoRunning READ videoRunning NOTIFY videoRunningChanged) - Q_PROPERTY(QString imageFile READ imageFile NOTIFY imageFileChanged) - Q_PROPERTY(QString videoFile READ videoFile NOTIFY videoFileChanged) - Q_PROPERTY(bool showFullScreen READ showFullScreen WRITE setShowFullScreen NOTIFY showFullScreenChanged) +public: explicit VideoReceiver(QObject* parent = nullptr); - ~VideoReceiver(); - -#if defined(QGC_GST_STREAMING) - virtual bool recording () { return _recording; } -#endif - - virtual bool videoRunning () { return _videoRunning; } - virtual QString imageFile () { return _imageFile; } - virtual QString videoFile () { return _videoFile; } - virtual bool showFullScreen () { return _showFullScreen; } - - virtual void grabImage (QString imageFile); - - virtual void setShowFullScreen (bool show) { _showFullScreen = show; emit showFullScreenChanged(); } - -#if defined(QGC_GST_STREAMING) - void setVideoSink (GstElement* videoSink); -#endif + ~VideoReceiver(void); + + typedef enum { + FILE_FORMAT_MIN = 0, + FILE_FORMAT_MKV = FILE_FORMAT_MIN, + FILE_FORMAT_MOV, + FILE_FORMAT_MP4, + FILE_FORMAT_MAX + } FILE_FORMAT; + + Q_PROPERTY(bool streaming READ streaming NOTIFY streamingChanged) + Q_PROPERTY(bool decoding READ decoding NOTIFY decodingChanged) + Q_PROPERTY(bool recording READ recording NOTIFY recordingChanged) + Q_PROPERTY(QSize videoSize READ videoSize NOTIFY videoSizeChanged) + + bool streaming(void) { + return _streaming; + } + + bool decoding(void) { + return _decoding; + } + + bool recording(void) { + return _recording; + } + + QSize videoSize(void) { + const quint32 size = _videoSize; + return QSize((size >> 16) & 0xFFFF, size & 0xFFFF); + } signals: - void videoRunningChanged (); - void imageFileChanged (); - void videoFileChanged (); - void showFullScreenChanged (); -#if defined(QGC_GST_STREAMING) - void recordingChanged (); - void msgErrorReceived (); - void msgEOSReceived (); - void msgStateChangedReceived (); - void gotFirstRecordingKeyFrame (); -#endif + void timeout(void); + void streamingChanged(void); + void decodingChanged(void); + void recordingChanged(void); + void recordingStarted(void); + void videoSizeChanged(void); + void screenshotComplete(void); public slots: - virtual void start (); - virtual void stop (); - virtual void setUri (const QString& uri); - virtual void stopRecording (); - virtual void startRecording (const QString& videoFile = QString()); + virtual void start(const QString& uri, unsigned timeout); + virtual void stop(void); + virtual void startDecoding(VideoSink* videoSink); + virtual void stopDecoding(void); + virtual void startRecording(const QString& videoFile, FILE_FORMAT format); + virtual void stopRecording(void); + virtual void takeScreenshot(const QString& imageFile); -protected slots: - virtual void _updateTimer (); #if defined(QGC_GST_STREAMING) - GstElement* _makeSource (const QString& uri); - GstElement* _makeFileSink (const QString& videoFile, unsigned format); - virtual void _restart_timeout (); - virtual void _handleError (); - virtual void _handleEOS (); - virtual void _handleStateChanged (); -#endif +protected slots: + virtual void _watchdog(void); + virtual void _handleEOS(void); protected: -#if defined(QGC_GST_STREAMING) - - typedef struct - { - GstPad* teepad; - GstElement* queue; - GstElement* filesink; - gboolean removing; - } Sink; - - bool _running; - bool _recording; - bool _streaming; - bool _starting; - bool _stopping; - bool _stop; - Sink* _sink; + void _setVideoSize(const QSize& size) { + _videoSize = ((quint32)size.width() << 16) | (quint32)size.height(); + emit videoSizeChanged(); + } + + virtual GstElement* _makeSource(const QString& uri); + virtual GstElement* _makeDecoder(GstCaps* caps, GstElement* videoSink); + virtual GstElement* _makeFileSink(const QString& videoFile, FILE_FORMAT format); + + virtual void _onNewSourcePad(GstPad* pad); + virtual void _onNewDecoderPad(GstPad* pad); + virtual bool _addDecoder(GstElement* src); + virtual bool _addVideoSink(GstPad* pad); + virtual void _noteTeeFrame(void); + virtual void _noteVideoSinkFrame(void); + virtual void _noteEndOfStream(void); + virtual void _unlinkBranch(GstElement* from); + virtual void _shutdownDecodingBranch (void); + virtual void _shutdownRecordingBranch(void); + + typedef std::function Task; + bool _isOurThread(void); + void _post(Task t); + void run(void); + +private: + static gboolean _onBusMessage(GstBus* bus, GstMessage* message, gpointer user_data); + static void _onNewPad(GstElement* element, GstPad* pad, gpointer data); + static void _wrapWithGhostPad(GstElement* element, GstPad* pad, gpointer data); + static void _linkPadWithOptionalBuffer(GstElement* element, GstPad* pad, gpointer data); + static gboolean _padProbe(GstElement* element, GstPad* pad, gpointer user_data); + static gboolean _autoplugQueryCaps(GstElement* bin, GstPad* pad, GstElement* element, GstQuery* query, gpointer data); + static gboolean _autoplugQueryContext(GstElement* bin, GstPad* pad, GstElement* element, GstQuery* query, gpointer data); + static gboolean _autoplugQuery(GstElement* bin, GstPad* pad, GstElement* element, GstQuery* query, gpointer data); + static GstPadProbeReturn _teeProbe(GstPad* pad, GstPadProbeInfo* info, gpointer user_data); + static GstPadProbeReturn _videoSinkProbe(GstPad* pad, GstPadProbeInfo* info, gpointer user_data); + static GstPadProbeReturn _eosProbe(GstPad* pad, GstPadProbeInfo* info, gpointer user_data); + static GstPadProbeReturn _keyframeWatch(GstPad* pad, GstPadProbeInfo* info, gpointer user_data); + + bool _removingDecoder; + bool _removingRecorder; + GstElement* _source; GstElement* _tee; + GstElement* _decoderValve; + GstElement* _recorderValve; + GstElement* _decoder; + GstElement* _videoSink; + GstElement* _fileSink; + GstElement* _pipeline; - void _noteVideoSinkFrame (); + qint64 _lastSourceFrameTime; + qint64 _lastVideoFrameTime; + bool _resetVideoSink; + gulong _videoSinkProbeId; - static gboolean _onBusMessage (GstBus* bus, GstMessage* message, gpointer user_data); - static GstPadProbeReturn _unlinkCallBack (GstPad* pad, GstPadProbeInfo* info, gpointer user_data); - static GstPadProbeReturn _videoSinkProbe (GstPad* pad, GstPadProbeInfo* info, gpointer user_data); - static GstPadProbeReturn _keyframeWatch (GstPad* pad, GstPadProbeInfo* info, gpointer user_data); + QTimer _watchdogTimer; - virtual void _unlinkRecordingBranch (GstPadProbeInfo* info); - virtual void _shutdownRecordingBranch(); - virtual void _shutdownPipeline (); - virtual void _cleanupOldVideos (); + //-- RTSP UDP reconnect timeout + uint64_t _udpReconnect_us; - GstElement* _pipeline; - GstElement* _videoSink; - guint64 _lastFrameId; - qint64 _lastFrameTime; + unsigned _timeout; - //-- Wait for Video Server to show up before starting - QTimer _frameTimer; - QTimer _restart_timer; - int _restart_time_ms; + QWaitCondition _taskQueueUpdate; + QMutex _taskQueueSync; + QQueue _taskQueue; + bool _shutdown; - //-- RTSP UDP reconnect timeout - uint64_t _udpReconnect_us; + static const char* _kFileMux[FILE_FORMAT_MAX - FILE_FORMAT_MIN]; +#else +private: #endif - QString _uri; - QString _imageFile; - QString _videoFile; - bool _videoRunning; - bool _showFullScreen; - VideoSettings* _videoSettings; + std::atomic _streaming; + std::atomic _decoding; + std::atomic _recording; + std::atomic_videoSize; }; - diff --git a/src/VideoStreaming/VideoStreaming.pri b/src/VideoStreaming/VideoStreaming.pri index b01539971c95b22971cd380cf6603a2417d7db76..b1af89f0c03373700cc9cad127d916f04122135b 100644 --- a/src/VideoStreaming/VideoStreaming.pri +++ b/src/VideoStreaming/VideoStreaming.pri @@ -15,7 +15,7 @@ LinuxBuild { QT += x11extras waylandclient CONFIG += link_pkgconfig packagesExist(gstreamer-1.0) { - PKGCONFIG += gstreamer-1.0 gstreamer-video-1.0 gstreamer-gl-1.0 + PKGCONFIG += gstreamer-1.0 gstreamer-video-1.0 gstreamer-gl-1.0 egl CONFIG += VideoEnabled } } else:MacBuild { diff --git a/src/VideoStreaming/gstqgcvideosinkbin.c b/src/VideoStreaming/gstqgcvideosinkbin.c index 28e6dc2070382e451fada537d4ae59d80c400e95..943e66821d5bd595d0c55f052783d70519a29757 100644 --- a/src/VideoStreaming/gstqgcvideosinkbin.c +++ b/src/VideoStreaming/gstqgcvideosinkbin.c @@ -125,6 +125,9 @@ _vsb_init(GstQgcVideoSinkBin *vsb) break; } + // FIXME: AV: temporally disable sync due to MPEG2-TS sync issues + g_object_set(vsb->qmlglsink, "sync", FALSE, NULL); + if ((glcolorconvert = gst_element_factory_make("glcolorconvert", NULL)) == NULL) { GST_ERROR_OBJECT(vsb, "gst_element_factory_make('glcolorconvert' failed)"); break; diff --git a/src/comm/MockLink.cc b/src/comm/MockLink.cc index 1d5311fcdadb26bba6dde02bbaffdb6c18d8fdd6..5dc76cf761ee29105c79cb1e7c336c94c69fcd5a 100644 --- a/src/comm/MockLink.cc +++ b/src/comm/MockLink.cc @@ -1013,9 +1013,9 @@ void MockLink::_respondWithAutopilotVersion(void) respondWithMavlinkMessage(msg); } -void MockLink::setMissionItemFailureMode(MockLinkMissionItemHandler::FailureMode_t failureMode) +void MockLink::setMissionItemFailureMode(MockLinkMissionItemHandler::FailureMode_t failureMode, MAV_MISSION_RESULT failureAckResult) { - _missionItemHandler.setMissionItemFailureMode(failureMode); + _missionItemHandler.setFailureMode(failureMode, failureAckResult); } void MockLink::_sendHomePosition(void) diff --git a/src/comm/MockLink.h b/src/comm/MockLink.h index 49cfd9cefdf3aef0e9e8f0e5ff497b91ad729242..c02d5ea99a9eecf043935197d527f4db5ee7cd67 100644 --- a/src/comm/MockLink.h +++ b/src/comm/MockLink.h @@ -134,7 +134,8 @@ public: /// Sets a failure mode for unit testing /// @param failureMode Type of failure to simulate - void setMissionItemFailureMode(MockLinkMissionItemHandler::FailureMode_t failureMode); + /// @param failureAckResult Error to send if one the ack error modes + void setMissionItemFailureMode(MockLinkMissionItemHandler::FailureMode_t failureMode, MAV_MISSION_RESULT failureAckResult); /// Called to send a MISSION_ACK message while the MissionManager is in idle state void sendUnexpectedMissionAck(MAV_MISSION_RESULT ackType) { _missionItemHandler.sendUnexpectedMissionAck(ackType); } diff --git a/src/comm/MockLinkMissionItemHandler.cc b/src/comm/MockLinkMissionItemHandler.cc index a7fcaa76a6d722f918f37f1101ddffc9adb5c1b7..a97b810b526073abea7d2abb1f7ffa98a380079d 100644 --- a/src/comm/MockLinkMissionItemHandler.cc +++ b/src/comm/MockLinkMissionItemHandler.cc @@ -200,7 +200,7 @@ void MockLinkMissionItemHandler::_handleMissionRequest(const mavlink_message_t& if ((_failureMode == FailReadRequest0ErrorAck && request.seq == 0) || (_failureMode == FailReadRequest1ErrorAck && request.seq == 1)) { - _sendAck(MAV_MISSION_ERROR); + _sendAck(_failureAckResult); } else { MissionItemBoth_t missionItemBoth; @@ -327,7 +327,7 @@ void MockLinkMissionItemHandler::_requestNextMissionItem(int sequenceNumber) if ((_failureMode == FailWriteRequest0ErrorAck && sequenceNumber == 0) || (_failureMode == FailWriteRequest1ErrorAck && sequenceNumber == 1)) { qCDebug(MockLinkMissionItemHandlerLog) << "_requestNextMissionItem sending ack error due to failure mode"; - _sendAck(MAV_MISSION_ERROR); + _sendAck(_failureAckResult); } else { mavlink_message_t message; @@ -408,7 +408,7 @@ void MockLinkMissionItemHandler::_handleMissionItem(const mavlink_message_t& msg _writeSequenceIndex++; if (_writeSequenceIndex < _writeSequenceCount) { if (_failureMode == FailWriteFinalAckMissingRequests && _writeSequenceIndex == 3) { - // Send MAV_MISSION_ACCPETED ack too early + // Send MAV_MISSION_ACCEPTED ack too early _sendAck(MAV_MISSION_ACCEPTED); } else { _requestNextMissionItem(_writeSequenceIndex); @@ -448,9 +448,10 @@ void MockLinkMissionItemHandler::sendUnexpectedMissionRequest(void) Q_ASSERT(false); } -void MockLinkMissionItemHandler::setMissionItemFailureMode(FailureMode_t failureMode) +void MockLinkMissionItemHandler::setFailureMode(FailureMode_t failureMode, MAV_MISSION_RESULT failureAckResult) { _failureMode = failureMode; + _failureAckResult = failureAckResult; } void MockLinkMissionItemHandler::shutdown(void) diff --git a/src/comm/MockLinkMissionItemHandler.h b/src/comm/MockLinkMissionItemHandler.h index 0424ddc52163d2a068fec61bb79dd9eb9e3de224..625c615bb7eff691d0af7f56a8da406aa1f06547 100644 --- a/src/comm/MockLinkMissionItemHandler.h +++ b/src/comm/MockLinkMissionItemHandler.h @@ -63,7 +63,8 @@ public: /// Sets a failure mode for unit testing /// @param failureMode Type of failure to simulate - void setMissionItemFailureMode(FailureMode_t failureMode); + /// @param failureAckResult Error to send if one the ack error modes + void setFailureMode(FailureMode_t failureMode, MAV_MISSION_RESULT failureAckResult); /// Called to send a MISSION_ACK message while the MissionManager is in idle state void sendUnexpectedMissionAck(MAV_MISSION_RESULT ackType); @@ -115,6 +116,7 @@ private: QTimer* _missionItemResponseTimer; FailureMode_t _failureMode; + MAV_MISSION_RESULT _failureAckResult; bool _sendHomePositionOnEmptyList; MAVLinkProtocol* _mavlinkProtocol; bool _failReadRequestListFirstResponse; diff --git a/src/ui/preferences/GeneralSettings.qml b/src/ui/preferences/GeneralSettings.qml index 3897dc61a7196323bb6f69588240b01b5463ce31..865e4fa19f11c4c5a526e672dbdb767fcd09087d 100644 --- a/src/ui/preferences/GeneralSettings.qml +++ b/src/ui/preferences/GeneralSettings.qml @@ -43,6 +43,7 @@ Rectangle { property Fact _followTarget: QGroundControl.settingsManager.appSettings.followTarget property real _panelWidth: _root.width * _internalWidthRatio property real _margins: ScreenTools.defaultFontPixelWidth + property var _planViewSettings: QGroundControl.settingsManager.planViewSettings property string _videoSource: QGroundControl.settingsManager.videoSettings.videoSource.value property bool _isGst: QGroundControl.videoManager.isGStreamer @@ -595,7 +596,7 @@ Rectangle { QGCLabel { id: planViewSectionLabel text: qsTr("Plan View") - visible: QGroundControl.settingsManager.planViewSettings.visible + visible: _planViewSettings.visible } Rectangle { Layout.preferredHeight: planViewCol.height + (_margins * 2) @@ -621,6 +622,17 @@ Rectangle { fact: QGroundControl.settingsManager.appSettings.defaultMissionItemAltitude } } + + FactCheckBox { + text: qsTr("Use MAV_CMD_CONDITION_GATE for pattern generation") + fact: QGroundControl.settingsManager.planViewSettings.useConditionGate + } + + FactCheckBox { + text: qsTr("Missions Do Not Require Takeoff Item") + fact: _planViewSettings.takeoffItemNotRequired + visible: _planViewSettings.takeoffItemNotRequired.visible + } } } @@ -883,16 +895,27 @@ Rectangle { visible: QGroundControl.settingsManager.adsbVehicleManagerSettings.visible } Rectangle { - Layout.preferredHeight: adsbGrid.height + (_margins * 2) + Layout.preferredHeight: adsbGrid.y + adsbGrid.height + _margins Layout.preferredWidth: adsbGrid.width + (_margins * 2) color: qgcPal.windowShade visible: adsbSectionLabel.visible Layout.fillWidth: true + QGCLabel { + id: warningLabel + anchors.margins: _margins + anchors.top: parent.top + anchors.left: parent.left + anchors.right: parent.right + font.pointSize: ScreenTools.smallFontPointSize + wrapMode: Text.WordWrap + text: qsTr("Note: These setting are not meant for use with an ADSB transponder which is situated on the vehicle.") + } + GridLayout { id: adsbGrid anchors.topMargin: _margins - anchors.top: parent.top + anchors.top: warningLabel.bottom Layout.fillWidth: true anchors.horizontalCenter: parent.horizontalCenter columns: 2 diff --git a/src/ui/toolbar/MainToolBar.qml b/src/ui/toolbar/MainToolBar.qml index 2e6eaf1167aa074236d42eaa3dc568164bd8d9f8..fb6dd695fe1fc25a7caa701d094061a9351d42da 100644 --- a/src/ui/toolbar/MainToolBar.qml +++ b/src/ui/toolbar/MainToolBar.qml @@ -42,7 +42,8 @@ Item { Connections { target: setupWindow onVisibleChanged: { - if(setupWindow.visible) { + if (setupWindow.visible) { + buttonRow.clearAllChecks() setupButton.checked = true } }