Reading range sensor data from one Serial Port and writing output to another

Question

I'm reading range/distance sensor data from one Serial Port, decoding it and then transmitting it to a secondary device in ASCII format. The sensor uses a 2-byte header with each byte containing the following hex values: 0x59 0x59. Additional data from the sensor is also sent in a 7-byte message, subsequent to the header.

The code works but I'm not convinced that I'm reading the data correctly in DataReceived event handler. How can I improve this code?

internal static class Program
{
    private static long _validMessageCount;

    private static SerialPort _distanceOutputSerialPort;

    private const int FrameHeader = 0x59;

    public static void Main()
    {
        SerialPort lidarSensorSerialPort = null;

        try
        {
            lidarSensorSerialPort = new SerialPort(AppSettings.InputPort)
            {
                BaudRate = AppSettings.InputBaudRate,
                DataBits = AppSettings.InputDataBits,
                Handshake = Handshake.None,
                Parity = Parity.None,
                StopBits = StopBits.One
            };

            _distanceOutputSerialPort = new SerialPort(AppSettings.OutputPort)
            {
                BaudRate = AppSettings.OutputBaudRate,
                DataBits = AppSettings.OutputDataBits,
                Handshake = Handshake.None,
                Parity = Parity.None,
                StopBits = StopBits.One,
                NewLine = "\r",
                Encoding = Encoding.ASCII
            };

            _distanceOutputSerialPort.Open();
            lidarSensorSerialPort.DataReceived += DataReceivedHandler;
            lidarSensorSerialPort.Open();

            Console.Write("Waiting for data...");
            Console.ReadKey();
        }
        catch (IOException e)
        {
            Console.WriteLine(e.Message);
        }
        catch (Exception e)
        {
            Console.WriteLine(e.Message);
        }
        finally
        {
            lidarSensorSerialPort?.Close();
            _distanceOutputSerialPort?.Close();

            Console.Write("Press any key to exit...");
            Console.ReadKey();
        }
    }

    private static void DataReceivedHandler(object sender, SerialDataReceivedEventArgs e)
    {
        var headerBytes = new byte[2];
        var measuredRangeBytes = new byte[7];
        var serialPort = (SerialPort) sender;

        if (serialPort.BytesToRead < 9)
            return;

        while (true)
        {
            if (headerBytes[0] == FrameHeader && headerBytes[1] == FrameHeader)
                break;

            serialPort.Read(headerBytes, 0, headerBytes.Length);
        }

        var bytesRead = serialPort.Read(measuredRangeBytes, 0, measuredRangeBytes.Length);

        if (bytesRead < measuredRangeBytes.Length)
        {
            Console.WriteLine($"Invalid data Received. [{bytesRead}]");
            return;
        }

        var completeBytes = headerBytes.Concat(measuredRangeBytes).ToArray();
        var checksum = Helpers.CalculateChecksum(completeBytes);
        var check = measuredRangeBytes[6];

        if (checksum != check)
        {
            Console.WriteLine("Invalid Checksum");
            return;
        }

        CalculateDistance(measuredRangeBytes);
    }

    private static void CalculateDistance(IList<byte> distanceBytes)
    {
        var distanceLow = distanceBytes[0];
        var distanceHigh = distanceBytes[1];
        var strengthLow = distanceBytes[2];
        var strengthHigh = distanceBytes[3];
        var realibilityLevel = distanceBytes[4];
        var exposureTime = distanceBytes[5];
        var distance = Math.Round(distanceHigh * 256d + distanceLow, 2);
        var strength = strengthHigh * 256 + strengthLow;
        var time = $"{DateTime.Now:HH:mm:ss.fff}";

        _validMessageCount++;

        Console.WriteLine(
            $"{_validMessageCount}. {time} | {distance} | {strength} | {realibilityLevel} | {exposureTime}");

        ConvertOuput(distance);
    }

    private static void ConvertOuput(double distance)
    {
        var distanceInMeters = Math.Round(distance / 100d, 2);
        var mdlLmSensorOutput = $"{distanceInMeters.ToString("F").PadLeft(6, '0')}m";

        _distanceOutputSerialPort.WriteLine(mdlLmSensorOutput);

        Console.WriteLine($"Output: {mdlLmSensorOutput}");
    }
}

Answer 1

This

if (serialPort.BytesToRead < 9)
    return;

while (true)
{
    if (headerBytes[0] == FrameHeader && headerBytes[1] == FrameHeader)
        break;

    serialPort.Read(headerBytes, 0, headerBytes.Length);
}  

can get dangerous. Not only because what @firda has stated in his answer but the reading of the serialport without questioning the serialPort.BytesToRead again. Assume that at the if you get a serialPort.BytesToRead == 9 but if (headerBytes[0] == FrameHeader && headerBytes[1] == FrameHeader) isn't true then somewhere down the road your code could crash with a TimeoutException because you read but there aren't any bytes left to read.

A better approach would be to just read all the bytes avaible and place them in a ConcurrentQueue<List<byte>> like so

private static ConcurrentQueue<List<byte>> dataQueue = new ConcurrentQueue<List<byte>>();
static void DataReceivedHandler(object sender, SerialDataReceivedEventArgs e)
{
    var serialPort = sender as SerialPort;
    if (serialPort == null || serialPort.BytesToRead == 0) { return; }

    var receivedData = new byte[serialPort.BytesToRead];
    var readBytes = serialPort.Read(receivedData, 0, serialPort.BytesToRead);

    dataQueue.Enqueue(receivedData.Take(readBytes).ToList());

}

which keeps your DataReceived eventhandler short and clean.

To process the received data you can add a Timer e.g set to an interval of 250ms which calls a method ProcessData in its elapsed event only if dataQueue.Count > 0 like so

private static System.Timers.Timer timer = new System.Timers.Timer();
static void Main(string[] args)
{
    timer.Interval = 250;
    timer.Elapsed += new ElapsedEventHandler(timer_Elapsed);
    timer.Start();

    // Your SerialPort initialization stuff here

}  

static void timer_Elapsed(object sender, ElapsedEventArgs e)
{
    if (dataQueue.Count == 0) { return; }
    ProcessData();
}  

And last but not least the ProcessData()

private static readonly ReaderWriterLockSlim locker = new ReaderWriterLockSlim();
private static void ProcessData()
{
    if (dataQueue.Count == 0) { return; }
    if (!locker.TryEnterWriteLock(100)) { return; }

    if (isProcessing)
    {
        locker.ExitWriteLock();
        return;
    }
    isProcessing = true;


    List<byte> currentData;
    var data = new List<byte>();
    var headerIndex = 0;
    while (dataQueue.Count > 0)
    {
        dataQueue.TryDequeue(out currentData);
        data.AddRange(currentData);
        if (data.Count >= 9)
        {
            headerIndex = FindHeaderBytesIndex(data);
            if (headerIndex != -1 && data.Count - headerIndex >= 9)
            {
                // now process the message starting at headerIndex
                byte[] message = new byte[9];
                data.CopyTo(message, headerIndex);
                var checksum = Helpers.CalculateChecksum(message);

                var check = message[8];
                if (checksum != check)
                {
                    Console.WriteLine("Invalid Checksum");
                }
                CalculateDistance(message.Skip(2).ToArray());
            }
        }
    }

    isProcessing = false;
    locker.ExitWriteLock();
}

which needs to be tested by you but at a quick glance it should work.

If you want to know more about SerialPort althought most code is VB.NET, take a look at this.

Answer 2

The main problem I see is that you could miss the frame header if it was preceeded by odd number of different bytes (0xAA 0x59 0x59 ... you will try to match AA59 and then 59XX missing 5959). I would therefore rewrite it like this:

static int headerBytes; // counter to help us find 5959
static byte[] message = new byte[7]; // no need to allocate new array every time
static void DataReceivedHandler(object sender, SerialDataReceivedEventArgs e)
{
//  loop to process all messages in case we get 18B or more
    for(;;)
    {
    //  read until we find two header bytes
        while(headerByes < 2)
        {
        //  see Heslacher's answer for why this check is so important
            if (serialPort.BytesToRead == 0) return;
            int b = serialPort.ReadByte();
            if (b == FrameHeader) headerBytes++;
            else headerBytes = 0;
        }
    //  wait until we have full message (7 bytes)
        if (serialPort.BytesToRead < message.Length)
            return;
    //  read the message
        var bytesRead = serialPort.Read(message, 0, message.Length) 
    //  reset headerBytes counter before any return statement
        headerBytes = 0;
    //  check that everything is according to plan
        if (bytesRead < message.Length)
        {// this should never happen because we checked BytesToRead
            Console.WriteLine($"Invalid data Received. [{bytesRead}]");
            return;
        }
    //  now do something with the message, but beware that it does not contain the header now
    //  so, Helpers.CalculateChecksum would have to be modified
    //  or message made bigger to always include the header (and change the code above a bit)
    //  ...
        CalculateDistance(message);
    }
}

EDIT: I didn't know that ReadByte can throw TimeoutException (see Heslacher's answer), was expecting -1 in that case. I actually do not use SerialPort but my own version, because SerialPort was bugged in .NET 3.5 and caused uncatchable exceptions in GC (from finalizer) when used with Virtual COM (USB) - that could crash whole App and there was no way to fight it.