diff --git a/PlaneGcsSdk_Shared/Communication/TcpConnection.cs b/PlaneGcsSdk_Shared/Communication/TcpConnection.cs
index 8cf256a..b0e194b 100644
--- a/PlaneGcsSdk_Shared/Communication/TcpConnection.cs
+++ b/PlaneGcsSdk_Shared/Communication/TcpConnection.cs
@@ -1,118 +1,125 @@
-#if !NETFX_CORE
-
-using System;
-using System.Net;
-using System.Net.Sockets;
-using System.Threading.Tasks;
-
-namespace Plane.Communication
-{
- ///
- /// 提供作为 TCP 客户端通信的能力。
- ///
- public class TcpConnection : TcpConnectionBase
- {
- private string _remoteHostname;
- const int tcpReceiveBufferSize = 20 * 1024; //40 * 1024,
- const int tcpReceiveTimeout = 1200;
-
- private int _remotePort;
-
- public TcpConnection(string remoteHostname, int remotePort = 5250)
- {
- _remoteHostname = remoteHostname;
- _remotePort = remotePort;
- _client = new TcpClient
- {
- ReceiveBufferSize = tcpReceiveBufferSize,
- ReceiveTimeout = tcpReceiveTimeout
- };
- }
-
- public void Open()
- {
- if (!IsOpen)
- {
- try
- {
- _client.Connect(_remoteHostname, _remotePort);
- }
- catch (SocketException)
- {
- CreateClientAndConnect();
- }
- catch (ObjectDisposedException)
- {
- CreateClientAndConnect();
- }
- _isBroken = false;
- }
-
- _stream = _client.GetStream();
- }
-
- public async Task BindIP(string ip)
- {
- bool bind = false;
- try
- {
- IPAddress IPLocal = IPAddress.Parse(ip);
- _client.Client.Bind(new IPEndPoint(IPLocal, 0));
- bind = true;
- }
- catch
- {
- bind = false;
- }
- await Task.Delay(10).ConfigureAwait(false);
- return bind;
- }
-
- public override async Task OpenAsync()
- {
- string logstr;
- if (!IsOpen)
- {
- try
- {
- await _client.ConnectAsync(_remoteHostname, _remotePort).ConfigureAwait(false);
- }
- //屏蔽掉异常处理
- //CreateClientAndConnectAsync会new一个TcpClient并且重新连接
- //之前设置的TcpClient中Socket Bind会无效，在多网卡工作时会导致断线重连的时间特别长
- catch (SocketException e)
- {
- logstr= e.Message;
- //await CreateClientAndConnectAsync();
- }
- catch (ObjectDisposedException)
- {
- //await CreateClientAndConnectAsync();
- }
- _isBroken = false;
- }
- _stream = _client.GetStream();
- }
-
- private void CreateClientAndConnect()
- {
- _client = new TcpClient(_remoteHostname, _remotePort)
- {
- ReceiveBufferSize = tcpReceiveBufferSize,
- ReceiveTimeout = tcpReceiveTimeout
- };
- }
-
- private async Task CreateClientAndConnectAsync()
- {
- _client = new TcpClient
- {
- ReceiveBufferSize = tcpReceiveBufferSize,// 40 * 1024,
- ReceiveTimeout = tcpReceiveTimeout// 1200
- };
- await _client.ConnectAsync(_remoteHostname, _remotePort).ConfigureAwait(false);
- }
- }
-}
-
-#endif
+#if !NETFX_CORE
+
+using System;
+using System.Net;
+using System.Net.Sockets;
+using System.Threading.Tasks;
+
+namespace Plane.Communication
+{
+ ///
+ /// 提供作为 TCP 客户端通信的能力。
+ ///
+ public class TcpConnection : TcpConnectionBase
+ {
+ private string _remoteHostname;
+ const int tcpReceiveBufferSize = 20 * 1024; //40 * 1024,
+ const int tcpReceiveTimeout = 1200;
+
+ private int _remotePort;
+
+ public TcpConnection(string remoteHostname, int remotePort = 5250)
+ {
+ _remoteHostname = remoteHostname;
+ _remotePort = remotePort;
+ _client = new TcpClient
+ {
+ ReceiveBufferSize = tcpReceiveBufferSize,
+ ReceiveTimeout = tcpReceiveTimeout
+ };
+ }
+
+ public void Open()
+ {
+ if (!IsOpen)
+ {
+ try
+ {
+ _client.Connect(_remoteHostname, _remotePort);
+ }
+ catch (SocketException)
+ {
+ CreateClientAndConnect();
+ }
+ catch (ObjectDisposedException)
+ {
+ CreateClientAndConnect();
+ }
+ _isBroken = false;
+ }
+
+ _stream = _client.GetStream();
+ }
+
+ public async Task BindIP(string ip)
+ {
+ bool bind = false;
+ try
+ {
+ IPAddress IPLocal = IPAddress.Parse(ip);
+ _client.Client.Bind(new IPEndPoint(IPLocal, 0));
+ bind = true;
+ }
+ catch
+ {
+ bind = false;
+ }
+ await Task.Delay(10).ConfigureAwait(false);
+ return bind;
+ }
+
+ public override async Task OpenAsync()
+ {
+ string logstr;
+ if (!IsOpen)
+ {
+ if (_client == null)
+ CreateClientAndConnect();
+ try
+ {
+ var connectTask = _client.ConnectAsync(_remoteHostname, _remotePort);
+ if (await Task.WhenAny(connectTask, Task.Delay(5000)) == connectTask)
+ {
+ await connectTask.ConfigureAwait(false);
+ }
+ else
+ {
+ // Connection timed out
+ throw new TimeoutException("Connection timed out");
+ }
+ }
+ catch (SocketException e)
+ {
+ logstr = e.Message;
+ }
+ catch (ObjectDisposedException)
+ {
+ }
+ _isBroken = false;
+ }
+ _stream = _client.GetStream();
+ }
+
+
+ private void CreateClientAndConnect()
+ {
+ _client = new TcpClient(_remoteHostname, _remotePort)
+ {
+ ReceiveBufferSize = tcpReceiveBufferSize,
+ ReceiveTimeout = tcpReceiveTimeout
+ };
+ }
+
+ private async Task CreateClientAndConnectAsync()
+ {
+ _client = new TcpClient
+ {
+ ReceiveBufferSize = tcpReceiveBufferSize,// 40 * 1024,
+ ReceiveTimeout = tcpReceiveTimeout// 1200
+ };
+ await _client.ConnectAsync(_remoteHostname, _remotePort).ConfigureAwait(false);
+ }
+ }
+}
+
+#endif
diff --git a/PlaneGcsSdk_Shared/CommunicationManagement/CommModule.cs b/PlaneGcsSdk_Shared/CommunicationManagement/CommModule.cs
index 1c29095..997cd1a 100644
--- a/PlaneGcsSdk_Shared/CommunicationManagement/CommModule.cs
+++ b/PlaneGcsSdk_Shared/CommunicationManagement/CommModule.cs
@@ -1,915 +1,918 @@
-using Plane.Communication;
-using Plane.Protocols;
-using System;
-using System.Collections.Generic;
-using System.Diagnostics;
-using System.Linq;
-using System.Net;
-using System.Net.NetworkInformation;
-using System.Net.Sockets;
-using System.Text;
-using System.Threading.Tasks;
-using System.Windows.Media;
-
-using System.Runtime.InteropServices;
-using Plane.Windows.Messages;
-
-namespace Plane.CommunicationManagement
-{
- //通信模块
- public partial class CommModuleManager: ExceptionThrownEventSource, IDisposable
- {
- [DllImport("kernel32")] //返回取得字符串缓冲区的长度
- private static extern int GetPrivateProfileString(string section, string key, string def, StringBuilder retVal, int size, string filePath);
-
- private static CommModuleManager _Instance = new CommModuleManager();
- public static CommModuleManager Instance { get { return _Instance; } }
-
- public TcpConnection Connection = null;
- public bool CommOK = false;
- private const string MODULE_IP = "192.168.199.51"; //"192.168.199.51"; // "192.168.199.51"; //在用版本 ----//"192.168.1.65"; //新版本
- private string ServerIP = MODULE_IP;
- // private const string LOCAL_IP = "192.168.199.50"; //"192.168.199.50";
- private const int PORT = 9551;
- private bool _disposed;
- public int CommModuleCopterCount = 0;
- public int CommModuleCurMode = 0;
- private long packetCountNum = 0;
-
-
- ///
- /// 用于判断写入任务操作超时的秒表。
- ///
- private Stopwatch _writeMissionStopwatch;
-
- private void LoadIPSet()
- {
- StringBuilder temp = new StringBuilder(255);
-
- string path = Environment.CurrentDirectory + @"\Config.ini";
-
- int i = GetPrivateProfileString("Default", "ServerIP", "", temp, 255, path);
- ServerIP= temp.ToString();
-
- if (ServerIP == "") ServerIP = MODULE_IP;
- }
-
-
- public async Task ConnectAsync()
- {
- LoadIPSet();
- Connection = new TcpConnection(ServerIP, PORT);
- await ConnectOpenAsync();
- }
-
- public void Connect()
- {
- LoadIPSet();
- Task.Run(async () =>
- {
- Connection = new TcpConnection(ServerIP, PORT);
- await ConnectOpenAsync();
- }
- );
- }
-
- public void CloseConnection()
- {
- CommOK = false;
- Connection?.Close();
- }
-
-
-
-
-
-
-
-
-/*
-
-
- //获取内网IP
- private IPAddress GetInternalIP()
- {
- NetworkInterface[] nics = NetworkInterface.GetAllNetworkInterfaces();
-
- foreach (NetworkInterface adapter in nics)
- {
- foreach (var uni in adapter.GetIPProperties().UnicastAddresses)
- {
- if (uni.Address.AddressFamily == AddressFamily.InterNetwork)
- {
- return uni.Address;
- }
- }
- }
- return null;
- }
-
- ///
- /// 获取本地连接IP地址、无线连接IP地址
- ///
- /// 0:本地连接，1:本地连接1，2:无线网络连接
- ///
- public static IPAddress GetIP(int k)
- {
- NetworkInterface[] interfaces = System.Net.NetworkInformation.NetworkInterface.GetAllNetworkInterfaces();
- int len = interfaces.Length;
- IPAddress[] mip = { IPAddress.Parse("0.0.0.0"), IPAddress.Parse("0.0.0.0"), IPAddress.Parse("0.0.0.0") };
-
- for (int i = 0; i < len; i++)
- {
- NetworkInterface ni = interfaces[i];
-
- if (ni.Name == "本地连接")
- {
- IPInterfaceProperties property = ni.GetIPProperties();
- foreach (UnicastIPAddressInformation ip in property.UnicastAddresses)
- {
- if (ip.Address.AddressFamily == System.Net.Sockets.AddressFamily.InterNetwork)
- {
- mip[0] = ip.Address;
- }
- }
- }
- else if (ni.Name == "本地连接2")
- {
- IPInterfaceProperties property = ni.GetIPProperties();
- foreach (UnicastIPAddressInformation ip in property.UnicastAddresses)
- {
- if (ip.Address.AddressFamily == System.Net.Sockets.AddressFamily.InterNetwork)
- {
- mip[1] = ip.Address;
- }
- }
- }
- else if (ni.Name == "无线网络连接")
- {
- IPInterfaceProperties property = ni.GetIPProperties();
- foreach (UnicastIPAddressInformation ip in property.UnicastAddresses)
- {
- if (ip.Address.AddressFamily == System.Net.Sockets.AddressFamily.InterNetwork)
- {
- mip[2] = ip.Address;
- }
- }
- }
- }
- return mip[k];
- }
-
- */
- private async Task ConnectOpenAsync()
- {
- CommOK = true;
- //不绑定IP也可以通讯，有线网络需要设置192.168.199.X网段地址
- // string locip = GetIP(0).ToString();
- // bool bind = await Connection.BindIP(locip);
- // if (bind)
- {
- try
- {
- await Connection.OpenAsync().ConfigureAwait(false);
- }
- catch
- { }
- }
-
- if (Connection.IsOnline)
- {
- Message.Connect(true);
- SendQuery();
- await StartReadingPacketsAsync();
- }
- else
- {
- Message.Connect(false);
- Reconnect();
- }
- }
-
- //循环发送查询，保持通信正常
- private void SendQuery()
- {
- Task.Run(async () =>
- {
- long lastPacketCountNum = 0;
- int faulttimes = 0; //等待没有数据次数
- const int revtimeout = 2000; //等待超时ms old=1200
- while (CommOK)
- {
- if (Connection != null)
- {
- //发送心跳包-等待回复消息
- await WriteCommPacketAsync(0, MavComm.COMM_QUERY_STATUS);
- // await SendQueryStatusPacketsAsync();
- await Task.Delay(revtimeout).ConfigureAwait(false);
- //等待是否接受到返回的数据包，如果没有收到认为连结断开，重新联接
- if (packetCountNum > lastPacketCountNum)
- {
- lastPacketCountNum = packetCountNum;
- if (faulttimes>0)
- Message.Show("收到新数据包，重置超时次数...");
- faulttimes = 0;
- }
- else
- {
- faulttimes++;
- Message.Show("设备回复超时("+ revtimeout + "ms)" + faulttimes+"次");
- //2次没数据包返回就重连
- if (faulttimes > 1)
- {
- Message.Show("长时间未收到设备回复数据包");
- break;
- }
- }
- }
- }
- Message.Show("主动断开连接，重连");
- Reconnect();
- }).ConfigureAwait(false);
- }
-
- /* //直接调用不用封装函数了
- private async Task SendQueryStatusPacketsAsync()
- {
- await WriteCommPacketAsync(0, MavComm.COMM_QUERY_STATUS);
- await Task.Delay(100);//100
- }
- */
-
- int times = 1;
- private void Reconnect()
- {
- // Message.Show($"正在重新连接...");
- Task.Run(async () =>
- {
- CloseConnection();
- await Task.Delay(250).ConfigureAwait(false);
- //Message.Show($"正在重连:次数{times++}");
- await Task.Delay(250).ConfigureAwait(false);
- await ConnectAsync();
-
- }).ConfigureAwait(false);
- }
-
- private async Task StartReadingPacketsAsync()
- {
- await Task.Run(async () =>
- {
- while (CommOK)
- {
- if (Connection == null || !Connection.IsOnline)
- {
- if (Connection==null)
- Message.Show("空连接异常");
- else if (!Connection.IsOnline)
- {
- Message.Show("检测到设备已断开连接");
- }
- break;
- }
-
- var packet = await ReadPacketAsync().ConfigureAwait(false);
- if (packet != null)
- {
- packetCountNum++;
- short serialNum = BitConverter.ToInt16(packet, 4);
- short copterNum = BitConverter.ToInt16(packet, 6);
- short commandType = BitConverter.ToInt16(packet, 8);
- byte[] dealData;
- switch (commandType)
- {
- case MavComm.COMM_DOWNLOAD_MODE:
- dealData = packet.Skip(10).ToArray();
- AnalyzeMissionStartPacket(copterNum, dealData);
- break;
-
- case MavComm.COMM_GET_COPTERS_COUNT:
- dealData = packet.Skip(16).ToArray();
- AnalyzeCopterInfosPacket(dealData);
- break;
-
- case MavComm.COMM_NOTIFICATION:
- short messageType = BitConverter.ToInt16(packet, 10);
- dealData = packet.Skip(12).ToArray();
- switch (messageType)
- {
- case (short)MavComm.MessageType.STR:
- AnalyzeStringPacket(copterNum, dealData);
- break;
-
- case (short)MavComm.MessageType.SCAN2:
- AnalyzeComm2RetrunPacket(copterNum, dealData);
- break;
-
- case (short)MavComm.MessageType.SCAN3:
- AnalyzeComm3RetrunPacket(copterNum, dealData);
- break;
- case 4:
- AnalyzeComm4RetrunPacket(copterNum, dealData);
- break;
- }
- break;
- case 0:
- int packetLength = packet.Length;
- short errorId = BitConverter.ToInt16(packet, packetLength - 6);
- short copterCount = BitConverter.ToInt16(packet, packetLength - 4);
- short curMode = BitConverter.ToInt16(packet, packetLength - 2);
- string msg = string.Format("错误Id={0}, 检测飞机总数={1}，工作模式={2} 流水号={3}", errorId, copterCount, curMode, serialNum);
- CommModuleCopterCount = copterCount;
- CommModuleCurMode = curMode;
- //Message.Show(msg);
- break;
- }
- }
- }
- }).ConfigureAwait(false);
- Message.Show("退出读设备数据........");
- //Reconnect();
- }
-
- private async Task ReadPacketAsync()
- {
- int readnumber = 0;
- if (Connection == null || !Connection.IsOnline)
- {
- return null;
- }
- try
- {
- byte[] bufferHaed = new byte[2];
- readnumber = await Connection.ReadAsync(bufferHaed, 0, 2);
- if (bufferHaed[0] == 0x13 && bufferHaed[1] == 0x77)
- {
- byte[] bufferLength = new byte[2];
- readnumber = await Connection.ReadAsync(bufferLength, 0, 2);
- short datalength = BitConverter.ToInt16(bufferLength, 0);
- //Console.WriteLine("datalength = "+ datalength);
-
- byte[] bufferData = new byte[datalength];
- readnumber = await Connection.ReadAsync(bufferData, 0, datalength);
-
-// foreach (byte b in bufferData)
-// {
-// Console.Write(b.ToString("X2"));
-// Console.Write(" ");
-// }
-// Console.WriteLine("");
-
- byte[] needCRCData = new byte[datalength + 4];
- Array.Copy(bufferHaed, 0, needCRCData, 0, 2);
- Array.Copy(bufferLength, 0, needCRCData, 2, 2);
- Array.Copy(bufferData, 0, needCRCData, 4, datalength);
-
- byte[] crc = checkCRC16(needCRCData);
- byte[] bufferCRC16 = new byte[2];
- readnumber = await Connection.ReadAsync(bufferCRC16, 0, 2);
-
- if (crc[0] == bufferCRC16[0] && crc[1] == bufferCRC16[1])
- {
- return needCRCData;
- }
- }
- }
- catch
- {
- //Console.WriteLine("错误");
- return null;
- }
- return null;
- }
-
-
-
- short serial_Number = 1;
-
- public async Task GenerateDataAsync(short copterId, short messageType, TMavCommPacket indata)
- {
- byte[] data;
- data = MavlinkUtil.StructureToByteArray(indata);
- await WriteCommPacketAsync(copterId, messageType, data);
- }
-
- public async Task UpdateCommModule()
- {
- MavComm.comm_update_copter_module commUpdate = new MavComm.comm_update_copter_module();
- commUpdate.mav_count = (short)CommModuleCopterCount;
- commUpdate.update_code = 0x7713;
- await GenerateDataAsync(0, MavComm.COMM_UPDATE_COPTER_MODULE, commUpdate);
- }
-
-
- public async Task TestModule(short ModuleNo, short TestLong=100)
- {
-
-
- byte[] packet = new byte[2];
- packet[0] = (byte)(ModuleNo);
- packet[1] = (byte)(TestLong);
- byte[] senddata = packet;
-
- // Message.Show("长度 = " + senddata.Length);
- await WriteCommPacketAsync(0, MavComm.COMM_TEST_MODULE, senddata);
- // await Task.Delay(1000).ConfigureAwait(false);
-
- }
-
-
-
- public async Task SetModulePower(short ModuleNo, short ModulePower)
- {
- byte[] packet = { 0x01, 0x00, 0x03, 0x00, 0x01, 0x0A };
- packet[2] = (byte)(ModuleNo & 0xFF);
- packet[3] = (byte)((ModuleNo & 0xFF00) >> 8);
- packet[5] = (byte)(ModulePower);
- await WriteCommPacketAsync(0, MavComm.COMM_TO_MODULE, packet);
- }
-
-
-
-
-
- bool temp = false;
- //测试用 灯光间隔1S闪烁
- public async Task TestLED(short id, string colorString)
- {
-
- MAVLink.mavlink_msg_id_led_control led = new MAVLink.mavlink_msg_id_led_control();
- led.target_system = 1;
- led.target_component = 1;//(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];
-
- if (colorString == "") colorString = "330033";
-
- Color color = (Color)ColorConverter.ConvertFromString("#" + colorString);
-
- led.custom_bytes[0] = color.R;
- led.custom_bytes[1] = color.G;
- led.custom_bytes[2] = color.B;
- led.custom_bytes[3] = 3;
- byte[] data;
- data = MavlinkUtil.StructureToByteArray(led);
-
- byte[] packet = new byte[data.Length + 6 + 2];
- packet[0] = MAVLink.MAVLINK_STX;
-
- packet[1] = (byte)(data.Length);
- packet[2] = 1;
- packet[3] = 255; // this is always 255 - MYGCS
-
- packet[4] = (byte)MAVLink.MAV_COMPONENT.MAV_COMP_ID_MISSIONPLANNER;
-
- packet[5] = MAVLink.MAVLINK_MSG_ID_LED_CONTROL;
- int i = 6;
- foreach (byte b in data)
- {
- packet[i] = b;
- i++;
- }
-
- ushort checksum = MavlinkCRC.crc_calculate(packet, packet[1] + 6);
- checksum = MavlinkCRC.crc_accumulate(MAVLink.MAVLINK_MESSAGE_CRCS[MAVLink.MAVLINK_MSG_ID_LED_CONTROL], 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;
-
-
- //await WriteCommPacketAsync(id, MavComm.COMM_DOWNLOAD_COMM, packet);
- byte[] senddata = packet;
-
-// for (int times = 0; times < 14; times++)
-// {
-// senddata = senddata.Concat(packet).ToArray();
-// }
-
-// byte[] myByteArray = Enumerable.Repeat((byte)0x08, 15).ToArray();
-//
-// senddata = senddata.Concat(myByteArray).ToArray();
-
- temp = !temp;
- while (temp)
- {
- Message.Show("测试灯光，长度 = " + senddata.Length);
- await WriteCommPacketAsync(id, MavComm.COMM_DOWNLOAD_COMM, senddata);
- await Task.Delay(1000).ConfigureAwait(false);
- }
- }
-
- //拉烟测试
- public async Task TestFire(short id, int channel)
- {
-
- MAVLink.mavlink_msg_id_led_control led = new MAVLink.mavlink_msg_id_led_control();
- led.target_system = 1;
- led.target_component = 1;//(byte)MAVLink.MAV_COMPONENT.MAV_COMP_ID_SYSTEM_CONTROL;;
- led.instance = 0;
- led.pattern = 0;
- led.custom_len = 1;//命令类型——测试拉烟
- led.custom_bytes = new byte[24];
-
- led.custom_bytes[0] = (byte)channel;
-
- byte[] data;
- data = MavlinkUtil.StructureToByteArray(led);
-
- byte[] packet = new byte[data.Length + 6 + 2];
- packet[0] = MAVLink.MAVLINK_STX;
-
- packet[1] = (byte)(data.Length);
- packet[2] = 1;
- packet[3] = 255; // this is always 255 - MYGCS
-
- packet[4] = (byte)MAVLink.MAV_COMPONENT.MAV_COMP_ID_MISSIONPLANNER;
-
- packet[5] = MAVLink.MAVLINK_MSG_ID_LED_CONTROL;
- int i = 6;
- foreach (byte b in data)
- {
- packet[i] = b;
- i++;
- }
-
- ushort checksum = MavlinkCRC.crc_calculate(packet, packet[1] + 6);
- checksum = MavlinkCRC.crc_accumulate(MAVLink.MAVLINK_MESSAGE_CRCS[MAVLink.MAVLINK_MSG_ID_LED_CONTROL], 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;
-
-
- //await WriteCommPacketAsync(id, MavComm.COMM_DOWNLOAD_COMM, packet);
- byte[] senddata = packet;
-
- for (int times = 0; times < 3; times++)
- {
- senddata = senddata.Concat(packet).ToArray();
- }
-
-
- temp = !temp;
- while (temp)
- {
- await WriteCommPacketAsync(id, MavComm.COMM_DOWNLOAD_COMM, packet);
- await Task.Delay(1000).ConfigureAwait(false);
- }
- }
-
-
- public async Task TestBattery(short id, float minivol)
- {
-
- MAVLink.mavlink_msg_id_led_control led = new MAVLink.mavlink_msg_id_led_control();
- led.target_system = 1;
- led.target_component = 1;//(byte)MAVLink.MAV_COMPONENT.MAV_COMP_ID_SYSTEM_CONTROL;;
- led.instance = 0;
- led.pattern = 0;
- led.custom_len = 5;//命令类型——测试电池电压
- led.custom_bytes = new byte[24];
-
- led.custom_bytes[0] = (byte)(int)(minivol); //整数部分
- led.custom_bytes[1] = (byte)(int)((minivol-(int)minivol)*100); //小数部分 --2位
-
-
-
-
-
- byte[] data;
- data = MavlinkUtil.StructureToByteArray(led);
-
- byte[] packet = new byte[data.Length + 6 + 2];
- packet[0] = MAVLink.MAVLINK_STX;
-
- packet[1] = (byte)(data.Length);
- packet[2] = 1;
- packet[3] = 255; // this is always 255 - MYGCS
-
- packet[4] = (byte)MAVLink.MAV_COMPONENT.MAV_COMP_ID_MISSIONPLANNER;
-
- packet[5] = MAVLink.MAVLINK_MSG_ID_LED_CONTROL;
- int i = 6;
- foreach (byte b in data)
- {
- packet[i] = b;
- i++;
- }
-
- ushort checksum = MavlinkCRC.crc_calculate(packet, packet[1] + 6);
- checksum = MavlinkCRC.crc_accumulate(MAVLink.MAVLINK_MESSAGE_CRCS[MAVLink.MAVLINK_MSG_ID_LED_CONTROL], 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;
-
-
- //await WriteCommPacketAsync(id, MavComm.COMM_DOWNLOAD_COMM, packet);
- byte[] senddata = packet;
-
- for (int times = 0; times < 3; times++)
- {
- senddata = senddata.Concat(packet).ToArray();
- }
-
-
- { //发3次
- await WriteCommPacketAsync(id, MavComm.COMM_DOWNLOAD_COMM, packet);
- await Task.Delay(1000).ConfigureAwait(false);
- await WriteCommPacketAsync(id, MavComm.COMM_DOWNLOAD_COMM, packet);
- await Task.Delay(1000).ConfigureAwait(false);
- await WriteCommPacketAsync(id, MavComm.COMM_DOWNLOAD_COMM, packet);
-
- }
- }
-
-
- ///
- /// 生成通信模块packet并且发送
- ///
- /// 飞机ID
- /// 命令类型
- /// 数据类型：空表示只切换模式
- /// 小批量数据包
- ///
- public async Task WriteCommPacketAsync(short copterId, short messageType, byte[] data = null, byte[] batchPacket = null)
- {
- //新盒子的协议--暂时不用-需要问张伟
- /*
- if (messageType == MavComm.COMM_DOWNLOAD_COMM && copterId == 0)
- {
- short byteNum;
- short length;
- if (batchPacket == null)
- {
- byteNum = 0;
- length = (short)((0x5 << 12) ^ byteNum);
- batchPacket = BitConverter.GetBytes(length);
- }
- else
- {
- byteNum = (short)(batchPacket.Length / 2);
- length = (short)((0x5 << 12) ^ byteNum);
- batchPacket = BitConverter.GetBytes(length).Concat(batchPacket).ToArray();
- }
- }
- */
- if (data != null && batchPacket != null) data = batchPacket.Concat(data).ToArray();
-
- int packetlength = data == null ? 0 : data.Length;
-
- //数据长度+10
- byte[] packet = new byte[packetlength + 10];
-
- byte[] buffer_header = new byte[2];
- buffer_header = BitConverter.GetBytes(MavComm.COMM_SEND_PACKET_HEADER);
- Array.Copy(buffer_header, 0, packet, 0, 2); //发送数据头 2字节
-
- byte[] buffer_length;
- buffer_length = BitConverter.GetBytes((Int16)(packetlength + 6));
- Array.Copy(buffer_length, 0, packet, 2, 2); //数据长度 2字节
-
- byte[] buffer_serial;
- buffer_serial = BitConverter.GetBytes(serial_Number++);
- Array.Copy(buffer_serial, 0, packet, 4, 2); //流水号 2字节
-
- byte[] buffer_copterId = new byte[2];
- buffer_copterId = BitConverter.GetBytes((Int16)copterId);
- Array.Copy(buffer_copterId, 0, packet, 6, 2); //飞机号 2字节
-
- byte[] buffer_messageType = new byte[2];
- buffer_messageType = BitConverter.GetBytes((Int16)messageType);
- Array.Copy(buffer_messageType, 0, packet, 8, 2); //命令类型 2字节
-
- if (data != null)
- Array.Copy(data, 0, packet, 10, data.Length); //数据内容 10字节开始
-
- byte[] buffer_CRC = checkCRC16(packet); //计算所有数据的crc
-
- byte[] buffer_packet = new byte[packet.Length + 2]; //分配新的带crc的数据包
- Array.Copy(packet, buffer_packet, packet.Length);
- Array.Copy(buffer_CRC, 0, buffer_packet, packet.Length, 2); //带crc的数据包
-
- if (Connection != null && Connection.IsOnline)
- {
- //if (messageType==83)
- // Console.WriteLine("RTCM:" + (ushort)rtcm.length + "[" + rtcm.packet[0] + "," + rtcm.packet[1] + "," + rtcm.packet[2] + "," + rtcm.packet[3] + "," + rtcm.packet[4] + "]");
- // Console.WriteLine("[{0}]Send rtcm:{1}", buffer_serial[0], packetlength);
- try
- {
- await Connection.WriteAsync(buffer_packet, 0, buffer_packet.Length);
- }
- catch
- {
-
- }
- }
-
- }
-
- public async Task MissionPacket(short copterId, byte messageType, TMavCommPacket[] indata)
- {
- //数据长度
- int dataLength = 6 + 2 + indata.Length * 32;
- byte[] data = new byte[dataLength];
-
- byte[] uses = new byte[] { 0, 0, 1, 0, 0, 0 };
- //填充初始数据
- Array.Copy(uses, 0, data, 0, 6);
-
- Int16 countNum = (Int16)indata.Length;
- Array.Copy(BitConverter.GetBytes(countNum), 0, data, 6, 2);
-
-
- //新盒子的协议--暂时不用-需要问张伟
- /*
- Int16 countNum = (Int16)indata.Length;
-
- byte[] uses = new byte[] { 0, 1, 0, 0, 0, 0 };
-
- Array.Copy(uses, 0, data, 0, 6);
-
- Array.Copy(BitConverter.GetBytes(countNum), 0, data, 2, 2);
- Array.Copy(BitConverter.GetBytes(countNum), 0, data, 6, 2);
- */
-
-
-
-
- int offset = 8;
-
- foreach (TMavCommPacket MavPacket in indata)
- {
- byte[] MavPacketData = MavlinkUtil.StructureToByteArray(MavPacket);
- byte[] tempData = new byte[32];
-
- //张伟只需要32个有用数据 0-27， 30-31， 34-35
- Array.Copy(MavPacketData, 0, tempData, 0, 28);
- Array.Copy(MavPacketData, 30, tempData, 28, 2);
- Array.Copy(MavPacketData, 34, tempData, 30, 2);
-
- Array.Copy(tempData, 0, data, offset, 32);
- offset += 32;
- }
-
- await WriteCommPacketAsync(copterId, messageType, data);
-
- if (_writeMissionStopwatch == null) _writeMissionStopwatch = Stopwatch.StartNew();
- else _writeMissionStopwatch.Restart();
-
- //先延时5秒判断通信模块是否返回错误ID，0为正确。 如果已经错误了就不需要等待下发了。
- MissionStartCopterId = 0; MissionErrorId = -1;
- ErrorCode = 0;
-
- if (await AwaitCommMissionStartAsync(copterId, 5000))
- {
- _writeMissionStopwatch.Restart();
- MissionSendCopterId = 0;
- //等待通信模块地面站数据下发后，才能开始下个飞机的航点写入
- //下发不代表写入成功
- return await AwaitCommMissionRequestAsync(copterId, 10000).ConfigureAwait(false);
- }
- else
- return false;
- }
-
- public async Task AwaitCommMissionStartAsync(short copterId, int millisecondsTimeout)
- {
- while (MissionStartCopterId != copterId)
- {
- if (_writeMissionStopwatch.ElapsedMilliseconds >= millisecondsTimeout)
- {
- return false;
- }
- await Task.Delay(50).ConfigureAwait(false);
- }
- if (MissionErrorId == 0)
- return true;
- else
- {
- //Message.Show($"{copterId}:返回错误--{MissionErrorId}");
- return false;
- }
- }
-
- public async Task AwaitCommMissionRequestAsync(short copterId, int millisecondsTimeout)
- {
- while (MissionSendCopterId != copterId)
- {
- if (_writeMissionStopwatch.ElapsedMilliseconds >= millisecondsTimeout || ErrorCode != 0)
- {
- return false;
- }
- await Task.Delay(50).ConfigureAwait(false);
- }
- return true;
- }
-
- private long GetCreatetime()
- {
- DateTime DateStart = new DateTime(2000, 1, 1, 0, 0, 0);
- return Convert.ToInt64((DateTime.Now - DateStart).TotalMilliseconds);
- }
-
- public static byte[] CRC16(byte[] data)
- {
- int len = data.Length;
- if (len > 0)
- {
- ushort crc = 0xFFFF;
-
- for (int i = 0; i < len; i++)
- {
- crc = (ushort)(crc ^ (data[i]));
- for (int j = 0; j < 8; j++)
- {
- crc = (crc & 1) != 0 ? (ushort)((crc >> 1) ^ 0xA001) : (ushort)(crc >> 1);
- }
- }
- byte hi = (byte)((crc & 0xFF00) >> 8); //高位置
- byte lo = (byte)(crc & 0x00FF); //低位置
- return new byte[] { hi, lo };
- }
- return new byte[] { 0, 0 };
- }
-
- public byte[] CRCCalc(byte[] crcbuf)
- {
- //计算并填写CRC校验码
- int crc = 0xffff;
- int len = crcbuf.Length;
- for (int n = 0; n < len; n++)
- {
- byte i;
- crc = crc ^ crcbuf[n];
- for (i = 0; i < 8; i++)
- {
- int TT;
- TT = crc & 1;
- crc = crc >> 1;
- crc = crc & 0x7fff;
- if (TT == 1)
- {
- crc = crc ^ 0xa001;
- }
- crc = crc & 0xffff;
- }
-
- }
- byte[] redata = new byte[2];
- redata[1] = (byte)((crc >> 8) & 0xff);
- redata[0] = (byte)((crc & 0xff));
- return redata;
- }
-
- public byte[] checkCRC16(byte[] puchMsg)
- {
-
- int usDataLen = puchMsg.Length;
-
- ushort iTemp = 0x0;
- ushort flag = 0x0;
-
- for (int i = 0; i < usDataLen; i++)
- {
- iTemp ^= puchMsg[i];
- for (int j = 0; j < 8; j++)
- {
- flag = (ushort)(iTemp & 0x01);
- iTemp >>= 1;
- if (flag == 1)
- {
- iTemp ^= 0xa001;
- }
- }
- }
- byte[] ret = BitConverter.GetBytes(iTemp);
- return ret;
- }
-
- public void Dispose()
- {
- if (!CommOK)
- {
- return;
- }
- CommOK = false;
- CloseConnection();
- }
- }
-}
+using Plane.Communication;
+using Plane.Protocols;
+using System;
+using System.Collections.Generic;
+using System.Diagnostics;
+using System.Linq;
+using System.Net;
+using System.Net.NetworkInformation;
+using System.Net.Sockets;
+using System.Text;
+using System.Threading.Tasks;
+using System.Windows.Media;
+
+using System.Runtime.InteropServices;
+using Plane.Windows.Messages;
+
+namespace Plane.CommunicationManagement
+{
+ //通信模块
+ public partial class CommModuleManager: ExceptionThrownEventSource, IDisposable
+ {
+ [DllImport("kernel32")] //返回取得字符串缓冲区的长度
+ private static extern int GetPrivateProfileString(string section, string key, string def, StringBuilder retVal, int size, string filePath);
+
+ private static CommModuleManager _Instance = new CommModuleManager();
+ public static CommModuleManager Instance { get { return _Instance; } }
+
+ public TcpConnection Connection = null;
+ public bool CommOK = false;
+ private const string MODULE_IP = "192.168.199.51"; //"192.168.199.51"; // "192.168.199.51"; //在用版本 ----//"192.168.1.65"; //新版本
+ private string ServerIP = MODULE_IP;
+ // private const string LOCAL_IP = "192.168.199.50"; //"192.168.199.50";
+ private const int PORT = 9551;
+ private bool _disposed;
+ public int CommModuleCopterCount = 0;
+ public int CommModuleCurMode = 0;
+ private long packetCountNum = 0;
+
+
+ ///
+ /// 用于判断写入任务操作超时的秒表。
+ ///
+ private Stopwatch _writeMissionStopwatch;
+
+ private void LoadIPSet()
+ {
+ StringBuilder temp = new StringBuilder(255);
+
+ string path = Environment.CurrentDirectory + @"\Config.ini";
+
+ int i = GetPrivateProfileString("Default", "ServerIP", "", temp, 255, path);
+ ServerIP= temp.ToString();
+
+ if (ServerIP == "") ServerIP = MODULE_IP;
+ }
+
+
+ public async Task ConnectAsync()
+ {
+ LoadIPSet();
+ Connection = new TcpConnection(ServerIP, PORT);
+ await ConnectOpenAsync();
+ }
+
+ public void Connect()
+ {
+ LoadIPSet();
+ Task.Run(async () =>
+ {
+ Connection = new TcpConnection(ServerIP, PORT);
+ await ConnectOpenAsync();
+ }
+ );
+ }
+
+ public void CloseConnection()
+ {
+ CommOK = false;
+ Connection?.Close();
+ }
+
+
+
+
+
+
+
+
+/*
+
+
+ //获取内网IP
+ private IPAddress GetInternalIP()
+ {
+ NetworkInterface[] nics = NetworkInterface.GetAllNetworkInterfaces();
+
+ foreach (NetworkInterface adapter in nics)
+ {
+ foreach (var uni in adapter.GetIPProperties().UnicastAddresses)
+ {
+ if (uni.Address.AddressFamily == AddressFamily.InterNetwork)
+ {
+ return uni.Address;
+ }
+ }
+ }
+ return null;
+ }
+
+ ///
+ /// 获取本地连接IP地址、无线连接IP地址
+ ///
+ /// 0:本地连接，1:本地连接1，2:无线网络连接
+ ///
+ public static IPAddress GetIP(int k)
+ {
+ NetworkInterface[] interfaces = System.Net.NetworkInformation.NetworkInterface.GetAllNetworkInterfaces();
+ int len = interfaces.Length;
+ IPAddress[] mip = { IPAddress.Parse("0.0.0.0"), IPAddress.Parse("0.0.0.0"), IPAddress.Parse("0.0.0.0") };
+
+ for (int i = 0; i < len; i++)
+ {
+ NetworkInterface ni = interfaces[i];
+
+ if (ni.Name == "本地连接")
+ {
+ IPInterfaceProperties property = ni.GetIPProperties();
+ foreach (UnicastIPAddressInformation ip in property.UnicastAddresses)
+ {
+ if (ip.Address.AddressFamily == System.Net.Sockets.AddressFamily.InterNetwork)
+ {
+ mip[0] = ip.Address;
+ }
+ }
+ }
+ else if (ni.Name == "本地连接2")
+ {
+ IPInterfaceProperties property = ni.GetIPProperties();
+ foreach (UnicastIPAddressInformation ip in property.UnicastAddresses)
+ {
+ if (ip.Address.AddressFamily == System.Net.Sockets.AddressFamily.InterNetwork)
+ {
+ mip[1] = ip.Address;
+ }
+ }
+ }
+ else if (ni.Name == "无线网络连接")
+ {
+ IPInterfaceProperties property = ni.GetIPProperties();
+ foreach (UnicastIPAddressInformation ip in property.UnicastAddresses)
+ {
+ if (ip.Address.AddressFamily == System.Net.Sockets.AddressFamily.InterNetwork)
+ {
+ mip[2] = ip.Address;
+ }
+ }
+ }
+ }
+ return mip[k];
+ }
+
+ */
+ private async Task ConnectOpenAsync()
+ {
+ CommOK = true;
+ //不绑定IP也可以通讯，有线网络需要设置192.168.199.X网段地址
+ // string locip = GetIP(0).ToString();
+ // bool bind = await Connection.BindIP(locip);
+ // if (bind)
+ {
+ try
+ {
+ if (!Connection.IsOnline)
+ await Connection.OpenAsync().ConfigureAwait(false);
+ }
+ catch
+ { }
+ }
+
+ if (Connection.IsOnline)
+ {
+ Message.Connect(true);
+ Message.Show($"通讯基站连接成功!");
+ times = 0;
+ SendQuery();
+ await StartReadingPacketsAsync();
+ }
+ else
+ {
+ Message.Connect(false);
+ Reconnect();
+ }
+ }
+
+ //循环发送查询，保持通信正常
+ private void SendQuery()
+ {
+ Task.Run(async () =>
+ {
+ long lastPacketCountNum = 0;
+ int faulttimes = 0; //等待没有数据次数
+ const int revtimeout = 2000; //等待超时ms old=1200
+ while (CommOK)
+ {
+ if (Connection != null)
+ {
+ //发送心跳包-等待回复消息
+ await WriteCommPacketAsync(0, MavComm.COMM_QUERY_STATUS);
+ // await SendQueryStatusPacketsAsync();
+ await Task.Delay(revtimeout).ConfigureAwait(false);
+ //等待是否接受到返回的数据包，如果没有收到认为连结断开，重新联接
+ if (packetCountNum > lastPacketCountNum)
+ {
+ lastPacketCountNum = packetCountNum;
+ if (faulttimes>0)
+ Message.Show("收到新数据包，重置超时次数...");
+ faulttimes = 0;
+ }
+ else
+ {
+ faulttimes++;
+ Message.Show("设备回复超时("+ revtimeout + "ms)" + faulttimes+"次");
+ //2次没数据包返回就重连
+ if (faulttimes > 1)
+ {
+ Message.Show("长时间未收到设备回复数据包");
+ break;
+ }
+ }
+ }
+ }
+ Message.Show("主动断开连接，重连");
+ Reconnect();
+ }).ConfigureAwait(false);
+ }
+
+ /* //直接调用不用封装函数了
+ private async Task SendQueryStatusPacketsAsync()
+ {
+ await WriteCommPacketAsync(0, MavComm.COMM_QUERY_STATUS);
+ await Task.Delay(100);//100
+ }
+ */
+
+ int times = 0;
+ private void Reconnect()
+ {
+ // Message.Show($"正在重新连接...");
+ Task.Run(async () =>
+ {
+ CloseConnection();
+ await Task.Delay(250).ConfigureAwait(false);
+ Message.Show($"正在重连:次数{++times}");
+ await Task.Delay(250).ConfigureAwait(false);
+ await ConnectAsync();
+
+ }).ConfigureAwait(false);
+ }
+
+ private async Task StartReadingPacketsAsync()
+ {
+ await Task.Run(async () =>
+ {
+ while (CommOK)
+ {
+ if (Connection == null || !Connection.IsOnline)
+ {
+ if (Connection==null)
+ Message.Show("空连接异常");
+ else if (!Connection.IsOnline)
+ {
+ Message.Show("检测到设备已断开连接");
+ }
+ break;
+ }
+
+ var packet = await ReadPacketAsync().ConfigureAwait(false);
+ if (packet != null)
+ {
+ packetCountNum++;
+ short serialNum = BitConverter.ToInt16(packet, 4);
+ short copterNum = BitConverter.ToInt16(packet, 6);
+ short commandType = BitConverter.ToInt16(packet, 8);
+ byte[] dealData;
+ switch (commandType)
+ {
+ case MavComm.COMM_DOWNLOAD_MODE:
+ dealData = packet.Skip(10).ToArray();
+ AnalyzeMissionStartPacket(copterNum, dealData);
+ break;
+
+ case MavComm.COMM_GET_COPTERS_COUNT:
+ dealData = packet.Skip(16).ToArray();
+ AnalyzeCopterInfosPacket(dealData);
+ break;
+
+ case MavComm.COMM_NOTIFICATION:
+ short messageType = BitConverter.ToInt16(packet, 10);
+ dealData = packet.Skip(12).ToArray();
+ switch (messageType)
+ {
+ case (short)MavComm.MessageType.STR:
+ AnalyzeStringPacket(copterNum, dealData);
+ break;
+
+ case (short)MavComm.MessageType.SCAN2:
+ AnalyzeComm2RetrunPacket(copterNum, dealData);
+ break;
+
+ case (short)MavComm.MessageType.SCAN3:
+ AnalyzeComm3RetrunPacket(copterNum, dealData);
+ break;
+ case 4:
+ AnalyzeComm4RetrunPacket(copterNum, dealData);
+ break;
+ }
+ break;
+ case 0:
+ int packetLength = packet.Length;
+ short errorId = BitConverter.ToInt16(packet, packetLength - 6);
+ short copterCount = BitConverter.ToInt16(packet, packetLength - 4);
+ short curMode = BitConverter.ToInt16(packet, packetLength - 2);
+ string msg = string.Format("错误Id={0}, 检测飞机总数={1}，工作模式={2} 流水号={3}", errorId, copterCount, curMode, serialNum);
+ CommModuleCopterCount = copterCount;
+ CommModuleCurMode = curMode;
+ //Message.Show(msg);
+ break;
+ }
+ }
+ }
+ }).ConfigureAwait(false);
+ Message.Show("退出读设备数据........");
+ //Reconnect();
+ }
+
+ private async Task ReadPacketAsync()
+ {
+ int readnumber = 0;
+ if (Connection == null || !Connection.IsOnline)
+ {
+ return null;
+ }
+ try
+ {
+ byte[] bufferHaed = new byte[2];
+ readnumber = await Connection.ReadAsync(bufferHaed, 0, 2);
+ if (bufferHaed[0] == 0x13 && bufferHaed[1] == 0x77)
+ {
+ byte[] bufferLength = new byte[2];
+ readnumber = await Connection.ReadAsync(bufferLength, 0, 2);
+ short datalength = BitConverter.ToInt16(bufferLength, 0);
+ //Console.WriteLine("datalength = "+ datalength);
+
+ byte[] bufferData = new byte[datalength];
+ readnumber = await Connection.ReadAsync(bufferData, 0, datalength);
+
+// foreach (byte b in bufferData)
+// {
+// Console.Write(b.ToString("X2"));
+// Console.Write(" ");
+// }
+// Console.WriteLine("");
+
+ byte[] needCRCData = new byte[datalength + 4];
+ Array.Copy(bufferHaed, 0, needCRCData, 0, 2);
+ Array.Copy(bufferLength, 0, needCRCData, 2, 2);
+ Array.Copy(bufferData, 0, needCRCData, 4, datalength);
+
+ byte[] crc = checkCRC16(needCRCData);
+ byte[] bufferCRC16 = new byte[2];
+ readnumber = await Connection.ReadAsync(bufferCRC16, 0, 2);
+
+ if (crc[0] == bufferCRC16[0] && crc[1] == bufferCRC16[1])
+ {
+ return needCRCData;
+ }
+ }
+ }
+ catch
+ {
+ //Console.WriteLine("错误");
+ return null;
+ }
+ return null;
+ }
+
+
+
+ short serial_Number = 1;
+
+ public async Task GenerateDataAsync(short copterId, short messageType, TMavCommPacket indata)
+ {
+ byte[] data;
+ data = MavlinkUtil.StructureToByteArray(indata);
+ await WriteCommPacketAsync(copterId, messageType, data);
+ }
+
+ public async Task UpdateCommModule()
+ {
+ MavComm.comm_update_copter_module commUpdate = new MavComm.comm_update_copter_module();
+ commUpdate.mav_count = (short)CommModuleCopterCount;
+ commUpdate.update_code = 0x7713;
+ await GenerateDataAsync(0, MavComm.COMM_UPDATE_COPTER_MODULE, commUpdate);
+ }
+
+
+ public async Task TestModule(short ModuleNo, short TestLong=100)
+ {
+
+
+ byte[] packet = new byte[2];
+ packet[0] = (byte)(ModuleNo);
+ packet[1] = (byte)(TestLong);
+ byte[] senddata = packet;
+
+ // Message.Show("长度 = " + senddata.Length);
+ await WriteCommPacketAsync(0, MavComm.COMM_TEST_MODULE, senddata);
+ // await Task.Delay(1000).ConfigureAwait(false);
+
+ }
+
+
+
+ public async Task SetModulePower(short ModuleNo, short ModulePower)
+ {
+ byte[] packet = { 0x01, 0x00, 0x03, 0x00, 0x01, 0x0A };
+ packet[2] = (byte)(ModuleNo & 0xFF);
+ packet[3] = (byte)((ModuleNo & 0xFF00) >> 8);
+ packet[5] = (byte)(ModulePower);
+ await WriteCommPacketAsync(0, MavComm.COMM_TO_MODULE, packet);
+ }
+
+
+
+
+
+ bool temp = false;
+ //测试用 灯光间隔1S闪烁
+ public async Task TestLED(short id, string colorString)
+ {
+
+ MAVLink.mavlink_msg_id_led_control led = new MAVLink.mavlink_msg_id_led_control();
+ led.target_system = 1;
+ led.target_component = 1;//(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];
+
+ if (colorString == "") colorString = "330033";
+
+ Color color = (Color)ColorConverter.ConvertFromString("#" + colorString);
+
+ led.custom_bytes[0] = color.R;
+ led.custom_bytes[1] = color.G;
+ led.custom_bytes[2] = color.B;
+ led.custom_bytes[3] = 3;
+ byte[] data;
+ data = MavlinkUtil.StructureToByteArray(led);
+
+ byte[] packet = new byte[data.Length + 6 + 2];
+ packet[0] = MAVLink.MAVLINK_STX;
+
+ packet[1] = (byte)(data.Length);
+ packet[2] = 1;
+ packet[3] = 255; // this is always 255 - MYGCS
+
+ packet[4] = (byte)MAVLink.MAV_COMPONENT.MAV_COMP_ID_MISSIONPLANNER;
+
+ packet[5] = MAVLink.MAVLINK_MSG_ID_LED_CONTROL;
+ int i = 6;
+ foreach (byte b in data)
+ {
+ packet[i] = b;
+ i++;
+ }
+
+ ushort checksum = MavlinkCRC.crc_calculate(packet, packet[1] + 6);
+ checksum = MavlinkCRC.crc_accumulate(MAVLink.MAVLINK_MESSAGE_CRCS[MAVLink.MAVLINK_MSG_ID_LED_CONTROL], 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;
+
+
+ //await WriteCommPacketAsync(id, MavComm.COMM_DOWNLOAD_COMM, packet);
+ byte[] senddata = packet;
+
+// for (int times = 0; times < 14; times++)
+// {
+// senddata = senddata.Concat(packet).ToArray();
+// }
+
+// byte[] myByteArray = Enumerable.Repeat((byte)0x08, 15).ToArray();
+//
+// senddata = senddata.Concat(myByteArray).ToArray();
+
+ temp = !temp;
+ while (temp)
+ {
+ Message.Show("测试灯光，长度 = " + senddata.Length);
+ await WriteCommPacketAsync(id, MavComm.COMM_DOWNLOAD_COMM, senddata);
+ await Task.Delay(1000).ConfigureAwait(false);
+ }
+ }
+
+ //拉烟测试
+ public async Task TestFire(short id, int channel)
+ {
+
+ MAVLink.mavlink_msg_id_led_control led = new MAVLink.mavlink_msg_id_led_control();
+ led.target_system = 1;
+ led.target_component = 1;//(byte)MAVLink.MAV_COMPONENT.MAV_COMP_ID_SYSTEM_CONTROL;;
+ led.instance = 0;
+ led.pattern = 0;
+ led.custom_len = 1;//命令类型——测试拉烟
+ led.custom_bytes = new byte[24];
+
+ led.custom_bytes[0] = (byte)channel;
+
+ byte[] data;
+ data = MavlinkUtil.StructureToByteArray(led);
+
+ byte[] packet = new byte[data.Length + 6 + 2];
+ packet[0] = MAVLink.MAVLINK_STX;
+
+ packet[1] = (byte)(data.Length);
+ packet[2] = 1;
+ packet[3] = 255; // this is always 255 - MYGCS
+
+ packet[4] = (byte)MAVLink.MAV_COMPONENT.MAV_COMP_ID_MISSIONPLANNER;
+
+ packet[5] = MAVLink.MAVLINK_MSG_ID_LED_CONTROL;
+ int i = 6;
+ foreach (byte b in data)
+ {
+ packet[i] = b;
+ i++;
+ }
+
+ ushort checksum = MavlinkCRC.crc_calculate(packet, packet[1] + 6);
+ checksum = MavlinkCRC.crc_accumulate(MAVLink.MAVLINK_MESSAGE_CRCS[MAVLink.MAVLINK_MSG_ID_LED_CONTROL], 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;
+
+
+ //await WriteCommPacketAsync(id, MavComm.COMM_DOWNLOAD_COMM, packet);
+ byte[] senddata = packet;
+
+ for (int times = 0; times < 3; times++)
+ {
+ senddata = senddata.Concat(packet).ToArray();
+ }
+
+
+ temp = !temp;
+ while (temp)
+ {
+ await WriteCommPacketAsync(id, MavComm.COMM_DOWNLOAD_COMM, packet);
+ await Task.Delay(1000).ConfigureAwait(false);
+ }
+ }
+
+
+ public async Task TestBattery(short id, float minivol)
+ {
+
+ MAVLink.mavlink_msg_id_led_control led = new MAVLink.mavlink_msg_id_led_control();
+ led.target_system = 1;
+ led.target_component = 1;//(byte)MAVLink.MAV_COMPONENT.MAV_COMP_ID_SYSTEM_CONTROL;;
+ led.instance = 0;
+ led.pattern = 0;
+ led.custom_len = 5;//命令类型——测试电池电压
+ led.custom_bytes = new byte[24];
+
+ led.custom_bytes[0] = (byte)(int)(minivol); //整数部分
+ led.custom_bytes[1] = (byte)(int)((minivol-(int)minivol)*100); //小数部分 --2位
+
+
+
+
+
+ byte[] data;
+ data = MavlinkUtil.StructureToByteArray(led);
+
+ byte[] packet = new byte[data.Length + 6 + 2];
+ packet[0] = MAVLink.MAVLINK_STX;
+
+ packet[1] = (byte)(data.Length);
+ packet[2] = 1;
+ packet[3] = 255; // this is always 255 - MYGCS
+
+ packet[4] = (byte)MAVLink.MAV_COMPONENT.MAV_COMP_ID_MISSIONPLANNER;
+
+ packet[5] = MAVLink.MAVLINK_MSG_ID_LED_CONTROL;
+ int i = 6;
+ foreach (byte b in data)
+ {
+ packet[i] = b;
+ i++;
+ }
+
+ ushort checksum = MavlinkCRC.crc_calculate(packet, packet[1] + 6);
+ checksum = MavlinkCRC.crc_accumulate(MAVLink.MAVLINK_MESSAGE_CRCS[MAVLink.MAVLINK_MSG_ID_LED_CONTROL], 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;
+
+
+ //await WriteCommPacketAsync(id, MavComm.COMM_DOWNLOAD_COMM, packet);
+ byte[] senddata = packet;
+
+ for (int times = 0; times < 3; times++)
+ {
+ senddata = senddata.Concat(packet).ToArray();
+ }
+
+
+ { //发3次
+ await WriteCommPacketAsync(id, MavComm.COMM_DOWNLOAD_COMM, packet);
+ await Task.Delay(1000).ConfigureAwait(false);
+ await WriteCommPacketAsync(id, MavComm.COMM_DOWNLOAD_COMM, packet);
+ await Task.Delay(1000).ConfigureAwait(false);
+ await WriteCommPacketAsync(id, MavComm.COMM_DOWNLOAD_COMM, packet);
+
+ }
+ }
+
+
+ ///
+ /// 生成通信模块packet并且发送
+ ///
+ /// 飞机ID
+ /// 命令类型
+ /// 数据类型：空表示只切换模式
+ /// 小批量数据包
+ ///
+ public async Task WriteCommPacketAsync(short copterId, short messageType, byte[] data = null, byte[] batchPacket = null)
+ {
+ //新盒子的协议--暂时不用-需要问张伟
+ /*
+ if (messageType == MavComm.COMM_DOWNLOAD_COMM && copterId == 0)
+ {
+ short byteNum;
+ short length;
+ if (batchPacket == null)
+ {
+ byteNum = 0;
+ length = (short)((0x5 << 12) ^ byteNum);
+ batchPacket = BitConverter.GetBytes(length);
+ }
+ else
+ {
+ byteNum = (short)(batchPacket.Length / 2);
+ length = (short)((0x5 << 12) ^ byteNum);
+ batchPacket = BitConverter.GetBytes(length).Concat(batchPacket).ToArray();
+ }
+ }
+ */
+ if (data != null && batchPacket != null) data = batchPacket.Concat(data).ToArray();
+
+ int packetlength = data == null ? 0 : data.Length;
+
+ //数据长度+10
+ byte[] packet = new byte[packetlength + 10];
+
+ byte[] buffer_header = new byte[2];
+ buffer_header = BitConverter.GetBytes(MavComm.COMM_SEND_PACKET_HEADER);
+ Array.Copy(buffer_header, 0, packet, 0, 2); //发送数据头 2字节
+
+ byte[] buffer_length;
+ buffer_length = BitConverter.GetBytes((Int16)(packetlength + 6));
+ Array.Copy(buffer_length, 0, packet, 2, 2); //数据长度 2字节
+
+ byte[] buffer_serial;
+ buffer_serial = BitConverter.GetBytes(serial_Number++);
+ Array.Copy(buffer_serial, 0, packet, 4, 2); //流水号 2字节
+
+ byte[] buffer_copterId = new byte[2];
+ buffer_copterId = BitConverter.GetBytes((Int16)copterId);
+ Array.Copy(buffer_copterId, 0, packet, 6, 2); //飞机号 2字节
+
+ byte[] buffer_messageType = new byte[2];
+ buffer_messageType = BitConverter.GetBytes((Int16)messageType);
+ Array.Copy(buffer_messageType, 0, packet, 8, 2); //命令类型 2字节
+
+ if (data != null)
+ Array.Copy(data, 0, packet, 10, data.Length); //数据内容 10字节开始
+
+ byte[] buffer_CRC = checkCRC16(packet); //计算所有数据的crc
+
+ byte[] buffer_packet = new byte[packet.Length + 2]; //分配新的带crc的数据包
+ Array.Copy(packet, buffer_packet, packet.Length);
+ Array.Copy(buffer_CRC, 0, buffer_packet, packet.Length, 2); //带crc的数据包
+
+ if (Connection != null && Connection.IsOnline)
+ {
+ //if (messageType==83)
+ // Console.WriteLine("RTCM:" + (ushort)rtcm.length + "[" + rtcm.packet[0] + "," + rtcm.packet[1] + "," + rtcm.packet[2] + "," + rtcm.packet[3] + "," + rtcm.packet[4] + "]");
+ // Console.WriteLine("[{0}]Send rtcm:{1}", buffer_serial[0], packetlength);
+ try
+ {
+ await Connection.WriteAsync(buffer_packet, 0, buffer_packet.Length);
+ }
+ catch
+ {
+
+ }
+ }
+
+ }
+
+ public async Task MissionPacket(short copterId, byte messageType, TMavCommPacket[] indata)
+ {
+ //数据长度
+ int dataLength = 6 + 2 + indata.Length * 32;
+ byte[] data = new byte[dataLength];
+
+ byte[] uses = new byte[] { 0, 0, 1, 0, 0, 0 };
+ //填充初始数据
+ Array.Copy(uses, 0, data, 0, 6);
+
+ Int16 countNum = (Int16)indata.Length;
+ Array.Copy(BitConverter.GetBytes(countNum), 0, data, 6, 2);
+
+
+ //新盒子的协议--暂时不用-需要问张伟
+ /*
+ Int16 countNum = (Int16)indata.Length;
+
+ byte[] uses = new byte[] { 0, 1, 0, 0, 0, 0 };
+
+ Array.Copy(uses, 0, data, 0, 6);
+
+ Array.Copy(BitConverter.GetBytes(countNum), 0, data, 2, 2);
+ Array.Copy(BitConverter.GetBytes(countNum), 0, data, 6, 2);
+ */
+
+
+
+
+ int offset = 8;
+
+ foreach (TMavCommPacket MavPacket in indata)
+ {
+ byte[] MavPacketData = MavlinkUtil.StructureToByteArray(MavPacket);
+ byte[] tempData = new byte[32];
+
+ //张伟只需要32个有用数据 0-27， 30-31， 34-35
+ Array.Copy(MavPacketData, 0, tempData, 0, 28);
+ Array.Copy(MavPacketData, 30, tempData, 28, 2);
+ Array.Copy(MavPacketData, 34, tempData, 30, 2);
+
+ Array.Copy(tempData, 0, data, offset, 32);
+ offset += 32;
+ }
+
+ await WriteCommPacketAsync(copterId, messageType, data);
+
+ if (_writeMissionStopwatch == null) _writeMissionStopwatch = Stopwatch.StartNew();
+ else _writeMissionStopwatch.Restart();
+
+ //先延时5秒判断通信模块是否返回错误ID，0为正确。 如果已经错误了就不需要等待下发了。
+ MissionStartCopterId = 0; MissionErrorId = -1;
+ ErrorCode = 0;
+
+ if (await AwaitCommMissionStartAsync(copterId, 5000))
+ {
+ _writeMissionStopwatch.Restart();
+ MissionSendCopterId = 0;
+ //等待通信模块地面站数据下发后，才能开始下个飞机的航点写入
+ //下发不代表写入成功
+ return await AwaitCommMissionRequestAsync(copterId, 10000).ConfigureAwait(false);
+ }
+ else
+ return false;
+ }
+
+ public async Task AwaitCommMissionStartAsync(short copterId, int millisecondsTimeout)
+ {
+ while (MissionStartCopterId != copterId)
+ {
+ if (_writeMissionStopwatch.ElapsedMilliseconds >= millisecondsTimeout)
+ {
+ return false;
+ }
+ await Task.Delay(50).ConfigureAwait(false);
+ }
+ if (MissionErrorId == 0)
+ return true;
+ else
+ {
+ //Message.Show($"{copterId}:返回错误--{MissionErrorId}");
+ return false;
+ }
+ }
+
+ public async Task AwaitCommMissionRequestAsync(short copterId, int millisecondsTimeout)
+ {
+ while (MissionSendCopterId != copterId)
+ {
+ if (_writeMissionStopwatch.ElapsedMilliseconds >= millisecondsTimeout || ErrorCode != 0)
+ {
+ return false;
+ }
+ await Task.Delay(50).ConfigureAwait(false);
+ }
+ return true;
+ }
+
+ private long GetCreatetime()
+ {
+ DateTime DateStart = new DateTime(2000, 1, 1, 0, 0, 0);
+ return Convert.ToInt64((DateTime.Now - DateStart).TotalMilliseconds);
+ }
+
+ public static byte[] CRC16(byte[] data)
+ {
+ int len = data.Length;
+ if (len > 0)
+ {
+ ushort crc = 0xFFFF;
+
+ for (int i = 0; i < len; i++)
+ {
+ crc = (ushort)(crc ^ (data[i]));
+ for (int j = 0; j < 8; j++)
+ {
+ crc = (crc & 1) != 0 ? (ushort)((crc >> 1) ^ 0xA001) : (ushort)(crc >> 1);
+ }
+ }
+ byte hi = (byte)((crc & 0xFF00) >> 8); //高位置
+ byte lo = (byte)(crc & 0x00FF); //低位置
+ return new byte[] { hi, lo };
+ }
+ return new byte[] { 0, 0 };
+ }
+
+ public byte[] CRCCalc(byte[] crcbuf)
+ {
+ //计算并填写CRC校验码
+ int crc = 0xffff;
+ int len = crcbuf.Length;
+ for (int n = 0; n < len; n++)
+ {
+ byte i;
+ crc = crc ^ crcbuf[n];
+ for (i = 0; i < 8; i++)
+ {
+ int TT;
+ TT = crc & 1;
+ crc = crc >> 1;
+ crc = crc & 0x7fff;
+ if (TT == 1)
+ {
+ crc = crc ^ 0xa001;
+ }
+ crc = crc & 0xffff;
+ }
+
+ }
+ byte[] redata = new byte[2];
+ redata[1] = (byte)((crc >> 8) & 0xff);
+ redata[0] = (byte)((crc & 0xff));
+ return redata;
+ }
+
+ public byte[] checkCRC16(byte[] puchMsg)
+ {
+
+ int usDataLen = puchMsg.Length;
+
+ ushort iTemp = 0x0;
+ ushort flag = 0x0;
+
+ for (int i = 0; i < usDataLen; i++)
+ {
+ iTemp ^= puchMsg[i];
+ for (int j = 0; j < 8; j++)
+ {
+ flag = (ushort)(iTemp & 0x01);
+ iTemp >>= 1;
+ if (flag == 1)
+ {
+ iTemp ^= 0xa001;
+ }
+ }
+ }
+ byte[] ret = BitConverter.GetBytes(iTemp);
+ return ret;
+ }
+
+ public void Dispose()
+ {
+ if (!CommOK)
+ {
+ return;
+ }
+ CommOK = false;
+ CloseConnection();
+ }
+ }
+}
diff --git a/PlaneGcsSdk_Shared/CopterControllers/GuidController.cs b/PlaneGcsSdk_Shared/CopterControllers/GuidController.cs
index bc7de4f..a3e504d 100644
--- a/PlaneGcsSdk_Shared/CopterControllers/GuidController.cs
+++ b/PlaneGcsSdk_Shared/CopterControllers/GuidController.cs
@@ -1,347 +1,355 @@
-using System;
-using System.Collections.Generic;
-using System.Text;
-
-namespace Plane.CopterControllers
-{
- class GuidController
- {
-
- static float fabsf(float vvalue)
- {
- return Math.Abs(vvalue);
-
- }
- static float sqrt(float vvalue)
- {
- return (float)Math.Sqrt(vvalue);
-
- }
-
-
- ///
- /// 计算小航点飞行
- ///
- ///
- ///
- ///
- ///
-
- //单算减速，不算加速的最小飞行时间
- static float getMinfligthtime_v2(float Distance, float fc_acc, float fc_maxspeed)
- {
- float fDis = fabsf(Distance); // 总距离
- float facc = fabsf(fc_acc); // 减速度
-
- // 物体开始时即以最大速度运动，不考虑加速过程
- float vel = fc_maxspeed;
-
- // 计算减速所需的时间和距离
- // 减速时间 (从最大速度减速到0)
- float dectime = vel / facc;
- // 减速阶段覆盖的距离
- float decdis = (vel * dectime) - (0.5f * facc * dectime * dectime);
-
- // 判断是否有足够的距离进行减速
- if (decdis >= fDis)
- {
- // 如果减速所需距离已经超过或等于总距离，调整最大速度
- // 使用公式 v^2 = u^2 + 2as 解出 v
- vel = (float)Math.Sqrt(2 * facc * fDis);
- // 重新计算减速时间
- dectime = vel / facc;
- }
-
- // 计算匀速阶段时间
- float unftime = 0.0f;
- if (decdis < fDis)
- {
- // 如果有剩余距离进行匀速运动
- unftime = (fDis - decdis) / vel;
- }
-
- // 总飞行时间 = 匀速阶段时间 + 减速阶段时间
- return unftime + dectime;
- }
-
- //单算减速，不算加速的最大飞行速度
- public static float CalculateMaximumVelocity(float distance, float time, float acceleration)
- {
- // 二次方程系数
- float a_coeff = 1;
- float b_coeff = -2 * time;
- float c_coeff = (2 * distance) / acceleration;
-
- // 计算判别式
- float discriminant = b_coeff * b_coeff - 4 * a_coeff * c_coeff;
-
- if (discriminant < 0)
- {
- return -1; // 无实数解
- }
-
- // 计算二次方程的根
- float t1_root1 = (-b_coeff + (float)Math.Sqrt(discriminant)) / (2 * a_coeff);
- float t1_root2 = (-b_coeff - (float)Math.Sqrt(discriminant)) / (2 * a_coeff);
-
- // 选择合理的根作为 t1
- float t1 = Math.Min(t1_root1, t1_root2);
- if (t1 < 0 || t1 > time)
- {
- return -1; // 无合理解
- }
-
- // 计算最大速度 v
- return acceleration * t1;
- }
-
- //单算加速，不算减速的距离速度计算
- static float getspeeddist_V2(float flight_time, float Distance, float curr_time, float fc_acc, float fc_maxspeed, out float vdist, out float vspeed)
- {
- vdist = 0;
- vspeed = 0;
- float desV = CalculateMaximumVelocity(Distance, flight_time, fc_acc);
- if ((desV == -1) || (desV > fc_maxspeed)) //飞不到
- {
- vspeed = Distance / flight_time;
- vdist = vspeed * curr_time; //匀速距离
- return 0;
-
- }
- float dect = desV / fc_acc; // 总加速时间
- float unit = flight_time - dect; //匀速段时间
- float decD = (fc_acc * dect * dect) / 2; //总加速距离
-
- if (dect > flight_time) dect = flight_time;//约束时间
- if (curr_time > dect) //大于加速段时间--匀速
- {
- vspeed = (Distance - decD) / (flight_time - dect);
- vdist = vspeed * (curr_time - dect);
- vdist = vdist + decD; //总距离=匀速距离+减速距离
- if (vdist > Distance) vdist = Distance; //约束距离
-
- }
- else //加速段
- {
- vspeed = fc_acc * curr_time;
- vdist = (fc_acc * curr_time * curr_time) / 2; //加速距离
- }
- return 0;
- }
-
- //单算减速，不算加速的距离速度计算
- static float getspeeddist_V3(float flight_time, float Distance, float curr_time, float fc_acc, float fc_maxspeed, out float vdist, out float vspeed)
- {
- vdist = 0;
- vspeed = 0;
- float desV = CalculateMaximumVelocity(Distance, flight_time, fc_acc);
- if ((desV == -1) || (desV > fc_maxspeed)) //飞不到
- {
- vspeed = Distance / flight_time;
- vdist = vspeed * curr_time; //匀速距离
- return 0;
-
- }
- float dect = desV / fc_acc; // 总减速时间
- float unit = flight_time - dect; //匀速段时间
- float decD = desV * dect - (fc_acc * dect * dect) / 2; //总减速距离
-
- if (dect > flight_time) dect = flight_time;//约束时间
- //匀速时间内
- if (curr_time < unit)
- {
- vspeed = (Distance - decD) / (flight_time - dect);
- vdist = vspeed * curr_time;
- }
- else
- {
- if (((flight_time - dect) * unit) == 0)
- vdist = 0;
- else
- vdist = (Distance - decD) / (flight_time - dect) * unit; //匀速距离
- float currdect = curr_time - unit; //减速时间
- if (currdect >= 0)
- {
- vspeed = desV - fc_acc * currdect;
- decD = desV * currdect - (fc_acc * currdect * currdect) / 2; //减速距离
- }
- vdist = vdist + decD; //总距离=匀速距离+减速距离
- if (vdist > Distance) vdist = Distance; //约束距离
- }
- return 0;
- }
-
-
-
- static float getMinfligthtime(float Distance, float fc_acc, float fc_maxspeed)
- {
- float fDis = fabsf(Distance);
- float facc = fabsf(fc_acc);
-
- float realflytime = 0.0f;
- //计算一半的距离
- float hafdis = (fDis / 2);
- //计算最大速度
- float vel = (float)sqrt(2 * facc * hafdis);
- //如果速度超过最大速度
- if (vel > fc_maxspeed)
- //使用最大速度
- vel = fc_maxspeed;
- //加速时间
- float acctim = vel / facc;
- //加速距离
- float accdis = vel * vel / (2 * facc);
- //匀速段时间
- float vtime = (hafdis - accdis) / vel;
- //到一半的时间
- float haftime = acctim + vtime;
- realflytime = haftime * 2;
- return realflytime;
- }
-
- ///计算飞行距离和速度 单位--米，秒----
- //返回 整个距离最大飞行速度
- ///flight_time 总飞行时间 Distance 飞行总距离 curr_time 当前飞行时间 fc_acc加速度, fc_maxspeed最大速度 vdist 计算的应该飞的距离 vspeed 计算的当前时间应该速度
- //========这是飞控正在使用的算法========
- static float getspeeddist_V1(float flight_time, float Distance, float curr_time, float fc_acc, float fc_maxspeed, out float vdist, out float vspeed)
- {
- //计划飞行时间
- float plan_flytime = flight_time;
- //计算距离用绝对值
- float fDis = fabsf(Distance);
- if (curr_time < 0)
- {
- vdist = 0;
- vspeed = 0;
- return 0.0f;
- }
- //导航加速度 米/秒*秒 ---不使用导航库
- float wpacc = fabsf(fc_acc);// g.fc_acc_xy;//1.5;
-
- //最大目标速度---米/秒
- float desired_velocity = 0;
- float dist = 0;
- //
- float speed = 0;
-
- //计算最小飞行时间
- float minflytime = getMinfligthtime(Distance, fc_acc, fc_maxspeed);
- if (flight_time < minflytime)
- plan_flytime = minflytime;// + 0.1f;
-
-
- //根据总飞行时间和总距离计算飞行速度----起始和结束速度都是0，中间按匀加速和匀减速计算，没考虑加加速度
- float delta = (0.5f * plan_flytime) * (0.5f * plan_flytime) - fDis / wpacc;
- if (delta >= 0)
- {
- desired_velocity = (0.5f * plan_flytime - sqrt(delta)) / (1 / wpacc);
- if (desired_velocity > fc_maxspeed)
- {
- plan_flytime = minflytime;
- desired_velocity = fc_maxspeed;
- }
- }
- else
- {
- desired_velocity = (0.5f * plan_flytime) / (1 / wpacc);
- }
- if (desired_velocity < 0.2f) desired_velocity = 0.2f; //限制最小速度0.2米/秒
- if (desired_velocity > fc_maxspeed) desired_velocity = fc_maxspeed;//限制最大速度10米/秒，应该参数化
-
- //计算实时速度
- if (curr_time <= (plan_flytime / 2.0))
- speed = curr_time * wpacc;
- else
- //需要减速
- speed = (plan_flytime - curr_time) * wpacc;
- //不能大于目标速度
- if (speed > desired_velocity)
- speed = desired_velocity;
- if (speed <= 0)
- speed = 0;
- vspeed = speed;
- //计算实时距离
- float V_start = 0.0f;
- float V_end = 0.0f;
- //加速段
- float t1 = (desired_velocity - V_start) / wpacc;
- //减速段
- float t3 = (desired_velocity - V_end) / wpacc;
- //平飞段
- float t2 = plan_flytime - t1 - t3;
- if (curr_time < t1)
- {
- dist = 0.5f * wpacc * curr_time * curr_time + curr_time * V_start;
- }
- else if (curr_time < t1 + t2)
- {
- dist = -0.5f * wpacc * t1 * t1 + (curr_time) * desired_velocity;
- }
- else
- {
- float t = curr_time - t1 - t2;
- dist = -0.5f * wpacc * t1 * t1 + curr_time * desired_velocity - 0.5f * wpacc * t * t;
- }
-
- if (fabsf(dist) > fabsf(Distance))
- vdist = Distance;
- else
- {
- if (Distance < 0)
- vdist = -dist;
- else vdist = dist;
- }
-
- return desired_velocity;
- }
-
- public static float getspeeddist(int flytype, float flight_time, float Distance, float curr_time, float fc_acc, float fc_maxspeed, out float vdist, out float vspeed)
- {
- switch (flytype)
- {
- case 0: //匀速
- //case 1: //匀速
- vspeed = Distance / flight_time;
- vdist = vspeed * curr_time;
- return 0;
- case 1: //同时计算加减速
- return getspeeddist_V1(flight_time, Distance, curr_time, fc_acc, fc_maxspeed, out vdist, out vspeed);
- case 2: //单加速
- return getspeeddist_V2(flight_time, Distance, curr_time, fc_acc, fc_maxspeed, out vdist, out vspeed);
- case 3: //单减速
- return getspeeddist_V3(flight_time, Distance, curr_time, fc_acc, fc_maxspeed, out vdist, out vspeed);
- default:
- vspeed = 0;
- vdist = 0;
- return 0;
- }
- }
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
- }
- }
+using System;
+using System.Collections.Generic;
+using System.Text;
+
+namespace Plane.CopterControllers
+{
+ class GuidController
+ {
+
+ float fabsf(float vvalue)
+ {
+ return Math.Abs(vvalue);
+
+ }
+ float sqrt(float vvalue)
+ {
+ return (float)Math.Sqrt(vvalue);
+
+ }
+
+
+ ///
+ /// 计算小航点飞行
+ ///
+ ///
+ ///
+ ///
+ ///
+
+ //单算减速，不算加速的最小飞行时间
+ float getMinfligthtime_v2(float Distance, float fc_acc, float fc_maxspeed)
+ {
+ float fDis = fabsf(Distance); // 总距离
+ float facc = fabsf(fc_acc); // 减速度
+
+ // 物体开始时即以最大速度运动，不考虑加速过程
+ float vel = fc_maxspeed;
+
+ // 计算减速所需的时间和距离
+ // 减速时间 (从最大速度减速到0)
+ float dectime = vel / facc;
+ // 减速阶段覆盖的距离
+ float decdis = (vel * dectime) - (0.5f * facc * dectime * dectime);
+
+ // 判断是否有足够的距离进行减速
+ if (decdis >= fDis)
+ {
+ // 如果减速所需距离已经超过或等于总距离，调整最大速度
+ // 使用公式 v^2 = u^2 + 2as 解出 v
+ vel = (float)Math.Sqrt(2 * facc * fDis);
+ // 重新计算减速时间
+ dectime = vel / facc;
+ }
+
+ // 计算匀速阶段时间
+ float unftime = 0.0f;
+ if (decdis < fDis)
+ {
+ // 如果有剩余距离进行匀速运动
+ unftime = (fDis - decdis) / vel;
+ }
+
+ // 总飞行时间 = 匀速阶段时间 + 减速阶段时间
+ return unftime + dectime;
+ }
+
+ //单算减速，不算加速的最大飞行速度
+ public float CalculateMaximumVelocity(float distance, float time, float acceleration)
+ {
+ // 二次方程系数
+ float a_coeff = 1;
+ float b_coeff = -2 * time;
+ float c_coeff = (2 * distance) / acceleration;
+
+ // 计算判别式
+ float discriminant = b_coeff * b_coeff - 4 * a_coeff * c_coeff;
+
+ if (discriminant < 0)
+ {
+ return -1; // 无实数解
+ }
+
+ // 计算二次方程的根
+ float t1_root1 = (-b_coeff + (float)Math.Sqrt(discriminant)) / (2 * a_coeff);
+ float t1_root2 = (-b_coeff - (float)Math.Sqrt(discriminant)) / (2 * a_coeff);
+
+ // 选择合理的根作为 t1
+ float t1 = Math.Min(t1_root1, t1_root2);
+ if (t1 < 0 || t1 > time)
+ {
+ return -1; // 无合理解
+ }
+
+ // 计算最大速度 v
+ return acceleration * t1;
+ }
+
+ //单算加速，不算减速的距离速度计算
+ float getspeeddist_V2(float flight_time, float Distance, float curr_time, float fc_acc, float fc_maxspeed, out float vdist, out float vspeed)
+ {
+ vdist = 0;
+ vspeed = 0;
+ float desV = CalculateMaximumVelocity(Distance, flight_time, fc_acc);
+ if ((desV == -1) || (desV > fc_maxspeed)) //飞不到
+ {
+ vspeed = Distance / flight_time;
+ vdist = vspeed * curr_time; //匀速距离
+ return 0;
+
+ }
+ float dect = desV / fc_acc; // 总加速时间
+ float unit = flight_time - dect; //匀速段时间
+ float decD = (fc_acc * dect * dect) / 2; //总加速距离
+
+ if (dect > flight_time) dect = flight_time;//约束时间
+ if (curr_time > dect) //大于加速段时间--匀速
+ {
+ vspeed = (Distance - decD) / (flight_time - dect);
+ vdist = vspeed * (curr_time - dect);
+ vdist = vdist + decD; //总距离=匀速距离+减速距离
+ if (vdist > Distance) vdist = Distance; //约束距离
+
+ }
+ else //加速段
+ {
+ vspeed = fc_acc * curr_time;
+ vdist = (fc_acc * curr_time * curr_time) / 2; //加速距离
+ }
+ return 0;
+ }
+
+ //单算减速，不算加速的距离速度计算
+ float getspeeddist_V3(float flight_time, float Distance, float curr_time, float fc_acc, float fc_maxspeed, out float vdist, out float vspeed)
+ {
+ vdist = 0;
+ vspeed = 0;
+ float desV = CalculateMaximumVelocity(Distance, flight_time, fc_acc);
+ if ((desV == -1) || (desV > fc_maxspeed)) //飞不到
+ {
+ vspeed = Distance / flight_time;
+ vdist = vspeed * curr_time; //匀速距离
+ return 0;
+
+ }
+ float dect = desV / fc_acc; // 总减速时间
+ float unit = flight_time - dect; //匀速段时间
+ float decD = desV * dect - (fc_acc * dect * dect) / 2; //总减速距离
+
+ if (dect > flight_time) dect = flight_time;//约束时间
+ //匀速时间内
+ if (curr_time < unit)
+ {
+ vspeed = (Distance - decD) / (flight_time - dect);
+ vdist = vspeed * curr_time;
+ }
+ else
+ {
+ if (((flight_time - dect) * unit) == 0)
+ vdist = 0;
+ else
+ vdist = (Distance - decD) / (flight_time - dect) * unit; //匀速距离
+ float currdect = curr_time - unit; //减速时间
+ if (currdect >= 0)
+ {
+ vspeed = desV - fc_acc * currdect;
+ decD = desV * currdect - (fc_acc * currdect * currdect) / 2; //减速距离
+ }
+ vdist = vdist + decD; //总距离=匀速距离+减速距离
+ if (vdist > Distance) vdist = Distance; //约束距离
+ }
+ return 0;
+ }
+
+
+
+ float getMinfligthtime(float Distance, float fc_acc, float fc_maxspeed)
+ {
+ float fDis = fabsf(Distance);
+ float facc = fabsf(fc_acc);
+
+ float realflytime = 0.0f;
+ //计算一半的距离
+ float hafdis = (fDis / 2);
+ //计算最大速度
+ float vel = (float)sqrt(2 * facc * hafdis);
+ //如果速度超过最大速度
+ if (vel > fc_maxspeed)
+ //使用最大速度
+ vel = fc_maxspeed;
+ //加速时间
+ float acctim = vel / facc;
+ //加速距离
+ float accdis = vel * vel / (2 * facc);
+ //匀速段时间
+ float vtime = (hafdis - accdis) / vel;
+ //到一半的时间
+ float haftime = acctim + vtime;
+ realflytime = haftime * 2;
+ return realflytime;
+ }
+
+ ///计算飞行距离和速度 单位--米，秒----
+ //返回 整个距离最大飞行速度
+ ///flight_time 总飞行时间 Distance 飞行总距离 curr_time 当前飞行时间 fc_acc加速度, fc_maxspeed最大速度 vdist 计算的应该飞的距离 vspeed 计算的当前时间应该速度
+ //========这是飞控正在使用的算法========
+ float getspeeddist_V1(float flight_time, float Distance, float curr_time, float fc_acc, float fc_maxspeed, out float vdist, out float vspeed)
+ {
+ float plan_flytime=_plan_flytime;
+ float desired_velocity=_desired_velocity;
+ float dist = 0;
+ //导航加速度 米/秒*秒 ---不使用导航库
+ float wpacc = fabsf(fc_acc);// g.fc_acc_xy;//1.5;
+ //
+ float speed = 0;
+ //计算实时速度
+ if (curr_time <= (plan_flytime / 2.0))
+ speed = curr_time * wpacc;
+ else
+ //需要减速
+ speed = (plan_flytime - curr_time) * wpacc;
+ //不能大于目标速度
+ if (speed > desired_velocity)
+ speed = desired_velocity;
+ if (speed <= 0)
+ speed = 0;
+ vspeed = speed;
+ //计算实时距离
+ float V_start = 0.0f;
+ float V_end = 0.0f;
+ //加速段
+ float t1 = (desired_velocity - V_start) / wpacc;
+ //减速段
+ float t3 = (desired_velocity - V_end) / wpacc;
+ //平飞段
+ float t2 = plan_flytime - t1 - t3;
+ if (curr_time < t1)
+ {
+ dist = 0.5f * wpacc * curr_time * curr_time + curr_time * V_start;
+ }
+ else if (curr_time < t1 + t2)
+ {
+ dist = -0.5f * wpacc * t1 * t1 + (curr_time) * desired_velocity;
+ }
+ else
+ {
+ float t = curr_time - t1 - t2;
+ dist = -0.5f * wpacc * t1 * t1 + curr_time * desired_velocity - 0.5f * wpacc * t * t;
+ }
+
+ if (fabsf(dist) > fabsf(Distance))
+ vdist = Distance;
+ else
+ {
+ if (Distance < 0)
+ vdist = -dist;
+ else vdist = dist;
+ }
+
+ return desired_velocity;
+ }
+
+ float _vspeed=0;
+
+ float _desired_velocity = 0;
+ float _plan_flytime = 0;
+
+ float initgetspeeddist_V1(float flight_time, float Distance, float fc_acc, float fc_maxspeed)
+ {
+ //计划飞行时间
+ float plan_flytime = flight_time;
+ //计算距离用绝对值
+ float fDis = fabsf(Distance);
+
+ //导航加速度 米/秒*秒 ---不使用导航库
+ float wpacc = fabsf(fc_acc);// g.fc_acc_xy;//1.5;
+
+ //最大目标速度---米/秒
+ float desired_velocity = 0;
+
+
+ //计算最小飞行时间
+ float minflytime = getMinfligthtime(Distance, fc_acc, fc_maxspeed);
+ if (flight_time < minflytime)
+ plan_flytime = minflytime;// + 0.1f;
+
+
+ //根据总飞行时间和总距离计算飞行速度----起始和结束速度都是0，中间按匀加速和匀减速计算，没考虑加加速度
+ float delta = (0.5f * plan_flytime) * (0.5f * plan_flytime) - fDis / wpacc;
+ if (delta >= 0)
+ {
+ desired_velocity = (0.5f * plan_flytime - sqrt(delta)) / (1 / wpacc);
+ if (desired_velocity > fc_maxspeed)
+ {
+ plan_flytime = minflytime;
+ desired_velocity = fc_maxspeed;
+ }
+ }
+ else
+ {
+ desired_velocity = (0.5f * plan_flytime) / (1 / wpacc);
+ }
+ if (desired_velocity < 0.2f) desired_velocity = 0.2f; //限制最小速度0.2米/秒
+ if (desired_velocity > fc_maxspeed) desired_velocity = fc_maxspeed;//限制最大速度10米/秒，应该参数化
+
+ _desired_velocity = desired_velocity;
+ _plan_flytime = plan_flytime;
+ return desired_velocity;
+ }
+
+
+ public float initgetspeeddist(int flytype, float flight_time, float Distance, float fc_acc, float fc_maxspeed)
+ {
+ flytype = 0;
+ switch (flytype)
+ {
+ case 0: //匀速
+ _vspeed = Distance / flight_time;
+ return 0;
+ case 1: //同时计算加减速
+ return initgetspeeddist_V1(flight_time, Distance, fc_acc, fc_maxspeed);
+ case 2: //单加速
+ return 0;
+ case 3: //单减速
+ return 0;
+ default:
+
+ return 0;
+ }
+
+ }
+ public float getspeeddist(int flytype, float flight_time, float Distance, float curr_time, float fc_acc, float fc_maxspeed, out float vdist, out float vspeed)
+ {
+ flytype = 0;
+ switch (flytype)
+ {
+ case 0: //匀速
+ //case 1: //匀速
+ vspeed = _vspeed;
+ vdist = vspeed * curr_time;
+ return 0;
+ case 1: //同时计算加减速
+ return getspeeddist_V1(flight_time, Distance, curr_time, fc_acc, fc_maxspeed, out vdist, out vspeed);
+ case 2: //单加速
+ return getspeeddist_V2(flight_time, Distance, curr_time, fc_acc, fc_maxspeed, out vdist, out vspeed);
+ case 3: //单减速
+ return getspeeddist_V3(flight_time, Distance, curr_time, fc_acc, fc_maxspeed, out vdist, out vspeed);
+ default:
+ vspeed = 0;
+ vdist = 0;
+ return 0;
+ }
+ }
+
+ }
+ }
diff --git a/PlaneGcsSdk_Shared/Copters/FakeCopter.cs b/PlaneGcsSdk_Shared/Copters/FakeCopter.cs
index 240748c..45a5958 100644
--- a/PlaneGcsSdk_Shared/Copters/FakeCopter.cs
+++ b/PlaneGcsSdk_Shared/Copters/FakeCopter.cs
@@ -1,922 +1,935 @@
-using Plane.Communication;
-using Plane.Geography;
-using System;
-using System.Diagnostics;
-using System.Drawing;
-using System.Threading;
-using System.Threading.Tasks;
-using static Plane.Copters.Constants;
-using System.Windows.Media;
-using Plane.CopterControllers;
-
-namespace Plane.Copters
-{
- ///
- /// 虚拟的 实现。
- ///
- [DebuggerDisplay("Name={Name}")]
- public partial class FakeCopter : CopterImplSharedPart, IFakeCopter
- {
- ///
- /// 心跳间隔，单位为毫秒。
- ///
- private const int HEARTBEAT_INTERVAL = 200;
- ///
- /// 更新虚拟飞行器状态的时间间隔，单位为毫秒。
- ///
- static private int UPDATE_INTERVAL =100; //默认100， i7电脑 150可以跑1000架 100可以跑500架 50可以200-300架
-
- static private int UPDATE_INTERVAL_TEMP = 50;
- ///
- /// 在一个更新间隔中的最大移动距离。
- ///
- private float MAX_MOVE_IN_INTERVAL = MAX_VEL * UPDATE_INTERVAL_TEMP / 1000;
-
- ///
- /// 高速模式下，在一个更新间隔中的最大移动距离。
- ///
- private float MAX_MOVE_IN_INTERVAL_FAST = MAX_VEL * UPDATE_INTERVAL_TEMP / 1000 * 4; //MAX_MOVE_IN_INTERVAL*4
-
-
-
- // private int _UPDATE_INTERVAL = 50;
-
- ///
- /// 对飞行器的模拟是否正在运行。
- ///
- private bool _isRunning = false;
-
- ///
- /// 上次计算速度时的位置。
- ///
- private ILocation _lastCalcSpeedPoint;
-
- ///
- /// 上次计算速度的时间。
- ///
- private DateTime _lastCalcSpeedTime;
-
- ///
- /// 高速模式下，按比例缩放过的在一个更新间隔中的最大移动距离。
- ///
- private float _scaledFastMaxMoveInInterval = MAX_VEL * UPDATE_INTERVAL_TEMP / 1000 * 4; //MAX_MOVE_IN_INTERVAL_FAST
-
- ///
- /// 按比例缩放过的在一个更新间隔中的最大移动距离。
- ///
- private float _scaledMaxMoveInInterval = MAX_VEL * UPDATE_INTERVAL_TEMP / 1000; //MAX_MOVE_IN_INTERVAL
-
- ///
- /// 速度缩放比例。
- ///
- private float _speedScale = 1;
-
- ///
- /// 自动起飞的目标高度。
- ///
- private int _takeOffTargetAltitude;
-
- ///
- /// FlyTo 的目标高度。
- ///
- private float _targetAlt;
- //航点开始高度
- private float _startAlt;
-
- private float _Lng_delta;
- private float _Lat_delta;
-
- private float _flytime;
- private DateTime _startTicks;
-
- private float _distance_xy;
- private float _distance_z;
- private float _xy_speed;
- private float _z_speed;
- private int _flytype;
- private float currflytime;
- // 根据飞行时间计算飞行距离。
- private float currdis_xy;
- private float currdis_z;
-
-
- // 目标点相对于开始位置的方向。
- private double _direction;
-
- ///
- /// FlyTo 的目标纬度。
- ///
- private double _targetLat;
-
- ///
- /// FlyTo 的目标经度。
- ///
- private double _targetLng;
- ///
- /// FlyTo 的目标纬度。
- ///
- private double _startLat;
-
- ///
- /// FlyTo 的目标经度。
- ///
- private double _startLng;
-
-
- ///
- /// FlyTo 的目标经度。
- ///
- private bool _ShowLED;
-
- private System.Drawing.ColorConverter rgbconverter;
-
-
- ///
- /// 使用 创建 实例。
- ///
- public FakeCopter() : this(SynchronizationContext.Current)
- {
-
-
- }
-
-
- new public int sim_update_int
- {
- get { return UPDATE_INTERVAL; }
- set
- {
- //最大移动距离
- MAX_MOVE_IN_INTERVAL = MAX_VEL * value / 1000;
- //快速模式最大移动距离
- MAX_MOVE_IN_INTERVAL_FAST = MAX_MOVE_IN_INTERVAL * 4;
- //速度缩放后快速速度距离
- _scaledFastMaxMoveInInterval = MAX_MOVE_IN_INTERVAL_FAST* _speedScale;
- //速度缩放后速度距离
- _scaledMaxMoveInInterval = MAX_MOVE_IN_INTERVAL* _speedScale;
- Set(nameof(sim_update_int), ref UPDATE_INTERVAL, value);
- }
- }
-
-
- public Task DoCommandAckAsync(ushort command, byte result)
- {
- return TaskUtils.CompletedTask;
- }
-
- ///
- /// 创建 实例。
- ///
- /// UI 线程的同步上下文。
- /// 自动起飞时的目标高度。
- /// 速度缩放比例。
- public FakeCopter(SynchronizationContext uiSyncContext, int takeOffTargetAltitude = 10, float speedScale = 1, IConnection connection = null)
- : base(uiSyncContext)
- {
- Latitude = 23.155382266268134;
- Longitude = 113.45038586296141;
-
- Elevation = 20;
- this.PropertyChanged += (sender, e) =>
- {
- switch (e.PropertyName)
- {
- case nameof(Altitude):
- Elevation = Altitude + 20;
- break;
-
- case nameof(IsUnlocked):
- if (IsUnlocked)
- {
- // 王海, 20151029, 在解锁时上次计算速度点、上次计算速度时刻。
- _lastCalcSpeedPoint = new PLLocation { Latitude = Latitude, Longitude = Longitude, Altitude = Altitude };
- _lastCalcSpeedTime = DateTime.Now;
- }
- break;
- }
- };
-
- _takeOffTargetAltitude = takeOffTargetAltitude;
-
- _speedScale = speedScale;
- _scaledMaxMoveInInterval = MAX_MOVE_IN_INTERVAL * _speedScale;
- _scaledFastMaxMoveInInterval = MAX_MOVE_IN_INTERVAL_FAST * _speedScale;
-
- this.Connection = connection ?? EmptyConnection.Instance;
- _isRunning = true;
- // 持续计算并更新虚拟飞行器的状态。
- // Task.Run(async () =>
-
- Task.Factory.StartNew(async () =>
-
- {
- while (_isRunning)
- {
- Update();
- await TaskUtils.Delay(UPDATE_INTERVAL).ConfigureAwait(false);
- }
- }
- , TaskCreationOptions.LongRunning
- );
-
-
- ++HeartbeatCount;
- GpsFixType = GpsFixType.Fix3D;
- GpsHdop = 1;
- IsGpsAccurate = true;
- HasSwitchedToGpsMode = true;
-
- /*
- // 持续假装收到飞行器发来的心跳。
- Task.Run(async () =>
- {
- while (true)
- {
- if (_isRunning)
- {
- ++HeartbeatCount;
-
- IsCheckingConnection = false;
-
- if (HeartbeatCount >= 10)
- {
- // 收到若干个心跳之后，设置一下 GPS 相关属性。
- GpsFixType = GpsFixType.Fix3D;
- GpsHdop = 1;
- IsGpsAccurate = true;
- HasSwitchedToGpsMode = true;
- }
-
- _uiSyncContext.Post(() => RaiseHeartbeatReceived(new HeartbeatReceivedEventArgs(HeartbeatCount)));
- }
- await TaskUtils.Delay(HEARTBEAT_INTERVAL).ConfigureAwait(false);
- }
- });
- */
- }
-
- public IConnection Connection { get; set; }
-
- public string Id { get; set; }
-
- private int ReturnToLaunchAltitude { get { return _takeOffTargetAltitude; } }
-
- public Task ConnectAsync()
- {
- IsConnected = true;
- _isRunning = true;
- IsCheckingConnection = true;
- return TaskUtils.CompletedTask;
- }
-
- public Task DisconnectAsync()
- {
- IsConnected = false;
- _isRunning = false;
- IsCheckingConnection = false;
- return TaskUtils.CompletedTask;
- }
-
- protected override Task FlyToCoreAsync(double lat, double lng, float alt, float flytime = 0, int flytype = 0)
- {
- if (!IsEmergencyHoverActive)
- {
- _targetLat = lat;
- _targetLng = lng;
- _targetAlt = alt;
- _startLat = Latitude;
- _startLng = Longitude ;
- _startAlt = Altitude;
- _direction = this.CalcDirection2D(lat, lng);
-
- _flytime = flytime*1000; //ms
- _startTicks = DateTime.Now; //ms
-
-
- _Lng_delta = (float)( Math.Sin(_direction) * GeographyUtils.CalcMetersToLngSpan(Latitude));
- _Lat_delta =(float)( Math.Cos(_direction) * GeographyUtils.METERS_TO_LAT_SPAN);
-
- //计算xy和x方向距离
- _distance_xy = (float)this.CalcDistance(lat, lng, Altitude);
- _distance_z = alt - Altitude;
- Console.WriteLine($"{this.Id}d:{_distance_xy},lat{this.Latitude},lng{this.Longitude },tlat:{lat}tlng:{lng}");
-
- _flytype = flytype;
- // _xy_speed = _distance_xy / _flytime;
- // _z_speed = _distance_z / _flytime;
- Mode = FlightMode.GUIDED;
-
-
- }
- return TaskUtils.CompletedTask;
- }
-
- public Task GetCopterDataAsync()
- {
- return TaskUtils.CompletedTask;
- }
-
- public Task GetParamAsync(string paramName, int millisecondsTimeout = Timeout.Infinite)
- {
- // TODO: 王海, 20150806, 实现仿真的 GetParamAsync。
- return Task.FromResult(0f);
- }
-
- public Task LockAsync()
- {
- if (IsUnlocked)
- {
- IsUnlocked = false;
- }
- return TaskUtils.CompletedTask;
- }
-
- public Task LEDAsync()
- {
- return TaskUtils.CompletedTask;
- }
-
- public Task MotorTestAsync(int motor)
- {
- return TaskUtils.CompletedTask;
- }
-
- public override Task SetChannelsAsync()
- {
- Channel1 = DesiredChannel1 ?? Channel1;
- Channel2 = DesiredChannel2 ?? Channel2;
- Channel3 = DesiredChannel3 ?? Channel3;
- Channel4 = DesiredChannel4 ?? Channel4;
- Channel5 = DesiredChannel5 ?? Channel5;
- Channel6 = DesiredChannel6 ?? Channel6;
- Channel7 = DesiredChannel7 ?? Channel7;
- Channel8 = DesiredChannel8 ?? Channel8;
- return TaskUtils.CompletedTask;
- }
-
- public override Task SetMobileControlAsync()
- {
- Channel1 = DesiredChannel1 ?? Channel1;
- Channel2 = DesiredChannel2 ?? Channel2;
- Channel3 = DesiredChannel3 ?? Channel3;
- Channel4 = DesiredChannel4 ?? Channel4;
- Channel5 = DesiredChannel5 ?? Channel5;
- Channel6 = DesiredChannel6 ?? Channel6;
- Channel7 = DesiredChannel7 ?? Channel7;
- Channel8 = DesiredChannel8 ?? Channel8;
- Yaw = DesiredYaw ?? Yaw;
- Heading = (short)Yaw;
- return TaskUtils.CompletedTask;
- }
-
- //得到收到的总数据量
- public void GetCommunicationNumber(out int recnumber, out int sendnumber)
- {
- recnumber = 0;
- sendnumber = 0;
- }
-
-
- //重设数据量
- public void ResetCommunicationNumber()
- {
-
- }
-
- public async Task DoCommandAsync(int actionid, float p1, float p2, float p3, float p4, float p5, float p6, float p7)
- {
-
- await Task.Delay(50).ConfigureAwait(false);
-
-
- }
- public Task SetParamAsync(string paramName, float value, int millisecondsTimeout = Timeout.Infinite)
- {
- // TODO: 王海, 20150807, 实现仿真的 SetParamAsync。
- return TaskUtils.CompletedTask;
- }
- public bool GetShowLEDAsync()
- {
- return _ShowLED;
- }
- public Task SetShowLEDAsync(bool Ledon)
- {
- _ShowLED = Ledon;
- RaiseLocationChanged();
- return TaskUtils.CompletedTask;
- }
-
-
- public Task SetShowRGBLEDAsync(int RedValue, int GreenValue, int BlueValue)
- {
- return TaskUtils.CompletedTask;
- }
-
-
- public Task SetShowLEDFlashAsync(int Flashtype, int millisecondsTime)
- {
- bool Ledison;
- Ledison = GetShowLEDAsync();
- if (!Ledison)
- {
- SetShowLEDAsync(true);
- Task.Delay(millisecondsTime).ConfigureAwait(false);
- SetShowLEDAsync(false);
- }
- else
- {
- SetShowLEDAsync(false);
- Task.Delay(millisecondsTime).ConfigureAwait(false);
- SetShowLEDAsync(true);
- }
-
- return TaskUtils.CompletedTask;
- }
-
- public void SetProperties(
- string id = null, //"Junqing's Drone",
- double? latitude = null, //23.14973333,
- double? longitude = null, //113.40974166,
- float? altitude = null, //0,
- string name = null, //"王海的飞行器",
- byte? batteryPer = null, //10,
- short? heading = null, //33,
- bool? isConnected = null, //true,
- float? pitch = null, //-70,
- float? roll = null, //28,
- byte? satCount = null, //6,
- float? airSpeed = null, //3.333,
- double? flightDistance = null, //100.388,
- double? flightDistance2D = null, // 100.88,
- TimeSpan? flightTimeSpan = null)
- {
- if (id != null) Id = id;
- if (name != null) Name = name;
- if (latitude != null) Latitude = latitude.Value;
- if (longitude != null) Longitude = longitude.Value;
- if (altitude != null) Altitude = altitude.Value;
- if (batteryPer != null) BatteryPer = batteryPer.Value;
- if (heading != null) Heading = heading.Value;
- if (isConnected != null) IsConnected = isConnected.Value;
- if (pitch != null) Pitch = pitch.Value;
- if (roll != null) Roll = roll.Value;
- if (satCount != null) SatCount = satCount.Value;
- if (airSpeed != null) AirSpeed = airSpeed.Value;
- if (flightDistance != null) FlightDistance = flightDistance.Value;
- if (flightDistance2D != null) FlightDistance2D = flightDistance2D.Value;
- if (flightTimeSpan != null) FlightTimeSpan = flightTimeSpan.Value;
- }
-
- public Task StartPairingAsync()
- {
- RaisePairingCompleted(new PairingCompletedEventArgs(true, 0, 0));
- return TaskUtils.CompletedTask;
- }
-
- public Task StopPairingAsync()
- {
- return TaskUtils.CompletedTask;
- }
-
- public async Task TakeOffAsync(float alt)
- {
- await UnlockAsync().ConfigureAwait(false);
- _takeOffTargetAltitude = (int)alt;
- await TakeOffAsync().ConfigureAwait(false);
- }
- public Task MissionStartAsync(int hour_utc, int minute_utc, int second_utc, double Missionlng, double Missionlat)
- {
- return TaskUtils.CompletedTask;
- }
-
-
- public async Task UnlockAsync()
- {
- if (!IsUnlocked)
- {
- await SetChannelsAsync(
- ch1: 1500,
- ch2: 1500,
- ch3: 1100,
- ch4: 1500
- ).ConfigureAwait(false);
- if (await SetModeAsync(FlightMode.ALT_HOLD).ConfigureAwait(false))
- {
- IsUnlocked = true;
- }
- }
- }
-
- ///
- /// 无论在不在紧急悬停状态下都可用的切换模式方法。
- ///
- /// 模式。
- /// 超时。
- /// 成功与否。
- internal override Task SetModeCoreAsync(FlightMode mode, int millisecondsTimeout = 5000)
- {
- Mode = mode;
- return Task.FromResult(true);
- }
-
- protected override void UpdateFlightDataIfNeeded()
- {
- //计算飞机距离，没必要
- /*
- if (!TakeOffPoint.IsNullOrEmpty())
- {
- FlightDistance = TakeOffPoint.CalcDistance(this);
- FlightDistance2D = TakeOffPoint.CalcDistance2D(this);
- }
-
-
- if (FlightTimeSpan.TotalSeconds > 0)
- {
- if (_lastCalcSpeedPoint.IsNullOrEmpty())
- {
- if (!this.IsNullOrEmpty())
- {
- _lastCalcSpeedPoint = new PLLocation(this);
- _lastCalcSpeedTime = DateTime.Now;
- }
- }
- else
- {
- var movedDistance = _lastCalcSpeedPoint.CalcDistance(this);
- var movedTime = DateTime.Now - _lastCalcSpeedTime;
- if (movedDistance >= 3 || movedTime.TotalSeconds >= 3)
- {
- var airSpeed = movedDistance / movedTime.TotalSeconds;
- if (airSpeed < 100) // 王海, 20151023, 速度过大时不正常，经纬度可能有错误。
- {
- AirSpeed = (float)airSpeed;
- }
- //Windows.Messages.Message.Show(AirSpeed.ToString());
- GroundSpeed = AirSpeed;
- _lastCalcSpeedPoint = new PLLocation(this);
- _lastCalcSpeedTime = DateTime.Now;
- }
- }
- }
- */
- }
-
- private void MoveToPointImmediately(double lat, double lng, float alt)
- {
- Latitude = lat;
- Longitude = lng;
- Altitude = alt;
- Roll = 0;
- Pitch = 0;
- }
- public Task InjectGpsDataAsync(byte[] data, ushort length)
- {
- return TaskUtils.CompletedTask;
-
- }
-
- public static System.Drawing.Color GetRandomColor()
- {
- Random rand = new Random();
- return System.Drawing.Color.FromArgb(rand.Next(256), rand.Next(256), rand.Next(256));
- }
-
- private DateTime led_laston;
- private System.Drawing.Color Led_color;
-
- //更新显示颜色，根据设置的led参数
- private void UpdateShowColor()
- {
- // 使用实例化的对象调用ConvertFromString
- //LEDShowColor = (System.Drawing.Color)converter.ConvertFromString("#" + LEDColor);
- // 创建ColorConverter实例用于颜色转换
- if ( rgbconverter==null) rgbconverter=new System.Drawing.ColorConverter();
- //简化版的颜色模拟
- switch (LEDMode)
- {
- case 0:
- if (LEDColor != null)
- LEDShowColor = (System.Drawing.Color)rgbconverter.ConvertFromString("#" + LEDColor);
- else LEDShowColor = (System.Drawing.Color)rgbconverter.ConvertFromString("#FFFFFF");
-
- break;
- //闪烁
- case 1:
- case 2:
- case 3:
- case 4:
- case 5:
- case 6:
- case 7:
- case 8:
- case 9:
- case 10:
- case 11:
- case 12:
- case 13:
- case 14:
-
- if (DateTime.Now.AddMilliseconds(-(LEDInterval*1000)) >= led_laston)
- {
- led_laston = DateTime.Now;
- if (LEDShowColor != System.Drawing.Color.Black)
- {
- LEDShowColor = System.Drawing.Color.Black;
-
- }
- else
- {
- if ((LEDMode == 1) || (LEDMode == 4) || (LEDMode == 5) || (LEDMode == 6) || (LEDMode == 7) || (LEDMode == 9) || (LEDMode == 11) || (LEDMode == 13))
- LEDShowColor = (System.Drawing.Color)rgbconverter.ConvertFromString("#" + LEDColor);
- if ((LEDMode == 2)||(LEDMode == 3) || (LEDMode == 8) || (LEDMode == 10) || (LEDMode == 12) || (LEDMode == 14))
- LEDShowColor = GetRandomColor();
- }
- }
-
- break;
-
- }
-
-
-
-
-
- }
-
- private bool ReachedDestination(double destLat, double destLng, float destAlt) => this.CalcDistance(destLat, destLng, destAlt) < 0.1;
-
- ///
- /// 计算并更新虚拟飞行器状态。
- ///
- private void Update()
- {
- if (IsUnlocked)
- {
- // 飞行器接触地面（只判断 Altitude <= 0）后，如果处于 LAND 模式或者保持 Channel3 < MIN_HOVER_CHANNEL，在一段时间后自动锁定。
- if ((Mode == FlightMode.LAND || Channel3 < MIN_HOVER_CHANNEL) && Altitude <= 0)
- {
- Task.Run(async () =>
- {
- for (int i = 0; i < 4000 / 20; i++)
- {
- await TaskUtils.Delay(20).ConfigureAwait(false);
- if ((Mode != FlightMode.LAND && Channel3 >= MIN_HOVER_CHANNEL) || !IsUnlocked)
- {
- return;
- }
- }
- IsUnlocked = false;
- });
- }
- else
- {
- switch (Mode)
- {
- case FlightMode.ACRO:
- break;
-
- case FlightMode.AUTO:
- /*
- // 暂时只管起飞。以最大速度上升，直到达到目标高度。
- if (Altitude < _takeOffTargetAltitude)
- {
- Altitude += _scaledMaxMoveInInterval;
- }
- else
- {
- Mode = FlightMode.LOITER;
- }
- */
- break;
-
- case FlightMode.GUIDED:
- // 王海, 20160317, 指点时也能体感控制若干通道。
- //考虑不更新这个，好像没必要-xu
- //UpdateWithChannels();
- //UpdateWithDestination(_targetLat, _targetLng, _targetAlt);
- if (FlightControlMode==1)
- UpdateWithDestination_v2(_targetLat, _targetLng, _targetAlt);
- else
- UpdateWithDestination(_targetLat, _targetLng, _targetAlt);
- break;
-
- case FlightMode.STABILIZE:
- case FlightMode.ALT_HOLD:
- case FlightMode.LOITER:
- UpdateWithChannels();
- break;
-
- case FlightMode.RTL:
- if (Altitude < ReturnToLaunchAltitude && !ReachedDestination(Latitude, Longitude, ReturnToLaunchAltitude))
- {
- UpdateWithDestination(Latitude, Longitude, ReturnToLaunchAltitude);
- }
- else if (Altitude > ReturnToLaunchAltitude && !ReachedDestination(TakeOffPoint.Latitude, TakeOffPoint.Longitude, Altitude))
- {
- UpdateWithDestination(TakeOffPoint.Latitude, TakeOffPoint.Longitude, Altitude);
- }
- else
- {
- UpdateWithDestination(TakeOffPoint.Latitude, TakeOffPoint.Longitude, ReturnToLaunchAltitude);
- }
- break;
-
- case FlightMode.CIRCLE:
- break;
-
- case FlightMode.POSITION:
- break;
-
- case FlightMode.LAND:
- // 王海, 20160317, 降落时也能体感控制若干通道。
- UpdateWithChannels();
- // 以最大速度降落，直到高度为 0。
- if (Altitude > 0)
- {
- if (Altitude > _scaledMaxMoveInInterval)
- {
- Altitude -= _scaledMaxMoveInInterval;
- }
- else
- {
- Altitude = 0;
- }
- Roll = 0;
- Pitch = 0;
- }
- break;
-
- case FlightMode.OF_LOITER:
- break;
-
- case FlightMode.TOY:
- break;
-
- default:
- break;
- }
- //UpdateFlightDataIfNeeded();
- }
- }
- else
- {
- // TODO: 王海, 20151228, 模拟空中锁定。
- // 锁定时直接把速度设为 0。
- AirSpeed = 0;
- }
-
- // UpdateShowColor();
-
-
-
- _uiSyncContext.Post(() =>
- {
- //位置变化需要在UI刷新
- RaiseLocationChangedIfNeeded();
- //高度变化需要在UI刷新
- // RaiseAltitudeChangedIfNeeded();
- //GPS数据用于显示
- // RaiseDataStreamReceived(PDataStreamType.GPS_RAW_INT);
- //不考虑姿态
- // RaiseAttitudeChanged();
- });
- }
-
- ///
- /// 根据各个通道值来更新位置、姿态等状态。
- ///
- private void UpdateWithChannels()
- {
- var maxMove = Mode == FlightMode.LOITER ? _scaledMaxMoveInInterval : _scaledFastMaxMoveInInterval;
- var ch3Delta = Channel3 > MAX_HOVER_CHANNEL ? Channel3 - MAX_HOVER_CHANNEL : Channel3 < MIN_HOVER_CHANNEL ? Channel3 - MIN_HOVER_CHANNEL : 0;
- if (ch3Delta != 0)
- {
- // 更新相对高度。
- Altitude = Math.Max(0, Altitude + maxMove * ch3Delta / MAX_CHANNEL_DELTA);
- }
-
- var yawRad = Yaw.DegToRad();
- var metersToLocalLng = GeographyUtils.CalcMetersToLngSpan(Latitude);
-
- var ch1Delta = Channel1 > MAX_HOVER_CHANNEL ? Channel1 - MAX_HOVER_CHANNEL : Channel1 < MIN_HOVER_CHANNEL ? Channel1 - MIN_HOVER_CHANNEL : 0;
- var xDistance = maxMove * ch1Delta / MAX_CHANNEL_DELTA;
- if (ch1Delta != 0)
- {
- // 更新横滚方向的纬度和经度。
- Latitude -= xDistance * Math.Sin(yawRad) * GeographyUtils.METERS_TO_LAT_SPAN;
- Longitude += xDistance * Math.Cos(yawRad) * metersToLocalLng;
- // 更新横滚姿态。
- Roll = MAX_TILT_IN_FLIGHT * ch1Delta / MAX_CHANNEL_DELTA;
- }
- else
- {
- Roll = 0;
- }
-
- var ch2Delta = Channel2 > MAX_HOVER_CHANNEL ? Channel2 - MAX_HOVER_CHANNEL : Channel2 < MIN_HOVER_CHANNEL ? Channel2 - MIN_HOVER_CHANNEL : 0;
- var yDistance = maxMove * ch2Delta / MAX_CHANNEL_DELTA;
- if (ch2Delta != 0)
- {
- // 更新俯仰方向的纬度和经度。
- Latitude -= yDistance * Math.Cos(yawRad) * GeographyUtils.METERS_TO_LAT_SPAN;
- Longitude -= yDistance * Math.Sin(yawRad) * metersToLocalLng;
- // 更新俯仰姿态。
- Pitch = MAX_TILT_IN_FLIGHT * ch2Delta / MAX_CHANNEL_DELTA;
- }
- else
- {
- Pitch = 0;
- }
- }
-
- ///
- /// 根据目的地来更新位置、姿态等状态。
- ///
- private void UpdateWithDestination(ILocation loc)
- {
- UpdateWithDestination(loc.Latitude, loc.Longitude, loc.Altitude);
- }
-
- ///
- /// 根据目的地来更新位置、姿态等状态。
- ///
- private void UpdateWithDestination(double lat, double lng, float alt)
- {
- // 与目标点之间的距离。
- var distance = this.CalcDistance(lat, lng, alt);
- // 距离已经很近，直接移动到目标点。
- if (distance < _scaledMaxMoveInInterval)
- {
- MoveToPointImmediately(lat, lng, alt);
- return;
- }
- // 在空间中的移动距离。
- var move = _scaledMaxMoveInInterval;
- // 竖直方向的移动距离。
- var altDelta = (float)(move * (alt - Altitude) / distance);
- // 更新高度。
- Altitude += altDelta;
- // 目标点相对于当前位置的方向。
- // var direction = this.CalcDirection2D(lat, lng);
- // 水平面上的移动距离。
- var moveInHorizontalPlane = Math.Sqrt(move * move - altDelta * altDelta);
- if (double.IsNaN(moveInHorizontalPlane))
- {
- MoveToPointImmediately(lat, lng, alt);
- return;
- }
-
- // 更新纬度。
- //Longitude += moveInHorizontalPlane * Math.Sin(direction) * GeographyUtils.CalcMetersToLngSpan(Latitude);
- Longitude += moveInHorizontalPlane * _Lng_delta;
- Latitude += moveInHorizontalPlane * _Lat_delta;
- // 更新经度。
- //Latitude += moveInHorizontalPlane * Math.Cos(direction) * GeographyUtils.METERS_TO_LAT_SPAN;
- }
- //新版本小航点计算移动位置
- private void UpdateWithDestination_v2(double lat, double lng, float alt)
- {
- //_flytime 总飞行时间 秒
- //_startTicks 开始飞行时间ms
- // _distance_xy 米
- // _distance_z 米
- //当前飞行时间
- if ((lat == Latitude) && (lng == Longitude) && (alt == Altitude)) return;
- currflytime =(float)( DateTime.Now -_startTicks).TotalMilliseconds;//实际飞行时间 ms
- //超时直接移动到目标点
- if (currflytime >= _flytime)
- {
- MoveToPointImmediately(lat, lng, alt);
- return;
- }
- //if (currflytime > 13000)
- // return;
- // 根据飞行时间计算飞行距离
-
- float vspeed = 0;
-
- GuidController.getspeeddist(_flytype, _flytime / 1000, _distance_xy, currflytime / 1000, acc_xy,maxspeed_xy , out currdis_xy, out vspeed);
-
- if (alt> Altitude)
- GuidController.getspeeddist(_flytype, _flytime / 1000, _distance_z, currflytime / 1000, acc_z,maxspeed_up, out currdis_z, out vspeed);
- else
- GuidController.getspeeddist(_flytype, _flytime / 1000, _distance_z, currflytime / 1000, acc_z,maxspeed_down, out currdis_z, out vspeed);
-
- Console.WriteLine($"{this.Id}：time:{currflytime / 1000} to {currdis_z}");
-
- // 距离已经很近，直接移动到目标点。
- if ((Math.Abs(_distance_xy-currdis_xy) < 0.1)&& (Math.Abs(_distance_z - currdis_z) < 0.1))
- {
- Console.WriteLine($"{this.Id} to tlat:{lat}tlng:{lng}");
-
- MoveToPointImmediately(lat, lng, alt);
- return;
- }
- // 更新位置
- Altitude = _startAlt+ currdis_z;
- Longitude = _startLng + currdis_xy*_Lng_delta;
- Latitude = _startLat + currdis_xy *_Lat_delta;
- }
-
- }
-}
+using Plane.Communication;
+using Plane.Geography;
+using System;
+using System.Diagnostics;
+using System.Drawing;
+using System.Threading;
+using System.Threading.Tasks;
+using static Plane.Copters.Constants;
+using System.Windows.Media;
+using Plane.CopterControllers;
+
+namespace Plane.Copters
+{
+ ///
+ /// 虚拟的 实现。
+ ///
+ [DebuggerDisplay("Name={Name}")]
+ public partial class FakeCopter : CopterImplSharedPart, IFakeCopter
+ {
+ ///
+ /// 心跳间隔，单位为毫秒。
+ ///
+ private const int HEARTBEAT_INTERVAL = 200;
+ ///
+ /// 更新虚拟飞行器状态的时间间隔，单位为毫秒。
+ ///
+ static private int UPDATE_INTERVAL =100; //默认100， i7电脑 150可以跑1000架 100可以跑500架 50可以200-300架
+
+ static private int UPDATE_INTERVAL_TEMP = 50;
+ ///
+ /// 在一个更新间隔中的最大移动距离。
+ ///
+ private float MAX_MOVE_IN_INTERVAL = MAX_VEL * UPDATE_INTERVAL_TEMP / 1000;
+
+ ///
+ /// 高速模式下，在一个更新间隔中的最大移动距离。
+ ///
+ private float MAX_MOVE_IN_INTERVAL_FAST = MAX_VEL * UPDATE_INTERVAL_TEMP / 1000 * 4; //MAX_MOVE_IN_INTERVAL*4
+
+
+
+ // private int _UPDATE_INTERVAL = 50;
+
+ ///
+ /// 对飞行器的模拟是否正在运行。
+ ///
+ private bool _isRunning = false;
+
+ ///
+ /// 上次计算速度时的位置。
+ ///
+ private ILocation _lastCalcSpeedPoint;
+
+ ///
+ /// 上次计算速度的时间。
+ ///
+ private DateTime _lastCalcSpeedTime;
+
+ ///
+ /// 高速模式下，按比例缩放过的在一个更新间隔中的最大移动距离。
+ ///
+ private float _scaledFastMaxMoveInInterval = MAX_VEL * UPDATE_INTERVAL_TEMP / 1000 * 4; //MAX_MOVE_IN_INTERVAL_FAST
+
+ ///
+ /// 按比例缩放过的在一个更新间隔中的最大移动距离。
+ ///
+ private float _scaledMaxMoveInInterval = MAX_VEL * UPDATE_INTERVAL_TEMP / 1000; //MAX_MOVE_IN_INTERVAL
+
+ ///
+ /// 速度缩放比例。
+ ///
+ private float _speedScale = 1;
+
+ ///
+ /// 自动起飞的目标高度。
+ ///
+ private int _takeOffTargetAltitude;
+
+ ///
+ /// FlyTo 的目标高度。
+ ///
+ private float _targetAlt;
+ //航点开始高度
+ private float _startAlt;
+
+ private float _Lng_delta;
+ private float _Lat_delta;
+
+ private float _flytime;
+ private DateTime _startTicks;
+
+ private float _distance_xy;
+ private float _distance_z;
+ private float _xy_speed;
+ private float _z_speed;
+ private int _flytype;
+ private float currflytime;
+ // 根据飞行时间计算飞行距离。
+ private float currdis_xy;
+ private float currdis_z;
+
+
+ // 目标点相对于开始位置的方向。
+ private double _direction;
+
+ ///
+ /// FlyTo 的目标纬度。
+ ///
+ private double _targetLat;
+
+ ///
+ /// FlyTo 的目标经度。
+ ///
+ private double _targetLng;
+ ///
+ /// FlyTo 的目标纬度。
+ ///
+ private double _startLat;
+
+ ///
+ /// FlyTo 的目标经度。
+ ///
+ private double _startLng;
+
+
+ ///
+ /// FlyTo 的目标经度。
+ ///
+ private bool _ShowLED;
+
+ private System.Drawing.ColorConverter rgbconverter;
+
+
+ ///
+ /// 使用 创建 实例。
+ ///
+ public FakeCopter() : this(SynchronizationContext.Current)
+ {
+
+
+ }
+
+
+ new public int sim_update_int
+ {
+ get { return UPDATE_INTERVAL; }
+ set
+ {
+ //最大移动距离
+ MAX_MOVE_IN_INTERVAL = MAX_VEL * value / 1000;
+ //快速模式最大移动距离
+ MAX_MOVE_IN_INTERVAL_FAST = MAX_MOVE_IN_INTERVAL * 4;
+ //速度缩放后快速速度距离
+ _scaledFastMaxMoveInInterval = MAX_MOVE_IN_INTERVAL_FAST* _speedScale;
+ //速度缩放后速度距离
+ _scaledMaxMoveInInterval = MAX_MOVE_IN_INTERVAL* _speedScale;
+ Set(nameof(sim_update_int), ref UPDATE_INTERVAL, value);
+ }
+ }
+
+
+ public Task DoCommandAckAsync(ushort command, byte result)
+ {
+ return TaskUtils.CompletedTask;
+ }
+
+ ///
+ /// 创建 实例。
+ ///
+ /// UI 线程的同步上下文。
+ /// 自动起飞时的目标高度。
+ /// 速度缩放比例。
+ public FakeCopter(SynchronizationContext uiSyncContext, int takeOffTargetAltitude = 10, float speedScale = 1, IConnection connection = null)
+ : base(uiSyncContext)
+ {
+ Latitude = 23.155382266268134;
+ Longitude = 113.45038586296141;
+
+ Elevation = 20;
+ this.PropertyChanged += (sender, e) =>
+ {
+ switch (e.PropertyName)
+ {
+ case nameof(Altitude):
+ Elevation = Altitude + 20;
+ break;
+
+ case nameof(IsUnlocked):
+ if (IsUnlocked)
+ {
+ // 王海, 20151029, 在解锁时上次计算速度点、上次计算速度时刻。
+ _lastCalcSpeedPoint = new PLLocation { Latitude = Latitude, Longitude = Longitude, Altitude = Altitude };
+ _lastCalcSpeedTime = DateTime.Now;
+ }
+ break;
+ }
+ };
+
+ _takeOffTargetAltitude = takeOffTargetAltitude;
+
+ _speedScale = speedScale;
+ _scaledMaxMoveInInterval = MAX_MOVE_IN_INTERVAL * _speedScale;
+ _scaledFastMaxMoveInInterval = MAX_MOVE_IN_INTERVAL_FAST * _speedScale;
+
+ this.Connection = connection ?? EmptyConnection.Instance;
+ _isRunning = true;
+ // 持续计算并更新虚拟飞行器的状态。
+ // Task.Run(async () =>
+
+ Task.Factory.StartNew(async () =>
+
+ {
+ while (_isRunning)
+ {
+ Update();
+ await TaskUtils.Delay(UPDATE_INTERVAL).ConfigureAwait(false);
+ }
+ }
+ , TaskCreationOptions.LongRunning
+ );
+
+
+ ++HeartbeatCount;
+ GpsFixType = GpsFixType.Fix3D;
+ GpsHdop = 1;
+ IsGpsAccurate = true;
+ HasSwitchedToGpsMode = true;
+ GC_xy = new GuidController();
+ GC_z = new GuidController();
+
+ /*
+ // 持续假装收到飞行器发来的心跳。
+ Task.Run(async () =>
+ {
+ while (true)
+ {
+ if (_isRunning)
+ {
+ ++HeartbeatCount;
+
+ IsCheckingConnection = false;
+
+ if (HeartbeatCount >= 10)
+ {
+ // 收到若干个心跳之后，设置一下 GPS 相关属性。
+ GpsFixType = GpsFixType.Fix3D;
+ GpsHdop = 1;
+ IsGpsAccurate = true;
+ HasSwitchedToGpsMode = true;
+ }
+
+ _uiSyncContext.Post(() => RaiseHeartbeatReceived(new HeartbeatReceivedEventArgs(HeartbeatCount)));
+ }
+ await TaskUtils.Delay(HEARTBEAT_INTERVAL).ConfigureAwait(false);
+ }
+ });
+ */
+ }
+
+ public IConnection Connection { get; set; }
+
+ public string Id { get; set; }
+
+ private int ReturnToLaunchAltitude { get { return _takeOffTargetAltitude; } }
+ private GuidController GC_xy;
+ private GuidController GC_z;
+
+
+ public Task ConnectAsync()
+ {
+ IsConnected = true;
+ _isRunning = true;
+ IsCheckingConnection = true;
+ return TaskUtils.CompletedTask;
+ }
+
+ public Task DisconnectAsync()
+ {
+ IsConnected = false;
+ _isRunning = false;
+ IsCheckingConnection = false;
+ return TaskUtils.CompletedTask;
+ }
+
+ protected override Task FlyToCoreAsync(double lat, double lng, float alt, float flytime = 0, int flytype = 0)
+ {
+ if (!IsEmergencyHoverActive)
+ {
+ _targetLat = lat;
+ _targetLng = lng;
+ _targetAlt = alt;
+ _startLat = Latitude;
+ _startLng = Longitude ;
+ _startAlt = Altitude;
+ _direction = this.CalcDirection2D(lat, lng);
+
+ _flytime = flytime*1000; //ms
+ _startTicks = DateTime.Now; //ms
+
+
+ _Lng_delta = (float)( Math.Sin(_direction) * GeographyUtils.CalcMetersToLngSpan(Latitude));
+ _Lat_delta =(float)( Math.Cos(_direction) * GeographyUtils.METERS_TO_LAT_SPAN);
+
+ //计算xy和x方向距离
+ _distance_xy = (float)this.CalcDistance(lat, lng, Altitude);
+ _distance_z = alt - Altitude;
+ Console.WriteLine($"{this.Id}d:{_distance_xy},lat{this.Latitude},lng{this.Longitude },tlat:{lat}tlng:{lng}");
+
+ _flytype = flytype;
+ // _xy_speed = _distance_xy / _flytime;
+ // _z_speed = _distance_z / _flytime;
+ GC_xy.initgetspeeddist(_flytype, _flytime / 1000, _distance_xy, acc_xy, maxspeed_xy);
+ if (_targetAlt>Altitude )
+ GC_z.initgetspeeddist(_flytype, _flytime / 1000, _distance_z, acc_z, maxspeed_up);
+ else
+ GC_z.initgetspeeddist(_flytype, _flytime / 1000, _distance_z, acc_z, maxspeed_down);
+
+
+
+ Mode = FlightMode.GUIDED;
+
+
+ }
+ return TaskUtils.CompletedTask;
+ }
+
+ public Task GetCopterDataAsync()
+ {
+ return TaskUtils.CompletedTask;
+ }
+
+ public Task GetParamAsync(string paramName, int millisecondsTimeout = Timeout.Infinite)
+ {
+ // TODO: 王海, 20150806, 实现仿真的 GetParamAsync。
+ return Task.FromResult(0f);
+ }
+
+ public Task LockAsync()
+ {
+ if (IsUnlocked)
+ {
+ IsUnlocked = false;
+ }
+ return TaskUtils.CompletedTask;
+ }
+
+ public Task LEDAsync()
+ {
+ return TaskUtils.CompletedTask;
+ }
+
+ public Task MotorTestAsync(int motor)
+ {
+ return TaskUtils.CompletedTask;
+ }
+
+ public override Task SetChannelsAsync()
+ {
+ Channel1 = DesiredChannel1 ?? Channel1;
+ Channel2 = DesiredChannel2 ?? Channel2;
+ Channel3 = DesiredChannel3 ?? Channel3;
+ Channel4 = DesiredChannel4 ?? Channel4;
+ Channel5 = DesiredChannel5 ?? Channel5;
+ Channel6 = DesiredChannel6 ?? Channel6;
+ Channel7 = DesiredChannel7 ?? Channel7;
+ Channel8 = DesiredChannel8 ?? Channel8;
+ return TaskUtils.CompletedTask;
+ }
+
+ public override Task SetMobileControlAsync()
+ {
+ Channel1 = DesiredChannel1 ?? Channel1;
+ Channel2 = DesiredChannel2 ?? Channel2;
+ Channel3 = DesiredChannel3 ?? Channel3;
+ Channel4 = DesiredChannel4 ?? Channel4;
+ Channel5 = DesiredChannel5 ?? Channel5;
+ Channel6 = DesiredChannel6 ?? Channel6;
+ Channel7 = DesiredChannel7 ?? Channel7;
+ Channel8 = DesiredChannel8 ?? Channel8;
+ Yaw = DesiredYaw ?? Yaw;
+ Heading = (short)Yaw;
+ return TaskUtils.CompletedTask;
+ }
+
+ //得到收到的总数据量
+ public void GetCommunicationNumber(out int recnumber, out int sendnumber)
+ {
+ recnumber = 0;
+ sendnumber = 0;
+ }
+
+
+ //重设数据量
+ public void ResetCommunicationNumber()
+ {
+
+ }
+
+ public async Task DoCommandAsync(int actionid, float p1, float p2, float p3, float p4, float p5, float p6, float p7)
+ {
+
+ await Task.Delay(50).ConfigureAwait(false);
+
+
+ }
+ public Task SetParamAsync(string paramName, float value, int millisecondsTimeout = Timeout.Infinite)
+ {
+ // TODO: 王海, 20150807, 实现仿真的 SetParamAsync。
+ return TaskUtils.CompletedTask;
+ }
+ public bool GetShowLEDAsync()
+ {
+ return _ShowLED;
+ }
+ public Task SetShowLEDAsync(bool Ledon)
+ {
+ _ShowLED = Ledon;
+ RaiseLocationChanged();
+ return TaskUtils.CompletedTask;
+ }
+
+
+ public Task SetShowRGBLEDAsync(int RedValue, int GreenValue, int BlueValue)
+ {
+ return TaskUtils.CompletedTask;
+ }
+
+
+ public Task SetShowLEDFlashAsync(int Flashtype, int millisecondsTime)
+ {
+ bool Ledison;
+ Ledison = GetShowLEDAsync();
+ if (!Ledison)
+ {
+ SetShowLEDAsync(true);
+ Task.Delay(millisecondsTime).ConfigureAwait(false);
+ SetShowLEDAsync(false);
+ }
+ else
+ {
+ SetShowLEDAsync(false);
+ Task.Delay(millisecondsTime).ConfigureAwait(false);
+ SetShowLEDAsync(true);
+ }
+
+ return TaskUtils.CompletedTask;
+ }
+
+ public void SetProperties(
+ string id = null, //"Junqing's Drone",
+ double? latitude = null, //23.14973333,
+ double? longitude = null, //113.40974166,
+ float? altitude = null, //0,
+ string name = null, //"王海的飞行器",
+ byte? batteryPer = null, //10,
+ short? heading = null, //33,
+ bool? isConnected = null, //true,
+ float? pitch = null, //-70,
+ float? roll = null, //28,
+ byte? satCount = null, //6,
+ float? airSpeed = null, //3.333,
+ double? flightDistance = null, //100.388,
+ double? flightDistance2D = null, // 100.88,
+ TimeSpan? flightTimeSpan = null)
+ {
+ if (id != null) Id = id;
+ if (name != null) Name = name;
+ if (latitude != null) Latitude = latitude.Value;
+ if (longitude != null) Longitude = longitude.Value;
+ if (altitude != null) Altitude = altitude.Value;
+ if (batteryPer != null) BatteryPer = batteryPer.Value;
+ if (heading != null) Heading = heading.Value;
+ if (isConnected != null) IsConnected = isConnected.Value;
+ if (pitch != null) Pitch = pitch.Value;
+ if (roll != null) Roll = roll.Value;
+ if (satCount != null) SatCount = satCount.Value;
+ if (airSpeed != null) AirSpeed = airSpeed.Value;
+ if (flightDistance != null) FlightDistance = flightDistance.Value;
+ if (flightDistance2D != null) FlightDistance2D = flightDistance2D.Value;
+ if (flightTimeSpan != null) FlightTimeSpan = flightTimeSpan.Value;
+ }
+
+ public Task StartPairingAsync()
+ {
+ RaisePairingCompleted(new PairingCompletedEventArgs(true, 0, 0));
+ return TaskUtils.CompletedTask;
+ }
+
+ public Task StopPairingAsync()
+ {
+ return TaskUtils.CompletedTask;
+ }
+
+ public async Task TakeOffAsync(float alt)
+ {
+ await UnlockAsync().ConfigureAwait(false);
+ _takeOffTargetAltitude = (int)alt;
+ await TakeOffAsync().ConfigureAwait(false);
+ }
+ public Task MissionStartAsync(int hour_utc, int minute_utc, int second_utc, double Missionlng, double Missionlat)
+ {
+ return TaskUtils.CompletedTask;
+ }
+
+
+ public async Task UnlockAsync()
+ {
+ if (!IsUnlocked)
+ {
+ await SetChannelsAsync(
+ ch1: 1500,
+ ch2: 1500,
+ ch3: 1100,
+ ch4: 1500
+ ).ConfigureAwait(false);
+ if (await SetModeAsync(FlightMode.ALT_HOLD).ConfigureAwait(false))
+ {
+ IsUnlocked = true;
+ }
+ }
+ }
+
+ ///
+ /// 无论在不在紧急悬停状态下都可用的切换模式方法。
+ ///
+ /// 模式。
+ /// 超时。
+ /// 成功与否。
+ internal override Task SetModeCoreAsync(FlightMode mode, int millisecondsTimeout = 5000)
+ {
+ Mode = mode;
+ return Task.FromResult(true);
+ }
+
+ protected override void UpdateFlightDataIfNeeded()
+ {
+ //计算飞机距离，没必要
+ /*
+ if (!TakeOffPoint.IsNullOrEmpty())
+ {
+ FlightDistance = TakeOffPoint.CalcDistance(this);
+ FlightDistance2D = TakeOffPoint.CalcDistance2D(this);
+ }
+
+
+ if (FlightTimeSpan.TotalSeconds > 0)
+ {
+ if (_lastCalcSpeedPoint.IsNullOrEmpty())
+ {
+ if (!this.IsNullOrEmpty())
+ {
+ _lastCalcSpeedPoint = new PLLocation(this);
+ _lastCalcSpeedTime = DateTime.Now;
+ }
+ }
+ else
+ {
+ var movedDistance = _lastCalcSpeedPoint.CalcDistance(this);
+ var movedTime = DateTime.Now - _lastCalcSpeedTime;
+ if (movedDistance >= 3 || movedTime.TotalSeconds >= 3)
+ {
+ var airSpeed = movedDistance / movedTime.TotalSeconds;
+ if (airSpeed < 100) // 王海, 20151023, 速度过大时不正常，经纬度可能有错误。
+ {
+ AirSpeed = (float)airSpeed;
+ }
+ //Windows.Messages.Message.Show(AirSpeed.ToString());
+ GroundSpeed = AirSpeed;
+ _lastCalcSpeedPoint = new PLLocation(this);
+ _lastCalcSpeedTime = DateTime.Now;
+ }
+ }
+ }
+ */
+ }
+
+ private void MoveToPointImmediately(double lat, double lng, float alt)
+ {
+ Latitude = lat;
+ Longitude = lng;
+ Altitude = alt;
+ Roll = 0;
+ Pitch = 0;
+ }
+ public Task InjectGpsDataAsync(byte[] data, ushort length)
+ {
+ return TaskUtils.CompletedTask;
+
+ }
+
+ public static System.Drawing.Color GetRandomColor()
+ {
+ Random rand = new Random();
+ return System.Drawing.Color.FromArgb(rand.Next(256), rand.Next(256), rand.Next(256));
+ }
+
+ private DateTime led_laston;
+ private System.Drawing.Color Led_color;
+
+ //更新显示颜色，根据设置的led参数
+ private void UpdateShowColor()
+ {
+ // 使用实例化的对象调用ConvertFromString
+ //LEDShowColor = (System.Drawing.Color)converter.ConvertFromString("#" + LEDColor);
+ // 创建ColorConverter实例用于颜色转换
+ if ( rgbconverter==null) rgbconverter=new System.Drawing.ColorConverter();
+ //简化版的颜色模拟
+ switch (LEDMode)
+ {
+ case 0:
+ if (LEDColor != null)
+ LEDShowColor = (System.Drawing.Color)rgbconverter.ConvertFromString("#" + LEDColor);
+ else LEDShowColor = (System.Drawing.Color)rgbconverter.ConvertFromString("#FFFFFF");
+
+ break;
+ //闪烁
+ case 1:
+ case 2:
+ case 3:
+ case 4:
+ case 5:
+ case 6:
+ case 7:
+ case 8:
+ case 9:
+ case 10:
+ case 11:
+ case 12:
+ case 13:
+ case 14:
+
+ if (DateTime.Now.AddMilliseconds(-(LEDInterval*1000)) >= led_laston)
+ {
+ led_laston = DateTime.Now;
+ if (LEDShowColor != System.Drawing.Color.Black)
+ {
+ LEDShowColor = System.Drawing.Color.Black;
+
+ }
+ else
+ {
+ if ((LEDMode == 1) || (LEDMode == 4) || (LEDMode == 5) || (LEDMode == 6) || (LEDMode == 7) || (LEDMode == 9) || (LEDMode == 11) || (LEDMode == 13))
+ LEDShowColor = (System.Drawing.Color)rgbconverter.ConvertFromString("#" + LEDColor);
+ if ((LEDMode == 2)||(LEDMode == 3) || (LEDMode == 8) || (LEDMode == 10) || (LEDMode == 12) || (LEDMode == 14))
+ LEDShowColor = GetRandomColor();
+ }
+ }
+
+ break;
+
+ }
+
+
+
+
+
+ }
+
+ private bool ReachedDestination(double destLat, double destLng, float destAlt) => this.CalcDistance(destLat, destLng, destAlt) < 0.1;
+
+ ///
+ /// 计算并更新虚拟飞行器状态。
+ ///
+ private void Update()
+ {
+ if (IsUnlocked)
+ {
+ // 飞行器接触地面（只判断 Altitude <= 0）后，如果处于 LAND 模式或者保持 Channel3 < MIN_HOVER_CHANNEL，在一段时间后自动锁定。
+ if ((Mode == FlightMode.LAND || Channel3 < MIN_HOVER_CHANNEL) && Altitude <= 0)
+ {
+ Task.Run(async () =>
+ {
+ for (int i = 0; i < 4000 / 20; i++)
+ {
+ await TaskUtils.Delay(20).ConfigureAwait(false);
+ if ((Mode != FlightMode.LAND && Channel3 >= MIN_HOVER_CHANNEL) || !IsUnlocked)
+ {
+ return;
+ }
+ }
+ IsUnlocked = false;
+ });
+ }
+ else
+ {
+ switch (Mode)
+ {
+ case FlightMode.ACRO:
+ break;
+
+ case FlightMode.AUTO:
+ /*
+ // 暂时只管起飞。以最大速度上升，直到达到目标高度。
+ if (Altitude < _takeOffTargetAltitude)
+ {
+ Altitude += _scaledMaxMoveInInterval;
+ }
+ else
+ {
+ Mode = FlightMode.LOITER;
+ }
+ */
+ break;
+
+ case FlightMode.GUIDED:
+ // 王海, 20160317, 指点时也能体感控制若干通道。
+ //考虑不更新这个，好像没必要-xu
+ //UpdateWithChannels();
+ //UpdateWithDestination(_targetLat, _targetLng, _targetAlt);
+ if (FlightControlMode==1)
+ UpdateWithDestination_v2(_targetLat, _targetLng, _targetAlt);
+ else
+ UpdateWithDestination(_targetLat, _targetLng, _targetAlt);
+ break;
+
+ case FlightMode.STABILIZE:
+ case FlightMode.ALT_HOLD:
+ case FlightMode.LOITER:
+ UpdateWithChannels();
+ break;
+
+ case FlightMode.RTL:
+ if (Altitude < ReturnToLaunchAltitude && !ReachedDestination(Latitude, Longitude, ReturnToLaunchAltitude))
+ {
+ UpdateWithDestination(Latitude, Longitude, ReturnToLaunchAltitude);
+ }
+ else if (Altitude > ReturnToLaunchAltitude && !ReachedDestination(TakeOffPoint.Latitude, TakeOffPoint.Longitude, Altitude))
+ {
+ UpdateWithDestination(TakeOffPoint.Latitude, TakeOffPoint.Longitude, Altitude);
+ }
+ else
+ {
+ UpdateWithDestination(TakeOffPoint.Latitude, TakeOffPoint.Longitude, ReturnToLaunchAltitude);
+ }
+ break;
+
+ case FlightMode.CIRCLE:
+ break;
+
+ case FlightMode.POSITION:
+ break;
+
+ case FlightMode.LAND:
+ // 王海, 20160317, 降落时也能体感控制若干通道。
+ UpdateWithChannels();
+ // 以最大速度降落，直到高度为 0。
+ if (Altitude > 0)
+ {
+ if (Altitude > _scaledMaxMoveInInterval)
+ {
+ Altitude -= _scaledMaxMoveInInterval;
+ }
+ else
+ {
+ Altitude = 0;
+ }
+ Roll = 0;
+ Pitch = 0;
+ }
+ break;
+
+ case FlightMode.OF_LOITER:
+ break;
+
+ case FlightMode.TOY:
+ break;
+
+ default:
+ break;
+ }
+ //UpdateFlightDataIfNeeded();
+ }
+ }
+ else
+ {
+ // TODO: 王海, 20151228, 模拟空中锁定。
+ // 锁定时直接把速度设为 0。
+ AirSpeed = 0;
+ }
+
+ // UpdateShowColor();
+
+
+
+ _uiSyncContext.Post(() =>
+ {
+ //位置变化需要在UI刷新
+ RaiseLocationChangedIfNeeded();
+ //高度变化需要在UI刷新
+ // RaiseAltitudeChangedIfNeeded();
+ //GPS数据用于显示
+ // RaiseDataStreamReceived(PDataStreamType.GPS_RAW_INT);
+ //不考虑姿态
+ // RaiseAttitudeChanged();
+ });
+ }
+
+ ///
+ /// 根据各个通道值来更新位置、姿态等状态。
+ ///
+ private void UpdateWithChannels()
+ {
+ var maxMove = Mode == FlightMode.LOITER ? _scaledMaxMoveInInterval : _scaledFastMaxMoveInInterval;
+ var ch3Delta = Channel3 > MAX_HOVER_CHANNEL ? Channel3 - MAX_HOVER_CHANNEL : Channel3 < MIN_HOVER_CHANNEL ? Channel3 - MIN_HOVER_CHANNEL : 0;
+ if (ch3Delta != 0)
+ {
+ // 更新相对高度。
+ Altitude = Math.Max(0, Altitude + maxMove * ch3Delta / MAX_CHANNEL_DELTA);
+ }
+
+ var yawRad = Yaw.DegToRad();
+ var metersToLocalLng = GeographyUtils.CalcMetersToLngSpan(Latitude);
+
+ var ch1Delta = Channel1 > MAX_HOVER_CHANNEL ? Channel1 - MAX_HOVER_CHANNEL : Channel1 < MIN_HOVER_CHANNEL ? Channel1 - MIN_HOVER_CHANNEL : 0;
+ var xDistance = maxMove * ch1Delta / MAX_CHANNEL_DELTA;
+ if (ch1Delta != 0)
+ {
+ // 更新横滚方向的纬度和经度。
+ Latitude -= xDistance * Math.Sin(yawRad) * GeographyUtils.METERS_TO_LAT_SPAN;
+ Longitude += xDistance * Math.Cos(yawRad) * metersToLocalLng;
+ // 更新横滚姿态。
+ Roll = MAX_TILT_IN_FLIGHT * ch1Delta / MAX_CHANNEL_DELTA;
+ }
+ else
+ {
+ Roll = 0;
+ }
+
+ var ch2Delta = Channel2 > MAX_HOVER_CHANNEL ? Channel2 - MAX_HOVER_CHANNEL : Channel2 < MIN_HOVER_CHANNEL ? Channel2 - MIN_HOVER_CHANNEL : 0;
+ var yDistance = maxMove * ch2Delta / MAX_CHANNEL_DELTA;
+ if (ch2Delta != 0)
+ {
+ // 更新俯仰方向的纬度和经度。
+ Latitude -= yDistance * Math.Cos(yawRad) * GeographyUtils.METERS_TO_LAT_SPAN;
+ Longitude -= yDistance * Math.Sin(yawRad) * metersToLocalLng;
+ // 更新俯仰姿态。
+ Pitch = MAX_TILT_IN_FLIGHT * ch2Delta / MAX_CHANNEL_DELTA;
+ }
+ else
+ {
+ Pitch = 0;
+ }
+ }
+
+ ///
+ /// 根据目的地来更新位置、姿态等状态。
+ ///
+ private void UpdateWithDestination(ILocation loc)
+ {
+ UpdateWithDestination(loc.Latitude, loc.Longitude, loc.Altitude);
+ }
+
+ ///
+ /// 根据目的地来更新位置、姿态等状态。
+ ///
+ private void UpdateWithDestination(double lat, double lng, float alt)
+ {
+ // 与目标点之间的距离。
+ var distance = this.CalcDistance(lat, lng, alt);
+ // 距离已经很近，直接移动到目标点。
+ if (distance < _scaledMaxMoveInInterval)
+ {
+ MoveToPointImmediately(lat, lng, alt);
+ return;
+ }
+ // 在空间中的移动距离。
+ var move = _scaledMaxMoveInInterval;
+ // 竖直方向的移动距离。
+ var altDelta = (float)(move * (alt - Altitude) / distance);
+ // 更新高度。
+ Altitude += altDelta;
+ // 目标点相对于当前位置的方向。
+ // var direction = this.CalcDirection2D(lat, lng);
+ // 水平面上的移动距离。
+ var moveInHorizontalPlane = Math.Sqrt(move * move - altDelta * altDelta);
+ if (double.IsNaN(moveInHorizontalPlane))
+ {
+ MoveToPointImmediately(lat, lng, alt);
+ return;
+ }
+
+ // 更新纬度。
+ //Longitude += moveInHorizontalPlane * Math.Sin(direction) * GeographyUtils.CalcMetersToLngSpan(Latitude);
+ Longitude += moveInHorizontalPlane * _Lng_delta;
+ Latitude += moveInHorizontalPlane * _Lat_delta;
+ // 更新经度。
+ //Latitude += moveInHorizontalPlane * Math.Cos(direction) * GeographyUtils.METERS_TO_LAT_SPAN;
+ }
+ //新版本小航点计算移动位置
+ private void UpdateWithDestination_v2(double lat, double lng, float alt)
+ {
+ //_flytime 总飞行时间 秒
+ //_startTicks 开始飞行时间ms
+ // _distance_xy 米
+ // _distance_z 米
+ //当前飞行时间
+ if ((lat == Latitude) && (lng == Longitude) && (alt == Altitude)) return;
+ currflytime =(float)( DateTime.Now -_startTicks).TotalMilliseconds;//实际飞行时间 ms
+ //超时直接移动到目标点
+ if (currflytime >= _flytime)
+ {
+ MoveToPointImmediately(lat, lng, alt);
+ return;
+ }
+ //if (currflytime > 13000)
+ // return;
+ // 根据飞行时间计算飞行距离
+
+ float vspeed = 0;
+
+ GC_xy.getspeeddist(_flytype, _flytime / 1000, _distance_xy, currflytime / 1000, acc_xy,maxspeed_xy , out currdis_xy, out vspeed);
+
+ if (alt> Altitude)
+ GC_z.getspeeddist(_flytype, _flytime / 1000, _distance_z, currflytime / 1000, acc_z,maxspeed_up, out currdis_z, out vspeed);
+ else
+ GC_z.getspeeddist(_flytype, _flytime / 1000, _distance_z, currflytime / 1000, acc_z,maxspeed_down, out currdis_z, out vspeed);
+
+ Console.WriteLine($"{this.Id}：time:{currflytime / 1000} to {currdis_z}");
+
+ // 距离已经很近，直接移动到目标点。
+ if ((Math.Abs(_distance_xy-currdis_xy) < 0.1)&& (Math.Abs(_distance_z - currdis_z) < 0.1))
+ {
+ Console.WriteLine($"{this.Id} to tlat:{lat}tlng:{lng}");
+
+ MoveToPointImmediately(lat, lng, alt);
+ return;
+ }
+ // 更新位置
+ Altitude = _startAlt+ currdis_z;
+ Longitude = _startLng + currdis_xy*_Lng_delta;
+ Latitude = _startLat + currdis_xy *_Lat_delta;
+ }
+
+ }
+}