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 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));
}
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);
}
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));
}
}
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 TPacket GeneratePacket <TPacket>()
where TPacket : IPacketV2 <IPayload>, new()
{
return(new TPacket
{
ComponenId = _identity.ComponentId,
SystemId = _identity.SystemId,
Sequence = _seq.GetNextSequenceNumber(),
});
}
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));
}
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);
}
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);
}
