diff --git a/PlaneGcsSdk.Contract_Shared/Copters/ICopterAttribute.cs b/PlaneGcsSdk.Contract_Shared/Copters/ICopterAttribute.cs index 5004970..e66a729 100644 --- a/PlaneGcsSdk.Contract_Shared/Copters/ICopterAttribute.cs +++ b/PlaneGcsSdk.Contract_Shared/Copters/ICopterAttribute.cs @@ -14,5 +14,7 @@ namespace Plane.Copters /// float GroundAlt { get; set; } float RetainInt{ get; } + //模拟飞行更新间隔 + int sim_update_int { get; set; } } } diff --git a/PlaneGcsSdk_Shared/CommunicationManagement/CommModule.cs b/PlaneGcsSdk_Shared/CommunicationManagement/CommModule.cs index e4a0919..e6efbb7 100644 --- a/PlaneGcsSdk_Shared/CommunicationManagement/CommModule.cs +++ b/PlaneGcsSdk_Shared/CommunicationManagement/CommModule.cs @@ -353,6 +353,25 @@ namespace Plane.CommunicationManagement 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); + + } + + + + bool temp = false; //测试用 灯光间隔1S闪烁 public async Task TestLED(short id, string colorString) @@ -363,7 +382,7 @@ namespace Plane.CommunicationManagement 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_len = 4; //命令类型——测试灯光 led.custom_bytes = new byte[24]; if (colorString == "") colorString = "330033"; @@ -421,7 +440,7 @@ namespace Plane.CommunicationManagement temp = !temp; while (temp) { - Message.Show("长度 = " + senddata.Length); + Message.Show("测试灯光,长度 = " + senddata.Length); await WriteCommPacketAsync(id, MavComm.COMM_DOWNLOAD_COMM, senddata); await Task.Delay(1000).ConfigureAwait(false); } @@ -436,7 +455,7 @@ namespace Plane.CommunicationManagement 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_len = 1;//命令类型——测试拉烟 led.custom_bytes = new byte[24]; led.custom_bytes[0] = (byte)channel; @@ -490,6 +509,77 @@ namespace Plane.CommunicationManagement } } + + 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并且发送 /// @@ -524,36 +614,37 @@ namespace Plane.CommunicationManagement 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); //数据头 + 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); //数据长度 + 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); //流水号 + 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); //飞机号 + 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); //命令类型 + Array.Copy(buffer_messageType, 0, packet, 8, 2); //命令类型 2字节 if (data != null) - Array.Copy(data, 0, packet, 10, data.Length); //数据内容 + Array.Copy(data, 0, packet, 10, data.Length); //数据内容 10字节开始 - byte[] buffer_CRC = checkCRC16(packet); + byte[] buffer_CRC = checkCRC16(packet); //计算所有数据的crc - byte[] buffer_packet = new byte[packet.Length + 2]; + 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); + Array.Copy(buffer_CRC, 0, buffer_packet, packet.Length, 2); //带crc的数据包 if (Connection != null && Connection.IsOnline) { @@ -571,10 +662,12 @@ namespace Plane.CommunicationManagement 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; diff --git a/PlaneGcsSdk_Shared/CommunicationManagement/CommModuleGenerateMavLink.cs b/PlaneGcsSdk_Shared/CommunicationManagement/CommModuleGenerateMavLink.cs index 06ac0c5..7121630 100644 --- a/PlaneGcsSdk_Shared/CommunicationManagement/CommModuleGenerateMavLink.cs +++ b/PlaneGcsSdk_Shared/CommunicationManagement/CommModuleGenerateMavLink.cs @@ -212,7 +212,7 @@ namespace Plane.CommunicationManagement led.target_component = 1;//(byte)MAVLink.MAV_COMPONENT.MAV_COMP_ID_SYSTEM_CONTROL;; led.instance = 0; led.pattern = 0; - led.custom_len = 6; + led.custom_len = 6; //测试LED灯光 led.custom_bytes = new byte[24]; led.custom_bytes[0] = cR; led.custom_bytes[1] = cG; diff --git a/PlaneGcsSdk_Shared/Copters/CopterImplSharedPart.cs b/PlaneGcsSdk_Shared/Copters/CopterImplSharedPart.cs index 142d57c..7c387e1 100644 --- a/PlaneGcsSdk_Shared/Copters/CopterImplSharedPart.cs +++ b/PlaneGcsSdk_Shared/Copters/CopterImplSharedPart.cs @@ -152,6 +152,8 @@ namespace Plane.Copters private float _GroundAlt = 0; + private int _sim_update_int = 50; + private CopterState _State; private string _StatusText; @@ -660,6 +662,17 @@ namespace Plane.Copters } + public int sim_update_int + { + get { return _sim_update_int; } + set + { + Set(nameof(sim_update_int), ref _sim_update_int, value); + } + } + + + public byte[] Retain { get { return _Retain; } diff --git a/PlaneGcsSdk_Shared/Copters/FakeCopter.cs b/PlaneGcsSdk_Shared/Copters/FakeCopter.cs index 917a514..459c39a 100644 --- a/PlaneGcsSdk_Shared/Copters/FakeCopter.cs +++ b/PlaneGcsSdk_Shared/Copters/FakeCopter.cs @@ -18,21 +18,24 @@ namespace Plane.Copters /// 心跳间隔,单位为毫秒。 /// private const int HEARTBEAT_INTERVAL = 200; + /// + /// 更新虚拟飞行器状态的时间间隔,单位为毫秒。 + /// + private int UPDATE_INTERVAL = 50; //默认100, i7电脑 150可以跑1000架 100可以跑500架 50可以200-300架 /// /// 在一个更新间隔中的最大移动距离。 /// - private const float MAX_MOVE_IN_INTERVAL = MAX_VEL * UPDATE_INTERVAL / 1000; + private float MAX_MOVE_IN_INTERVAL = MAX_VEL * 50 / 1000; /// /// 高速模式下,在一个更新间隔中的最大移动距离。 /// - private const float MAX_MOVE_IN_INTERVAL_FAST = MAX_MOVE_IN_INTERVAL * 4; + private float MAX_MOVE_IN_INTERVAL_FAST = MAX_VEL * 50 / 1000 * 4; //MAX_MOVE_IN_INTERVAL*4 - /// - /// 更新虚拟飞行器状态的时间间隔,单位为毫秒。 - /// - private const int UPDATE_INTERVAL = 50; //默认100, i7电脑 150可以跑1000架 100可以跑500架 50可以200-300架 + + + // private int _UPDATE_INTERVAL = 50; /// /// 对飞行器的模拟是否正在运行。 @@ -52,12 +55,12 @@ namespace Plane.Copters /// /// 高速模式下,按比例缩放过的在一个更新间隔中的最大移动距离。 /// - private float _scaledFastMaxMoveInInterval = MAX_MOVE_IN_INTERVAL_FAST; + private float _scaledFastMaxMoveInInterval = MAX_VEL * 50 / 1000 * 4; //MAX_MOVE_IN_INTERVAL_FAST /// /// 按比例缩放过的在一个更新间隔中的最大移动距离。 /// - private float _scaledMaxMoveInInterval = MAX_MOVE_IN_INTERVAL; + private float _scaledMaxMoveInInterval = MAX_VEL * 50 / 1000; //MAX_MOVE_IN_INTERVAL /// /// 速度缩放比例。 @@ -91,12 +94,31 @@ namespace Plane.Copters private bool _ShowLED; + + /// /// 使用 创建 实例。 /// 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; diff --git a/PlaneGcsSdk_Shared/Protocols/MavCommTypes.cs b/PlaneGcsSdk_Shared/Protocols/MavCommTypes.cs index 63686bf..242b8d9 100644 --- a/PlaneGcsSdk_Shared/Protocols/MavCommTypes.cs +++ b/PlaneGcsSdk_Shared/Protocols/MavCommTypes.cs @@ -84,6 +84,15 @@ namespace Plane.Protocols /// public const short COMM_UPDATE_COPTER_MODULE = 0xFD; + + + /// + /// 测试模块 + /// + public const short COMM_TEST_MODULE = 0x3C; + + + #endregion public enum CommMode