diff --git a/Controls/GimbalVideoControl.cs b/Controls/GimbalVideoControl.cs index d185342d36..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); @@ -649,7 +655,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,10 +772,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.WriteLine(" done."); + // We must not have any reported video streams. Try to request them. + selectedCamera?.RequestVideoStreamInformation(); + DebugConsoleWrite("Requested video stream information"); // Come back later and see if any streams have been reported AutoConnectTimer.Start(); return; diff --git a/ExtLibs/ArduPilot/CurrentState.cs b/ExtLibs/ArduPilot/CurrentState.cs index 70e05a1e90..b93642b2d7 100644 --- a/ExtLibs/ArduPilot/CurrentState.cs +++ b/ExtLibs/ArduPilot/CurrentState.cs @@ -4526,8 +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.GimbalManager?.Discover(); + MAV.Camera?.UpdateRateIfChanged(MAV.cs.ratestatus); // use ratestatus until we create a new setting for this } 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. /// diff --git a/ExtLibs/ArduPilot/Mavlink/GimbalManagerProtocol.cs b/ExtLibs/ArduPilot/Mavlink/GimbalManagerProtocol.cs index c48193bd2c..38d69dc31b 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. @@ -15,19 +21,20 @@ 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. 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 +42,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 { - first_discover = false; - mavint.OnPacketReceived += MessagesHandler; + 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 + { + 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,16 +126,8 @@ private void MessagesHandler(object sender, MAVLink.MAVLinkMessage message) { ManagerInfo[0] = gmi; } - } - 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; - } + have_gimbal_manager_information = true; } if (message.msgid == (uint)MAVLink.MAVLINK_MSG_ID.GIMBAL_DEVICE_ATTITUDE_STATUS) @@ -93,32 +151,11 @@ 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. + /// 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)) @@ -128,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; } @@ -196,25 +242,26 @@ 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; - 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 3cd030b7f4..c908ce34fe 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 @@ -575,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) { @@ -967,6 +972,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 +6837,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; + } + } +}