diff --git a/Controls/GimbalControlSettingsForm.cs b/Controls/GimbalControlSettingsForm.cs index c6375d4906..85419c8056 100644 --- 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 index a67805e7eb..34b983958f 100644 --- 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 index cbef3e0cf7..29f99c20f0 100644 --- 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; + /// + /// Horizontal field of view of the camera, in degrees. Uses the latest received value from the camera if available and `UseFOVStatus` is true. + /// + 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; + } + } + /// /// Initializes the camera protocol by setting up message parsing and requesting initial camera information. /// @@ -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)