public SerialTelemetryLink(IDeviceManager deviceManager, IMavLinkMessageParser messageParser, IDispatcherServices dispatcherServices)
{
_deviceManager = deviceManager;
_messageParser = messageParser;
_dispatcherServices = dispatcherServices;
Messages = new ObservableCollection <MAVLinkMessage>();
_messageParser.MessageParsed += (sndr, msg) =>
{
Complete(msg);
_dispatcherServices.Invoke(() =>
{
MessageParsed(sndr, msg);
lock (this)
{
MessagesReceived++;
if (!Messages.Where(emsg => emsg.MessageInfo.MsgId == msg.MessageInfo.MsgId).Any())
{
Messages.Insert(0, msg);
if (Messages.Count == 100)
{
Messages.Remove(Messages.Last());
}
}
}
});
};
}
public async Task <InvokeResult> RefreshAdaptersAsync()
{
if (!_isConnected)
{
await _wifiAdapter.ScanAsync();
foreach (var networks in _wifiAdapter.NetworkReport.AvailableNetworks)
{
if (networks.Ssid.StartsWith("TELLO"))
{
await _wifiAdapter.ConnectAsync(networks, WiFiReconnectionKind.Automatic);
_dispatcherService.Invoke(() => Connected?.Invoke(this, new WiFiConnection(networks)));
Debug.WriteLine("CONNEcTED TO TELLO!");
_isConnected = true;
return(InvokeResult.Success);
}
}
}
_dispatcherService.Invoke(() => Disconnected?.Invoke(this, null));
return(InvokeResult.FromError("Could not connected"));
}
private async void StartListening()
{
_running = true;
var buffer = new byte[4096];
//https://mavlink.io/en/guide/serialization.html
//https://mavlink.io/en/protocol/overview.html
//TODO: Move to a service class
while (_running)
{
var readCount = await _serialPort.ReadAsync(buffer, 0, buffer.Length);
_dispatcherServices.Invoke(() =>
{
BytesReceived += readCount;
});
if (readCount > 0)
{
_messageParser.HandleBuffer(buffer, readCount);
}
}
}
protected virtual void HandlePacketReceived(object sender, MavLinkPacket e)
{
_dispatcher.Invoke(() =>
{
MessagesReceived++;
BytesReceived += e.PayLoadLength + 7;
switch (e.MessageId)
{
case UasMessages.Statustext:
var txt = System.Text.ASCIIEncoding.ASCII.GetString(e.Payload);
if (txt.ToLower().Contains("prearm"))
{
var existingMessage = PreArmMessages.Where(m => m.Message == txt).FirstOrDefault();
if (existingMessage != null)
{
PreArmMessages.Remove(existingMessage);
}
PreArmMessages.Insert(0, new StatusMessage(txt));
var oldItems = PreArmMessages.Where(oldMsg => oldMsg.IsExpired).ToList();
foreach (var itm in oldItems)
{
PreArmMessages.Remove(itm);
}
}
else
{
var existingMessage = StatusMessages.Where(m => m.Message == txt).FirstOrDefault();
if (existingMessage != null)
{
StatusMessages.Remove(existingMessage);
}
StatusMessages.Insert(0, new StatusMessage(txt));
var oldItems = StatusMessages.Where(oldMsg => oldMsg.IsExpired).ToList();
foreach (var itm in oldItems)
{
StatusMessages.Remove(itm);
}
}
break;
case UasMessages.Heartbeat:
var hb = e.Message as MavLink.UasHeartbeat;
SystemId = e.SystemId;
ComponentId = e.ComponentId;
break;
default:
var msg = Messages.Where(m => m.MessageId == e.MessageId).FirstOrDefault();
if (msg != null)
{
msg.ReceiptCount++;
}
else
{
e.ReceiptCount++;
Messages.Add(e);
}
break;
}
OnPacketReceived?.Invoke(this, e);
OnMessageReceived?.Invoke(this, e.Message);
Complete(e);
});
}
