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 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 = _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,
}
};
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.Command == command)
// 21.04.2019 comment this filter, because work in progress https://mavlink.io/en/messages/common.html#COMMAND_ACK
// .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);
}
