GimbalControlSettings: implement FOV settings

ArduPilot · Oct 11, 2024 · 0fcc32d · 0fcc32d
1 parent a92d08b
commit 0fcc32d
Show file tree

Hide file tree

Showing 3 changed files with 57 additions and 8 deletions.
diff --git a/Controls/GimbalControlSettingsForm.cs b/Controls/GimbalControlSettingsForm.cs
@@ -268,8 +268,10 @@ public class GimbalControlSettings
 
         [Preferences("Zoom Speed (unitless)", ControlType.DecimalUpDown, Min = 0.01, Max = 1, Increment = 0.1, DecimalPlaces = 2)]
         public decimal ZoomSpeed { get; set; }
-        [Preferences("Camera FOV (deg)", ControlType.DecimalUpDown, Min = 0.01, Max = 180, Increment = 1, DecimalPlaces = 2)]
-        public decimal CameraFOV { get; set; }
+        [Preferences("Camera Horizontal FOV (deg)", ControlType.DecimalUpDown, Min = 0.01, Max = 180, Increment = 1, DecimalPlaces = 2)]
+        public decimal CameraHFOV { get; set; }
+        [Preferences("Camera Vertical FOV (deg)", ControlType.DecimalUpDown, Min = 0.01, Max = 180, Increment = 1, DecimalPlaces = 2)]
+        public decimal CameraVFOV { get; set; }
 
         // Boolean options
         [Preferences("Use FOV Reported by Camera", ControlType.Checkbox)]
@@ -311,7 +313,8 @@ public GimbalControlSettings()
             SlewSpeedNormal = 5m; // deg/sec
             SlewSpeedFast = 25m; // deg/sec
             ZoomSpeed = 1.0m; // unitless [0, 1]
-            CameraFOV = 50.0m; // horizontal, degrees
+            CameraHFOV = 40.0m; // horizontal, degrees
+            CameraVFOV = 30.0m; // vertical, degrees
 
             // Default boolean options
             DefaultLockedMode = false;

diff --git a/Controls/GimbalVideoControl.cs b/Controls/GimbalVideoControl.cs
@@ -554,6 +554,13 @@ private void UITimer_Tick(object sender, EventArgs e)
             takePictureToolStripMenuItem.Enabled = selectedCamera?.CanCaptureImage ?? false;
             startRecordingToolStripMenuItem.Enabled = selectedCamera?.CanCaptureVideo ?? false;
             stopRecordingToolStripMenuItem.Enabled = selectedCamera?.CanCaptureVideo ?? false;
+
+            if (selectedCamera != null)
+            {
+                selectedCamera.HFOV = (float)preferences.CameraHFOV;
+                selectedCamera.VFOV = (float)preferences.CameraVFOV;
+                selectedCamera.UseFOVStatus = preferences.UseFOVReportedByCamera;
+            }
         }
 
         private void yawLockToolStripMenuItem_Click(object sender, EventArgs e)

diff --git a/ExtLibs/ArduPilot/Mavlink/CameraProtocol.cs b/ExtLibs/ArduPilot/Mavlink/CameraProtocol.cs
@@ -106,6 +106,45 @@ public bool CanCaptureImage
             }
         }
 
+        public bool UseFOVStatus { get; set; } = true;
+
+        public float _hfov = float.NaN;
+        /// <summary>
+        /// Horizontal field of view of the camera, in degrees. Uses the latest received value from the camera if available and `UseFOVStatus` is true.
+        /// </summary>
+        public float HFOV
+        {
+            get
+            {
+                if (!UseFOVStatus || CameraFOVStatus.hfov == float.NaN)
+                {
+                    return _hfov;
+                }
+                return CameraFOVStatus.hfov;
+            }
+            set
+            {
+                _hfov = value;
+            }
+        }
+
+        public float _vfov = float.NaN;
+        public float VFOV
+        {
+            get
+            {
+                if (!UseFOVStatus || CameraFOVStatus.vfov == float.NaN)
+                {
+                    return _vfov;
+                }
+                return CameraFOVStatus.vfov;
+            }
+            set
+            {
+                _vfov = value;
+            }
+        }
+
         /// <summary>
         /// Initializes the camera protocol by setting up message parsing and requesting initial camera information.
         /// </summary>
@@ -384,12 +423,12 @@ public PointLatLngAlt CalculateImagePointLocation(double x, double y)
 
             var dist = camPosition.GetDistance(imagePosition);
             var down_elevation = Math.Atan2(height, dist); // zero means pointing level, pi/2 is straight down
-            down_elevation += y / 2 * CameraFOVStatus.vfov * Math.PI / 180;
+            down_elevation += y / 2 * VFOV * Math.PI / 180;
             down_elevation = Math.Max(0.0001, down_elevation);
             var out_distance = height * Math.Cos(down_elevation) / Math.Sin(down_elevation);
             out_distance = Math.Min(out_distance, 1e5);
 
-            var side_angle = x / 2 * CameraFOVStatus.hfov * Math.PI / 180;
+            var side_angle = x / 2 * HFOV * Math.PI / 180;
             var side_distance = Math.Sqrt(out_distance * out_distance + height * height) * Math.Tan(side_angle);
 
             var bearing = camPosition.GetBearing(imagePosition);
@@ -408,10 +447,10 @@ public PointLatLngAlt CalculateImagePointLocation(double x, double y)
         private Vector3 CalculateImagePointVectorCameraFrame(double x, double y)
         {
             var vector = new Vector3(1, 0, 0); // Camera-frame vector pointing straight ahead
-            if (CameraFOVStatus.hfov != float.NaN && CameraFOVStatus.vfov != float.NaN && x != 0 && y != 0)
+            if (HFOV != float.NaN && VFOV != float.NaN && x != 0 && y != 0)
             {
-                var hfov = CameraFOVStatus.hfov * Math.PI / 180;
-                var vfov = CameraFOVStatus.vfov * Math.PI / 180;
+                var hfov = HFOV * Math.PI / 180;
+                var vfov = VFOV * Math.PI / 180;
 
                 vector.y = Math.Tan(x * hfov / 2); // x in the image is toward the right side of the plane (positive y in camera frame)
                 vector.z = Math.Tan(y * vfov / 2); // y in the image is down (z in camera frame)