More fixes

src/devices/Seatalk1/AutoPilotRemoteController.cs

@@ -25,7 +25,7 @@ namespace Iot.Device.Seatalk1
     /// </remarks>
     public class AutoPilotRemoteController : MarshalByRefObject
     {
-        private const double AngleEpsilon = 1.1; // The protocol can only give angles in whole degrees
+        private const double AngleEpsilon = 0.9; // The protocol can only give angles in whole degrees
         private static readonly TimeSpan MaximumTimeout = TimeSpan.FromSeconds(6);
         private readonly SeatalkInterface _parentInterface;
         private readonly object _lock = new object();

src/devices/Seatalk1/SeatalkToNmeaConverter.cs

@@ -2,7 +2,9 @@
 // The .NET Foundation licenses this file to you under the MIT license.
 
 using System;
+using System.Collections.Concurrent;
 using System.Collections.Generic;
+using System.Threading;
 using Iot.Device.Common;
 using Iot.Device.Nmea0183;
 using Iot.Device.Nmea0183.Sentences;
@@ -25,6 +27,9 @@ public class SeatalkToNmeaConverter : NmeaSinkAndSource
         private SeatalkInterface _seatalkInterface;
         private bool _isDisposed;
         private ILogger _logger;
+        private BlockingCollection<Action> _sendQueue;
+        private Thread? _sendThread;
+        private CancellationTokenSource _terminatingCancellationTokenSource;
 
         /// <summary>
         /// Construct an instance of this class.
@@ -34,6 +39,8 @@ public class SeatalkToNmeaConverter : NmeaSinkAndSource
         public SeatalkToNmeaConverter(string interfaceName, string portName)
             : base(interfaceName)
         {
+            _terminatingCancellationTokenSource = new CancellationTokenSource();
+            _sendQueue = new BlockingCollection<Action>();
             _sentencesToTranslate = new();
             _logger = this.GetCurrentClassLogger();
             _seatalkInterface = new SeatalkInterface(portName);
@@ -50,6 +57,7 @@ public SeatalkToNmeaConverter(string interfaceName, string portName)
         /// RSA (Seatalk->Nmea)
         /// MWV (Nmea->Seatalk)
         /// RMB (Nmea->Seatalk)
+        /// HTC (Nmea->Seatalk, translated into commands)
         /// </remarks>
         public List<SentenceId> SentencesToTranslate => _sentencesToTranslate;
 
@@ -97,9 +105,22 @@ public override void StartDecode()
                 throw new ObjectDisposedException(nameof(SeatalkToNmeaConverter));
             }
 
+            _sendThread = new Thread(ProcessSendQueue);
+            _sendThread.Start();
             _seatalkInterface.StartDecode();
         }
 
+        private void ProcessSendQueue()
+        {
+            while (!_terminatingCancellationTokenSource.IsCancellationRequested)
+            {
+                if (_sendQueue.TryTake(out var item, -1, _terminatingCancellationTokenSource.Token))
+                {
+                    item();
+                }
+            }
+        }
+
         /// <summary>
         /// Send a sentence through the Seatalk port. This ignores unknown or disabled sentences.
         /// </summary>
@@ -126,6 +147,7 @@ public override void SendSentence(NmeaSinkAndSource source, NmeaSentence sentenc
                 }
             }
 
+            // Translations NMEA->Seatalk
             if (DoTranslate(sentence, out WindSpeedAndAngle? mwv) && mwv != null)
             {
                 ApparentWindAngle awa = new ApparentWindAngle()
@@ -147,6 +169,52 @@ public override void SendSentence(NmeaSinkAndSource source, NmeaSentence sentenc
                 NavigationToWaypoint nwp = new NavigationToWaypoint(rmb.CrossTrackError, rmb.BearingToWayPoint, true, rmb.DistanceToWayPoint);
                 _seatalkInterface.SendMessage(nwp);
             }
+
+            if (DoTranslate(sentence, out HeadingAndTrackControl? htc) && htc != null)
+            {
+                var ap = _seatalkInterface.GetAutopilotRemoteController();
+                if (ap.IsOperating && htc.DesiredHeading.HasValue)
+                {
+                    _sendQueue.Add(() => ap.TurnTo(htc.DesiredHeading.Value, null));
+                }
+
+                AutopilotStatus desiredStatus = htc.Status switch
+                {
+                    "M" => AutopilotStatus.Standby,
+                    "S" => AutopilotStatus.Auto,
+                    "W" => AutopilotStatus.Wind,
+                    "T" => AutopilotStatus.Track,
+
+                    _ => AutopilotStatus.Undefined,
+                };
+
+                _sendQueue.Add(() =>
+                {
+                    TurnDirection? confirm = null;
+
+                    if (desiredStatus == AutopilotStatus.Track)
+                    {
+                        if (!ap.SetStatus(desiredStatus, ref confirm))
+                        {
+                            ap.SetStatus(desiredStatus, ref confirm);
+                        }
+                    }
+                    else
+                    {
+                        ap.SetStatus(desiredStatus, ref confirm);
+                    }
+                });
+
+                if (ap.DeadbandMode == DeadbandMode.Automatic && htc.OffHeadingLimit.HasValue && htc.OffHeadingLimit.Value.Equals(Angle.Zero, Angle.FromDegrees(0.5)))
+                {
+                    _sendQueue.Add(() => ap.SetDeadbandMode(DeadbandMode.Minimal));
+                }
+
+                if (ap.DeadbandMode == DeadbandMode.Minimal && htc.OffHeadingLimit.HasValue && !htc.OffHeadingLimit.Value.Equals(Angle.Zero, Angle.FromDegrees(0.5)))
+                {
+                    _sendQueue.Add(() => ap.SetDeadbandMode(DeadbandMode.Automatic));
+                }
+            }
         }
 
         private bool DoTranslate<T>(NmeaSentence sentence, out T? convertedSentence)
@@ -167,6 +235,15 @@ private bool DoTranslate<T>(NmeaSentence sentence, out T? convertedSentence)
         /// </summary>
         public override void StopDecode()
         {
+            _terminatingCancellationTokenSource.Cancel();
+            _sendQueue.CompleteAdding();
+            if (_sendThread != null)
+            {
+                _sendThread.Join();
+                _sendThread = null;
+            }
+
+            _sendQueue.Dispose();
             _seatalkInterface.Dispose();
             _isDisposed = true;
         }