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); }