diff --git a/PlaneGcsSdk.Contract_Shared/Communication/IConnection.cs b/PlaneGcsSdk.Contract_Shared/Communication/IConnection.cs index 0b66a78..b8c38f2 100644 --- a/PlaneGcsSdk.Contract_Shared/Communication/IConnection.cs +++ b/PlaneGcsSdk.Contract_Shared/Communication/IConnection.cs @@ -13,6 +13,9 @@ namespace Plane.Communication /// event EventHandler ExceptionThrown; + + int BytesToRead(); + /// /// 获取一个值，指示通信是否已开启。 /// diff --git a/PlaneGcsSdk.Contract_Shared/Copters/GpsFixType.cs b/PlaneGcsSdk.Contract_Shared/Copters/GpsFixType.cs index 364c16c..77bd3aa 100644 --- a/PlaneGcsSdk.Contract_Shared/Copters/GpsFixType.cs +++ b/PlaneGcsSdk.Contract_Shared/Copters/GpsFixType.cs @@ -4,7 +4,11 @@ { NoFix = 0, Fix2D = 2, - Fix3D = 3 + Fix3D = 3, + DGPS3D=4, + RTK3D= 5 + + } public static class ByteExtensions diff --git a/PlaneGcsSdk.Contract_Shared/Copters/ICopterActionsSharedByCopterManager.cs b/PlaneGcsSdk.Contract_Shared/Copters/ICopterActionsSharedByCopterManager.cs index d4b774e..2f560aa 100644 --- a/PlaneGcsSdk.Contract_Shared/Copters/ICopterActionsSharedByCopterManager.cs +++ b/PlaneGcsSdk.Contract_Shared/Copters/ICopterActionsSharedByCopterManager.cs @@ -126,5 +126,10 @@ namespace Plane.Copters /// /// 表示此命令异步发送操作的 实例。 Task UnlockAsync(); + + Task InjectGpsDataAsync(byte[] data, ushort length); + + + } } diff --git a/PlaneGcsSdk_Shared/Communication/CompositeConnection.cs b/PlaneGcsSdk_Shared/Communication/CompositeConnection.cs index 09363bb..a95c722 100644 --- a/PlaneGcsSdk_Shared/Communication/CompositeConnection.cs +++ b/PlaneGcsSdk_Shared/Communication/CompositeConnection.cs @@ -21,6 +21,33 @@ namespace Plane.Communication public event EventHandler ExceptionThrown; + public int BytesToRead() + { + int result; + for (int i = 0; i < _candidateConnections.Count; i++) + { + if (_candidateConnections[i].IsOpen) + { + try + { + result = _candidateConnections[i].BytesToRead(); + if (result != 0) + { + + return result; + } + } + catch (Exception ex) + { + Debug.WriteLine(ex); + ExceptionThrown?.Invoke(this, new ExceptionThrownEventArgs(ex)); + } + } + } + return 0; + } + + public bool IsOpen { get diff --git a/PlaneGcsSdk_Shared/Communication/EmptyConnection.cs b/PlaneGcsSdk_Shared/Communication/EmptyConnection.cs index 632969b..3e0df92 100644 --- a/PlaneGcsSdk_Shared/Communication/EmptyConnection.cs +++ b/PlaneGcsSdk_Shared/Communication/EmptyConnection.cs @@ -20,6 +20,11 @@ namespace Plane.Communication IsOpen = false; } + public int BytesToRead() + { + return 0; + } + public Task OpenAsync() { IsOpen = true; diff --git a/PlaneGcsSdk_Shared/Communication/SerialPortConnection.cs b/PlaneGcsSdk_Shared/Communication/SerialPortConnection.cs index 43494a9..27dd485 100644 --- a/PlaneGcsSdk_Shared/Communication/SerialPortConnection.cs +++ b/PlaneGcsSdk_Shared/Communication/SerialPortConnection.cs @@ -55,6 +55,7 @@ namespace Plane.Communication { _port.Open(); _internalIsOpen = true; + } public Task OpenAsync() @@ -63,6 +64,13 @@ namespace Plane.Communication return TaskUtils.CompletedTask; } + + public int BytesToRead() + { + return _port.BytesToRead; + } + + public int Read(byte[] buffer, int offset, int count) { while (Available < count) diff --git a/PlaneGcsSdk_Shared/Communication/TcpConnectionBase.cs b/PlaneGcsSdk_Shared/Communication/TcpConnectionBase.cs index b64e23b..208443a 100644 --- a/PlaneGcsSdk_Shared/Communication/TcpConnectionBase.cs +++ b/PlaneGcsSdk_Shared/Communication/TcpConnectionBase.cs @@ -57,6 +57,13 @@ namespace Plane.Communication public abstract Task OpenAsync(); + public int BytesToRead() + { + return 0; + } + + + public int Read(byte[] buffer, int offset, int count) { try diff --git a/PlaneGcsSdk_Shared/Communication/UdpConnectionBase.cs b/PlaneGcsSdk_Shared/Communication/UdpConnectionBase.cs index 4d3442e..74daf1c 100644 --- a/PlaneGcsSdk_Shared/Communication/UdpConnectionBase.cs +++ b/PlaneGcsSdk_Shared/Communication/UdpConnectionBase.cs @@ -20,6 +20,10 @@ namespace Plane.Communication public abstract void Close(); public abstract Task OpenAsync(); + public int BytesToRead() + { + return 0; + } public async Task ReadAsync(byte[] buffer, int offset, int size) { diff --git a/PlaneGcsSdk_Shared/Communication/UdpThroughDtuServiceConnection.cs b/PlaneGcsSdk_Shared/Communication/UdpThroughDtuServiceConnection.cs index d7097a4..9806f35 100644 --- a/PlaneGcsSdk_Shared/Communication/UdpThroughDtuServiceConnection.cs +++ b/PlaneGcsSdk_Shared/Communication/UdpThroughDtuServiceConnection.cs @@ -93,6 +93,10 @@ namespace Plane.Communication return 0; } } + public int BytesToRead() + { + return 0; + } public virtual async Task WriteAsync(byte[] buffer, int offset, int count) { diff --git a/PlaneGcsSdk_Shared/CopterManagement/SingleCopterManager.cs b/PlaneGcsSdk_Shared/CopterManagement/SingleCopterManager.cs index bb22edd..10cce52 100644 --- a/PlaneGcsSdk_Shared/CopterManagement/SingleCopterManager.cs +++ b/PlaneGcsSdk_Shared/CopterManagement/SingleCopterManager.cs @@ -186,6 +186,13 @@ namespace Plane.CopterManagement { return Copter.TakeOffAsync(); } + public Task InjectGpsDataAsync(byte[] data, ushort length) + { + return Copter.InjectGpsDataAsync(data, length); + } + + + public Task TakeOffAsync(float alt) { diff --git a/PlaneGcsSdk_Shared/Copters/EmptyCopter.cs b/PlaneGcsSdk_Shared/Copters/EmptyCopter.cs index e215448..fcbdcf6 100644 --- a/PlaneGcsSdk_Shared/Copters/EmptyCopter.cs +++ b/PlaneGcsSdk_Shared/Copters/EmptyCopter.cs @@ -92,6 +92,13 @@ namespace Plane.Copters return TaskUtils.CompletedTask; } + public Task InjectGpsDataAsync(byte[] data, ushort length) + { + return TaskUtils.CompletedTask; + + } + + internal override Task SetModeCoreAsync(FlightMode mode, int millisecondsTimeout = 5000) { return Task.FromResult(true); @@ -101,6 +108,10 @@ namespace Plane.Copters { return TaskUtils.CompletedTask; } + + + + } #if PRIVATE diff --git a/PlaneGcsSdk_Shared/Copters/FakeCopter.cs b/PlaneGcsSdk_Shared/Copters/FakeCopter.cs index 651b834..7c0b3f6 100644 --- a/PlaneGcsSdk_Shared/Copters/FakeCopter.cs +++ b/PlaneGcsSdk_Shared/Copters/FakeCopter.cs @@ -420,6 +420,12 @@ namespace Plane.Copters Roll = 0; Pitch = 0; } + public Task InjectGpsDataAsync(byte[] data, ushort length) + { + return TaskUtils.CompletedTask; + + } + private bool ReachedDestination(double destLat, double destLng, float destAlt) => this.CalcDistance(destLat, destLng, destAlt) < 0.1; diff --git a/PlaneGcsSdk_Shared/Copters/PLCopter.cs b/PlaneGcsSdk_Shared/Copters/PLCopter.cs index 89b423d..d2213c9 100644 --- a/PlaneGcsSdk_Shared/Copters/PLCopter.cs +++ b/PlaneGcsSdk_Shared/Copters/PLCopter.cs @@ -133,6 +133,12 @@ namespace Plane.Copters } + public async Task InjectGpsDataAsync(byte[] data, ushort length) + { + await _internalCopter.InjectGpsDataAsync(data, length).ConfigureAwait(false); + } + + public async Task SetShowLEDAsync(bool Ledon) { diff --git a/PlaneGcsSdk_Shared/Copters/PlaneCopter.cs b/PlaneGcsSdk_Shared/Copters/PlaneCopter.cs index 37ccaec..ba88b05 100644 --- a/PlaneGcsSdk_Shared/Copters/PlaneCopter.cs +++ b/PlaneGcsSdk_Shared/Copters/PlaneCopter.cs @@ -1234,6 +1234,26 @@ namespace Plane.Copters await GeneratePacketAsync(MAVLink.MAVLINK_MSG_ID_SET_MODE, mode); } + + + + public async Task InjectGpsDataAsync(byte[] data, ushort length) + { + MAVLink.mavlink_gps_inject_data_t gps = new MAVLink.mavlink_gps_inject_data_t(); + var msglen = 110; + var len = (length % msglen) == 0 ? length / msglen : (length / msglen) + 1; + for (int a = 0; a < len; a++) + { + gps.data = new byte[msglen]; + int copy = Math.Min(length - a * msglen, msglen); + Array.Copy(data, a * msglen, gps.data, 0, copy); + gps.len = (byte)copy; + gps.target_component = compid; + gps.target_system = sysid; + await GeneratePacketAsync(MAVLink.MAVLINK_MSG_ID_GPS_INJECT_DATA, gps); + } + } + public bool TranslateMode(ac2modes modein, ref MAVLink.mavlink_set_mode_t mode) { //MAVLink09.mavlink_set_mode_t mode = new MAVLink09.mavlink_set_mode_t(); diff --git a/PlaneGcsSdk_Shared/Protocols/MAVLinkTypes.cs b/PlaneGcsSdk_Shared/Protocols/MAVLinkTypes.cs index 506854b..6e9f466 100644 --- a/PlaneGcsSdk_Shared/Protocols/MAVLinkTypes.cs +++ b/PlaneGcsSdk_Shared/Protocols/MAVLinkTypes.cs @@ -2913,6 +2913,27 @@ namespace Plane.Protocols }; + public const byte MAVLINK_MSG_ID_GPS_INJECT_DATA = 123; + [StructLayout(LayoutKind.Sequential, Pack = 1, Size = 113)] + /// data for injecting into the onboard GPS (used for DGPS) + public struct mavlink_gps_inject_data_t + { + /// System ID + public byte target_system; + /// Component ID + public byte target_component; + /// data length + public byte len; + /// raw data (110 is enough for 12 satellites of RTCMv2) + [MarshalAs(UnmanagedType.ByValArray, SizeConst = 110)] + public byte[] data; + + }; + + + + + public const byte MAVLINK_MSG_ID_BATTERY_STATUS = 147; [StructLayout(LayoutKind.Sequential, Pack = 1, Size = 16)] public struct mavlink_battery_status_t