Skip to content

Commit

Permalink
Add camera tracking functionality
Browse files Browse the repository at this point in the history
  • Loading branch information
@DonLakeFlyer @julianoes
DonLakeFlyer authored and julianoes committed Apr 11, 2024
1 parent 8da219a commit fbe6126
Show file tree
Hide file tree
Showing 9 changed files with 395 additions and 5 deletions.
1 change: 1 addition & 0 deletions qgcimages.qrc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -185,6 +185,7 @@
<file alias="subMenuButtonImage.png">resources/CogWheels.png</file>
<file alias="subVehicleArrowOpaque.png">src/FlightMap/Images/sub.png</file>
<file alias="TelemRSSI.svg">src/ui/toolbar/Images/TelemRSSI.svg</file>
<file alias="TrackingIcon.svg">src/ui/toolbar/Images/TrackingIcon.svg</file>
<file alias="TuningComponentIcon.png">src/AutoPilotPlugins/Common/Images/TuningComponentIcon.png</file>
<file alias="vehicleArrowOpaque.svg">src/FlightMap/Images/vehicleArrowOpaque.svg</file>
<file alias="vehicleArrowOutline.svg">src/FlightMap/Images/vehicleArrowOutline.svg</file>
Expand Down
138 changes: 138 additions & 0 deletions src/Camera/QGCCameraControl.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -187,6 +187,15 @@ QGCCameraControl::QGCCameraControl(const mavlink_camera_information_t *info, Veh
_recTimer.setSingleShot(false);
_recTimer.setInterval(333);
connect(&_recTimer, &QTimer::timeout, this, &QGCCameraControl::_recTimerHandler);
//-- Tracking
if(_info.flags & CAMERA_CAP_FLAGS_HAS_TRACKING_RECTANGLE) {
_trackingStatus = static_cast<TrackingStatus>(_trackingStatus | TRACKING_RECTANGLE);
_trackingStatus = static_cast<TrackingStatus>(_trackingStatus | TRACKING_SUPPORTED);
}
if(_info.flags & CAMERA_CAP_FLAGS_HAS_TRACKING_POINT) {
_trackingStatus = static_cast<TrackingStatus>(_trackingStatus | TRACKING_POINT);
_trackingStatus = static_cast<TrackingStatus>(_trackingStatus | TRACKING_SUPPORTED);
}
}

//-----------------------------------------------------------------------------
Expand Down Expand Up @@ -1612,6 +1621,45 @@ QGCCameraControl::handleVideoStatus(const mavlink_video_stream_status_t* vs)
}
}

//-----------------------------------------------------------------------------
void
QGCCameraControl::handleTrackingImageStatus(const mavlink_camera_tracking_image_status_t *tis)
{
if (tis) {
_trackingImageStatus = *tis;

if (_trackingImageStatus.tracking_status == 0 || !trackingEnabled()) {
_trackingImageRect = {};
qCDebug(CameraControlLog) << "Tracking off";
} else {
if (_trackingImageStatus.tracking_mode == 2) {
_trackingImageRect = QRectF(QPointF(_trackingImageStatus.rec_top_x, _trackingImageStatus.rec_top_y),
QPointF(_trackingImageStatus.rec_bottom_x, _trackingImageStatus.rec_bottom_y));
} else {
float r = _trackingImageStatus.radius;
if (qIsNaN(r) || r <= 0 ) {
r = 0.05f;
}
// Bottom is NAN so that we can draw perfect square using video aspect ratio
_trackingImageRect = QRectF(QPointF(_trackingImageStatus.point_x - r, _trackingImageStatus.point_y - r),
QPointF(_trackingImageStatus.point_x + r, NAN));
}
// get rectangle into [0..1] boundaries
_trackingImageRect.setLeft(std::min(std::max(_trackingImageRect.left(), 0.0), 1.0));
_trackingImageRect.setTop(std::min(std::max(_trackingImageRect.top(), 0.0), 1.0));
_trackingImageRect.setRight(std::min(std::max(_trackingImageRect.right(), 0.0), 1.0));
_trackingImageRect.setBottom(std::min(std::max(_trackingImageRect.bottom(), 0.0), 1.0));

qCDebug(CameraControlLog) << "Tracking Image Status [left:" << _trackingImageRect.left()
<< "top:" << _trackingImageRect.top()
<< "right:" << _trackingImageRect.right()
<< "bottom:" << _trackingImageRect.bottom() << "]";
}

emit trackingImageStatusChanged();
}
}

//-----------------------------------------------------------------------------
void
QGCCameraControl::setCurrentStream(int stream)
Expand Down Expand Up @@ -2275,3 +2323,93 @@ QGCVideoStreamInfo::update(const mavlink_video_stream_status_t* vs)
}
return changed;
}

//-----------------------------------------------------------------------------
void
QGCCameraControl::setTrackingEnabled(bool set)
{
if(set) {
_trackingStatus = static_cast<TrackingStatus>(_trackingStatus | TRACKING_ENABLED);
} else {
_trackingStatus = static_cast<TrackingStatus>(_trackingStatus & ~TRACKING_ENABLED);
}
emit trackingEnabledChanged();
}

//-----------------------------------------------------------------------------
void
QGCCameraControl::startTracking(QRectF rec)
{
if(_trackingMarquee != rec) {
_trackingMarquee = rec;

qCDebug(CameraControlLog) << "Start Tracking (Rectangle: ["
<< static_cast<float>(rec.x()) << ", "
<< static_cast<float>(rec.y()) << "] - ["
<< static_cast<float>(rec.x() + rec.width()) << ", "
<< static_cast<float>(rec.y() + rec.height()) << "]";

_vehicle->sendMavCommand(_compID,
MAV_CMD_CAMERA_TRACK_RECTANGLE,
true,
static_cast<float>(rec.x()),
static_cast<float>(rec.y()),
static_cast<float>(rec.x() + rec.width()),
static_cast<float>(rec.y() + rec.height()));
}
}

//-----------------------------------------------------------------------------
void
QGCCameraControl::startTracking(QPointF point, double radius)
{
if(_trackingPoint != point || _trackingRadius != radius) {
_trackingPoint = point;
_trackingRadius = radius;

qCDebug(CameraControlLog) << "Start Tracking (Point: ["
<< static_cast<float>(point.x()) << ", "
<< static_cast<float>(point.y()) << "], Radius: "
<< static_cast<float>(radius);

_vehicle->sendMavCommand(_compID,
MAV_CMD_CAMERA_TRACK_POINT,
true,
static_cast<float>(point.x()),
static_cast<float>(point.y()),
static_cast<float>(radius));
}
}

//-----------------------------------------------------------------------------
void
QGCCameraControl::stopTracking()
{
qCDebug(CameraControlLog) << "Stop Tracking";

//-- Stop Tracking
_vehicle->sendMavCommand(_compID,
MAV_CMD_CAMERA_STOP_TRACKING,
true);

//-- Stop Sending Tracking Status
_vehicle->sendMavCommand(_compID,
MAV_CMD_SET_MESSAGE_INTERVAL,
true,
MAVLINK_MSG_ID_CAMERA_TRACKING_IMAGE_STATUS,
-1);

// reset tracking image rectangle
_trackingImageRect = {};
}

//-----------------------------------------------------------------------------
void
QGCCameraControl::_requestTrackingStatus()
{
_vehicle->sendMavCommand(_compID,
MAV_CMD_SET_MESSAGE_INTERVAL,
true,
MAVLINK_MSG_ID_CAMERA_TRACKING_IMAGE_STATUS,
500000); // Interval (us)
}
36 changes: 36 additions & 0 deletions src/Camera/QGCCameraControl.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -138,12 +138,21 @@ class QGCCameraControl : public FactGroup
THERMAL_PIP,
};

enum TrackingStatus {
TRACKING_UNKNOWN = 0,
TRACKING_SUPPORTED = 1,
TRACKING_ENABLED = 2,
TRACKING_RECTANGLE = 4,
TRACKING_POINT = 8
};

Q_ENUM(CameraMode)
Q_ENUM(VideoStatus)
Q_ENUM(PhotoStatus)
Q_ENUM(PhotoMode)
Q_ENUM(StorageStatus)
Q_ENUM(ThermalViewMode)
Q_ENUM(TrackingStatus)

Q_PROPERTY(int version READ version NOTIFY infoChanged)
Q_PROPERTY(QString modelName READ modelName NOTIFY infoChanged)
Expand All @@ -158,6 +167,7 @@ class QGCCameraControl : public FactGroup
Q_PROPERTY(bool hasZoom READ hasZoom NOTIFY infoChanged)
Q_PROPERTY(bool hasFocus READ hasFocus NOTIFY infoChanged)
Q_PROPERTY(bool hasVideoStream READ hasVideoStream NOTIFY infoChanged)
Q_PROPERTY(bool hasTracking READ hasTracking NOTIFY infoChanged)
Q_PROPERTY(bool photosInVideoMode READ photosInVideoMode NOTIFY infoChanged)
Q_PROPERTY(bool videoInPhotoMode READ videoInPhotoMode NOTIFY infoChanged)
Q_PROPERTY(bool isBasic READ isBasic NOTIFY infoChanged)
Expand Down Expand Up @@ -197,6 +207,10 @@ class QGCCameraControl : public FactGroup
Q_PROPERTY(QStringList streamLabels READ streamLabels NOTIFY streamLabelsChanged)
Q_PROPERTY(ThermalViewMode thermalMode READ thermalMode WRITE setThermalMode NOTIFY thermalModeChanged)
Q_PROPERTY(double thermalOpacity READ thermalOpacity WRITE setThermalOpacity NOTIFY thermalOpacityChanged)
Q_PROPERTY(bool trackingEnabled READ trackingEnabled WRITE setTrackingEnabled NOTIFY trackingEnabledChanged)
Q_PROPERTY(TrackingStatus trackingStatus READ trackingStatus CONSTANT)
Q_PROPERTY(bool trackingImageStatus READ trackingImageStatus NOTIFY trackingImageStatusChanged)
Q_PROPERTY(QRectF trackingImageRect READ trackingImageRect NOTIFY trackingImageStatusChanged)

Q_INVOKABLE virtual void setVideoMode ();
Q_INVOKABLE virtual void setPhotoMode ();
Expand All @@ -213,6 +227,9 @@ class QGCCameraControl : public FactGroup
Q_INVOKABLE virtual void stopZoom ();
Q_INVOKABLE virtual void stopStream ();
Q_INVOKABLE virtual void resumeStream ();
Q_INVOKABLE virtual void startTracking (QRectF rec);
Q_INVOKABLE virtual void startTracking (QPointF point, double radius);
Q_INVOKABLE virtual void stopTracking ();

virtual int version () { return _version; }
virtual QString modelName () { return _modelName; }
Expand All @@ -226,6 +243,7 @@ class QGCCameraControl : public FactGroup
virtual bool hasModes () { return _info.flags & CAMERA_CAP_FLAGS_HAS_MODES; }
virtual bool hasZoom () { return _info.flags & CAMERA_CAP_FLAGS_HAS_BASIC_ZOOM; }
virtual bool hasFocus () { return _info.flags & CAMERA_CAP_FLAGS_HAS_BASIC_FOCUS; }
virtual bool hasTracking () { return _trackingStatus & TRACKING_SUPPORTED; }
virtual bool hasVideoStream () { return _info.flags & CAMERA_CAP_FLAGS_HAS_VIDEO_STREAM; }
virtual bool photosInVideoMode () { return _info.flags & CAMERA_CAP_FLAGS_CAN_CAPTURE_IMAGE_IN_VIDEO_MODE; }
virtual bool videoInPhotoMode () { return _info.flags & CAMERA_CAP_FLAGS_CAN_CAPTURE_VIDEO_IN_IMAGE_MODE; }
Expand Down Expand Up @@ -287,9 +305,18 @@ class QGCCameraControl : public FactGroup
virtual void handleParamValue (const mavlink_param_ext_value_t& value);
virtual void handleStorageInfo (const mavlink_storage_information_t& st);
virtual void handleBatteryStatus (const mavlink_battery_status_t& bs);
virtual void handleTrackingImageStatus(const mavlink_camera_tracking_image_status_t *tis);
virtual void handleVideoInfo (const mavlink_video_stream_information_t *vi);
virtual void handleVideoStatus (const mavlink_video_stream_status_t *vs);

virtual bool trackingEnabled () { return _trackingStatus & TRACKING_ENABLED; }
virtual void setTrackingEnabled (bool set);

virtual TrackingStatus trackingStatus () { return _trackingStatus; }

virtual bool trackingImageStatus() { return _trackingImageStatus.tracking_status == 1; }
virtual QRectF trackingImageRect() { return _trackingImageRect; }

/// Notify controller a parameter has changed
virtual void factChanged (Fact* pFact);
/// Allow controller to modify or invalidate incoming parameter
Expand Down Expand Up @@ -328,6 +355,8 @@ class QGCCameraControl : public FactGroup
void autoStreamChanged ();
void recordTimeChanged ();
void streamLabelsChanged ();
void trackingEnabledChanged ();
void trackingImageStatusChanged ();
void thermalModeChanged ();
void thermalOpacityChanged ();
void storageStatusChanged ();
Expand All @@ -338,6 +367,7 @@ class QGCCameraControl : public FactGroup
virtual void _setCameraMode (CameraMode mode);
virtual void _requestStreamInfo (uint8_t streamID);
virtual void _requestStreamStatus (uint8_t streamID);
virtual void _requestTrackingStatus ();
virtual QGCVideoStreamInfo* _findStream (uint8_t streamID, bool report = true);
virtual QGCVideoStreamInfo* _findStream (const QString name);

Expand Down Expand Up @@ -428,4 +458,10 @@ protected slots:
QStringList _streamLabels;
ThermalViewMode _thermalMode = THERMAL_BLEND;
double _thermalOpacity = 85.0;
TrackingStatus _trackingStatus = TRACKING_UNKNOWN;
QRectF _trackingMarquee;
QPointF _trackingPoint;
double _trackingRadius = 0.0;
mavlink_camera_tracking_image_status_t _trackingImageStatus;
QRectF _trackingImageRect;
};
16 changes: 16 additions & 0 deletions src/Camera/QGCCameraManager.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -101,6 +101,9 @@ QGCCameraManager::_mavlinkMessageReceived(const mavlink_message_t& message)
case MAVLINK_MSG_ID_BATTERY_STATUS:
_handleBatteryStatus(message);
break;
case MAVLINK_MSG_ID_CAMERA_TRACKING_IMAGE_STATUS:
_handleTrackingImageStatus(message);
break;
}
}
}
Expand Down Expand Up @@ -205,6 +208,7 @@ QGCCameraManager::_findCamera(int id)
void
QGCCameraManager::_handleCameraInfo(const mavlink_message_t& message)
{
qCDebug(CameraManagerLog) << "_handleCameraInfo";
//-- Have we requested it?
QString sCompID = QString::number(message.compid);
if(_cameraInfoRequest.contains(sCompID) && !_cameraInfoRequest[sCompID]->infoReceived) {
Expand Down Expand Up @@ -369,6 +373,18 @@ QGCCameraManager::_handleBatteryStatus(const mavlink_message_t& message)
}
}

//-----------------------------------------------------------------------------
void
QGCCameraManager::_handleTrackingImageStatus(const mavlink_message_t& message)
{
QGCCameraControl* pCamera = _findCamera(message.compid);
if(pCamera) {
mavlink_camera_tracking_image_status_t tis;
mavlink_msg_camera_tracking_image_status_decode(&message, &tis);
pCamera->handleTrackingImageStatus(&tis);
}
}

//-----------------------------------------------------------------------------
void
QGCCameraManager::_requestCameraInfo(int compID)
Expand Down
1 change: 1 addition & 0 deletions src/Camera/QGCCameraManager.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -85,6 +85,7 @@ protected slots:
virtual void _handleVideoStreamInfo (const mavlink_message_t& message);
virtual void _handleVideoStreamStatus(const mavlink_message_t& message);
virtual void _handleBatteryStatus (const mavlink_message_t& message);
virtual void _handleTrackingImageStatus(const mavlink_message_t& message);

protected:

Expand Down
8 changes: 8 additions & 0 deletions src/FlightDisplay/FlightDisplayViewVideo.qml
View file
Original file line number Diff line number Diff line change
Expand Up @@ -36,6 +36,13 @@ Item {
property bool _hasZoom: _camera && _camera.hasZoom
property int _fitMode: QGroundControl.settingsManager.videoSettings.videoFit.rawValue

function getWidth() {
return videoBackground.getWidth()
}
function getHeight() {
return videoBackground.getHeight()
}

property double _thermalHeightFactor: 0.85 //-- TODO

Image {
Expand Down Expand Up @@ -65,6 +72,7 @@ Item {
}

Rectangle {
id: videoBackground
anchors.fill: parent
color: "black"
visible: QGroundControl.videoManager.decoding
Expand Down
Loading

0 comments on commit fbe6126

Please sign in to comment.