From 72a9e28ed1b242b6f2a3328de941994432e77a31 Mon Sep 17 00:00:00 2001 From: zxd Date: Thu, 23 Aug 2018 22:37:39 +0800 Subject: [PATCH] =?UTF-8?q?=E4=B8=BB=E8=A6=81=E4=BF=AE=E6=94=B9=E5=8F=96?= =?UTF-8?q?=E6=B6=88UDPserver=20=E6=9B=B4=E6=8D=A2=E9=80=9A=E4=BF=A1?= =?UTF-8?q?=E6=A8=A1=E5=9D=97=E7=9A=84=E9=80=9A=E4=BF=A1=20=E6=9C=80?= =?UTF-8?q?=E5=B0=8F=E8=B7=9D=E7=A6=BB=E7=9A=84=E4=BF=AE=E6=94=B9=20?= =?UTF-8?q?=E6=97=A0=E6=82=AC=E5=81=9C=E7=9A=84=E5=BF=AB=E9=80=9F=E8=88=AA?= =?UTF-8?q?=E7=82=B9=E6=A8=A1=E6=8B=9F=20=E6=97=B6=E9=97=B4=E9=95=BF?= =?UTF-8?q?=E5=BA=A6=E7=9A=84=E9=99=90=E5=AE=9A=20=E5=A6=82flyto=E7=9A=84?= =?UTF-8?q?=E6=97=B6=E9=97=B4=E9=99=90=E5=AE=9A=E4=B8=BA4095=20=E8=AE=A1?= =?UTF-8?q?=E5=88=92=E8=B7=AF=E7=BA=BF=E7=9A=84=E4=BF=AE=E6=94=B9=EF=BC=9A?= =?UTF-8?q?=E5=8F=AA=E6=98=BE=E7=A4=BA=E5=BD=93=E5=89=8D=E8=88=AA=E7=82=B9?= =?UTF-8?q?=E7=9A=84=E8=B7=AF=E7=BA=BF=20=E9=A3=9E=E6=8E=A7=E7=89=88?= =?UTF-8?q?=E6=9C=AC=E6=A3=80=E6=B5=8B=20=E8=88=AA=E7=82=B9=E5=A4=B1?= =?UTF-8?q?=E8=B4=A5=E7=BB=AD=E5=86=99=E5=92=8C=E8=88=AA=E7=82=B9=E5=8D=95?= =?UTF-8?q?=E5=86=99?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../ConnectionEstablishedEventArgs.cs | 2 ++ .../Copters/ICopter.cs | 1 + .../Copters/ICopterStatus.cs | 2 ++ .../Communication/TcpConnectionBase.cs | 24 ++++++++++++++++--- .../Copters/CopterImplSharedPart.cs | 24 +++++++++++++++++++ .../EHCopter.InternalCopterEventHandlers.cs | 13 +++++++--- PlaneGcsSdk_Shared/Copters/PLCopter.cs | 2 +- .../Copters/PlaneCopter.PacketAnalysis.cs | 22 +++++++++++++++++ PlaneGcsSdk_Shared/Copters/PlaneCopter.cs | 3 +++ .../PlaneGcsSdk_Shared.projitems | 5 ++++ PlaneGcsSdk_Shared/Protocols/MAVLinkTypes.cs | 11 +++++++-- PlaneGcsSdk_Shared/Protocols/MavlinkUtil.cs | 2 +- 12 files changed, 101 insertions(+), 10 deletions(-) diff --git a/PlaneGcsSdk.Contract_Shared/Communication/ConnectionEstablishedEventArgs.cs b/PlaneGcsSdk.Contract_Shared/Communication/ConnectionEstablishedEventArgs.cs index a14f92e..a0cd93b 100644 --- a/PlaneGcsSdk.Contract_Shared/Communication/ConnectionEstablishedEventArgs.cs +++ b/PlaneGcsSdk.Contract_Shared/Communication/ConnectionEstablishedEventArgs.cs @@ -13,5 +13,7 @@ namespace Plane.Communication public IConnection Connection { get; private set; } public string RemoteAddress { get; private set; } + + } } diff --git a/PlaneGcsSdk.Contract_Shared/Copters/ICopter.cs b/PlaneGcsSdk.Contract_Shared/Copters/ICopter.cs index c701378..e038ccb 100644 --- a/PlaneGcsSdk.Contract_Shared/Copters/ICopter.cs +++ b/PlaneGcsSdk.Contract_Shared/Copters/ICopter.cs @@ -4,5 +4,6 @@ namespace Plane.Copters { public partial interface ICopter : ICopterStatus, ICopterEvents, ICopterActions, ICopterCommunication, INotifyPropertyChanged { + } } diff --git a/PlaneGcsSdk.Contract_Shared/Copters/ICopterStatus.cs b/PlaneGcsSdk.Contract_Shared/Copters/ICopterStatus.cs index e7a9a77..47c6a19 100644 --- a/PlaneGcsSdk.Contract_Shared/Copters/ICopterStatus.cs +++ b/PlaneGcsSdk.Contract_Shared/Copters/ICopterStatus.cs @@ -172,5 +172,7 @@ namespace Plane.Copters /// 获取当前电池电压，单位为伏特。 /// float Voltage { get; } + + byte[] Retain { get; } } } diff --git a/PlaneGcsSdk_Shared/Communication/TcpConnectionBase.cs b/PlaneGcsSdk_Shared/Communication/TcpConnectionBase.cs index c1e7231..b866599 100644 --- a/PlaneGcsSdk_Shared/Communication/TcpConnectionBase.cs +++ b/PlaneGcsSdk_Shared/Communication/TcpConnectionBase.cs @@ -37,6 +37,21 @@ namespace Plane.Communication } } + public bool IsOnline + { + get + { + try + { + return !((_client.Client.Poll(1000, SelectMode.SelectRead) && (_client.Client.Available == 0)) || !_client.Client.Connected); + } + catch (ObjectDisposedException) + { + return false; + } + } + } + public bool IsOpen { get @@ -54,8 +69,8 @@ namespace Plane.Communication public void Close() { - _stream.Close(); - _client.Close(); + _stream?.Close(); + _client?.Close(); } public abstract Task OpenAsync(); @@ -92,7 +107,10 @@ namespace Plane.Communication { while (Available < count) { - if (!IsOpen) return 0; + //if (!IsOpen) + // return 0; + if (!IsOnline) + return 0; await Task.Delay(5).ConfigureAwait(false); } return await _stream.ReadAsync(buffer, offset, count); diff --git a/PlaneGcsSdk_Shared/Copters/CopterImplSharedPart.cs b/PlaneGcsSdk_Shared/Copters/CopterImplSharedPart.cs index 3a0e62b..a3cf514 100644 --- a/PlaneGcsSdk_Shared/Copters/CopterImplSharedPart.cs +++ b/PlaneGcsSdk_Shared/Copters/CopterImplSharedPart.cs @@ -123,6 +123,8 @@ namespace Plane.Copters private bool _IsCheckingConnection; + private bool _CommModuleConnected; + private bool _IsConnected; private bool _IsEmergencyHoverActive; @@ -138,6 +140,7 @@ namespace Plane.Copters private double _Longitude; private FlightMode _Mode; + private FlightMode _CommModuleMode; private float _Pitch; @@ -145,6 +148,8 @@ namespace Plane.Copters private byte _SatCount; + private byte[] _Retain = new byte[4]; + private CopterState _State; private string _StatusText; @@ -513,6 +518,12 @@ namespace Plane.Copters protected set { Set(nameof(IsConnected), ref _IsConnected, value); } } + public bool CommModuleConnected + { + get { return _CommModuleConnected; } + set { Set(nameof(CommModuleConnected), ref _CommModuleConnected, value); } + } + public bool IsEmergencyHoverActive { get { return _IsEmergencyHoverActive; } @@ -584,6 +595,7 @@ namespace Plane.Copters public string Name { get; set; } + public float Pitch { get { return _Pitch; } @@ -602,6 +614,12 @@ namespace Plane.Copters protected set { Set(nameof(SatCount), ref _SatCount, value); } } + public byte[] Retain + { + get { return _Retain; } + protected set { Set(nameof(Retain), ref _Retain, value); } + } + public CopterState State { get { return _State; } @@ -638,6 +656,12 @@ namespace Plane.Copters protected set { Set(nameof(Yaw), ref _Yaw, value); } } + public FlightMode CommModuleMode + { + get { return _CommModuleMode; } + protected set { Set(nameof(CommModuleMode), ref _CommModuleMode, value); } + } + #if PRIVATE public #else diff --git a/PlaneGcsSdk_Shared/Copters/EHCopter.InternalCopterEventHandlers.cs b/PlaneGcsSdk_Shared/Copters/EHCopter.InternalCopterEventHandlers.cs index 608f7f3..46c96d6 100644 --- a/PlaneGcsSdk_Shared/Copters/EHCopter.InternalCopterEventHandlers.cs +++ b/PlaneGcsSdk_Shared/Copters/EHCopter.InternalCopterEventHandlers.cs @@ -29,10 +29,17 @@ namespace Plane.Copters SatCount = _internalCopter.satcount; GpsFixType = _internalCopter.gpsstatus.ToGpsFixType(); GpsHdop = _internalCopter.gpshdop; - Elevation = _internalCopter.gpsalt; - - if (IsGpsAccurate) + Altitude = _internalCopter.gpsalt; + Retain = BitConverter.GetBytes(_internalCopter.retain); + Voltage = _internalCopter.battery_voltage; + CommModuleMode = (FlightMode)_internalCopter.commModuleMode; + IsUnlocked = _internalCopter.isUnlocked; + Yaw = _internalCopter.yaw; + HeartbeatCount++; + + if (SatCount > 8) { + IsGpsAccurate = true; RecordLat = _internalCopter.lat; RecordLng = _internalCopter.lng; } diff --git a/PlaneGcsSdk_Shared/Copters/PLCopter.cs b/PlaneGcsSdk_Shared/Copters/PLCopter.cs index d4752b1..904c66a 100644 --- a/PlaneGcsSdk_Shared/Copters/PLCopter.cs +++ b/PlaneGcsSdk_Shared/Copters/PLCopter.cs @@ -348,6 +348,7 @@ namespace Plane.Copters ch3: 1100, ch4: 1500 ).ConfigureAwait(false); + if (await SetModeAsync(FlightMode.ALT_HOLD).ConfigureAwait(false)) { await _internalCopter.DoARMAsync(true).ConfigureAwait(false); @@ -360,7 +361,6 @@ namespace Plane.Copters await Task.Run(async () => { - Console.WriteLine("LEDAsync"); await _internalCopter.DoLEDAsync().ConfigureAwait(false); }).ConfigureAwait(false); diff --git a/PlaneGcsSdk_Shared/Copters/PlaneCopter.PacketAnalysis.cs b/PlaneGcsSdk_Shared/Copters/PlaneCopter.PacketAnalysis.cs index af7b983..2d875a2 100644 --- a/PlaneGcsSdk_Shared/Copters/PlaneCopter.PacketAnalysis.cs +++ b/PlaneGcsSdk_Shared/Copters/PlaneCopter.PacketAnalysis.cs @@ -373,5 +373,27 @@ namespace Plane.Copters heading = vfr.heading; RaiseReceiveDataStreamEventOnUIThread(buffer[5]); } + + /// + /// 处理通信模块发过来的数据 + /// + /// + public void AnalyzeCommMouldePositionIntPacket(byte[] buffer) + { + var info = buffer.ByteArrayToStructure(0); + lat = info.lat * 1.0e-7; + lng = info.lng * 1.0e-7; + commModuleMode = (uint)info.control_mode; + gpsstatus = (byte)info.gps_status; + gpsalt = ((float)info.alt) / 1000; + satcount = info.gps_num_sats; + isUnlocked = info.lock_status == 1; + + battery_voltage = ((float)info.battery_voltage) / 1000; + yaw = ((float)info.yaw / 100) % 360; + + retain = info.retain; + RaiseReceiveDataStreamEventOnUIThread(MAVLink.MAVLINK_MSG_ID_GPS_RAW_INT); + } } } diff --git a/PlaneGcsSdk_Shared/Copters/PlaneCopter.cs b/PlaneGcsSdk_Shared/Copters/PlaneCopter.cs index 38b360a..c806ae7 100644 --- a/PlaneGcsSdk_Shared/Copters/PlaneCopter.cs +++ b/PlaneGcsSdk_Shared/Copters/PlaneCopter.cs @@ -42,6 +42,8 @@ namespace Plane.Copters public bool failsafe { get; set; } public uint mode { get; set; } + public uint commModuleMode { get; set; } + public bool isUnlocked { get; set; } public float battery_voltage { get; set; } public byte battery_remaining { get; set; } @@ -54,6 +56,7 @@ namespace Plane.Copters public byte gpsstatus { get; set; } public float gpshdop { get; set; } public byte satcount { get; set; } + public int retain { get; set; } public float groundspeed { get; set; } public float groundcourse { get; set; } public double lat { get; set; } diff --git a/PlaneGcsSdk_Shared/PlaneGcsSdk_Shared.projitems b/PlaneGcsSdk_Shared/PlaneGcsSdk_Shared.projitems index eb29cd0..ceddaef 100644 --- a/PlaneGcsSdk_Shared/PlaneGcsSdk_Shared.projitems +++ b/PlaneGcsSdk_Shared/PlaneGcsSdk_Shared.projitems @@ -9,12 +9,17 @@ Plane + + + + + diff --git a/PlaneGcsSdk_Shared/Protocols/MAVLinkTypes.cs b/PlaneGcsSdk_Shared/Protocols/MAVLinkTypes.cs index 1202b95..3fd1bf6 100644 --- a/PlaneGcsSdk_Shared/Protocols/MAVLinkTypes.cs +++ b/PlaneGcsSdk_Shared/Protocols/MAVLinkTypes.cs @@ -31,8 +31,8 @@ namespace Plane.Protocols //public static readonly byte[] MAVLINK_MESSAGE_LENGTHS = new byte[] { 9, 31, 12, 0, 14, 28, 3, 32, 0, 0, 0, 6, 0, 0, 0, 0, 0, 0, 0, 0, 20, 2, 25, 23, 30, 101, 22, 26, 16, 14, 28, 32, 28, 28, 22, 22, 21, 6, 6, 37, 4, 4, 2, 2, 4, 2, 2, 3, 13, 12, 19, 17, 15, 15, 27, 25, 18, 18, 20, 20, 9, 34, 26, 46, 36, 0, 6, 4, 0, 11, 18, 0, 0, 0, 20, 0, 33, 3, 0, 0, 20, 22, 0, 0, 0, 0, 0, 0, 0, 28, 56, 42, 33, 0, 0, 0, 0, 0, 0, 0, 26, 32, 32, 20, 32, 62, 54, 64, 84, 9, 254, 249, 9, 36, 26, 64, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 16, 33, 25, 42, 8, 4, 12, 15, 13, 6, 15, 14, 0, 12, 3, 8, 28, 44, 3, 9, 22, 12, 18, 34, 66, 98, 8, 48, 19, 3, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 36, 30, 18, 18, 51, 9, 0 }; //public static readonly byte[] MAVLINK_MESSAGE_CRCS = new byte[] {50, 124, 137, 0, 237, 217, 104, 119, 0, 0, 0, 89, 0, 0, 0, 0, 0, 0, 0, 0, 214, 159, 220, 168, 24, 23, 170, 144, 67, 115, 39, 246, 185, 104, 237, 244, 222, 212, 9, 254, 230, 28, 28, 132, 221, 232, 11, 153, 41, 39, 214, 223, 141, 33, 15, 3, 100, 24, 239, 238, 30, 240, 183, 130, 130, 118, 148, 21, 0, 243, 124, 0, 0, 0, 20, 0, 152, 143, 0, 0, 127, 106, 0, 0, 0, 0, 0, 0, 0, 231, 183, 63, 54, 0, 0, 0, 0, 0, 0, 0, 175, 102, 158, 208, 56, 93, 211, 108, 32, 185, 235, 93, 124, 124, 119, 4, 76, 128, 56, 116, 134, 237, 203, 250, 87, 203, 220, 0, 0, 0, 29, 223, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 177, 241, 15, 134, 219, 208, 188, 84, 22, 19, 21, 134, 0, 78, 68, 189, 127, 154, 21, 21, 144, 1, 234, 73, 181, 22, 83, 167, 138, 234, 240, 47, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 59, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 204, 49, 170, 44, 83, 46, 0 }; - public static readonly byte[] MAVLINK_MESSAGE_CRCS = new byte[] { 50, 124, 137, 0, 237, 217, 104, 119, 0, 0, 0, 89, 0, 0, 0, 0, 0, 0, 0, 0, 214, 159, 220, 168, 24, 23, 170, 144, 67, 115, 39, 246, 185, 104, 237, 244, 222, 212, 9, 254, 230, 28, 28, 132, 221, 232, 11, 153, 41, 39, 214, 223, 141, 33, 15, 3, 100, 24, 239, 238, 30, 240, 183, 130, 130, 118, 148, 21, 0, 243, 124, 0, 0, 0, 20, 0, 152, 143, 0, 0, 127, 106, 0, 0, 0, 0, 0, 0, 0, 231, 183, 63, 54, 0, 0, 0, 0, 0, 0, 0, 175, 102, 158, 208, 56, 93, 211, 108, 32, 185, 235, 93, 124, 124, 119, 4, 76, 128, 56, 116, 134, 237, 203, 250, 87, 203, 220, 0, 0, 0, 29, 223, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 177, 241, 15, 134, 219, 208, 188, 84, 22, 19, 21, 134, 0, 78, 68, 189, 127, 154, 21, 21, 144, 1, 234, 73, 181, 22, 83, 167, 138, 234, 240, 47, 0, 0, 0, 0, 0, 0, 0, 72, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 59, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 204, 49, 170, 44, 83, 46, 0 }; - + //public static readonly byte[] MAVLINK_MESSAGE_CRCS = new byte[] { 50, 124, 137, 0, 237, 217, 104, 119, 0, 0, 0, 89, 0, 0, 0, 0, 0, 0, 0, 0, 214, 159, 220, 168, 24, 23, 170, 144, 67, 115, 39, 246, 185, 104, 237, 244, 222, 212, 9, 254, 230, 28, 28, 132, 221, 232, 11, 153, 41, 39, 214, 223, 141, 33, 15, 3, 100, 24, 239, 238, 30, 240, 183, 130, 130, 118, 148, 21, 0, 243, 124, 0, 0, 0, 20, 0, 152, 143, 0, 0, 127, 106, 0, 0, 0, 0, 0, 0, 0, 231, 183, 63, 54, 0, 0, 0, 0, 0, 0, 0, 175, 102, 158, 208, 56, 93, 211, 108, 32, 185, 235, 93, 124, 124, 119, 4, 76, 128, 56, 116, 134, 237, 203, 250, 87, 203, 220, 0, 0, 0, 29, 223, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 177, 241, 15, 134, 219, 208, 188, 84, 22, 19, 21, 134, 0, 78, 68, 189, 127, 154, 21, 21, 144, 1, 234, 73, 181, 22, 83, 167, 138, 234, 240, 47, 0, 0, 0, 0, 0, 0, 0, 72, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 59, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 204, 49, 170, 44, 83, 46, 0 }; + public static readonly byte[] MAVLINK_MESSAGE_CRCS = new byte[] { 50, 124, 137, 0, 237, 217, 104, 119, 0, 0, 0, 89, 0, 0, 0, 0, 0, 0, 0, 0, 214, 159, 220, 168, 24, 23, 170, 144, 67, 115, 39, 246, 185, 104, 237, 244, 222, 212, 9, 254, 230, 28, 28, 132, 221, 232, 11, 153, 41, 39, 214, 223, 141, 33, 15, 3, 100, 24, 239, 238, 30, 240, 183, 130, 130, 118, 148, 21, 0, 243, 124, 0, 0, 0, 20, 0, 152, 143, 0, 0, 127, 106, 0, 0, 0, 0, 0, 0, 0, 231, 183, 63, 54, 0, 0, 0, 0, 0, 0, 0, 175, 102, 158, 208, 56, 93, 211, 108, 32, 185, 235, 93, 124, 124, 119, 4, 76, 128, 56, 116, 134, 237, 203, 250, 87, 203, 220, 0, 0, 0, 29, 223, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 177, 241, 15, 134, 219, 208, 188, 84, 22, 19, 21, 134, 0, 78, 68, 189, 127, 154, 21, 21, 144, 1, 234, 73, 181, 22, 83, 167, 138, 234, 240, 47, 0, 0, 0, 0, 85, 0, 0, 72, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 59, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 204, 49, 170, 44, 83, 46, 0 }; //public static Type[] MAVLINK_MESSAGE_INFO = new Type[] { typeof(mavlink_heartbeat_t), typeof(mavlink_sys_status_t), typeof(mavlink_system_time_t), null, typeof(mavlink_ping_t), typeof(mavlink_change_operator_control_t), typeof(mavlink_change_operator_control_ack_t), typeof(mavlink_auth_key_t), null, null, null, typeof(mavlink_set_mode_t), null, null, null, null, null, null, null, null, typeof(mavlink_param_request_read_t), typeof(mavlink_param_request_list_t), typeof(mavlink_param_value_t), typeof(mavlink_param_set_t), typeof(mavlink_gps_raw_int_t), typeof(mavlink_gps_status_t), typeof(mavlink_scaled_imu_t), typeof(mavlink_raw_imu_t), typeof(mavlink_raw_pressure_t), typeof(mavlink_scaled_pressure_t), typeof(mavlink_attitude_t), typeof(mavlink_attitude_quaternion_t), typeof(mavlink_local_position_ned_t), typeof(mavlink_global_position_int_t), typeof(mavlink_rc_channels_scaled_t), typeof(mavlink_rc_channels_raw_t), typeof(mavlink_servo_output_raw_t), typeof(mavlink_mission_request_partial_list_t), typeof(mavlink_mission_write_partial_list_t), typeof(mavlink_mission_item_t), typeof(mavlink_mission_request_t), typeof(mavlink_mission_set_current_t), typeof(mavlink_mission_current_t), typeof(mavlink_mission_request_list_t), typeof(mavlink_mission_count_t), typeof(mavlink_mission_clear_all_t), typeof(mavlink_mission_item_reached_t), typeof(mavlink_mission_ack_t), typeof(mavlink_set_gps_global_origin_t), typeof(mavlink_gps_global_origin_t), typeof(mavlink_set_local_position_setpoint_t), typeof(mavlink_local_position_setpoint_t), typeof(mavlink_global_position_setpoint_int_t), typeof(mavlink_set_global_position_setpoint_int_t), typeof(mavlink_safety_set_allowed_area_t), typeof(mavlink_safety_allowed_area_t), typeof(mavlink_set_roll_pitch_yaw_thrust_t), typeof(mavlink_set_roll_pitch_yaw_speed_thrust_t), typeof(mavlink_roll_pitch_yaw_thrust_setpoint_t), typeof(mavlink_roll_pitch_yaw_speed_thrust_setpoint_t), typeof(mavlink_set_quad_motors_setpoint_t), typeof(mavlink_set_quad_swarm_roll_pitch_yaw_thrust_t), typeof(mavlink_nav_controller_output_t), typeof(mavlink_set_quad_swarm_led_roll_pitch_yaw_thrust_t), typeof(mavlink_state_correction_t), null, typeof(mavlink_request_data_stream_t), typeof(mavlink_data_stream_t), null, typeof(mavlink_manual_control_t), typeof(mavlink_rc_channels_override_t), null, null, null, typeof(mavlink_vfr_hud_t), null, typeof(mavlink_command_long_t), typeof(mavlink_command_ack_t), null, null, typeof(mavlink_roll_pitch_yaw_rates_thrust_setpoint_t), typeof(mavlink_manual_setpoint_t), null, null, null, null, null, null, null, typeof(mavlink_local_position_ned_system_global_offset_t), typeof(mavlink_hil_state_t), typeof(mavlink_hil_controls_t), typeof(mavlink_hil_rc_inputs_raw_t), null, null, null, null, null, null, null, typeof(mavlink_optical_flow_t), typeof(mavlink_global_vision_position_estimate_t), typeof(mavlink_vision_position_estimate_t), typeof(mavlink_vision_speed_estimate_t), typeof(mavlink_vicon_position_estimate_t), typeof(mavlink_highres_imu_t), null, null, null, null, typeof(mavlink_file_transfer_start_t), typeof(mavlink_file_transfer_dir_list_t), typeof(mavlink_file_transfer_res_t), null, null, null, null, null, null, null, null, null, null, null, null, null, null, null, null, null, null, null, null, null, null, null, null, null, null, null, null, null, null, null, null, null, null, typeof(mavlink_battery_status_t), typeof(mavlink_setpoint_8dof_t), typeof(mavlink_setpoint_6dof_t), typeof(mavlink_sensor_offsets_t), typeof(mavlink_set_mag_offsets_t), typeof(mavlink_meminfo_t), typeof(mavlink_ap_adc_t), typeof(mavlink_digicam_configure_t), typeof(mavlink_digicam_control_t), typeof(mavlink_mount_configure_t), typeof(mavlink_mount_control_t), typeof(mavlink_mount_status_t), null, typeof(mavlink_fence_point_t), typeof(mavlink_fence_fetch_point_t), typeof(mavlink_fence_status_t), typeof(mavlink_ahrs_t), typeof(mavlink_simstate_t), typeof(mavlink_hwstatus_t), typeof(mavlink_radio_t), typeof(mavlink_limits_status_t), typeof(mavlink_wind_t), typeof(mavlink_data16_t), typeof(mavlink_data32_t), typeof(mavlink_data64_t), typeof(mavlink_data96_t), typeof(mavlink_rangefinder_t), null, null, null, null, null, null, null, null, null, null, null, null, null, null, null, null, null, null, null, null, null, null, null, null, null, null, typeof(mavlink_rc_mobile_control_t), typeof(mavlink_set_pair_t), null, null, null, null, null, null, null, null, null, null, null, null, null, null, null, null, null, null, null, null, null, null, null, null, null, null, null, null, null, null, null, null, null, null, null, null, null, null, null, null, null, null, null, null, null, null, null, typeof(mavlink_memory_vect_t), typeof(mavlink_debug_vect_t), typeof(mavlink_named_value_float_t), typeof(mavlink_named_value_int_t), typeof(mavlink_statustext_t), typeof(mavlink_debug_t), null }; public static Type[] MAVLINK_MESSAGE_INFO = new Type[] { typeof(mavlink_heartbeat_t), typeof(mavlink_sys_status_t), typeof(mavlink_system_time_t), null, typeof(mavlink_ping_t), typeof(mavlink_change_operator_control_t), typeof(mavlink_change_operator_control_ack_t), typeof(mavlink_auth_key_t), null, null, null, typeof(mavlink_set_mode_t), null, null, null, null, null, null, null, null, typeof(mavlink_param_request_read_t), typeof(mavlink_param_request_list_t), typeof(mavlink_param_value_t), typeof(mavlink_param_set_t), typeof(mavlink_gps_raw_int_t), typeof(mavlink_gps_status_t), typeof(mavlink_scaled_imu_t), typeof(mavlink_raw_imu_t), typeof(mavlink_raw_pressure_t), typeof(mavlink_scaled_pressure_t), typeof(mavlink_attitude_t), typeof(mavlink_attitude_quaternion_t), typeof(mavlink_local_position_ned_t), typeof(mavlink_global_position_int_t), typeof(mavlink_rc_channels_scaled_t), typeof(mavlink_rc_channels_raw_t), typeof(mavlink_servo_output_raw_t), typeof(mavlink_mission_request_partial_list_t), typeof(mavlink_mission_write_partial_list_t), typeof(mavlink_mission_item_t), typeof(mavlink_mission_request_t), typeof(mavlink_mission_set_current_t), typeof(mavlink_mission_current_t), typeof(mavlink_mission_request_list_t), typeof(mavlink_mission_count_t), typeof(mavlink_mission_clear_all_t), typeof(mavlink_mission_item_reached_t), typeof(mavlink_mission_ack_t), typeof(mavlink_set_gps_global_origin_t), typeof(mavlink_gps_global_origin_t), typeof(mavlink_set_local_position_setpoint_t), typeof(mavlink_local_position_setpoint_t), typeof(mavlink_global_position_setpoint_int_t), typeof(mavlink_set_global_position_setpoint_int_t), typeof(mavlink_safety_set_allowed_area_t), typeof(mavlink_safety_allowed_area_t), typeof(mavlink_set_roll_pitch_yaw_thrust_t), typeof(mavlink_set_roll_pitch_yaw_speed_thrust_t), typeof(mavlink_roll_pitch_yaw_thrust_setpoint_t), typeof(mavlink_roll_pitch_yaw_speed_thrust_setpoint_t), typeof(mavlink_set_quad_motors_setpoint_t), typeof(mavlink_set_quad_swarm_roll_pitch_yaw_thrust_t), typeof(mavlink_nav_controller_output_t), typeof(mavlink_set_quad_swarm_led_roll_pitch_yaw_thrust_t), typeof(mavlink_state_correction_t), null, typeof(mavlink_request_data_stream_t), typeof(mavlink_data_stream_t), null, typeof(mavlink_manual_control_t), typeof(mavlink_rc_channels_override_t), null, null, null, typeof(mavlink_vfr_hud_t), null, typeof(mavlink_command_long_t), typeof(mavlink_command_ack_t), null, null, typeof(mavlink_roll_pitch_yaw_rates_thrust_setpoint_t), typeof(mavlink_manual_setpoint_t), null, null, null, null, null, null, null, typeof(mavlink_local_position_ned_system_global_offset_t), typeof(mavlink_hil_state_t), typeof(mavlink_hil_controls_t), typeof(mavlink_hil_rc_inputs_raw_t), null, null, null, null, null, null, null, typeof(mavlink_optical_flow_t), typeof(mavlink_global_vision_position_estimate_t), typeof(mavlink_vision_position_estimate_t), typeof(mavlink_vision_speed_estimate_t), typeof(mavlink_vicon_position_estimate_t), typeof(mavlink_highres_imu_t), null, null, null, null, typeof(mavlink_file_transfer_start_t), typeof(mavlink_file_transfer_dir_list_t), typeof(mavlink_file_transfer_res_t), null, null, null, null, null, null, null, null, null, null, null, null, null, null, null, null, null, null, null, null, null, null, null, null, null, null, null, null, null, null, null, null, null, null, typeof(mavlink_battery_status_t), typeof(mavlink_setpoint_8dof_t), typeof(mavlink_setpoint_6dof_t), typeof(mavlink_sensor_offsets_t), typeof(mavlink_set_mag_offsets_t), typeof(mavlink_meminfo_t), typeof(mavlink_ap_adc_t), typeof(mavlink_digicam_configure_t), typeof(mavlink_digicam_control_t), typeof(mavlink_mount_configure_t), typeof(mavlink_mount_control_t), typeof(mavlink_mount_status_t), null, typeof(mavlink_fence_point_t), typeof(mavlink_fence_fetch_point_t), typeof(mavlink_fence_status_t), typeof(mavlink_ahrs_t), typeof(mavlink_simstate_t), typeof(mavlink_hwstatus_t), typeof(mavlink_radio_t), typeof(mavlink_limits_status_t), typeof(mavlink_wind_t), typeof(mavlink_data16_t), typeof(mavlink_data32_t), typeof(mavlink_data64_t), typeof(mavlink_data96_t), typeof(mavlink_rangefinder_t), null, null, null, null, null, null, null, null, null, null, null, null, typeof(mavlink_msg_id_led_control), null, null, null, null, null, null, null, null, null, null, null, null, null, typeof(mavlink_rc_mobile_control_t), typeof(mavlink_set_pair_t), null, null, null, null, null, null, null, null, null, null, null, null, null, null, null, null, null, null, null, null, null, null, null, null, null, null, null, null, null, null, null, null, null, null, null, null, null, null, null, null, null, null, null, null, null, null, null, typeof(mavlink_memory_vect_t), typeof(mavlink_debug_vect_t), typeof(mavlink_named_value_float_t), typeof(mavlink_named_value_int_t), typeof(mavlink_statustext_t), typeof(mavlink_debug_t), null }; @@ -3064,6 +3064,13 @@ namespace Plane.Protocols }; + public const byte MAVLINK_MSG_ID_AUTOPILOT_VERSION_REQUEST = 183; + [StructLayout(LayoutKind.Sequential, Pack = 1, Size = 2)] + public struct mavlink_sversion_request + { + public Int16 version; + } + public const byte MAVLINK_MSG_ID_MEMORY_VECT = 249; [StructLayout(LayoutKind.Sequential, Pack = 1, Size = 36)] diff --git a/PlaneGcsSdk_Shared/Protocols/MavlinkUtil.cs b/PlaneGcsSdk_Shared/Protocols/MavlinkUtil.cs index 6a0b717..4e36007 100644 --- a/PlaneGcsSdk_Shared/Protocols/MavlinkUtil.cs +++ b/PlaneGcsSdk_Shared/Protocols/MavlinkUtil.cs @@ -26,7 +26,7 @@ namespace Plane.Protocols /// The newly created mavlink packet internal static TMavlinkPacket ByteArrayToStructure(this byte[] bytearray, int startoffset = 6) where TMavlinkPacket : struct { - if (bytearray[0] == 'U') + if (bytearray[0] == 'U' && startoffset == 6 ) { throw new NotSupportedException("bytearray[0] == 'U' is not supported."); //ByteArrayToStructureEndian(bytearray, ref obj, startoffset);