public async Task GetTheSameMessageAsSendAsync()
{
HeartbeatMessage receivedHeartbeat = default(HeartbeatMessage);
IMavlinkCommunicator mavlinkCommunicator = MavlinkCommunicatorFactory.Create(Stream);
IMessageNotifier messageNotifier = mavlinkCommunicator.SubscribeForReceive(m => m.Id == MessageId.Heartbeat);
messageNotifier.MessageReceived += (s, e) =>
{
receivedHeartbeat = (HeartbeatMessage)e.Message;
};
HeartbeatMessage sentHeartbetat = new HeartbeatMessage
{
Autopilot = MavAutopilot.Ardupilotmega,
BaseMode =
MavModeFlag.CustomModeEnabled | MavModeFlag.StabilizeEnabled | MavModeFlag.ManualInputEnabled,
MavlinkVersion = 3,
Type = MavType.Quadrotor,
SystemStatus = MavState.Active,
CustomMode = 0
};
await mavlinkCommunicator.SendMessageAsync(sentHeartbetat, 1, 1);
Stream.Seek(0, SeekOrigin.Begin);
Thread.Sleep(3000);
Assert.AreEqual(sentHeartbetat.Autopilot, receivedHeartbeat.Autopilot);
Assert.AreEqual(sentHeartbetat.Type, receivedHeartbeat.Type);
Assert.AreEqual(sentHeartbetat.CustomMode, receivedHeartbeat.CustomMode);
Assert.AreEqual(sentHeartbetat.BaseMode, receivedHeartbeat.BaseMode);
Assert.AreEqual(sentHeartbetat.SystemStatus, receivedHeartbeat.SystemStatus);
Assert.AreEqual(sentHeartbetat.MavlinkVersion, receivedHeartbeat.MavlinkVersion);
mavlinkCommunicator.Dispose();
Thread.Sleep(1000);
}
private static void Main(string[] args)
{
var serialPort = new SerialPort("COM7", 115200);
serialPort.Open();
MavlinkCommunicatorFactory mavlinkCommunicatorFactory = new MavlinkCommunicatorFactory();
IMavlinkCommunicator mavlinkCommunicator = mavlinkCommunicatorFactory.Create(serialPort.BaseStream);
IMessageNotifier heartbeatNotifier =
mavlinkCommunicator.SubscribeForReceive(m => m.Id == MessageId.Heartbeat);
// IMessageNotifier systemTimeNotifier =
// mavlinkCommunicator.SubscribeForReceive(m => m.Id == MessageId.SystemTime);
//
// IMessageNotifier sysStatusNotifier =
// mavlinkCommunicator.SubscribeForReceive(m => m.Id == MessageId.SysStatus);
heartbeatNotifier.MessageReceived += (sender, eventArgs) =>
{
HeartbeatMessage heartbeatMessage = (HeartbeatMessage)eventArgs.Message;
Console.WriteLine($"Autopilot type: {heartbeatMessage.Autopilot}");
Console.WriteLine($"Vehicle type: {heartbeatMessage.Type}");
Console.WriteLine($"Base mode type: {heartbeatMessage.BaseMode}");
};
// systemTimeNotifier.MessageReceived += (sender, eventArgs) =>
// {
// SystemTimeMessage heartbeatMessage = (SystemTimeMessage)eventArgs.Message;
// Console.WriteLine($"TimeBootMs: {heartbeatMessage.TimeBootMs}");
// Console.WriteLine($"Time Unix uSec: {heartbeatMessage.TimeUnixUsec}");
// };
//
// sysStatusNotifier.MessageReceived += (sender, eventArgs) =>
// {
// SysStatusMessage heartbeatMessage = (SysStatusMessage)eventArgs.Message;
// Console.WriteLine($"Sensors enabled: {heartbeatMessage.SensorsEnabled}");
// };
while (true)
{
;
}
}
public async Task GetTheSameMessageCountAsSendAsync()
{
int messagesToSend = 20;
int messagesReceived = 0;
int messagesSuccessfullySent = 0;
IMavlinkCommunicator mavlinkCommunicator = MavlinkCommunicatorFactory.Create(Stream);
IMessageNotifier messageNotifier = mavlinkCommunicator.SubscribeForReceive(m => m.Id == MessageId.Heartbeat);
messageNotifier.MessageReceived += (s, e) =>
{
Interlocked.Increment(ref messagesReceived);
};
HeartbeatMessage sentHeartbeat = new HeartbeatMessage
{
Autopilot = MavAutopilot.Ardupilotmega,
BaseMode =
MavModeFlag.CustomModeEnabled | MavModeFlag.StabilizeEnabled | MavModeFlag.ManualInputEnabled,
MavlinkVersion = 3,
Type = MavType.Quadrotor,
SystemStatus = MavState.Active,
CustomMode = 0
};
for (int i = 0; i < messagesToSend; i++)
{
bool result = await mavlinkCommunicator.SendMessageAsync(sentHeartbeat, 1, 1);
if (result)
{
messagesSuccessfullySent++;
}
}
Stream.Seek(0, SeekOrigin.Begin);
Thread.Sleep(8000);
Assert.AreEqual(messagesSuccessfullySent, messagesReceived, 0,
$"Successfully sent {messagesSuccessfullySent}. Successfully received {messagesReceived}");
mavlinkCommunicator.Dispose();
Thread.Sleep(1000);
}