diff --git a/PlaneGcsSdk_Shared/CommunicationManagement/CommModule.cs b/PlaneGcsSdk_Shared/CommunicationManagement/CommModule.cs index 6928324..c65dad7 100644 --- a/PlaneGcsSdk_Shared/CommunicationManagement/CommModule.cs +++ b/PlaneGcsSdk_Shared/CommunicationManagement/CommModule.cs @@ -154,10 +154,19 @@ namespace Plane.CommunicationManagement break; case MavComm.COMM_GET_COPTERS_COUNT: - dealData = packet.Skip(16).ToArray(); + dealData = packet.Skip(18).ToArray(); AnalyzeCopterInfosPacket(dealData); break; + case MavComm.COMM_GET_CURR_SYS_PARM: + dealData = packet.Skip(18).ToArray(); + Analyze_curr_sys_parm_Packet(dealData); + break; + + case MavComm.COMM_SET_CURR_SYS_PARM: + + break; + case MavComm.COMM_NOTIFICATION: short messageType = BitConverter.ToInt16(packet, 10); dealData = packet.Skip(12).ToArray(); diff --git a/PlaneGcsSdk_Shared/CommunicationManagement/CommModuleGenerateMavLink.cs b/PlaneGcsSdk_Shared/CommunicationManagement/CommModuleGenerateMavLink.cs index c5333c0..962bc18 100644 --- a/PlaneGcsSdk_Shared/CommunicationManagement/CommModuleGenerateMavLink.cs +++ b/PlaneGcsSdk_Shared/CommunicationManagement/CommModuleGenerateMavLink.cs @@ -49,6 +49,72 @@ namespace Plane.CommunicationManagement return GeneratePacket(MAVLink.MAVLINK_MSG_ID_RC_CHANNELS_OVERRIDE, rc); } + /// + /// 设置飞机上通信模块的回传功率 + /// + /// + public async Task SetCopterCommBackPower(short id, byte power) + { + byte[] data = new byte[12]; + short num = (0x5 << 12) ^ 1; + byte frequencyPoint = 16; + int frequencyValue = 0x50; + + Array.Copy(BitConverter.GetBytes(num), 0, data, 0, 2); //广播个数 + Array.Copy(BitConverter.GetBytes(id), 0, data, 2, 2); //小批量 当前只对1个采用小批量模式广播 + data[4] = 0x00; + data[5] = 0xFC; + data[6] = power; + data[7] = frequencyPoint; + Array.Copy(BitConverter.GetBytes(frequencyValue), 0, data, 8, 4); + + await WriteCommPacketAsync(0, MavComm.COMM_SET_BACK_POWER, data).ConfigureAwait(false); + } + + public async Task GetCurr_Sys_Parm() + { + configurationData = null; + configData1 = null; + configData2 = null; + + byte[] data = new byte[2]; + data = BitConverter.GetBytes((short)0x0000); + await WriteCommPacketAsync(0, MavComm.COMM_GET_CURR_SYS_PARM, data).ConfigureAwait(false); + + await Task.Delay(100); + + data = BitConverter.GetBytes((short)0x0001); + await WriteCommPacketAsync(0, MavComm.COMM_GET_CURR_SYS_PARM, data).ConfigureAwait(false); + } + + public async Task SetCurr_Sys_Parm() + { + if (configurationData == null) return; + + byte[] data = new byte[configurationData.Length + 2]; + + Array.Copy(BitConverter.GetBytes((short)configurationData.Length), data, 2); + Array.Copy(configurationData,0, data, 2, configurationData.Length); + + await WriteCommPacketAsync(0, MavComm.COMM_SET_CURR_SYS_PARM, data).ConfigureAwait(false); + } + + public async Task SetCommCount(short count, short startNum) + { + byte[] data = new byte[4]; + Array.Copy(BitConverter.GetBytes(count), 0, data, 0, 2); + Array.Copy(BitConverter.GetBytes(startNum), 0, data, 2, 2); + + await WriteCommPacketAsync(0, MavComm.COMM_SET_MAV_COUNT, data).ConfigureAwait(false); + } + + + /// + /// 写入航点 + /// + /// 飞机ID + /// 航点列表 + /// public async Task WriteMissionListAsync(short id, List missions) { missions.Insert(0, PLCopter.PRE_TAKE_OFF_MISSION); @@ -160,6 +226,22 @@ namespace Plane.CommunicationManagement await WriteCommPacketAsync(copterId, MavComm.COMM_DOWNLOAD_COMM, data, batchPacket).ConfigureAwait(false); } + /// + /// 返航偏移 + /// + /// + /// + public async Task RLTOffsetAsync(float lat, float lng, IEnumerable copters = null) + { + short copterId = 0; + byte[] batchPacket = null; + GetCopterIds(copters, out copterId, out batchPacket); + + byte[] data = DoCommandAsync(MAVLink.MAV_CMD.DO_SET_HOME, 2, 0, 0, lat, lng, 0, 0); + + await WriteCommPacketAsync(copterId, MavComm.COMM_DOWNLOAD_COMM, data, batchPacket).ConfigureAwait(false); + } + /// /// LED闪烁白灯 /// @@ -691,7 +773,15 @@ namespace Plane.CommunicationManagement else BatchByte = BatchByte.Concat(tempByte).ToArray(); } - return BatchByte; + short byteNum = (short)(BatchByte.Length / 2); + short length = (short)((0x5 << 12) ^ byteNum); + byte[] ret = new byte[2+ BatchByte.Length]; + + Array.Copy(BitConverter.GetBytes(length), ret, 2); + + Array.Copy(BatchByte, 0, ret, 2, BatchByte.Length); + + return ret; } private void GetCopterIds(IEnumerable copters, out short copterId, out byte[] batchPacket) @@ -701,9 +791,10 @@ namespace Plane.CommunicationManagement if (copters != null) { - if (copters.Count() == 1) - copterId = short.Parse(copters.FirstOrDefault().Id); - else + //为提高通信稳定性,无论1架还是多架全部改为小批量广播 + //if (copters.Count() == 1) + // copterId = short.Parse(copters.FirstOrDefault().Id); + //else { batchPacket = BatchCopterIdData(copters); } diff --git a/PlaneGcsSdk_Shared/CommunicationManagement/CommModulePacketAnalysis.cs b/PlaneGcsSdk_Shared/CommunicationManagement/CommModulePacketAnalysis.cs index dca8b7b..6ed9b9d 100644 --- a/PlaneGcsSdk_Shared/CommunicationManagement/CommModulePacketAnalysis.cs +++ b/PlaneGcsSdk_Shared/CommunicationManagement/CommModulePacketAnalysis.cs @@ -21,6 +21,9 @@ namespace Plane.CommunicationManagement public event EventHandler CommunicationConnected; private Dictionary missionWriteState = new Dictionary(); private static readonly object MissinLocker = new object(); + private byte[] configData1 = null; + private byte[] configData2 = null; + private byte[] configurationData = null; public Dictionary MissionWriteState { @@ -138,6 +141,8 @@ namespace Plane.CommunicationManagement } + + private void AnalyzeCopterInfosPacket(byte[] buffer) { ushort beginNum = BitConverter.ToUInt16(buffer, 0); @@ -175,6 +180,35 @@ namespace Plane.CommunicationManagement } } + private void Analyze_curr_sys_parm_Packet(byte[] buffer) + { + Message.Show($"buffer length = {buffer.Length}"); + short num = BitConverter.ToInt16(buffer, 0); + short offset = BitConverter.ToInt16(buffer, 2); + short curLength = BitConverter.ToInt16(buffer, 4); + short allLength = BitConverter.ToInt16(buffer, 6); + + if (num == 0) + { + configData1 = new byte[curLength]; + Array.Copy(buffer, 8, configData1, 0, curLength); + Message.Show($"part0 长度={curLength}, 总长{allLength}"); + } + else if (num == 1) + { + configData2 = new byte[curLength]; + Array.Copy(buffer, 8, configData2, 0, curLength); + Message.Show($"part1 长度={curLength}, 总长{allLength}"); + } + + if (configData1 != null && configData2 != null) + { + Message.Show("合成数据"); + configurationData = new byte[configData1.Length + configData2.Length]; + configurationData = configData1.Concat(configData2).ToArray(); + } + } + private void AnalyzeComm4RetrunPacket(short copterId, byte[] buffer) { byte[] packet = buffer.Take(28).ToArray(); diff --git a/PlaneGcsSdk_Shared/Protocols/MavCommTypes.cs b/PlaneGcsSdk_Shared/Protocols/MavCommTypes.cs index 1006063..4f64b95 100644 --- a/PlaneGcsSdk_Shared/Protocols/MavCommTypes.cs +++ b/PlaneGcsSdk_Shared/Protocols/MavCommTypes.cs @@ -84,6 +84,21 @@ namespace Plane.Protocols /// public const short COMM_UPDATE_COPTER_MODULE = 0xFD; + /// + /// 修改飞机回传功率 + /// + public const short COMM_SET_BACK_POWER = 0x00FA; + + /// + /// 读取当前配置信息 + /// + public const short COMM_GET_CURR_SYS_PARM = 0x00CC; + + /// + /// 改写当前配置信息 + /// + public const short COMM_SET_CURR_SYS_PARM = 0x00EC; + #endregion public enum CommMode