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