ardupilot/Configurator/Configurator.Net/CommsSession.cs

138 lines
3.5 KiB
C#
Raw Normal View History

using System;
using System.Collections.Generic;
using System.ComponentModel;
using System.IO.Ports;
using System.Text;
using System.Threading;
using ArducopterConfigurator.PresentationModels;
namespace ArducopterConfigurator
{
public interface IComms
{
event Action<string> LineOfDataReceived;
string CommPort { get; set; }
bool IsConnected { get; }
IEnumerable<string> ListCommPorts();
void Send(string send);
bool Connect();
bool DisConnect();
}
/// <summary>
/// Represents a session of communication with the Arducopter
/// </summary>
/// <remarks>
/// Looks after connection state etc
/// </remarks>
public class CommsSession : IComms
{
private readonly SerialPort _sp;
private BackgroundWorker _bgWorker;
public event Action<string> LineOfDataReceived;
public CommsSession()
{
_sp = new SerialPort();
}
public string CommPort { get; set; }
public bool Connect()
{
_sp.BaudRate = 115200;
_sp.PortName = CommPort;
try
{
_sp.Open();
_sp.ReadTimeout = 50000;
// start the reading BG thread
_bgWorker = new BackgroundWorker();
_bgWorker.DoWork += bgWorker_DoWork;
_bgWorker.WorkerReportsProgress = true;
_bgWorker.WorkerSupportsCancellation = true;
_bgWorker.ProgressChanged += bgWorker_ProgressChanged;
_bgWorker.RunWorkerAsync();
}
catch (Exception ex)
{
Error = ex.Message;
}
return true;
}
public bool DisConnect()
{
_bgWorker.CancelAsync();
_sp.Close();
return true;
}
void bgWorker_ProgressChanged(object sender, ProgressChangedEventArgs e)
{
// Thanks to BG worker, this should be raised on the UI thread
var lineReceived = e.UserState as string;
// for (int i = 0; i < lineReceived.Length; i++)
// {
// var c = lineReceived[i];
// Console.WriteLine("{0}] U+{1:x4} {2}", i, (int)c, (int)c);
// }
//
//
//
if (LineOfDataReceived != null)
LineOfDataReceived(lineReceived);
}
void bgWorker_DoWork(object sender, DoWorkEventArgs e)
{
while (!_bgWorker.CancellationPending)
{
try
{
var line = _sp.ReadLine();
_bgWorker.ReportProgress(0, line);
}
catch(TimeoutException)
{
// continue
}
catch(System.IO.IOException) // when the port gets killed
{}
catch(ObjectDisposedException)
{}
}
}
private string Error { get; set;}
public bool IsConnected
{
get { return _sp.IsOpen; }
}
public IEnumerable<string> ListCommPorts()
{
return SerialPort.GetPortNames();
}
public void Send(string send)
{
if (_sp.IsOpen)
_sp.Write(send);
}
}
}