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 async Task SetPositionTargetLocalNed(uint timeBootMs, MavFrame coordinateFrame, PositionTargetTypemask typeMask, float x,
float y, float z, float vx, float vy, float vz, float afx, float afy, float afz, float yaw, float yawRate,
CancellationToken cancel)
{
var packet = new SetPositionTargetLocalNedPacket
{
ComponenId = _config.ComponentId,
SystemId = _config.SystemId,
Payload =
{
TimeBootMs = timeBootMs,
TargetComponent = _config.TargetComponentId,
TargetSystem = _config.TargetSystemId,
CoordinateFrame = coordinateFrame,
TypeMask = typeMask,
X = x,
Y = y,
Z = z,
Vx = vx,
Vy = vy,
Vz = vz,
Afx = afx,
Afy = afy,
Afz = afz,
Yaw = yaw,
YawRate = yawRate,
}
};
await _connection.Send(packet, cancel).ConfigureAwait(false);
}
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 SetPositionTargetGlobalInt(uint timeBootMs, MavFrame coordinateFrame, int latInt, int lonInt, float alt,
float vx, float vy, float vz, float afx, float afy, float afz, float yaw,
float yawRate, PositionTargetTypemask typeMask, CancellationToken cancel)
{
Logger.Debug($"{LogSend} {nameof(SetPositionTargetGlobalInt)} ");
return(InternalSend <SetPositionTargetGlobalIntPacket>(_ =>
{
_.Payload.TimeBootMs = timeBootMs;
_.Payload.TargetSystem = Identity.TargetSystemId;
_.Payload.TargetComponent = Identity.TargetComponentId;
_.Payload.CoordinateFrame = coordinateFrame;
_.Payload.LatInt = latInt;
_.Payload.LonInt = lonInt;
_.Payload.Alt = alt;
_.Payload.Vx = vx;
_.Payload.Vy = vy;
_.Payload.Vz = vz;
_.Payload.Afx = afx;
_.Payload.Afy = afy;
_.Payload.Afz = afz;
_.Payload.Yaw = yaw;
_.Payload.YawRate = yawRate;
_.Payload.TypeMask = typeMask;
}, cancel));
}
public Task SetPositionTargetGlobalInt(uint timeBootMs, MavFrame coordinateFrame, int latInt, int lonInt, float alt,
float vx, float vy, float vz, float afx, float afy, float afz, float yaw,
float yawRate, PositionTargetTypemask typeMask, CancellationToken cancel)
{
var packet = new SetPositionTargetGlobalIntPacket
{
ComponenId = _config.ComponentId,
SystemId = _config.SystemId,
Payload =
{
TimeBootMs = timeBootMs,
TargetSystem = _config.TargetSystemId,
TargetComponent = _config.TargetComponentId,
CoordinateFrame = coordinateFrame,
LatInt = latInt,
LonInt = lonInt,
Alt = alt,
Vx = vx,
Vy = vy,
Vz = vz,
Afx = afx,
Afy = afy,
Afz = afz,
Yaw = yaw,
YawRate = yawRate,
TypeMask = typeMask,
}
};
return(_connection.Send(packet, cancel));
}
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);
}
private async Task GoTo(GeoPoint location, MavFrame frame, CancellationToken cancel, double?yawDeg = null, double?vx = null, double?vy = null, double?vz = null)
{
await EnsureInGuidedMode(cancel).ConfigureAwait(false);
var yaw = yawDeg.HasValue ? (float?)GeoMath.DegreesToRadians(yawDeg.Value) : null;
await Mavlink.Common.SetPositionTargetGlobalInt(0, frame, cancel, (int)(location.Latitude * 10000000), (int)(location.Longitude * 10000000), (float)location.Altitude, yaw : yaw).ConfigureAwait(false);
}
private async Task GoTo(GeoPoint location, MavFrame frame, CancellationToken cancel, double?yawDeg = null, double?vx = null, double?vy = null, double?vz = null)
{
await EnsureInGuidedMode(cancel);
if (location.Altitude.HasValue == false)
{
throw new MavlinkException(RS.VehicleArdupilot_GoTo_Altitude_of_end_point_is_null);
}
var yaw = yawDeg.HasValue ? (float?)GeoMath.DegreesToRadians(yawDeg.Value) : null;
await Mavlink.Common.SetPositionTargetGlobalInt(0, frame, cancel, (int)(location.Latitude * 10000000), (int)(location.Longitude * 10000000), (float)location.Altitude.Value, yaw : yaw);
}
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 Task SetPositionTargetLocalNed(uint timeBootMs, MavFrame coordinateFrame, PositionTargetTypemask typeMask, float x,
float y, float z, float vx, float vy, float vz, float afx, float afy, float afz, float yaw, float yawRate,
CancellationToken cancel)
{
return(InternalSend <SetPositionTargetLocalNedPacket>(_ =>
{
_.Payload.TimeBootMs = timeBootMs;
_.Payload.TargetComponent = Identity.TargetComponentId;
_.Payload.TargetSystem = Identity.TargetSystemId;
_.Payload.CoordinateFrame = coordinateFrame;
_.Payload.TypeMask = typeMask;
_.Payload.X = x;
_.Payload.Y = y;
_.Payload.Z = z;
_.Payload.Vx = vx;
_.Payload.Vy = vy;
_.Payload.Vz = vz;
_.Payload.Afx = afx;
_.Payload.Afy = afy;
_.Payload.Afz = afz;
_.Payload.Yaw = yaw;
_.Payload.YawRate = yawRate;
}, cancel));
}
public static Task SetPositionTargetLocalNed(this IMavlinkOffboardMode src, uint timeBootMs, MavFrame coordinateFrame, float?x,
float?y, float?z, float?vx, float?vy, float?vz, float?afx, float?afy, float?afz, float?yaw, float?yawRate,
CancellationToken cancel)
{
PositionTargetTypemask mask = 0;
if (!x.HasValue)
{
mask |= PositionTargetTypemask.PositionTargetTypemaskXIgnore;
}
if (!y.HasValue)
{
mask |= PositionTargetTypemask.PositionTargetTypemaskYIgnore;
}
if (!z.HasValue)
{
mask |= PositionTargetTypemask.PositionTargetTypemaskZIgnore;
}
if (!vx.HasValue)
{
mask |= PositionTargetTypemask.PositionTargetTypemaskVxIgnore;
}
if (!vy.HasValue)
{
mask |= PositionTargetTypemask.PositionTargetTypemaskVyIgnore;
}
if (!vz.HasValue)
{
mask |= PositionTargetTypemask.PositionTargetTypemaskVzIgnore;
}
if (!afx.HasValue)
{
mask |= PositionTargetTypemask.PositionTargetTypemaskAxIgnore;
}
if (!afy.HasValue)
{
mask |= PositionTargetTypemask.PositionTargetTypemaskVyIgnore;
}
if (!afz.HasValue)
{
mask |= PositionTargetTypemask.PositionTargetTypemaskVzIgnore;
}
if (!yaw.HasValue)
{
mask |= PositionTargetTypemask.PositionTargetTypemaskYawIgnore;
}
if (!yawRate.HasValue)
{
mask |= PositionTargetTypemask.PositionTargetTypemaskYawRateIgnore;
}
return(src.SetPositionTargetLocalNed(timeBootMs, coordinateFrame, mask, x ?? 0, y ?? 0, z ?? 0, vx ?? 0, vy ?? 0, vz ?? 0, afx ?? 0, afy ?? 0, afz ?? 0, yaw ?? 0, yawRate ?? 0, 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);
}
public static Task SetPositionTargetGlobalInt(this IMavlinkCommon src, uint timeBootMs, MavFrame coordFrame, CancellationToken cancel,
int?latInt = null,
int?lonInt = null,
float?alt = null,
float?vx = null,
float?vy = null,
float?vz = null,
float?afx = null,
float?afy = null,
float?afz = null,
float?yaw = null,
float?yawRate = null)
{
var mask = default(PositionTargetTypemask);
if (latInt.HasValue)
{
mask |= PositionTargetTypemask.PositionTargetTypemaskXIgnore;
}
if (lonInt.HasValue)
{
mask |= PositionTargetTypemask.PositionTargetTypemaskYIgnore;
}
if (alt.HasValue)
{
mask |= PositionTargetTypemask.PositionTargetTypemaskZIgnore;
}
if (vx.HasValue)
{
mask |= PositionTargetTypemask.PositionTargetTypemaskVxIgnore;
}
if (vy.HasValue)
{
mask |= PositionTargetTypemask.PositionTargetTypemaskVyIgnore;
}
if (vz.HasValue)
{
mask |= PositionTargetTypemask.PositionTargetTypemaskVzIgnore;
}
if (afx.HasValue)
{
mask |= PositionTargetTypemask.PositionTargetTypemaskAxIgnore;
}
if (afy.HasValue)
{
mask |= PositionTargetTypemask.PositionTargetTypemaskAyIgnore;
}
if (afz.HasValue)
{
mask |= PositionTargetTypemask.PositionTargetTypemaskAzIgnore;
}
if (yaw.HasValue)
{
mask |= PositionTargetTypemask.PositionTargetTypemaskYawIgnore;
}
if (yawRate.HasValue)
{
mask |= PositionTargetTypemask.PositionTargetTypemaskYawRateIgnore;
}
mask = ~mask;
return(src.SetPositionTargetGlobalInt(timeBootMs, coordFrame, latInt ?? 0, lonInt ?? 0, alt ?? 0, vx ?? 0,
vy ?? 0, vz ?? 0, afx ?? 0, afy ?? 0, afz ?? 0, yaw ?? 0, yawRate ?? 0, mask, cancel));
}
