From f0a4484cfcbaa208c26706c096ff5fba6419839b Mon Sep 17 00:00:00 2001 From: pxzleo Date: Tue, 26 Mar 2024 21:58:48 +0800 Subject: [PATCH] =?UTF-8?q?=E6=94=AF=E6=8C=81=E7=B4=A7=E6=80=A5=E8=BF=94?= =?UTF-8?q?=E8=88=AA=20=E6=94=AF=E6=8C=81=E8=BF=94=E5=9B=9E=E9=A3=9E?= =?UTF-8?q?=E6=9C=BA=E9=94=99=E8=AF=AF?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../CommunicationManagement/CommModule.cs | 2 +- .../CommModuleGenerateMavLink.cs | 91 +- .../CommModulePacketAnalysis.cs | 426 +-- .../Copters/CopterImplSharedPart.cs | 2887 +++++++------- .../EHCopter.InternalCopterEventHandlers.cs | 626 +-- PlaneGcsSdk_Shared/Copters/FakeCopter.cs | 114 +- .../Copters/PlaneCopter.PacketAnalysis.cs | 806 ++-- PlaneGcsSdk_Shared/Copters/PlaneCopter.cs | 3356 +++++++++-------- PlaneGcsSdk_Shared/Protocols/MavCommTypes.cs | 364 +- 9 files changed, 4433 insertions(+), 4239 deletions(-) diff --git a/PlaneGcsSdk_Shared/CommunicationManagement/CommModule.cs b/PlaneGcsSdk_Shared/CommunicationManagement/CommModule.cs index ce6076f..9a2214f 100644 --- a/PlaneGcsSdk_Shared/CommunicationManagement/CommModule.cs +++ b/PlaneGcsSdk_Shared/CommunicationManagement/CommModule.cs @@ -310,7 +310,7 @@ namespace Plane.CommunicationManagement case (short)MavComm.MessageType.SCAN3: AnalyzeComm3RetrunPacket(copterNum, dealData); break; - case 4: + case 4: //版本13通讯模块收到飞机返回的数据 AnalyzeComm4RetrunPacket(copterNum, dealData); break; } diff --git a/PlaneGcsSdk_Shared/CommunicationManagement/CommModuleGenerateMavLink.cs b/PlaneGcsSdk_Shared/CommunicationManagement/CommModuleGenerateMavLink.cs index 68f0813..086a252 100644 --- a/PlaneGcsSdk_Shared/CommunicationManagement/CommModuleGenerateMavLink.cs +++ b/PlaneGcsSdk_Shared/CommunicationManagement/CommModuleGenerateMavLink.cs @@ -851,14 +851,10 @@ namespace Plane.CommunicationManagement param7 = param7 | 1; if (UseTransModule) { - byte[] packet = DoCommandAsync(MAVLink.MAV_CMD.MISSION_START, - (float)takeoffcentloc.Latitude * 1000, - (float)takeoffcentloc.Longitude * 1000,//起飞高度不用传=0 - (float)taskcentloc.Latitude * 1000, - (float)taskcentloc.Longitude * 1000, - taskcentloc.Altitude, - mindistance, param7 - ); + byte[] packet = DoCommandAsync(MAVLink.MAV_CMD.RETURN_TO_LAUNCH, + (float)takeoffcentloc.Latitude * 10000000, + (float)takeoffcentloc.Longitude * 10000000,//起飞高度不用传=0 + mindistance*100, 0, 0,0,0); short copterId = 0; byte[] batchPacket = null; await WriteCommPacketAsync(copterId, MavComm.COMM_DOWNLOAD_COMM, packet, batchPacket).ConfigureAwait(false); @@ -1104,6 +1100,55 @@ namespace Plane.CommunicationManagement } } + /// + /// 重启飞控 + /// + /// + /// + public async Task DoRestartFCAsync(IEnumerable copters = null) + { + if (UseTransModule) + { + short copterId = 0; + byte[] batchPacket = null; + GetCopterIds(copters, out copterId, out batchPacket); + //只校陀螺仪，详细说明见;https://mavlink.io/en/messages/common.html#MAV_CMD_PREFLIGHT_REBOOT_SHUTDOWN + byte[] packet = DoCommandAsync(MAVLink.MAV_CMD.PREFLIGHT_REBOOT_SHUTDOWN, 1, 1,1, 0, 0, 0, 0); + await WriteCommPacketAsync(copterId, MavComm.COMM_DOWNLOAD_COMM, packet, batchPacket).ConfigureAwait(false); + } + else + { + if (copters != null) + foreach (var vcopter in copters) + await vcopter.DoCommandAsync((int)MAVLink.MAV_CMD.PREFLIGHT_REBOOT_SHUTDOWN, 1, 1, 1, 0, 0, 0, 0); + + } + } + + /// + /// 校准陀螺仪 + /// + /// + /// + public async Task DoCalibrationPreflightAsync(IEnumerable copters = null) + { + if (UseTransModule) + { + short copterId = 0; + byte[] batchPacket = null; + GetCopterIds(copters, out copterId, out batchPacket); + //只校陀螺仪，详细说明见;https://mavlink.io/en/messages/common.html#MAV_CMD_PREFLIGHT_CALIBRATION + byte[] packet = DoCommandAsync(MAVLink.MAV_CMD.PREFLIGHT_CALIBRATION, 1, 0, 0, 0, 0, 0, 0); + await WriteCommPacketAsync(copterId, MavComm.COMM_DOWNLOAD_COMM, packet, batchPacket).ConfigureAwait(false); + } + else + { + if (copters != null) + foreach (var vcopter in copters) + await vcopter.DoCommandAsync((int)MAVLink.MAV_CMD.PREFLIGHT_CALIBRATION, 1, 0, 0, 0, 0, 0, 0); + + } + } /// /// 校准指南针 /// @@ -1227,6 +1272,16 @@ namespace Plane.CommunicationManagement } + // get sum crc 计算数据校验和 + public byte checkrtrcmsum(byte[] data, ushort length) + { + byte rtcm_sumcrc = 0; + for ( int i=0; i< length; i++) + { + rtcm_sumcrc += data[i]; + } + return rtcm_sumcrc; + } /// @@ -1252,26 +1307,28 @@ namespace Plane.CommunicationManagement //gps.data[0] = rtcm_tmpser++; gps.len = (byte)datalen; gps.target_component = rtcm_tmpser++; - gps.target_system = 1; + //实测一旦收到数据都是正确的，张伟已经做过crc检验了，为了兼容性不再做了 + //如果需要做，飞控里面需要把注释的代码放开，并且不再兼容之前的固件 + gps.target_system = 1;// checkrtrcmsum(gps.data, (ushort)datalen); - // Console.WriteLine(" ["+ gps.target_component + "]总长: "+ length +" 发送第 "+ (a+1) + " 个包,长度: " + (ushort)datalen ); + // Console.WriteLine(" ["+ gps.target_component + "]总长: "+ length +" 发送第 "+ (a+1) + " 个包,长度: " + (ushort)datalen ); byte[] packet = GenerateRTKPacketAsync(MAVLink.MAVLINK_MSG_ID_GPS_INJECT_DATA, gps, gps.len); await WriteCommPacketAsync(0, MavComm.COMM_DOWNLOAD_COMM, packet).ConfigureAwait(false); - await Task.Delay(50).ConfigureAwait(false); //没有延时得话如果超过110长度，连续发可能收不到 + await Task.Delay(80).ConfigureAwait(false); //没有延时得话如果超过110长度，连续发可能收不到 //重发一次，有序列号(target_component)飞机可以检测出来重复接收的 //需要新固件支持 - // await WriteCommPacketAsync(0, MavComm.COMM_DOWNLOAD_COMM, packet).ConfigureAwait(false); - // await Task.Delay(50).ConfigureAwait(false); //没有延时得话如果超过110长度，连续发可能收不到 + // await WriteCommPacketAsync(0, MavComm.COMM_DOWNLOAD_COMM, packet).ConfigureAwait(false); + // await Task.Delay(80).ConfigureAwait(false); //没有延时得话如果超过110长度，连续发可能收不到 - /* + //重发一次，有序列号(target_component)飞机可以检测出来重复接收的--需要新固件支持 - await WriteCommPacketAsync(0, MavComm.COMM_DOWNLOAD_COMM, packet).ConfigureAwait(false); - await Task.Delay(50).ConfigureAwait(false); //没有延时得话如果超过110长度，连续发可能收不到 - */ + //await WriteCommPacketAsync(0, MavComm.COMM_DOWNLOAD_COMM, packet).ConfigureAwait(false); + // await Task.Delay(80).ConfigureAwait(false); //没有延时得话如果超过110长度，连续发可能收不到 + } diff --git a/PlaneGcsSdk_Shared/CommunicationManagement/CommModulePacketAnalysis.cs b/PlaneGcsSdk_Shared/CommunicationManagement/CommModulePacketAnalysis.cs index 5eb2799..3585957 100644 --- a/PlaneGcsSdk_Shared/CommunicationManagement/CommModulePacketAnalysis.cs +++ b/PlaneGcsSdk_Shared/CommunicationManagement/CommModulePacketAnalysis.cs @@ -1,213 +1,213 @@ -using Plane.Communication; -using Plane.Protocols; -using Plane.Windows.Messages; -using System; -using System.Collections.Generic; -using System.Diagnostics; -using System.Linq; -using System.Text; -using System.Threading.Tasks; - -namespace Plane.CommunicationManagement -{ - public partial class CommModuleManager - { - private int MissionStartCopterId = 0; - private int MissionSendCopterId = 0; - private int MissionErrorId = -1; - private int ErrorCode = 0; - public event EventHandler CommunicationReceived; - public event EventHandler CommunicationCopterDisconnect; - public event EventHandler CommunicationConnected; - private Dictionary missionWriteState = new Dictionary(); - private static readonly object MissinLocker = new object(); - - public Dictionary MissionWriteState - { - get {return missionWriteState; } - } - - public void ClearMissionWriteState() - { - missionWriteState.Clear(); - } - - private void AnalyzeMissionStartPacket(short copterId, byte[] buffer) - { - short errorId = BitConverter.ToInt16(buffer, 0); - MissionStartCopterId = copterId; - MissionErrorId = errorId; - if(errorId != 0) - Message.Show($"{copterId}:返回 = {errorId}"); - } - - private void AnalyzeStringPacket(short copterId, byte[] buffer) - { - int count = Array.IndexOf(buffer, (byte)0); - string msg = System.Text.Encoding.Default.GetString(buffer, 0, count).Replace("tcpserver", "flicube"); - Message.Show(msg); - } - - private void AnalyzeComm2RetrunPacket(short copterId, byte[] buffer) - { - ushort ret = BitConverter.ToUInt16(buffer, 0); - switch (ret) - { - case MavComm.SEARCH_FINDCOPTER: - Message.Show(copterId.ToString() + "---飞机开始相应"); - break; - - case MavComm.SEARCH_COMPLETED: - Message.Show(copterId.ToString() + "---设置成功"); - break; - - case MavComm.SEARCH_OUTTIME: - Message.Show("超时无响应"); - break; - - case MavComm.MISSION_SEND_COMPLETED: - MissionSendCopterId = copterId; - break; - - case MavComm.P2P_SEND_FAILED: - Message.Show("点对点通信发送失败"); - break; - default: - if (ret > MavComm.ERROR_CODE_START && ret <= MavComm.ERROR_CODE_END) - { - ErrorCode = ret; - Message.Show($"{copterId}--错误码：{ret}"); - } - - break; - } - } - - private void AnalyzeComm3RetrunPacket(short copterId, byte[] buffer) - { - byte type = buffer[0]; - byte connectRet; - switch (type) - { - case 0x01: - //connectRet = buffer[1]; - //if (connectRet == 0x0) //飞机连接上 不管飞控和通信模块的连接 - CommunicationConnected?.Invoke(this, new CommunicationConnectEventArgs(copterId)); - break; - case 0x02: - connectRet = buffer[1]; - if (connectRet == 0x0) //飞机断开 - CommunicationCopterDisconnect?.Invoke(this, new CommunicationCopterDisconnectEventArgs(copterId)); - break; - - case 0x03: - SaveMissionWriteStat(copterId, buffer[1]); - break; - - case 0x04: - Message.Show($"飞机{copterId}:功率设置成功，新回传功率：{buffer[1]}"); - break; - - case 0x0e: - if (copterId == 0) - Message.Show("----------全部更新完成----------"); - else - Message.Show($"飞机{copterId}:更新进度{buffer[1]}%"); - break; - - default: - // Message.Show($"Comm3返回：{type}"); - break; - } - } - - private void SaveMissionWriteStat(short copterId, byte wirteMissRet) - { - Task.Run(async () => - { - lock (MissinLocker) - { - // var copter = _copterManager.Copters.FirstOrDefault(c => c.Id == arrs[0]); - - if (wirteMissRet == 0x20) - { - if (missionWriteState.ContainsKey(copterId)) - missionWriteState[copterId].WriteSucceed = true; - Message.Show($"ID:{copterId}:航点写入成功"); - } - if (wirteMissRet > 0x20 && wirteMissRet < 0x30) - { - if (missionWriteState.ContainsKey(copterId)) - missionWriteState[copterId].WriteSucceed = false; - Message.Show($"ID:{copterId}:航点写入失败"); - } - } - await Task.Delay(10).ConfigureAwait(false); - } - ); - - } - - private void AnalyzeCopterInfosPacket(byte[] buffer) - { - ushort beginNum = BitConverter.ToUInt16(buffer, 0); - ushort endNum = BitConverter.ToUInt16(buffer, 2); - int count = endNum - beginNum + 1; - byte[] copter_packets = buffer.Skip(4).ToArray(); - if (copter_packets.Length != count * 4) - { - return; - } - - int offset = 0; - for (int i = beginNum; i <= endNum; i++) - { - byte[] onePacket = copter_packets.Skip(offset).Take(4).ToArray(); - UInt16 state = BitConverter.ToUInt16(onePacket, 0); - short copterId = (short)i; - switch (state >> 13) - { - //0B000 - case 0x0000 >> 13: - CommunicationCopterDisconnect?.Invoke(this, new CommunicationCopterDisconnectEventArgs(copterId)); - break; - //0B100 - case 0x8000 >> 13: - break; - //0B110 - case 0xC000 >> 13: - //0B111 - case 0xE000 >> 13: - CommunicationConnected?.Invoke(this, new CommunicationConnectEventArgs(copterId)); - break; - } - offset += 4; - } - } - - private void AnalyzeComm4RetrunPacket(short copterId, byte[] buffer) - { - byte[] packet = buffer.Take(28).ToArray(); - byte[] state_packet = buffer.Skip(28).ToArray(); - UInt16 state = BitConverter.ToUInt16(state_packet,0); - byte version = state_packet[3]; - switch (state >> 13) - { - //0B000 - case 0x0000 >> 13: - CommunicationCopterDisconnect?.Invoke(this, new CommunicationCopterDisconnectEventArgs(copterId)); - break; - //0B100 - case 0x8000 >> 13: - break; - //0B110 - case 0xC000 >> 13: - //0B111 - case 0xE000 >> 13: - CommunicationReceived?.Invoke(this, new CommunicationReceiveCopterInfoEventArgs(copterId, packet, version)); - break; - } - - } - } -} +using Plane.Communication; +using Plane.Protocols; +using Plane.Windows.Messages; +using System; +using System.Collections.Generic; +using System.Diagnostics; +using System.Linq; +using System.Text; +using System.Threading.Tasks; + +namespace Plane.CommunicationManagement +{ + public partial class CommModuleManager + { + private int MissionStartCopterId = 0; + private int MissionSendCopterId = 0; + private int MissionErrorId = -1; + private int ErrorCode = 0; + public event EventHandler CommunicationReceived; + public event EventHandler CommunicationCopterDisconnect; + public event EventHandler CommunicationConnected; + private Dictionary missionWriteState = new Dictionary(); + private static readonly object MissinLocker = new object(); + + public Dictionary MissionWriteState + { + get {return missionWriteState; } + } + + public void ClearMissionWriteState() + { + missionWriteState.Clear(); + } + + private void AnalyzeMissionStartPacket(short copterId, byte[] buffer) + { + short errorId = BitConverter.ToInt16(buffer, 0); + MissionStartCopterId = copterId; + MissionErrorId = errorId; + if(errorId != 0) + Message.Show($"{copterId}:返回 = {errorId}"); + } + + private void AnalyzeStringPacket(short copterId, byte[] buffer) + { + int count = Array.IndexOf(buffer, (byte)0); + string msg = System.Text.Encoding.Default.GetString(buffer, 0, count).Replace("tcpserver", "flicube"); + Message.Show(msg); + } + + private void AnalyzeComm2RetrunPacket(short copterId, byte[] buffer) + { + ushort ret = BitConverter.ToUInt16(buffer, 0); + switch (ret) + { + case MavComm.SEARCH_FINDCOPTER: + Message.Show(copterId.ToString() + "---飞机开始相应"); + break; + + case MavComm.SEARCH_COMPLETED: + Message.Show(copterId.ToString() + "---设置成功"); + break; + + case MavComm.SEARCH_OUTTIME: + Message.Show("超时无响应"); + break; + + case MavComm.MISSION_SEND_COMPLETED: + MissionSendCopterId = copterId; + break; + + case MavComm.P2P_SEND_FAILED: + Message.Show("点对点通信发送失败"); + break; + default: + if (ret > MavComm.ERROR_CODE_START && ret <= MavComm.ERROR_CODE_END) + { + ErrorCode = ret; + Message.Show($"{copterId}--错误码：{ret}"); + } + + break; + } + } + + private void AnalyzeComm3RetrunPacket(short copterId, byte[] buffer) + { + byte type = buffer[0]; + byte connectRet; + switch (type) + { + case 0x01: + //connectRet = buffer[1]; + //if (connectRet == 0x0) //飞机连接上 不管飞控和通信模块的连接 + CommunicationConnected?.Invoke(this, new CommunicationConnectEventArgs(copterId)); + break; + case 0x02: + connectRet = buffer[1]; + if (connectRet == 0x0) //飞机断开 + CommunicationCopterDisconnect?.Invoke(this, new CommunicationCopterDisconnectEventArgs(copterId)); + break; + + case 0x03: + SaveMissionWriteStat(copterId, buffer[1]); + break; + + case 0x04: + Message.Show($"飞机{copterId}:功率设置成功，新回传功率：{buffer[1]}"); + break; + + case 0x0e: + if (copterId == 0) + Message.Show("----------全部更新完成----------"); + else + Message.Show($"飞机{copterId}:更新进度{buffer[1]}%"); + break; + + default: + // Message.Show($"Comm3返回：{type}"); + break; + } + } + + private void SaveMissionWriteStat(short copterId, byte wirteMissRet) + { + Task.Run(async () => + { + lock (MissinLocker) + { + // var copter = _copterManager.Copters.FirstOrDefault(c => c.Id == arrs[0]); + + if (wirteMissRet == 0x20) + { + if (missionWriteState.ContainsKey(copterId)) + missionWriteState[copterId].WriteSucceed = true; + Message.Show($"ID:{copterId}:航点写入成功"); + } + if (wirteMissRet > 0x20 && wirteMissRet < 0x30) + { + if (missionWriteState.ContainsKey(copterId)) + missionWriteState[copterId].WriteSucceed = false; + Message.Show($"ID:{copterId}:航点写入失败"); + } + } + await Task.Delay(10).ConfigureAwait(false); + } + ); + + } + + private void AnalyzeCopterInfosPacket(byte[] buffer) + { + ushort beginNum = BitConverter.ToUInt16(buffer, 0); + ushort endNum = BitConverter.ToUInt16(buffer, 2); + int count = endNum - beginNum + 1; + byte[] copter_packets = buffer.Skip(4).ToArray(); + if (copter_packets.Length != count * 4) + { + return; + } + + int offset = 0; + for (int i = beginNum; i <= endNum; i++) + { + byte[] onePacket = copter_packets.Skip(offset).Take(4).ToArray(); + UInt16 state = BitConverter.ToUInt16(onePacket, 0); + short copterId = (short)i; + switch (state >> 13) + { + //0B000 + case 0x0000 >> 13: + CommunicationCopterDisconnect?.Invoke(this, new CommunicationCopterDisconnectEventArgs(copterId)); + break; + //0B100 + case 0x8000 >> 13: + break; + //0B110 + case 0xC000 >> 13: + //0B111 + case 0xE000 >> 13: + CommunicationConnected?.Invoke(this, new CommunicationConnectEventArgs(copterId)); + break; + } + offset += 4; + } + } + + private void AnalyzeComm4RetrunPacket(short copterId, byte[] buffer) + { + byte[] packet = buffer.Take(28).ToArray(); + byte[] state_packet = buffer.Skip(28).ToArray(); + UInt16 state = BitConverter.ToUInt16(state_packet,0); + byte version = state_packet[3]; + switch (state >> 13) + { + //0B000 + case 0x0000 >> 13: + CommunicationCopterDisconnect?.Invoke(this, new CommunicationCopterDisconnectEventArgs(copterId)); + break; + //0B100 + case 0x8000 >> 13: + break; + //0B110 + case 0xC000 >> 13: + //0B111 + case 0xE000 >> 13: //最后正在用的版本 + CommunicationReceived?.Invoke(this, new CommunicationReceiveCopterInfoEventArgs(copterId, packet, version)); + break; + } + + } + } +} diff --git a/PlaneGcsSdk_Shared/Copters/CopterImplSharedPart.cs b/PlaneGcsSdk_Shared/Copters/CopterImplSharedPart.cs index 5b0aca5..07043a5 100644 --- a/PlaneGcsSdk_Shared/Copters/CopterImplSharedPart.cs +++ b/PlaneGcsSdk_Shared/Copters/CopterImplSharedPart.cs @@ -1,1396 +1,1491 @@ -using Plane.Geography; -using System; -using System.ComponentModel; -using System.Drawing; -using System.Threading; -using System.Threading.Tasks; - -namespace Plane.Copters -{ - public abstract partial class CopterImplSharedPart : PLObservableObject, ILocation - { - protected CopterState? _forcedState; - - protected bool _isMobileControlActive; - - protected float? _lastChangedAlt; - - protected ILocation _lastChangedLocation; - - protected DateTime _lastRaiseAltitudeChangedTime; - - protected DateTime _lastRaiseLocationChangedTime; - - protected bool _shouldFollow; - - protected DateTime? _takeOffTime; - - /// - /// 更新 的时间间隔。 - /// - private static readonly TimeSpan _intervalToUpdateFlightTimeSpan = TimeSpan.FromSeconds(0.5); - - private float _followAltDifference; - - private float _followDistance; - - private bool _followKeepFacingTarget; - - private bool _followKeep3DRelativeLocations; - - private bool _followKeepYawDifference; - - private ILocation _followLastDestination; - - private IVisibleStatus _followTarget; - - private float _followSelfDirectionFromTarget; - - private float _followTargetOriginalYaw; - - private float? _followYaw; - - private DateTime _lastUpdateStatusTextTime; - - private int _mobileControlIntervalMilli = 50; - - #region Backing Fields - - private float _AirSpeed; - - private float _Altitude; - - private byte _BatteryPer = 100; - - private ushort _Channel1; - - private ushort _Channel2; - - private ushort _Channel3; - - private ushort _Channel4; - - private ushort _Channel5; - - private ushort _Channel6; - - private ushort _Channel7; - - private ushort _Channel8; - - private ushort? _DesiredChannel1; - - private ushort? _DesiredChannel2; - - private ushort? _DesiredChannel3; - - private ushort? _DesiredChannel4; - - private ushort? _DesiredChannel5; - - private ushort? _DesiredChannel6; - - private ushort? _DesiredChannel7; - - private ushort? _DesiredChannel8; - - private float? _DesiredYaw; - - private float _Elevation; - - private int? _FirmwareVersion; - - private string _FirmwareVersionText; - - private double _FlightDistance; - - private double _FlightDistance2D; - private int _FlightControlMode; - - private TimeSpan _FlightTime; - - private GpsFixType _GpsFixType; - - private float _GpsHdop; - - private float _GroundSpeed; - - private bool _HasSwitchedToGpsMode; - - private short _Heading; - - private ulong _HeartbeatCount; - - private bool _IsAbsolutelyConnected; - - private bool _IsCheckingConnection; - - private bool _CommModuleConnected; - - private bool _IsConnected; - - private bool _IsEmergencyHoverActive; - - private bool _IsGpsAccurate; - - private bool _IsPairing; - - private bool _IsUnlocked; - - private double _Latitude; - - private double _Longitude; - - private FlightMode _Mode; - private FlightMode _CommModuleMode; - - private float _Pitch; - - private float _Roll; - - private byte _SatCount; - - private byte[] _Retain = new byte[4]; - - private float _GroundAlt = 0; - - private int _sim_update_int = 50; - - private CopterState _State; - - private string _StatusText; - - private ILocation _TakeOffPoint; - - private uint _TimebootMs; - - private float _Voltage; - - private float _Yaw; - - //初始时位置：使用地图Center - private double _RecordLat; - - private double _RecordLng; - - - private int _VirtualId; - - private bool _DisplayVirtualId = false; - - private bool _DisplayID = true; - - private float _maxspeed_xy=3; - private float _maxspeed_down=3; - private float _maxspeed_up = 3; - private float _acc_z=1f; - private float _acc_xy=2.5f; - - #endregion Backing Fields - - public CopterImplSharedPart(SynchronizationContext uiSyncContext) : base(uiSyncContext) - { - PropertyChanged += CopterImplSharedPart_PropertyChanged; - /* - Task.Run(async () => - { - while (true) - { - UpdateFlightTimeSpanIfNeeded(); - await Task.Delay(_intervalToUpdateFlightTimeSpan).ConfigureAwait(false); - } - }); - - Task.Run(async () => - { - while (true) - { - if (!_isMobileControlActive) - { - await EnsureChannelsSetAsync(); - } - if (_lastUpdateStatusTextTime.AddSeconds(2) <= DateTime.Now) - { - StatusText = null; - } - await Task.Delay(500); //50 - } - }); - - Task.Run(async () => - { - var lastHeartbeatCount = HeartbeatCount; - while (true) - { - if (!IsConnected || lastHeartbeatCount == HeartbeatCount) - { - // 如果 !IsConnected（连接通道断开），肯定失联了； - // 或者，过了一段时间，心跳数没变化，认为飞行器失联。 - IsAbsolutelyConnected = false; - - // 此处等待的时间较短，目的是在飞行器重新连上时快速知晓。 - await Task.Delay(500).ConfigureAwait(false); - } - else - { - // 过了一段时间，心跳数变化了，确认飞行器连接正常。 - IsAbsolutelyConnected = true; - - // 记录此时的心跳数。 - lastHeartbeatCount = HeartbeatCount; - - // 此处等待的时间较长，目的是提高失联判断的可信程度。 - await Task.Delay(5000).ConfigureAwait(false); - } - } - - }); - */ - } - - public event EventHandler AltitudeChanged; - - public event EventHandler AttitudeChanged; - -#if PRIVATE - public event EventHandler DataStreamReceived; -#endif - - public event EventHandler HeartbeatReceived; - - public event EventHandler LocationChanged; - - public event EventHandler MissionItemReceived; - - public event EventHandler PairingCompleted; - - public event EventHandler SensorDataReceived; - - public event EventHandler SystemStatusReceived; - - public event EventHandler TextReceived; - - - protected enum PDataStreamType - { - SYS_STATUS = 1, // MAVLINK_MSG_ID_SYS_STATUS, - GPS_RAW_INT = 24, // MAVLINK_MSG_ID_GPS_RAW_INT, - RC_CHANNELS_RAW = 35 // MAVLINK_MSG_ID_RC_CHANNELS_RAW - } - - public float AirSpeed - { - get { return _AirSpeed; } - protected set { Set(nameof(AirSpeed), ref _AirSpeed, value); } - } - - public float Altitude - { - get { return _Altitude; } - protected set { Set(nameof(Altitude), ref _Altitude, value); } - } - - public byte BatteryPer - { - get { return _BatteryPer; } - protected set { Set(nameof(BatteryPer), ref _BatteryPer, value); } - } - - public ushort Channel1 - { - get { return _Channel1; } - protected set { Set(nameof(Channel1), ref _Channel1, value); } - } - - public ushort Channel2 - { - get { return _Channel2; } - protected set { Set(nameof(Channel2), ref _Channel2, value); } - } - - public ushort Channel3 - { - get { return _Channel3; } - protected set { Set(nameof(Channel3), ref _Channel3, value); } - } - - public ushort Channel4 - { - get { return _Channel4; } - protected set { Set(nameof(Channel4), ref _Channel4, value); } - } - - public ushort Channel5 - { - get { return _Channel5; } - protected set { Set(nameof(Channel5), ref _Channel5, value); } - } - - public ushort Channel6 - { - get { return _Channel6; } - protected set { Set(nameof(Channel6), ref _Channel6, value); } - } - - public ushort Channel7 - { - get { return _Channel7; } - protected set { Set(nameof(Channel7), ref _Channel7, value); } - } - - public ushort Channel8 - { - get { return _Channel8; } - protected set { Set(nameof(Channel8), ref _Channel8, value); } - } - - public ushort? DesiredChannel1 - { - get { return _DesiredChannel1; } - set { if (!IsEmergencyHoverActive) { Set(nameof(DesiredChannel1), ref _DesiredChannel1, value); } } - } - - public ushort? DesiredChannel2 - { - get { return _DesiredChannel2; } - set { if (!IsEmergencyHoverActive) { Set(nameof(DesiredChannel2), ref _DesiredChannel2, value); } } - } - - public ushort? DesiredChannel3 - { - get { return _DesiredChannel3; } - set - { - // 王海, 20160317, 紧急悬停时可调节高度。 - //if (!IsEmergencyHoverActive) - //{ - if (Set(nameof(DesiredChannel3), ref _DesiredChannel3, value)) - { - if (State == CopterState.Initialized) SetStateAccordingToMode(Mode); - } - //} - } - } - - public ushort? DesiredChannel4 - { - get { return _DesiredChannel4; } - set { if (!IsEmergencyHoverActive) { Set(nameof(DesiredChannel4), ref _DesiredChannel4, value); } } - } - - public ushort? DesiredChannel5 - { - get { return _DesiredChannel5; } - set { if (!IsEmergencyHoverActive) { Set(nameof(DesiredChannel5), ref _DesiredChannel5, value); } } - } - - public ushort? DesiredChannel6 - { - get { return _DesiredChannel6; } - set { if (!IsEmergencyHoverActive) { Set(nameof(DesiredChannel6), ref _DesiredChannel6, value); } } - } - - public ushort? DesiredChannel7 - { - get { return _DesiredChannel7; } - set { if (!IsEmergencyHoverActive) { Set(nameof(DesiredChannel7), ref _DesiredChannel7, value); } } - } - - public ushort? DesiredChannel8 - { - get { return _DesiredChannel8; } - set { if (!IsEmergencyHoverActive) { Set(nameof(DesiredChannel8), ref _DesiredChannel8, value); } } - } - - public float? DesiredYaw - { - get { return _DesiredYaw; } - set { if (!IsEmergencyHoverActive && !(State == CopterState.Following && _followKeepFacingTarget)) { Set(nameof(DesiredYaw), ref _DesiredYaw, value); } } - } - - public float Elevation - { - get { return _Elevation; } - protected set { Set(nameof(Elevation), ref _Elevation, value); } - } - - public int VirtualId - { - get { return _VirtualId; } - set { Set(nameof(VirtualId), ref _VirtualId, value); RefreashLoc(); } - } - - public bool DisplayVirtualId - { - get { return _DisplayVirtualId; } - set { Set(nameof(DisplayVirtualId), ref _DisplayVirtualId, value); RefreashLoc(); } - } - - public bool DisplayID - { - get { return _DisplayID; } - set { Set(nameof(DisplayID), ref _DisplayID, value); RefreashLoc(); } - } - - - - /// - /// 获取固件版本。 - /// - public int? FirmwareVersion - { - get { return _FirmwareVersion; } - protected set - { - if (Set(nameof(FirmwareVersion), ref _FirmwareVersion, value)) - { - if (value == null) - { - FirmwareVersionText = null; - } - else - { - int major, minor, build; - int temp; -#if NETFX_CORE - major = value.Value / 0x1000; - temp = value.Value % 0x1000; - minor = temp / 0x100; - build = temp % 0x100; -#else - major = Math.DivRem(value.Value, 0x1000, out temp); - minor = Math.DivRem(temp, 0x100, out build); -#endif - FirmwareVersionText = $"{major}.{minor}.{build}"; - } - } - } - } - - /// - /// 获取固件版本的文本形式。 - /// - public string FirmwareVersionText - { - get { return _FirmwareVersionText; } - protected set { Set(nameof(FirmwareVersionText), ref _FirmwareVersionText, value); } - } - - public double FlightDistance - { - get { return _FlightDistance; } - protected set { Set(nameof(FlightDistance), ref _FlightDistance, value); } - } - - public double FlightDistance2D - { - get { return _FlightDistance2D; } - protected set { Set(nameof(FlightDistance2D), ref _FlightDistance2D, value); } - } - - //FlightControlMode=1表示同时到达，速度可变,用于新固件 - //=0表示固定速度，用于老固件 - public int FlightControlMode - { - get { return _FlightControlMode; } - set { Set(nameof(FlightControlMode), ref _FlightControlMode, value); } - } - - - public TimeSpan FlightTimeSpan - { - get { return _FlightTime; } - protected set { Set(nameof(FlightTimeSpan), ref _FlightTime, value); } - } - - /// - /// 获取 GPS 锁定状态。 - /// - public GpsFixType GpsFixType - { - get { return _GpsFixType; } - protected set { Set(nameof(GpsFixType), ref _GpsFixType, value); } - } - - /// - /// 获取 GPS HDOP（horizontal dilution of position），单位为米。 - /// - public float GpsHdop - { - get { return _GpsHdop; } - protected set { Set(nameof(GpsHdop), ref _GpsHdop, value); } - } - - public float GroundSpeed - { - get { return _GroundSpeed; } - protected set { Set(nameof(GroundSpeed), ref _GroundSpeed, value); } - } - - public bool HasSwitchedToGpsMode - { - get { return _HasSwitchedToGpsMode; } - protected set { Set(nameof(HasSwitchedToGpsMode), ref _HasSwitchedToGpsMode, value); } - } - - public short Heading - { - get { return _Heading; } - protected set { Set(nameof(Heading), ref _Heading, value); } - } - - public ulong HeartbeatCount - { - get { return _HeartbeatCount; } - protected set { Set(nameof(HeartbeatCount), ref _HeartbeatCount, value); } - } - - /// - /// 获取一个值，指示是否确定飞行器与地面站正常连接。在实现中，一般根据对心跳包的接收情况判断。 - /// - public bool IsAbsolutelyConnected - { - get { return _IsAbsolutelyConnected; } - protected set { Set(nameof(IsAbsolutelyConnected), ref _IsAbsolutelyConnected, value); } - } - - /// - /// 获取或设置一个值，指示是否正在检查飞行器与地面站的连接以确定通信正常。在实现中，一般在建立连接后设为 true，在收到飞行器发回的数据后设为 false。 - /// - public bool IsCheckingConnection - { - get { return _IsCheckingConnection; } - set { Set(nameof(IsCheckingConnection), ref _IsCheckingConnection, value); } - } - - public bool IsConnected - { - get { return _IsConnected; } - 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; } - protected set { Set(nameof(IsEmergencyHoverActive), ref _IsEmergencyHoverActive, value); } - } - - /// - /// 获取一个值，指示当前的 GPS 定位是否足够精确。 - /// - public bool IsGpsAccurate - { - get { return _IsGpsAccurate; } - protected set { Set(nameof(IsGpsAccurate), ref _IsGpsAccurate, value); } - } - - public bool IsPairing - { - get { return _IsPairing; } - protected set { Set(nameof(IsPairing), ref _IsPairing, value); } - } - - public bool IsUnlocked - { - get { return _IsUnlocked; } - protected set - { - if (Set(nameof(IsUnlocked), ref _IsUnlocked, value)) - { - if (value) - { - State = CopterState.Initialized; - } - else - { - State = CopterState.Locked; - } - } - } - } - - public double Latitude - { - get { return _Latitude; } - protected set { Set(nameof(Latitude), ref _Latitude, value); } - } - - public double Longitude - { - get { return _Longitude; } - protected set { Set(nameof(Longitude), ref _Longitude, value); } - } - - public double RecordLat - { - get { return _RecordLat; } - set { Set(nameof(RecordLat), ref _RecordLat, value); } - } - - public double RecordLng - { - get { return _RecordLng; } - set { Set(nameof(RecordLng), ref _RecordLng, value); } - } - - /// - /// 获取任务总数。 - /// - public ushort? MissionCount { get; protected set; } - - public string Name { get; set; } - - public float Pitch - { - get { return _Pitch; } - protected set { Set(nameof(Pitch), ref _Pitch, value); } - } - - public float Roll - { - get { return _Roll; } - protected set { Set(nameof(Roll), ref _Roll, value); } - } - - public byte SatCount - { - get { return _SatCount; } - protected set { Set(nameof(SatCount), ref _SatCount, value); } - } - - public float RetainInt - { - get { return BitConverter.ToSingle(Retain,0); } - } - - public float GroundAlt - { - get { return _GroundAlt; } - set { - Set(nameof(GroundAlt), ref _GroundAlt, value); - RaisePropertyChanged(nameof(GroundAlt)); - } - } - - - public int sim_update_int - { - get { return _sim_update_int; } - set - { - Set(nameof(sim_update_int), ref _sim_update_int, value); - } - } - - - public float maxspeed_xy { - get { return _maxspeed_xy; } - set - { - Set(nameof(maxspeed_xy), ref _maxspeed_xy, value); - } - } - - public float maxspeed_up - { - get { return _maxspeed_up; } - set - { - Set(nameof(maxspeed_up), ref _maxspeed_up, value); - } - } - public float maxspeed_down - { - get { return _maxspeed_down; } - set - { - Set(nameof(maxspeed_down), ref _maxspeed_down, value); - } - } - public float acc_z - { - get { return _acc_z; } - set - { - Set(nameof(acc_z), ref _acc_z, value); - } - } - public float acc_xy - { - get { return _acc_xy; } - set - { - Set(nameof(acc_xy), ref _acc_xy, value); - } - } - - - - public byte[] Retain - { - get { return _Retain; } - protected set - { - Set(nameof(Retain), ref _Retain, value); - RaisePropertyChanged(nameof(RetainInt)); - } - } - - public CopterState State - { - get { return _State; } - protected set { Set(nameof(State), ref _State, _forcedState ?? value); } - } - - public string StatusText - { - get { return _StatusText; } - protected set - { - if (Set(nameof(StatusText), ref _StatusText, value)) - { - _lastUpdateStatusTextTime = DateTime.Now; - } - } - } - - public ILocation TakeOffPoint - { - get { return _TakeOffPoint; } - protected set { Set(nameof(TakeOffPoint), ref _TakeOffPoint, value); } - } - - public float Voltage - { - get { return _Voltage; } - protected set { Set(nameof(Voltage), ref _Voltage, value); } - } - - public float Yaw - { - get { return _Yaw; } - protected set { Set(nameof(Yaw), ref _Yaw, value); } - } - - public FlightMode CommModuleMode - { - get { return _CommModuleMode; } - protected set { Set(nameof(CommModuleMode), ref _CommModuleMode, value); } - } - - private byte _CommModuleVersion; - public byte CommModuleVersion - { - get { return _CommModuleVersion; } - protected set { Set(nameof(CommModuleVersion), ref _CommModuleVersion, value); } - } - - private string _LEDColor; - public string LEDColor - { - get { return _LEDColor; } - set { - if (value!=_LEDColor) - { - Set(nameof(LEDColor), ref _LEDColor, value); - //强制刷新颜色--在刷新位置时才刷新颜色 - RefreashLoc(); - - } - } - } - - private Color _LEDShowColor; - public Color LEDShowColor - { - get { return _LEDShowColor; } - set - { - - Set(nameof(LEDShowColor), ref _LEDShowColor, value); - - } - } - - - private int _LEDMode; - public int LEDMode - { - get { return _LEDMode; } - set - { - if (value != _LEDMode) - { - Set(nameof(LEDMode), ref _LEDMode, value); - //强制刷新颜色--在刷新位置时才刷新颜色 - // RefreashLoc(); - } - } - } - private float _LEDInterval; - public float LEDInterval - { - get { return _LEDInterval; } - set - { - if (value != _LEDInterval) - { - Set(nameof(LEDInterval), ref _LEDInterval, value); - //强制刷新颜色--在刷新位置时才刷新颜色 - // RefreashLoc(); - } - } - } - - -#if PRIVATE - public -#else - internal -#endif - FlightMode Mode - { - get { return _Mode; } - set - { - // 王海, 20160325, Mode 属性改成 internal 了，如果仍然用 Set 或者 RaisePropertyChanged 方法，会抛异常（"Property not found"）。现决定不引发 PropertyChanged 事件了。 - var changed = _Mode != value; - if (changed) _Mode = value; - - // 如果在已解锁状态下满足某些条件，就根据模式设置 State。 - if (IsUnlocked) - { - var shouldSetState = false; - if (changed) - { - if (State == CopterState.Following) - { - if (value != FlightMode.GUIDED) - { - // 跟随时切换到其他模式，停止跟随并且设置 State。 - _shouldFollow = false; - shouldSetState = true; - } - } - else - { - // 非跟随时模式变化了，设置 State。 - shouldSetState = true; - } - } - if (shouldSetState) SetStateAccordingToMode(value, defaultState: CopterState.Initialized); - } - } - } - - public async Task FloatAsync() - { - if (await SetModeAsync(FlightMode.ALT_HOLD).ConfigureAwait(false)) - { - await SetChannelsAsync( - ch1: 1500, - ch2: 1500, - ch3: 1500, - ch4: 1500 - ).ConfigureAwait(false); - } - } - - /// - /// 使飞行器飞往水平面上指定的点。 - /// - /// 纬度。 - /// 经度。 - /// 表示此命令异步发送操作的 实例。 - public Task FlyToAsync(double lat, double lng) - { - return FlyToAsync(lat, lng, Altitude); - } - - public Task FlyToAsync(double lat, double lng, float alt, float flytime = 0, int flytype = 0) - { - _shouldFollow = false; - //State = CopterState.CommandMode; - return FlyToCoreAsync(lat, lng, alt, flytime, flytype); - } - - public void Follow(IVisibleStatus target, bool keepYawDifference = false, bool keepFacingTarget = true, bool keep3DRelativeLocations = false) - { - - } - - public async Task HoverAsync() - { - if (await SetModeAsync(FlightMode.LOITER).ConfigureAwait(false)) - { - await SetChannelsAsync( - ch1: 1500, - ch2: 1500, - ch3: 1500, - ch4: 1500 - ).ConfigureAwait(false); - } - } - public async Task GuidAsync() - { - if (await SetModeAsync(FlightMode.GUIDED).ConfigureAwait(false)) - { - await SetChannelsAsync( - ch1: 1500, - ch2: 1500, - ch3: 1500, - ch4: 1500 - ).ConfigureAwait(false); - } - } - - public async Task LandAsync() - { - await SetModeAsync(FlightMode.LAND).ConfigureAwait(false); - await SetChannelsAsync(ch1: 1500, ch2: 1500).ConfigureAwait(false); - } - - public async Task ReturnToLaunchAsync() - { - await SetModeAsync(FlightMode.RTL).ConfigureAwait(false); - await SetChannelsAsync(ch1: 1500, ch2: 1500).ConfigureAwait(false); - } - - public async Task SetChannelsAsync(ushort? ch1 = null, ushort? ch2 = null, ushort? ch3 = null, ushort? ch4 = null, ushort? ch5 = null, ushort? ch6 = null, ushort? ch7 = null, ushort? ch8 = null) - { - SetTargets(ch1, ch2, ch3, ch4, ch5, ch6, ch7, ch8); - - await SetChannelsAsync().ConfigureAwait(false); - } - - public abstract Task SetChannelsAsync(); - - public async Task SetMobileControlAsync(ushort? ch1 = null, ushort? ch2 = null, ushort? ch3 = null, ushort? ch4 = null, ushort? ch5 = null, ushort? ch6 = null, ushort? ch7 = null, ushort? ch8 = null, float? yaw = null) // 王海, 20150912, 将来如有需要再补上 TargetAlt, float? alt = null) - { - SetTargets(ch1, ch2, ch3, ch4, ch5, ch6, ch7, ch8, yaw); - - await SetMobileControlAsync().ConfigureAwait(false); - } - - public abstract Task SetMobileControlAsync(); - - public async Task StartEmergencyHoverAsync() - { - _shouldFollow = false; - State = CopterState.HoverMode; - IsEmergencyHoverActive = true; - - // 王海, 20151028, 先 IsEmergencyHoverActive = true 再设置目标通道是为了防止别的线程正好在 IsEmergencyHoverActive == false 时设置通道。 - // 王海, 20151019, 在紧急悬停状态，外部设置 DesiredChannel[1-4] 无效，只能如此设置。 - SetFieldAndRaisePropertyChanged(ref _DesiredChannel1, 1500, nameof(DesiredChannel1)); - SetFieldAndRaisePropertyChanged(ref _DesiredChannel2, 1500, nameof(DesiredChannel2)); - SetFieldAndRaisePropertyChanged(ref _DesiredChannel3, 1500, nameof(DesiredChannel3)); - SetFieldAndRaisePropertyChanged(ref _DesiredChannel4, 1500, nameof(DesiredChannel4)); - - await SetMobileControlAsync().ConfigureAwait(false); - - switch (Mode) - { - // 王海, 20151019, 波子说一律切 LOITER。 - //case FlightMode.ALT_HOLD: - // break; - //case FlightMode.LOITER: - // break; - //case FlightMode.STABILIZE: - // await SetModeCoreAsync(FlightMode.ALT_HOLD).ConfigureAwait(false); - // break; - //case FlightMode.AUTO: - //case FlightMode.GUIDED: - //case FlightMode.RTL: - //case FlightMode.CIRCLE: - //case FlightMode.LAND: - default: - await SetModeCoreAsync(FlightMode.LOITER).ConfigureAwait(false); - break; - } - } - - public async void StartMobileControl(int? millisecondsInterval = null) - { - if (millisecondsInterval != null) _mobileControlIntervalMilli = millisecondsInterval.Value; - if (!_isMobileControlActive) - { - _isMobileControlActive = true; - while (_isMobileControlActive) - { - if (State == CopterState.Following) - { - await SetMobileControlAsync(ch1: 1500, ch2: 1500).ConfigureAwait(false); - } - else { - switch (Mode) - { - case FlightMode.STABILIZE: - case FlightMode.ALT_HOLD: - case FlightMode.LOITER: - await SetMobileControlAsync().ConfigureAwait(false); - break; - - case FlightMode.GUIDED: - case FlightMode.RTL: - await SetMobileControlAsync(ch1: 1500, ch2: 1500, ch3: 1500).ConfigureAwait(false); - break; - - case FlightMode.LAND: - await SetMobileControlAsync(ch3: 1500); - break; - - default: - await SetMobileControlAsync(ch1: 1500, ch2: 1500, yaw: Yaw).ConfigureAwait(false); - break; - } - } - await Task.Delay(_mobileControlIntervalMilli).ConfigureAwait(false); - } - } - } - - public void StopEmergencyHover() - { - IsEmergencyHoverActive = false; - } - - public void StopMobileControl() - { - _isMobileControlActive = false; - } - - public async Task TakeOffAsync() - { - - - /* await SetChannelsAsync( - ch1: 1500, - ch2: 1500, - ch3: 1500, - ch4: 1500 - ).ConfigureAwait(false); - _forcedState = CopterState.TakingOff; - State = CopterState.TakingOff; - await DoCommandAsync(MAVLink.MAV_CMD.TAKEOFF, 0, 0, 0, 0, 0, 0, alt).ConfigureAwait(false); - var temp = Task.Run(async () => - { - var startTime = DateTime.Now; - var secondsTimeout = alt / 0.7; - while (currentTakeOffCount == _takeOffCount) - { - if (alt - Altitude < 0.2 || startTime.AddSeconds(secondsTimeout) <= DateTime.Now) - { - _forcedState = null; - await HoverAsync().ConfigureAwait(false); - break; - } - await Task.Delay(50).ConfigureAwait(false); - } - }); -*/ - - - - - - - - - if (await SetModeAsync(FlightMode.AUTO).ConfigureAwait(false)) - { - await SetChannelsAsync( - ch1: 1500, - ch2: 1500, - ch3: 1500, - ch4: 1500 - ).ConfigureAwait(false); - } - - } - - /// - /// 切换飞行模式，返回的 实例在确认切换成功或超时后完成。 - /// - /// 飞行模式。 - /// 超时时间，单位为毫秒。 - /// 表示切换模式操作的 实例。若切换模式成功，异步操作的结果为 true；否则为 false。 -#if PRIVATE - public -#else - internal -#endif - async Task SetModeAsync(FlightMode mode, int millisecondsTimeout = 5000) - { - if (_shouldFollow && mode != FlightMode.GUIDED) _shouldFollow = false; - // 王海, 20160317, 紧急悬停时可返航或降落。 - if (mode == FlightMode.RTL || mode == FlightMode.LAND) - { - StopEmergencyHover(); - } - if (!IsEmergencyHoverActive) - { - return await SetModeCoreAsync(mode, millisecondsTimeout).ConfigureAwait(false); - } - return false; - } - - /// - /// 无论在不在紧急悬停状态下都可用的切换模式方法。 - /// - /// 模式。 - /// 超时。 - /// 成功与否。 - internal abstract Task SetModeCoreAsync(FlightMode mode, int millisecondsTimeout = 5000); - - protected async Task EnsureChannelsSetAsync() - { - if ( - (DesiredChannel1 != null && Channel1 != DesiredChannel1) || - (DesiredChannel2 != null && Channel2 != DesiredChannel2) || - (DesiredChannel3 != null && Channel3 != DesiredChannel3) || - (DesiredChannel4 != null && Channel4 != DesiredChannel4) || - (DesiredChannel5 != null && Channel5 != DesiredChannel5) || - (DesiredChannel6 != null && Channel6 != DesiredChannel6) || - (DesiredChannel7 != null && Channel7 != DesiredChannel7) || - (DesiredChannel8 != null && Channel8 != DesiredChannel8) - ) // Any channel not set. - { - await SetChannelsAsync().ConfigureAwait(false); - } - } - - /// - /// 不主动设置 State，仅发送飞往某点命令。 - /// - /// 纬度。 - /// 经度。 - /// 高度。 - /// 表示此异步发送操作的 实例。 - protected abstract Task FlyToCoreAsync(double lat, double lng, float alt, float flytime = 0, int flytype = 0); - - protected void RaiseAltitudeChanged() => AltitudeChanged?.Invoke(this, EventArgs.Empty); - - protected void RaiseAltitudeChangedIfNeeded() - { - if (_lastChangedAlt == null || Math.Abs(Altitude - _lastChangedAlt.Value) >= 0.5 || DateTime.Now.AddMilliseconds(-500) >= _lastRaiseAltitudeChangedTime) - { - RaiseAltitudeChanged(); - _lastChangedAlt = Altitude; - _lastRaiseAltitudeChangedTime = DateTime.Now; - } - } - - protected void RaiseAttitudeChanged() => AttitudeChanged?.Invoke(this, EventArgs.Empty); - - protected void RaiseDataStreamReceived(PDataStreamType dataStreamType) - { -#if PRIVATE - DataStreamReceived?.Invoke(this, new DataStreamReceivedEventArgs((DataStreamType)dataStreamType)); -#endif - } - - protected void RaiseHeartbeatReceived(HeartbeatReceivedEventArgs e) - { - HeartbeatReceived?.Invoke(this, e); - } - - protected void RaiseLocationChanged() => LocationChanged?.Invoke(this, EventArgs.Empty); - - public void RefreashLoc() - { - RaiseLocationChangedIfNeeded(true); - } - - protected void RaiseLocationChangedIfNeeded(bool forcemk=false) - { - //强制刷新位置 - if (forcemk) - { - RaiseLocationChanged(); - return; - } - //减少计算量，在模拟飞机很多时花时间 - bool EnRaise = true; - if (_lastChangedLocation != null) - EnRaise = (Altitude != _lastChangedLocation.Altitude) || - (Latitude != _lastChangedLocation.Latitude) || - (Longitude != _lastChangedLocation.Longitude); - //if (_lastChangedLocation == null || this.CalcDistance(_lastChangedLocation) >= 0.5 || DateTime.Now.AddMilliseconds(-500) >= _lastRaiseLocationChangedTime) - if (EnRaise) - { - RaiseLocationChanged(); - _lastChangedLocation = new PLLocation(this); - _lastRaiseLocationChangedTime = DateTime.Now; - } - - } - - protected void RaiseMissionItemReceived(MissionItemReceivedEventArgs e) - { - MissionItemReceived?.Invoke(this, e); - } - - protected void RaisePairingCompleted(PairingCompletedEventArgs e) - { - var handler = PairingCompleted; - if (handler == null) return; - if (SynchronizationContext.Current == _uiSyncContext) - { - handler(this, e); - } - else - { - _uiSyncContext.Post(() => handler(this, e)); - } - } - - protected void RaiseSensorDataReceived(EventArgs e) - { - SensorDataReceived?.Invoke(this, e); - } - - protected void RaiseSysStatusReceived(SystemStatusReceivedEventArgs e) - { - SystemStatusReceived?.Invoke(this, e); - } - - - protected void RaiseTextReceived(MessageCreatedEventArgs e) - { - TextReceived?.Invoke(this, e); - } - - - - protected void SetTargets(ushort? ch1 = null, ushort? ch2 = null, ushort? ch3 = null, ushort? ch4 = null, ushort? ch5 = null, ushort? ch6 = null, ushort? ch7 = null, ushort? ch8 = null, float? yaw = null) - { - if (ch1 != null) DesiredChannel1 = ch1; - if (ch2 != null) DesiredChannel2 = ch2; - if (ch3 != null) DesiredChannel3 = ch3; - if (ch4 != null) DesiredChannel4 = ch4; - if (ch5 != null) DesiredChannel5 = ch5; - if (ch6 != null) DesiredChannel6 = ch6; - if (ch7 != null) DesiredChannel7 = ch7; - if (ch8 != null) DesiredChannel8 = ch8; - if (yaw != null) DesiredYaw = yaw; - } - - protected virtual void UpdateFlightDataIfNeeded() - { - if (!TakeOffPoint.IsNullOrEmpty()) - { - FlightDistance = TakeOffPoint.CalcDistance(this); - } - } - - protected void UpdateFlightTimeSpanIfNeeded() - { - if (_takeOffTime != null && IsUnlocked) - { - var flightTimeSpan = DateTime.Now - _takeOffTime.Value; - if (flightTimeSpan - FlightTimeSpan >= _intervalToUpdateFlightTimeSpan) - { - FlightTimeSpan = flightTimeSpan; - } - } - } - - private void CopterImplSharedPart_PropertyChanged(object sender, PropertyChangedEventArgs e) - { - switch (e.PropertyName) - { - case nameof(IsConnected): - if (!IsConnected) - { - IsCheckingConnection = false; - IsAbsolutelyConnected = false; - } - break; - - case nameof(IsUnlocked): - if (IsUnlocked) - { - // 王海, 20151029, 在解锁时重置起飞点、起飞时间、飞行距离、飞行时间、速度。 - TakeOffPoint = new PLLocation { Latitude = Latitude, Longitude = Longitude, Altitude = Altitude }; - _takeOffTime = DateTime.Now; - FlightDistance = 0; - FlightDistance2D = 0; - FlightTimeSpan = TimeSpan.FromSeconds(0); - AirSpeed = 0; - GroundSpeed = 0; - } - else - { - if (IsEmergencyHoverActive) StopEmergencyHover(); - } - break; - - default: - break; - } - } - - private void SetFieldAndRaisePropertyChanged(ref ushort? field, ushort value, string propertyName) - { - if (field != value) - { - field = value; - RaisePropertyChanged(propertyName); - } - } - - private void SetFieldAndRaisePropertyChanged(ref float? field, float value, string propertyName) - { - if (field != value) - { - field = value; - RaisePropertyChanged(propertyName); - } - } - - private void SetStateAccordingToMode(FlightMode value, CopterState? defaultState = null) - { - switch (value) - { - case FlightMode.ALT_HOLD: - State = CopterState.FloatMode; - break; - - case FlightMode.AUTO: - State = CopterState.TakingOff; - break; - - case FlightMode.CIRCLE: - State = CopterState.Circling; - break; - - case FlightMode.GUIDED: - State = CopterState.CommandMode; - break; - - case FlightMode.LOITER: - State = CopterState.HoverMode; - break; - - case FlightMode.RTL: - State = CopterState.Returning; - break; - - case FlightMode.LAND: - State = CopterState.Landing; - break; - - default: - if (defaultState != null) State = defaultState.Value; - break; - } - } - } -} +using Plane.Geography; +using System; +using System.Collections.Generic; +using System.ComponentModel; +using System.Drawing; +using System.Threading; +using System.Threading.Tasks; + +namespace Plane.Copters +{ + public abstract partial class CopterImplSharedPart : PLObservableObject, ILocation + { + protected CopterState? _forcedState; + + protected bool _isMobileControlActive; + + protected float? _lastChangedAlt; + + protected ILocation _lastChangedLocation; + + protected DateTime _lastRaiseAltitudeChangedTime; + + protected DateTime _lastRaiseLocationChangedTime; + + protected bool _shouldFollow; + + protected DateTime? _takeOffTime; + + /// + /// 更新 的时间间隔。 + /// + private static readonly TimeSpan _intervalToUpdateFlightTimeSpan = TimeSpan.FromSeconds(0.5); + + private float _followAltDifference; + + private float _followDistance; + + private bool _followKeepFacingTarget; + + private bool _followKeep3DRelativeLocations; + + private bool _followKeepYawDifference; + + private ILocation _followLastDestination; + + private IVisibleStatus _followTarget; + + private float _followSelfDirectionFromTarget; + + private float _followTargetOriginalYaw; + + private float? _followYaw; + + private DateTime _lastUpdateStatusTextTime; + + private int _mobileControlIntervalMilli = 50; + + #region Backing Fields + + private float _AirSpeed; + + private float _Altitude; + + private byte _BatteryPer = 100; + + private ushort _Channel1; + + private ushort _Channel2; + + private ushort _Channel3; + + private ushort _Channel4; + + private ushort _Channel5; + + private ushort _Channel6; + + private ushort _Channel7; + + private ushort _Channel8; + + private ushort? _DesiredChannel1; + + private ushort? _DesiredChannel2; + + private ushort? _DesiredChannel3; + + private ushort? _DesiredChannel4; + + private ushort? _DesiredChannel5; + + private ushort? _DesiredChannel6; + + private ushort? _DesiredChannel7; + + private ushort? _DesiredChannel8; + + private float? _DesiredYaw; + + private float _Elevation; + + private int? _FirmwareVersion; + + private string _FirmwareVersionText; + + private double _FlightDistance; + + private double _FlightDistance2D; + private int _FlightControlMode; + + private TimeSpan _FlightTime; + + private GpsFixType _GpsFixType; + + private float _GpsHdop; + + private float _GroundSpeed; + + private bool _HasSwitchedToGpsMode; + + private short _Heading; + + private byte _CopterErrorCode=0; + + private bool _CopterPreCheckPass=true; + private String _CopterPreCheckPassStr; + + private String _CopterErrorString; + + private ulong _HeartbeatCount; + + private bool _IsAbsolutelyConnected; + + private bool _IsCheckingConnection; + + private bool _CommModuleConnected; + + private bool _IsConnected; + + private bool _IsEmergencyHoverActive; + + private bool _IsGpsAccurate; + + private bool _IsPairing; + + private bool _IsUnlocked; + + private double _Latitude; + + private double _Longitude; + + private FlightMode _Mode; + private FlightMode _CommModuleMode; + + private float _Pitch; + + private float _Roll; + + private byte _SatCount; + + private byte[] _Retain = new byte[4]; + + private float _GroundAlt = 0; + + private int _sim_update_int = 50; + + private CopterState _State; + + private string _StatusText; + + private ILocation _TakeOffPoint; + + private uint _TimebootMs; + + private float _Voltage; + + private float _Yaw; + + //初始时位置：使用地图Center + private double _RecordLat; + + private double _RecordLng; + + + private int _VirtualId; + + private bool _DisplayVirtualId = false; + + private bool _DisplayID = true; + + private float _maxspeed_xy=3; + private float _maxspeed_down=3; + private float _maxspeed_up = 3; + private float _acc_z=1f; + private float _acc_xy=2.5f; + + #endregion Backing Fields + + public CopterImplSharedPart(SynchronizationContext uiSyncContext) : base(uiSyncContext) + { + PropertyChanged += CopterImplSharedPart_PropertyChanged; + /* + Task.Run(async () => + { + while (true) + { + UpdateFlightTimeSpanIfNeeded(); + await Task.Delay(_intervalToUpdateFlightTimeSpan).ConfigureAwait(false); + } + }); + + Task.Run(async () => + { + while (true) + { + if (!_isMobileControlActive) + { + await EnsureChannelsSetAsync(); + } + if (_lastUpdateStatusTextTime.AddSeconds(2) <= DateTime.Now) + { + StatusText = null; + } + await Task.Delay(500); //50 + } + }); + + Task.Run(async () => + { + var lastHeartbeatCount = HeartbeatCount; + while (true) + { + if (!IsConnected || lastHeartbeatCount == HeartbeatCount) + { + // 如果 !IsConnected（连接通道断开），肯定失联了； + // 或者，过了一段时间，心跳数没变化，认为飞行器失联。 + IsAbsolutelyConnected = false; + + // 此处等待的时间较短，目的是在飞行器重新连上时快速知晓。 + await Task.Delay(500).ConfigureAwait(false); + } + else + { + // 过了一段时间，心跳数变化了，确认飞行器连接正常。 + IsAbsolutelyConnected = true; + + // 记录此时的心跳数。 + lastHeartbeatCount = HeartbeatCount; + + // 此处等待的时间较长，目的是提高失联判断的可信程度。 + await Task.Delay(5000).ConfigureAwait(false); + } + } + + }); + */ + } + + public event EventHandler AltitudeChanged; + + public event EventHandler AttitudeChanged; + +#if PRIVATE + public event EventHandler DataStreamReceived; +#endif + + public event EventHandler HeartbeatReceived; + + public event EventHandler LocationChanged; + + public event EventHandler MissionItemReceived; + + public event EventHandler PairingCompleted; + + public event EventHandler SensorDataReceived; + + public event EventHandler SystemStatusReceived; + + public event EventHandler TextReceived; + + + protected enum PDataStreamType + { + SYS_STATUS = 1, // MAVLINK_MSG_ID_SYS_STATUS, + GPS_RAW_INT = 24, // MAVLINK_MSG_ID_GPS_RAW_INT, + RC_CHANNELS_RAW = 35 // MAVLINK_MSG_ID_RC_CHANNELS_RAW + } + + private static readonly Dictionary ErrorIdToString = new Dictionary + { + {2, "气压计异常"}, + {3, "磁罗盘异常"}, + {4, "GPS异常"}, + {5, "惯性传感器异常"}, + {6, "参数异常"}, + {7, "遥控异常"}, + {8, "飞控电压低"}, + {9, "电池电压低"}, + {10, "空速传感器失败"}, + {11, "日志记录失败"}, + {12, "安全开关未开"}, + {13, "GPS配置异常"}, + {14, "系统异常"}, + {15, "任务异常"}, + {16, "测距传感器异常"}, + {17, "摄像头异常"}, + {18, "混控异常"}, + {19, "版本异常"}, + {20, "FFT异常"}, + {21, "陀螺仪无数据"}, + {22, "陀螺仪没校准"}, + {23, "加速计没数据"}, + {24, "加速计没校准"}, + {25, "加速计需要重启"}, + {26, "加速计不一致"}, + {27, "陀螺仪不一致"}, + + }; + public String getcoptererrorstr(byte errorcode) + { + String errorstr = "未知"; + ErrorIdToString.TryGetValue(errorcode, out errorstr); + String retstr = "["+errorcode.ToString()+"]" + errorstr; + return retstr; + } + public float AirSpeed + { + get { return _AirSpeed; } + protected set { Set(nameof(AirSpeed), ref _AirSpeed, value); } + } + + public float Altitude + { + get { return _Altitude; } + protected set { Set(nameof(Altitude), ref _Altitude, value); } + } + + public byte BatteryPer + { + get { return _BatteryPer; } + protected set { Set(nameof(BatteryPer), ref _BatteryPer, value); } + } + + public ushort Channel1 + { + get { return _Channel1; } + protected set { Set(nameof(Channel1), ref _Channel1, value); } + } + + public ushort Channel2 + { + get { return _Channel2; } + protected set { Set(nameof(Channel2), ref _Channel2, value); } + } + + public ushort Channel3 + { + get { return _Channel3; } + protected set { Set(nameof(Channel3), ref _Channel3, value); } + } + + public ushort Channel4 + { + get { return _Channel4; } + protected set { Set(nameof(Channel4), ref _Channel4, value); } + } + + public ushort Channel5 + { + get { return _Channel5; } + protected set { Set(nameof(Channel5), ref _Channel5, value); } + } + + public ushort Channel6 + { + get { return _Channel6; } + protected set { Set(nameof(Channel6), ref _Channel6, value); } + } + + public ushort Channel7 + { + get { return _Channel7; } + protected set { Set(nameof(Channel7), ref _Channel7, value); } + } + + public ushort Channel8 + { + get { return _Channel8; } + protected set { Set(nameof(Channel8), ref _Channel8, value); } + } + + public ushort? DesiredChannel1 + { + get { return _DesiredChannel1; } + set { if (!IsEmergencyHoverActive) { Set(nameof(DesiredChannel1), ref _DesiredChannel1, value); } } + } + + public ushort? DesiredChannel2 + { + get { return _DesiredChannel2; } + set { if (!IsEmergencyHoverActive) { Set(nameof(DesiredChannel2), ref _DesiredChannel2, value); } } + } + + public ushort? DesiredChannel3 + { + get { return _DesiredChannel3; } + set + { + // 王海, 20160317, 紧急悬停时可调节高度。 + //if (!IsEmergencyHoverActive) + //{ + if (Set(nameof(DesiredChannel3), ref _DesiredChannel3, value)) + { + if (State == CopterState.Initialized) SetStateAccordingToMode(Mode); + } + //} + } + } + + public ushort? DesiredChannel4 + { + get { return _DesiredChannel4; } + set { if (!IsEmergencyHoverActive) { Set(nameof(DesiredChannel4), ref _DesiredChannel4, value); } } + } + + public ushort? DesiredChannel5 + { + get { return _DesiredChannel5; } + set { if (!IsEmergencyHoverActive) { Set(nameof(DesiredChannel5), ref _DesiredChannel5, value); } } + } + + public ushort? DesiredChannel6 + { + get { return _DesiredChannel6; } + set { if (!IsEmergencyHoverActive) { Set(nameof(DesiredChannel6), ref _DesiredChannel6, value); } } + } + + public ushort? DesiredChannel7 + { + get { return _DesiredChannel7; } + set { if (!IsEmergencyHoverActive) { Set(nameof(DesiredChannel7), ref _DesiredChannel7, value); } } + } + + public ushort? DesiredChannel8 + { + get { return _DesiredChannel8; } + set { if (!IsEmergencyHoverActive) { Set(nameof(DesiredChannel8), ref _DesiredChannel8, value); } } + } + + public float? DesiredYaw + { + get { return _DesiredYaw; } + set { if (!IsEmergencyHoverActive && !(State == CopterState.Following && _followKeepFacingTarget)) { Set(nameof(DesiredYaw), ref _DesiredYaw, value); } } + } + + public float Elevation + { + get { return _Elevation; } + protected set { Set(nameof(Elevation), ref _Elevation, value); } + } + + public int VirtualId + { + get { return _VirtualId; } + set { Set(nameof(VirtualId), ref _VirtualId, value); RefreashLoc(); } + } + + public bool DisplayVirtualId + { + get { return _DisplayVirtualId; } + set { Set(nameof(DisplayVirtualId), ref _DisplayVirtualId, value); RefreashLoc(); } + } + + public bool DisplayID + { + get { return _DisplayID; } + set { Set(nameof(DisplayID), ref _DisplayID, value); RefreashLoc(); } + } + + + + /// + /// 获取固件版本。 + /// + public int? FirmwareVersion + { + get { return _FirmwareVersion; } + protected set + { + if (Set(nameof(FirmwareVersion), ref _FirmwareVersion, value)) + { + if (value == null) + { + FirmwareVersionText = null; + } + else + { + int major, minor, build; + int temp; +#if NETFX_CORE + major = value.Value / 0x1000; + temp = value.Value % 0x1000; + minor = temp / 0x100; + build = temp % 0x100; +#else + major = Math.DivRem(value.Value, 0x1000, out temp); + minor = Math.DivRem(temp, 0x100, out build); +#endif + FirmwareVersionText = $"{major}.{minor}.{build}"; + } + } + } + } + + /// + /// 获取固件版本的文本形式。 + /// + public string FirmwareVersionText + { + get { return _FirmwareVersionText; } + protected set { Set(nameof(FirmwareVersionText), ref _FirmwareVersionText, value); } + } + + public double FlightDistance + { + get { return _FlightDistance; } + protected set { Set(nameof(FlightDistance), ref _FlightDistance, value); } + } + + public double FlightDistance2D + { + get { return _FlightDistance2D; } + protected set { Set(nameof(FlightDistance2D), ref _FlightDistance2D, value); } + } + + //FlightControlMode=1表示同时到达，速度可变,用于新固件 + //=0表示固定速度，用于老固件 + public int FlightControlMode + { + get { return _FlightControlMode; } + set { Set(nameof(FlightControlMode), ref _FlightControlMode, value); } + } + + + public TimeSpan FlightTimeSpan + { + get { return _FlightTime; } + protected set { Set(nameof(FlightTimeSpan), ref _FlightTime, value); } + } + + /// + /// 获取 GPS 锁定状态。 + /// + public GpsFixType GpsFixType + { + get { return _GpsFixType; } + protected set { Set(nameof(GpsFixType), ref _GpsFixType, value); } + } + + /// + /// 获取 GPS HDOP（horizontal dilution of position），单位为米。 + /// + public float GpsHdop + { + get { return _GpsHdop; } + protected set { Set(nameof(GpsHdop), ref _GpsHdop, value); } + } + + /// + /// 飞机回传的错误码，可知道飞机当前状态，比如是否加速计不一致，是否磁罗盘不一致等，是黄灯的具体代码。 + /// + public byte CopterErrorCode + { + get { return _CopterErrorCode; } + protected set { Set(nameof(CopterErrorCode), ref _CopterErrorCode, value); } + } + + /// + /// 飞机预检查是否通过。 + /// + public bool CopterPreCheckPass + { + get { return _CopterPreCheckPass; } + protected set { + Set(nameof(CopterPreCheckPass), ref _CopterPreCheckPass, value); + if (_CopterPreCheckPass) + CopterPreCheckPassStr= "通过"; + else + CopterPreCheckPassStr= "异常"; + + + } + } + + /// + /// 飞机预检查是否通过。 + /// + public string CopterPreCheckPassStr + { + get + { + return _CopterPreCheckPassStr; + } + protected set { Set(nameof(CopterPreCheckPassStr), ref _CopterPreCheckPassStr, value); } + + } + + + + /// + /// 飞机回传的最后错误码转换的字符串，可知道飞机当前状态，比如是否加速计不一致，是否磁罗盘不一致等，是黄灯的具体代码。 + /// + public String CopterErrorString + { + get { return _CopterErrorString; } + protected set { Set(nameof(CopterErrorString), ref _CopterErrorString, value); } + } + + public float GroundSpeed + { + get { return _GroundSpeed; } + protected set { Set(nameof(GroundSpeed), ref _GroundSpeed, value); } + } + + public bool HasSwitchedToGpsMode + { + get { return _HasSwitchedToGpsMode; } + protected set { Set(nameof(HasSwitchedToGpsMode), ref _HasSwitchedToGpsMode, value); } + } + + public short Heading + { + get { return _Heading; } + protected set { Set(nameof(Heading), ref _Heading, value); } + } + + public ulong HeartbeatCount + { + get { return _HeartbeatCount; } + protected set { Set(nameof(HeartbeatCount), ref _HeartbeatCount, value); } + } + + /// + /// 获取一个值，指示是否确定飞行器与地面站正常连接。在实现中，一般根据对心跳包的接收情况判断。 + /// + public bool IsAbsolutelyConnected + { + get { return _IsAbsolutelyConnected; } + protected set { Set(nameof(IsAbsolutelyConnected), ref _IsAbsolutelyConnected, value); } + } + + /// + /// 获取或设置一个值，指示是否正在检查飞行器与地面站的连接以确定通信正常。在实现中，一般在建立连接后设为 true，在收到飞行器发回的数据后设为 false。 + /// + public bool IsCheckingConnection + { + get { return _IsCheckingConnection; } + set { Set(nameof(IsCheckingConnection), ref _IsCheckingConnection, value); } + } + + public bool IsConnected + { + get { return _IsConnected; } + 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; } + protected set { Set(nameof(IsEmergencyHoverActive), ref _IsEmergencyHoverActive, value); } + } + + /// + /// 获取一个值，指示当前的 GPS 定位是否足够精确。 + /// + public bool IsGpsAccurate + { + get { return _IsGpsAccurate; } + protected set { Set(nameof(IsGpsAccurate), ref _IsGpsAccurate, value); } + } + + public bool IsPairing + { + get { return _IsPairing; } + protected set { Set(nameof(IsPairing), ref _IsPairing, value); } + } + + public bool IsUnlocked + { + get { return _IsUnlocked; } + protected set + { + if (Set(nameof(IsUnlocked), ref _IsUnlocked, value)) + { + if (value) + { + State = CopterState.Initialized; + } + else + { + State = CopterState.Locked; + } + } + } + } + + public double Latitude + { + get { return _Latitude; } + protected set { Set(nameof(Latitude), ref _Latitude, value); } + } + + public double Longitude + { + get { return _Longitude; } + protected set { Set(nameof(Longitude), ref _Longitude, value); } + } + + public double RecordLat + { + get { return _RecordLat; } + set { Set(nameof(RecordLat), ref _RecordLat, value); } + } + + public double RecordLng + { + get { return _RecordLng; } + set { Set(nameof(RecordLng), ref _RecordLng, value); } + } + + /// + /// 获取任务总数。 + /// + public ushort? MissionCount { get; protected set; } + + public string Name { get; set; } + + public float Pitch + { + get { return _Pitch; } + protected set { Set(nameof(Pitch), ref _Pitch, value); } + } + + public float Roll + { + get { return _Roll; } + protected set { Set(nameof(Roll), ref _Roll, value); } + } + + public byte SatCount + { + get { return _SatCount; } + protected set { Set(nameof(SatCount), ref _SatCount, value); } + } + + public float RetainInt + { + get { return BitConverter.ToSingle(Retain,0); } + } + + public float GroundAlt + { + get { return _GroundAlt; } + set { + Set(nameof(GroundAlt), ref _GroundAlt, value); + RaisePropertyChanged(nameof(GroundAlt)); + } + } + + + public int sim_update_int + { + get { return _sim_update_int; } + set + { + Set(nameof(sim_update_int), ref _sim_update_int, value); + } + } + + + public float maxspeed_xy { + get { return _maxspeed_xy; } + set + { + Set(nameof(maxspeed_xy), ref _maxspeed_xy, value); + } + } + + public float maxspeed_up + { + get { return _maxspeed_up; } + set + { + Set(nameof(maxspeed_up), ref _maxspeed_up, value); + } + } + public float maxspeed_down + { + get { return _maxspeed_down; } + set + { + Set(nameof(maxspeed_down), ref _maxspeed_down, value); + } + } + public float acc_z + { + get { return _acc_z; } + set + { + Set(nameof(acc_z), ref _acc_z, value); + } + } + public float acc_xy + { + get { return _acc_xy; } + set + { + Set(nameof(acc_xy), ref _acc_xy, value); + } + } + + + + public byte[] Retain + { + get { return _Retain; } + protected set + { + Set(nameof(Retain), ref _Retain, value); + RaisePropertyChanged(nameof(RetainInt)); + } + } + + public CopterState State + { + get { return _State; } + protected set { Set(nameof(State), ref _State, _forcedState ?? value); } + } + + public string StatusText + { + get { return _StatusText; } + protected set + { + if (Set(nameof(StatusText), ref _StatusText, value)) + { + _lastUpdateStatusTextTime = DateTime.Now; + } + } + } + + public ILocation TakeOffPoint + { + get { return _TakeOffPoint; } + protected set { Set(nameof(TakeOffPoint), ref _TakeOffPoint, value); } + } + + public float Voltage + { + get { return _Voltage; } + protected set { Set(nameof(Voltage), ref _Voltage, value); } + } + + public float Yaw + { + get { return _Yaw; } + protected set { Set(nameof(Yaw), ref _Yaw, value); } + } + + public FlightMode CommModuleMode + { + get { return _CommModuleMode; } + protected set { Set(nameof(CommModuleMode), ref _CommModuleMode, value); } + } + + private byte _CommModuleVersion; + public byte CommModuleVersion + { + get { return _CommModuleVersion; } + protected set { Set(nameof(CommModuleVersion), ref _CommModuleVersion, value); } + } + + private string _LEDColor; + public string LEDColor + { + get { return _LEDColor; } + set { + if (value!=_LEDColor) + { + Set(nameof(LEDColor), ref _LEDColor, value); + //强制刷新颜色--在刷新位置时才刷新颜色 + RefreashLoc(); + + } + } + } + + private Color _LEDShowColor; + public Color LEDShowColor + { + get { return _LEDShowColor; } + set + { + + Set(nameof(LEDShowColor), ref _LEDShowColor, value); + + } + } + + + private int _LEDMode; + public int LEDMode + { + get { return _LEDMode; } + set + { + if (value != _LEDMode) + { + Set(nameof(LEDMode), ref _LEDMode, value); + //强制刷新颜色--在刷新位置时才刷新颜色 + // RefreashLoc(); + } + } + } + private float _LEDInterval; + public float LEDInterval + { + get { return _LEDInterval; } + set + { + if (value != _LEDInterval) + { + Set(nameof(LEDInterval), ref _LEDInterval, value); + //强制刷新颜色--在刷新位置时才刷新颜色 + // RefreashLoc(); + } + } + } + + +#if PRIVATE + public +#else + internal +#endif + FlightMode Mode + { + get { return _Mode; } + set + { + // 王海, 20160325, Mode 属性改成 internal 了，如果仍然用 Set 或者 RaisePropertyChanged 方法，会抛异常（"Property not found"）。现决定不引发 PropertyChanged 事件了。 + var changed = _Mode != value; + if (changed) _Mode = value; + + // 如果在已解锁状态下满足某些条件，就根据模式设置 State。 + if (IsUnlocked) + { + var shouldSetState = false; + if (changed) + { + if (State == CopterState.Following) + { + if (value != FlightMode.GUIDED) + { + // 跟随时切换到其他模式，停止跟随并且设置 State。 + _shouldFollow = false; + shouldSetState = true; + } + } + else + { + // 非跟随时模式变化了，设置 State。 + shouldSetState = true; + } + } + if (shouldSetState) SetStateAccordingToMode(value, defaultState: CopterState.Initialized); + } + } + } + + public async Task FloatAsync() + { + if (await SetModeAsync(FlightMode.ALT_HOLD).ConfigureAwait(false)) + { + await SetChannelsAsync( + ch1: 1500, + ch2: 1500, + ch3: 1500, + ch4: 1500 + ).ConfigureAwait(false); + } + } + + /// + /// 使飞行器飞往水平面上指定的点。 + /// + /// 纬度。 + /// 经度。 + /// 表示此命令异步发送操作的 实例。 + public Task FlyToAsync(double lat, double lng) + { + return FlyToAsync(lat, lng, Altitude); + } + + public Task FlyToAsync(double lat, double lng, float alt, float flytime = 0, int flytype = 0) + { + _shouldFollow = false; + //State = CopterState.CommandMode; + return FlyToCoreAsync(lat, lng, alt, flytime, flytype); + } + + public void Follow(IVisibleStatus target, bool keepYawDifference = false, bool keepFacingTarget = true, bool keep3DRelativeLocations = false) + { + + } + + public async Task HoverAsync() + { + if (await SetModeAsync(FlightMode.LOITER).ConfigureAwait(false)) + { + await SetChannelsAsync( + ch1: 1500, + ch2: 1500, + ch3: 1500, + ch4: 1500 + ).ConfigureAwait(false); + } + } + public async Task GuidAsync() + { + if (await SetModeAsync(FlightMode.GUIDED).ConfigureAwait(false)) + { + await SetChannelsAsync( + ch1: 1500, + ch2: 1500, + ch3: 1500, + ch4: 1500 + ).ConfigureAwait(false); + } + } + + public async Task LandAsync() + { + await SetModeAsync(FlightMode.LAND).ConfigureAwait(false); + await SetChannelsAsync(ch1: 1500, ch2: 1500).ConfigureAwait(false); + } + + public async Task ReturnToLaunchAsync() + { + await SetModeAsync(FlightMode.RTL).ConfigureAwait(false); + await SetChannelsAsync(ch1: 1500, ch2: 1500).ConfigureAwait(false); + } + + public async Task SetChannelsAsync(ushort? ch1 = null, ushort? ch2 = null, ushort? ch3 = null, ushort? ch4 = null, ushort? ch5 = null, ushort? ch6 = null, ushort? ch7 = null, ushort? ch8 = null) + { + SetTargets(ch1, ch2, ch3, ch4, ch5, ch6, ch7, ch8); + + await SetChannelsAsync().ConfigureAwait(false); + } + + public abstract Task SetChannelsAsync(); + + public async Task SetMobileControlAsync(ushort? ch1 = null, ushort? ch2 = null, ushort? ch3 = null, ushort? ch4 = null, ushort? ch5 = null, ushort? ch6 = null, ushort? ch7 = null, ushort? ch8 = null, float? yaw = null) // 王海, 20150912, 将来如有需要再补上 TargetAlt, float? alt = null) + { + SetTargets(ch1, ch2, ch3, ch4, ch5, ch6, ch7, ch8, yaw); + + await SetMobileControlAsync().ConfigureAwait(false); + } + + public abstract Task SetMobileControlAsync(); + + public async Task StartEmergencyHoverAsync() + { + _shouldFollow = false; + State = CopterState.HoverMode; + IsEmergencyHoverActive = true; + + // 王海, 20151028, 先 IsEmergencyHoverActive = true 再设置目标通道是为了防止别的线程正好在 IsEmergencyHoverActive == false 时设置通道。 + // 王海, 20151019, 在紧急悬停状态，外部设置 DesiredChannel[1-4] 无效，只能如此设置。 + SetFieldAndRaisePropertyChanged(ref _DesiredChannel1, 1500, nameof(DesiredChannel1)); + SetFieldAndRaisePropertyChanged(ref _DesiredChannel2, 1500, nameof(DesiredChannel2)); + SetFieldAndRaisePropertyChanged(ref _DesiredChannel3, 1500, nameof(DesiredChannel3)); + SetFieldAndRaisePropertyChanged(ref _DesiredChannel4, 1500, nameof(DesiredChannel4)); + + await SetMobileControlAsync().ConfigureAwait(false); + + switch (Mode) + { + // 王海, 20151019, 波子说一律切 LOITER。 + //case FlightMode.ALT_HOLD: + // break; + //case FlightMode.LOITER: + // break; + //case FlightMode.STABILIZE: + // await SetModeCoreAsync(FlightMode.ALT_HOLD).ConfigureAwait(false); + // break; + //case FlightMode.AUTO: + //case FlightMode.GUIDED: + //case FlightMode.RTL: + //case FlightMode.CIRCLE: + //case FlightMode.LAND: + default: + await SetModeCoreAsync(FlightMode.LOITER).ConfigureAwait(false); + break; + } + } + + public async void StartMobileControl(int? millisecondsInterval = null) + { + if (millisecondsInterval != null) _mobileControlIntervalMilli = millisecondsInterval.Value; + if (!_isMobileControlActive) + { + _isMobileControlActive = true; + while (_isMobileControlActive) + { + if (State == CopterState.Following) + { + await SetMobileControlAsync(ch1: 1500, ch2: 1500).ConfigureAwait(false); + } + else { + switch (Mode) + { + case FlightMode.STABILIZE: + case FlightMode.ALT_HOLD: + case FlightMode.LOITER: + await SetMobileControlAsync().ConfigureAwait(false); + break; + + case FlightMode.GUIDED: + case FlightMode.RTL: + await SetMobileControlAsync(ch1: 1500, ch2: 1500, ch3: 1500).ConfigureAwait(false); + break; + + case FlightMode.LAND: + await SetMobileControlAsync(ch3: 1500); + break; + + default: + await SetMobileControlAsync(ch1: 1500, ch2: 1500, yaw: Yaw).ConfigureAwait(false); + break; + } + } + await Task.Delay(_mobileControlIntervalMilli).ConfigureAwait(false); + } + } + } + + public void StopEmergencyHover() + { + IsEmergencyHoverActive = false; + } + + public void StopMobileControl() + { + _isMobileControlActive = false; + } + + public async Task TakeOffAsync() + { + + + /* await SetChannelsAsync( + ch1: 1500, + ch2: 1500, + ch3: 1500, + ch4: 1500 + ).ConfigureAwait(false); + _forcedState = CopterState.TakingOff; + State = CopterState.TakingOff; + await DoCommandAsync(MAVLink.MAV_CMD.TAKEOFF, 0, 0, 0, 0, 0, 0, alt).ConfigureAwait(false); + var temp = Task.Run(async () => + { + var startTime = DateTime.Now; + var secondsTimeout = alt / 0.7; + while (currentTakeOffCount == _takeOffCount) + { + if (alt - Altitude < 0.2 || startTime.AddSeconds(secondsTimeout) <= DateTime.Now) + { + _forcedState = null; + await HoverAsync().ConfigureAwait(false); + break; + } + await Task.Delay(50).ConfigureAwait(false); + } + }); +*/ + + + + + + + + + if (await SetModeAsync(FlightMode.AUTO).ConfigureAwait(false)) + { + await SetChannelsAsync( + ch1: 1500, + ch2: 1500, + ch3: 1500, + ch4: 1500 + ).ConfigureAwait(false); + } + + } + + /// + /// 切换飞行模式，返回的 实例在确认切换成功或超时后完成。 + /// + /// 飞行模式。 + /// 超时时间，单位为毫秒。 + /// 表示切换模式操作的 实例。若切换模式成功，异步操作的结果为 true；否则为 false。 +#if PRIVATE + public +#else + internal +#endif + async Task SetModeAsync(FlightMode mode, int millisecondsTimeout = 5000) + { + if (_shouldFollow && mode != FlightMode.GUIDED) _shouldFollow = false; + // 王海, 20160317, 紧急悬停时可返航或降落。 + if (mode == FlightMode.RTL || mode == FlightMode.LAND) + { + StopEmergencyHover(); + } + if (!IsEmergencyHoverActive) + { + return await SetModeCoreAsync(mode, millisecondsTimeout).ConfigureAwait(false); + } + return false; + } + + /// + /// 无论在不在紧急悬停状态下都可用的切换模式方法。 + /// + /// 模式。 + /// 超时。 + /// 成功与否。 + internal abstract Task SetModeCoreAsync(FlightMode mode, int millisecondsTimeout = 5000); + + protected async Task EnsureChannelsSetAsync() + { + if ( + (DesiredChannel1 != null && Channel1 != DesiredChannel1) || + (DesiredChannel2 != null && Channel2 != DesiredChannel2) || + (DesiredChannel3 != null && Channel3 != DesiredChannel3) || + (DesiredChannel4 != null && Channel4 != DesiredChannel4) || + (DesiredChannel5 != null && Channel5 != DesiredChannel5) || + (DesiredChannel6 != null && Channel6 != DesiredChannel6) || + (DesiredChannel7 != null && Channel7 != DesiredChannel7) || + (DesiredChannel8 != null && Channel8 != DesiredChannel8) + ) // Any channel not set. + { + await SetChannelsAsync().ConfigureAwait(false); + } + } + + /// + /// 不主动设置 State，仅发送飞往某点命令。 + /// + /// 纬度。 + /// 经度。 + /// 高度。 + /// 表示此异步发送操作的 实例。 + protected abstract Task FlyToCoreAsync(double lat, double lng, float alt, float flytime = 0, int flytype = 0); + + protected void RaiseAltitudeChanged() => AltitudeChanged?.Invoke(this, EventArgs.Empty); + + protected void RaiseAltitudeChangedIfNeeded() + { + if (_lastChangedAlt == null || Math.Abs(Altitude - _lastChangedAlt.Value) >= 0.5 || DateTime.Now.AddMilliseconds(-500) >= _lastRaiseAltitudeChangedTime) + { + RaiseAltitudeChanged(); + _lastChangedAlt = Altitude; + _lastRaiseAltitudeChangedTime = DateTime.Now; + } + } + + protected void RaiseAttitudeChanged() => AttitudeChanged?.Invoke(this, EventArgs.Empty); + + protected void RaiseDataStreamReceived(PDataStreamType dataStreamType) + { +#if PRIVATE + DataStreamReceived?.Invoke(this, new DataStreamReceivedEventArgs((DataStreamType)dataStreamType)); +#endif + } + + protected void RaiseHeartbeatReceived(HeartbeatReceivedEventArgs e) + { + HeartbeatReceived?.Invoke(this, e); + } + + protected void RaiseLocationChanged() => LocationChanged?.Invoke(this, EventArgs.Empty); + + public void RefreashLoc() + { + RaiseLocationChangedIfNeeded(true); + } + + protected void RaiseLocationChangedIfNeeded(bool forcemk=false) + { + //强制刷新位置 + if (forcemk) + { + RaiseLocationChanged(); + return; + } + //减少计算量，在模拟飞机很多时花时间 + bool EnRaise = true; + if (_lastChangedLocation != null) + EnRaise = (Altitude != _lastChangedLocation.Altitude) || + (Latitude != _lastChangedLocation.Latitude) || + (Longitude != _lastChangedLocation.Longitude); + //if (_lastChangedLocation == null || this.CalcDistance(_lastChangedLocation) >= 0.5 || DateTime.Now.AddMilliseconds(-500) >= _lastRaiseLocationChangedTime) + if (EnRaise) + { + RaiseLocationChanged(); + _lastChangedLocation = new PLLocation(this); + _lastRaiseLocationChangedTime = DateTime.Now; + } + + } + + protected void RaiseMissionItemReceived(MissionItemReceivedEventArgs e) + { + MissionItemReceived?.Invoke(this, e); + } + + protected void RaisePairingCompleted(PairingCompletedEventArgs e) + { + var handler = PairingCompleted; + if (handler == null) return; + if (SynchronizationContext.Current == _uiSyncContext) + { + handler(this, e); + } + else + { + _uiSyncContext.Post(() => handler(this, e)); + } + } + + protected void RaiseSensorDataReceived(EventArgs e) + { + SensorDataReceived?.Invoke(this, e); + } + + protected void RaiseSysStatusReceived(SystemStatusReceivedEventArgs e) + { + SystemStatusReceived?.Invoke(this, e); + } + + + protected void RaiseTextReceived(MessageCreatedEventArgs e) + { + TextReceived?.Invoke(this, e); + } + + + + protected void SetTargets(ushort? ch1 = null, ushort? ch2 = null, ushort? ch3 = null, ushort? ch4 = null, ushort? ch5 = null, ushort? ch6 = null, ushort? ch7 = null, ushort? ch8 = null, float? yaw = null) + { + if (ch1 != null) DesiredChannel1 = ch1; + if (ch2 != null) DesiredChannel2 = ch2; + if (ch3 != null) DesiredChannel3 = ch3; + if (ch4 != null) DesiredChannel4 = ch4; + if (ch5 != null) DesiredChannel5 = ch5; + if (ch6 != null) DesiredChannel6 = ch6; + if (ch7 != null) DesiredChannel7 = ch7; + if (ch8 != null) DesiredChannel8 = ch8; + if (yaw != null) DesiredYaw = yaw; + } + + protected virtual void UpdateFlightDataIfNeeded() + { + if (!TakeOffPoint.IsNullOrEmpty()) + { + FlightDistance = TakeOffPoint.CalcDistance(this); + } + } + + protected void UpdateFlightTimeSpanIfNeeded() + { + if (_takeOffTime != null && IsUnlocked) + { + var flightTimeSpan = DateTime.Now - _takeOffTime.Value; + if (flightTimeSpan - FlightTimeSpan >= _intervalToUpdateFlightTimeSpan) + { + FlightTimeSpan = flightTimeSpan; + } + } + } + + private void CopterImplSharedPart_PropertyChanged(object sender, PropertyChangedEventArgs e) + { + switch (e.PropertyName) + { + case nameof(IsConnected): + if (!IsConnected) + { + IsCheckingConnection = false; + IsAbsolutelyConnected = false; + } + break; + + case nameof(IsUnlocked): + if (IsUnlocked) + { + // 王海, 20151029, 在解锁时重置起飞点、起飞时间、飞行距离、飞行时间、速度。 + TakeOffPoint = new PLLocation { Latitude = Latitude, Longitude = Longitude, Altitude = Altitude }; + _takeOffTime = DateTime.Now; + FlightDistance = 0; + FlightDistance2D = 0; + FlightTimeSpan = TimeSpan.FromSeconds(0); + AirSpeed = 0; + GroundSpeed = 0; + } + else + { + if (IsEmergencyHoverActive) StopEmergencyHover(); + } + break; + + default: + break; + } + } + + private void SetFieldAndRaisePropertyChanged(ref ushort? field, ushort value, string propertyName) + { + if (field != value) + { + field = value; + RaisePropertyChanged(propertyName); + } + } + + private void SetFieldAndRaisePropertyChanged(ref float? field, float value, string propertyName) + { + if (field != value) + { + field = value; + RaisePropertyChanged(propertyName); + } + } + + private void SetStateAccordingToMode(FlightMode value, CopterState? defaultState = null) + { + switch (value) + { + case FlightMode.ALT_HOLD: + State = CopterState.FloatMode; + break; + + case FlightMode.AUTO: + State = CopterState.TakingOff; + break; + + case FlightMode.CIRCLE: + State = CopterState.Circling; + break; + + case FlightMode.GUIDED: + State = CopterState.CommandMode; + break; + + case FlightMode.LOITER: + State = CopterState.HoverMode; + break; + + case FlightMode.RTL: + State = CopterState.Returning; + break; + + case FlightMode.LAND: + State = CopterState.Landing; + break; + + default: + if (defaultState != null) State = defaultState.Value; + break; + } + } + } +} diff --git a/PlaneGcsSdk_Shared/Copters/EHCopter.InternalCopterEventHandlers.cs b/PlaneGcsSdk_Shared/Copters/EHCopter.InternalCopterEventHandlers.cs index f00a989..a676db5 100644 --- a/PlaneGcsSdk_Shared/Copters/EHCopter.InternalCopterEventHandlers.cs +++ b/PlaneGcsSdk_Shared/Copters/EHCopter.InternalCopterEventHandlers.cs @@ -1,309 +1,317 @@ -using System; -using static Plane.Protocols.MAVLink; - -namespace Plane.Copters -{ - public partial class PLCopter : CopterImplSharedPart - { - private bool _fetchingFirmwareVersion; - - private void _internalCopter_ConnectionBroken(object sender, EventArgs e) - { - IsConnected = _internalCopter.IsConnected; - } - - private void _internalCopter_GetLogDataEvent(string log) - { - //飞机消息日志，后面需要改为记录方式 - StatusText =Name+":"+log; - var e = new MessageCreatedEventArgs { Message = StatusText }; - RaiseTextReceived(e); - } - - private void _internalCopter_ReceiveDataStreamEvent(object sender, byte StreamType) - { - switch (StreamType) - { - case MAVLINK_MSG_ID_GPS_RAW_INT: //需要 - { - SatCount = _internalCopter.satcount; - GpsFixType = _internalCopter.gpsstatus.ToGpsFixType(); - GpsHdop = _internalCopter.gpshdop; - Altitude = _internalCopter.gpsalt; - Retain = BitConverter.GetBytes(_internalCopter.retain); - - Voltage = _internalCopter.battery_voltage; - CommModuleMode = (FlightMode)_internalCopter.commModuleMode; - CommModuleVersion = _internalCopter.commModuleVersion; - IsUnlocked = _internalCopter.isUnlocked; - //Yaw = _internalCopter.yaw; - Heading = _internalCopter.heading; - HeartbeatCount++; - - if (SatCount > 8) - { - IsGpsAccurate = true; - RecordLat = _internalCopter.lat; - RecordLng = _internalCopter.lng; - } - - Latitude = RecordLat; - Longitude = RecordLng; - UpdateFlightDataIfNeeded(); - RaiseLocationChangedIfNeeded(); - - RaiseDataStreamReceived(PDataStreamType.GPS_RAW_INT); - - break; - } - case MAVLINK_MSG_ID_ATTITUDE: //无用 - { - Roll = _internalCopter.roll; - Pitch = _internalCopter.pitch; - Yaw = _internalCopter.yaw; - TimebootMs = _internalCopter.timebootms; - RaiseAttitudeChanged(); - - break; - } - case MAVLINK_MSG_ID_RC_CHANNELS_RAW: //只用于显示,可以不要 - { - Channel1 = _internalCopter.ch1in; - Channel2 = _internalCopter.ch2in; - Channel3 = _internalCopter.ch3in; - Channel4 = _internalCopter.ch4in; - Channel5 = _internalCopter.ch5in; - Channel6 = _internalCopter.ch6in; - Channel7 = _internalCopter.ch7in; - Channel8 = _internalCopter.ch8in; - - RaiseDataStreamReceived(PDataStreamType.RC_CHANNELS_RAW); - - break; - } - case MAVLINK_MSG_ID_VFR_HUD: - { - Heading = _internalCopter.heading; //有用 - Altitude = _internalCopter.alt; //有用 - AirSpeed = _internalCopter.airspeed; //没用 - GroundSpeed = _internalCopter.groundspeed; //没用 - - RaiseAltitudeChangedIfNeeded(); - - break; - } - } - } - - private async void _internalCopter_ReceiveHeartBearEvent(object sender, uint iError, ulong iCount) - { - IsUnlocked = _internalCopter.armed; - Mode = (FlightMode)_internalCopter.mode; - // 王海, 20160311, 将闲置的 apname 用作已经切过 GPS 模式的标志。 - HasSwitchedToGpsMode = (byte)_internalCopter.apname != 0; - ++HeartbeatCount; - - IsCheckingConnection = false; - /* - if (FirmwareVersion == null && !_fetchingFirmwareVersion) - { - try - { - _fetchingFirmwareVersion = true; - FirmwareVersion = (int)await GetParamAsync("FIRMWARE_VERSION", 5000); - } - catch (TimeoutException) - { - // 吞掉。 - } - finally - { - _fetchingFirmwareVersion = false; - } - } - */ - RaiseHeartbeatReceived(new HeartbeatReceivedEventArgs(iCount)); - } - - private void _internalCopter_ReceiveSensorEvent(object sender) - { - RaiseSensorDataReceived(EventArgs.Empty); - } - - private void _internalCopter_ReceiveSysStatusEvent(object sender, byte StreamType, bool GPSBadHealth, bool GyroBadHealth, bool AccelBadHealth, bool CompassBadHealth, bool BarometerBadHealth, bool canTakeOff) - { - Voltage = _internalCopter.battery_voltage / 1000; - - if (_internalCopter.battery_remaining == 255) // 剩余电量未知，需要计算。 - { - CalcBatteryPer(); - } - else - { - BatteryPer = _internalCopter.battery_remaining; - } - - const int MIN_VERSION_WITH_CAN_TAKE_OFF_FIELD = 12308; - const int MIN_VERSION_OF_GENERATION2 = 0x3000; - if (FirmwareVersion != null) - { - if (FirmwareVersion.Value >= MIN_VERSION_WITH_CAN_TAKE_OFF_FIELD) - { - // 飞控提供了 canTakeOff。 - IsGpsAccurate = canTakeOff; - } - else if (FirmwareVersion.Value >= MIN_VERSION_OF_GENERATION2) - { - // 2.0 飞行器，飞控未提供 canTakeOff。 - IsGpsAccurate = SatCount >= 12; - } - else - { - // 1.0 飞行器，飞控不提供 canTakeOff。 - IsGpsAccurate = SatCount >= 6; - } - } - else - IsGpsAccurate = SatCount >= 8; - - FlightDistance2D = _internalCopter.FlightDistance2D; - - RaiseSysStatusReceived(new SystemStatusReceivedEventArgs(GPSBadHealth, GyroBadHealth, AccelBadHealth, CompassBadHealth, BarometerBadHealth)); - } - - private void _internalCopter_PacketHandled(object sender, PlaneCopter.PacketReceivedEventArgs e) - { - switch (e.Packet[5]) - { - case MAVLINK_MSG_ID_MISSION_COUNT: - MissionCount = _internalCopter.WpCount; - break; - - default: - break; - } - } - - private void _internalCopter_UnhandledPacketReceived(object sender, PlaneCopter.PacketReceivedEventArgs e) - { - switch (e.Packet[5]) - { - case MAVLINK_MSG_ID_MISSION_ACK: - AnalyzeMissionAckPacket(e.Packet); - break; - - case MAVLINK_MSG_ID_MISSION_COUNT: - AnalyzeMissionCountPacket(e.Packet); - break; - - case MAVLINK_MSG_ID_MISSION_ITEM: - AnalyzeMissionItemPacket(e.Packet); - break; - - case MAVLINK_MSG_ID_MISSION_REQUEST: - AnalyzeMissionRequestPacket(e.Packet); - break; - - case MAVLINK_MSG_ID_SET_PAIR: - AnalyzeSetPairPacket(e.Packet); - break; - - default: - break; - } - } - -#if PRIVATE - protected virtual void RegisterInternalCopterEventHandlers() -#else - - private void RegisterInternalCopterEventHandlers() -#endif - { - _internalCopter.GetLogDataEvent += _internalCopter_GetLogDataEvent; - _internalCopter.ReceiveDataStreamEvent += _internalCopter_ReceiveDataStreamEvent; - _internalCopter.ReceiveHeartBearEvent += _internalCopter_ReceiveHeartBearEvent; - _internalCopter.ReceiveSensorEvent += _internalCopter_ReceiveSensorEvent; - _internalCopter.ReceiveSysStatusEvent += _internalCopter_ReceiveSysStatusEvent; - _internalCopter.ConnectionBroken += _internalCopter_ConnectionBroken; - _internalCopter.PacketHandled += _internalCopter_PacketHandled; - _internalCopter.UnhandledPacketReceived += _internalCopter_UnhandledPacketReceived; - _internalCopter.MessageCreated += _internalCopter_MessageCreated; - } - - private void _internalCopter_MessageCreated(object sender, MessageCreatedEventArgs e) - { - var txte = new MessageCreatedEventArgs { Message = Name + ":" +e.Message }; - RaiseTextReceived(txte); - } - - #region 计算剩余电量 - - private int bPerTimes; - private int outBatteryPer; - private int[] tPerTimes = new int[20]; - private int v_num; - - /// - /// 计算电量并在条件满足时更新 属性。 - /// - private void CalcBatteryPer() - { - float volmax = 0f; - float volmin = 0f; - if (Voltage > 5 && Voltage < 9) - { - volmax = 8.2f; - volmin = 7f; - } - else if (Voltage >= 9 - && Voltage < 13.6) - { - volmax = 11.6f; - volmin = 10.2f; - } - else if (Voltage >= 13.6 - && Voltage < 17.2) - { - volmax = 16.3f; - volmin = 14.2f; - } - else if (Voltage >= 17.2 - && Voltage < 26.2) - { - volmax = 24.8f; - volmin = 21.2f; - } - - int batteryPer = (int)Math.Round((Voltage - volmin) * 100 / (volmax - volmin)); - - if (batteryPer == -1 || volmax == 0 || volmin == 0) - return; - - if (bPerTimes < 20) - { - tPerTimes[bPerTimes] = batteryPer; - bPerTimes += 1; - } - else - { - tPerTimes[v_num] = batteryPer; - v_num++; - if (v_num == 20) - v_num = 0; - } - for (int i = 0; i < bPerTimes; i++) - { - outBatteryPer += tPerTimes[i]; - } - - outBatteryPer = outBatteryPer / bPerTimes; - if (outBatteryPer < BatteryPer && bPerTimes > 18) - { - BatteryPer = (byte)outBatteryPer; - } - } - - #endregion 计算剩余电量 - } -} +using System; +using System.Collections.Generic; +using static Plane.Protocols.MAVLink; + +namespace Plane.Copters +{ + + + public partial class PLCopter : CopterImplSharedPart + { + + private bool _fetchingFirmwareVersion; + + private void _internalCopter_ConnectionBroken(object sender, EventArgs e) + { + IsConnected = _internalCopter.IsConnected; + } + + private void _internalCopter_GetLogDataEvent(string log) + { + //飞机消息日志，后面需要改为记录方式 + StatusText =Name+":"+log; + var e = new MessageCreatedEventArgs { Message = StatusText }; + RaiseTextReceived(e); + } + + + private void _internalCopter_ReceiveDataStreamEvent(object sender, byte StreamType) + { + switch (StreamType) + { + case MAVLINK_MSG_ID_GPS_RAW_INT: //需要 + { + SatCount = _internalCopter.satcount; + GpsFixType = _internalCopter.gpsstatus.ToGpsFixType(); + GpsHdop = _internalCopter.gpshdop; + CopterErrorCode =_internalCopter.errorcode; + CopterPreCheckPass = _internalCopter.precheckok; + CopterErrorString = getcoptererrorstr(_internalCopter.errorcode); + Altitude = _internalCopter.gpsalt; + Retain = BitConverter.GetBytes(_internalCopter.retain); + + Voltage = _internalCopter.battery_voltage; + CommModuleMode = (FlightMode)_internalCopter.commModuleMode; + CommModuleVersion = _internalCopter.commModuleVersion; + IsUnlocked = _internalCopter.isUnlocked; + //Yaw = _internalCopter.yaw; + Heading = _internalCopter.heading; + HeartbeatCount++; + + if (SatCount > 8) + { + IsGpsAccurate = true; + RecordLat = _internalCopter.lat; + RecordLng = _internalCopter.lng; + } + + Latitude = RecordLat; + Longitude = RecordLng; + UpdateFlightDataIfNeeded(); + RaiseLocationChangedIfNeeded(); + + RaiseDataStreamReceived(PDataStreamType.GPS_RAW_INT); + + break; + } + case MAVLINK_MSG_ID_ATTITUDE: //无用 + { + Roll = _internalCopter.roll; + Pitch = _internalCopter.pitch; + Yaw = _internalCopter.yaw; + TimebootMs = _internalCopter.timebootms; + RaiseAttitudeChanged(); + + break; + } + case MAVLINK_MSG_ID_RC_CHANNELS_RAW: //只用于显示,可以不要 + { + Channel1 = _internalCopter.ch1in; + Channel2 = _internalCopter.ch2in; + Channel3 = _internalCopter.ch3in; + Channel4 = _internalCopter.ch4in; + Channel5 = _internalCopter.ch5in; + Channel6 = _internalCopter.ch6in; + Channel7 = _internalCopter.ch7in; + Channel8 = _internalCopter.ch8in; + + RaiseDataStreamReceived(PDataStreamType.RC_CHANNELS_RAW); + + break; + } + case MAVLINK_MSG_ID_VFR_HUD: + { + Heading = _internalCopter.heading; //有用 + Altitude = _internalCopter.alt; //有用 + AirSpeed = _internalCopter.airspeed; //没用 + GroundSpeed = _internalCopter.groundspeed; //没用 + + RaiseAltitudeChangedIfNeeded(); + + break; + } + } + } + + private async void _internalCopter_ReceiveHeartBearEvent(object sender, uint iError, ulong iCount) + { + IsUnlocked = _internalCopter.armed; + Mode = (FlightMode)_internalCopter.mode; + // 王海, 20160311, 将闲置的 apname 用作已经切过 GPS 模式的标志。 + HasSwitchedToGpsMode = (byte)_internalCopter.apname != 0; + ++HeartbeatCount; + + IsCheckingConnection = false; + /* + if (FirmwareVersion == null && !_fetchingFirmwareVersion) + { + try + { + _fetchingFirmwareVersion = true; + FirmwareVersion = (int)await GetParamAsync("FIRMWARE_VERSION", 5000); + } + catch (TimeoutException) + { + // 吞掉。 + } + finally + { + _fetchingFirmwareVersion = false; + } + } + */ + RaiseHeartbeatReceived(new HeartbeatReceivedEventArgs(iCount)); + } + + private void _internalCopter_ReceiveSensorEvent(object sender) + { + RaiseSensorDataReceived(EventArgs.Empty); + } + + private void _internalCopter_ReceiveSysStatusEvent(object sender, byte StreamType, bool GPSBadHealth, bool GyroBadHealth, bool AccelBadHealth, bool CompassBadHealth, bool BarometerBadHealth, bool canTakeOff) + { + Voltage = _internalCopter.battery_voltage / 1000; + + if (_internalCopter.battery_remaining == 255) // 剩余电量未知，需要计算。 + { + CalcBatteryPer(); + } + else + { + BatteryPer = _internalCopter.battery_remaining; + } + + const int MIN_VERSION_WITH_CAN_TAKE_OFF_FIELD = 12308; + const int MIN_VERSION_OF_GENERATION2 = 0x3000; + if (FirmwareVersion != null) + { + if (FirmwareVersion.Value >= MIN_VERSION_WITH_CAN_TAKE_OFF_FIELD) + { + // 飞控提供了 canTakeOff。 + IsGpsAccurate = canTakeOff; + } + else if (FirmwareVersion.Value >= MIN_VERSION_OF_GENERATION2) + { + // 2.0 飞行器，飞控未提供 canTakeOff。 + IsGpsAccurate = SatCount >= 12; + } + else + { + // 1.0 飞行器，飞控不提供 canTakeOff。 + IsGpsAccurate = SatCount >= 6; + } + } + else + IsGpsAccurate = SatCount >= 8; + + FlightDistance2D = _internalCopter.FlightDistance2D; + + RaiseSysStatusReceived(new SystemStatusReceivedEventArgs(GPSBadHealth, GyroBadHealth, AccelBadHealth, CompassBadHealth, BarometerBadHealth)); + } + + private void _internalCopter_PacketHandled(object sender, PlaneCopter.PacketReceivedEventArgs e) + { + switch (e.Packet[5]) + { + case MAVLINK_MSG_ID_MISSION_COUNT: + MissionCount = _internalCopter.WpCount; + break; + + default: + break; + } + } + + private void _internalCopter_UnhandledPacketReceived(object sender, PlaneCopter.PacketReceivedEventArgs e) + { + switch (e.Packet[5]) + { + case MAVLINK_MSG_ID_MISSION_ACK: + AnalyzeMissionAckPacket(e.Packet); + break; + + case MAVLINK_MSG_ID_MISSION_COUNT: + AnalyzeMissionCountPacket(e.Packet); + break; + + case MAVLINK_MSG_ID_MISSION_ITEM: + AnalyzeMissionItemPacket(e.Packet); + break; + + case MAVLINK_MSG_ID_MISSION_REQUEST: + AnalyzeMissionRequestPacket(e.Packet); + break; + + case MAVLINK_MSG_ID_SET_PAIR: + AnalyzeSetPairPacket(e.Packet); + break; + + default: + break; + } + } + +#if PRIVATE + protected virtual void RegisterInternalCopterEventHandlers() +#else + + private void RegisterInternalCopterEventHandlers() +#endif + { + _internalCopter.GetLogDataEvent += _internalCopter_GetLogDataEvent; + _internalCopter.ReceiveDataStreamEvent += _internalCopter_ReceiveDataStreamEvent; + _internalCopter.ReceiveHeartBearEvent += _internalCopter_ReceiveHeartBearEvent; + _internalCopter.ReceiveSensorEvent += _internalCopter_ReceiveSensorEvent; + _internalCopter.ReceiveSysStatusEvent += _internalCopter_ReceiveSysStatusEvent; + _internalCopter.ConnectionBroken += _internalCopter_ConnectionBroken; + _internalCopter.PacketHandled += _internalCopter_PacketHandled; + _internalCopter.UnhandledPacketReceived += _internalCopter_UnhandledPacketReceived; + _internalCopter.MessageCreated += _internalCopter_MessageCreated; + } + + private void _internalCopter_MessageCreated(object sender, MessageCreatedEventArgs e) + { + var txte = new MessageCreatedEventArgs { Message = Name + ":" +e.Message }; + RaiseTextReceived(txte); + } + + #region 计算剩余电量 + + private int bPerTimes; + private int outBatteryPer; + private int[] tPerTimes = new int[20]; + private int v_num; + + /// + /// 计算电量并在条件满足时更新 属性。 + /// + private void CalcBatteryPer() + { + float volmax = 0f; + float volmin = 0f; + if (Voltage > 5 && Voltage < 9) + { + volmax = 8.2f; + volmin = 7f; + } + else if (Voltage >= 9 + && Voltage < 13.6) + { + volmax = 11.6f; + volmin = 10.2f; + } + else if (Voltage >= 13.6 + && Voltage < 17.2) + { + volmax = 16.3f; + volmin = 14.2f; + } + else if (Voltage >= 17.2 + && Voltage < 26.2) + { + volmax = 24.8f; + volmin = 21.2f; + } + + int batteryPer = (int)Math.Round((Voltage - volmin) * 100 / (volmax - volmin)); + + if (batteryPer == -1 || volmax == 0 || volmin == 0) + return; + + if (bPerTimes < 20) + { + tPerTimes[bPerTimes] = batteryPer; + bPerTimes += 1; + } + else + { + tPerTimes[v_num] = batteryPer; + v_num++; + if (v_num == 20) + v_num = 0; + } + for (int i = 0; i < bPerTimes; i++) + { + outBatteryPer += tPerTimes[i]; + } + + outBatteryPer = outBatteryPer / bPerTimes; + if (outBatteryPer < BatteryPer && bPerTimes > 18) + { + BatteryPer = (byte)outBatteryPer; + } + } + + #endregion 计算剩余电量 + } +} diff --git a/PlaneGcsSdk_Shared/Copters/FakeCopter.cs b/PlaneGcsSdk_Shared/Copters/FakeCopter.cs index 52777ec..83750bf 100644 --- a/PlaneGcsSdk_Shared/Copters/FakeCopter.cs +++ b/PlaneGcsSdk_Shared/Copters/FakeCopter.cs @@ -134,9 +134,10 @@ namespace Plane.Copters private PLLocation _takeoffcentloc; private PLLocation _taskcentloc; private float _mindistance; - private int _rettime; + private float _rettime; private bool _descending; int Emergency_Retstatus = 0; + DateTime EmergencyRetTime; //返航路径第一个航点 PLLocation Emergency_firstloc; @@ -290,45 +291,42 @@ namespace Plane.Copters IsCheckingConnection = false; return TaskUtils.CompletedTask; } + private int minretalt=15; //最低返航高度 + private int minretalt_first = 25; //第一次返航高度 + //返航位置1 + private PLLocation EmergencyRetPLLocation1; + private void BuildPath() { - float taralt; - //返航高度参数 - float retalt = 15; + //计算当前位置和起飞中心点的距离 + float dis = (float)this.CalcDistance(_takeoffcentloc.Latitude, _takeoffcentloc.Longitude, 0); + _rettime = (dis - _mindistance) * 1.0f; - if (_descending) - { - if (Altitude <= retalt) taralt = Altitude; - else - { - float maxalt = Altitude - retalt; - //降低一个随机高度，随机高度介于0和maxalt之间，也就是目标高度不要低于retalt, 防止飞机在同一高度飞行 - taralt = (float)(Altitude - (new Random().NextDouble() * maxalt)); - } - Emergency_firstloc = new PLLocation(this.Latitude,this.Longitude, taralt); - } - else - { - //计算当前位置和起飞点的距离 - float dis = (float)this.CalcDistance(TakeOffPoint.Latitude, TakeOffPoint.Longitude,Altitude); - //随机水平飞行距离 - float maxdis = (float)new Random().NextDouble() * dis; + //计算方向--角度 + float takeoffdir =(float)_takeoffcentloc.CalcDirection2D(TakeOffPoint)* 57.29578f; + //计算起飞点距离 + float takeoffdis = (float)TakeOffPoint.CalcDistance2D(_takeoffcentloc.Latitude, _takeoffcentloc.Longitude); + + + ILocation2D take1 = TakeOffPoint.CalcLatLngSomeMetersAway2D(takeoffdir, takeoffdis); + //第一次返航点 + EmergencyRetPLLocation1 = new PLLocation(take1.Latitude, take1.Longitude, minretalt_first); - } - - } + + } public Task EmergencyRetAsync(PLLocation takeoffcentloc, PLLocation taskcentloc, float mindistance, int rettime, bool descending) { _takeoffcentloc = takeoffcentloc; _taskcentloc = taskcentloc; _mindistance = mindistance; - _rettime = rettime; + // _rettime = rettime; _descending = descending; Emergency_Retstatus = 0; + EmergencyRetTime = DateTime.Now; //计算返航路径 BuildPath(); Mode = FlightMode.EMERG_RTL; @@ -531,6 +529,9 @@ namespace Plane.Copters if (flightDistance != null) FlightDistance = flightDistance.Value; if (flightDistance2D != null) FlightDistance2D = flightDistance2D.Value; if (flightTimeSpan != null) FlightTimeSpan = flightTimeSpan.Value; + CopterPreCheckPass = true; + //CopterErrorCode = 2; + //CopterErrorString = getcoptererrorstr(CopterErrorCode); } public Task StartPairingAsync() @@ -554,7 +555,7 @@ namespace Plane.Copters public Task MissionStartAsync(int hour_utc, int minute_utc, int second_utc, double Missionlng, double Missionlat) { MissionStartTime = DateTime.Now; - TakeOffPoint = new PLLocation(Missionlat, Missionlng, 0); + TakeOffPoint = new PLLocation(this.Latitude, this.Longitude, 0); return TaskUtils.CompletedTask; } @@ -799,7 +800,7 @@ namespace Plane.Copters case FlightMode.LAND: // 王海, 20160317, 降落时也能体感控制若干通道。 - UpdateWithChannels(); + // UpdateWithChannels(); // 以最大速度降落，直到高度为 0。 if (Altitude > 0) { @@ -826,42 +827,59 @@ namespace Plane.Copters switch(Emergency_Retstatus) { case 0: //等待返航 - if ((DateTime.Now- MissionStartTime).TotalSeconds > _rettime) + if ((DateTime.Now - EmergencyRetTime).TotalSeconds > _rettime) { Emergency_Retstatus = 1; //平飞或降落随机距离 - FlyToCoreAsync(Emergency_firstloc.Latitude, Emergency_firstloc.Longitude, Emergency_firstloc.Altitude); - //FlyToCoreAsync里面把模式改成了GUIDED，这里改回来 - Mode = FlightMode.EMERG_RTL; - } - - - break; - case 1: //返航阶段一:等待平飞或降落随机距离并到达,然后飞到起飞点上空 + //直接返航 + FlyToCoreAsync(TakeOffPoint.Latitude, TakeOffPoint.Longitude, minretalt); + //先到第一返航点再到起飞点上空 + //FlyToCoreAsync(EmergencyRetPLLocation1.Latitude, EmergencyRetPLLocation1.Longitude, EmergencyRetPLLocation1.Altitude); - UpdateWithDestination(Emergency_firstloc.Latitude, Emergency_firstloc.Longitude, Emergency_firstloc.Altitude); - if (ReachedDestination(Emergency_firstloc.Latitude, Emergency_firstloc.Longitude, Emergency_firstloc.Altitude)) - { - FlyToCoreAsync(TakeOffPoint.Latitude, TakeOffPoint.Longitude, Emergency_firstloc.Altitude); //FlyToCoreAsync里面把模式改成了GUIDED，这里改回来 Mode = FlightMode.EMERG_RTL; - Emergency_Retstatus = 2; } - - break; - case 2: //返航阶段二:等待到达起飞点上空,然后降落 + break; - UpdateWithDestination(TakeOffPoint.Latitude, TakeOffPoint.Longitude, Emergency_firstloc.Altitude); - if (ReachedDestination(TakeOffPoint.Latitude, TakeOffPoint.Longitude, Emergency_firstloc.Altitude)) + + case 1: //返航阶段一:等待平飞或降落随机距离并到达,然后飞到起飞点上空 + + //直接返航 + UpdateWithDestination(TakeOffPoint.Latitude, TakeOffPoint.Longitude, minretalt); + if (ReachedDestination(TakeOffPoint.Latitude, TakeOffPoint.Longitude, minretalt)) { - FlyToCoreAsync(TakeOffPoint.Latitude, TakeOffPoint.Longitude, Emergency_firstloc.Altitude); - //FlyToCoreAsync里面把模式改成了GUIDED，这里改回来 Mode = FlightMode.LAND; + //FlyToCoreAsync里面把模式改成了GUIDED，这里改回来 Emergency_Retstatus = 3; } + + /* // //先到第一返航点再到起飞点上空 + UpdateWithDestination(EmergencyRetPLLocation1.Latitude, EmergencyRetPLLocation1.Longitude, EmergencyRetPLLocation1.Altitude); + if (ReachedDestination(EmergencyRetPLLocation1.Latitude, EmergencyRetPLLocation1.Longitude, EmergencyRetPLLocation1.Altitude)) + { + FlyToCoreAsync(TakeOffPoint.Latitude, TakeOffPoint.Longitude, minretalt); + Mode = FlightMode.EMERG_RTL; + //FlyToCoreAsync里面把模式改成了GUIDED，这里改回来 + Emergency_Retstatus = 2; + } + */ + + break; + case 2: //返航阶段一:等待平飞或降落随机距离并到达,然后飞到起飞点上空 + + UpdateWithDestination(TakeOffPoint.Latitude, TakeOffPoint.Longitude, minretalt); + if (ReachedDestination(TakeOffPoint.Latitude, TakeOffPoint.Longitude, minretalt)) + { + Mode = FlightMode.LAND; + //FlyToCoreAsync里面把模式改成了GUIDED，这里改回来 + Emergency_Retstatus = 3; + } + break; case 3: //降落 + break; + } break; diff --git a/PlaneGcsSdk_Shared/Copters/PlaneCopter.PacketAnalysis.cs b/PlaneGcsSdk_Shared/Copters/PlaneCopter.PacketAnalysis.cs index 93b93a6..4befee6 100644 --- a/PlaneGcsSdk_Shared/Copters/PlaneCopter.PacketAnalysis.cs +++ b/PlaneGcsSdk_Shared/Copters/PlaneCopter.PacketAnalysis.cs @@ -1,402 +1,404 @@ -//#define LOG_PACKETS - -using Plane.Protocols; -using System; -using System.Diagnostics; -using System.Text; -using System.Threading.Tasks; -using TaskTools.Utilities; - -#if LOG_PACKETS - -using System.Diagnostics; - -#endif - -namespace Plane.Copters -{ - partial class PlaneCopter - { - private void AnalyzeAttitudePacket(byte[] buffer) - { - var att = buffer.ByteArrayToStructure(6); - roll = att.roll * rad2deg; - pitch = att.pitch * rad2deg; - yaw = att.yaw * rad2deg; - timebootms = att.time_boot_ms; - RaiseReceiveDataStreamEventOnUIThread(buffer[5]); - } - - private void AnalyzeGlobalPositionIntPacket(byte[] buffer) - { - var loc = buffer.ByteArrayToStructure(6); - useLocation = true; - if (loc.lat == 0 && loc.lon == 0) - { - useLocation = false; - } - else - { - gpsalt = loc.alt / 1000.0f; - lat = loc.lat / 10000000.0; - lng = loc.lon / 10000000.0; - } - RaiseReceiveDataStreamEventOnUIThread(buffer[5]); - } - -#if LOG_GPS_RAW_INT - private Stopwatch _lastGpsRawIntPacketTime = new Stopwatch(); - private TimeSpan _lastGpsRawIntPacketTimeSpan; -#endif - - private void AnalyzeGpsRawIntPacket(byte[] buffer) - { -#if LOG_GPS_RAW_INT - if (!_lastGpsRawIntPacketTime.IsRunning) - { - _lastGpsRawIntPacketTime.Start(); - } - else - { - var elapsed = _lastGpsRawIntPacketTime.Elapsed; - Debug.WriteLine(elapsed - _lastGpsRawIntPacketTimeSpan, "----------------------------GpsRawInt"); - _lastGpsRawIntPacketTimeSpan = elapsed; - } -#endif - var gps = buffer.ByteArrayToStructure(6); - long mylat = BitConverter.ToInt32(buffer, 14); - long mylong = BitConverter.ToInt32(buffer, 18); - //if (!useLocation) - //{ - lat = gps.lat * 1.0e-7; - lng = gps.lon * 1.0e-7; - //} - gpsalt = gps.alt * 1.0e-7f; // using vfr as includes baro calc - gpsstatus = gps.fix_type; //0-1: no fix, 2: 2D fix, 3: 3D fix - gpshdop = (float)Math.Round(gps.eph / 100.0, 2); - satcount = gps.satellites_visible; - groundspeed = gps.vel * 1.0e-2f; - groundcourse = gps.cog * 1.0e-2f; - RaiseReceiveDataStreamEventOnUIThread(buffer[5]); - } - -#if LOG_PACKETS - private DateTime _lastHeartbeatTime = DateTime.MinValue; -#endif - - private void AnalyzeHeartbeatPacket(byte[] buffer) - { -#if LOG_PACKETS - var now = DateTime.Now; - if (_lastHeartbeatTime != DateTime.MinValue) - { - Debug.WriteLine(now - _lastHeartbeatTime, "心跳间隔"); - } - _lastHeartbeatTime = now; -#endif - DataTimeOut = 0; - var hb = buffer.ByteArrayToStructure(6); - mavlinkversion = hb.mavlink_version; - aptype = (MAVLink.MAV_TYPE)hb.type; - apname = (MAVLink.MAV_AUTOPILOT)hb.autopilot; - sysid = buffer[3]; - compid = buffer[4]; - recvpacketcount = buffer[2]; - armed = (hb.base_mode & (byte)MAVLink.MAV_MODE_FLAG.SAFETY_ARMED) == (byte)MAVLink.MAV_MODE_FLAG.SAFETY_ARMED; - failsafe = hb.system_status == (byte)MAVLink.MAV_STATE.CRITICAL; - mode = hb.custom_mode; - - var handler = ReceiveHeartBearEvent; - if (handler != null) - { - RunOnUIThread(() => - { - handler(this, iErrorData, iDataCount); - }); - } - - if (_autoRequestData && mavlinkversion == 3 && SendReq == false) - { - SendReq = true; - Task.Run(GetCopterDataAsync); - } - } - - private void AnalyzeMemInfoPacket(byte[] buffer) - { - var mem = buffer.ByteArrayToStructure(6); - freemem = mem.freemem; - brklevel = mem.brkval; - RaiseReceiveDataStreamEventOnUIThread(buffer[5]); - } - - private void AnalyzeMissionCurrentPacket(byte[] buffer) - { - var wpcur = buffer.ByteArrayToStructure(6); - RaiseReceiveDataStreamEventOnUIThread(buffer[5]); - } - - private void AnalyzeNavControllerOutputPacket(byte[] buffer) - { - var nav = buffer.ByteArrayToStructure(6); - nav_roll = nav.nav_roll; - nav_pitch = nav.nav_pitch; - nav_bearing = nav.nav_bearing; - target_bearing = nav.target_bearing; - wp_dist = nav.wp_dist; - alt_error = nav.alt_error; - aspd_error = nav.aspd_error / 100.0f; - xtrack_error = nav.xtrack_error; - RaiseReceiveDataStreamEventOnUIThread(buffer[5]); - } - - private void AnalyzeParamValuePacket(byte[] buffer) - { - var par = buffer.ByteArrayToStructure(6); - param_total = (par.param_count); - string paramID = Encoding.ASCII.GetString(par.param_id); - int pos = paramID.IndexOf('\0'); - if (pos != -1) - { - paramID = paramID.Substring(0, pos); - } - try - { - param[paramID] = (par.param_value); - param_types[paramID] = (MAVLink.MAV_PARAM_TYPE)par.param_type; - } - catch (IndexOutOfRangeException ex) - { - Debug.WriteLine(ex); - } - - var handler = ReceiveParamEvent; - if (handler != null) - { - RunOnUIThread(() => - { - handler(this, paramID, par.param_index, par.param_count); - }); - } - } - - private void AnalyzeRadioPacket(byte[] buffer) - { - var radio = buffer.ByteArrayToStructure(6); - //Debug.Print("fix:" + radio.fixedp + " noise:" + radio.noise + " remnoise:" + radio.remnoise + " remrssi:" + radio.remrssi + " rssi:" + radio.rssi + " rxerr:" + radio.rxerrors + " txbuf:" + radio.txbuf); - remrssi = radio.remrssi; - rssi = radio.rssi; - RaiseReceiveDataStreamEventOnUIThread(buffer[5]); - } - - private void AnalyzeRawImuPacket(byte[] buffer) - { - var imu = buffer.ByteArrayToStructure(6); - - gx = imu.xgyro; - gy = imu.ygyro; - gz = imu.zgyro; - - ax = imu.xacc; - ay = imu.yacc; - az = imu.zacc; - - mx = imu.xmag; - my = imu.ymag; - mz = imu.zmag; - - var handler = ReceiveSensorEvent; - if (handler != null) - { - RunOnUIThread(() => - { - handler(this); - }); - } - } - - private void AnalyzeRCChannelsRawPacket(byte[] buffer) - { - var rcin = buffer.ByteArrayToStructure(6); - - ch1in = rcin.chan1_raw; - ch2in = rcin.chan2_raw; - ch3in = rcin.chan3_raw; - ch4in = rcin.chan4_raw; - ch5in = rcin.chan5_raw; - ch6in = rcin.chan6_raw; - ch7in = rcin.chan7_raw; - ch8in = rcin.chan8_raw; - if (bInitChannel == false) - { - bInitChannel = true; - } - - //percent - rxrssi = (float)((rcin.rssi / 255.0) * 100.0); - - RaiseReceiveDataStreamEventOnUIThread(buffer[5]); - } - - private void AnalyzeScaledImu2Packet(byte[] buffer) - { - var imu2 = buffer.ByteArrayToStructure(6); - gx2 = imu2.xgyro; - gy2 = imu2.ygyro; - gz2 = imu2.zgyro; - - ax2 = imu2.xacc; - ay2 = imu2.yacc; - az2 = imu2.zacc; - - mx2 = imu2.xmag; - my2 = imu2.ymag; - mz2 = imu2.zmag; - - var handler = ReceiveScaleImu2Event; - if (handler != null) - { - RunOnUIThread(() => handler(this)); - } - } - - private void AnalyzeSensorOffsetsPacket(byte[] buffer) - { - var sensofs = buffer.ByteArrayToStructure(6); - - mag_ofs_x = sensofs.mag_ofs_x; - mag_ofs_y = sensofs.mag_ofs_y; - mag_ofs_z = sensofs.mag_ofs_z; - mag_declination = sensofs.mag_declination; - - raw_press = sensofs.raw_press; - raw_temp = sensofs.raw_temp; - - gyro_cal_x = sensofs.gyro_cal_x; - gyro_cal_y = sensofs.gyro_cal_y; - gyro_cal_z = sensofs.gyro_cal_z; - - accel_cal_x = sensofs.accel_cal_x; - accel_cal_y = sensofs.accel_cal_y; - accel_cal_z = sensofs.accel_cal_z; - - var handler = ReceiveSensorEvent; - if (handler != null) - { - RunOnUIThread(() => - { - handler(this); - }); - } - } - - private void AnalyzeStatusTextPacket(byte[] buffer) - { - var stext = buffer.ByteArrayToStructure(6); - var length = Array.IndexOf(stext.text, 0); - var log = Encoding.ASCII.GetString(stext.text, 0, length); - var handler = GetLogDataEvent; - if (handler != null) - { - RunOnUIThread(() => handler(log)); - } - } - - private void AnalyzeSysStatusPacket(byte[] buffer) - { - var sysstatus = buffer.ByteArrayToStructure(6); - battery_voltage = sysstatus.voltage_battery; - battery_remaining = sysstatus.battery_remaining; - current_battery = sysstatus.current_battery / 100.0f; - - var sensors_enabled = new Mavlink_Sensors(sysstatus.onboard_control_sensors_enabled); - var sensors_health = new Mavlink_Sensors(sysstatus.onboard_control_sensors_health); - - if (sensors_health.gps != sensors_enabled.gps) - { - bGPSBadHealth = true; - } - else - { - bGPSBadHealth = false; - } - if (sensors_health.gyro != sensors_enabled.gyro) - { - bGyroBadHealth = true; - } - else - { - bGyroBadHealth = false; - } - if (sensors_health.accelerometer != sensors_enabled.accelerometer) - { - bAccel = true; - } - else - { - bAccel = false; - } - if (sensors_health.compass != sensors_enabled.compass) - { - bCompass = true; - } - else - { - bCompass = false; - } - if (sensors_health.barometer != sensors_enabled.barometer) - { - bBarometer = true; - } - else - { - bBarometer = false; - } - FlightDistance2D = sysstatus.errors_count2; - var handler = ReceiveSysStatusEvent; - if (handler != null) - { - RunOnUIThread(() => - { - handler(this, buffer[5], bGPSBadHealth, bGyroBadHealth, bAccel, bCompass, bBarometer, sensors_health.CanTakeOff); - }); - } - } - - private void AnalyzeVfrHudPacket(byte[] buffer) - { - var vfr = buffer.ByteArrayToStructure(6); - groundspeed = vfr.groundspeed; - airspeed = vfr.airspeed; - altorigin = vfr.alt; // this might include baro - ch3percent = vfr.throttle; - heading = vfr.heading; - RaiseReceiveDataStreamEventOnUIThread(buffer[5]); - } - - /// - /// 处理通信模块发过来的数据 - /// - /// - public void AnalyzeCommMouldePositionIntPacket(byte[] buffer, byte version) - { - 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; - - heading = (short)((info.heading / 100) % 360); - - commModuleVersion = version; - - retain = info.retain; - RaiseReceiveDataStreamEventOnUIThread(MAVLink.MAVLINK_MSG_ID_GPS_RAW_INT); - } - } -} +//#define LOG_PACKETS + +using Plane.Protocols; +using System; +using System.Diagnostics; +using System.Text; +using System.Threading.Tasks; +using TaskTools.Utilities; + +#if LOG_PACKETS + +using System.Diagnostics; + +#endif + +namespace Plane.Copters +{ + partial class PlaneCopter + { + private void AnalyzeAttitudePacket(byte[] buffer) + { + var att = buffer.ByteArrayToStructure(6); + roll = att.roll * rad2deg; + pitch = att.pitch * rad2deg; + yaw = att.yaw * rad2deg; + timebootms = att.time_boot_ms; + RaiseReceiveDataStreamEventOnUIThread(buffer[5]); + } + + private void AnalyzeGlobalPositionIntPacket(byte[] buffer) + { + var loc = buffer.ByteArrayToStructure(6); + useLocation = true; + if (loc.lat == 0 && loc.lon == 0) + { + useLocation = false; + } + else + { + gpsalt = loc.alt / 1000.0f; + lat = loc.lat / 10000000.0; + lng = loc.lon / 10000000.0; + } + RaiseReceiveDataStreamEventOnUIThread(buffer[5]); + } + +#if LOG_GPS_RAW_INT + private Stopwatch _lastGpsRawIntPacketTime = new Stopwatch(); + private TimeSpan _lastGpsRawIntPacketTimeSpan; +#endif + + private void AnalyzeGpsRawIntPacket(byte[] buffer) + { +#if LOG_GPS_RAW_INT + if (!_lastGpsRawIntPacketTime.IsRunning) + { + _lastGpsRawIntPacketTime.Start(); + } + else + { + var elapsed = _lastGpsRawIntPacketTime.Elapsed; + Debug.WriteLine(elapsed - _lastGpsRawIntPacketTimeSpan, "----------------------------GpsRawInt"); + _lastGpsRawIntPacketTimeSpan = elapsed; + } +#endif + var gps = buffer.ByteArrayToStructure(6); + long mylat = BitConverter.ToInt32(buffer, 14); + long mylong = BitConverter.ToInt32(buffer, 18); + //if (!useLocation) + //{ + lat = gps.lat * 1.0e-7; + lng = gps.lon * 1.0e-7; + //} + gpsalt = gps.alt * 1.0e-7f; // using vfr as includes baro calc + gpsstatus = gps.fix_type; //0-1: no fix, 2: 2D fix, 3: 3D fix + gpshdop = (float)Math.Round(gps.eph / 100.0, 2); + satcount = gps.satellites_visible; + groundspeed = gps.vel * 1.0e-2f; + groundcourse = gps.cog * 1.0e-2f; + RaiseReceiveDataStreamEventOnUIThread(buffer[5]); + } + +#if LOG_PACKETS + private DateTime _lastHeartbeatTime = DateTime.MinValue; +#endif + + private void AnalyzeHeartbeatPacket(byte[] buffer) + { +#if LOG_PACKETS + var now = DateTime.Now; + if (_lastHeartbeatTime != DateTime.MinValue) + { + Debug.WriteLine(now - _lastHeartbeatTime, "心跳间隔"); + } + _lastHeartbeatTime = now; +#endif + DataTimeOut = 0; + var hb = buffer.ByteArrayToStructure(6); + mavlinkversion = hb.mavlink_version; + aptype = (MAVLink.MAV_TYPE)hb.type; + apname = (MAVLink.MAV_AUTOPILOT)hb.autopilot; + sysid = buffer[3]; + compid = buffer[4]; + recvpacketcount = buffer[2]; + armed = (hb.base_mode & (byte)MAVLink.MAV_MODE_FLAG.SAFETY_ARMED) == (byte)MAVLink.MAV_MODE_FLAG.SAFETY_ARMED; + failsafe = hb.system_status == (byte)MAVLink.MAV_STATE.CRITICAL; + mode = hb.custom_mode; + + var handler = ReceiveHeartBearEvent; + if (handler != null) + { + RunOnUIThread(() => + { + handler(this, iErrorData, iDataCount); + }); + } + + if (_autoRequestData && mavlinkversion == 3 && SendReq == false) + { + SendReq = true; + Task.Run(GetCopterDataAsync); + } + } + + private void AnalyzeMemInfoPacket(byte[] buffer) + { + var mem = buffer.ByteArrayToStructure(6); + freemem = mem.freemem; + brklevel = mem.brkval; + RaiseReceiveDataStreamEventOnUIThread(buffer[5]); + } + + private void AnalyzeMissionCurrentPacket(byte[] buffer) + { + var wpcur = buffer.ByteArrayToStructure(6); + RaiseReceiveDataStreamEventOnUIThread(buffer[5]); + } + + private void AnalyzeNavControllerOutputPacket(byte[] buffer) + { + var nav = buffer.ByteArrayToStructure(6); + nav_roll = nav.nav_roll; + nav_pitch = nav.nav_pitch; + nav_bearing = nav.nav_bearing; + target_bearing = nav.target_bearing; + wp_dist = nav.wp_dist; + alt_error = nav.alt_error; + aspd_error = nav.aspd_error / 100.0f; + xtrack_error = nav.xtrack_error; + RaiseReceiveDataStreamEventOnUIThread(buffer[5]); + } + + private void AnalyzeParamValuePacket(byte[] buffer) + { + var par = buffer.ByteArrayToStructure(6); + param_total = (par.param_count); + string paramID = Encoding.ASCII.GetString(par.param_id); + int pos = paramID.IndexOf('\0'); + if (pos != -1) + { + paramID = paramID.Substring(0, pos); + } + try + { + param[paramID] = (par.param_value); + param_types[paramID] = (MAVLink.MAV_PARAM_TYPE)par.param_type; + } + catch (IndexOutOfRangeException ex) + { + Debug.WriteLine(ex); + } + + var handler = ReceiveParamEvent; + if (handler != null) + { + RunOnUIThread(() => + { + handler(this, paramID, par.param_index, par.param_count); + }); + } + } + + private void AnalyzeRadioPacket(byte[] buffer) + { + var radio = buffer.ByteArrayToStructure(6); + //Debug.Print("fix:" + radio.fixedp + " noise:" + radio.noise + " remnoise:" + radio.remnoise + " remrssi:" + radio.remrssi + " rssi:" + radio.rssi + " rxerr:" + radio.rxerrors + " txbuf:" + radio.txbuf); + remrssi = radio.remrssi; + rssi = radio.rssi; + RaiseReceiveDataStreamEventOnUIThread(buffer[5]); + } + + private void AnalyzeRawImuPacket(byte[] buffer) + { + var imu = buffer.ByteArrayToStructure(6); + + gx = imu.xgyro; + gy = imu.ygyro; + gz = imu.zgyro; + + ax = imu.xacc; + ay = imu.yacc; + az = imu.zacc; + + mx = imu.xmag; + my = imu.ymag; + mz = imu.zmag; + + var handler = ReceiveSensorEvent; + if (handler != null) + { + RunOnUIThread(() => + { + handler(this); + }); + } + } + + private void AnalyzeRCChannelsRawPacket(byte[] buffer) + { + var rcin = buffer.ByteArrayToStructure(6); + + ch1in = rcin.chan1_raw; + ch2in = rcin.chan2_raw; + ch3in = rcin.chan3_raw; + ch4in = rcin.chan4_raw; + ch5in = rcin.chan5_raw; + ch6in = rcin.chan6_raw; + ch7in = rcin.chan7_raw; + ch8in = rcin.chan8_raw; + if (bInitChannel == false) + { + bInitChannel = true; + } + + //percent + rxrssi = (float)((rcin.rssi / 255.0) * 100.0); + + RaiseReceiveDataStreamEventOnUIThread(buffer[5]); + } + + private void AnalyzeScaledImu2Packet(byte[] buffer) + { + var imu2 = buffer.ByteArrayToStructure(6); + gx2 = imu2.xgyro; + gy2 = imu2.ygyro; + gz2 = imu2.zgyro; + + ax2 = imu2.xacc; + ay2 = imu2.yacc; + az2 = imu2.zacc; + + mx2 = imu2.xmag; + my2 = imu2.ymag; + mz2 = imu2.zmag; + + var handler = ReceiveScaleImu2Event; + if (handler != null) + { + RunOnUIThread(() => handler(this)); + } + } + + private void AnalyzeSensorOffsetsPacket(byte[] buffer) + { + var sensofs = buffer.ByteArrayToStructure(6); + + mag_ofs_x = sensofs.mag_ofs_x; + mag_ofs_y = sensofs.mag_ofs_y; + mag_ofs_z = sensofs.mag_ofs_z; + mag_declination = sensofs.mag_declination; + + raw_press = sensofs.raw_press; + raw_temp = sensofs.raw_temp; + + gyro_cal_x = sensofs.gyro_cal_x; + gyro_cal_y = sensofs.gyro_cal_y; + gyro_cal_z = sensofs.gyro_cal_z; + + accel_cal_x = sensofs.accel_cal_x; + accel_cal_y = sensofs.accel_cal_y; + accel_cal_z = sensofs.accel_cal_z; + + var handler = ReceiveSensorEvent; + if (handler != null) + { + RunOnUIThread(() => + { + handler(this); + }); + } + } + + private void AnalyzeStatusTextPacket(byte[] buffer) + { + var stext = buffer.ByteArrayToStructure(6); + var length = Array.IndexOf(stext.text, 0); + var log = Encoding.ASCII.GetString(stext.text, 0, length); + var handler = GetLogDataEvent; + if (handler != null) + { + RunOnUIThread(() => handler(log)); + } + } + + private void AnalyzeSysStatusPacket(byte[] buffer) + { + var sysstatus = buffer.ByteArrayToStructure(6); + battery_voltage = sysstatus.voltage_battery; + battery_remaining = sysstatus.battery_remaining; + current_battery = sysstatus.current_battery / 100.0f; + + var sensors_enabled = new Mavlink_Sensors(sysstatus.onboard_control_sensors_enabled); + var sensors_health = new Mavlink_Sensors(sysstatus.onboard_control_sensors_health); + + if (sensors_health.gps != sensors_enabled.gps) + { + bGPSBadHealth = true; + } + else + { + bGPSBadHealth = false; + } + if (sensors_health.gyro != sensors_enabled.gyro) + { + bGyroBadHealth = true; + } + else + { + bGyroBadHealth = false; + } + if (sensors_health.accelerometer != sensors_enabled.accelerometer) + { + bAccel = true; + } + else + { + bAccel = false; + } + if (sensors_health.compass != sensors_enabled.compass) + { + bCompass = true; + } + else + { + bCompass = false; + } + if (sensors_health.barometer != sensors_enabled.barometer) + { + bBarometer = true; + } + else + { + bBarometer = false; + } + FlightDistance2D = sysstatus.errors_count2; + var handler = ReceiveSysStatusEvent; + if (handler != null) + { + RunOnUIThread(() => + { + handler(this, buffer[5], bGPSBadHealth, bGyroBadHealth, bAccel, bCompass, bBarometer, sensors_health.CanTakeOff); + }); + } + } + + private void AnalyzeVfrHudPacket(byte[] buffer) + { + var vfr = buffer.ByteArrayToStructure(6); + groundspeed = vfr.groundspeed; + airspeed = vfr.airspeed; + altorigin = vfr.alt; // this might include baro + ch3percent = vfr.throttle; + heading = vfr.heading; + RaiseReceiveDataStreamEventOnUIThread(buffer[5]); + } + + /// + /// 飞控返回数据 处理通信模块发过来的28个字节数据 + /// + /// + public void AnalyzeCommMouldePositionIntPacket(byte[] buffer, byte version) + { + 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; + errorcode= (byte)info.error_code; + precheckok = info.rpecheck_fault == 0; + gpsalt = ((float)info.alt) / 1000; + satcount = info.gps_num_sats; + isUnlocked = info.lock_status == 1; + + battery_voltage = ((float)info.battery_voltage) / 1000; + + heading = (short)((info.heading / 100) % 360); + + commModuleVersion = version; + + 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 4a7107a..3e7273e 100644 --- a/PlaneGcsSdk_Shared/Copters/PlaneCopter.cs +++ b/PlaneGcsSdk_Shared/Copters/PlaneCopter.cs @@ -1,1676 +1,1680 @@ -// #define LOG_SEQUENCE_NUMBER -//#define LOG_PACKETS - -using Plane.Protocols; -using Plane.Communication; -using TaskTools.Utilities; -using System; -using System.Collections.Generic; -using System.Linq; -using System.Text; -using System.Threading; -using System.Diagnostics; -using System.IO; -using System.Threading.Tasks; - -namespace Plane.Copters -{ -#if PRIVATE - public -#else - internal -#endif - partial class PlaneCopter - { - uint iErrorData; - ulong iDataCount; - private int _recnumber = 0; - private int _sendnumber = 0; - - public bool IsConnected { get { return Connection.IsOpen; } } - - public string Id { get; set; } - - public byte mavlinkversion { get; set; } - public MAVLink.MAV_TYPE aptype { get; set; } - public MAVLink.MAV_AUTOPILOT apname { get; set; } - private bool CommOK = true; - public byte sysid { get; set; } - public byte compid { get; set; } - public byte recvpacketcount { get; set; } - public bool armed { get; set; } - public bool failsafe { get; set; } - - public uint mode { get; set; } - public uint commModuleMode { get; set; } - public byte commModuleVersion { get; set; } - - public bool isUnlocked { get; set; } - - public float battery_voltage { get; set; } - public byte battery_remaining { get; set; } - public float current_battery { get; set; } - public float freemem { get; set; } - public float brklevel { get; set; } - - public bool useLocation { get; set; } - public float gpsalt { get; set; } - public byte gpsstatus { get; set; } - public float gpshdop { get; set; } - public byte satcount { get; set; } - public float retain { get; set; } - public float groundspeed { get; set; } - public float groundcourse { get; set; } - public double lat { get; set; } - public double lng { get; set; } - public float nav_roll { get; set; } - public float nav_pitch { get; set; } - public short nav_bearing { get; set; } - public short target_bearing { get; set; } - public float wp_dist { get; set; } - public float alt_error { get; set; } - public float aspd_error { get; set; } - public float xtrack_error { get; set; } - public float roll { get; set; } - public float pitch { get; set; } - public float yaw { get; set; } - public short heading { get; set; } - public float airspeed { get; set; } - public float alt { get { return altorigin - altoffset; } } - public float altoffset = 0; - public float altorigin { get; private set; } = 0; - public float ch3percent { get; set; } - - public ushort ch1in { get; set; } - public ushort ch2in { get; set; } - public ushort ch3in { get; set; } - public ushort ch4in { get; set; } - public ushort ch5in { get; set; } - public ushort ch6in { get; set; } - public ushort ch7in { get; set; } - public ushort ch8in { get; set; } - public float rxrssi { get; set; } - - public byte remrssi { get; set; } - - public uint timebootms { get; set; } - - private int DataTimeOut = 0; - - public Dictionary param = new Dictionary(); - public Dictionary param_types = new Dictionary(); - - public IConnection Connection { get; set; } - - public ushort FlightDistance2D { get; private set; } - - private SynchronizationContext _uiSyncContext; - private bool _autoSendHeartbeat = true; - private bool _autoRequestData = true; - - public PlaneCopter(IConnection connection, SynchronizationContext uiSyncContext, bool autoSendHeartbeat = true, bool autoRequestData = true) - { - this.Connection = connection; - _uiSyncContext = uiSyncContext; - _autoSendHeartbeat = autoSendHeartbeat; - _autoRequestData = autoRequestData; - } - - public async Task ConnectAsync() - { - //sysid = 2; - SendReq = false; - CommOK = true; - await Connection.OpenAsync().ConfigureAwait(false); - StartCommunication(); - } - - public async Task DisconnectAsync() - { -#if DEBUG && !NETFX_CORE && LOG_SEQUENCE_NUMBER - File.AppendAllText($"SequenceNumbers_{Id}.log", _sequenceLog.ToString()); -#endif - await StopDatastreamAsync(MAVLink.MAV_DATA_STREAM.ALL, 0); - CommOK = false; - Connection.Close(); - } - - public async Task GetCopterDataAsync() - { - //停止所有数据流 - await StopGetDatastreamAsync(MAVLink.MAV_DATA_STREAM.ALL, 0).ConfigureAwait(false); - //需要电池电压，电流，GPS数据 - /* - * 目前接收的数据是 - * gps 38 字节 - hud 28 - SYS_STATUS 39 - MISSION_CURRENT 10 - HEARTBEAT 17 - 共132字节 - 5hz 592/s 太高 - */ - await RequestDatastreamAsync(MAVLink.MAV_DATA_STREAM.EXTENDED_STATUS, 1).ConfigureAwait(false); //10 1电池电压、电流、电量,152-内存、62-导航控制输出、24-GPS数据、42-航点数据 - //不需要姿态信息 - // await RequestDatastreamAsync(MAVLink.MAV_DATA_STREAM.EXTRA1, 1).ConfigureAwait(false); //5 请求 飞行姿态 roll、pitch、yaw、rollspeed、pitchspeed、yawspeed--- 30 - //需要高度和机头朝向数据 - await RequestDatastreamAsync(MAVLink.MAV_DATA_STREAM.EXTRA2, 1).ConfigureAwait(false); //10 请求vfr airspeed(空速)、groundspeed(地速)、alt(高度)、climb(爬升率)、heading(头朝向)、throttle(实际油门) ---74 - //不需要通道数据 - //await RequestDatastreamAsync(MAVLink.MAV_DATA_STREAM.RC_CHANNELS, 1).ConfigureAwait(false); //1 请求 通道数据 - //不需要原始数据 - // await RequestDatastreamAsync(MAVLink.MAV_DATA_STREAM.RAW_SENSORS, ratesensors).ConfigureAwait(false); // 请求 传感器原始值 - - /* //已经停止所有数据了，不用单独停止 - // Disable ATTITUDE_CONTROLLER_OUTPUT, POSITION_CONTROLLER_OUTPUT, NAV_CONTROLLER_OUTPUT. | - await StopGetDatastreamAsync(MAVLink.MAV_DATA_STREAM.RAW_CONTROLLER, 0).ConfigureAwait(false); - // Disable LOCAL_POSITION, GLOBAL_POSITION/GLOBAL_POSITION_INT messages. | - await StopGetDatastreamAsync(MAVLink.MAV_DATA_STREAM.POSITION, 0).ConfigureAwait(false); - // Disable EXTRA3 (Dependent on the autopilot |) - await StopGetDatastreamAsync(MAVLink.MAV_DATA_STREAM.EXTRA3, 0).ConfigureAwait(false); - // Disable Plane Stream - await StopGetDatastreamAsync(MAVLink.MAV_DATA_STREAM.GHOST_STREAM, 0).ConfigureAwait(false); - // Disable RAW_SENSORS - await StopGetDatastreamAsync(MAVLink.MAV_DATA_STREAM.RAW_SENSORS, 0).ConfigureAwait(false); // - // 5 Disable MSG_ATTITUDE roll、pitch、yaw、rollspeed、pitchspeed、yawspeed-- - 30 - await StopGetDatastreamAsync(MAVLink.MAV_DATA_STREAM.EXTRA1, 0).ConfigureAwait(false); // - */ - - } - - private async void StartCommunication() - { - try - { - var writingTask = _autoSendHeartbeat ? StartSendingHeartbeatsAsync() : TaskUtils.CompletedTask; - var readingTask = StartReadingPacketsAsync(); - await Task.WhenAll(writingTask, readingTask).ConfigureAwait(false); - } - catch (Exception ex) - { - RaiseMessageCreated(ex.ToString()); - } - } - -#if DEBUG && LOG_PACKETS - - private Stopwatch _sendHeartbeatStopwatch = new Stopwatch(); - private TimeSpan _sendHeartbeatTimeSpan; - -#endif - - public async Task StartSendingHeartbeatsAsync() - { - await Task.Run(async () => - { - while (CommOK) - { - if (!IsConnected) - { - break; - } - // if (heatbeatSend.Second != DateTime.Now.Second) - if (heatbeatSend.AddMilliseconds(1000) <= DateTime.Now) - { - var htb = new MAVLink.mavlink_heartbeat_t() - { - type = (byte)MAVLink.MAV_TYPE.GCS, - autopilot = (byte)MAVLink.MAV_AUTOPILOT.GENERIC, - mavlink_version = 3, - }; - // await SendPacketAsync(htb).ConfigureAwait(false); //发心跳包 -#if DEBUG && LOG_PACKETS - if (!_sendHeartbeatStopwatch.IsRunning) - { - _sendHeartbeatStopwatch.Start(); - } - else - { - var elapsed = _sendHeartbeatStopwatch.Elapsed; - Debug.WriteLine(elapsed - _sendHeartbeatTimeSpan, "--------------------------发送心跳间隔"); - _sendHeartbeatTimeSpan = elapsed; - } -#endif - heatbeatSend = DateTime.Now; - DataTimeOut += 1; - if (DataTimeOut > 3) - { - bInitChannel = false; - SendReq = false; - } - } - else - { - await Task.Delay(100).ConfigureAwait(false); - } - } - }).ConfigureAwait(false); - } - - private async Task StartReadingPacketsAsync() - { - await Task.Run(async () => - { - while (CommOK) - { - if (!IsConnected) - { - RaiseEventOnUIThread(ConnectionBroken); - break; - } - - var packet = await ReadPacketAsync().ConfigureAwait(false); - if (packet != null) - { - PacketReceived?.Invoke(this, new PacketReceivedEventArgs(packet)); - var handled = true; - // Debug.WriteLine(packet[5],"收到数据类型"); - switch (packet[5]) - { - case MAVLink.MAVLINK_MSG_ID_HEARTBEAT: //0 - AnalyzeHeartbeatPacket(packet); - break; - case MAVLink.MAVLINK_MSG_ID_SYS_STATUS: //1 - AnalyzeSysStatusPacket(packet); - break; - case MAVLink.MAVLINK_MSG_ID_GPS_RAW_INT: //24 - AnalyzeGpsRawIntPacket(packet); - break; - case MAVLink.MAVLINK_MSG_ID_VFR_HUD: //74 - AnalyzeVfrHudPacket(packet); - break; - case MAVLink.MAVLINK_MSG_ID_STATUSTEXT: //253 - AnalyzeStatusTextPacket(packet); - break; - - ////以上为基本数据， - - case MAVLink.MAVLINK_MSG_ID_RADIO: //109 - AnalyzeRadioPacket(packet); - break; - case MAVLink.MAVLINK_MSG_ID_PARAM_VALUE: //22 - AnalyzeParamValuePacket(packet); - break; - case MAVLink.MAVLINK_MSG_ID_MEMINFO: //152 - AnalyzeMemInfoPacket(packet); - break; - case MAVLink.MAVLINK_MSG_ID_MISSION_CURRENT: //42 - AnalyzeMissionCurrentPacket(packet); - break; - - case MAVLink.MAVLINK_MSG_ID_GLOBAL_POSITION_INT: //33 - AnalyzeGlobalPositionIntPacket(packet); - break; - case MAVLink.MAVLINK_MSG_ID_NAV_CONTROLLER_OUTPUT: //62 - AnalyzeNavControllerOutputPacket(packet); - break; - case MAVLink.MAVLINK_MSG_ID_ATTITUDE: //30 - AnalyzeAttitudePacket(packet); - break; - - case MAVLink.MAVLINK_MSG_ID_RC_CHANNELS_RAW: //35 - AnalyzeRCChannelsRawPacket(packet); - break; - case MAVLink.MAVLINK_MSG_ID_RAW_IMU: //没有用27 - AnalyzeRawImuPacket(packet); - break; - case MAVLink.MAVLINK_MSG_ID_SCALED_IMU2: //没有用 116 - AnalyzeScaledImu2Packet(packet); - break; - case MAVLink.MAVLINK_MSG_ID_SENSOR_OFFSETS: //150 - AnalyzeSensorOffsetsPacket(packet); - break; - - default: - handled = false; - UnhandledPacketReceived?.Invoke(this, new PacketReceivedEventArgs(packet)); - break; - } - if (handled) - { - PacketHandled?.Invoke(this, new PacketReceivedEventArgs(packet)); - } - } - } - }).ConfigureAwait(false); - } - - public async Task GetParamsListAsync() - { - MAVLink.mavlink_param_request_list_t req = new MAVLink.mavlink_param_request_list_t(); - req.target_system = sysid; - req.target_component = compid; - await GeneratePacketAsync(MAVLink.MAVLINK_MSG_ID_PARAM_REQUEST_LIST, req); - } - - public static List> getModesList() - { - var flightModes = EnumTranslator.Translate(); - return flightModes.ToList(); - } - - // 王海, 20151029, 由于只在 myComm 循环中读取数据包，因此去掉了锁。 - //volatile object readlock = new object(); - -#if DEBUG && LOG_PACKETS - private StringBuilder _log = new StringBuilder(); - private Stopwatch _positionStopwatch = new Stopwatch(); - private TimeSpan _lastPositionTimeSpan; -#endif - -#if DEBUG && LOG_SEQUENCE_NUMBER - private StringBuilder _sequenceLog = new StringBuilder("-----------------------\n"); -#endif - - private byte[] _readBuffer = new byte[263]; - - /// - /// 异步读取数据包。// 王海, 20151029, 去掉了锁，不要在除读数据包循环之外的地方调用此方法。 - /// - /// 表示此异步操作的 实例，其结果为读取到的数据包。 - private async Task ReadPacketAsync() - { - if (!Connection.IsOpen) - { - return null; - } - - int length = 0; - int readnumber = 0; //_recnumber; - try - { - readnumber = await Connection.ReadAsync(_readBuffer, 0, 1); - _recnumber += readnumber; - if (readnumber == 0) - { - return null; - } - if (_readBuffer[0] == MAVLink.MAVLINK_STX) - { - readnumber = await Connection.ReadAsync(_readBuffer, 1, 5); - _recnumber += readnumber; - if (readnumber == 0) - { - return null; - } - -#if DEBUG && LOG_SEQUENCE_NUMBER - // _sequenceLog.Append(DateTime.Now.ToString("HHmmss.fff")).Append('\t').Append(string.Format("{0,3}", _readBuffer[2].ToString())).Append('\t').Append(string.Format("{0,3}", _readBuffer[5].ToString())); - _sequenceLog.Append(DateTime.Now.ToString()).Append('\t').Append(string.Format("{0,3}", _readBuffer[2].ToString())).Append('\t').Append(string.Format("{0,3}", _readBuffer[5].ToString())); -#endif - // packet length - length = 6 + _readBuffer[1] + 2; // header + data + checksum - readnumber = await Connection.ReadAsync(_readBuffer, 6, length - 6); - _recnumber += readnumber; - if (readnumber == 0) - { - return null; - } - } - else - { -#if DEBUG && LOG_PACKETS - _log.AppendLine("---------------"); - _log.AppendLine("unexpected head: " + _readBuffer[0]); -#endif - - return null; - } - } - catch - { - return null; - } - - uint crcvalue = BitConverter.ToUInt16(_readBuffer, length - 2); - -#if DEBUG && LOG_PACKETS - _log.AppendLine("---------------"); - for (int i = 0; i < 6; i++) - { - _log.Append(_readBuffer[i]).Append(" "); - } - _log.AppendLine(); - for (int i = 6; i < length - 2; i++) - { - _log.Append(_readBuffer[i]).Append(" "); - } - _log.AppendLine(); - for (int i = length - 2; i < length; i++) - { - _log.Append(_readBuffer[i]).Append(" "); - } - _log.AppendLine(); -#endif - byte messageType = _readBuffer[5]; - -#if DEBUG && LOG_PACKETS - if (messageType == 33) - { - if (_positionStopwatch.IsRunning) - { - var elapsed = _positionStopwatch.Elapsed; - Debug.WriteLine((elapsed - _lastPositionTimeSpan).TotalMilliseconds, "-----------------------------------------------position packet"); - _lastPositionTimeSpan = elapsed; - } - else - { - _positionStopwatch.Start(); - } - } -#endif - - ushort checksum = MavlinkCRC.crc_calculate(_readBuffer, length - 2); - checksum = MavlinkCRC.crc_accumulate(MAVLink.MAVLINK_MESSAGE_CRCS[messageType], checksum); - iDataCount += 1; - -#if DEBUG && LOG_SEQUENCE_NUMBER - _sequenceLog.Append('\t').Append(crcvalue == checksum).AppendLine(); -#endif - - if (crcvalue == checksum) - { - return _readBuffer; - } - else - { -#if DEBUG && LOG_PACKETS - _log.AppendLine("crc not passed"); -#endif - - iErrorData += 1; - var handler = ErrorDataEvent; - if (handler != null) - { - RunOnUIThread(() => handler(iErrorData, iDataCount, _readBuffer[5])); - } - return null; - } - } - - public MAVState MAV = new MAVState(); - - /// - /// Used to extract mission from log file - both sent or received - /// - /// packet - void getWPsfromstream(ref byte[] buffer) - { - if (buffer[5] == (byte)MAVLink.MAVLINK_MSG_ID_MISSION_COUNT) - { - // clear old - MAV.wps.Clear(); - } - - if (buffer[5] == (byte)MAVLink.MAVLINK_MSG_ID_MISSION_ITEM) - { - MAVLink.mavlink_mission_item_t wp = buffer.ByteArrayToStructure(6); - - if (wp.current == 2) - { - // guide mode wp - MAV.GuidedMode = wp; - } - else - { - MAV.wps[wp.seq] = wp; - } - - //Console.WriteLine("WP # {7} cmd {8} p1 {0} p2 {1} p3 {2} p4 {3} x {4} y {5} z {6}", wp.param1, wp.param2, wp.param3, wp.param4, wp.x, wp.y, wp.z, wp.seq, wp.command); - } - - if (buffer[5] == (byte)MAVLink.MAVLINK_MSG_ID_RALLY_POINT) - { - MAVLink.mavlink_rally_point_t rallypt = buffer.ByteArrayToStructure(6); - - MAV.rallypoints[rallypt.idx] = rallypt; - - //Console.WriteLine("RP # {0} {1} {2} {3} {4}", rallypt.idx, rallypt.lat, rallypt.lng, rallypt.alt, rallypt.break_alt); - } - - if (buffer[5] == (byte)MAVLink.MAVLINK_MSG_ID_FENCE_POINT) - { - MAVLink.mavlink_fence_point_t fencept = buffer.ByteArrayToStructure(6); - - MAV.fencepoints[fencept.idx] = fencept; - } - } - - - /// - /// used for outbound packet sending - /// - public int packetcount = 0; - private bool SendReq; -#pragma warning disable CS0414 // The field is assigned but its value is never used - private bool giveComport; -#pragma warning restore CS0414 // The field is assigned but its value is never used - - public async Task GeneratePacketAsync(byte messageType, TMavlinkPacket indata) - { - byte[] data; - - if (mavlinkversion == 3) - { - data = MavlinkUtil.StructureToByteArray(indata); - } - else - { - data = MavlinkUtil.StructureToByteArrayBigEndian(indata); - } - // data = MavlinkUtil.StructureToByteArrayBigEndian(indata); - //Console.WriteLine(DateTime.Now + " PC Doing req "+ messageType + " " + this.BytesToRead); - byte[] packet = new byte[data.Length + 6 + 2]; - - if (mavlinkversion == 3 || mavlinkversion == 0) - { - packet[0] = MAVLink.MAVLINK_STX; - } - else if (mavlinkversion == 2) - { - packet[0] = (byte)'U'; - } - packet[1] = (byte)(data.Length); - packet[2] = (byte)packetcount; - //packet[2] = 35; - packetcount++; - - packet[3] = 255; // this is always 255 - MYGCS - - packet[4] = (byte)MAVLink.MAV_COMPONENT.MAV_COMP_ID_MISSIONPLANNER; - - packet[5] = messageType; - - int i = 6; - foreach (byte b in data) - { - packet[i] = b; - i++; - } - - ushort checksum = MavlinkCRC.crc_calculate(packet, packet[1] + 6); - - if (mavlinkversion == 3 || mavlinkversion == 0) - { - checksum = MavlinkCRC.crc_accumulate(MAVLink.MAVLINK_MESSAGE_CRCS[messageType], checksum); - } - - byte ck_a = (byte)(checksum & 0xFF); ///< High byte - byte ck_b = (byte)(checksum >> 8); ///< Low byte - - packet[i] = ck_a; - i += 1; - packet[i] = ck_b; - i += 1; - - if (IsConnected) - { - try - { - await Connection.WriteAsync(packet, 0, i); - // Console.WriteLine("sendout:" + i); - _sendnumber += i; //统计发送数量 - } - catch - { - // 王海, 20151031, 异常已通过 IConnection.ExceptionThrown 事件传到外部，此处吞掉就行了。 - } - } - } - - - public async Task GenerateRTKPacketAsync(byte messageType, TMavlinkPacket indata,int rtklength) - { - byte[] data; - int rtkdataleng= rtklength + 3; - if (mavlinkversion == 3) - { - data = MavlinkUtil.StructureToByteArray(indata); - } - else - { - data = MavlinkUtil.StructureToByteArrayBigEndian(indata); - } - // data = MavlinkUtil.StructureToByteArrayBigEndian(indata); - //Console.WriteLine(DateTime.Now + " PC Doing req "+ messageType + " " + this.BytesToRead); - - byte[] packet = new byte[rtkdataleng + 6 + 2]; - - if (mavlinkversion == 3 || mavlinkversion == 0) - { - packet[0] = MAVLink.MAVLINK_STX; - } - else if (mavlinkversion == 2) - { - packet[0] = (byte)'U'; - } - packet[1] = (byte)(rtkdataleng); - packet[2] = (byte)packetcount; - //packet[2] = 35; - packetcount++; - - packet[3] = 255; // this is always 255 - MYGCS - - packet[4] = (byte)MAVLink.MAV_COMPONENT.MAV_COMP_ID_MISSIONPLANNER; - - packet[5] = messageType; - - int i = 6; - foreach (byte b in data) - { - if (i < rtkdataleng + 6 ) - packet[i] = b; - else break; - i++; - } - - ushort checksum = MavlinkCRC.crc_calculate(packet, packet[1] + 6); - - if (mavlinkversion == 3 || mavlinkversion == 0) - { - checksum = MavlinkCRC.crc_accumulate(MAVLink.MAVLINK_MESSAGE_CRCS[messageType], checksum); - } - - byte ck_a = (byte)(checksum & 0xFF); ///< High byte - byte ck_b = (byte)(checksum >> 8); ///< Low byte - - packet[i] = ck_a; - i += 1; - packet[i] = ck_b; - i += 1; - - if (IsConnected) - { - try - { - await Connection.WriteAsync(packet, 0, i); - //Console.WriteLine("sendout:" + i); - _sendnumber += i; //统计发送数量 - } - catch - { - // 王海, 20151031, 异常已通过 IConnection.ExceptionThrown 事件传到外部，此处吞掉就行了。 - } - } - } - - public async Task DoLEDAsync() - { - return await DoLEDCommandAsync(); - } - - public async Task DoLEDCommandAsync() - { - giveComport = true; - MAVLink.mavlink_msg_id_led_control led = new MAVLink.mavlink_msg_id_led_control(); - led.target_system = sysid; - led.target_component = compid;//(byte)MAVLink.MAV_COMPONENT.MAV_COMP_ID_SYSTEM_CONTROL;; - led.instance = 0; - led.pattern = 0; - led.custom_len = 4; - led.custom_bytes = new byte[24]; - led.custom_bytes[0] = 255; - led.custom_bytes[1] = 255; - led.custom_bytes[2] = 255; - led.custom_bytes[3] = 2; - - await GeneratePacketAsync(MAVLink.MAVLINK_MSG_ID_LED_CONTROL, led); - return true; - } - - public async Task DoARMAsync(bool armit) - { - return await DoCommandAsync(MAVLink.MAV_CMD.COMPONENT_ARM_DISARM, armit ? 1 : 0, 21196, 0, 0, 0, 0, 0); - } - - public async Task DoMotorTestAsync(int motor, MAVLink.MOTOR_TEST_THROTTLE_TYPE thr_type, int throttle, int timeout) - { - return await DoCommandAsync(MAVLink.MAV_CMD.DO_MOTOR_TEST, (float)motor, (float)(byte)thr_type, (float)throttle, (float)timeout, 0, 0, 0); - } - - public async Task DoTakeoffAsync() - { - /// Takeoff from ground / hand |Minimum pitch (if airspeed sensor present), desired pitch without sensor| Empty| Empty| Yaw angle (if magnetometer present), ignored without magnetometer| Latitude| Longitude| Altitude| - return await DoCommandAsync(MAVLink.MAV_CMD.TAKEOFF, 0, 0, 0, -1, 0, 0, 15); - } - public async Task DoMissionStartAsync(int hour_utc, int minute_utc, int second_utc, double Missionlng, double Missionlat) - { - /// Takeoff from ground / hand |Minimum pitch (if airspeed sensor present), desired pitch without sensor| Empty| Empty| Yaw angle (if magnetometer present), ignored without magnetometer| Latitude| Longitude| Altitude| - return await DoCommandAsync(MAVLink.MAV_CMD.MISSION_START, hour_utc, minute_utc, second_utc, (float)Missionlng, (float)Missionlat, 0, 0); - } - - - public async Task DoCommandAsync(MAVLink.MAV_CMD actionid, float p1, float p2, float p3, float p4, float p5, float p6, float p7) - { - giveComport = true; - //byte[] buffer; - - MAVLink.mavlink_command_long_t req = new MAVLink.mavlink_command_long_t(); - - req.target_system = sysid; - req.target_component = compid; - - if (actionid == MAVLink.MAV_CMD.COMPONENT_ARM_DISARM) - { - req.target_component = (byte)MAVLink.MAV_COMPONENT.MAV_COMP_ID_SYSTEM_CONTROL; - } - - req.command = (ushort)actionid; - - - Console.WriteLine("P1 = " + p1); - req.param1 = p1; - req.param2 = p2; - req.param3 = p3; - req.param4 = p4; - req.param5 = p5; - req.param6 = p6; - req.param7 = p7; - - await GeneratePacketAsync(MAVLink.MAVLINK_MSG_ID_COMMAND_LONG, req); - return true; - - //DateTime start = DateTime.Now; - //int retrys = 3; - - //int timeout = 2000; - - ////// imu calib take a little while - //if (actionid == MAVLink.MAV_CMD.PREFLIGHT_CALIBRATION && p5 == 1) - //{ - // // this is for advanced accel offsets, and blocks execution - // return true; - //} - //else if (actionid == MAVLink.MAV_CMD.PREFLIGHT_CALIBRATION) - //{ - // retrys = 1; - // timeout = 25000; - //} - //else if (actionid == MAVLink.MAV_CMD.PREFLIGHT_REBOOT_SHUTDOWN) - //{ - // await GeneratePacketAsync(MAVLink.MAVLINK_MSG_ID_COMMAND_LONG, req); - // giveComport = false; - // return true; - //} - //else if (actionid == MAVLink.MAV_CMD.COMPONENT_ARM_DISARM) - //{ - // // 10 seconds as may need an imu calib - // timeout = 10000; - //} - - //while (true) - //{ - // if (!(start.AddMilliseconds(timeout) > DateTime.Now)) - // { - // if (retrys > 0) - // { - // //log.Info("doAction Retry " + retrys); - // await GeneratePacketAsync(MAVLink.MAVLINK_MSG_ID_COMMAND_LONG, req); - // start = DateTime.Now; - // retrys--; - // continue; - // } - // giveComport = false; - // throw new Exception("Timeout on read - doAction"); - // } - - // buffer = readPacket(); - // if (buffer.Length > 5) - // { - // if (buffer[5] == MAVLink.MAVLINK_MSG_ID_COMMAND_ACK) - // { - // var ack = buffer.ByteArrayToStructure(6); - // if (ack.result == (byte)MAVLink.MAV_RESULT.ACCEPTED) - // { - // giveComport = false; - // return true; - // } - // else - // { - // giveComport = false; - // return false; - // } - // } - // } - //} - } - - - public double GetBearing(float lat1,float lng1,float lat2,float lng2) - { - var latitude1 = Math.PI / 180*lat1; - var latitude2 = Math.PI / 180 * lat2; - var longitudeDifference = Math.PI / 180*(lng2 - lng1); - - var y = Math.Sin(longitudeDifference) * Math.Cos(latitude2); - var x = Math.Cos(latitude1) * Math.Sin(latitude2) - Math.Sin(latitude1) * Math.Cos(latitude2) * Math.Cos(longitudeDifference); - - return (180 / Math.PI*(Math.Atan2(y, x)) + 360) % 360; - } - - - public async Task SetGuidedModeWPAsync(Locationwp gotohere) - { - //if (gotohere.alt == 0 || gotohere.lat == 0 || gotohere.lng == 0) - // return; - - //giveComport = true; - - //try - //{ - //gotohere.id = (byte)MAVLink.MAV_CMD.WAYPOINT; - //老方法无法设置速度 - // await SetGuidedWPAsync(gotohere, 0, MAVLink.MAV_FRAME.GLOBAL_RELATIVE_ALT, (byte)2); - //} - //catch { } - - //giveComport = false; - - - //老方法无法设置速度 - await SetWPAsync(gotohere, 0, MAVLink.MAV_FRAME.GLOBAL_RELATIVE_ALT, (byte)2); - - /* - TaskTools.HIL.Vector3 targetVelocity = new TaskTools.HIL.Vector3(); - float targspeed = (float)2.0; //目标速度 - double targetbearing= GetBearing((float)lat, (float)lng, (float)gotohere.lat, (float)gotohere.lng); //目标机头方向 - targetVelocity.x = Math.Cos(targetbearing * 180 / Math.PI) * targspeed; //分解速度 - targetVelocity.y = Math.Sin(targetbearing * 180 / Math.PI) * targspeed; //分解速度 - // await setPositionTargetGlobalInt(true, false, false, MAVLink.MAV_FRAME.GLOBAL_RELATIVE_ALT, - // gotohere.lat, gotohere.lng, gotohere.alt, targetVelocity.x, targetVelocity.y, 2.0); - - await setPositionTargetGlobalInt(true, false, false, MAVLink.MAV_FRAME.GLOBAL_RELATIVE_ALT, - gotohere.lat, gotohere.lng, gotohere.alt, 0, 0, 0); - */ - - - } - - - - - public async Task setPositionTargetGlobalInt( bool pos, bool vel, bool acc, MAVLink.MAV_FRAME frame, double lat, double lng, double alt, double vx, double vy, double vz) - { - MAVLink.mavlink_set_position_target_global_int_t target = new MAVLink.mavlink_set_position_target_global_int_t() - { - target_system = sysid, - target_component = compid, - alt = (float)alt, - lat_int = (int)(lat * 1e7), - lon_int = (int)(lng * 1e7), - coordinate_frame = (byte)frame, - vx = (float)vx, - vy = (float)vy, - vz = (float)vz - }; - - target.type_mask = 7 + 56 + 448; - - if (pos) - target.type_mask -= 7; - if (vel) - target.type_mask -= 56; - if (acc) - target.type_mask -= 448; - - await GeneratePacketAsync(MAVLink.SET_POSITION_TARGET_GLOBAL_INT, target); - - } - - - public async Task SetNewWPAltAsync(Locationwp gotohere) - { - //givecomport = true; - - //try - //{ - gotohere.id = (byte)MAVLink.MAV_CMD.WAYPOINT; - - //MAVLink.MAV_MISSION_RESULT ans = - await SetWPAsync(gotohere, 0, MAVLink.MAV_FRAME.GLOBAL_RELATIVE_ALT, (byte)3); - - // if (ans != mavlink.mav_mission_result.mav_mission_accepted) - // throw new exception("alt change failed"); - //} - //catch { givecomport = false; /*log.error(ex);*/ throw; } - - //givecomport = false; - } - - public async Task SetWPAsync(Locationwp loc, ushort index, MAVLink.MAV_FRAME frame, byte current = 0) - { - giveComport = true; - MAVLink.mavlink_mission_item_t req = new MAVLink.mavlink_mission_item_t(); - - req.target_system = sysid; - req.target_component = compid; // MAVLINK_MSG_ID_MISSION_ITEM - - req.command = loc.id; - - - req.current = current; - - req.frame = (byte)frame; - - req.x = (float)(loc.lat); - req.y = (float)(loc.lng); - req.z = (float)(loc.alt); - - req.param1 = loc.p1; - req.param2 = loc.p2; - req.param3 = loc.p3; - req.param4 = loc.p4; - - req.seq = index; - - //log.InfoFormat("setWP {6} frame {0} cmd {1} p1 {2} x {3} y {4} z {5}", req.frame, req.command, req.param1, req.x, req.y, req.z, index); - - // request - await GeneratePacketAsync(MAVLink.MAVLINK_MSG_ID_MISSION_ITEM, req); - } - - public async Task SetChannelsAsync(ushort? ch1 = null, ushort? ch2 = null, ushort? ch3 = null, ushort? ch4 = null, ushort? ch5 = null, ushort? ch6 = null, ushort? ch7 = null, ushort? ch8 = null) - { - //某些情况下ch4设置成0会导致自旋 - ushort checkyaw = ch4 ?? ch4in; - if ((checkyaw < 1200)|| (checkyaw > 1800)) - checkyaw = 1500; - //// - MAVLink.mavlink_rc_channels_override_t rc = new MAVLink.mavlink_rc_channels_override_t(); - rc.target_component = compid; - rc.target_system = sysid; - rc.chan1_raw = ch1 ?? ch1in; - rc.chan2_raw = ch2 ?? ch2in; - rc.chan3_raw = ch3 ?? ch3in; - rc.chan4_raw = checkyaw; - rc.chan5_raw = ch5 ?? ch5in; - rc.chan6_raw = ch6 ?? ch6in; - rc.chan7_raw = ch7 ?? ch7in; - rc.chan8_raw = ch8 ?? ch8in; - await SendPacketAsync(rc); - } - - public async Task SetMobileControlAsync(ushort? ch1 = null, ushort? ch2 = null, ushort? ch3 = null, ushort? ch4 = null, ushort? ch5 = null, ushort? ch6 = null, ushort? ch7 = null, ushort? ch8 = null, float? yaw = null, float? alt = null) - { - /* MAVLink.mavlink_rc_mobile_control_t rc = new MAVLink.mavlink_rc_mobile_control_t(); - rc.target_component = compid; - rc.target_system = sysid; - rc.chan1_raw = ch1 ?? ch1in; - rc.chan2_raw = ch2 ?? ch2in; - rc.chan3_raw = ch3 ?? ch3in; - rc.chan4_raw = ch4 ?? ch4in; - rc.chan5_raw = ch5 ?? ch5in; - rc.chan6_raw = ch6 ?? ch6in; - rc.chan7_raw = ch7 ?? ch7in; - rc.chan8_raw = ch8 ?? ch8in; - rc.Yaw = yaw ?? this.yaw; - rc.alt = alt ?? this.alt; - await SendPacketAsync(rc); - */ - float ch4yaw; - ch4yaw = yaw ?? this.yaw; - if (ch4yaw != this.yaw) - { - ch4yaw = ch4yaw % 360; - await DoCommandAsync(MAVLink.MAV_CMD.CONDITION_YAW, ch4yaw, 0, 1, 0, 0, 0, 0); - } - - /* - float ch4yaw; - MAVLink.mavlink_rc_channels_override_t rc = new MAVLink.mavlink_rc_channels_override_t(); - rc.target_component = compid; - rc.target_system = sysid; - rc.chan1_raw = ch1 ?? ch1in; - rc.chan2_raw = ch2 ?? ch2in; - rc.chan3_raw = ch3 ?? ch3in; - rc.chan4_raw = 1500; - rc.chan5_raw = ch5 ?? ch5in; - rc.chan6_raw = ch6 ?? ch6in; - rc.chan7_raw = ch7 ?? ch7in; - rc.chan8_raw = ch8 ?? ch8in; - ch4yaw= yaw ?? this.yaw; - rc.chan4_raw =(ushort)ch4yaw; - await SendPacketAsync(rc); - */ - - } - - public async Task GetParamAsync(string paramName, int millisecondsTimeout = Timeout.Infinite) - { - if (param.ContainsKey(paramName)) param[paramName] = null; - var paramId = Encoding.ASCII.GetBytes(paramName); - Array.Resize(ref paramId, 16); - var req = new MAVLink.mavlink_param_request_read_t - { - param_id = paramId, - param_index = -1, - target_component = compid, - target_system = sysid - }; - var stopwatch = Stopwatch.StartNew(); - while (true) - { - for (int ii = 0; ii < 5; ii++) - { - await SendPacketAsync(req); - await Task.Delay(10).ConfigureAwait(false); - } - - int i = 0; - for (; i < 5 && (!param.ContainsKey(paramName) || param[paramName] == null); i++) - { - await Task.Delay(50).ConfigureAwait(false); - } - if (i < 5) - { - stopwatch.Stop(); - break; - } - else if (millisecondsTimeout != Timeout.Infinite && stopwatch.ElapsedMilliseconds >= millisecondsTimeout) - { - stopwatch.Stop(); - throw new TimeoutException("The GetParamAsync operation has timed out."); - } - } - return param[paramName].Value; - } - - public async Task SendPacketAsync(TMavlinkPacket indata) - { - bool validPacket = false; - byte a = 0; - var packetType = indata.GetType(); - foreach (Type ty in MAVLink.MAVLINK_MESSAGE_INFO) - { - if (ty == packetType) - { - validPacket = true; - await GeneratePacketAsync(a, indata); - return; - } - a++; - } - if (!validPacket) - { - RaiseMessageCreated("The packet type is not valid."); - //log.Info("Mavlink : NOT VALID PACKET await SendPacketAsync() " + indata.GetType().ToString()); - } - } - - public double[] packetspersecond { get; set; } - DateTime[] packetspersecondbuild = new DateTime[256]; - - byte ratesensors = 1; - public byte RateSensors { get { return ratesensors; } set { ratesensors = value; } } - - private DateTime heatbeatSend; - private bool bInitChannel = false; - private bool bGPSBadHealth; - private bool bGyroBadHealth; - private bool bAccel; - private bool bCompass; - private bool bBarometer; - - public short gx; - public short gy; - public short gz; - public short ax; - public short ay; - public short az; - public short mx; - public short my; - public short mz; - - public short gx2; - public short gy2; - public short gz2; - public short ax2; - public short ay2; - public short az2; - public short mx2; - public short my2; - public short mz2; - - public float mag_ofs_x; - public float mag_ofs_y; - public float mag_ofs_z; - public float mag_declination; - public int raw_press; - public int raw_temp; - public float gyro_cal_x; - public float gyro_cal_y; - public float gyro_cal_z; - public float accel_cal_x; - public float accel_cal_y; - public float accel_cal_z; - - public ushort param_total; - public ushort WpCount; - - public async Task RequestDatastreamAsync(MAVLink.MAV_DATA_STREAM id, byte hzrate) - { - - double pps = 0; - - switch (id) - { - case MAVLink.MAV_DATA_STREAM.ALL: - - break; - case MAVLink.MAV_DATA_STREAM.EXTENDED_STATUS: - if (packetspersecondbuild[MAVLink.MAVLINK_MSG_ID_SYS_STATUS] < DateTime.Now.AddSeconds(-2)) - break; - pps = packetspersecond[MAVLink.MAVLINK_MSG_ID_SYS_STATUS]; - if (hzratecheck(pps, hzrate)) - { - return; - } - break; - case MAVLink.MAV_DATA_STREAM.EXTRA1: - if (packetspersecondbuild[MAVLink.MAVLINK_MSG_ID_ATTITUDE] < DateTime.Now.AddSeconds(-2)) - break; - pps = packetspersecond[MAVLink.MAVLINK_MSG_ID_ATTITUDE]; - if (hzratecheck(pps, hzrate)) - { - return; - } - break; - case MAVLink.MAV_DATA_STREAM.EXTRA2: - if (packetspersecondbuild[MAVLink.MAVLINK_MSG_ID_VFR_HUD] < DateTime.Now.AddSeconds(-2)) - break; - pps = packetspersecond[MAVLink.MAVLINK_MSG_ID_VFR_HUD]; - if (hzratecheck(pps, hzrate)) - { - return; - } - break; - case MAVLink.MAV_DATA_STREAM.EXTRA3: - if (packetspersecondbuild[MAVLink.MAVLINK_MSG_ID_AHRS] < DateTime.Now.AddSeconds(-2)) - break; - pps = packetspersecond[MAVLink.MAVLINK_MSG_ID_AHRS]; - if (hzratecheck(pps, hzrate)) - { - return; - } - break; - case MAVLink.MAV_DATA_STREAM.POSITION: - if (packetspersecondbuild[MAVLink.MAVLINK_MSG_ID_GLOBAL_POSITION_INT] < DateTime.Now.AddSeconds(-2)) - break; - pps = packetspersecond[MAVLink.MAVLINK_MSG_ID_GLOBAL_POSITION_INT]; - if (hzratecheck(pps, hzrate)) - { - return; - } - break; - case MAVLink.MAV_DATA_STREAM.RAW_CONTROLLER: - if (packetspersecondbuild[MAVLink.MAVLINK_MSG_ID_RC_CHANNELS_SCALED] < DateTime.Now.AddSeconds(-2)) - break; - pps = packetspersecond[MAVLink.MAVLINK_MSG_ID_RC_CHANNELS_SCALED]; - if (hzratecheck(pps, hzrate)) - { - return; - } - break; - case MAVLink.MAV_DATA_STREAM.RAW_SENSORS: - if (packetspersecondbuild[MAVLink.MAVLINK_MSG_ID_RAW_IMU] < DateTime.Now.AddSeconds(-2)) - break; - pps = packetspersecond[MAVLink.MAVLINK_MSG_ID_RAW_IMU]; - if (hzratecheck(pps, hzrate)) - { - return; - } - break; - case MAVLink.MAV_DATA_STREAM.RC_CHANNELS: - if (packetspersecondbuild[MAVLink.MAVLINK_MSG_ID_RC_CHANNELS_RAW] < DateTime.Now.AddSeconds(-2)) - break; - pps = packetspersecond[MAVLink.MAVLINK_MSG_ID_RC_CHANNELS_RAW]; - if (hzratecheck(pps, hzrate)) - { - return; - } - break; - } - - //packetspersecond[temp[5]]; - - if (pps == 0 && hzrate == 0) - { - return; - } - - // log.InfoFormat("Request stream {0} at {1} hz", Enum.Parse(typeof(MAV_DATA_STREAM), id.ToString()), hzrate); - await GetDatastreamAsync(id, hzrate); - } - - public async Task StopDatastreamAsync(MAVLink.MAV_DATA_STREAM id, byte hzrate) - { - - double pps = 0; - - switch (id) - { - case MAVLink.MAV_DATA_STREAM.ALL: - - break; - case MAVLink.MAV_DATA_STREAM.EXTENDED_STATUS: - if (packetspersecondbuild[MAVLink.MAVLINK_MSG_ID_SYS_STATUS] < DateTime.Now.AddSeconds(-2)) - break; - pps = packetspersecond[MAVLink.MAVLINK_MSG_ID_SYS_STATUS]; - if (hzratecheck(pps, hzrate)) - { - return; - } - break; - case MAVLink.MAV_DATA_STREAM.EXTRA1: - if (packetspersecondbuild[MAVLink.MAVLINK_MSG_ID_ATTITUDE] < DateTime.Now.AddSeconds(-2)) - break; - pps = packetspersecond[MAVLink.MAVLINK_MSG_ID_ATTITUDE]; - if (hzratecheck(pps, hzrate)) - { - return; - } - break; - case MAVLink.MAV_DATA_STREAM.EXTRA2: - if (packetspersecondbuild[MAVLink.MAVLINK_MSG_ID_VFR_HUD] < DateTime.Now.AddSeconds(-2)) - break; - pps = packetspersecond[MAVLink.MAVLINK_MSG_ID_VFR_HUD]; - if (hzratecheck(pps, hzrate)) - { - return; - } - break; - case MAVLink.MAV_DATA_STREAM.EXTRA3: - if (packetspersecondbuild[MAVLink.MAVLINK_MSG_ID_AHRS] < DateTime.Now.AddSeconds(-2)) - break; - pps = packetspersecond[MAVLink.MAVLINK_MSG_ID_AHRS]; - if (hzratecheck(pps, hzrate)) - { - return; - } - break; - case MAVLink.MAV_DATA_STREAM.POSITION: - if (packetspersecondbuild[MAVLink.MAVLINK_MSG_ID_GLOBAL_POSITION_INT] < DateTime.Now.AddSeconds(-2)) - break; - pps = packetspersecond[MAVLink.MAVLINK_MSG_ID_GLOBAL_POSITION_INT]; - if (hzratecheck(pps, hzrate)) - { - return; - } - break; - case MAVLink.MAV_DATA_STREAM.RAW_CONTROLLER: - if (packetspersecondbuild[MAVLink.MAVLINK_MSG_ID_RC_CHANNELS_SCALED] < DateTime.Now.AddSeconds(-2)) - break; - pps = packetspersecond[MAVLink.MAVLINK_MSG_ID_RC_CHANNELS_SCALED]; - if (hzratecheck(pps, hzrate)) - { - return; - } - break; - case MAVLink.MAV_DATA_STREAM.RAW_SENSORS: - if (packetspersecondbuild[MAVLink.MAVLINK_MSG_ID_RAW_IMU] < DateTime.Now.AddSeconds(-2)) - break; - pps = packetspersecond[MAVLink.MAVLINK_MSG_ID_RAW_IMU]; - if (hzratecheck(pps, hzrate)) - { - return; - } - break; - //case MAVLink.MAV_DATA_STREAM.SCALED_IMU2: - - // break; - case MAVLink.MAV_DATA_STREAM.RC_CHANNELS: - if (packetspersecondbuild[MAVLink.MAVLINK_MSG_ID_RC_CHANNELS_RAW] < DateTime.Now.AddSeconds(-2)) - break; - pps = packetspersecond[MAVLink.MAVLINK_MSG_ID_RC_CHANNELS_RAW]; - if (hzratecheck(pps, hzrate)) - { - return; - } - break; - } - //log.InfoFormat("Request stream {0} at {1} hz", Enum.Parse(typeof(MAV_DATA_STREAM), id.ToString()), hzrate); - await StopGetDatastreamAsync(id, hzrate); - } - - bool hzratecheck(double pps, int hzrate) - { - - if (hzrate == 0 && pps == 0) - { - return true; - } - else if (hzrate == 1 && pps >= 0.5 && pps <= 2) - { - return true; - } - else if (hzrate == 3 && pps >= 2 && hzrate < 5) - { - return true; - } - else if (hzrate == 10 && pps > 5 && hzrate < 15) - { - return true; - } - else if (hzrate > 15 && pps > 15) - { - return true; - } - - return false; - - } - - async Task GetDatastreamAsync(MAVLink.MAV_DATA_STREAM id, byte hzrate) - { - MAVLink.mavlink_request_data_stream_t req = new MAVLink.mavlink_request_data_stream_t(); - req.target_system = sysid; - req.target_component = compid; - - req.req_message_rate = hzrate; - req.start_stop = 1; // start - req.req_stream_id = (byte)id; // id - - - // send each one twice. - await GeneratePacketAsync(MAVLink.MAVLINK_MSG_ID_REQUEST_DATA_STREAM, req); - await GeneratePacketAsync(MAVLink.MAVLINK_MSG_ID_REQUEST_DATA_STREAM, req); - } - public void GetCommunicationNumber(out int recnumber, out int sendnumber) - { - recnumber=_recnumber; - sendnumber = _sendnumber; - } - - - //重设数据量 - public void ResetCommunicationNumber() - { - _recnumber = 0; - _sendnumber = 0; - } - async Task StopGetDatastreamAsync(MAVLink.MAV_DATA_STREAM id, byte hzrate) - { - MAVLink.mavlink_request_data_stream_t req = new MAVLink.mavlink_request_data_stream_t(); - req.target_system = sysid; - req.target_component = compid; - - req.req_message_rate = 0; - req.start_stop = 0; // start - req.req_stream_id = (byte)id; // id - - // send each one twice. - await GeneratePacketAsync(MAVLink.MAVLINK_MSG_ID_REQUEST_DATA_STREAM, req); - await GeneratePacketAsync(MAVLink.MAVLINK_MSG_ID_REQUEST_DATA_STREAM, req); - } - - public async Task SetParamAsync(string paramname, float value) - { - // param type is set here, however it is always sent over the air as a float 100int = 100f. - var req = new MAVLink.mavlink_param_set_t { target_system = sysid, target_component = compid, param_type = (byte)param_types[paramname] }; - - byte[] temp = Encoding.GetEncoding("ASCII").GetBytes(paramname); - - Array.Resize(ref temp, 16); - req.param_id = temp; - req.param_value = (value); - - await GeneratePacketAsync((byte)MAVLink.MAVLINK_MSG_ID_PARAM_SET, req); - } - - public async Task SetParam2Async(string paramname, float value) - { - // param type is set here, however it is always sent over the air as a float 100int = 100f. - var req = new MAVLink.mavlink_param_set_t { target_system = sysid, target_component = compid, param_type = 4 }; - - byte[] temp = Encoding.GetEncoding("ASCII").GetBytes(paramname); - - Array.Resize(ref temp, 16); - req.param_id = temp; - req.param_value = (value); - - await GeneratePacketAsync((byte)MAVLink.MAVLINK_MSG_ID_PARAM_SET, req); - } - - public async Task SetModeAsync(ac2modes modein) - { - try - { - MAVLink.mavlink_set_mode_t mode = new MAVLink.mavlink_set_mode_t(); - - if (TranslateMode(modein, ref mode)) - { - await SetModeAsync(mode); - } - } - catch { RaiseMessageCreated("Failed to change Modes"); } - } - - public async Task SetModeAsync(string modein) - { - try - { - MAVLink.mavlink_set_mode_t mode = new MAVLink.mavlink_set_mode_t(); - - if (TranslateMode(modein, ref mode)) - { - await SetModeAsync(mode); - } - } - catch { RaiseMessageCreated("Failed to change Modes"); } - } - - public async Task SetModeAsync(MAVLink.mavlink_set_mode_t mode, MAVLink.MAV_MODE_FLAG base_mode = 0) - { - mode.base_mode |= (byte)base_mode; - - await GeneratePacketAsync(MAVLink.MAVLINK_MSG_ID_SET_MODE, mode); - await Task.Delay(10); - 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; - int datalen = 0; - var len = (length % msglen) == 0 ? length / msglen : (length / msglen) + 1; - for (int a = 0; a < len; a++) - { - datalen = Math.Min(length - a * msglen, msglen); - gps.data = new byte[msglen]; - Array.Copy(data, a * msglen, gps.data, 0, datalen); - gps.len = (byte)datalen; - gps.target_component = compid; - gps.target_system = sysid; - await GenerateRTKPacketAsync(MAVLink.MAVLINK_MSG_ID_GPS_INJECT_DATA, gps, gps.len); - // Console.WriteLine("send:" + (ushort)gps.len); - } - } - - public bool TranslateMode(ac2modes modein, ref MAVLink.mavlink_set_mode_t mode) - { - //MAVLink09.mavlink_set_mode_t mode = new MAVLink09.mavlink_set_mode_t(); - mode.target_system = sysid; - - switch (modein) - { - case ac2modes.STABILIZE: - case ac2modes.AUTO: - case ac2modes.RTL: - case ac2modes.LOITER: - case ac2modes.ACRO: - case ac2modes.ALT_HOLD: - case ac2modes.CIRCLE: - case ac2modes.POSITION: - case ac2modes.LAND: - case ac2modes.OF_LOITER: - case ac2modes.GUIDED: - mode.base_mode = (byte)MAVLink.MAV_MODE_FLAG.CUSTOM_MODE_ENABLED; - mode.custom_mode = (uint)modein; - break; - default: - RaiseMessageCreated("错误模式: " + modein); - return false; - } - return true; - } - - public bool TranslateMode(string modein, ref MAVLink.mavlink_set_mode_t mode) - { - //MAVLink09.mavlink_set_mode_t mode = new MAVLink09.mavlink_set_mode_t(); - mode.target_system = sysid; - - var modeValue = EnumTranslator.GetValue(modein.ToUpper()); - switch (modeValue) - { - case (int)ac2modes.STABILIZE: - case (int)ac2modes.AUTO: - case (int)ac2modes.RTL: - case (int)ac2modes.LOITER: - case (int)ac2modes.ACRO: - case (int)ac2modes.ALT_HOLD: - case (int)ac2modes.CIRCLE: - case (int)ac2modes.POSITION: - case (int)ac2modes.LAND: - case (int)ac2modes.OF_LOITER: - case (int)ac2modes.GUIDED: - mode.base_mode = (byte)MAVLink.MAV_MODE_FLAG.CUSTOM_MODE_ENABLED; - mode.custom_mode = (uint)modeValue; - break; - default: - RaiseMessageCreated("错误模式: " + modein); - return false; - } - return true; - } - - public async Task GetWPsAsync() - { - MAVLink.mavlink_mission_request_list_t req = new MAVLink.mavlink_mission_request_list_t(); - - req.target_system = sysid; - req.target_component = compid; - - await GeneratePacketAsync((byte)MAVLink.MAVLINK_MSG_ID_MISSION_REQUEST_LIST, req); - } - - /// - /// Gets specfied WP - /// - /// - /// WP - public async Task GetWPAsync(ushort index) - { - giveComport = true; - MAVLink.mavlink_mission_request_t req = new MAVLink.mavlink_mission_request_t(); - req.target_system = sysid; - req.target_component = compid; - req.seq = index; - //Console.WriteLine("getwp req "+ DateTime.Now.Millisecond); - // request - await GeneratePacketAsync((byte)MAVLink.MAVLINK_MSG_ID_MISSION_REQUEST, req); - } - - public async Task SetWPTotalAsync(ushort wp_total) - { - giveComport = true; - MAVLink.mavlink_mission_count_t req = new MAVLink.mavlink_mission_count_t(); - - req.target_system = sysid; - req.target_component = compid; // MSG_NAMES.MISSION_COUNT - - req.count = wp_total; - - await GeneratePacketAsync((byte)MAVLink.MAVLINK_MSG_ID_MISSION_COUNT, req); - } - - /// 任务信息。 - /// 任务编号。 - /// 坐标系。 - /// 0：在 Auto 模式下飞往指定位置；2：切到 GUIDED 模式，立即飞往指定位置；3：仅改变高度，立即执行。 - /// 指示是否自动继续执行下一个任务。 - public async Task WriteWPAsync(Locationwp loc, ushort index, MAVLink.MAV_FRAME frame, byte current = 0, byte autocontinue = 1) - { - MAVLink.mavlink_mission_item_t req = new MAVLink.mavlink_mission_item_t(); - - req.target_system = sysid; - req.target_component = compid; // MSG_NAMES.MISSION_ITEM - - req.command = loc.id; - - req.current = current; - req.autocontinue = autocontinue; - - req.frame = (byte)frame; - req.y = (float)(loc.lng); - req.x = (float)(loc.lat); - req.z = (float)(loc.alt); - - req.param1 = loc.p1; - req.param2 = loc.p2; - req.param3 = loc.p3; - req.param4 = loc.p4; - - req.seq = index; - - //log.InfoFormat("setWP {6} frame {0} cmd {1} p1 {2} x {3} y {4} z {5}", req.frame, req.command, req.param1, req.x, req.y, req.z, index); - - // request - await GeneratePacketAsync((byte)MAVLink.MAVLINK_MSG_ID_MISSION_ITEM, req); - } - - const float rad2deg = (float)(180 / Math.PI); - const float deg2rad = (float)(1.0 / rad2deg); - - public async Task SetSensorOffsetsAsync(sensoroffsetsenum sensor, float x, float y, float z) - { - return await DoCommandAsync(Plane.Protocols.MAVLink.MAV_CMD.PREFLIGHT_SET_SENSOR_OFFSETS, (int)sensor, x, y, z, 0, 0, 0); - } - - public byte rssi; - - - private void RaiseMessageCreated(string message) - { - var handler = MessageCreated; - if (handler != null) - { - var e = new MessageCreatedEventArgs { Message = message }; - RunOnUIThread(() => - { - handler(this, e); - }); - } - } - - private void RunOnUIThread(Action action) - { - if (SynchronizationContext.Current == _uiSyncContext) - { - action(); - } - else - { - _uiSyncContext.Post(delegate { action(); }, null); - } - } - - private void RaiseEventOnUIThread(EventHandler ev) - { - if (ev != null) - { - RunOnUIThread(() => ev(this, EventArgs.Empty)); - } - } - - private void RaiseEventOnUIThread(EventHandler ev, TEventArgs e) - { - if (ev != null) - { - RunOnUIThread(() => ev(this, e)); - } - } - } -} +// #define LOG_SEQUENCE_NUMBER +//#define LOG_PACKETS + +using Plane.Protocols; +using Plane.Communication; +using TaskTools.Utilities; +using System; +using System.Collections.Generic; +using System.Linq; +using System.Text; +using System.Threading; +using System.Diagnostics; +using System.IO; +using System.Threading.Tasks; + +namespace Plane.Copters +{ +#if PRIVATE + public +#else + internal +#endif + partial class PlaneCopter + { + uint iErrorData; + ulong iDataCount; + private int _recnumber = 0; + private int _sendnumber = 0; + + public bool IsConnected { get { return Connection.IsOpen; } } + + public string Id { get; set; } + + public byte mavlinkversion { get; set; } + public MAVLink.MAV_TYPE aptype { get; set; } + public MAVLink.MAV_AUTOPILOT apname { get; set; } + private bool CommOK = true; + public byte sysid { get; set; } + public byte compid { get; set; } + public byte recvpacketcount { get; set; } + public bool armed { get; set; } + public bool failsafe { get; set; } + + public uint mode { get; set; } + public uint commModuleMode { get; set; } + public byte commModuleVersion { get; set; } + + public bool isUnlocked { get; set; } + + public float battery_voltage { get; set; } + public byte battery_remaining { get; set; } + public float current_battery { get; set; } + public float freemem { get; set; } + public float brklevel { get; set; } + + public bool useLocation { get; set; } + public float gpsalt { get; set; } + public bool precheckok { get; set; } + + public byte gpsstatus { get; set; } + public byte errorcode { get; set; } + + public float gpshdop { get; set; } + public byte satcount { get; set; } + public float retain { get; set; } + public float groundspeed { get; set; } + public float groundcourse { get; set; } + public double lat { get; set; } + public double lng { get; set; } + public float nav_roll { get; set; } + public float nav_pitch { get; set; } + public short nav_bearing { get; set; } + public short target_bearing { get; set; } + public float wp_dist { get; set; } + public float alt_error { get; set; } + public float aspd_error { get; set; } + public float xtrack_error { get; set; } + public float roll { get; set; } + public float pitch { get; set; } + public float yaw { get; set; } + public short heading { get; set; } + public float airspeed { get; set; } + public float alt { get { return altorigin - altoffset; } } + public float altoffset = 0; + public float altorigin { get; private set; } = 0; + public float ch3percent { get; set; } + + public ushort ch1in { get; set; } + public ushort ch2in { get; set; } + public ushort ch3in { get; set; } + public ushort ch4in { get; set; } + public ushort ch5in { get; set; } + public ushort ch6in { get; set; } + public ushort ch7in { get; set; } + public ushort ch8in { get; set; } + public float rxrssi { get; set; } + + public byte remrssi { get; set; } + + public uint timebootms { get; set; } + + private int DataTimeOut = 0; + + public Dictionary param = new Dictionary(); + public Dictionary param_types = new Dictionary(); + + public IConnection Connection { get; set; } + + public ushort FlightDistance2D { get; private set; } + + private SynchronizationContext _uiSyncContext; + private bool _autoSendHeartbeat = true; + private bool _autoRequestData = true; + + public PlaneCopter(IConnection connection, SynchronizationContext uiSyncContext, bool autoSendHeartbeat = true, bool autoRequestData = true) + { + this.Connection = connection; + _uiSyncContext = uiSyncContext; + _autoSendHeartbeat = autoSendHeartbeat; + _autoRequestData = autoRequestData; + } + + public async Task ConnectAsync() + { + //sysid = 2; + SendReq = false; + CommOK = true; + await Connection.OpenAsync().ConfigureAwait(false); + StartCommunication(); + } + + public async Task DisconnectAsync() + { +#if DEBUG && !NETFX_CORE && LOG_SEQUENCE_NUMBER + File.AppendAllText($"SequenceNumbers_{Id}.log", _sequenceLog.ToString()); +#endif + await StopDatastreamAsync(MAVLink.MAV_DATA_STREAM.ALL, 0); + CommOK = false; + Connection.Close(); + } + + public async Task GetCopterDataAsync() + { + //停止所有数据流 + await StopGetDatastreamAsync(MAVLink.MAV_DATA_STREAM.ALL, 0).ConfigureAwait(false); + //需要电池电压，电流，GPS数据 + /* + * 目前接收的数据是 + * gps 38 字节 + hud 28 + SYS_STATUS 39 + MISSION_CURRENT 10 + HEARTBEAT 17 + 共132字节 + 5hz 592/s 太高 + */ + await RequestDatastreamAsync(MAVLink.MAV_DATA_STREAM.EXTENDED_STATUS, 1).ConfigureAwait(false); //10 1电池电压、电流、电量,152-内存、62-导航控制输出、24-GPS数据、42-航点数据 + //不需要姿态信息 + // await RequestDatastreamAsync(MAVLink.MAV_DATA_STREAM.EXTRA1, 1).ConfigureAwait(false); //5 请求 飞行姿态 roll、pitch、yaw、rollspeed、pitchspeed、yawspeed--- 30 + //需要高度和机头朝向数据 + await RequestDatastreamAsync(MAVLink.MAV_DATA_STREAM.EXTRA2, 1).ConfigureAwait(false); //10 请求vfr airspeed(空速)、groundspeed(地速)、alt(高度)、climb(爬升率)、heading(头朝向)、throttle(实际油门) ---74 + //不需要通道数据 + //await RequestDatastreamAsync(MAVLink.MAV_DATA_STREAM.RC_CHANNELS, 1).ConfigureAwait(false); //1 请求 通道数据 + //不需要原始数据 + // await RequestDatastreamAsync(MAVLink.MAV_DATA_STREAM.RAW_SENSORS, ratesensors).ConfigureAwait(false); // 请求 传感器原始值 + + /* //已经停止所有数据了，不用单独停止 + // Disable ATTITUDE_CONTROLLER_OUTPUT, POSITION_CONTROLLER_OUTPUT, NAV_CONTROLLER_OUTPUT. | + await StopGetDatastreamAsync(MAVLink.MAV_DATA_STREAM.RAW_CONTROLLER, 0).ConfigureAwait(false); + // Disable LOCAL_POSITION, GLOBAL_POSITION/GLOBAL_POSITION_INT messages. | + await StopGetDatastreamAsync(MAVLink.MAV_DATA_STREAM.POSITION, 0).ConfigureAwait(false); + // Disable EXTRA3 (Dependent on the autopilot |) + await StopGetDatastreamAsync(MAVLink.MAV_DATA_STREAM.EXTRA3, 0).ConfigureAwait(false); + // Disable Plane Stream + await StopGetDatastreamAsync(MAVLink.MAV_DATA_STREAM.GHOST_STREAM, 0).ConfigureAwait(false); + // Disable RAW_SENSORS + await StopGetDatastreamAsync(MAVLink.MAV_DATA_STREAM.RAW_SENSORS, 0).ConfigureAwait(false); // + // 5 Disable MSG_ATTITUDE roll、pitch、yaw、rollspeed、pitchspeed、yawspeed-- - 30 + await StopGetDatastreamAsync(MAVLink.MAV_DATA_STREAM.EXTRA1, 0).ConfigureAwait(false); // + */ + + } + + private async void StartCommunication() + { + try + { + var writingTask = _autoSendHeartbeat ? StartSendingHeartbeatsAsync() : TaskUtils.CompletedTask; + var readingTask = StartReadingPacketsAsync(); + await Task.WhenAll(writingTask, readingTask).ConfigureAwait(false); + } + catch (Exception ex) + { + RaiseMessageCreated(ex.ToString()); + } + } + +#if DEBUG && LOG_PACKETS + + private Stopwatch _sendHeartbeatStopwatch = new Stopwatch(); + private TimeSpan _sendHeartbeatTimeSpan; + +#endif + + public async Task StartSendingHeartbeatsAsync() + { + await Task.Run(async () => + { + while (CommOK) + { + if (!IsConnected) + { + break; + } + // if (heatbeatSend.Second != DateTime.Now.Second) + if (heatbeatSend.AddMilliseconds(1000) <= DateTime.Now) + { + var htb = new MAVLink.mavlink_heartbeat_t() + { + type = (byte)MAVLink.MAV_TYPE.GCS, + autopilot = (byte)MAVLink.MAV_AUTOPILOT.GENERIC, + mavlink_version = 3, + }; + // await SendPacketAsync(htb).ConfigureAwait(false); //发心跳包 +#if DEBUG && LOG_PACKETS + if (!_sendHeartbeatStopwatch.IsRunning) + { + _sendHeartbeatStopwatch.Start(); + } + else + { + var elapsed = _sendHeartbeatStopwatch.Elapsed; + Debug.WriteLine(elapsed - _sendHeartbeatTimeSpan, "--------------------------发送心跳间隔"); + _sendHeartbeatTimeSpan = elapsed; + } +#endif + heatbeatSend = DateTime.Now; + DataTimeOut += 1; + if (DataTimeOut > 3) + { + bInitChannel = false; + SendReq = false; + } + } + else + { + await Task.Delay(100).ConfigureAwait(false); + } + } + }).ConfigureAwait(false); + } + + private async Task StartReadingPacketsAsync() + { + await Task.Run(async () => + { + while (CommOK) + { + if (!IsConnected) + { + RaiseEventOnUIThread(ConnectionBroken); + break; + } + + var packet = await ReadPacketAsync().ConfigureAwait(false); + if (packet != null) + { + PacketReceived?.Invoke(this, new PacketReceivedEventArgs(packet)); + var handled = true; + // Debug.WriteLine(packet[5],"收到数据类型"); + switch (packet[5]) + { + case MAVLink.MAVLINK_MSG_ID_HEARTBEAT: //0 + AnalyzeHeartbeatPacket(packet); + break; + case MAVLink.MAVLINK_MSG_ID_SYS_STATUS: //1 + AnalyzeSysStatusPacket(packet); + break; + case MAVLink.MAVLINK_MSG_ID_GPS_RAW_INT: //24 + AnalyzeGpsRawIntPacket(packet); + break; + case MAVLink.MAVLINK_MSG_ID_VFR_HUD: //74 + AnalyzeVfrHudPacket(packet); + break; + case MAVLink.MAVLINK_MSG_ID_STATUSTEXT: //253 + AnalyzeStatusTextPacket(packet); + break; + + ////以上为基本数据， + + case MAVLink.MAVLINK_MSG_ID_RADIO: //109 + AnalyzeRadioPacket(packet); + break; + case MAVLink.MAVLINK_MSG_ID_PARAM_VALUE: //22 + AnalyzeParamValuePacket(packet); + break; + case MAVLink.MAVLINK_MSG_ID_MEMINFO: //152 + AnalyzeMemInfoPacket(packet); + break; + case MAVLink.MAVLINK_MSG_ID_MISSION_CURRENT: //42 + AnalyzeMissionCurrentPacket(packet); + break; + + case MAVLink.MAVLINK_MSG_ID_GLOBAL_POSITION_INT: //33 + AnalyzeGlobalPositionIntPacket(packet); + break; + case MAVLink.MAVLINK_MSG_ID_NAV_CONTROLLER_OUTPUT: //62 + AnalyzeNavControllerOutputPacket(packet); + break; + case MAVLink.MAVLINK_MSG_ID_ATTITUDE: //30 + AnalyzeAttitudePacket(packet); + break; + + case MAVLink.MAVLINK_MSG_ID_RC_CHANNELS_RAW: //35 + AnalyzeRCChannelsRawPacket(packet); + break; + case MAVLink.MAVLINK_MSG_ID_RAW_IMU: //没有用27 + AnalyzeRawImuPacket(packet); + break; + case MAVLink.MAVLINK_MSG_ID_SCALED_IMU2: //没有用 116 + AnalyzeScaledImu2Packet(packet); + break; + case MAVLink.MAVLINK_MSG_ID_SENSOR_OFFSETS: //150 + AnalyzeSensorOffsetsPacket(packet); + break; + + default: + handled = false; + UnhandledPacketReceived?.Invoke(this, new PacketReceivedEventArgs(packet)); + break; + } + if (handled) + { + PacketHandled?.Invoke(this, new PacketReceivedEventArgs(packet)); + } + } + } + }).ConfigureAwait(false); + } + + public async Task GetParamsListAsync() + { + MAVLink.mavlink_param_request_list_t req = new MAVLink.mavlink_param_request_list_t(); + req.target_system = sysid; + req.target_component = compid; + await GeneratePacketAsync(MAVLink.MAVLINK_MSG_ID_PARAM_REQUEST_LIST, req); + } + + public static List> getModesList() + { + var flightModes = EnumTranslator.Translate(); + return flightModes.ToList(); + } + + // 王海, 20151029, 由于只在 myComm 循环中读取数据包，因此去掉了锁。 + //volatile object readlock = new object(); + +#if DEBUG && LOG_PACKETS + private StringBuilder _log = new StringBuilder(); + private Stopwatch _positionStopwatch = new Stopwatch(); + private TimeSpan _lastPositionTimeSpan; +#endif + +#if DEBUG && LOG_SEQUENCE_NUMBER + private StringBuilder _sequenceLog = new StringBuilder("-----------------------\n"); +#endif + + private byte[] _readBuffer = new byte[263]; + + /// + /// 异步读取数据包。// 王海, 20151029, 去掉了锁，不要在除读数据包循环之外的地方调用此方法。 + /// + /// 表示此异步操作的 实例，其结果为读取到的数据包。 + private async Task ReadPacketAsync() + { + if (!Connection.IsOpen) + { + return null; + } + + int length = 0; + int readnumber = 0; //_recnumber; + try + { + readnumber = await Connection.ReadAsync(_readBuffer, 0, 1); + _recnumber += readnumber; + if (readnumber == 0) + { + return null; + } + if (_readBuffer[0] == MAVLink.MAVLINK_STX) + { + readnumber = await Connection.ReadAsync(_readBuffer, 1, 5); + _recnumber += readnumber; + if (readnumber == 0) + { + return null; + } + +#if DEBUG && LOG_SEQUENCE_NUMBER + // _sequenceLog.Append(DateTime.Now.ToString("HHmmss.fff")).Append('\t').Append(string.Format("{0,3}", _readBuffer[2].ToString())).Append('\t').Append(string.Format("{0,3}", _readBuffer[5].ToString())); + _sequenceLog.Append(DateTime.Now.ToString()).Append('\t').Append(string.Format("{0,3}", _readBuffer[2].ToString())).Append('\t').Append(string.Format("{0,3}", _readBuffer[5].ToString())); +#endif + // packet length + length = 6 + _readBuffer[1] + 2; // header + data + checksum + readnumber = await Connection.ReadAsync(_readBuffer, 6, length - 6); + _recnumber += readnumber; + if (readnumber == 0) + { + return null; + } + } + else + { +#if DEBUG && LOG_PACKETS + _log.AppendLine("---------------"); + _log.AppendLine("unexpected head: " + _readBuffer[0]); +#endif + + return null; + } + } + catch + { + return null; + } + + uint crcvalue = BitConverter.ToUInt16(_readBuffer, length - 2); + +#if DEBUG && LOG_PACKETS + _log.AppendLine("---------------"); + for (int i = 0; i < 6; i++) + { + _log.Append(_readBuffer[i]).Append(" "); + } + _log.AppendLine(); + for (int i = 6; i < length - 2; i++) + { + _log.Append(_readBuffer[i]).Append(" "); + } + _log.AppendLine(); + for (int i = length - 2; i < length; i++) + { + _log.Append(_readBuffer[i]).Append(" "); + } + _log.AppendLine(); +#endif + byte messageType = _readBuffer[5]; + +#if DEBUG && LOG_PACKETS + if (messageType == 33) + { + if (_positionStopwatch.IsRunning) + { + var elapsed = _positionStopwatch.Elapsed; + Debug.WriteLine((elapsed - _lastPositionTimeSpan).TotalMilliseconds, "-----------------------------------------------position packet"); + _lastPositionTimeSpan = elapsed; + } + else + { + _positionStopwatch.Start(); + } + } +#endif + + ushort checksum = MavlinkCRC.crc_calculate(_readBuffer, length - 2); + checksum = MavlinkCRC.crc_accumulate(MAVLink.MAVLINK_MESSAGE_CRCS[messageType], checksum); + iDataCount += 1; + +#if DEBUG && LOG_SEQUENCE_NUMBER + _sequenceLog.Append('\t').Append(crcvalue == checksum).AppendLine(); +#endif + + if (crcvalue == checksum) + { + return _readBuffer; + } + else + { +#if DEBUG && LOG_PACKETS + _log.AppendLine("crc not passed"); +#endif + + iErrorData += 1; + var handler = ErrorDataEvent; + if (handler != null) + { + RunOnUIThread(() => handler(iErrorData, iDataCount, _readBuffer[5])); + } + return null; + } + } + + public MAVState MAV = new MAVState(); + + /// + /// Used to extract mission from log file - both sent or received + /// + /// packet + void getWPsfromstream(ref byte[] buffer) + { + if (buffer[5] == (byte)MAVLink.MAVLINK_MSG_ID_MISSION_COUNT) + { + // clear old + MAV.wps.Clear(); + } + + if (buffer[5] == (byte)MAVLink.MAVLINK_MSG_ID_MISSION_ITEM) + { + MAVLink.mavlink_mission_item_t wp = buffer.ByteArrayToStructure(6); + + if (wp.current == 2) + { + // guide mode wp + MAV.GuidedMode = wp; + } + else + { + MAV.wps[wp.seq] = wp; + } + + //Console.WriteLine("WP # {7} cmd {8} p1 {0} p2 {1} p3 {2} p4 {3} x {4} y {5} z {6}", wp.param1, wp.param2, wp.param3, wp.param4, wp.x, wp.y, wp.z, wp.seq, wp.command); + } + + if (buffer[5] == (byte)MAVLink.MAVLINK_MSG_ID_RALLY_POINT) + { + MAVLink.mavlink_rally_point_t rallypt = buffer.ByteArrayToStructure(6); + + MAV.rallypoints[rallypt.idx] = rallypt; + + //Console.WriteLine("RP # {0} {1} {2} {3} {4}", rallypt.idx, rallypt.lat, rallypt.lng, rallypt.alt, rallypt.break_alt); + } + + if (buffer[5] == (byte)MAVLink.MAVLINK_MSG_ID_FENCE_POINT) + { + MAVLink.mavlink_fence_point_t fencept = buffer.ByteArrayToStructure(6); + + MAV.fencepoints[fencept.idx] = fencept; + } + } + + + /// + /// used for outbound packet sending + /// + public int packetcount = 0; + private bool SendReq; +#pragma warning disable CS0414 // The field is assigned but its value is never used + private bool giveComport; +#pragma warning restore CS0414 // The field is assigned but its value is never used + + public async Task GeneratePacketAsync(byte messageType, TMavlinkPacket indata) + { + byte[] data; + + if (mavlinkversion == 3) + { + data = MavlinkUtil.StructureToByteArray(indata); + } + else + { + data = MavlinkUtil.StructureToByteArrayBigEndian(indata); + } + // data = MavlinkUtil.StructureToByteArrayBigEndian(indata); + //Console.WriteLine(DateTime.Now + " PC Doing req "+ messageType + " " + this.BytesToRead); + byte[] packet = new byte[data.Length + 6 + 2]; + + if (mavlinkversion == 3 || mavlinkversion == 0) + { + packet[0] = MAVLink.MAVLINK_STX; + } + else if (mavlinkversion == 2) + { + packet[0] = (byte)'U'; + } + packet[1] = (byte)(data.Length); + packet[2] = (byte)packetcount; + //packet[2] = 35; + packetcount++; + + packet[3] = 255; // this is always 255 - MYGCS + + packet[4] = (byte)MAVLink.MAV_COMPONENT.MAV_COMP_ID_MISSIONPLANNER; + + packet[5] = messageType; + + int i = 6; + foreach (byte b in data) + { + packet[i] = b; + i++; + } + + ushort checksum = MavlinkCRC.crc_calculate(packet, packet[1] + 6); + + if (mavlinkversion == 3 || mavlinkversion == 0) + { + checksum = MavlinkCRC.crc_accumulate(MAVLink.MAVLINK_MESSAGE_CRCS[messageType], checksum); + } + + byte ck_a = (byte)(checksum & 0xFF); ///< High byte + byte ck_b = (byte)(checksum >> 8); ///< Low byte + + packet[i] = ck_a; + i += 1; + packet[i] = ck_b; + i += 1; + + if (IsConnected) + { + try + { + await Connection.WriteAsync(packet, 0, i); + // Console.WriteLine("sendout:" + i); + _sendnumber += i; //统计发送数量 + } + catch + { + // 王海, 20151031, 异常已通过 IConnection.ExceptionThrown 事件传到外部，此处吞掉就行了。 + } + } + } + + + public async Task GenerateRTKPacketAsync(byte messageType, TMavlinkPacket indata,int rtklength) + { + byte[] data; + int rtkdataleng= rtklength + 3; + if (mavlinkversion == 3) + { + data = MavlinkUtil.StructureToByteArray(indata); + } + else + { + data = MavlinkUtil.StructureToByteArrayBigEndian(indata); + } + // data = MavlinkUtil.StructureToByteArrayBigEndian(indata); + //Console.WriteLine(DateTime.Now + " PC Doing req "+ messageType + " " + this.BytesToRead); + + byte[] packet = new byte[rtkdataleng + 6 + 2]; + + if (mavlinkversion == 3 || mavlinkversion == 0) + { + packet[0] = MAVLink.MAVLINK_STX; + } + else if (mavlinkversion == 2) + { + packet[0] = (byte)'U'; + } + packet[1] = (byte)(rtkdataleng); + packet[2] = (byte)packetcount; + //packet[2] = 35; + packetcount++; + + packet[3] = 255; // this is always 255 - MYGCS + + packet[4] = (byte)MAVLink.MAV_COMPONENT.MAV_COMP_ID_MISSIONPLANNER; + + packet[5] = messageType; + + int i = 6; + foreach (byte b in data) + { + if (i < rtkdataleng + 6 ) + packet[i] = b; + else break; + i++; + } + + ushort checksum = MavlinkCRC.crc_calculate(packet, packet[1] + 6); + + if (mavlinkversion == 3 || mavlinkversion == 0) + { + checksum = MavlinkCRC.crc_accumulate(MAVLink.MAVLINK_MESSAGE_CRCS[messageType], checksum); + } + + byte ck_a = (byte)(checksum & 0xFF); ///< High byte + byte ck_b = (byte)(checksum >> 8); ///< Low byte + + packet[i] = ck_a; + i += 1; + packet[i] = ck_b; + i += 1; + + if (IsConnected) + { + try + { + await Connection.WriteAsync(packet, 0, i); + //Console.WriteLine("sendout:" + i); + _sendnumber += i; //统计发送数量 + } + catch + { + // 王海, 20151031, 异常已通过 IConnection.ExceptionThrown 事件传到外部，此处吞掉就行了。 + } + } + } + + public async Task DoLEDAsync() + { + return await DoLEDCommandAsync(); + } + + public async Task DoLEDCommandAsync() + { + giveComport = true; + MAVLink.mavlink_msg_id_led_control led = new MAVLink.mavlink_msg_id_led_control(); + led.target_system = sysid; + led.target_component = compid;//(byte)MAVLink.MAV_COMPONENT.MAV_COMP_ID_SYSTEM_CONTROL;; + led.instance = 0; + led.pattern = 0; + led.custom_len = 4; + led.custom_bytes = new byte[24]; + led.custom_bytes[0] = 255; + led.custom_bytes[1] = 255; + led.custom_bytes[2] = 255; + led.custom_bytes[3] = 2; + + await GeneratePacketAsync(MAVLink.MAVLINK_MSG_ID_LED_CONTROL, led); + return true; + } + + public async Task DoARMAsync(bool armit) + { + return await DoCommandAsync(MAVLink.MAV_CMD.COMPONENT_ARM_DISARM, armit ? 1 : 0, 21196, 0, 0, 0, 0, 0); + } + + public async Task DoMotorTestAsync(int motor, MAVLink.MOTOR_TEST_THROTTLE_TYPE thr_type, int throttle, int timeout) + { + return await DoCommandAsync(MAVLink.MAV_CMD.DO_MOTOR_TEST, (float)motor, (float)(byte)thr_type, (float)throttle, (float)timeout, 0, 0, 0); + } + + public async Task DoTakeoffAsync() + { + /// Takeoff from ground / hand |Minimum pitch (if airspeed sensor present), desired pitch without sensor| Empty| Empty| Yaw angle (if magnetometer present), ignored without magnetometer| Latitude| Longitude| Altitude| + return await DoCommandAsync(MAVLink.MAV_CMD.TAKEOFF, 0, 0, 0, -1, 0, 0, 15); + } + public async Task DoMissionStartAsync(int hour_utc, int minute_utc, int second_utc, double Missionlng, double Missionlat) + { + /// Takeoff from ground / hand |Minimum pitch (if airspeed sensor present), desired pitch without sensor| Empty| Empty| Yaw angle (if magnetometer present), ignored without magnetometer| Latitude| Longitude| Altitude| + return await DoCommandAsync(MAVLink.MAV_CMD.MISSION_START, hour_utc, minute_utc, second_utc, (float)Missionlng, (float)Missionlat, 0, 0); + } + + + public async Task DoCommandAsync(MAVLink.MAV_CMD actionid, float p1, float p2, float p3, float p4, float p5, float p6, float p7) + { + giveComport = true; + //byte[] buffer; + + MAVLink.mavlink_command_long_t req = new MAVLink.mavlink_command_long_t(); + + req.target_system = sysid; + req.target_component = compid; + + if (actionid == MAVLink.MAV_CMD.COMPONENT_ARM_DISARM) + { + req.target_component = (byte)MAVLink.MAV_COMPONENT.MAV_COMP_ID_SYSTEM_CONTROL; + } + + req.command = (ushort)actionid; + + + Console.WriteLine("P1 = " + p1); + req.param1 = p1; + req.param2 = p2; + req.param3 = p3; + req.param4 = p4; + req.param5 = p5; + req.param6 = p6; + req.param7 = p7; + + await GeneratePacketAsync(MAVLink.MAVLINK_MSG_ID_COMMAND_LONG, req); + return true; + + //DateTime start = DateTime.Now; + //int retrys = 3; + + //int timeout = 2000; + + ////// imu calib take a little while + //if (actionid == MAVLink.MAV_CMD.PREFLIGHT_CALIBRATION && p5 == 1) + //{ + // // this is for advanced accel offsets, and blocks execution + // return true; + //} + //else if (actionid == MAVLink.MAV_CMD.PREFLIGHT_CALIBRATION) + //{ + // retrys = 1; + // timeout = 25000; + //} + //else if (actionid == MAVLink.MAV_CMD.PREFLIGHT_REBOOT_SHUTDOWN) + //{ + // await GeneratePacketAsync(MAVLink.MAVLINK_MSG_ID_COMMAND_LONG, req); + // giveComport = false; + // return true; + //} + //else if (actionid == MAVLink.MAV_CMD.COMPONENT_ARM_DISARM) + //{ + // // 10 seconds as may need an imu calib + // timeout = 10000; + //} + + //while (true) + //{ + // if (!(start.AddMilliseconds(timeout) > DateTime.Now)) + // { + // if (retrys > 0) + // { + // //log.Info("doAction Retry " + retrys); + // await GeneratePacketAsync(MAVLink.MAVLINK_MSG_ID_COMMAND_LONG, req); + // start = DateTime.Now; + // retrys--; + // continue; + // } + // giveComport = false; + // throw new Exception("Timeout on read - doAction"); + // } + + // buffer = readPacket(); + // if (buffer.Length > 5) + // { + // if (buffer[5] == MAVLink.MAVLINK_MSG_ID_COMMAND_ACK) + // { + // var ack = buffer.ByteArrayToStructure(6); + // if (ack.result == (byte)MAVLink.MAV_RESULT.ACCEPTED) + // { + // giveComport = false; + // return true; + // } + // else + // { + // giveComport = false; + // return false; + // } + // } + // } + //} + } + + + public double GetBearing(float lat1,float lng1,float lat2,float lng2) + { + var latitude1 = Math.PI / 180*lat1; + var latitude2 = Math.PI / 180 * lat2; + var longitudeDifference = Math.PI / 180*(lng2 - lng1); + + var y = Math.Sin(longitudeDifference) * Math.Cos(latitude2); + var x = Math.Cos(latitude1) * Math.Sin(latitude2) - Math.Sin(latitude1) * Math.Cos(latitude2) * Math.Cos(longitudeDifference); + + return (180 / Math.PI*(Math.Atan2(y, x)) + 360) % 360; + } + + + public async Task SetGuidedModeWPAsync(Locationwp gotohere) + { + //if (gotohere.alt == 0 || gotohere.lat == 0 || gotohere.lng == 0) + // return; + + //giveComport = true; + + //try + //{ + //gotohere.id = (byte)MAVLink.MAV_CMD.WAYPOINT; + //老方法无法设置速度 + // await SetGuidedWPAsync(gotohere, 0, MAVLink.MAV_FRAME.GLOBAL_RELATIVE_ALT, (byte)2); + //} + //catch { } + + //giveComport = false; + + + //老方法无法设置速度 + await SetWPAsync(gotohere, 0, MAVLink.MAV_FRAME.GLOBAL_RELATIVE_ALT, (byte)2); + + /* + TaskTools.HIL.Vector3 targetVelocity = new TaskTools.HIL.Vector3(); + float targspeed = (float)2.0; //目标速度 + double targetbearing= GetBearing((float)lat, (float)lng, (float)gotohere.lat, (float)gotohere.lng); //目标机头方向 + targetVelocity.x = Math.Cos(targetbearing * 180 / Math.PI) * targspeed; //分解速度 + targetVelocity.y = Math.Sin(targetbearing * 180 / Math.PI) * targspeed; //分解速度 + // await setPositionTargetGlobalInt(true, false, false, MAVLink.MAV_FRAME.GLOBAL_RELATIVE_ALT, + // gotohere.lat, gotohere.lng, gotohere.alt, targetVelocity.x, targetVelocity.y, 2.0); + + await setPositionTargetGlobalInt(true, false, false, MAVLink.MAV_FRAME.GLOBAL_RELATIVE_ALT, + gotohere.lat, gotohere.lng, gotohere.alt, 0, 0, 0); + */ + + + } + + + + + public async Task setPositionTargetGlobalInt( bool pos, bool vel, bool acc, MAVLink.MAV_FRAME frame, double lat, double lng, double alt, double vx, double vy, double vz) + { + MAVLink.mavlink_set_position_target_global_int_t target = new MAVLink.mavlink_set_position_target_global_int_t() + { + target_system = sysid, + target_component = compid, + alt = (float)alt, + lat_int = (int)(lat * 1e7), + lon_int = (int)(lng * 1e7), + coordinate_frame = (byte)frame, + vx = (float)vx, + vy = (float)vy, + vz = (float)vz + }; + + target.type_mask = 7 + 56 + 448; + + if (pos) + target.type_mask -= 7; + if (vel) + target.type_mask -= 56; + if (acc) + target.type_mask -= 448; + + await GeneratePacketAsync(MAVLink.SET_POSITION_TARGET_GLOBAL_INT, target); + + } + + + public async Task SetNewWPAltAsync(Locationwp gotohere) + { + //givecomport = true; + + //try + //{ + gotohere.id = (byte)MAVLink.MAV_CMD.WAYPOINT; + + //MAVLink.MAV_MISSION_RESULT ans = + await SetWPAsync(gotohere, 0, MAVLink.MAV_FRAME.GLOBAL_RELATIVE_ALT, (byte)3); + + // if (ans != mavlink.mav_mission_result.mav_mission_accepted) + // throw new exception("alt change failed"); + //} + //catch { givecomport = false; /*log.error(ex);*/ throw; } + + //givecomport = false; + } + + public async Task SetWPAsync(Locationwp loc, ushort index, MAVLink.MAV_FRAME frame, byte current = 0) + { + giveComport = true; + MAVLink.mavlink_mission_item_t req = new MAVLink.mavlink_mission_item_t(); + + req.target_system = sysid; + req.target_component = compid; // MAVLINK_MSG_ID_MISSION_ITEM + + req.command = loc.id; + + + req.current = current; + + req.frame = (byte)frame; + + req.x = (float)(loc.lat); + req.y = (float)(loc.lng); + req.z = (float)(loc.alt); + + req.param1 = loc.p1; + req.param2 = loc.p2; + req.param3 = loc.p3; + req.param4 = loc.p4; + + req.seq = index; + + //log.InfoFormat("setWP {6} frame {0} cmd {1} p1 {2} x {3} y {4} z {5}", req.frame, req.command, req.param1, req.x, req.y, req.z, index); + + // request + await GeneratePacketAsync(MAVLink.MAVLINK_MSG_ID_MISSION_ITEM, req); + } + + public async Task SetChannelsAsync(ushort? ch1 = null, ushort? ch2 = null, ushort? ch3 = null, ushort? ch4 = null, ushort? ch5 = null, ushort? ch6 = null, ushort? ch7 = null, ushort? ch8 = null) + { + //某些情况下ch4设置成0会导致自旋 + ushort checkyaw = ch4 ?? ch4in; + if ((checkyaw < 1200)|| (checkyaw > 1800)) + checkyaw = 1500; + //// + MAVLink.mavlink_rc_channels_override_t rc = new MAVLink.mavlink_rc_channels_override_t(); + rc.target_component = compid; + rc.target_system = sysid; + rc.chan1_raw = ch1 ?? ch1in; + rc.chan2_raw = ch2 ?? ch2in; + rc.chan3_raw = ch3 ?? ch3in; + rc.chan4_raw = checkyaw; + rc.chan5_raw = ch5 ?? ch5in; + rc.chan6_raw = ch6 ?? ch6in; + rc.chan7_raw = ch7 ?? ch7in; + rc.chan8_raw = ch8 ?? ch8in; + await SendPacketAsync(rc); + } + + public async Task SetMobileControlAsync(ushort? ch1 = null, ushort? ch2 = null, ushort? ch3 = null, ushort? ch4 = null, ushort? ch5 = null, ushort? ch6 = null, ushort? ch7 = null, ushort? ch8 = null, float? yaw = null, float? alt = null) + { + /* MAVLink.mavlink_rc_mobile_control_t rc = new MAVLink.mavlink_rc_mobile_control_t(); + rc.target_component = compid; + rc.target_system = sysid; + rc.chan1_raw = ch1 ?? ch1in; + rc.chan2_raw = ch2 ?? ch2in; + rc.chan3_raw = ch3 ?? ch3in; + rc.chan4_raw = ch4 ?? ch4in; + rc.chan5_raw = ch5 ?? ch5in; + rc.chan6_raw = ch6 ?? ch6in; + rc.chan7_raw = ch7 ?? ch7in; + rc.chan8_raw = ch8 ?? ch8in; + rc.Yaw = yaw ?? this.yaw; + rc.alt = alt ?? this.alt; + await SendPacketAsync(rc); + */ + float ch4yaw; + ch4yaw = yaw ?? this.yaw; + if (ch4yaw != this.yaw) + { + ch4yaw = ch4yaw % 360; + await DoCommandAsync(MAVLink.MAV_CMD.CONDITION_YAW, ch4yaw, 0, 1, 0, 0, 0, 0); + } + + /* + float ch4yaw; + MAVLink.mavlink_rc_channels_override_t rc = new MAVLink.mavlink_rc_channels_override_t(); + rc.target_component = compid; + rc.target_system = sysid; + rc.chan1_raw = ch1 ?? ch1in; + rc.chan2_raw = ch2 ?? ch2in; + rc.chan3_raw = ch3 ?? ch3in; + rc.chan4_raw = 1500; + rc.chan5_raw = ch5 ?? ch5in; + rc.chan6_raw = ch6 ?? ch6in; + rc.chan7_raw = ch7 ?? ch7in; + rc.chan8_raw = ch8 ?? ch8in; + ch4yaw= yaw ?? this.yaw; + rc.chan4_raw =(ushort)ch4yaw; + await SendPacketAsync(rc); + */ + + } + + public async Task GetParamAsync(string paramName, int millisecondsTimeout = Timeout.Infinite) + { + if (param.ContainsKey(paramName)) param[paramName] = null; + var paramId = Encoding.ASCII.GetBytes(paramName); + Array.Resize(ref paramId, 16); + var req = new MAVLink.mavlink_param_request_read_t + { + param_id = paramId, + param_index = -1, + target_component = compid, + target_system = sysid + }; + var stopwatch = Stopwatch.StartNew(); + while (true) + { + for (int ii = 0; ii < 5; ii++) + { + await SendPacketAsync(req); + await Task.Delay(10).ConfigureAwait(false); + } + + int i = 0; + for (; i < 5 && (!param.ContainsKey(paramName) || param[paramName] == null); i++) + { + await Task.Delay(50).ConfigureAwait(false); + } + if (i < 5) + { + stopwatch.Stop(); + break; + } + else if (millisecondsTimeout != Timeout.Infinite && stopwatch.ElapsedMilliseconds >= millisecondsTimeout) + { + stopwatch.Stop(); + throw new TimeoutException("The GetParamAsync operation has timed out."); + } + } + return param[paramName].Value; + } + + public async Task SendPacketAsync(TMavlinkPacket indata) + { + bool validPacket = false; + byte a = 0; + var packetType = indata.GetType(); + foreach (Type ty in MAVLink.MAVLINK_MESSAGE_INFO) + { + if (ty == packetType) + { + validPacket = true; + await GeneratePacketAsync(a, indata); + return; + } + a++; + } + if (!validPacket) + { + RaiseMessageCreated("The packet type is not valid."); + //log.Info("Mavlink : NOT VALID PACKET await SendPacketAsync() " + indata.GetType().ToString()); + } + } + + public double[] packetspersecond { get; set; } + DateTime[] packetspersecondbuild = new DateTime[256]; + + byte ratesensors = 1; + public byte RateSensors { get { return ratesensors; } set { ratesensors = value; } } + + private DateTime heatbeatSend; + private bool bInitChannel = false; + private bool bGPSBadHealth; + private bool bGyroBadHealth; + private bool bAccel; + private bool bCompass; + private bool bBarometer; + + public short gx; + public short gy; + public short gz; + public short ax; + public short ay; + public short az; + public short mx; + public short my; + public short mz; + + public short gx2; + public short gy2; + public short gz2; + public short ax2; + public short ay2; + public short az2; + public short mx2; + public short my2; + public short mz2; + + public float mag_ofs_x; + public float mag_ofs_y; + public float mag_ofs_z; + public float mag_declination; + public int raw_press; + public int raw_temp; + public float gyro_cal_x; + public float gyro_cal_y; + public float gyro_cal_z; + public float accel_cal_x; + public float accel_cal_y; + public float accel_cal_z; + + public ushort param_total; + public ushort WpCount; + + public async Task RequestDatastreamAsync(MAVLink.MAV_DATA_STREAM id, byte hzrate) + { + + double pps = 0; + + switch (id) + { + case MAVLink.MAV_DATA_STREAM.ALL: + + break; + case MAVLink.MAV_DATA_STREAM.EXTENDED_STATUS: + if (packetspersecondbuild[MAVLink.MAVLINK_MSG_ID_SYS_STATUS] < DateTime.Now.AddSeconds(-2)) + break; + pps = packetspersecond[MAVLink.MAVLINK_MSG_ID_SYS_STATUS]; + if (hzratecheck(pps, hzrate)) + { + return; + } + break; + case MAVLink.MAV_DATA_STREAM.EXTRA1: + if (packetspersecondbuild[MAVLink.MAVLINK_MSG_ID_ATTITUDE] < DateTime.Now.AddSeconds(-2)) + break; + pps = packetspersecond[MAVLink.MAVLINK_MSG_ID_ATTITUDE]; + if (hzratecheck(pps, hzrate)) + { + return; + } + break; + case MAVLink.MAV_DATA_STREAM.EXTRA2: + if (packetspersecondbuild[MAVLink.MAVLINK_MSG_ID_VFR_HUD] < DateTime.Now.AddSeconds(-2)) + break; + pps = packetspersecond[MAVLink.MAVLINK_MSG_ID_VFR_HUD]; + if (hzratecheck(pps, hzrate)) + { + return; + } + break; + case MAVLink.MAV_DATA_STREAM.EXTRA3: + if (packetspersecondbuild[MAVLink.MAVLINK_MSG_ID_AHRS] < DateTime.Now.AddSeconds(-2)) + break; + pps = packetspersecond[MAVLink.MAVLINK_MSG_ID_AHRS]; + if (hzratecheck(pps, hzrate)) + { + return; + } + break; + case MAVLink.MAV_DATA_STREAM.POSITION: + if (packetspersecondbuild[MAVLink.MAVLINK_MSG_ID_GLOBAL_POSITION_INT] < DateTime.Now.AddSeconds(-2)) + break; + pps = packetspersecond[MAVLink.MAVLINK_MSG_ID_GLOBAL_POSITION_INT]; + if (hzratecheck(pps, hzrate)) + { + return; + } + break; + case MAVLink.MAV_DATA_STREAM.RAW_CONTROLLER: + if (packetspersecondbuild[MAVLink.MAVLINK_MSG_ID_RC_CHANNELS_SCALED] < DateTime.Now.AddSeconds(-2)) + break; + pps = packetspersecond[MAVLink.MAVLINK_MSG_ID_RC_CHANNELS_SCALED]; + if (hzratecheck(pps, hzrate)) + { + return; + } + break; + case MAVLink.MAV_DATA_STREAM.RAW_SENSORS: + if (packetspersecondbuild[MAVLink.MAVLINK_MSG_ID_RAW_IMU] < DateTime.Now.AddSeconds(-2)) + break; + pps = packetspersecond[MAVLink.MAVLINK_MSG_ID_RAW_IMU]; + if (hzratecheck(pps, hzrate)) + { + return; + } + break; + case MAVLink.MAV_DATA_STREAM.RC_CHANNELS: + if (packetspersecondbuild[MAVLink.MAVLINK_MSG_ID_RC_CHANNELS_RAW] < DateTime.Now.AddSeconds(-2)) + break; + pps = packetspersecond[MAVLink.MAVLINK_MSG_ID_RC_CHANNELS_RAW]; + if (hzratecheck(pps, hzrate)) + { + return; + } + break; + } + + //packetspersecond[temp[5]]; + + if (pps == 0 && hzrate == 0) + { + return; + } + + // log.InfoFormat("Request stream {0} at {1} hz", Enum.Parse(typeof(MAV_DATA_STREAM), id.ToString()), hzrate); + await GetDatastreamAsync(id, hzrate); + } + + public async Task StopDatastreamAsync(MAVLink.MAV_DATA_STREAM id, byte hzrate) + { + + double pps = 0; + + switch (id) + { + case MAVLink.MAV_DATA_STREAM.ALL: + + break; + case MAVLink.MAV_DATA_STREAM.EXTENDED_STATUS: + if (packetspersecondbuild[MAVLink.MAVLINK_MSG_ID_SYS_STATUS] < DateTime.Now.AddSeconds(-2)) + break; + pps = packetspersecond[MAVLink.MAVLINK_MSG_ID_SYS_STATUS]; + if (hzratecheck(pps, hzrate)) + { + return; + } + break; + case MAVLink.MAV_DATA_STREAM.EXTRA1: + if (packetspersecondbuild[MAVLink.MAVLINK_MSG_ID_ATTITUDE] < DateTime.Now.AddSeconds(-2)) + break; + pps = packetspersecond[MAVLink.MAVLINK_MSG_ID_ATTITUDE]; + if (hzratecheck(pps, hzrate)) + { + return; + } + break; + case MAVLink.MAV_DATA_STREAM.EXTRA2: + if (packetspersecondbuild[MAVLink.MAVLINK_MSG_ID_VFR_HUD] < DateTime.Now.AddSeconds(-2)) + break; + pps = packetspersecond[MAVLink.MAVLINK_MSG_ID_VFR_HUD]; + if (hzratecheck(pps, hzrate)) + { + return; + } + break; + case MAVLink.MAV_DATA_STREAM.EXTRA3: + if (packetspersecondbuild[MAVLink.MAVLINK_MSG_ID_AHRS] < DateTime.Now.AddSeconds(-2)) + break; + pps = packetspersecond[MAVLink.MAVLINK_MSG_ID_AHRS]; + if (hzratecheck(pps, hzrate)) + { + return; + } + break; + case MAVLink.MAV_DATA_STREAM.POSITION: + if (packetspersecondbuild[MAVLink.MAVLINK_MSG_ID_GLOBAL_POSITION_INT] < DateTime.Now.AddSeconds(-2)) + break; + pps = packetspersecond[MAVLink.MAVLINK_MSG_ID_GLOBAL_POSITION_INT]; + if (hzratecheck(pps, hzrate)) + { + return; + } + break; + case MAVLink.MAV_DATA_STREAM.RAW_CONTROLLER: + if (packetspersecondbuild[MAVLink.MAVLINK_MSG_ID_RC_CHANNELS_SCALED] < DateTime.Now.AddSeconds(-2)) + break; + pps = packetspersecond[MAVLink.MAVLINK_MSG_ID_RC_CHANNELS_SCALED]; + if (hzratecheck(pps, hzrate)) + { + return; + } + break; + case MAVLink.MAV_DATA_STREAM.RAW_SENSORS: + if (packetspersecondbuild[MAVLink.MAVLINK_MSG_ID_RAW_IMU] < DateTime.Now.AddSeconds(-2)) + break; + pps = packetspersecond[MAVLink.MAVLINK_MSG_ID_RAW_IMU]; + if (hzratecheck(pps, hzrate)) + { + return; + } + break; + //case MAVLink.MAV_DATA_STREAM.SCALED_IMU2: + + // break; + case MAVLink.MAV_DATA_STREAM.RC_CHANNELS: + if (packetspersecondbuild[MAVLink.MAVLINK_MSG_ID_RC_CHANNELS_RAW] < DateTime.Now.AddSeconds(-2)) + break; + pps = packetspersecond[MAVLink.MAVLINK_MSG_ID_RC_CHANNELS_RAW]; + if (hzratecheck(pps, hzrate)) + { + return; + } + break; + } + //log.InfoFormat("Request stream {0} at {1} hz", Enum.Parse(typeof(MAV_DATA_STREAM), id.ToString()), hzrate); + await StopGetDatastreamAsync(id, hzrate); + } + + bool hzratecheck(double pps, int hzrate) + { + + if (hzrate == 0 && pps == 0) + { + return true; + } + else if (hzrate == 1 && pps >= 0.5 && pps <= 2) + { + return true; + } + else if (hzrate == 3 && pps >= 2 && hzrate < 5) + { + return true; + } + else if (hzrate == 10 && pps > 5 && hzrate < 15) + { + return true; + } + else if (hzrate > 15 && pps > 15) + { + return true; + } + + return false; + + } + + async Task GetDatastreamAsync(MAVLink.MAV_DATA_STREAM id, byte hzrate) + { + MAVLink.mavlink_request_data_stream_t req = new MAVLink.mavlink_request_data_stream_t(); + req.target_system = sysid; + req.target_component = compid; + + req.req_message_rate = hzrate; + req.start_stop = 1; // start + req.req_stream_id = (byte)id; // id + + + // send each one twice. + await GeneratePacketAsync(MAVLink.MAVLINK_MSG_ID_REQUEST_DATA_STREAM, req); + await GeneratePacketAsync(MAVLink.MAVLINK_MSG_ID_REQUEST_DATA_STREAM, req); + } + public void GetCommunicationNumber(out int recnumber, out int sendnumber) + { + recnumber=_recnumber; + sendnumber = _sendnumber; + } + + + //重设数据量 + public void ResetCommunicationNumber() + { + _recnumber = 0; + _sendnumber = 0; + } + async Task StopGetDatastreamAsync(MAVLink.MAV_DATA_STREAM id, byte hzrate) + { + MAVLink.mavlink_request_data_stream_t req = new MAVLink.mavlink_request_data_stream_t(); + req.target_system = sysid; + req.target_component = compid; + + req.req_message_rate = 0; + req.start_stop = 0; // start + req.req_stream_id = (byte)id; // id + + // send each one twice. + await GeneratePacketAsync(MAVLink.MAVLINK_MSG_ID_REQUEST_DATA_STREAM, req); + await GeneratePacketAsync(MAVLink.MAVLINK_MSG_ID_REQUEST_DATA_STREAM, req); + } + + public async Task SetParamAsync(string paramname, float value) + { + // param type is set here, however it is always sent over the air as a float 100int = 100f. + var req = new MAVLink.mavlink_param_set_t { target_system = sysid, target_component = compid, param_type = (byte)param_types[paramname] }; + + byte[] temp = Encoding.GetEncoding("ASCII").GetBytes(paramname); + + Array.Resize(ref temp, 16); + req.param_id = temp; + req.param_value = (value); + + await GeneratePacketAsync((byte)MAVLink.MAVLINK_MSG_ID_PARAM_SET, req); + } + + public async Task SetParam2Async(string paramname, float value) + { + // param type is set here, however it is always sent over the air as a float 100int = 100f. + var req = new MAVLink.mavlink_param_set_t { target_system = sysid, target_component = compid, param_type = 4 }; + + byte[] temp = Encoding.GetEncoding("ASCII").GetBytes(paramname); + + Array.Resize(ref temp, 16); + req.param_id = temp; + req.param_value = (value); + + await GeneratePacketAsync((byte)MAVLink.MAVLINK_MSG_ID_PARAM_SET, req); + } + + public async Task SetModeAsync(ac2modes modein) + { + try + { + MAVLink.mavlink_set_mode_t mode = new MAVLink.mavlink_set_mode_t(); + + if (TranslateMode(modein, ref mode)) + { + await SetModeAsync(mode); + } + } + catch { RaiseMessageCreated("Failed to change Modes"); } + } + + public async Task SetModeAsync(string modein) + { + try + { + MAVLink.mavlink_set_mode_t mode = new MAVLink.mavlink_set_mode_t(); + + if (TranslateMode(modein, ref mode)) + { + await SetModeAsync(mode); + } + } + catch { RaiseMessageCreated("Failed to change Modes"); } + } + + public async Task SetModeAsync(MAVLink.mavlink_set_mode_t mode, MAVLink.MAV_MODE_FLAG base_mode = 0) + { + mode.base_mode |= (byte)base_mode; + + await GeneratePacketAsync(MAVLink.MAVLINK_MSG_ID_SET_MODE, mode); + await Task.Delay(10); + 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; + int datalen = 0; + var len = (length % msglen) == 0 ? length / msglen : (length / msglen) + 1; + for (int a = 0; a < len; a++) + { + datalen = Math.Min(length - a * msglen, msglen); + gps.data = new byte[msglen]; + Array.Copy(data, a * msglen, gps.data, 0, datalen); + gps.len = (byte)datalen; + gps.target_component = compid; + gps.target_system = sysid; + await GenerateRTKPacketAsync(MAVLink.MAVLINK_MSG_ID_GPS_INJECT_DATA, gps, gps.len); + // Console.WriteLine("send:" + (ushort)gps.len); + } + } + + public bool TranslateMode(ac2modes modein, ref MAVLink.mavlink_set_mode_t mode) + { + //MAVLink09.mavlink_set_mode_t mode = new MAVLink09.mavlink_set_mode_t(); + mode.target_system = sysid; + + switch (modein) + { + case ac2modes.STABILIZE: + case ac2modes.AUTO: + case ac2modes.RTL: + case ac2modes.LOITER: + case ac2modes.ACRO: + case ac2modes.ALT_HOLD: + case ac2modes.CIRCLE: + case ac2modes.POSITION: + case ac2modes.LAND: + case ac2modes.OF_LOITER: + case ac2modes.GUIDED: + mode.base_mode = (byte)MAVLink.MAV_MODE_FLAG.CUSTOM_MODE_ENABLED; + mode.custom_mode = (uint)modein; + break; + default: + RaiseMessageCreated("错误模式: " + modein); + return false; + } + return true; + } + + public bool TranslateMode(string modein, ref MAVLink.mavlink_set_mode_t mode) + { + //MAVLink09.mavlink_set_mode_t mode = new MAVLink09.mavlink_set_mode_t(); + mode.target_system = sysid; + + var modeValue = EnumTranslator.GetValue(modein.ToUpper()); + switch (modeValue) + { + case (int)ac2modes.STABILIZE: + case (int)ac2modes.AUTO: + case (int)ac2modes.RTL: + case (int)ac2modes.LOITER: + case (int)ac2modes.ACRO: + case (int)ac2modes.ALT_HOLD: + case (int)ac2modes.CIRCLE: + case (int)ac2modes.POSITION: + case (int)ac2modes.LAND: + case (int)ac2modes.OF_LOITER: + case (int)ac2modes.GUIDED: + mode.base_mode = (byte)MAVLink.MAV_MODE_FLAG.CUSTOM_MODE_ENABLED; + mode.custom_mode = (uint)modeValue; + break; + default: + RaiseMessageCreated("错误模式: " + modein); + return false; + } + return true; + } + + public async Task GetWPsAsync() + { + MAVLink.mavlink_mission_request_list_t req = new MAVLink.mavlink_mission_request_list_t(); + + req.target_system = sysid; + req.target_component = compid; + + await GeneratePacketAsync((byte)MAVLink.MAVLINK_MSG_ID_MISSION_REQUEST_LIST, req); + } + + /// + /// Gets specfied WP + /// + /// + /// WP + public async Task GetWPAsync(ushort index) + { + giveComport = true; + MAVLink.mavlink_mission_request_t req = new MAVLink.mavlink_mission_request_t(); + req.target_system = sysid; + req.target_component = compid; + req.seq = index; + //Console.WriteLine("getwp req "+ DateTime.Now.Millisecond); + // request + await GeneratePacketAsync((byte)MAVLink.MAVLINK_MSG_ID_MISSION_REQUEST, req); + } + + public async Task SetWPTotalAsync(ushort wp_total) + { + giveComport = true; + MAVLink.mavlink_mission_count_t req = new MAVLink.mavlink_mission_count_t(); + + req.target_system = sysid; + req.target_component = compid; // MSG_NAMES.MISSION_COUNT + + req.count = wp_total; + + await GeneratePacketAsync((byte)MAVLink.MAVLINK_MSG_ID_MISSION_COUNT, req); + } + + /// 任务信息。 + /// 任务编号。 + /// 坐标系。 + /// 0：在 Auto 模式下飞往指定位置；2：切到 GUIDED 模式，立即飞往指定位置；3：仅改变高度，立即执行。 + /// 指示是否自动继续执行下一个任务。 + public async Task WriteWPAsync(Locationwp loc, ushort index, MAVLink.MAV_FRAME frame, byte current = 0, byte autocontinue = 1) + { + MAVLink.mavlink_mission_item_t req = new MAVLink.mavlink_mission_item_t(); + + req.target_system = sysid; + req.target_component = compid; // MSG_NAMES.MISSION_ITEM + + req.command = loc.id; + + req.current = current; + req.autocontinue = autocontinue; + + req.frame = (byte)frame; + req.y = (float)(loc.lng); + req.x = (float)(loc.lat); + req.z = (float)(loc.alt); + + req.param1 = loc.p1; + req.param2 = loc.p2; + req.param3 = loc.p3; + req.param4 = loc.p4; + + req.seq = index; + + //log.InfoFormat("setWP {6} frame {0} cmd {1} p1 {2} x {3} y {4} z {5}", req.frame, req.command, req.param1, req.x, req.y, req.z, index); + + // request + await GeneratePacketAsync((byte)MAVLink.MAVLINK_MSG_ID_MISSION_ITEM, req); + } + + const float rad2deg = (float)(180 / Math.PI); + const float deg2rad = (float)(1.0 / rad2deg); + + public async Task SetSensorOffsetsAsync(sensoroffsetsenum sensor, float x, float y, float z) + { + return await DoCommandAsync(Plane.Protocols.MAVLink.MAV_CMD.PREFLIGHT_SET_SENSOR_OFFSETS, (int)sensor, x, y, z, 0, 0, 0); + } + + public byte rssi; + + + private void RaiseMessageCreated(string message) + { + var handler = MessageCreated; + if (handler != null) + { + var e = new MessageCreatedEventArgs { Message = message }; + RunOnUIThread(() => + { + handler(this, e); + }); + } + } + + private void RunOnUIThread(Action action) + { + if (SynchronizationContext.Current == _uiSyncContext) + { + action(); + } + else + { + _uiSyncContext.Post(delegate { action(); }, null); + } + } + + private void RaiseEventOnUIThread(EventHandler ev) + { + if (ev != null) + { + RunOnUIThread(() => ev(this, EventArgs.Empty)); + } + } + + private void RaiseEventOnUIThread(EventHandler ev, TEventArgs e) + { + if (ev != null) + { + RunOnUIThread(() => ev(this, e)); + } + } + } +} diff --git a/PlaneGcsSdk_Shared/Protocols/MavCommTypes.cs b/PlaneGcsSdk_Shared/Protocols/MavCommTypes.cs index 91da9cb..2159c36 100644 --- a/PlaneGcsSdk_Shared/Protocols/MavCommTypes.cs +++ b/PlaneGcsSdk_Shared/Protocols/MavCommTypes.cs @@ -1,177 +1,187 @@ -using System; -using System.Collections.Generic; -using System.Runtime.InteropServices; -using System.Text; - -namespace Plane.Protocols -{ - /* - * - * 通信模块协议 - * - */ - public class MavComm - { - /// - /// 发送数据时的数据包头 - /// - public const ushort COMM_SEND_PACKET_HEADER = 0x9551; - /// - /// 接受数据时的数据包头 - /// - public const ushort COMM_RECEIVE_PACKET_HEADER = 0x7713; - - - #region 命令类型 2字节 - //-----------------命令类型----------------- - /// - /// 查询状态 - /// - public const short COMM_QUERY_STATUS = 0x00; - - /// - /// 变更飞机总数及参与者 - /// - public const short COMM_SET_MAV_COUNT = 0x10; - - /// - /// 获取飞机详细信息 - /// - public const short COMM_GET_COPTERS_COUNT = 0x20; - - /// - /// 时间同步，不改变当前模式 - /// - public const short COMM_ASYN_TIME = 0x30; - - /// - /// 进入空闲模式 - /// - public const short COMM_IDLE_MODE = 0x50; - - /// - /// 进入搜索模式，命名编号targetID memcpy(pdata,input,2); - /// - public const short COMM_SEARCH_MODE = 0x51; - - /// - /// 进入航点下载模式 (写航点) - /// - public const short COMM_DOWNLOAD_MODE = 0x52; - - /// - /// 下载模式通信 - /// - public const short COMM_DOWNLOAD_COMM = 0x53; - - /// - /// 进入飞行模式（无负载） - /// - public const short COMM_FLIGHT_MODE = 0x54; - - /// - /// 进入飞行模式（有负载数据） - /// - public const short COMM_FLIGHT_LOAD_MODE = 0x55; - - - /// - /// 发送非针对飞控的内部控制指令 - /// - public const short COMM_TO_MODULE = 0x5F; - - - - /// - /// 通信模块 - /// - public const short COMM_NOTIFICATION = 0x1234; - - /// - /// 空中升级(更新通信模块飞机端) - /// - public const short COMM_UPDATE_COPTER_MODULE = 0xFD; - - - - /// - /// 测试模块 - /// - public const short COMM_TEST_MODULE = 0x3C; - - - - #endregion - - public enum CommMode - { - IDLE = 0, - SEARCH = 1, - DOWNLOAD = 3, - FLIGHT = 4 - } - - public enum MessageType - { - STR = 0, - SCAN2 = 2, - SCAN3 = 3 - } - - //search 搜索模式相关 - public const ushort SEARCH_COMPLETED = 0x0502; //搜索模式 扫描成功 - public const ushort FREQUENCY_HOPPING_COMPLETED = 0x0503; //搜索模式 跳频表更新成功 - public const ushort SEARCH_FINDCOPTER = 0x0401; //搜索模式 找到指定目标 - public const ushort FREQUENCY_HOPPING_ERROR = 0x0100; //搜索模式 跳频表更新错误 - public const ushort SEARCH_OUTTIME = 0xFFFF; //搜索模式 没有人应答 超时时间20S - - - public const ushort MISSION_SEND_COMPLETED = 0x0503; //航点传输到飞控成功，可开始传输下个飞机航点信息, - //需要再后续等待一个成功消息，才算完成 - - public const ushort P2P_SEND_FAILED = 0x0600; //点对点通信发送失败 - - public const ushort ERROR_CODE_START = 0x0100; - public const ushort ERROR_CODE_END = 0x03FF; - - - [StructLayout(LayoutKind.Sequential, Pack = 1, Size = 2)] - public struct comm_set_mav_count - { - public Int16 mav_count; //飞机总数 - }; - - [StructLayout(LayoutKind.Sequential, Pack = 1, Size = 4)] - public struct comm_update_copter_module - { - public Int16 mav_count; //飞机总数 - public Int16 update_code; - }; - - - [StructLayout(LayoutKind.Sequential, Pack = 1, Size = 8)] - public struct comm_asyn_time - { - public Int64 time_stamp; - }; - - [StructLayout(LayoutKind.Sequential, Pack = 1, Size = 28)] - public struct comm_copter_info - { - public Int32 control_mode; - public UInt32 lat; - public UInt32 lng; - public float retain; - public Int32 alt; - - public Int16 gps_status; - - public byte lock_status; - public byte gps_num_sats; - public Int16 battery_voltage; - public UInt16 heading; - - }; - - } -} +using System; +using System.Collections.Generic; +using System.Runtime.InteropServices; +using System.Text; + +namespace Plane.Protocols +{ + /* + * + * 通信模块协议 + * + */ + public class MavComm + { + /// + /// 发送数据时的数据包头 + /// + public const ushort COMM_SEND_PACKET_HEADER = 0x9551; + /// + /// 接受数据时的数据包头 + /// + public const ushort COMM_RECEIVE_PACKET_HEADER = 0x7713; + + + #region 命令类型 2字节 + //-----------------命令类型----------------- + /// + /// 查询状态 + /// + public const short COMM_QUERY_STATUS = 0x00; + + /// + /// 变更飞机总数及参与者 + /// + public const short COMM_SET_MAV_COUNT = 0x10; + + /// + /// 获取飞机详细信息 + /// + public const short COMM_GET_COPTERS_COUNT = 0x20; + + /// + /// 时间同步，不改变当前模式 + /// + public const short COMM_ASYN_TIME = 0x30; + + /// + /// 进入空闲模式 + /// + public const short COMM_IDLE_MODE = 0x50; + + /// + /// 进入搜索模式，命名编号targetID memcpy(pdata,input,2); + /// + public const short COMM_SEARCH_MODE = 0x51; + + /// + /// 进入航点下载模式 (写航点) + /// + public const short COMM_DOWNLOAD_MODE = 0x52; + + /// + /// 下载模式通信 + /// + public const short COMM_DOWNLOAD_COMM = 0x53; + + /// + /// 进入飞行模式（无负载） + /// + public const short COMM_FLIGHT_MODE = 0x54; + + /// + /// 进入飞行模式（有负载数据） + /// + public const short COMM_FLIGHT_LOAD_MODE = 0x55; + + + /// + /// 发送非针对飞控的内部控制指令 + /// + public const short COMM_TO_MODULE = 0x5F; + + + + /// + /// 通信模块 + /// + public const short COMM_NOTIFICATION = 0x1234; + + /// + /// 空中升级(更新通信模块飞机端) + /// + public const short COMM_UPDATE_COPTER_MODULE = 0xFD; + + + + /// + /// 测试模块 + /// + public const short COMM_TEST_MODULE = 0x3C; + + + + #endregion + + public enum CommMode + { + IDLE = 0, + SEARCH = 1, + DOWNLOAD = 3, + FLIGHT = 4 + } + + public enum MessageType + { + STR = 0, + SCAN2 = 2, + SCAN3 = 3 + } + + //search 搜索模式相关 + public const ushort SEARCH_COMPLETED = 0x0502; //搜索模式 扫描成功 + public const ushort FREQUENCY_HOPPING_COMPLETED = 0x0503; //搜索模式 跳频表更新成功 + public const ushort SEARCH_FINDCOPTER = 0x0401; //搜索模式 找到指定目标 + public const ushort FREQUENCY_HOPPING_ERROR = 0x0100; //搜索模式 跳频表更新错误 + public const ushort SEARCH_OUTTIME = 0xFFFF; //搜索模式 没有人应答 超时时间20S + + + public const ushort MISSION_SEND_COMPLETED = 0x0503; //航点传输到飞控成功，可开始传输下个飞机航点信息, + //需要再后续等待一个成功消息，才算完成 + + public const ushort P2P_SEND_FAILED = 0x0600; //点对点通信发送失败 + + public const ushort ERROR_CODE_START = 0x0100; + public const ushort ERROR_CODE_END = 0x03FF; + + + [StructLayout(LayoutKind.Sequential, Pack = 1, Size = 2)] + public struct comm_set_mav_count + { + public Int16 mav_count; //飞机总数 + }; + + [StructLayout(LayoutKind.Sequential, Pack = 1, Size = 4)] + public struct comm_update_copter_module + { + public Int16 mav_count; //飞机总数 + public Int16 update_code; + }; + + + [StructLayout(LayoutKind.Sequential, Pack = 1, Size = 8)] + public struct comm_asyn_time + { + public Int64 time_stamp; + }; + + [StructLayout(LayoutKind.Sequential, Pack = 1, Size = 28)] + public struct comm_copter_info + { + // public Int32 control_mode; //飞行模式(自动，悬停，定高，返航) + public byte control_mode; //飞行模式(自动，悬停，定高，返航) + public byte rpecheck_fault; //是否有 预解锁错误 + public byte reserveddata1; //保留 + public byte reserveddata2; //保留 + + + public UInt32 lat; // 经度 in 1E7 degrees + public UInt32 lng; // 维度 in 1E7 degrees + public float retain; // 写参数序列号,返回版本号等不特定返回数据 + public Int32 alt; // millimeters above home + + + public byte gps_status; //锁定类型 (无锁定,3D锁定，RTK浮点，RTK固定) + public byte error_code; //错误号，0表示无错误 --放到低位为了和之前兼容 + + + + public byte lock_status; //以及是否解锁 + public byte gps_num_sats; // 卫星数量 + public Int16 battery_voltage; // 电池电压mV + public UInt16 heading; //方向角度 + + }; + + } +}