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)