diff --git a/src/Camera/SimulatedCameraControl.h b/src/Camera/SimulatedCameraControl.h index 3611a4f5981..813c9fe815d 100644 --- a/src/Camera/SimulatedCameraControl.h +++ b/src/Camera/SimulatedCameraControl.h @@ -15,6 +15,7 @@ #include class VideoManager; +#include "Vehicle.h" class Vehicle; /// Creates a simulated Camera Control which supports: @@ -65,8 +66,43 @@ class SimulatedCameraControl : public MavlinkCameraControl void stopStream () override {} bool stopTakePhoto () override { return false;} void resumeStream () override {} - void startTracking (QRectF /*rec*/) override {} - void startTracking (QPointF /*point*/, double /*radius*/) override {} + void startTracking (QRectF rec) override { + // if(_trackingMarquee != rec) { + // _trackingMarquee = rec; + + qCDebug(CameraControlLog) << "Start Tracking (Rectangle: [" + << static_cast(rec.x()) << ", " + << static_cast(rec.y()) << "] - [" + << static_cast(rec.x() + rec.width()) << ", " + << static_cast(rec.y() + rec.height()) << "]"; + + _vehicle->sendMavCommand(1, + MAV_CMD_CAMERA_TRACK_RECTANGLE, + true, + static_cast(rec.x()), + static_cast(rec.y()), + static_cast(rec.x() + rec.width()), + static_cast(rec.y() + rec.height())); + // } + } + void startTracking (QPointF point, double radius) override { + // if(_trackingPoint != point || _trackingRadius != radius) { + // _trackingPoint = point; + // _trackingRadius = radius; + + qCDebug(CameraControlLog) << "Start Tracking (Point: [" + << static_cast(point.x()) << ", " + << static_cast(point.y()) << "], Radius: " + << static_cast(radius); + + _vehicle->sendMavCommand(1, + MAV_CMD_CAMERA_TRACK_POINT, + true, + static_cast(point.x()), + static_cast(point.y()), + static_cast(radius)); + // } + } void stopTracking () override {} int version () override { return 0; } QString modelName () override { return QStringLiteral("Simulated Camera"); } diff --git a/src/Camera/VehicleCameraControl.cc b/src/Camera/VehicleCameraControl.cc index 5d9607e3a49..23bbcf4213a 100644 --- a/src/Camera/VehicleCameraControl.cc +++ b/src/Camera/VehicleCameraControl.cc @@ -114,6 +114,7 @@ VehicleCameraControl::VehicleCameraControl(const mavlink_camera_information_t *i , _vehicle(vehicle) , _compID(compID) { + qCDebug(CameraControlLog) << "Vehicle Camera Control Object initialised"; QQmlEngine::setObjectOwnership(this, QQmlEngine::CppOwnership); memcpy(&_info, info, sizeof(mavlink_camera_information_t)); connect(this, &VehicleCameraControl::dataReady, this, &VehicleCameraControl::_dataReady); @@ -2311,7 +2312,7 @@ VehicleCameraControl::startTracking(QRectF rec) << static_cast(rec.x() + rec.width()) << ", " << static_cast(rec.y() + rec.height()) << "]"; - _vehicle->sendMavCommand(_compID, + _vehicle->sendMavCommand(0, MAV_CMD_CAMERA_TRACK_RECTANGLE, true, static_cast(rec.x()), @@ -2334,7 +2335,7 @@ VehicleCameraControl::startTracking(QPointF point, double radius) << static_cast(point.y()) << "], Radius: " << static_cast(radius); - _vehicle->sendMavCommand(_compID, + _vehicle->sendMavCommand(0, MAV_CMD_CAMERA_TRACK_POINT, true, static_cast(point.x()), diff --git a/src/FlightDisplay/FlyViewVideo.qml b/src/FlightDisplay/FlyViewVideo.qml index ab393ddf58e..3e11df24c47 100644 --- a/src/FlightDisplay/FlyViewVideo.qml +++ b/src/FlightDisplay/FlyViewVideo.qml @@ -113,7 +113,7 @@ Item { //create a new rectangle at the wanted position if(videoStreaming._camera) { - if (videoStreaming._camera.trackingEnabled) { + if (true) { trackingROI = trackingROIComponent.createObject(flyViewVideoMouseArea, { "x": mouse.x, "y": mouse.y @@ -142,10 +142,9 @@ Item { //if there is already a selection, delete it if (trackingROI !== null) { trackingROI.destroy(); - } - + } if(videoStreaming._camera) { - if (videoStreaming._camera.trackingEnabled) { + if (true) { // order coordinates --> top/left and bottom/right x0 = Math.min(_track_rec_x, mouse.x) x1 = Math.max(_track_rec_x, mouse.x) @@ -210,7 +209,7 @@ Item { running: true onTriggered: { if (videoStreaming._camera) { - if (videoStreaming._camera.trackingEnabled && videoStreaming._camera.trackingImageStatus) { + if (true) { var margin_hor = (parent.parent.width - videoStreaming.getWidth()) / 2 var margin_ver = (parent.parent.height - videoStreaming.getHeight()) / 2 var left = margin_hor + videoStreaming.getWidth() * videoStreaming._camera.trackingImageRect.left diff --git a/src/FlightMap/Widgets/PhotoVideoControl.qml b/src/FlightMap/Widgets/PhotoVideoControl.qml index 22bc144f338..e9c46f56597 100644 --- a/src/FlightMap/Widgets/PhotoVideoControl.qml +++ b/src/FlightMap/Widgets/PhotoVideoControl.qml @@ -254,7 +254,7 @@ Rectangle { id: trackingControls Layout.alignment: Qt.AlignHCenter spacing: _margins - visible: _camera && _camera.hasTracking + visible: true Rectangle { Layout.alignment: Qt.AlignHCenter @@ -276,10 +276,10 @@ Rectangle { MouseArea { anchors.fill: parent onClicked: { - _camera.trackingEnabled = !_camera.trackingEnabled; - if (!_camera.trackingEnabled) { - !camera.stopTracking() - } + _camera.trackingEnabled = true; // !_camera.trackingEnabled; + // if (!_camera.trackingEnabled) { + // !camera.stopTracking() + //} } } } @@ -289,7 +289,7 @@ Rectangle { Layout.alignment: Qt.AlignHCenter text: qsTr("Camera Tracking") font.pointSize: ScreenTools.defaultFontPointSize - visible: _camera && _camera.hasTracking + visible: true //_camera && _camera.hasTracking } }