diff --git a/src/Camera/QGCCameraManager.cc b/src/Camera/QGCCameraManager.cc index 97ff2bbd9884..926911f50a7f 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,34 @@ 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; + if (auto* cam = mgr->currentCameraInstance()) { + } +} + /*===========================================================================*/ QGCCameraManager::CameraStruct::CameraStruct(QGCCameraManager *manager_, uint8_t compID_, Vehicle *vehicle_) @@ -148,6 +183,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 +327,25 @@ 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 / 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); + + if (sensorH > 0.0 && sensorV > 0.0 && focal > 0.0) { + const double hfovDeg = 2.0 * std::atan(sensorH / (2.0 * focal)) * 180.0 / M_PI; + const double vfovDeg = 2.0 * std::atan(sensorV / (2.0 * focal)) * 180.0 / M_PI; + } } void QGCCameraManager::_checkForLostCameras() @@ -364,13 +421,16 @@ 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); + + zoomValueCurrent = static_cast(settings.zoomLevel); + requestCameraFovForComp(message.compid); } } @@ -680,3 +740,41 @@ const QVariantList &QGCCameraManager::cameraList() const } return _cameraList; } + +void QGCCameraManager::requestCameraFovForComp(int compId) { + _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() const { + if (auto* cam = const_cast(this)->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 = 9.0/16.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; + SettingsManager::instance()->gimbalControllerSettings()->CameraHFov()->setRawValue(QVariant::fromValue(fov.hfov)); + SettingsManager::instance()->gimbalControllerSettings()->CameraVFov()->setRawValue(QVariant::fromValue(vfovDeg)); +} + +int QGCCameraManager::currentZoomLevel() const { + return zoomValueCurrent; +} diff --git a/src/Camera/QGCCameraManager.h b/src/Camera/QGCCameraManager.h index 4aa7aa1f23b1..c82a1469832f 100644 --- a/src/Camera/QGCCameraManager.h +++ b/src/Camera/QGCCameraManager.h @@ -82,6 +82,15 @@ class QGCCameraManager : public QObject CameraStruct* findCameraStruct(uint8_t compId) const { return _cameraInfoRequest.value(QString::number(compId), nullptr); } + int zoomValueCurrent = 0; + int currentZoomLevel() const; + double aspectForComp(int compId) const; + double currentCameraAspect() const; + Q_INVOKABLE void requestCameraFovForComp(int compId); + +private: + QHash _aspectByCompId; + signals: void camerasChanged(); void cameraLabelsChanged(); @@ -118,6 +127,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; diff --git a/src/Gimbal/Gimbal.h b/src/Gimbal/Gimbal.h index 133909c4db8b..c5d4ccd96ad0 100644 --- a/src/Gimbal/Gimbal.h +++ b/src/Gimbal/Gimbal.h @@ -111,4 +111,4 @@ class Gimbal : public FactGroup bool _retracted = false; bool _haveControl = false; bool _othersHaveControl = false; -}; +}; \ No newline at end of file diff --git a/src/Gimbal/GimbalController.cc b/src/Gimbal/GimbalController.cc index 1405828c5f4f..24434e363d34 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 e570271e7651..7ad9ea706f75 100644 --- a/src/Gimbal/GimbalController.h +++ b/src/Gimbal/GimbalController.h @@ -50,6 +50,8 @@ class GimbalController : public QObject Q_INVOKABLE void releaseGimbalControl(); Q_INVOKABLE void sendRate(); + 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 +107,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 efc667b2150f..562dcfc24e3d 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" } ] } \ No newline at end of file diff --git a/src/Settings/GimbalControllerSettings.cc b/src/Settings/GimbalControllerSettings.cc index c3dd13eba3db..40c3ab5ec973 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 7b8e375109e5..80577c11b297 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 22e5dea1b6a0..8d160d303888 100644 --- a/src/UI/toolbar/GimbalIndicator.qml +++ b/src/UI/toolbar/GimbalIndicator.qml @@ -273,6 +273,32 @@ Item { } } + SettingsGroupLayout { + heading: qsTr("Zoom speed") + showDividers: false + + QGCLabel { + text: qsTr("Max speed (min zoom):") + visible: true + } + + FactTextField { + fact: QGroundControl.settingsManager.gimbalControllerSettings.zoomMaxSpeed + visible: true + } + + QGCLabel { + text: qsTr("Min speed (max zoom):") + visible: true + } + + FactTextField { + fact: QGroundControl.settingsManager.gimbalControllerSettings.zoomMinSpeed + visible: true + } + + } + SettingsGroupLayout { LabelledFactTextField { label: qsTr("Joystick buttons speed:") diff --git a/src/Vehicle/VehicleSetup/JoystickConfigCalibration.qml b/src/Vehicle/VehicleSetup/JoystickConfigCalibration.qml index aa88f0d05989..a8a5aaf2906a 100644 --- a/src/Vehicle/VehicleSetup/JoystickConfigCalibration.qml +++ b/src/Vehicle/VehicleSetup/JoystickConfigCalibration.qml @@ -199,5 +199,3 @@ Item { } } } - -