diff --git a/src/Camera/QGCCameraManager.cc b/src/Camera/QGCCameraManager.cc index 0c570230dc4..8af5c4ea464 100644 --- a/src/Camera/QGCCameraManager.cc +++ b/src/Camera/QGCCameraManager.cc @@ -18,6 +18,13 @@ #include "QGCVideoStreamInfo.h" #include "SimulatedCameraControl.h" +#include +#include "GimbalControllerSettings.h" +#include "SettingsManager.h" +#include + +constexpr double kPi = std::numbers::pi_v; + QGC_LOGGING_CATEGORY(CameraManagerLog, "Camera.QGCCameraManager") namespace { @@ -28,6 +35,32 @@ namespace { QVariantList QGCCameraManager::_cameraList; +static void _requestFovOnZoom_Handler( + void* user, + MAV_RESULT result, + Vehicle::RequestMessageResultHandlerFailureCode_t failureCode, + const mavlink_message_t& message) +{ + auto* mgr = static_cast(user); + + if (result != MAV_RESULT_ACCEPTED) { + qCDebug(CameraManagerLog) << "CAMERA_FOV_STATUS request failed, result:" + << result << "failure:" << failureCode; + return; + } + + if (message.msgid != MAVLINK_MSG_ID_CAMERA_FOV_STATUS) { + qCDebug(CameraManagerLog) << "Unexpected msg id:" << message.msgid; + return; + } + + mavlink_camera_fov_status_t fov{}; + + mavlink_msg_camera_fov_status_decode(&message, &fov); + + if (!mgr) return; +} + /*===========================================================================*/ QGCCameraManager::CameraStruct::CameraStruct(QGCCameraManager *manager_, uint8_t compID_, Vehicle *vehicle_) @@ -148,6 +181,9 @@ void QGCCameraManager::_mavlinkMessageReceived(const mavlink_message_t &message) case MAVLINK_MSG_ID_CAMERA_TRACKING_IMAGE_STATUS: _handleTrackingImageStatus(message); break; + case MAVLINK_MSG_ID_CAMERA_FOV_STATUS: + _handleCameraFovStatus(message); + break; default: break; } @@ -289,6 +325,20 @@ void QGCCameraManager::_handleCameraInfo(const mavlink_message_t& message) _cameraInfoRequest[sCompID]->backoffTimer.stop(); qCDebug(CameraManagerLog) << "Success for compId" << QGCMAVLink::compIdToString(message.compid) << "- reset retry counter"; } + + double aspect = std::numeric_limits::quiet_NaN(); + + if (info.resolution_h > 0 && info.resolution_v > 0) { + aspect = double(info.resolution_v) / double(info.resolution_h); + } else if (info.sensor_size_h > 0.f && info.sensor_size_v > 0.f) { + aspect = double(info.sensor_size_v) / double(info.sensor_size_h); + } + + _aspectByCompId.insert(message.compid, aspect); + + const double sensorH = static_cast(info.sensor_size_h); + const double sensorV = static_cast(info.sensor_size_v); + const double focal = static_cast(info.focal_length); } void QGCCameraManager::_checkForLostCameras() @@ -364,13 +414,27 @@ void QGCCameraManager::_handleStorageInfo(const mavlink_message_t &message) } } -void QGCCameraManager::_handleCameraSettings(const mavlink_message_t &message) +void QGCCameraManager::_handleCameraSettings(const mavlink_message_t& message) { - MavlinkCameraControl *pCamera = _findCamera(message.compid); + auto pCamera = _findCamera(message.compid); if (pCamera) { mavlink_camera_settings_t settings{}; mavlink_msg_camera_settings_decode(&message, &settings); pCamera->handleSettings(settings); + + const int newZoom = static_cast(settings.zoomLevel); + if (QThread::currentThread() == thread()) { + _setCurrentZoomLevel(newZoom); + } else { + QMetaObject::invokeMethod( + this, + "_setCurrentZoomLevel", + Qt::QueuedConnection, + Q_ARG(int, newZoom) + ); + } + + requestCameraFovForComp(message.compid); } } @@ -680,3 +744,71 @@ const QVariantList &QGCCameraManager::cameraList() const } return _cameraList; } + +void QGCCameraManager::requestCameraFovForComp(int compId) { + if (!_vehicle) { + qCWarning(CameraManagerLog) << "requestCameraFovForComp: vehicle is null"; + return; + } + _vehicle->requestMessage(_requestFovOnZoom_Handler, /*user*/this, + compId, MAVLINK_MSG_ID_CAMERA_FOV_STATUS); +} + +//----------------------------------------------------------------------------- +double QGCCameraManager::aspectForComp(int compId) const { + auto it = _aspectByCompId.constFind(compId); + return (it == _aspectByCompId.cend()) + ? std::numeric_limits::quiet_NaN() + : it.value(); +} + +double QGCCameraManager::currentCameraAspect(){ + if (auto* cam = currentCameraInstance()) { + return aspectForComp(cam->compID()); + } + return std::numeric_limits::quiet_NaN(); +} +void QGCCameraManager::_handleCameraFovStatus(const mavlink_message_t& message) +{ + mavlink_camera_fov_status_t fov{}; + mavlink_msg_camera_fov_status_decode(&message, &fov); + + if (!std::isfinite(fov.hfov) || fov.hfov <= 0.0 || fov.hfov >= 180.0) { + return; + } + + double aspect = aspectForComp(message.compid); + if (!std::isfinite(aspect) || aspect <= 0.0) { + aspect = 16.0 / 9.0; + } + + const double hfovRad = fov.hfov * kPi / 180.0; + const double vfovRad = 2.0 * std::atan(std::tan(hfovRad * 0.5) * aspect); + const double vfovDeg = vfovRad * 180.0 / kPi; + + if (!std::isfinite(vfovDeg) || vfovDeg <= 0.0 || vfovDeg >= 180.0) { + qCWarning(CameraManagerLog) << "Invalid calculated VFOV:" << vfovDeg + << "hfov:" << fov.hfov + << "aspect:" << aspect + << "compId:" << message.compid; + return; + } + + auto* settings = SettingsManager::instance()->gimbalControllerSettings(); + settings->CameraHFov()->setRawValue(fov.hfov); + settings->CameraVFov()->setRawValue(vfovDeg); +} + +void QGCCameraManager::_setCurrentZoomLevel(int level) +{ + if (_zoomValueCurrent == level) { + return; + } + _zoomValueCurrent = level; + emit currentZoomLevelChanged(); +} + +int QGCCameraManager::currentZoomLevel() const +{ + return _zoomValueCurrent; +} diff --git a/src/Camera/QGCCameraManager.h b/src/Camera/QGCCameraManager.h index 4aa7aa1f23b..36aad33ee4b 100644 --- a/src/Camera/QGCCameraManager.h +++ b/src/Camera/QGCCameraManager.h @@ -44,6 +44,7 @@ class QGCCameraManager : public QObject Q_PROPERTY(QStringList cameraLabels READ cameraLabels NOTIFY cameraLabelsChanged) Q_PROPERTY(MavlinkCameraControl* currentCameraInstance READ currentCameraInstance NOTIFY currentCameraChanged) Q_PROPERTY(int currentCamera READ currentCamera WRITE setCurrentCamera NOTIFY currentCameraChanged) + Q_PROPERTY(int currentZoomLevel READ currentZoomLevel NOTIFY currentZoomLevelChanged) friend class QGCCameraManagerTest; @@ -82,12 +83,22 @@ class QGCCameraManager : public QObject CameraStruct* findCameraStruct(uint8_t compId) const { return _cameraInfoRequest.value(QString::number(compId), nullptr); } + int currentZoomLevel() const; + double aspectForComp(int compId) const; + double currentCameraAspect(); + Q_INVOKABLE void requestCameraFovForComp(int compId); + +private: + int _zoomValueCurrent = 0; + signals: void camerasChanged(); void cameraLabelsChanged(); void currentCameraChanged(); void streamChanged(); + void currentZoomLevelChanged(); + protected slots: void _vehicleReady(bool ready); void _mavlinkMessageReceived(const mavlink_message_t& message); @@ -103,6 +114,9 @@ protected slots: void _stopVideoRecording(); void _toggleVideoRecording(); +private slots: + void _setCurrentZoomLevel(int level); + private: MavlinkCameraControl* _findCamera(int id); void _requestCameraInfo(CameraStruct* cameraInfo); @@ -118,6 +132,7 @@ protected slots: void _handleBatteryStatus(const mavlink_message_t& message); void _handleTrackingImageStatus(const mavlink_message_t& message); void _addCameraControlToLists(MavlinkCameraControl* cameraControl); + void _handleCameraFovStatus(const mavlink_message_t& message); QPointer _vehicle; QPointer _simulatedCameraControl; @@ -132,4 +147,6 @@ protected slots: QTimer _camerasLostHeartbeatTimer; QMap _cameraInfoRequest; static QVariantList _cameraList; + + QHash _aspectByCompId; }; diff --git a/src/Gimbal/GimbalController.cc b/src/Gimbal/GimbalController.cc index 1405828c5f4..24434e363d3 100644 --- a/src/Gimbal/GimbalController.cc +++ b/src/Gimbal/GimbalController.cc @@ -15,6 +15,9 @@ #include "QmlObjectListModel.h" #include "SettingsManager.h" #include "Vehicle.h" +#include +#include "Gimbal.h" +#include "QGCCameraManager.h" QGC_LOGGING_CATEGORY(GimbalControllerLog, "Gimbal.GimbalController") @@ -591,6 +594,62 @@ void GimbalController::sendRate() } } +void GimbalController::sendGimbalRate(float pitch_rate_deg_s, float yaw_rate_deg_s) +{ + if (!_tryGetGimbalControl()) { + return; + } + + _sendGimbalAttitudeRates(pitch_rate_deg_s, yaw_rate_deg_s); + + if (pitch_rate_deg_s == 0.f && yaw_rate_deg_s == 0.f) { + _rateSenderTimer.stop(); + } else { + _rateSenderTimer.start(); + } +} + +void GimbalController::_sendGimbalAttitudeRates(float pitch_rate_deg_s, + float yaw_rate_deg_s) +{ + + auto sharedLink = _vehicle->vehicleLinkManager()->primaryLink().lock(); + if (!sharedLink) { + qCDebug(GimbalControllerLog) << "_sendGimbalAttitudeRates: primary link gone!"; + return; + } + + uint32_t flags = + GIMBAL_MANAGER_FLAGS_ROLL_LOCK | + GIMBAL_MANAGER_FLAGS_PITCH_LOCK | + GIMBAL_MANAGER_FLAGS_YAW_IN_VEHICLE_FRAME; // use vehicle/body frame + + // Preserve current yaw-lock state instead of changing it: + if (_activeGimbal->yawLock()) { + flags |= GIMBAL_MANAGER_FLAGS_YAW_LOCK; + } + + const float qnan[4] = {NAN, NAN, NAN, NAN}; + mavlink_message_t msg; + + mavlink_msg_gimbal_manager_set_attitude_pack_chan( + MAVLinkProtocol::instance()->getSystemId(), + MAVLinkProtocol::getComponentId(), + sharedLink->mavlinkChannel(), + &msg, + _vehicle->id(), + static_cast(_activeGimbal->managerCompid()->rawValue().toUInt()), + flags, + static_cast(_activeGimbal->deviceId()->rawValue().toUInt()), + qnan, + NAN, + qDegreesToRadians(pitch_rate_deg_s), + qDegreesToRadians(yaw_rate_deg_s) + ); + + _vehicle->sendMessageOnLinkThreadSafe(sharedLink.get(), msg); +} + void GimbalController::_rateSenderTimeout() { // Send rate again to avoid timeout on autopilot side. diff --git a/src/Gimbal/GimbalController.h b/src/Gimbal/GimbalController.h index e570271e765..2b0a271c7db 100644 --- a/src/Gimbal/GimbalController.h +++ b/src/Gimbal/GimbalController.h @@ -50,6 +50,11 @@ class GimbalController : public QObject Q_INVOKABLE void releaseGimbalControl(); Q_INVOKABLE void sendRate(); + /// Send gimbal attitude rates directly without using active gimbal's rate properties + /// @param pitch_rate_deg_s Pitch rate in degrees per second + /// @param yaw_rate_deg_s Yaw rate in degrees per second + Q_INVOKABLE void sendGimbalRate(float pitch_rate_deg_s, float yaw_rate_deg_s); + signals: void activeGimbalChanged(); void showAcquireGimbalControlPopup(); // This triggers a popup in QML asking the user for aproval to take control @@ -105,6 +110,8 @@ private slots: bool _tryGetGimbalControl(); bool _yawInVehicleFrame(uint32_t flags); + void _sendGimbalAttitudeRates(float pitch_rate_deg_s, float yaw_rate_deg_s); + QTimer _rateSenderTimer; Vehicle *_vehicle = nullptr; Gimbal *_activeGimbal = nullptr; diff --git a/src/Settings/GimbalController.SettingsGroup.json b/src/Settings/GimbalController.SettingsGroup.json index e45fac5d1da..343a2550dfa 100644 --- a/src/Settings/GimbalController.SettingsGroup.json +++ b/src/Settings/GimbalController.SettingsGroup.json @@ -21,14 +21,14 @@ "name": "CameraVFov", "shortDesc": "Vertical camera field of view", "type": "uint32", - "default": 70, + "default": 80, "units": "deg" }, { "name": "CameraHFov", "shortDesc": "Horizontal camera field of view", "type": "uint32", - "default": 70, + "default": 80, "units": "deg" }, { @@ -63,6 +63,20 @@ "type": "uint32", "default": 20, "units": "deg/s" +}, +{ + "name": "zoomMinSpeed", + "shortDesc": "Maximum gimbal speed for min zoom (deg/sec)", + "type": "uint32", + "default": 5, + "units": "deg/s" +}, +{ + "name": "zoomMaxSpeed", + "shortDesc": "Minimum gimbal speed for max zoom (deg/sec)", + "type": "uint32", + "default": 60, + "units": "deg/s" } ] } diff --git a/src/Settings/GimbalControllerSettings.cc b/src/Settings/GimbalControllerSettings.cc index c3dd13eba3d..40c3ab5ec97 100644 --- a/src/Settings/GimbalControllerSettings.cc +++ b/src/Settings/GimbalControllerSettings.cc @@ -22,3 +22,5 @@ DECLARE_SETTINGSFACT(GimbalControllerSettings, showAzimuthIndicatorOnMap) DECLARE_SETTINGSFACT(GimbalControllerSettings, toolbarIndicatorShowAzimuth) DECLARE_SETTINGSFACT(GimbalControllerSettings, toolbarIndicatorShowAcquireReleaseControl) DECLARE_SETTINGSFACT(GimbalControllerSettings, joystickButtonsSpeed) +DECLARE_SETTINGSFACT(GimbalControllerSettings, zoomMaxSpeed) +DECLARE_SETTINGSFACT(GimbalControllerSettings, zoomMinSpeed) diff --git a/src/Settings/GimbalControllerSettings.h b/src/Settings/GimbalControllerSettings.h index 7b8e375109e..80577c11b29 100644 --- a/src/Settings/GimbalControllerSettings.h +++ b/src/Settings/GimbalControllerSettings.h @@ -30,4 +30,6 @@ class GimbalControllerSettings : public SettingsGroup DEFINE_SETTINGFACT(toolbarIndicatorShowAzimuth) DEFINE_SETTINGFACT(toolbarIndicatorShowAcquireReleaseControl) DEFINE_SETTINGFACT(joystickButtonsSpeed) + DEFINE_SETTINGFACT(zoomMaxSpeed) + DEFINE_SETTINGFACT(zoomMinSpeed) }; diff --git a/src/UI/toolbar/GimbalIndicator.qml b/src/UI/toolbar/GimbalIndicator.qml index 562b121b63d..e5470a0160d 100644 --- a/src/UI/toolbar/GimbalIndicator.qml +++ b/src/UI/toolbar/GimbalIndicator.qml @@ -273,6 +273,22 @@ Item { } } + SettingsGroupLayout { + heading: qsTr("Zoom speed") + showDividers: false + + LabelledFactTextField { + label: qsTr("Max speed (min zoom)") + fact: _gimbalControllerSettings.zoomMaxSpeed + } + + LabelledFactTextField { + label: qsTr("Min speed (max zoom)") + fact: _gimbalControllerSettings.zoomMinSpeed + } + + } + SettingsGroupLayout { LabelledFactTextField { label: qsTr("Joystick buttons speed:")