public Task SendCommandLong(MavCmd command, float param1, float param2, float param3,
float param4, float param5, float param6, float param7, int attemptCount, CancellationToken cancel)
{
var packet = new CommandLongPacket
{
ComponenId = _identity.ComponentId,
SystemId = _identity.SystemId,
Sequence = _seq.GetNextSequenceNumber(),
Payload =
{
Command = command,
TargetComponent = _identity.TargetComponentId,
TargetSystem = _identity.TargetSystemId,
Confirmation = 0,
Param1 = param1,
Param2 = param2,
Param3 = param3,
Param4 = param4,
Param5 = param5,
Param6 = param6,
Param7 = param7,
}
};
return(_connection.Send(packet, cancel));
}
private async void OnRequest(CommandLongPacket obj)
{
// wait until prev been executed
if (Interlocked.CompareExchange(ref _isBusy, 1, 0) == 1 && obj.Payload.Confirmation == 0)
{
_logger.Warn($"Reject command {obj.Payload.Command}(Param1:{obj.Payload.Param1},Param2:{obj.Payload.Param2},Param3:{obj.Payload.Param3},Param4:{obj.Payload.Param4},Param5:{obj.Payload.Param5},Param6:{obj.Payload.Param6},Param7:{obj.Payload.Param7}): too busy now");
SafeSendCommandAck(obj.Payload.Command, MavResult.MavResultTemporarilyRejected, obj.SystemId, obj.ComponenId);
return;
}
CommandLongDelegate callback;
if (_registry.TryGetValue(obj.Payload.Command, out callback) == false)
{
_logger.Warn($"Reject unknown command {obj.Payload.Command}(Param1:{obj.Payload.Param1},Param2:{obj.Payload.Param2},Param3:{obj.Payload.Param3},Param4:{obj.Payload.Param4},Param5:{obj.Payload.Param5},Param6:{obj.Payload.Param6},Param7:{obj.Payload.Param7})");
SafeSendCommandAck(obj.Payload.Command, MavResult.MavResultUnsupported, obj.SystemId, obj.ComponenId);
return;
}
try
{
_logger.Info($"Command {obj.Payload.Command}(Param1:{obj.Payload.Param1},Param2:{obj.Payload.Param2},Param3:{obj.Payload.Param3},Param4:{obj.Payload.Param4},Param5:{obj.Payload.Param5},Param6:{obj.Payload.Param6},Param7:{obj.Payload.Param7})");
var args = new CommandArgs
{
Subject = new DeviceIdentity
{
ComponentId = obj.ComponenId,
SystemId = obj.SystemId,
},
Param1 = obj.Payload.Param1,
Param2 = obj.Payload.Param2,
Param3 = obj.Payload.Param3,
Param4 = obj.Payload.Param4,
Param5 = obj.Payload.Param5,
Param6 = obj.Payload.Param6,
Param7 = obj.Payload.Param7,
};
var result = await callback(args, _disposeCancel.Token).ConfigureAwait(false);
SafeSendCommandAck(obj.Payload.Command, result.ResultCode, obj.SystemId, obj.ComponenId, result.ResultValue);
}
catch (Exception e)
{
_logger.Error(
$"Error to execute command {obj.Payload.Command}(Param1:{obj.Payload.Param1},Param2:{obj.Payload.Param2},Param3:{obj.Payload.Param3},Param4:{obj.Payload.Param4},Param5:{obj.Payload.Param5},Param6:{obj.Payload.Param6},Param7:{obj.Payload.Param7}):{e.Message}");
SafeSendCommandAck(obj.Payload.Command, MavResult.MavResultFailed, obj.SystemId, obj.ComponenId);
}
finally
{
Interlocked.Exchange(ref _isBusy, 0);
}
}
public async Task <AutopilotVersionPayload> GetAutopilotVersion(CancellationToken cancel)
{
var packet = new CommandLongPacket
{
ComponenId = _identity.ComponentId,
SystemId = _identity.SystemId,
Sequence = _seq.GetNextSequenceNumber(),
Payload =
{
Command = MavCmd.MavCmdRequestAutopilotCapabilities,
TargetComponent = _identity.TargetComponentId,
TargetSystem = _identity.TargetSystemId,
Confirmation = 0,
Param1 = 1,
}
};
var result = await _connection.SendAndWaitAnswer <AutopilotVersionPacket, AutopilotVersionPayload>(packet, _identity.TargetSystemId, _identity.TargetComponentId, cancel);
return(result.Payload);
}
public async Task <CommandAckPayload> CommandLong(MavCmd command, float param1, float param2, float param3, float param4, float param5, float param6, float param7, int attemptCount, CancellationToken cancel)
{
var packet = new CommandLongPacket
{
ComponenId = _identity.ComponentId,
SystemId = _identity.SystemId,
Sequence = _seq.GetNextSequenceNumber(),
Payload =
{
Command = command,
TargetComponent = _identity.TargetComponentId,
TargetSystem = _identity.TargetSystemId,
Confirmation = 0,
Param1 = param1,
Param2 = param2,
Param3 = param3,
Param4 = param4,
Param5 = param5,
Param6 = param6,
Param7 = param7,
}
};
byte currentAttept = 0;
CommandAckPacket result = null;
while (currentAttept < attemptCount)
{
packet.Payload.Confirmation = currentAttept;
++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);
}
