public Task SendDebugFloatArray(string name, ushort arrayId, float[] data)
{
if (string.IsNullOrWhiteSpace(name))
throw new ArgumentException("Value cannot be null or whitespace.", nameof(name));
if (name.Length > _maxDebugFloatArrayNameLength)
{
throw new ArgumentException($"Name '{name}' is too long for parameter name (max size {_maxDebugFloatArrayNameLength})", nameof(name));
}
if (data.Length > _maxDebugFloatArrayDataLength)
{
throw new ArgumentException($"Data is too long (max size {_maxDebugFloatArrayDataLength})", nameof(name));
}
var packet = new DebugFloatArrayPacket()
{
ComponenId = _identity.ComponentId,
SystemId = _identity.SystemId,
CompatFlags = 0,
IncompatFlags = 0,
Sequence = _seq.GetNextSequenceNumber(),
Payload =
{
Name = name.ToCharArray(),
TimeUsec = (uint)(DateTime.Now - _bootTime).TotalMilliseconds,
ArrayId = arrayId,
}
};
data.CopyTo(packet.Payload.Data,0);
return _connection.Send(packet, _disposableCancel.Token);
}
public Task SendCommandInt(MavCmd command, MavFrame frame, bool current, bool autocontinue,
float param1, float param2,
float param3, float param4, int x, int y, float z, int attemptCount, CancellationToken cancel)
{
var packet = new CommandIntPacket()
{
ComponenId = _identity.ComponentId,
SystemId = _identity.SystemId,
Sequence = _seq.GetNextSequenceNumber(),
Payload =
{
Command = command,
TargetComponent = _identity.TargetComponentId,
TargetSystem = _identity.TargetSystemId,
Frame = frame,
Param1 = param1,
Param2 = param2,
Param3 = param3,
Param4 = param4,
Current = (byte)(current ? 1:0),
Autocontinue = (byte)(autocontinue ? 1:0),
X = x,
Y = y,
Z = z,
}
};
return(_connection.Send(packet, cancel));
}
protected Task InternalSend <TPacketSend>(Action <TPacketSend> fillPacket, CancellationToken cancel = default)
where TPacketSend : IPacketV2 <IPayload>, new()
{
var packet = GeneratePacket <TPacketSend>();
fillPacket(packet);
Logger.Trace($"{LogSend} send {packet.Name}");
return(_connection.Send(packet, cancel));
}
public async Task ReadAllParams(CancellationToken cancel, IProgress <double> progress = null)
{
progress = progress ?? new Progress <double>();
var packet = new ParamRequestListPacket
{
ComponenId = _config.ComponentId,
SystemId = _config.SystemId,
Payload =
{
TargetComponent = _config.TargetComponenId,
TargetSystem = _config.TargetSystemId,
}
};
var samplesBySecond = _connection
.Where(FilterVehicle)
.Where(_ => _.MessageId == ParamValuePacket.PacketMessageId)
.Cast <ParamValuePacket>().Buffer(TimeSpan.FromSeconds(1)).Next();
await _connection.Send(packet, cancel).ConfigureAwait(false);
var timeout = DateTime.Now + TimeSpan.FromMilliseconds(_config.TimeoutToReadAllParamsMs);
int?totalCnt = null;
progress.Report(0);
var paramsNames = new HashSet <string>();
foreach (var paramsPart in samplesBySecond)
{
if (DateTime.Now >= timeout)
{
throw new TimeoutException(string.Format(RS.Vehicle_ReadAllParams_Timeout_to_read_all_params_from_Vehicle, _config.TimeoutToReadAllParamsMs));
}
foreach (var p in paramsPart)
{
totalCnt = totalCnt ?? p.Payload.ParamCount;
var name = GetParamName(p.Payload);
paramsNames.Add(name);
}
if (totalCnt.HasValue && totalCnt.Value <= paramsNames.Count)
{
break;
}
progress.Report(totalCnt == null ? 0 : Math.Min(1d, paramsNames.Count / (double)totalCnt));
}
progress.Report(1);
}
public Task SetMode(uint baseMode, uint customMode, CancellationToken cancel)
{
var packet = new SetModePacket()
{
ComponenId = _config.ComponentId,
SystemId = _config.SystemId,
Payload =
{
TargetSystem = _config.TargetSystemId,
BaseMode = (MavMode)baseMode,
CustomMode = customMode,
}
};
return(_connection.Send(packet, cancel));
}
public Task RequestAllParams(CancellationToken cancel)
{
_logger.Info($"Request all params from vehicle[{_identity}]");
var packet = new ParamRequestListPacket
{
ComponenId = _identity.ComponentId,
SystemId = _identity.SystemId,
Payload =
{
TargetComponent = _identity.TargetComponentId,
TargetSystem = _identity.TargetSystemId,
}
};
return(_connection.Send(packet, cancel));
}
public async Task SetPositionTargetLocalNed(uint timeBootMs, MavFrame coordinateFrame, PositionTargetTypemask typeMask, float x,
float y, float z, float vx, float vy, float vz, float afx, float afy, float afz, float yaw, float yawRate,
CancellationToken cancel)
{
var packet = new SetPositionTargetLocalNedPacket
{
ComponenId = _config.ComponentId,
SystemId = _config.SystemId,
Payload =
{
TimeBootMs = timeBootMs,
TargetComponent = _config.TargetComponentId,
TargetSystem = _config.TargetSystemId,
CoordinateFrame = coordinateFrame,
TypeMask = typeMask,
X = x,
Y = y,
Z = z,
Vx = vx,
Vy = vy,
Vz = vz,
Afx = afx,
Afy = afy,
Afz = afz,
Yaw = yaw,
YawRate = yawRate,
}
};
await _connection.Send(packet, cancel).ConfigureAwait(false);
}
/// <summary>
/// Subscribe to connection packet pipe fore waiting answer packet and then send request
/// </summary>
/// <typeparam name="TAnswerPacket"></typeparam>
/// <typeparam name="TAnswerPayload"></typeparam>
/// <param name="src"></param>
/// <param name="packet"></param>
/// <param name="targetSystem"></param>
/// <param name="targetComponent"></param>
/// <param name="cancel"></param>
/// <param name="filter"></param>
/// <returns></returns>
public static async Task <TAnswerPacket> SendAndWaitAnswer <TAnswerPacket, TAnswerPayload>(this IMavlinkV2Connection src, IPacketV2 <IPayload> packet, int targetSystem, int targetComponent, CancellationToken cancel, Func <TAnswerPacket, bool> filter = null)
where TAnswerPacket : IPacketV2 <TAnswerPayload>, new() where TAnswerPayload : IPayload
{
var p = new TAnswerPacket();
var eve = new AsyncAutoResetEvent(false);
IDisposable subscribe = null;
filter = filter ?? (_ => true);
var result = default(TAnswerPacket);
try
{
subscribe = src.Where(_ => _.ComponenId == targetComponent && _.SystemId == targetSystem && _.MessageId == p.MessageId)
.Cast <TAnswerPacket>()
.FirstAsync(filter)
.Subscribe(_ =>
{
result = _;
eve.Set();
});
await src.Send(packet, cancel).ConfigureAwait(false);
await eve.WaitAsync(cancel);
}
finally
{
subscribe?.Dispose();
}
return(result);
}
private void SafeSendCommandAck(MavCmd cmd, MavResult result, byte systemId, byte componenId, int resultParam2 = 0)
{
try
{
_connection.Send(new CommandAckPacket
{
ComponenId = _identity.ComponentId,
SystemId = _identity.SystemId,
Sequence = _seq.GetNextSequenceNumber(),
CompatFlags = 0,
IncompatFlags = 0,
Payload =
{
Command = cmd,
Result = result,
ResultParam2 = resultParam2,
TargetSystem = systemId,
TargetComponent = componenId
}
}, _disposeCancel.Token);
}
catch (Exception e)
{
_logger.Error(string.Format("Error to send CommandAckPacket. Command: {0:G}. Result: {1:G}. TargetSystemId: {2}. TargetComponentId: {3}. {4}", cmd, result, systemId, componenId, e.Message));
}
}
private async void OnTick(long l)
{
if (Interlocked.CompareExchange(ref _isSending, 1, 0) == 1)
{
LogSkipped();
return;
}
IDisposable dispose = null;
try
{
dispose = await _dataLock.ReaderLockAsync();
((IPacketV2 <IPayload>)_packet).Sequence = _seq.GetNextSequenceNumber();
await _connection.Send((IPacketV2 <IPayload>) _packet, _disposeCancellation.Token);
LogSuccess();
}
catch (Exception e)
{
LogError(e);
}
finally
{
dispose?.Dispose();
Interlocked.Exchange(ref _isSending, 0);
}
}
public Task MissionItem(MavFrame frame, MavCmd cmd, bool current, bool autoContinue, float param1, float param2, float param3,
float param4, float x, float y, float z, MavMissionType missionType, int attemptCount, CancellationToken cancel)
{
var packet = new MissionItemPacket()
{
ComponenId = _config.ComponentId,
SystemId = _config.SystemId,
Payload =
{
TargetComponent = _config.TargetComponenId,
TargetSystem = _config.TargetSystemId,
Seq = 0,
Frame = frame,
Command = cmd,
Current = 2,
Autocontinue = (byte)(autoContinue? 1:0),
Param1 = param1,
Param2 = param2,
Param3 = param3,
Param4 = param4,
X = x,
Y = y,
Z = 20,
MissionType = missionType
}
};
_mavlink.Send(packet, cancel);
return(Task.CompletedTask);
}
public Task SendLoggingData(byte targetSystemId, byte targetComponentId, ushort seq, byte firstMessageOffset, byte[] data)
{
if (data.Length > _maxDataLength)
{
throw new ArgumentException($"Data is too long (max size {_maxDataLength})", nameof(data));
}
var packet = new LoggingDataPacket
{
ComponenId = _identity.ComponentId,
SystemId = _identity.SystemId,
CompatFlags = 0,
IncompatFlags = 0,
Sequence = _seq.GetNextSequenceNumber(),
Payload =
{
TargetComponent = targetComponentId,
TargetSystem = targetSystemId,
FirstMessageOffset = firstMessageOffset,
Length = (byte)data.Length,
Sequence = seq,
Data = data
}
};
//data.CopyTo(packet.Payload.Data, 0);
return(_connection.Send(packet, _disposableCancel.Token));
}
protected Task Send <TPacket, TPayload>(Action <TPayload> fillPayload)
where TPacket : IPacketV2 <TPayload>, new() where TPayload : IPayload
{
var packet = new TPacket
{
ComponenId = _identity.ComponentId,
SystemId = _identity.SystemId,
CompatFlags = 0,
IncompatFlags = 0,
Sequence = _seq.GetNextSequenceNumber(),
};
fillPayload(packet.Payload);
return(_connection.Send((IPacketV2 <IPayload>)packet, DisposeCancel));
}
/// <summary>
/// Subscribe to connection packet pipe fore waiting answer packet and then send request
/// </summary>
/// <typeparam name="TAnswerPacket"></typeparam>
/// <typeparam name="TAnswerPayload"></typeparam>
/// <param name="src"></param>
/// <param name="packet"></param>
/// <param name="targetSystem"></param>
/// <param name="targetComponent"></param>
/// <param name="cancel"></param>
/// <param name="filter"></param>
/// <returns></returns>
public static async Task <TAnswerPacket> SendAndWaitAnswer <TAnswerPacket, TAnswerPayload>(this IMavlinkV2Connection src, IPacketV2 <IPayload> packet, int targetSystem, int targetComponent, CancellationToken cancel, Func <TAnswerPacket, bool> filter = null)
where TAnswerPacket : IPacketV2 <TAnswerPayload>, new() where TAnswerPayload : IPayload
{
var p = new TAnswerPacket();
var tcs = new TaskCompletionSource <TAnswerPacket>();
using var c1 = cancel.Register(() => tcs.TrySetCanceled());
filter = filter ?? (_ => true);
using var subscribe = src.Where(_ => _.ComponenId == targetComponent && _.SystemId == targetSystem && _.MessageId == p.MessageId)
.Cast <TAnswerPacket>()
.FirstAsync(filter)
.Subscribe(_ => tcs.TrySetResult(_));
await src.Send(packet, cancel).ConfigureAwait(false);
return(await tcs.Task.ConfigureAwait(false));
}
public async Task SendRtcmData(byte[] data, int length, CancellationToken cancel)
{
if (length > MaxMessageLength * 4)
{
_logger.Error($"RTCM message for DGPS is too large '{length}'");
}
// number of packets we need, including a termination packet if needed
var pktCount = length / MaxMessageLength + 1;
if (pktCount >= 4)
{
pktCount = 4;
}
using (var linked = CancellationTokenSource.CreateLinkedTokenSource(_disposeCancel.Token, cancel))
{
for (var i = 0; i < pktCount; i++)
{
var pkt = new GpsRtcmDataPacket()
{
ComponenId = _identity.ComponentId,
SystemId = _identity.SystemId,
CompatFlags = 0,
IncompatFlags = 0,
Sequence = _seq.GetNextSequenceNumber(),
};
// 1 means message is fragmented
pkt.Payload.Flags = (byte)(pktCount > 1 ? 1 : 0);
// next 2 bits are the fragment ID
pkt.Payload.Flags += (byte)((i & 0x3) << 1);
// the remaining 5 bits are used for the sequence ID
pkt.Payload.Flags += (byte)((Interlocked.Increment(ref _seqNumber) & 0x1f) << 3);
var dataLength = Math.Min(length - i * MaxMessageLength, MaxMessageLength);
var dataArray = new byte[dataLength];
Array.Copy(data, i * MaxMessageLength, dataArray, 0, dataLength);
pkt.Payload.Data = dataArray;
pkt.Payload.Len = (byte)dataLength;
await _connection.Send(pkt, linked.Token);
}
}
}
public async Task SendData(byte targetSystemId, byte targetComponentId, byte targetNetworkId, ushort messageType, byte[] data, CancellationToken cancel)
{
using var linked = CancellationTokenSource.CreateLinkedTokenSource(_disposeCancel.Token, cancel);
await _connection.Send(new V2ExtensionPacket
{
ComponenId = _identity.ComponentId,
SystemId = _identity.SystemId,
CompatFlags = 0,
IncompatFlags = 0,
Sequence = _seq.GetNextSequenceNumber(),
Payload =
{
MessageType = messageType,
Payload = data,
TargetComponent = targetComponentId,
TargetSystem = targetSystemId,
TargetNetwork = targetNetworkId,
}
}, linked.Token).ConfigureAwait(false);
}
public Task SendListDirectory(string path, ushort index, CancellationToken cancel)
{
var packet = new FileTransferProtocolPacket
{
ComponenId = _identity.ComponentId,
SystemId = _identity.SystemId,
Sequence = _seq.GetNextSequenceNumber(),
Payload =
{
TargetComponent = _identity.TargetComponentId,
TargetSystem = _identity.TargetSystemId,
TargetNetwork = 0,
}
};
var ftp = new FtpMessagePayload
{
Size = (byte)path.Length,
Offset = index,
Data = Encoding.ASCII.GetBytes(path)
};
return(_connection.Send(packet, cancel));
}
private async void TrySend(long l)
{
if (Interlocked.CompareExchange(ref _isSending, 1, 0) == 1)
{
return;
}
try
{
KeyValuePair <MavSeverity, string> res;
if (_messageQueue.TryDequeue(out res))
{
await _connection.Send(new StatustextPacket
{
ComponenId = _identity.ComponenId,
SystemId = _identity.SystemId,
CompatFlags = 0,
IncompatFlags = 0,
Sequence = _seq.GetNextSequenceNumber(),
Payload =
{
Severity = res.Key,
Text = res.Value.ToCharArray()
}
}, _disposeCancel.Token);
}
}
catch (Exception e)
{
_logger.Error(e, $"Error occured to send packet: {e.Message}");
}
finally
{
Interlocked.Exchange(ref _isSending, 0);
}
}
public async Task <int> MissionRequestCount(int attemptCount, CancellationToken cancel)
{
_logger.Trace($"[MISSION]=> Begin request items count with {attemptCount} attempts");
var packet = new MissionRequestListPacket()
{
ComponenId = _identity.ComponentId,
SystemId = _identity.SystemId,
Sequence = _seq.GetNextSequenceNumber(),
Payload =
{
TargetComponent = _identity.TargetComponentId,
TargetSystem = _identity.TargetSystemId,
}
};
byte currentAttept = 0;
MissionCountPacket result = null;
while (currentAttept < attemptCount)
{
++currentAttept;
using (var timeoutCancel = new CancellationTokenSource(_config.CommandTimeoutMs))
using (var linkedCancel = CancellationTokenSource.CreateLinkedTokenSource(cancel, timeoutCancel.Token))
{
IDisposable subscribe = null;
try
{
var eve = new AsyncAutoResetEvent(false);
subscribe = _mavlink.Where(FilterVehicle).Where(_ => _.MessageId == MissionCountPacket.PacketMessageId)
.Cast <MissionCountPacket>()
.FirstAsync()
.Subscribe(_ =>
{
result = _;
eve.Set();
});
await _mavlink.Send(packet, linkedCancel.Token).ConfigureAwait(false);
await eve.WaitAsync(linkedCancel.Token);
break;
}
catch (TaskCanceledException e)
{
_logger.Warn(e, $"[MISSION]=> Request {currentAttept} of {attemptCount} items count error:{e.Message}");
if (!timeoutCancel.IsCancellationRequested)
{
throw;
}
}
finally
{
subscribe?.Dispose();
}
}
}
if (result == null)
{
throw new TimeoutException(string.Format("Timeout to request mission items with '{0}' attempts (timeout {0} times by {1:g} )", currentAttept, TimeSpan.FromMilliseconds(_config.CommandTimeoutMs)));
}
_logger.Info($"[MISSION]<== Mission item count: {result.Payload.Count} items");
return(result.Payload.Count);
}
public async Task <CommandAckPayload> CommandInt(MavCmd command, MavFrame frame, bool current, bool autocontinue, float param1, float param2,
float param3, float param4, int x, int y, float z, int attemptCount, CancellationToken cancel)
{
var packet = new CommandIntPacket()
{
ComponenId = _config.ComponentId,
SystemId = _config.SystemId,
Payload =
{
Command = command,
TargetComponent = _config.TargetComponenId,
TargetSystem = _config.TargetSystemId,
Frame = frame,
Param1 = param1,
Param2 = param2,
Param3 = param3,
Param4 = param4,
Current = (byte)(current ? 1:0),
Autocontinue = (byte)(autocontinue ? 1:0),
X = x,
Y = y,
Z = z,
}
};
byte currentAttept = 0;
CommandAckPacket result = null;
while (currentAttept < attemptCount)
{
++currentAttept;
using (var timeoutCancel = new CancellationTokenSource(_config.CommandTimeoutMs))
using (var linkedCancel = CancellationTokenSource.CreateLinkedTokenSource(cancel, timeoutCancel.Token))
{
IDisposable subscribe = null;
try
{
var eve = new AsyncAutoResetEvent(false);
subscribe = _connection.Where(FilterVehicle).Where(_ => _.MessageId == CommandAckPacket.PacketMessageId)
.Cast <CommandAckPacket>()
.FirstAsync(_ => _.Payload.TargetComponent == _config.ComponentId &&
_.Payload.TargetSystem == _config.SystemId).Subscribe(_ =>
{
result = _;
eve.Set();
});
await _connection.Send(packet, linkedCancel.Token).ConfigureAwait(false);
await eve.WaitAsync(linkedCancel.Token);
break;
}
catch (TaskCanceledException)
{
if (!timeoutCancel.IsCancellationRequested)
{
throw;
}
}
finally
{
subscribe?.Dispose();
}
}
}
if (result == null)
{
throw new TimeoutException(string.Format("Timeout to execute command '{0:G}' with '{1}' attempts (timeout {1} times by {2:g} )", command, currentAttept, TimeSpan.FromMilliseconds(_config.CommandTimeoutMs)));
}
return(result.Payload);
}
