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