Asv Mavlink Help

With Virtual Connection

Create console application

Note: Rider will be used in this guide, you are free to choose any other IDE for C# that you prefer.

  1. Open your IDE and select the Project Type: Console

    image

  2. Leave everything as default. You may choose a different project name if you wish.

  3. Press Create to create a project.

  4. You should now see the default project page.

    image

Install Required NuGet Packages

  1. Open the NuGet Package Manager and search for the Asv.Mavlink packet.

  2. Install the latest version (this guide is relevant to version 4.0.0 and higher)

    image

The DeepEqual package is also used in this guide for testing purposes.

image

Setup task completion source and cancellation token

  1. Create a task completion source. You'll need it to wait for a client initialization.

var tcsInitialize = new TaskCompletionSource();
  1. Create a cancellation token source with a timeout. The cancellation token will trigger after 20 seconds. You can change this by setting a different time span.

using var cancelInitialize = new CancellationTokenSource(TimeSpan.FromSeconds(20), TimeProvider.System);
  1. Register a delegate that will be called when the cancellation token is canceled. This will stop tcs from running in a loop when the token fails.

cancelInitialize.Token.Register(() => tcsInitialize.TrySetCanceled());

Note: Virtual MAVLink connection is used for testing purposes only. You'll need to create a router for a real MAVLink connection. Refer to With Ardu SITL for more information.

  1. Create a virtual connection.

var protocol = Protocol.Create(builder => { builder.SetTimeProvider(TimeProvider.System); builder.RegisterMavlinkV2Protocol(); builder.Features.RegisterBroadcastFeature<MavlinkMessage>(); builder.Formatters.RegisterSimpleFormatter(); }); var link = protocol.CreateVirtualConnection();
  1. Create a MAVLink client identity. Assign SystemId, ComponentId, TargetSystemId, and TargetComponentId. You can read more about it in the official mavlink guide.

var identity = new MavlinkClientIdentity(1,2,3,4);

Device creation

  1. Create a packet sequence calculator.

// Class for calculating the next sequence number of a packet var clientSeq = new PacketSequenceCalculator();
  1. Provide core services with a log factory to create a logger and a different time provider. This class provides everything that your device or MAVLink microservice will use.

var clientCore = new CoreServices( link.Client, clientSeq, logFactory: null, timeProvider: TimeProvider.System, new DefaultMeterFactory() );
  1. Create a default device id.

var deviceId = new MavlinkClientDeviceId("COPTER", identity);
  1. Create a configuration for the client device.

MavlinkClientDeviceConfig config = new() { Heartbeat = { HeartbeatTimeoutMs = 1000, LinkQualityWarningSkipCount = 3, RateMovingAverageFilter = 10, } };
  1. Finally, create a MavlinkClient client device.

var client = new MavlinkClientDevice( deviceId, config, [], clientCore );
  1. Create a packet sequence calculator and core services for the server.

var serverSeq = new PacketSequenceCalculator(); var serverCore = new CoreServices( link.Server, serverSeq, logFactory: null, timeProvider: TimeProvider.System, new DefaultMeterFactory() );
  1. Create heartbeat server.

var server = new HeartbeatServer( identity.Target, new MavlinkHeartbeatServerConfig(), serverCore );
  1. Start the server (note: some servers may not have this method).

server.Start();
  1. Create a variable to save the payload sent from the server.

HeartbeatPayload? payload = null;
  1. Set the payload for the MAVLink package. This operation triggers the process that sends data to the client.

server.Set(p => { p.Type = MavType.MavTypeBattery; p.BaseMode = MavModeFlag.MavModeFlagGuidedEnabled; Console.WriteLine($"From Server Type: {p.Type}, From server baseMode: {p.BaseMode}"); payload = p; });

Heartbeat client initialization

  1. Call client initialization

client.Initialize();
  1. Wait until the client is initialized

var sub2 = client.State .Subscribe(x => { if (x != ClientDeviceState.Complete) { return; } tcsInitialize.TrySetResult(); }); await tcsInitialize.Task;
  1. Dispose the subscription, because we don't need it anymore

sub2.Dispose();

Setup for Heartbeat test

Setup new tcs and cancellation token for the heartbeat test

var tcsHeartbeat = new TaskCompletionSource(); using var cancelHeartbeat = new CancellationTokenSource(TimeSpan.FromSeconds(20), TimeProvider.System); cancelHeartbeat.Token.Register(() => tcsHeartbeat.TrySetCanceled());

Check for the Heartbeat client

var heartbeat = client.GetMicroservice<IHeartbeatClient>(); if (heartbeat is null) { throw new Exception("Heartbeat client not found"); }

Send data from server to client

  1. Asv.Mavlink uses the R3 library, which allows you to use a reactive programming approach. To catch data received from the server, subscribe to its source.

HeartbeatPayload? result = null; var sub = heartbeat.RawHeartbeat .Subscribe(p => { if (p is null) { return; } Console.WriteLine($"Heartbeat type: {p.Type}, Heartbeat baseMode: {p.BaseMode}"); result = p; tcsHeartbeat.TrySetResult(); });
  1. Wait for the result and remove subscription

await tcsHeartbeat.Task; sub.Dispose();

Compare data

Console.WriteLine( !payload.IsDeepEqual(result) ? "Result is not equal to payload from server" : "Payload from server is equal to result" );

Code's output

Proper console output should look something like that:

From Server Type: MavTypeBattery, From server baseMode: MavModeFlagGuidedEnabled Heartbeat type: MavTypeBattery, Heartbeat baseMode: MavModeFlagGuidedEnabled Payload from server is equal to result Process finished with exit code 0.

Clear the resources

await Task.Delay(TimeSpan.FromMilliseconds(500)); // Wait till the server gets unlocked client.Dispose(); server.Dispose(); heartbeat.Dispose();

Complete code

using Asv.Common; using Asv.IO; using Asv.Mavlink; using Asv.Mavlink.Minimal; using DeepEqual.Syntax; using R3; // Setup for device initialization var tcsInitialize = new TaskCompletionSource(); using var cancelInitialize = new CancellationTokenSource(TimeSpan.FromSeconds(20), TimeProvider.System); cancelInitialize.Token.Register(() => tcsInitialize.TrySetCanceled()); // Virtual mavlink connection var protocol = Protocol.Create(builder => { builder.SetTimeProvider(TimeProvider.System); builder.RegisterMavlinkV2Protocol(); builder.Features.RegisterBroadcastFeature<MavlinkMessage>(); builder.Formatters.RegisterSimpleFormatter(); }); var link = protocol.CreateVirtualConnection(); // // Client Device var identity = new MavlinkClientIdentity(1,2,3,4); var clientSeq = new PacketSequenceCalculator(); var clientCore = new CoreServices( link.Client, clientSeq, logFactory: null, timeProvider: TimeProvider.System, new DefaultMeterFactory() ); var deviceId = new MavlinkClientDeviceId("COPTER", identity); MavlinkClientDeviceConfig config = new() { Heartbeat = { HeartbeatTimeoutMs = 1000, LinkQualityWarningSkipCount = 3, RateMovingAverageFilter = 10, } }; var client = new MavlinkClientDevice( deviceId, config, [], clientCore ); // // Heartbeat server var serverSeq = new PacketSequenceCalculator(); var serverCore = new CoreServices( link.Server, serverSeq, logFactory: null, timeProvider: TimeProvider.System, new DefaultMeterFactory() ); var server = new HeartbeatServer( identity.Target, new MavlinkHeartbeatServerConfig(), serverCore ); server.Start(); HeartbeatPayload? payload = null; server.Set(p => { p.Type = MavType.MavTypeBattery; p.BaseMode = MavModeFlag.MavModeFlagGuidedEnabled; Console.WriteLine($"From Server Type: {p.Type}, From server baseMode: {p.BaseMode}"); payload = p; }); // // Heartbeat client initialization client.Initialize(); var sub2 = client.State .Subscribe(x => { if (x != ClientDeviceState.Complete) { return; } tcsInitialize.TrySetResult(); }); await tcsInitialize.Task; sub2.Dispose(); // // Setup for Heartbeat test var tcsHeartbeat = new TaskCompletionSource(); using var cancelHeartbeat = new CancellationTokenSource(TimeSpan.FromSeconds(20), TimeProvider.System); cancelHeartbeat.Token.Register(() => tcsHeartbeat.TrySetCanceled()); // // Test var heartbeat = client.GetMicroservice<IHeartbeatClient>(); if (heartbeat is null) { throw new Exception("Heartbeat client not found"); } HeartbeatPayload? result = null; var sub = heartbeat.RawHeartbeat .Subscribe(p => { if (p is null) { return; } Console.WriteLine($"Heartbeat type: {p.Type}, Heartbeat baseMode: {p.BaseMode}"); result = p; tcsHeartbeat.TrySetResult(); }); await tcsHeartbeat.Task; sub.Dispose(); Console.WriteLine( !payload.IsDeepEqual(result) ? "Result is not equal to payload from server" : "Payload from server is equal to result" ); // await Task.Delay(TimeSpan.FromMilliseconds(500)); // Wait till the server gets unlocked // Dispose client.Dispose(); server.Dispose(); heartbeat.Dispose(); //
Last modified: 23 May 2025