From 2ae769b5d4d31b38ef818812010c8afadc1471fc Mon Sep 17 00:00:00 2001 From: pxzleo Date: Mon, 31 Jul 2017 01:09:32 +0800 Subject: [PATCH] =?UTF-8?q?=E5=8A=A0=E5=85=A5RTK=E6=94=AF=E6=8C=81=20?= =?UTF-8?q?=E5=9C=B0=E9=9D=A2=E7=AB=99=E5=8F=91=E9=80=81=E6=8E=A5=E6=94=B6?= =?UTF-8?q?=E5=88=B0=E7=9A=84=E5=9F=BA=E7=AB=99=E6=95=B0=E6=8D=AE=E5=88=B0?= =?UTF-8?q?=E9=A3=9E=E6=9C=BA=EF=BC=8C=E6=95=B0=E6=8D=AE=E4=B8=8D=E5=81=9A?= =?UTF-8?q?=E4=BB=BB=E4=BD=95=E4=BF=AE=E6=94=B9=E7=9B=B4=E6=8E=A5=E6=89=93?= =?UTF-8?q?=E5=8C=85=E5=8F=91=E9=80=81=E5=88=B0=E6=AF=8F=E6=9E=B6=E9=A3=9E?= =?UTF-8?q?=E6=9C=BA?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../Communication/IConnection.cs | 3 +++ .../Copters/GpsFixType.cs | 6 ++++- .../ICopterActionsSharedByCopterManager.cs | 5 ++++ .../Communication/CompositeConnection.cs | 27 +++++++++++++++++++ .../Communication/EmptyConnection.cs | 5 ++++ .../Communication/SerialPortConnection.cs | 8 ++++++ .../Communication/TcpConnectionBase.cs | 7 +++++ .../Communication/UdpConnectionBase.cs | 4 +++ .../UdpThroughDtuServiceConnection.cs | 4 +++ .../CopterManagement/SingleCopterManager.cs | 7 +++++ PlaneGcsSdk_Shared/Copters/EmptyCopter.cs | 11 ++++++++ PlaneGcsSdk_Shared/Copters/FakeCopter.cs | 6 +++++ PlaneGcsSdk_Shared/Copters/PLCopter.cs | 6 +++++ PlaneGcsSdk_Shared/Copters/PlaneCopter.cs | 20 ++++++++++++++ PlaneGcsSdk_Shared/Protocols/MAVLinkTypes.cs | 21 +++++++++++++++ 15 files changed, 139 insertions(+), 1 deletion(-) 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