From 125ef35a99dc7b6a44dfe6c6886e242fbf978a5e Mon Sep 17 00:00:00 2001 From: Bob Long Date: Fri, 27 Feb 2026 19:10:48 +1100 Subject: [PATCH 1/6] Add MessageRateManager for lease-based rates - Lease-based wrapper around GET/SET_MESSAGE_INTERVAL. - Fastest active lease wins; restores baseline on release. --- ExtLibs/ArduPilot/Mavlink/MAVLinkInterface.cs | 7 + .../ArduPilot/Mavlink/MessageRateManager.cs | 709 ++++++++++++++++++ 2 files changed, 716 insertions(+) create mode 100644 ExtLibs/ArduPilot/Mavlink/MessageRateManager.cs diff --git a/ExtLibs/ArduPilot/Mavlink/MAVLinkInterface.cs b/ExtLibs/ArduPilot/Mavlink/MAVLinkInterface.cs index 3cd030b7f4..e29d7070b3 100644 --- a/ExtLibs/ArduPilot/Mavlink/MAVLinkInterface.cs +++ b/ExtLibs/ArduPilot/Mavlink/MAVLinkInterface.cs @@ -282,6 +282,8 @@ public bool giveComport public bool ReadOnly = false; + public MessageRateManager RateManager { get; private set; } + public TerrainFollow Terrain; public event ProgressEventHandler Progress; @@ -491,6 +493,8 @@ public MAVLinkInterface() _mavlink2count = 0; _mavlink2signed = 0; + RateManager = new MessageRateManager(this); + AIS.Start(this); // new hearbeat detected @@ -967,6 +971,7 @@ No Mavlink Heartbeat Packets where read from this port - Verify Baud Rate and se MAV.packetslost = 0; MAV.synclost = 0; _openComplete = true; + RateManager.OnConnectionOpen(); } private string getAppVersion() @@ -6831,6 +6836,8 @@ public void Dispose() _bytesSentSubj.Dispose(); if (MAVlist != null) MAVlist.Dispose(); + if (RateManager != null) + RateManager.Dispose(); this.Close(); diff --git a/ExtLibs/ArduPilot/Mavlink/MessageRateManager.cs b/ExtLibs/ArduPilot/Mavlink/MessageRateManager.cs new file mode 100644 index 0000000000..985c0873a4 --- /dev/null +++ b/ExtLibs/ArduPilot/Mavlink/MessageRateManager.cs @@ -0,0 +1,709 @@ +using System; +using System.Collections.Generic; +using System.Diagnostics; +using System.Linq; +using System.Reflection; +using System.Threading; +using System.Threading.Tasks; +using log4net; +using static MAVLink; + +namespace MissionPlanner.ArduPilot.Mavlink +{ + /// + /// Represents a disposable lease on a MAVLink message rate. + /// + public sealed class MessageRateLease : IDisposable + { + internal readonly uint MsgId; + internal readonly byte SysId; + internal readonly byte CompId; + internal readonly double Hz; + internal readonly string Owner; + internal int Released; + + private readonly MessageRateManager _manager; + + internal MessageRateLease(MessageRateManager manager, uint msgId, byte sysid, + byte compid, double hz, string owner) + { + _manager = manager; + MsgId = msgId; + SysId = sysid; + CompId = compid; + Hz = hz; + Owner = owner ?? ""; + } + + public void Dispose() + { + _manager.Release(this); + } + } + + /// + /// Provides lease-based management of per-message MAVLink streaming rates. + /// + /// + /// One instance per . The fastest active lease wins. + /// When all leases for a message are released, SET_MESSAGE_INTERVAL(0) restores the + /// autopilot's default rate. + /// + public class MessageRateManager : IDisposable + { + private static readonly ILog log = LogManager.GetLogger( + MethodBase.GetCurrentMethod().DeclaringType); + + private readonly MAVLinkInterface _port; + private readonly object _lock = new object(); + + private readonly Dictionary<(uint msgId, byte sysid, byte compid), List> _leases + = new Dictionary<(uint, byte, byte), List>(); + + private readonly HashSet<(uint msgId, byte sysid, byte compid)> _nackedMessages + = new HashSet<(uint, byte, byte)>(); + + // Messages awaiting a confirmed SET_MESSAGE_INTERVAL(0) to restore default rate. + // Added on last-lease release, removed once the SET-0 is ACKed. + private readonly HashSet<(uint msgId, byte sysid, byte compid)> _pendingRestores + = new HashSet<(uint, byte, byte)>(); + + // Persistent MESSAGE_INTERVAL subscriptions per target, for detecting + // unsupported messages (interval_us == 0) after fire-and-forget GETs. + private readonly Dictionary<(byte sysid, byte compid), int> _intervalSubs + = new Dictionary<(byte, byte), int>(); + + // Per-message packet counting for accurate rate measurement. + // SubscribeToPacketType increments the counter; Tick snapshots it. + private readonly Dictionary<(uint msgId, byte sysid, byte compid), long> _packetCounts + = new Dictionary<(uint, byte, byte), long>(); + private readonly Dictionary<(uint msgId, byte sysid, byte compid), int> _packetSubs + = new Dictionary<(uint, byte, byte), int>(); + private readonly Dictionary<(uint msgId, byte sysid, byte compid), (long count, long ticks)> _tickSnapshots + = new Dictionary<(uint, byte, byte), (long, long)>(); + + private readonly CancellationTokenSource _cts = new CancellationTokenSource(); + private Task _loop; + private bool _drainingRestores; + private int _disposed; + + /// + /// Initializes a new instance of the class. + /// + /// The MAVLink interface to manage rates on. + public MessageRateManager(MAVLinkInterface port) + { + _port = port; + } + + /// + /// Requests a message at a minimum rate. + /// + /// Target system ID. + /// Target component ID. + /// MAVLink message ID. + /// Desired minimum rate in Hz. + /// Diagnostic label for logging. + /// A lease that must be disposed when the rate is no longer needed. + public MessageRateLease Subscribe(byte sysid, byte compid, + MAVLINK_MSG_ID msgId, double hz, string owner = null) + { + var id = (uint)msgId; + if (id > ushort.MaxValue) + throw new ArgumentOutOfRangeException(nameof(msgId), + $"Message ID {id} exceeds MESSAGE_INTERVAL's uint16 range"); + + var lease = new MessageRateLease(this, id, sysid, compid, hz, owner); + + if (sysid == 0 || compid == 0) + return lease; + + var key = (id, sysid, compid); + + int? desiredUs; + + lock (_lock) + { + if (_nackedMessages.Contains(key)) + { + log.InfoFormat("RateManager: {0} msg {1} ({2},{3}) previously NACKed, skipping", + owner ?? "", id, sysid, compid); + return lease; + } + + if (!_leases.TryGetValue(key, out var list)) + { + list = new List(); + _leases[key] = list; + } + + _pendingRestores.Remove(key); + list.Add(lease); + desiredUs = FastestLeaseIntervalUs(key); + EnsurePacketCounter(id, sysid, compid); + } + + EnsureSubMessageInterval(sysid, compid); + if (desiredUs.HasValue) + FireAndForgetSet(id, sysid, compid, desiredUs.Value); + + // If the message has never arrived, GET to learn whether the AP + // supports it (the MESSAGE_INTERVAL callback handles interval_us == 0). + if (!HasEverReceived(id, sysid, compid)) + FireAndForgetGet(id, sysid, compid); + + log.InfoFormat("RateManager: {0} subscribed msg {1} ({2},{3}) at {4:F1} Hz", + owner ?? "", id, sysid, compid, hz); + + EnsureLoopStarted(); + return lease; + } + + /// + /// Releases a lease and adjusts the active rate accordingly. + /// + internal void Release(MessageRateLease lease) + { + if (Interlocked.CompareExchange(ref lease.Released, 1, 0) != 0) + return; + + if (lease.SysId == 0 || lease.CompId == 0) + return; + + var key = (lease.MsgId, lease.SysId, lease.CompId); + int? newDesiredUs; + + lock (_lock) + { + if (!_leases.TryGetValue(key, out var list)) + return; + + list.Remove(lease); + + if (list.Count == 0) + { + _leases.Remove(key); + RemovePacketCounter(lease.MsgId, lease.SysId, lease.CompId); + newDesiredUs = 0; + } + else + { + newDesiredUs = FastestLeaseIntervalUs(key); + } + } + + if (newDesiredUs == 0) + { + lock (_lock) + { + _pendingRestores.Add(key); + } + // Kick off an attempt to restore. + _ = RetryPendingRestoresAsync(); + } + else if (newDesiredUs.HasValue) + { + FireAndForgetSet(lease.MsgId, lease.SysId, lease.CompId, newDesiredUs.Value); + } + + log.InfoFormat("RateManager: {0} released msg {1} ({2},{3}){4}", + lease.Owner, lease.MsgId, lease.SysId, lease.CompId, + newDesiredUs == 0 ? " -- restoring default" : ""); + + TryRemoveSubMessageInterval(lease.SysId, lease.CompId); + } + + /// + /// Resets cached state and re-sends rates for all active leases. + /// + /// + /// Call after connection negotiation completes. + /// + public void OnConnectionOpen() + { + List<(uint msgId, byte sysid, byte compid)> keys; + + lock (_lock) + { + _nackedMessages.Clear(); + _pendingRestores.Clear(); + _tickSnapshots.Clear(); + keys = _leases.Keys.ToList(); + } + + foreach (var key in keys) + { + EnsureSubMessageInterval(key.sysid, key.compid); + + int? desiredUs; + lock (_lock) + { + desiredUs = FastestLeaseIntervalUs(key); + } + if (desiredUs.HasValue) + FireAndForgetSet(key.msgId, key.sysid, key.compid, desiredUs.Value); + } + + EnsureLoopStarted(); + } + + public void Dispose() + { + if (Interlocked.CompareExchange(ref _disposed, 1, 0) != 0) + return; + + _cts.Cancel(); + + lock (_lock) + { + foreach (var sub in _intervalSubs.Values) + { + try { _port.UnSubscribeToPacketType(sub); } + catch { } + } + _intervalSubs.Clear(); + + foreach (var sub in _packetSubs.Values) + { + try { _port.UnSubscribeToPacketType(sub); } + catch { } + } + _packetSubs.Clear(); + _packetCounts.Clear(); + _tickSnapshots.Clear(); + } + + _cts.Dispose(); + } + + // --- Tick loop --- + + private async Task RunLoopAsync() + { + while (!_cts.IsCancellationRequested) + { + try + { + await Task.Delay(30_000, _cts.Token).ConfigureAwait(false); + Tick(); + await RetryPendingRestoresAsync().ConfigureAwait(false); + } + catch (OperationCanceledException) { break; } + catch (Exception ex) + { + log.Error("RateManager: tick failed", ex); + } + } + } + + private void EnsureLoopStarted() + { + lock (_lock) + { + if (_loop == null || _loop.IsCompleted) + _loop = Task.Run(() => RunLoopAsync()); + } + } + + /// + /// Re-sends SET_MESSAGE_INTERVAL for any tracked message whose observed rate is too low. + /// + private void Tick() + { + List<(uint msgId, byte sysid, byte compid)> keys; + lock (_lock) + { + keys = _leases.Keys.ToList(); + } + + foreach (var key in keys) + { + if (_cts.IsCancellationRequested) + return; + + try + { + int? desiredUs; + lock (_lock) + { + if (_nackedMessages.Contains(key)) + continue; + desiredUs = FastestLeaseIntervalUs(key); + } + if (!desiredUs.HasValue) + continue; + + bool satisfied = IsRateSatisfied(key, desiredUs.Value); + + // Reset snapshot so next tick measures a fresh window. + lock (_lock) + { + if (_packetCounts.TryGetValue(key, out var count)) + _tickSnapshots[key] = (count, Stopwatch.GetTimestamp()); + } + + if (satisfied) + continue; + + FireAndForgetSet(key.msgId, key.sysid, key.compid, desiredUs.Value); + + // If the message has never arrived, retry the GET -- the initial + // response may have been lost. + if (!HasEverReceived(key.msgId, key.sysid, key.compid)) + FireAndForgetGet(key.msgId, key.sysid, key.compid); + } + catch (Exception ex) + { + log.Error(string.Format("RateManager: tick failed for msg {0} ({1},{2})", + key.msgId, key.sysid, key.compid), ex); + } + } + } + + /// + /// Determines whether the observed packet rate meets the desired rate. + /// + /// + /// Uses count-based measurement and scales observed rate by + /// 1 / linkqualitygcs to estimate the autopilot's actual send rate. + /// + private bool IsRateSatisfied((uint msgId, byte sysid, byte compid) key, int desiredUs) + { + long currentCount; + (long count, long ticks) snapshot; + + lock (_lock) + { + if (!_packetCounts.TryGetValue(key, out currentCount)) + return false; + + if (!_tickSnapshots.TryGetValue(key, out snapshot)) + return false; // Should not happen -- created in EnsurePacketCounter. + } + + double elapsed = (double)(Stopwatch.GetTimestamp() - snapshot.ticks) / Stopwatch.Frequency; + if (elapsed < 1.0) + return true; // Too soon to judge; don't re-send. + + long delta = currentCount - snapshot.count; + if (delta <= 0) + return false; // No packets at all since last snapshot. + + double observedHz = delta / elapsed; + double desiredHz = IntervalUsToHz(desiredUs); + + // Compensate for link quality: if we're only receiving 80% of packets, + // the autopilot is likely sending 1/0.8 = 1.25x what we see. + // Cap the multiplier at 2x so bad LQ doesn't make us accept anything. + double lqFraction = GetLinkQuality(key.sysid, key.compid); + double lqMultiplier = lqFraction > 0.5 ? 1.0 / lqFraction : 2.0; + double estimatedSendHz = observedHz * lqMultiplier; + + return estimatedSendHz >= desiredHz - 1.0; + } + + /// + /// Returns link quality as a fraction (0.0-1.0) for the given target. + /// + /// + /// Falls back to 1.0 (perfect) if unavailable. + /// + private double GetLinkQuality(byte sysid, byte compid) + { + try + { + int lq = _port.MAVlist[sysid, compid].cs.linkqualitygcs; + return lq > 0 ? lq / 100.0 : 1.0; + } + catch { return 1.0; } + } + + private bool HasEverReceived(uint msgId, byte sysid, byte compid) + { + try + { + return _port.MAVlist[sysid, compid].packetspersecondbuild.ContainsKey(msgId); + } + catch { return false; } + } + + // --- MESSAGE_INTERVAL subscription --- + + /// + /// Subscribes to MESSAGE_INTERVAL from the specified target if not already subscribed. + /// + private void EnsureSubMessageInterval(byte sysid, byte compid) + { + var targetKey = (sysid, compid); + + lock (_lock) + { + if (_intervalSubs.ContainsKey(targetKey)) + return; + + byte s = sysid, c = compid; + var sub = _port.SubscribeToPacketType( + MAVLINK_MSG_ID.MESSAGE_INTERVAL, + msg => + { + var data = msg.ToStructure(); + OnMessageInterval(data.message_id, s, c, data.interval_us); + return true; + }, + sysid, compid); + + _intervalSubs[targetKey] = sub; + } + } + + /// + /// Removes the MESSAGE_INTERVAL subscription if no leases remain for this target. + /// + private void TryRemoveSubMessageInterval(byte sysid, byte compid) + { + var targetKey = (sysid, compid); + + lock (_lock) + { + bool hasLeases = _leases.Keys.Any(k => k.sysid == sysid && k.compid == compid); + if (hasLeases) + return; + + if (!_intervalSubs.TryGetValue(targetKey, out var sub)) + return; + + _intervalSubs.Remove(targetKey); + try { _port.UnSubscribeToPacketType(sub); } + catch { } + } + } + + /// + /// Marks a message as unsupported when a MESSAGE_INTERVAL response reports zero. + /// + private void OnMessageInterval(ushort messageId, byte sysid, byte compid, int intervalUs) + { + if (intervalUs != 0) + return; + + var key = ((uint)messageId, sysid, compid); + + lock (_lock) + { + if (!_leases.ContainsKey(key)) + return; + + _nackedMessages.Add(key); + _leases.Remove(key); + RemovePacketCounter((uint)messageId, sysid, compid); + } + + log.WarnFormat("RateManager: msg {0} ({1},{2}) not available (interval_us=0)", + messageId, sysid, compid); + + TryRemoveSubMessageInterval(sysid, compid); + } + + // --- Packet counting --- + + /// + /// Subscribes to a message type for counting if not already subscribed. + /// + /// + /// Caller must hold . Calls into + /// under lock -- + /// safe because that method does not call back into this class. + /// + private void EnsurePacketCounter(uint msgId, byte sysid, byte compid) + { + var key = (msgId, sysid, compid); + if (_packetSubs.ContainsKey(key)) + return; + + _packetCounts[key] = 0; + _tickSnapshots[key] = (0, Stopwatch.GetTimestamp()); + var sub = _port.SubscribeToPacketType( + (MAVLINK_MSG_ID)msgId, + msg => + { + lock (_lock) { _packetCounts[key]++; } + return true; + }, + sysid, compid); + _packetSubs[key] = sub; + } + + /// + /// Removes the packet counter subscription for a message. + /// + /// + /// Caller must hold . Calls into + /// under lock -- + /// safe because that method does not call back into this class. + /// + private void RemovePacketCounter(uint msgId, byte sysid, byte compid) + { + var key = (msgId, sysid, compid); + if (!_packetSubs.TryGetValue(key, out var sub)) + return; + + _packetSubs.Remove(key); + _packetCounts.Remove(key); + _tickSnapshots.Remove(key); + try { _port.UnSubscribeToPacketType(sub); } + catch { } + } + + // --- Command helpers --- + + /// + /// Sends SET_MESSAGE_INTERVAL(0) with acknowledgment and removes the pending restore on success. + /// + private async Task RestoreDefaultAsync(uint msgId, byte sysid, byte compid) + { + var key = (msgId, sysid, compid); + // Wait briefly for any in-progress requireAck exchange to finish + // so we don't fight over COMMAND_ACK packets. + for (int i = 0; i < 5 && _port.giveComport; i++) + await Task.Delay(1000, _cts.Token).ConfigureAwait(false); + if (_port.giveComport) + return; + try + { + bool acked = await _port.doCommandAsync(sysid, compid, + MAV_CMD.SET_MESSAGE_INTERVAL, + (float)msgId, 0, + 0, 0, 0, 0, 0, + true).ConfigureAwait(false); + + if (acked) + { + lock (_lock) { _pendingRestores.Remove(key); } + log.InfoFormat("RateManager: msg {0} ({1},{2}) default restored (ACKed)", + msgId, sysid, compid); + } + } + catch (Exception ex) + { + log.WarnFormat("RateManager: msg {0} ({1},{2}) default restore failed: {3}", + msgId, sysid, compid, ex.Message); + } + } + + /// + /// Retries any pending default-rate restores that have not yet been acknowledged. + /// + private async Task RetryPendingRestoresAsync() + { + lock (_lock) + { + if (_pendingRestores.Count == 0) + return; + // Prevent concurrent runs, since we can't tell which ACK corresponds to which message. + if (_drainingRestores) + return; + _drainingRestores = true; + } + + try + { + List<(uint msgId, byte sysid, byte compid)> pending; + lock (_lock) + { + pending = _pendingRestores.ToList(); + } + + foreach (var key in pending) + { + if (_cts.IsCancellationRequested) + return; + + await RestoreDefaultAsync(key.msgId, key.sysid, key.compid) + .ConfigureAwait(false); + } + } + finally + { + lock (_lock) { _drainingRestores = false; } + } + } + + // --- Fire-and-forget command helpers --- + + private void FireAndForgetSet(uint msgId, byte sysid, byte compid, int intervalUs) + { + // Reset the snapshot so IsRateSatisfied measures from now, not from + // before the rate change. + var key = (msgId, sysid, compid); + lock (_lock) + { + if (_packetCounts.TryGetValue(key, out var count)) + _tickSnapshots[key] = (count, Stopwatch.GetTimestamp()); + } + + try + { + _port.doCommandAsync(sysid, compid, + MAV_CMD.SET_MESSAGE_INTERVAL, + (float)msgId, intervalUs, + 0, 0, 0, 0, 0, + false) + .ContinueWith(t => log.Debug("RateManager: fire-and-forget SET failed: " + + t.Exception?.InnerException?.Message), TaskContinuationOptions.OnlyOnFaulted); + } + catch (Exception ex) + { + log.Debug("RateManager: fire-and-forget SET failed: " + ex.Message); + } + } + + private void FireAndForgetGet(uint msgId, byte sysid, byte compid) + { + try + { + _port.doCommandAsync(sysid, compid, + MAV_CMD.GET_MESSAGE_INTERVAL, + (float)msgId, + 0, 0, 0, 0, 0, 0, + false) + .ContinueWith(t => log.Debug("RateManager: fire-and-forget GET failed: " + + t.Exception?.InnerException?.Message), TaskContinuationOptions.OnlyOnFaulted); + } + catch (Exception ex) + { + log.Debug("RateManager: fire-and-forget GET failed: " + ex.Message); + } + } + + // --- Helpers --- + + /// + /// Returns the interval in microseconds of the fastest active lease for a key. + /// + /// Caller must hold . + private int? FastestLeaseIntervalUs((uint, byte, byte) key) + { + if (!_leases.TryGetValue(key, out var list) || list.Count == 0) + return null; + + int min = int.MaxValue; + foreach (var lease in list) + { + int us = HzToIntervalUs(lease.Hz); + if (us > 0 && us < min) + min = us; + } + return min == int.MaxValue ? (int?)null : min; + } + + private static int HzToIntervalUs(double hz) + { + return hz > 0 ? (int)(1e6 / hz) : 0; + } + + private static double IntervalUsToHz(int intervalUs) + { + return intervalUs > 0 ? 1e6 / intervalUs : 0; + } + } +} From 1817fe9e3278ba363d0c9d6eeb82f8df8d448050 Mon Sep 17 00:00:00 2001 From: Bob Long Date: Fri, 6 Mar 2026 17:51:40 +1100 Subject: [PATCH 2/6] CameraProtocol: use lease-based message rates --- Controls/GimbalVideoControl.cs | 8 +- ExtLibs/ArduPilot/CurrentState.cs | 2 +- ExtLibs/ArduPilot/Mavlink/CameraProtocol.cs | 324 +++++++++++++------- 3 files changed, 224 insertions(+), 110 deletions(-) diff --git a/Controls/GimbalVideoControl.cs b/Controls/GimbalVideoControl.cs index d185342d36..e65bf125aa 100644 --- a/Controls/GimbalVideoControl.cs +++ b/Controls/GimbalVideoControl.cs @@ -649,7 +649,7 @@ private void VideoBox_Click(object sender, EventArgs e) } else if ((Control.ModifierKeys, me.Button) == preferences.TrackObjectUnderMouse) { - selectedCamera?.RequestTrackingMessageInterval(5); + selectedCamera?.SubscribeTracking(5); var x = (float)point.Value.x; var y = (float)point.Value.y; if (dragStartPoint.HasValue) @@ -766,9 +766,9 @@ private void AutoConnectTimerCallback(object sender, System.Timers.ElapsedEventA { if (CameraProtocol.VideoStreams.Count < 1) { - Console.Write("Requesting camera information..."); - // We must not have any reported video streams. Try to request them - selectedCamera?.RequestCameraInformationAsync().Wait(); + Console.Write("Requesting video stream information..."); + // We must not have any reported video streams. Try to request them. + selectedCamera?.RequestVideoStreamInformation(); Console.WriteLine(" done."); // Come back later and see if any streams have been reported AutoConnectTimer.Start(); diff --git a/ExtLibs/ArduPilot/CurrentState.cs b/ExtLibs/ArduPilot/CurrentState.cs index 70e05a1e90..4898304d84 100644 --- a/ExtLibs/ArduPilot/CurrentState.cs +++ b/ExtLibs/ArduPilot/CurrentState.cs @@ -4526,7 +4526,7 @@ public void UpdateCurrentSettings(Action bs, bool updatenow, mavinterface.requestDatastream(MAVLink.MAV_DATA_STREAM.RC_CHANNELS, MAV.cs.raterc, MAV.sysid, MAV.compid); // request rc info - MAV.Camera?.RequestMessageIntervals(MAV.cs.ratestatus); // use ratestatus until we create a new setting for this + MAV.Camera?.UpdateRateIfChanged(MAV.cs.ratestatus); // use ratestatus until we create a new setting for this MAV.GimbalManager?.Discover(); } catch diff --git a/ExtLibs/ArduPilot/Mavlink/CameraProtocol.cs b/ExtLibs/ArduPilot/Mavlink/CameraProtocol.cs index 70141e8c97..ef5770856e 100644 --- a/ExtLibs/ArduPilot/Mavlink/CameraProtocol.cs +++ b/ExtLibs/ArduPilot/Mavlink/CameraProtocol.cs @@ -1,6 +1,7 @@ using System; using System.Collections.Concurrent; using System.Collections.Generic; +using System.Linq; using System.Reflection; using System.Text.RegularExpressions; using System.Threading.Tasks; @@ -26,6 +27,13 @@ public class CameraProtocol // Tracks whether we have received a `CAMERA_INFORMATION` message yet private bool have_camera_information = false; + // Lease-based streaming rates managed by the connection's MessageRateManager. + private readonly object _leaseLock = new object(); + private List _streamingLeases = new List(); + private MessageRateLease _leaseTracking; + private int _lastRateHz = -1; + private int _lastTrackingRateHz; + public MAVLink.mavlink_camera_information_t CameraInformation { get; private set; } public MAVLink.mavlink_camera_settings_t CameraSettings { get; private set; } public MAVLink.mavlink_camera_capture_status_t CameraCaptureStatus { get; private set; } @@ -206,67 +214,89 @@ public float VFOV } /// - /// Initializes the camera protocol by setting up message parsing and requesting initial camera information. + /// Initializes the camera protocol by setting up message parsing and requesting camera information. /// - /// MAVState parent of this driver - public Task StartID(MAVState mavState) + /// + /// Sends fire-and-forget SET_MESSAGE_INTERVAL + GET_MESSAGE_INTERVAL for + /// CAMERA_INFORMATION, then watches the MESSAGE_INTERVAL reply to confirm. + /// Retries up to 3 times; stops on success, interval_us == 0 (not supported), + /// or if CAMERA_INFORMATION arrives in the meantime. + /// + /// The MAVState instance for the target camera. + public async Task StartID(MAVState mavState) { parent = mavState; mavState.parent.OnPacketReceived += ParseMessages; - return RequestCameraInformationAsync(); - } + if (parent?.parent == null) + return; + + var port = parent.parent; + + // Ask the target to stream CAMERA_INFORMATION every ~30s so we discover + // cameras even if plugged in after boot. + const ushort camInfoId = (ushort)MAVLink.MAVLINK_MSG_ID.CAMERA_INFORMATION; + const float intervalUs = 30_000_000; // 30 s + + // Watch MESSAGE_INTERVAL replies for our message to confirm acceptance or rejection. + bool confirmed = false; + var sub = port.SubscribeToPacketType( + MAVLink.MAVLINK_MSG_ID.MESSAGE_INTERVAL, + msg => + { + var data = msg.ToStructure(); + if (data.message_id != camInfoId) + return true; + + if (data.interval_us == 0) + log.Info("Camera: CAMERA_INFORMATION not supported (interval_us=0)"); + else + log.InfoFormat("Camera: CAMERA_INFORMATION interval confirmed at {0} us", data.interval_us); + + confirmed = true; + return true; + }, + parent.sysid, parent.compid); - /// - /// Sends an asynchronous request to fetch camera information via. - /// - public async Task RequestCameraInformationAsync() - { try { - if (parent?.parent != null) + for (int attempt = 0; attempt < 3; attempt++) { - // New-style request - var resp = await parent.parent.doCommandAsync( - parent.sysid, parent.compid, - MAVLink.MAV_CMD.REQUEST_MESSAGE, - (float)MAVLink.MAVLINK_MSG_ID.CAMERA_INFORMATION, - 0, 0, 0, 0, 0, 0 - ); - // Fall back to deprecated request message - if (!resp) + if (have_camera_information || confirmed) + break; + + try { - await parent.parent.doCommandAsync( - parent.sysid, parent.compid, - MAVLink.MAV_CMD.REQUEST_CAMERA_INFORMATION, - 0, 0, 0, 0, 0, 0, 0, - false // Don't wait for response - ); + _ = port.doCommandAsync(parent.sysid, parent.compid, + MAVLink.MAV_CMD.SET_MESSAGE_INTERVAL, + camInfoId, intervalUs, + 0, 0, 0, 0, 0, false); + + _ = port.doCommandAsync(parent.sysid, parent.compid, + MAVLink.MAV_CMD.GET_MESSAGE_INTERVAL, + camInfoId, + 0, 0, 0, 0, 0, 0, false); + } + catch (Exception ex) + { + log.Debug("Camera: SET/GET_MESSAGE_INTERVAL failed: " + ex.Message); } - // Get video stream information as well - await parent.parent.doCommandAsync( - parent.sysid, parent.compid, - MAVLink.MAV_CMD.REQUEST_MESSAGE, - (float)MAVLink.MAVLINK_MSG_ID.VIDEO_STREAM_INFORMATION, - 0, 0, 0, 0, 0, 0, - false // Don't wait for response - ); + await Task.Delay(5000).ConfigureAwait(false); } } - catch (Exception ex) + finally { - log.Error(ex); + port.UnSubscribeToPacketType(sub); } } /// - /// Event handler for OnPacketReceived. - /// Parses incoming MAVLink messages related to camera operations and updates internal state accordingly. + /// Parses incoming MAVLink messages and updates camera state. /// - /// MAVLink interface - /// MAVLink message to parse + /// The MAVLink interface that received the message. + /// The received MAVLink message. public void ParseMessages(object sender, MAVLink.MAVLinkMessage message) { if (message.sysid != parent.sysid || message.compid != parent.compid) @@ -275,8 +305,16 @@ public void ParseMessages(object sender, MAVLink.MAVLinkMessage message) switch ((MAVLink.MAVLINK_MSG_ID)message.msgid) { case MAVLink.MAVLINK_MSG_ID.CAMERA_INFORMATION: - have_camera_information = true; CameraInformation = (MAVLink.mavlink_camera_information_t)message.data; + if (!have_camera_information) + { + have_camera_information = true; + if (_lastRateHz > 0) + TakeStreamingLeases(_lastRateHz); + + if ((CameraInformation.flags & (int)MAVLink.CAMERA_CAP_FLAGS.HAS_VIDEO_STREAM) != 0) + RequestVideoStreamWithRetry(); + } break; case MAVLink.MAVLINK_MSG_ID.CAMERA_SETTINGS: CameraSettings = (MAVLink.mavlink_camera_settings_t)message.data; @@ -298,98 +336,174 @@ public void ParseMessages(object sender, MAVLink.MAVLinkMessage message) } /// - /// Requests that the camera send specific messages types at a specified rate. - /// The messages are selected based on the camera's reported capabilities. + /// Re-subscribes streaming leases if the desired rate has changed. /// - /// Message frequency in messages per second. - public void RequestMessageIntervals(int ratehz) + /// + /// Call periodically (e.g. from the rate-request loop). Does nothing until + /// CAMERA_INFORMATION has been received. New leases are taken before old + /// ones are disposed so there is no gap in coverage. + /// Zero or negative values release all streaming leases (the RateManager + /// restores the autopilot's default rate). + /// + /// Desired rate in Hz. <= 0 releases leases. + public void UpdateRateIfChanged(int ratehz) { - if (ratehz < 0) - { - // -1 means don't try to configure message intervals + if (!have_camera_information || parent?.parent == null) + return; + + if (ratehz == _lastRateHz) return; + + if (ratehz <= 0) + ReleaseStreamingLeases(); + else + TakeStreamingLeases(ratehz); + } + + /// + /// Takes (or replaces) streaming leases at the given rate based on camera capabilities. + /// + private void TakeStreamingLeases(int ratehz) + { + var rm = parent.parent.RateManager; + double hz = ratehz; + string owner = $"Camera({parent.sysid},{parent.compid})"; + + // Build new lease set, then swap and dispose old + var newLeases = new List + { + rm.Subscribe(parent.sysid, parent.compid, MAVLink.MAVLINK_MSG_ID.CAMERA_FOV_STATUS, hz, owner), + rm.Subscribe(parent.sysid, parent.compid, MAVLink.MAVLINK_MSG_ID.CAMERA_SETTINGS, hz, owner), + rm.Subscribe(parent.sysid, parent.compid, MAVLink.MAVLINK_MSG_ID.CAMERA_CAPTURE_STATUS, hz, owner), + }; + + List old; + lock (_leaseLock) + { + old = _streamingLeases; + _streamingLeases = newLeases; + _lastRateHz = ratehz; } + foreach (var lease in old) + lease.Dispose(); + } - if (parent?.parent == null) + /// + /// Releases all streaming leases, restoring the autopilot's default rates. + /// + private void ReleaseStreamingLeases() + { + List old; + lock (_leaseLock) { - return; + old = _streamingLeases; + _streamingLeases = new List(); + _lastRateHz = 0; } + foreach (var lease in old) + lease.Dispose(); + } - // ratehz of 0 means "stop sending", which is what -1 interval_us means in the MAVLink message - float interval_us = ratehz > 0 ? (float)(1e6 / ratehz) : -1; + /// + /// Subscribes to CAMERA_TRACKING_IMAGE_STATUS at the given rate. + /// + /// + /// Replaces any existing tracking lease. + /// + /// Desired rate in Hz. + public void SubscribeTracking(int ratehz) + { + if (!have_camera_information || parent?.parent == null) + return; - Task.Run(RequestCameraInformationAsync); + if (ratehz == _lastTrackingRateHz) + return; - // Request FOV status - Task.Run(async () => - { - await parent.parent.doCommandAsync( - parent.sysid, parent.compid, - MAVLink.MAV_CMD.SET_MESSAGE_INTERVAL, - (float)MAVLink.MAVLINK_MSG_ID.CAMERA_FOV_STATUS, - interval_us, - 0, 0, 0, 0, 0, - false // Don't wait for response - ).ConfigureAwait(false); - }); + var newLease = parent.parent.RateManager.Subscribe( + parent.sysid, parent.compid, + MAVLink.MAVLINK_MSG_ID.CAMERA_TRACKING_IMAGE_STATUS, + ratehz, + $"Camera({parent.sysid},{parent.compid})"); - // Get camera settings - if (HasModes || HasZoom || HasFocus) + MessageRateLease old; + lock (_leaseLock) { - Task.Run(async () => - { - await parent.parent.doCommandAsync( - parent.sysid, parent.compid, - MAVLink.MAV_CMD.SET_MESSAGE_INTERVAL, - (float)MAVLink.MAVLINK_MSG_ID.CAMERA_SETTINGS, - interval_us, - 0, 0, 0, 0, 0, - false // Don't wait for response - ).ConfigureAwait(false); - }); + old = _leaseTracking; + _leaseTracking = newLease; + _lastTrackingRateHz = ratehz; } + old?.Dispose(); + } - // We use the capability flags directly here, and NOT whether we are currently able to do these things - var can_capture_video = (CameraInformation.flags & (int)MAVLink.CAMERA_CAP_FLAGS.CAPTURE_VIDEO) > 0; - var can_capture_image = (CameraInformation.flags & (int)MAVLink.CAMERA_CAP_FLAGS.CAPTURE_IMAGE) > 0; - if (can_capture_video || can_capture_image) + /// + /// Releases the tracking message lease if one is active. + /// + public void StopTracking() + { + MessageRateLease old; + lock (_leaseLock) { - Task.Run(async () => - { - await parent.parent.doCommandAsync( - parent.sysid, parent.compid, - MAVLink.MAV_CMD.SET_MESSAGE_INTERVAL, - (float)MAVLink.MAVLINK_MSG_ID.CAMERA_CAPTURE_STATUS, - interval_us, - 0, 0, 0, 0, 0, - false // Don't wait for response - ).ConfigureAwait(false); - }); + old = _leaseTracking; + _leaseTracking = null; + _lastTrackingRateHz = 0; } + old?.Dispose(); } - public void RequestTrackingMessageInterval(int ratehz) + /// + /// Sends REQUEST_MESSAGE for VIDEO_STREAM_INFORMATION with retries and backoff. + /// + private void RequestVideoStreamWithRetry() { if (parent?.parent == null) - { return; - } - float interval_us = (float)(1e6 / ratehz); + var port = parent.parent; + byte sysid = parent.sysid, compid = parent.compid; Task.Run(async () => { - await parent.parent.doCommandAsync( - parent.sysid, parent.compid, - MAVLink.MAV_CMD.SET_MESSAGE_INTERVAL, - (float)MAVLink.MAVLINK_MSG_ID.CAMERA_TRACKING_IMAGE_STATUS, - interval_us, - 0, 0, 0, 0, 0, - false // Don't wait for response - ).ConfigureAwait(false); + for (int i = 0; i < 3; i++) + { + // Stop trying if we disconnect + if (parent?.parent?.BaseStream?.IsOpen != true) + break; + + if (VideoStreams.Keys.Any(k => k.Item1 == sysid && k.Item2 == compid)) + break; + + try + { + _ = port.doCommandAsync(sysid, compid, + MAVLink.MAV_CMD.REQUEST_MESSAGE, + (float)MAVLink.MAVLINK_MSG_ID.VIDEO_STREAM_INFORMATION, + 0, 0, 0, 0, 0, 0, false); + } + catch (Exception ex) + { + log.Debug("Camera: REQUEST_MESSAGE(VIDEO_STREAM_INFORMATION) failed: " + ex.Message); + } + + await Task.Delay(5000).ConfigureAwait(false); + } }); } + /// + /// Requests VIDEO_STREAM_INFORMATION from the camera (one-shot, best-effort). + /// + public void RequestVideoStreamInformation() + { + if (parent?.parent == null) + return; + + parent.parent.doCommand( + parent.sysid, parent.compid, + MAVLink.MAV_CMD.REQUEST_MESSAGE, + (float)MAVLink.MAVLINK_MSG_ID.VIDEO_STREAM_INFORMATION, + 0, 0, 0, 0, 0, 0, false); + } + /// /// Sends command to capture one image. /// From 3ef26da94f93b418e92636462de51f164a3dd4d3 Mon Sep 17 00:00:00 2001 From: Bob Long Date: Wed, 11 Mar 2026 12:02:04 +1100 Subject: [PATCH 3/6] GimbalManager: message-interval-based discovery --- ExtLibs/ArduPilot/CurrentState.cs | 1 - .../Mavlink/GimbalManagerProtocol.cs | 101 +++++++++++++++--- ExtLibs/ArduPilot/Mavlink/MAVLinkInterface.cs | 5 +- 3 files changed, 90 insertions(+), 17 deletions(-) diff --git a/ExtLibs/ArduPilot/CurrentState.cs b/ExtLibs/ArduPilot/CurrentState.cs index 4898304d84..b93642b2d7 100644 --- a/ExtLibs/ArduPilot/CurrentState.cs +++ b/ExtLibs/ArduPilot/CurrentState.cs @@ -4527,7 +4527,6 @@ public void UpdateCurrentSettings(Action bs, bool updatenow, MAV.sysid, MAV.compid); // request rc info MAV.Camera?.UpdateRateIfChanged(MAV.cs.ratestatus); // use ratestatus until we create a new setting for this - MAV.GimbalManager?.Discover(); } catch { diff --git a/ExtLibs/ArduPilot/Mavlink/GimbalManagerProtocol.cs b/ExtLibs/ArduPilot/Mavlink/GimbalManagerProtocol.cs index c48193bd2c..8bf6d64681 100644 --- a/ExtLibs/ArduPilot/Mavlink/GimbalManagerProtocol.cs +++ b/ExtLibs/ArduPilot/Mavlink/GimbalManagerProtocol.cs @@ -1,13 +1,19 @@ using System; using System.Collections.Concurrent; +using System.Reflection; +using System.Threading; using System.Threading.Tasks; +using log4net; using MissionPlanner.Utilities; namespace MissionPlanner.ArduPilot.Mavlink { public class GimbalManagerProtocol { - CurrentState cs; + private static readonly ILog log = LogManager.GetLogger(MethodBase.GetCurrentMethod().DeclaringType); + + private readonly CurrentState cs; + private readonly MAVLinkInterface mavint; // Stores the last GIMBAL_MANAGER_INFORMATION message for each gimbal device/component ID. // This index will be 1-6, or MAVLink component IDs 154, 171-175. @@ -27,7 +33,14 @@ public class GimbalManagerProtocol public ConcurrentDictionary GimbalStatus = new ConcurrentDictionary(); - private readonly MAVLinkInterface mavint; + private bool have_gimbal_manager_information = false; + private int _started; + + // Uncomment the Console.WriteLine line to enable debug output. + private static void DebugConsoleWrite(string format, params object[] args) + { + // Console.WriteLine(format, args); + } public GimbalManagerProtocol(MAVLinkInterface mavint, CurrentState cs) { @@ -35,18 +48,77 @@ public GimbalManagerProtocol(MAVLinkInterface mavint, CurrentState cs) this.cs = cs; } - private bool first_discover = true; - public void Discover() + /// + /// Initializes the gimbal manager protocol and begins GIMBAL_MANAGER_INFORMATION discovery. + /// + /// + /// Sends fire-and-forget SET_MESSAGE_INTERVAL + GET_MESSAGE_INTERVAL for + /// GIMBAL_MANAGER_INFORMATION, then watches the MESSAGE_INTERVAL reply to confirm. + /// Retries up to 3 times; stops on success, interval_us == 0 (not supported), + /// or if GIMBAL_MANAGER_INFORMATION arrives in the meantime. + /// + /// Target system ID. + /// Target component ID. + public async Task StartID(byte sysid, byte compid) { - if (first_discover) + if (Interlocked.Exchange(ref _started, 1) != 0) + return; + + mavint.OnPacketReceived += MessagesHandler; + + const ushort msgId = (ushort)MAVLink.MAVLINK_MSG_ID.GIMBAL_MANAGER_INFORMATION; + const float intervalUs = 30_000_000; // 30 s + + bool confirmed = false; + var sub = mavint.SubscribeToPacketType( + MAVLink.MAVLINK_MSG_ID.MESSAGE_INTERVAL, + msg => + { + var data = msg.ToStructure(); + if (data.message_id != msgId) + return true; + + if (data.interval_us == 0) + log.Info("GimbalManager: GIMBAL_MANAGER_INFORMATION not supported (interval_us=0)"); + else + log.InfoFormat("GimbalManager: GIMBAL_MANAGER_INFORMATION interval confirmed at {0} us", data.interval_us); + + confirmed = true; + return true; + }, + sysid, compid); + + try + { + for (int attempt = 0; attempt < 3; attempt++) + { + if (have_gimbal_manager_information || confirmed) + break; + + try + { + _ = mavint.doCommandAsync(sysid, compid, + MAVLink.MAV_CMD.SET_MESSAGE_INTERVAL, + msgId, intervalUs, + 0, 0, 0, 0, 0, false); + + _ = mavint.doCommandAsync(sysid, compid, + MAVLink.MAV_CMD.GET_MESSAGE_INTERVAL, + msgId, + 0, 0, 0, 0, 0, 0, false); + } + catch (Exception ex) + { + log.Debug("GimbalManager: SET/GET_MESSAGE_INTERVAL failed: " + ex.Message); + } + + await Task.Delay(5000).ConfigureAwait(false); + } + } + finally { - first_discover = false; - mavint.OnPacketReceived += MessagesHandler; + mavint.UnSubscribeToPacketType(sub); } - - mavint.doCommand(0, 0, MAVLink.MAV_CMD.REQUEST_MESSAGE, - (float)MAVLink.MAVLINK_MSG_ID.GIMBAL_MANAGER_INFORMATION, - 0, 0, 0, 0, 0, 0, false); } private void MessagesHandler(object sender, MAVLink.MAVLinkMessage message) @@ -60,6 +132,8 @@ private void MessagesHandler(object sender, MAVLink.MAVLinkMessage message) { ManagerInfo[0] = gmi; } + + have_gimbal_manager_information = true; } if (message.msgid == (uint)MAVLink.MAVLINK_MSG_ID.GIMBAL_MANAGER_STATUS) @@ -205,16 +279,15 @@ public Task SetRCYawLockAsync(bool yaw_lock, byte gimbal_device_id = 0) public Task SetAttitudeAsync(Quaternion q, bool yaw_lock, byte gimbal_device_id = 0) { var pitch = q.get_euler_pitch() * MathHelper.rad2deg; - var yaw = q.get_euler_yaw() * MathHelper.rad2deg; + var yaw = q.get_euler_yaw() * MathHelper.rad2deg; if (!yaw_lock) { yaw -= cs.yaw; } - Console.WriteLine("SetAttitudeAsync: pitch={0}, yaw={1}, yaw_lock={2}", pitch, yaw < 0 ? yaw + 360 : yaw, yaw_lock); + DebugConsoleWrite("SetAttitudeAsync: pitch={0}, yaw={1}, yaw_lock={2}", pitch, yaw < 0 ? yaw + 360 : yaw, yaw_lock); return SetAnglesCommandAsync(pitch, yaw, yaw_lock, gimbal_device_id); - //return Task.FromResult(true); } private double wrap_180(double angle) diff --git a/ExtLibs/ArduPilot/Mavlink/MAVLinkInterface.cs b/ExtLibs/ArduPilot/Mavlink/MAVLinkInterface.cs index e29d7070b3..c908ce34fe 100644 --- a/ExtLibs/ArduPilot/Mavlink/MAVLinkInterface.cs +++ b/ExtLibs/ArduPilot/Mavlink/MAVLinkInterface.cs @@ -579,8 +579,9 @@ private void OnMAVDetected(object sender, (byte, byte) tuple) await Task.Delay(2000); - MAVlist[tuple.Item1, tuple.Item2] - .GimbalManager.Discover(); + await MAVlist[tuple.Item1, tuple.Item2] + .GimbalManager.StartID(tuple.Item1, tuple.Item2) + .ConfigureAwait(false); } catch (Exception e) { From 3bc01e895aa5842fde631f5d06f4a247a4606973 Mon Sep 17 00:00:00 2001 From: Bob Long Date: Wed, 11 Mar 2026 12:04:28 +1100 Subject: [PATCH 4/6] GimbalManager: fix yaw frame detection Attitude status sends all the flags we need to determine the yaw frame, we don't need to hit up manager status for that. Manager status flags aren't even correctly populated in ArduPilot, but we lucked out because - Nothing was requesting manager status, and it isn't sent by default - No flags at all gets interpreted as body-frame yaw, which happens to be accidentally correct for ArduPilot anyway. We don't actually need manager status at all now, so I have removed it. --- .../Mavlink/GimbalManagerProtocol.cs | 48 ++++--------------- 1 file changed, 10 insertions(+), 38 deletions(-) diff --git a/ExtLibs/ArduPilot/Mavlink/GimbalManagerProtocol.cs b/ExtLibs/ArduPilot/Mavlink/GimbalManagerProtocol.cs index 8bf6d64681..62357b5e4e 100644 --- a/ExtLibs/ArduPilot/Mavlink/GimbalManagerProtocol.cs +++ b/ExtLibs/ArduPilot/Mavlink/GimbalManagerProtocol.cs @@ -21,12 +21,6 @@ public class GimbalManagerProtocol public ConcurrentDictionary ManagerInfo = new ConcurrentDictionary(); - // Stores the GIMBAL_MANAGER_STATUS message for each gimbal device/component ID. - // This index will be 1-6, or MAVLink component IDs 154, 171-175. - // Index 0 is used to store the message of the first (lowest) gimbal ID. - public ConcurrentDictionary ManagerStatus = - new ConcurrentDictionary(); - // Stores the GIMBAL_DEVICE_ATTITUDE_STATUS message for each gimbal device/component ID. // This index will be 1-6, or MAVLink component IDs 154, 171-175. // Index 0 is used to store the message of the first (lowest) gimbal ID. @@ -136,16 +130,6 @@ private void MessagesHandler(object sender, MAVLink.MAVLinkMessage message) have_gimbal_manager_information = true; } - if (message.msgid == (uint)MAVLink.MAVLINK_MSG_ID.GIMBAL_MANAGER_STATUS) - { - var gms = (MAVLink.mavlink_gimbal_manager_status_t)message.data; - ManagerStatus[gms.gimbal_device_id] = gms; - if (!ManagerStatus.ContainsKey(0) || gms.gimbal_device_id <= ManagerStatus[0].gimbal_device_id) - { - ManagerStatus[0] = gms; - } - } - if (message.msgid == (uint)MAVLink.MAVLINK_MSG_ID.GIMBAL_DEVICE_ATTITUDE_STATUS) { var gds = (MAVLink.mavlink_gimbal_device_attitude_status_t)message.data; @@ -167,27 +151,6 @@ public bool HasAllCapability(MAVLink.GIMBAL_MANAGER_CAP_FLAGS flags, byte gimbal return ManagerInfo.TryGetValue(gimbal_device_id, out var info) && ((info.cap_flags & (uint)flags) == (uint)flags); } - public bool HasStatusFlag(MAVLink.GIMBAL_DEVICE_FLAGS flags, byte gimbal_device_id = 0) - { - return ManagerStatus.TryGetValue(gimbal_device_id, out var status) && ((status.flags & (uint)flags) != 0); - } - - public bool YawInVehicleFrame(byte gimbal_device_id = 0) - { - bool yaw_in_earth_frame = HasStatusFlag(MAVLink.GIMBAL_DEVICE_FLAGS.YAW_IN_EARTH_FRAME, gimbal_device_id); - bool yaw_in_vehicle_frame = HasStatusFlag(MAVLink.GIMBAL_DEVICE_FLAGS.YAW_IN_VEHICLE_FRAME, gimbal_device_id); - - // Some older protocols don't set YAW_IN_EARTH_FRAME or YAW_IN_VEHICLE_FRAME flags, - // with those, we have to infer it from whether YAW_LOCK is set. - if (!yaw_in_earth_frame && !yaw_in_vehicle_frame) - { - bool yaw_lock = HasStatusFlag(MAVLink.GIMBAL_DEVICE_FLAGS.YAW_LOCK, gimbal_device_id); - yaw_in_vehicle_frame = !yaw_lock; - } - - return yaw_in_vehicle_frame; - } - /// /// Get the reported attitude of the gimbal. Yaw always reported relative to the earth frame. /// @@ -202,7 +165,16 @@ public Quaternion GetAttitude(byte gimbal_device_id = 0) var q = new Quaternion(status.q[0], status.q[1], status.q[2], status.q[3]); - if (YawInVehicleFrame(gimbal_device_id)) + // Rotate from vehicle frame to earth frame if needed. + var flags = (MAVLink.GIMBAL_DEVICE_FLAGS)status.flags; + bool yaw_in_earth = (flags & MAVLink.GIMBAL_DEVICE_FLAGS.YAW_IN_EARTH_FRAME) != 0; + bool yaw_in_vehicle = (flags & MAVLink.GIMBAL_DEVICE_FLAGS.YAW_IN_VEHICLE_FRAME) != 0; + + // Older protocols may set neither flag; infer from YAW_LOCK. + if (!yaw_in_earth && !yaw_in_vehicle) + yaw_in_vehicle = (flags & MAVLink.GIMBAL_DEVICE_FLAGS.YAW_LOCK) == 0; + + if (yaw_in_vehicle) { q = Quaternion.from_euler(0, 0, cs.yaw * MathHelper.deg2rad) * q; } From 53a5b2ab366bfde52e9deeb2078d7154c60c9160 Mon Sep 17 00:00:00 2001 From: Bob Long Date: Wed, 11 Mar 2026 13:12:56 +1100 Subject: [PATCH 5/6] GimbalManager: tighten XML doc comments --- .../ArduPilot/Mavlink/GimbalManagerProtocol.cs | 18 ++++++++++-------- 1 file changed, 10 insertions(+), 8 deletions(-) diff --git a/ExtLibs/ArduPilot/Mavlink/GimbalManagerProtocol.cs b/ExtLibs/ArduPilot/Mavlink/GimbalManagerProtocol.cs index 62357b5e4e..38d69dc31b 100644 --- a/ExtLibs/ArduPilot/Mavlink/GimbalManagerProtocol.cs +++ b/ExtLibs/ArduPilot/Mavlink/GimbalManagerProtocol.cs @@ -152,10 +152,10 @@ public bool HasAllCapability(MAVLink.GIMBAL_MANAGER_CAP_FLAGS flags, byte gimbal } /// - /// Get the reported attitude of the gimbal. Yaw always reported relative to the earth frame. + /// Gets the reported attitude of the gimbal in the earth frame. /// - /// Device ID of the gimbal. 0 means all gimbals - /// + /// Device ID of the gimbal. 0 means the first gimbal. + /// The gimbal attitude quaternion, or null if no status has been received. public Quaternion GetAttitude(byte gimbal_device_id = 0) { if (!GimbalStatus.TryGetValue(gimbal_device_id, out var status)) @@ -242,12 +242,14 @@ public Task SetRCYawLockAsync(bool yaw_lock, byte gimbal_device_id = 0) } /// - /// Set the attitude of the gimbal with a quaternion. Yaw always reported relative to the earth frame. + /// Commands the gimbal to a given attitude. /// - /// Gimbal attitude quaternion - /// True if the gimbal should continue to point in this orientation. False if it should follow the yaw of the vehicle. - /// Device ID of the gimbal. 0 means all gimbals - /// + /// Desired attitude quaternion in the earth frame. + /// + /// true to hold the earth-frame yaw; false to follow vehicle yaw. + /// + /// Device ID of the gimbal. 0 means the first gimbal. + /// true if the command was acknowledged; otherwise, false. public Task SetAttitudeAsync(Quaternion q, bool yaw_lock, byte gimbal_device_id = 0) { var pitch = q.get_euler_pitch() * MathHelper.rad2deg; From 506ab983e125dbf2aae25d80829544d28bdba46e Mon Sep 17 00:00:00 2001 From: Bob Long Date: Wed, 11 Mar 2026 12:16:30 +1100 Subject: [PATCH 6/6] GimbalVideoControl: remove console writes Changed to a no-op method that can be uncommented for debugging/testing. --- Controls/GimbalVideoControl.cs | 33 +++++++++++++++++++-------------- 1 file changed, 19 insertions(+), 14 deletions(-) diff --git a/Controls/GimbalVideoControl.cs b/Controls/GimbalVideoControl.cs index e65bf125aa..092bfed0aa 100644 --- a/Controls/GimbalVideoControl.cs +++ b/Controls/GimbalVideoControl.cs @@ -29,6 +29,12 @@ public partial class GimbalVideoControl : UserControl, IMessageFilter // logger private static readonly ILog log = LogManager.GetLogger(System.Reflection.MethodBase.GetCurrentMethod().DeclaringType); + // Uncomment the Console.WriteLine line to enable debug output. + private static void DebugConsoleWrite(string format, params object[] args) + { + // Console.WriteLine(format, args); + } + private GimbalControlSettings preferences = new GimbalControlSettings(); private readonly GStreamer _stream = new GStreamer(); @@ -433,7 +439,7 @@ private void HandleHeldKeys() previousPitchRate = pitch; previousYawRate = yaw; selectedGimbalManager?.SetRatesCommandAsync(pitch, yaw, yaw_lock, selectedGimbalID); - Console.WriteLine($"Pitch: {pitch}, Yaw: {yaw}"); + DebugConsoleWrite("Pitch: {0}, Yaw: {1}", pitch, yaw); } float zoom = 0; @@ -452,7 +458,7 @@ private void HandleHeldKeys() { previousZoomRate = zoom; selectedCamera?.SetZoomAsync(zoom, CAMERA_ZOOM_TYPE.ZOOM_TYPE_CONTINUOUS); - Console.WriteLine($"Zoom: {zoom}"); + DebugConsoleWrite("Zoom: {0}", zoom); } } @@ -506,7 +512,7 @@ private void HandleKeyPress(Keys key) private void TakePicture() { - Console.WriteLine("Take picture"); + DebugConsoleWrite("Take picture"); selectedCamera?.TakeSinglePictureAsync(); } @@ -515,12 +521,12 @@ private void SetRecording(bool start) isRecording = start; if(start) { - Console.WriteLine("Start recording"); + DebugConsoleWrite("Start recording"); selectedCamera?.StartRecordingAsync(); } else { - Console.WriteLine("Stop recording"); + DebugConsoleWrite("Stop recording"); selectedCamera?.StopRecordingAsync(); } } @@ -528,7 +534,7 @@ private void SetRecording(bool start) private void SetYawLock(bool locked) { string message = locked ? "lock" : "follow"; - Console.WriteLine($"Set yaw {message}"); + DebugConsoleWrite("Set yaw {0}", message); yaw_lock = locked; yawLockToolStripMenuItem.Checked = locked; selectedGimbalManager?.SetRatesCommandAsync(previousPitchRate, previousYawRate, yaw_lock, selectedGimbalID); @@ -536,25 +542,25 @@ private void SetYawLock(bool locked) private void Retract() { - Console.WriteLine("Retract"); + DebugConsoleWrite("Retract"); selectedGimbalManager?.RetractAsync(); } private void Neutral() { - Console.WriteLine("Neutral"); + DebugConsoleWrite("Neutral"); selectedGimbalManager?.NeutralAsync(); } private void PointDown() { - Console.WriteLine("Point down"); + DebugConsoleWrite("Point down"); selectedGimbalManager?.SetAnglesCommandAsync(-90, 0, false, selectedGimbalID); } private void Home() { - Console.WriteLine("Home"); + DebugConsoleWrite("Home"); var loc = MainV2.comPort?.MAV?.cs.HomeLocation; selectedGimbalManager?.SetROILocationAsync(loc.Lat, loc.Lng, loc.Alt, frame: MAV_FRAME.GLOBAL); } @@ -632,8 +638,8 @@ private void VideoBox_Click(object sender, EventArgs e) return; } q = attitude * q; - Console.WriteLine("Attitude: {0:0.0} {1:0.0} {2:0.0}", attitude.get_euler_yaw() * MathHelper.rad2deg, attitude.get_euler_pitch() * MathHelper.rad2deg, attitude.get_euler_roll() * MathHelper.rad2deg); - Console.WriteLine("New: {0:0.0} {1:0.0} {2:0.0}", q.get_euler_yaw() * MathHelper.rad2deg, q.get_euler_pitch() * MathHelper.rad2deg, q.get_euler_roll() * MathHelper.rad2deg); + DebugConsoleWrite("Attitude: {0:0.0} {1:0.0} {2:0.0}", attitude.get_euler_yaw() * MathHelper.rad2deg, attitude.get_euler_pitch() * MathHelper.rad2deg, attitude.get_euler_roll() * MathHelper.rad2deg); + DebugConsoleWrite("New: {0:0.0} {1:0.0} {2:0.0}", q.get_euler_yaw() * MathHelper.rad2deg, q.get_euler_pitch() * MathHelper.rad2deg, q.get_euler_roll() * MathHelper.rad2deg); selectedGimbalManager?.SetAttitudeAsync(q, yaw_lock, selectedGimbalID); @@ -766,10 +772,9 @@ private void AutoConnectTimerCallback(object sender, System.Timers.ElapsedEventA { if (CameraProtocol.VideoStreams.Count < 1) { - Console.Write("Requesting video stream information..."); // We must not have any reported video streams. Try to request them. selectedCamera?.RequestVideoStreamInformation(); - Console.WriteLine(" done."); + DebugConsoleWrite("Requested video stream information"); // Come back later and see if any streams have been reported AutoConnectTimer.Start(); return;