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 WriteMissionItem(ushort seq, 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)
{
Logger.Info($"{LogSend} Write mission item");
// Ardupilot has custom implementation see => https://mavlink.io/en/services/mission.html#flight-plan-missions
return(InternalSend <MissionItemPacket>(_ =>
{
_.Payload.TargetComponent = Identity.TargetComponentId;
_.Payload.TargetSystem = Identity.TargetSystemId;
_.Payload.Seq = seq;
_.Payload.Frame = frame;
_.Payload.Command = cmd;
_.Payload.Current = (byte)(current ? 1 : 0);
_.Payload.Autocontinue = (byte)(autoContinue ? 1 : 0);
_.Payload.Param1 = param1;
_.Payload.Param2 = param2;
_.Payload.Param3 = param3;
_.Payload.Param4 = param4;
_.Payload.X = x;
_.Payload.Y = y;
_.Payload.Z = z;
_.Payload.MissionType = missionType;
}, cancel));
}
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 WriteMissionItem(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 = _identity.ComponentId,
SystemId = _identity.SystemId,
Sequence = _seq.GetNextSequenceNumber(),
Payload =
{
TargetComponent = _identity.TargetComponentId,
TargetSystem = _identity.TargetSystemId,
Seq = 0,
Frame = frame,
Command = cmd,
Current = (byte)(current ? 2:0),
Autocontinue = (byte)(autoContinue? 1:0),
Param1 = param1,
Param2 = param2,
Param3 = param3,
Param4 = param4,
X = x,
Y = y,
Z = z,
MissionType = missionType
}
};
return(_mavlink.Send(packet, cancel));
}
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));
}
public async Task SendCommandLong(MavCmd command, float param1, float param2, float param3,
float param4, float param5, float param6, float param7, CancellationToken cancel)
{
await InternalSend <CommandLongPacket>((packet) =>
{
packet.Payload.Command = command;
packet.Payload.TargetComponent = Identity.TargetComponentId;
packet.Payload.TargetSystem = Identity.TargetSystemId;
packet.Payload.Confirmation = 0;
packet.Payload.Param1 = param1;
packet.Payload.Param2 = param2;
packet.Payload.Param3 = param3;
packet.Payload.Param4 = param4;
packet.Payload.Param5 = param5;
packet.Payload.Param6 = param6;
packet.Payload.Param7 = param7;
}, cancel).ConfigureAwait(false);
await InternalSend <CommandLongPacket>((packet) =>
{
packet.Payload.Command = command;
packet.Payload.TargetComponent = Identity.TargetComponentId;
packet.Payload.TargetSystem = Identity.TargetSystemId;
packet.Payload.Confirmation = 1;
packet.Payload.Param1 = param1;
packet.Payload.Param2 = param2;
packet.Payload.Param3 = param3;
packet.Payload.Param4 = param4;
packet.Payload.Param5 = param5;
packet.Payload.Param6 = param6;
packet.Payload.Param7 = param7;
}, cancel).ConfigureAwait(false);
}
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 <CommandAckPayload> CommandLong(MavCmd command, float param1, float param2, float param3, float param4, float param5, float param6, float param7, int attemptCount, CancellationToken cancel)
{
return(InternalCall <CommandAckPayload, CommandLongPacket, CommandAckPacket>((packet) =>
{
packet.Payload.Command = command;
packet.Payload.TargetComponent = Identity.TargetComponentId;
packet.Payload.TargetSystem = Identity.TargetSystemId;
packet.Payload.Confirmation = 0;
packet.Payload.Param1 = param1;
packet.Payload.Param2 = param2;
packet.Payload.Param3 = param3;
packet.Payload.Param4 = param4;
packet.Payload.Param5 = param5;
packet.Payload.Param6 = param6;
packet.Payload.Param7 = param7;
}, _ => _.Payload.Command == command, _ => _.Payload, attemptCount, (_, att) => _.Payload.Confirmation = (byte)att, _config.CommandTimeoutMs, cancel));
}
public 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)
{
return(InternalCall <CommandAckPayload, CommandIntPacket, CommandAckPacket>((packet) =>
{
packet.Payload.Command = command;
packet.Payload.TargetComponent = Identity.TargetComponentId;
packet.Payload.TargetSystem = Identity.TargetSystemId;
packet.Payload.Frame = frame;
packet.Payload.Param1 = param1;
packet.Payload.Param2 = param2;
packet.Payload.Param3 = param3;
packet.Payload.Param4 = param4;
packet.Payload.Current = (byte)(current ? 1 : 0);
packet.Payload.Autocontinue = (byte)(autocontinue ? 1 : 0);
packet.Payload.X = x;
packet.Payload.Y = y;
packet.Payload.Z = z;
}, _ => _.Payload.Command == command, _ => _.Payload, attemptCount, null, _config.CommandTimeoutMs, cancel));
}
public CommandLongDelegate this[MavCmd cmd]
{
set { _registry.AddOrUpdate(cmd, value, (mavCmd, del) => value); }
}
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);
}
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 = _config.ComponentId,
SystemId = _config.SystemId,
Payload =
{
Command = command,
TargetComponent = _config.TargetComponenId,
TargetSystem = _config.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.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);
}
