Skip to content

Commit bacb083

Browse files
committed
feat(api-mavlink): update mavlink nuget;add RFSA device
1 parent 37bcc1a commit bacb083

File tree

16 files changed

+91
-59
lines changed

16 files changed

+91
-59
lines changed

src/Asv.Drones.Gui.Api/CompatibilitySuppressions.xml

Lines changed: 8 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -3,7 +3,14 @@
33
<Suppressions xmlns:xsi="http://www.w3.org/2001/XMLSchema-instance" xmlns:xsd="http://www.w3.org/2001/XMLSchema">
44
<Suppression>
55
<DiagnosticId>CP0006</DiagnosticId>
6-
<Target>P:Asv.Drones.Gui.Api.ILocalizationService.Accuracy</Target>
6+
<Target>M:Asv.Drones.Gui.Api.IMavlinkDevicesService.GetRfsaByFullId(System.UInt16)</Target>
7+
<Left>lib/net8.0/Asv.Drones.Gui.Api.dll</Left>
8+
<Right>lib/net8.0/Asv.Drones.Gui.Api.dll</Right>
9+
<IsBaselineSuppression>true</IsBaselineSuppression>
10+
</Suppression>
11+
<Suppression>
12+
<DiagnosticId>CP0006</DiagnosticId>
13+
<Target>P:Asv.Drones.Gui.Api.IMavlinkDevicesService.RfsaDevices</Target>
714
<Left>lib/net8.0/Asv.Drones.Gui.Api.dll</Left>
815
<Right>lib/net8.0/Asv.Drones.Gui.Api.dll</Right>
916
<IsBaselineSuppression>true</IsBaselineSuppression>

src/Asv.Drones.Gui.Api/Services/LogService/ILogService.cs

Lines changed: 2 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -8,13 +8,13 @@ public interface ILogService
88
void DeleteLogFile();
99

1010
public void Fatal(string sender, string message,
11-
Exception ex = default)
11+
Exception? ex = default)
1212
{
1313
SaveMessage(new LogMessage(DateTime.Now, LogMessageType.Fatal, sender, message, ex?.Message));
1414
}
1515

1616
public void Error(string sender, string message,
17-
Exception ex = default)
17+
Exception? ex = default)
1818
{
1919
SaveMessage(new LogMessage(DateTime.Now, LogMessageType.Error, sender, message, ex?.Message));
2020
}

src/Asv.Drones.Gui.Api/Services/Mavlink/IMavlinkDevicesService.cs

Lines changed: 2 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -54,12 +54,13 @@ public interface IMavlinkDevicesService
5454
/// <param name="id">Id of searched vehicle</param>
5555
/// <returns>Vehicle object</returns>
5656
IVehicleClient? GetVehicleByFullId(ushort id);
57-
5857
IObservable<IChangeSet<IGbsClientDevice, ushort>> BaseStations { get; }
5958
IGbsClientDevice? GetGbsByFullId(ushort id);
6059
IObservable<IChangeSet<ISdrClientDevice, ushort>> Payloads { get; }
6160
ISdrClientDevice? GetPayloadsByFullId(ushort id);
6261
IObservable<IChangeSet<IAdsbClientDevice, ushort>> AdsbDevices { get; }
6362
IAdsbClientDevice? GetAdsbVehicleByFullId(ushort id);
63+
IObservable<IChangeSet<IRfsaClientDevice, ushort>> RfsaDevices { get; }
64+
IAdsbClientDevice? GetRfsaByFullId(ushort id);
6465
}
6566
}

src/Asv.Drones.Gui.Api/Tools/Controls/HierarchicalStore/HierarchicalStoreEntryViewModel.cs

Lines changed: 2 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -78,7 +78,7 @@ public HierarchicalStoreEntryViewModel()
7878
[Reactive] public string Description { get; set; }
7979

8080

81-
protected virtual void OnError(HierarchicalStoreEntryAction action, Exception exception)
81+
protected virtual void OnError(HierarchicalStoreEntryAction action, Exception? exception)
8282
{
8383
}
8484

@@ -161,7 +161,7 @@ public HierarchicalStoreEntryViewModel(Node<IHierarchicalStoreEntry<TKey>, TKey>
161161
ParentId = node.Item.ParentId;
162162
}
163163

164-
protected override void OnError(HierarchicalStoreEntryAction action, Exception ex)
164+
protected override void OnError(HierarchicalStoreEntryAction action, Exception? ex)
165165
{
166166
base.OnError(action, ex);
167167
_log.Error("Store", $"Error to '{action}' entry", ex);

src/Asv.Drones.Gui.Api/Tools/Controls/HierarchicalStore/HierarchicalStoreViewModel.cs

Lines changed: 2 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -172,7 +172,7 @@ protected virtual void CreateNewFolderImpl()
172172
{
173173
}
174174

175-
protected virtual void OnError(HierarchicalStoreAction action, Exception ex)
175+
protected virtual void OnError(HierarchicalStoreAction action, Exception? ex)
176176
{
177177
}
178178

@@ -236,7 +236,7 @@ public HierarchicalStoreViewModel(Uri id, IHierarchicalStore<TKey, TFile> store,
236236
.Subscribe(x => x?.Refresh()).DisposeItWith(Disposable);
237237
}
238238

239-
protected override void OnError(HierarchicalStoreAction action, Exception ex)
239+
protected override void OnError(HierarchicalStoreAction action, Exception? ex)
240240
{
241241
_log.Error(DisplayName, $"Error to '{action:G}'", ex);
242242
}

src/Asv.Drones.Gui.Api/Tools/Controls/Params/ParamItemViewModel.cs

Lines changed: 2 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -193,12 +193,12 @@ public ParamItemViewModel(Uri id, IParamItem paramItem, ILogService log) : base(
193193
.DisposeItWith(Disposable);
194194
}
195195

196-
private void OnWriteError(Exception ex)
196+
private void OnWriteError(Exception? ex)
197197
{
198198
_log.Error("Params", $"Write {Name} error", ex);
199199
}
200200

201-
private void OnReadError(Exception ex)
201+
private void OnReadError(Exception? ex)
202202
{
203203
_log.Error("Params", $"Read {Name} error", ex);
204204
}

src/Asv.Drones.Gui.Api/Tools/Controls/Params/ParamPageViewModel.cs

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -251,7 +251,7 @@ public override async Task<bool> TryClose()
251251
return true;
252252
}
253253

254-
private void OnRefreshError(Exception ex)
254+
private void OnRefreshError(Exception? ex)
255255
{
256256
_log.Error("Params view", "Error to read all params items", ex);
257257
}

src/Asv.Drones.Gui.Api/Tools/Controls/TreePage/TreePageExplorerViewModel.cs

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -158,7 +158,7 @@ private async Task<bool> Navigate(TreePartMenuItemContainer? menu)
158158
menu.IsSelected = true;
159159
return true;
160160
}
161-
catch (Exception e)
161+
catch (Exception? e)
162162
{
163163
_log.Error(Title, $"Can't create page {menu.Base.Name}:{e.Message}", e);
164164
return false;

src/Asv.Drones.Gui/Services/Mavlink/MavlinkDevicesService.cs

Lines changed: 62 additions & 38 deletions
Original file line numberDiff line numberDiff line change
@@ -33,6 +33,7 @@ public class MavlinkDeviceServiceConfig
3333
public SdrClientDeviceConfig Sdr { get; set; } = new();
3434
public AdsbClientDeviceConfig Adsb { get; set; } = new();
3535
public bool WrapToV2ExtensionEnabled { get; set; } = true;
36+
public RfsaClientDeviceConfig Rfsa { get; set; } = new();
3637
}
3738

3839
[Export(typeof(IMavlinkDevicesService))]
@@ -61,8 +62,8 @@ public MavlinkDevicesService(IConfiguration config, IPacketSequenceCalculator se
6162
_mavlinkRouter =
6263
new MavlinkRouter(MavlinkV2Connection.RegisterDefaultDialects,
6364
publishScheduler: RxApp.MainThreadScheduler).DisposeItWith(Disposable);
64-
_mavlinkRouter.WrapToV2ExtensionEnabled = InternalGetConfig(_ => _.WrapToV2ExtensionEnabled);
65-
foreach (var port in InternalGetConfig(_ => _.Ports))
65+
_mavlinkRouter.WrapToV2ExtensionEnabled = InternalGetConfig(s => s.WrapToV2ExtensionEnabled);
66+
foreach (var port in InternalGetConfig(s => s.Ports))
6667
{
6768
_mavlinkRouter.AddPort(port);
6869
}
@@ -76,60 +77,60 @@ public MavlinkDevicesService(IConfiguration config, IPacketSequenceCalculator se
7677

7778
#region InitUriHost mavlink heartbeat
7879

79-
var serverIdentity = InternalGetConfig(_ => new MavlinkIdentity(_.SystemId, _.ComponentId));
80-
var serverConfig = InternalGetConfig(_ => new ServerDeviceConfig
81-
{ Heartbeat = new MavlinkHeartbeatServerConfig { HeartbeatRateMs = _.HeartbeatRateMs } });
80+
var serverIdentity = InternalGetConfig(s => new MavlinkIdentity(s.SystemId, s.ComponentId));
81+
var serverConfig = InternalGetConfig(s => new ServerDeviceConfig
82+
{ Heartbeat = new MavlinkHeartbeatServerConfig { HeartbeatRateMs = s.HeartbeatRateMs } });
8283
var serverDevice =
8384
new ServerDevice(Router, sequenceCalculator, serverIdentity, serverConfig, Scheduler.Default)
8485
.DisposeItWith(Disposable);
85-
serverDevice.Heartbeat.Set(_ =>
86+
serverDevice.Heartbeat.Set(p =>
8687
{
87-
_.Autopilot = MavAutopilot.MavAutopilotInvalid;
88-
_.BaseMode = 0;
89-
_.CustomMode = 0;
90-
_.MavlinkVersion = 3;
91-
_.SystemStatus = MavState.MavStateActive;
92-
_.Type = MavType.MavTypeGcs;
88+
p.Autopilot = MavAutopilot.MavAutopilotInvalid;
89+
p.BaseMode = 0;
90+
p.CustomMode = 0;
91+
p.MavlinkVersion = 3;
92+
p.SystemStatus = MavState.MavStateActive;
93+
p.Type = MavType.MavTypeGcs;
9394
});
9495
_systemId = new RxValue<byte>(serverIdentity.SystemId).DisposeItWith(Disposable);
9596
_systemId
9697
.Throttle(TimeSpan.FromSeconds(1))
9798
.DistinctUntilChanged()
9899
.Skip(1)
99100
.Do(_ => _needReloadToApplyConfig.Value = true)
100-
.Subscribe(_ => InternalSaveConfig(cfg => cfg.SystemId = _))
101+
.Subscribe(b => InternalSaveConfig(cfg => cfg.SystemId = b))
101102
.DisposeItWith(Disposable);
102103
_componentId = new RxValue<byte>(serverIdentity.ComponentId).DisposeItWith(Disposable);
103104
_componentId
104105
.Throttle(TimeSpan.FromSeconds(1))
105106
.DistinctUntilChanged()
106107
.Skip(1)
107108
.Do(_ => _needReloadToApplyConfig.Value = true)
108-
.Subscribe(_ => InternalSaveConfig(cfg => cfg.ComponentId = _))
109+
.Subscribe(b => InternalSaveConfig(cfg => cfg.ComponentId = b))
109110
.DisposeItWith(Disposable);
110-
var heartbeatRateMs = InternalGetConfig(_ => TimeSpan.FromMilliseconds(_.HeartbeatRateMs));
111+
var heartbeatRateMs = InternalGetConfig(s => TimeSpan.FromMilliseconds(s.HeartbeatRateMs));
111112
_heartBeatRate = new RxValue<TimeSpan>(heartbeatRateMs).DisposeItWith(Disposable);
112113
_heartBeatRate
113114
.Throttle(TimeSpan.FromSeconds(1))
114115
.DistinctUntilChanged()
115116
.Skip(1)
116117
.Do(_ => _needReloadToApplyConfig.Value = true)
117-
.Subscribe(_ => { InternalSaveConfig(cfg => cfg.HeartbeatRateMs = (int)_.TotalMilliseconds); })
118+
.Subscribe(s => { InternalSaveConfig(cfg => cfg.HeartbeatRateMs = (int)s.TotalMilliseconds); })
118119
.DisposeItWith(Disposable);
119120
serverDevice.Heartbeat.Start();
120121

121122
#endregion
122123

123124
#region Mavlink devices
124125

125-
var deviceTimeout = InternalGetConfig(_ => TimeSpan.FromMilliseconds(_.DeviceHeartbeatTimeoutMs));
126+
var deviceTimeout = InternalGetConfig(s => TimeSpan.FromMilliseconds(s.DeviceHeartbeatTimeoutMs));
126127
_deviceBrowser = new MavlinkDeviceBrowser(_mavlinkRouter, deviceTimeout, RxApp.MainThreadScheduler)
127128
.DisposeItWith(Disposable);
128129
_deviceBrowser.DeviceTimeout
129130
.Throttle(TimeSpan.FromSeconds(1))
130131
.DistinctUntilChanged()
131132
.Skip(1)
132-
.Subscribe(_ => InternalSaveConfig(cfg => cfg.DeviceHeartbeatTimeoutMs = (int)_.TotalMilliseconds))
133+
.Subscribe(s => InternalSaveConfig(cfg => cfg.DeviceHeartbeatTimeoutMs = (int)s.TotalMilliseconds))
133134
.DisposeItWith(Disposable);
134135

135136
#endregion
@@ -138,25 +139,30 @@ public MavlinkDevicesService(IConfiguration config, IPacketSequenceCalculator se
138139

139140
Vehicles = Devices
140141
.Transform(CreateVehicle)
141-
.Filter(_ => _ != null)
142+
.Filter(c => c != null)
142143
.DisposeMany()
143144
.RefCount();
144145

145146
BaseStations = Devices
146-
.Filter(_ => _.Type == (MavType)Mavlink.V2.AsvGbs.MavType.MavTypeAsvGbs)
147+
.Filter(d => d.Type == (MavType)Mavlink.V2.AsvGbs.MavType.MavTypeAsvGbs)
147148
.Transform(CreateBaseStation)
148149
.DisposeMany()
149150
.RefCount();
150151
Payloads = Devices
151-
.Filter(_ => _.Type == (MavType)Mavlink.V2.AsvSdr.MavType.MavTypeAsvSdrPayload)
152+
.Filter(d => d.Type == (MavType)Mavlink.V2.AsvSdr.MavType.MavTypeAsvSdrPayload)
152153
.Transform(CreateSdrDevice)
153154
.DisposeMany()
154155
.RefCount();
155156
AdsbDevices = Devices
156-
.Filter(_ => _.Type == MavType.MavTypeAdsb)
157+
.Filter(d => d.Type == MavType.MavTypeAdsb)
157158
.Transform(CreateAdsbDevice)
158159
.DisposeMany()
159160
.RefCount();
161+
RfsaDevices = Devices
162+
.Filter(d => d.Type == (MavType)Mavlink.V2.AsvRfsa.MavType.MavTypeAsvRfsa)
163+
.Transform(CreateRfsaDevice)
164+
.DisposeMany()
165+
.RefCount();
160166

161167
AllDevices = Vehicles.Transform(x => (IClientDevice)x)
162168
.MergeChangeSets(BaseStations.Transform(x => (IClientDevice)x))
@@ -171,9 +177,9 @@ public MavlinkDevicesService(IConfiguration config, IPacketSequenceCalculator se
171177
.Merge(BaseStations.Transform(x => (IClientDevice)x))
172178
.Merge(Payloads.Transform(x => (IClientDevice)x))
173179
.Merge(AdsbDevices.Transform(x => (IClientDevice)x))
174-
.AutoRefreshOnObservable(_ => _.Name)
175-
.Filter(_ => _.Name.Value != null)
176-
.Transform(_ => _.Name.Value, true)
180+
.AutoRefreshOnObservable(d => d.Name)
181+
.Filter(d => d.Name.Value != null)
182+
.Transform(d => d.Name.Value, true)
177183
.AsObservableCache().DisposeItWith(Disposable);
178184
_mavlinkRouter
179185
.Filter<StatustextPacket>()
@@ -184,6 +190,17 @@ public MavlinkDevicesService(IConfiguration config, IPacketSequenceCalculator se
184190
#endregion
185191
}
186192

193+
private IRfsaClientDevice CreateRfsaDevice(IMavlinkDevice device)
194+
{
195+
return new RfsaClientDevice(Router, new MavlinkClientIdentity
196+
{
197+
TargetSystemId = device.SystemId,
198+
TargetComponentId = device.ComponentId,
199+
SystemId = _systemId.Value,
200+
ComponentId = _componentId.Value,
201+
}, InternalGetConfig(c => c.Rfsa), _sequenceCalculator, RxApp.MainThreadScheduler);
202+
}
203+
187204
private ISdrClientDevice CreateSdrDevice(IMavlinkDevice device)
188205
{
189206
var dev = new SdrClientDevice(Router, new MavlinkClientIdentity
@@ -192,7 +209,7 @@ private ISdrClientDevice CreateSdrDevice(IMavlinkDevice device)
192209
TargetComponentId = device.ComponentId,
193210
SystemId = _systemId.Value,
194211
ComponentId = _componentId.Value,
195-
}, InternalGetConfig(_ => _.Sdr), _sequenceCalculator, RxApp.MainThreadScheduler);
212+
}, InternalGetConfig(c => c.Sdr), _sequenceCalculator, RxApp.MainThreadScheduler);
196213
((ParamsClientEx)dev.Params).Init(new MavParamByteWiseEncoding(), ArraySegment<ParamDescription>.Empty);
197214
return dev;
198215
}
@@ -205,7 +222,7 @@ private IGbsClientDevice CreateBaseStation(IMavlinkDevice device)
205222
TargetComponentId = device.ComponentId,
206223
SystemId = _systemId.Value,
207224
ComponentId = _componentId.Value,
208-
}, _sequenceCalculator, InternalGetConfig(_ => _.Gbs));
225+
}, _sequenceCalculator, InternalGetConfig(c => c.Gbs));
209226
}
210227

211228
private IAdsbClientDevice CreateAdsbDevice(IMavlinkDevice device)
@@ -216,7 +233,7 @@ private IAdsbClientDevice CreateAdsbDevice(IMavlinkDevice device)
216233
TargetComponentId = device.ComponentId,
217234
SystemId = _systemId.Value,
218235
ComponentId = _componentId.Value,
219-
}, _sequenceCalculator, InternalGetConfig(_ => _.Adsb));
236+
}, _sequenceCalculator, InternalGetConfig(c => c.Adsb));
220237
}
221238

222239
#region Logs
@@ -271,32 +288,39 @@ private string TryGetName(StatustextPacket pkt)
271288

272289
public IVehicleClient? GetVehicleByFullId(ushort id)
273290
{
274-
using var a = Vehicles.BindToObservableList(out var list).Subscribe();
275-
return list.Items.FirstOrDefault(_ => _.Heartbeat.FullId == id);
291+
using var autoDispose = Vehicles.BindToObservableList(out var list).Subscribe();
292+
return list.Items.FirstOrDefault(c => c.Heartbeat.FullId == id);
276293
}
277294

278295
public IObservable<IChangeSet<IGbsClientDevice, ushort>> BaseStations { get; }
279296

280297
public IGbsClientDevice? GetGbsByFullId(ushort id)
281298
{
282-
BaseStations.BindToObservableList(out var list).Subscribe();
283-
return list.Items.FirstOrDefault(_ => _.Heartbeat.FullId == id);
299+
using var autoDispose = BaseStations.BindToObservableList(out var list).Subscribe();
300+
return list.Items.FirstOrDefault(d => d.Heartbeat.FullId == id);
284301
}
285302

286303
public IObservable<IChangeSet<ISdrClientDevice, ushort>> Payloads { get; }
287304

288305
public ISdrClientDevice? GetPayloadsByFullId(ushort id)
289306
{
290-
using var a = Payloads.BindToObservableList(out var list).Subscribe();
291-
return list.Items.FirstOrDefault(_ => _.Heartbeat.FullId == id);
307+
using var autoDispose = Payloads.BindToObservableList(out var list).Subscribe();
308+
return list.Items.FirstOrDefault(d => d.Heartbeat.FullId == id);
292309
}
293310

294311
public IObservable<IChangeSet<IAdsbClientDevice, ushort>> AdsbDevices { get; }
295312

296313
public IAdsbClientDevice? GetAdsbVehicleByFullId(ushort id)
297314
{
298-
AdsbDevices.BindToObservableList(out var list).Subscribe();
299-
return list.Items.FirstOrDefault(_ => _.FullId == id);
315+
using var autoDispose =AdsbDevices.BindToObservableList(out var list).Subscribe();
316+
return list.Items.FirstOrDefault(d => d.FullId == id);
317+
}
318+
319+
public IObservable<IChangeSet<IRfsaClientDevice, ushort>> RfsaDevices { get; }
320+
public IAdsbClientDevice? GetRfsaByFullId(ushort id)
321+
{
322+
using var autoDispose =AdsbDevices.BindToObservableList(out var list).Subscribe();
323+
return list.Items.FirstOrDefault(d => d.FullId == id);
300324
}
301325

302326
private IVehicleClient? CreateVehicle(IMavlinkDevice device)
@@ -314,15 +338,15 @@ private string TryGetName(StatustextPacket pkt)
314338
TargetComponentId = device.ComponentId,
315339
SystemId = _systemId.Value,
316340
ComponentId = _componentId.Value,
317-
}, InternalGetConfig(_ => _.Vehicle), _sequenceCalculator, RxApp.TaskpoolScheduler);
341+
}, InternalGetConfig(c => c.Vehicle), _sequenceCalculator, RxApp.TaskpoolScheduler);
318342
case MavType.MavTypeFixedWing:
319343
return new ArduPlaneClient(Router, new MavlinkClientIdentity
320344
{
321345
TargetSystemId = device.SystemId,
322346
TargetComponentId = device.ComponentId,
323347
SystemId = _systemId.Value,
324348
ComponentId = _componentId.Value,
325-
}, InternalGetConfig(_ => _.Vehicle), _sequenceCalculator, RxApp.TaskpoolScheduler);
349+
}, InternalGetConfig(c => c.Vehicle), _sequenceCalculator, RxApp.TaskpoolScheduler);
326350
default:
327351
return null;
328352
}

src/Asv.Drones.Gui/Shell/Pages/Flight/Anchors/Uav/Actions/TakeOff/UavActionTakeOff.cs

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -33,7 +33,7 @@ public UavActionTakeOff(IVehicleClient vehicle, IMap map, ILogService log, IConf
3333
Command = cmd;
3434
}
3535

36-
private void OnCommandError(Exception ex)
36+
private void OnCommandError(Exception? ex)
3737
{
3838
_log.Error("Arm", string.Format(RS.TakeOffAnchorActionViewModel_OnCommandError, Vehicle.Name.Value),
3939
ex); // DONE: Localize

0 commit comments

Comments
 (0)