ServoPID: Misc failed attempts to make serial com more robust

Author: Marcus Sonestedt

ServoPID/ServoPIDControl/ArduinoCom.cs

@@ -38,7 +38,7 @@ public class ArduinoCom : IDisposable
         private SerialPort _port;
         private readonly StringBuilder _readBuf = new StringBuilder();
         private ArduinoModel _model;
-        private readonly DispatcherTimer _timer = new DispatcherTimer() {Interval = TimeSpan.FromMilliseconds(10)};
+        private readonly DispatcherTimer _timer = new DispatcherTimer {Interval = TimeSpan.FromMilliseconds(250)};
         private readonly object _portLock = new object();
 
         public ArduinoCom()
@@ -54,7 +54,7 @@ private void TimerOnTick(object sender, EventArgs e)
             if ((!_port?.IsOpen ?? false) || Model == null)
                 return;
 
-            SendCommand(Command.GetServoData, (byte)Model.Servos.Count);
+            SendCommand(Command.GetServoData, (byte) Model.Servos.Count);
         }
 
         public class StringEventArgs : EventArgs
@@ -71,39 +71,51 @@ private void PortOnDataReceived(object sender, SerialDataReceivedEventArgs e)
                 _readBuf.Append(_port.ReadExisting());
             }
 
-            while (_readBuf.ToString().Contains('\n'))
+            string str;
+            while ((str = _readBuf.ToString()).IndexOfAny(new [] {'\n', '\r'}) >= 0)
             {
-                //Debug.WriteLine(_readBuf.ToString());
-
-                var lines = _readBuf.ToString().Split('\n');
-                foreach (var line in lines.Select(l => l.Trim()).Where(l => !string.IsNullOrWhiteSpace(l)))
-                    LineReceived(line);
-
+                var lines = str.Split(new[] {'\n','\r'}, StringSplitOptions.RemoveEmptyEntries);
+                Application.Current.Dispatcher.Invoke(() => LineReceived(lines.First().Trim()));
                 _readBuf.Clear();
+                _readBuf.Append(string.Join("\n", lines.Skip(1)));
             }
         }
 
         private void LineReceived(string line)
         {
             if (line.StartsWith("DT "))
             {
-                if (float.TryParse(line.Substring(3), out var dt))
-                    Model.DeltaTime = dt;
-                else
-                    Debug.WriteLine("Bad DT received: " + line);
-                
+                var parts = line.Split(' ');
+                try
+                {
+                    Model.DeltaTime = float.Parse(parts[1]);
+                    Model.MinDt = float.Parse(parts[2]);
+                    Model.MaxDt = float.Parse(parts[3]);
+                }
+                catch (Exception e)
+                {
+                    Debug.WriteLine($"Bad DT received: {line} - {e.Message}");
+                }
+
                 return;
             }
 
             if (line.StartsWith("NS "))
             {
                 Application.Current.Dispatcher.Invoke(() =>
                 {
-                    Model.Servos.Clear();
+                    if (!int.TryParse(line.Substring(3), out var numServos)) 
+                        return;
 
-                    if (int.TryParse(line.Substring(3), out var numServos))
-                        for (var i = 0; i < numServos; ++i)
-                            Model.Servos.Add(new ServoPidModel(Model.Servos.Count));
+                    if (numServos >= 33)
+                    {
+                        Debug.WriteLine("Too many servos: " + numServos);
+                        return;
+                    }
+                   
+                    Model.Servos.Clear();
+                    for (var i = 0; i < numServos; ++i)
+                        Model.Servos.Add(new ServoPidModel(Model.Servos.Count));
                 });
 
                 SendCommand(Command.GetServoParams);
@@ -141,10 +153,22 @@ private void LineReceived(string line)
                 }
                 catch (Exception e)
                 {
-                    Debug.WriteLine("Bad servo data: " + line + " - " + e.Message);
+                    Debug.WriteLine($"Bad servo data: {line} - {e.Message}");
                     return;
                 }
             }
+            else if (line.StartsWith("ERR: "))
+            {
+                lock (_portLock)
+                {
+                    _port.WriteLine("RST");
+                }
+            }
+            else
+            {
+                Debug.WriteLine($"Ignored: {line}");
+                return;
+            }
 
             MessageReceived?.Invoke(this, new StringEventArgs {Message = line});
         }
@@ -221,18 +245,16 @@ private void ServosOnCollectionChanged(object sender, NotifyCollectionChangedEve
                     servo.PropertyChanged += ServoOnPropertyChanged;
 
             if (e.Action == NotifyCollectionChangedAction.Reset)
-            {
                 foreach (var servo in Model.Servos)
                     servo.PropertyChanged -= ServoOnPropertyChanged;
-            }
         }
 
         private void ModelOnPropertyChanged(object sender, PropertyChangedEventArgs e)
         {
             if (e.PropertyName == null || e.PropertyName == nameof(Model.Enabled))
                 SendCommand(Command.EnableRegulator, (byte) (Model.Enabled ? 1 : 0));
 
-            if ((e.PropertyName == null || e.PropertyName == nameof(Model.PortName)))
+            if (e.PropertyName == null || e.PropertyName == nameof(Model.PortName))
                 ConnectPort();
 
             if (e.PropertyName == null || e.PropertyName == nameof(Model.PollPidData))
@@ -288,7 +310,10 @@ private void ConnectPort()
                 {
                     _port = new SerialPort(Model.PortName)
                     {
-                        BaudRate = 115200
+                        BaudRate = 115200,
+                        NewLine = "\n",
+                        ReadBufferSize = 4096,
+                        WriteBufferSize = 4096,
                     };
                     _port.Open();
 

ServoPID/ServoPIDControl/ArduinoModel.cs

@@ -11,14 +11,16 @@ namespace ServoPIDControl
 {
     public class ArduinoModel : INotifyPropertyChanged, IDisposable
     {
-        private string _portName = "COM3";
+        private string _portName = null;
         private bool _enabled = true;
         private float _deltaTime;
         private bool _connected;
         private bool _pollPidData;
         private string[] _comPorts;
 
         private readonly DispatcherTimer _timer;
+        private float _minDt;
+        private float _maxDt;
 
         public event PropertyChangedEventHandler PropertyChanged;
 
@@ -102,6 +104,28 @@ internal set
             }
         }
 
+        public float MinDt
+        {
+            get => _minDt;
+            set
+            {
+                if (value.Equals(_minDt)) return;
+                _minDt = value;
+                OnPropertyChanged();
+            }
+        }
+
+        public float MaxDt
+        {
+            get => _maxDt;
+            set
+            {
+                if (value.Equals(_maxDt)) return;
+                _maxDt = value;
+                OnPropertyChanged();
+            }
+        }
+
         [NotifyPropertyChangedInvocator]
         protected virtual void OnPropertyChanged([CallerMemberName] string propertyName = null)
         {

ServoPID/ServoPIDControl/MainWindow.xaml

@@ -26,14 +26,20 @@
             </StackPanel.Resources>
             <TextBlock Text="COM Port:" />
             <ComboBox x:Name="PortNameComboBox" SelectedItem="{Binding PortName}" Text="COM Port"
-                      ItemsSource="{Binding ComPorts, Mode=Onetime}" />
+                      ItemsSource="{Binding ComPorts, Mode=OneWay}" />
             <CheckBox x:Name="EnabledCheckBox" IsChecked="{Binding Enabled}" Content="Regulator enabled"
                       IsEnabled="{Binding Connected, Mode=OneWay}" />
             <CheckBox x:Name="PollPidDataCheckBox" IsChecked="{Binding PollPidData}" Content="Poll PID data"
                       IsEnabled="{Binding Connected}" />
             <TextBlock Text="Delta Time:" />
             <TextBox x:Name="DeltaTimeTextBox" Text="{Binding DeltaTime, Mode=OneWay, StringFormat=F6}" IsReadOnly="true"
                      IsEnabled="{Binding Connected}" TextAlignment="Right" />
+            <TextBlock Text="Min Delta Time:" />
+            <TextBox x:Name="MinDeltaTimeTextBox" Text="{Binding MinDt, Mode=OneWay, StringFormat=F6}" IsReadOnly="true"
+                     IsEnabled="{Binding Connected}" TextAlignment="Right" />
+            <TextBlock Text="Max Delta Time:" />
+            <TextBox x:Name="MaxDeltaTimeTextBox" Text="{Binding MaxDt, Mode=OneWay, StringFormat=F6}" IsReadOnly="true"
+                     IsEnabled="{Binding Connected}" TextAlignment="Right" />
         </StackPanel>
         <DataGrid Grid.Row="0" Grid.Column="1"
                   ItemsSource="{Binding Servos}" x:Name="ServosDataGrid" IsEnabled="{Binding Connected}">

ServoPID/ServoPIDControl/MainWindow.xaml.cs

@@ -1,4 +1,6 @@
-using System.Windows;
+using System.ComponentModel;
+using System.Linq;
+using System.Windows;
 using ServoPIDControl.Annotations;
 
 namespace ServoPIDControl
@@ -17,11 +19,22 @@ public MainWindow()
 
             Loaded += OnLoaded;
             Unloaded += OnUnloaded;
+
+            Model.PropertyChanged += ModelOnPropertyChanged;
+        }
+
+        private void ModelOnPropertyChanged(object sender, PropertyChangedEventArgs e)
+        {
+            if (e.PropertyName == nameof(Model.ComPorts) && Model.PortName == null && Model.ComPorts != null)
+                Model.PortName = Model.ComPorts.FirstOrDefault();
         }
 
         private void OnLoaded(object sender, RoutedEventArgs e)
         {
             _arduinoCom.Model = Model;
+
+            if (Model.PortName == null && Model.ComPorts != null)
+                Model.PortName = Model.ComPorts.FirstOrDefault();
         }
 
         private void OnUnloaded(object sender, RoutedEventArgs e)