Gimbal speed auto fov

src/Camera/QGCCameraManager.cc

@@ -18,6 +18,13 @@
 #include "QGCVideoStreamInfo.h"
 #include "SimulatedCameraControl.h"
 
+#include <cmath>
+#include "GimbalControllerSettings.h"
+#include "SettingsManager.h"
+#include <numbers>
+
+constexpr double kPi = std::numbers::pi_v<double>;
+
 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<QGCCameraManager*>(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<double>::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<double>(info.sensor_size_h);
+    const double sensorV = static_cast<double>(info.sensor_size_v);
+    const double focal   = static_cast<double>(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<double>(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<double>::quiet_NaN()
+           : it.value();
+}
+
+double QGCCameraManager::currentCameraAspect() const {
+    if (auto* cam = const_cast<QGCCameraManager*>(this)->currentCameraInstance()) {
+        return aspectForComp(cam->compID());
+    }
+    return std::numeric_limits<double>::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;
+}

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<int, double> _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> _vehicle;
     QPointer<SimulatedCameraControl> _simulatedCameraControl;

src/Gimbal/Gimbal.h

@@ -111,4 +111,4 @@ class Gimbal : public FactGroup
     bool _retracted = false;
     bool _haveControl = false;
     bool _othersHaveControl = false;
-};
+};

src/Gimbal/GimbalController.cc

@@ -15,6 +15,9 @@
 #include "QmlObjectListModel.h"
 #include "SettingsManager.h"
 #include "Vehicle.h"
+#include <cmath>
+#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<uint8_t>(_activeGimbal->managerCompid()->rawValue().toUInt()),
+        flags,
+        static_cast<uint8_t>(_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.

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;

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"
 }
 ]
 }

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)

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)
 };

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:")

src/Vehicle/VehicleSetup/JoystickConfigCalibration.qml

@@ -199,5 +199,3 @@ Item {
         }
     }
 }
-
-