通信模块协议的添加和修改
小批量、读写通信地面站配置、修改连接id总数
This commit is contained in:
zxd
parent
e10e19aaf7
commit
d46804bc23
@ -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();
|
||||
|
||||
@ -49,6 +49,72 @@ namespace Plane.CommunicationManagement
|
||||
return GeneratePacket(MAVLink.MAVLINK_MSG_ID_RC_CHANNELS_OVERRIDE, rc);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 设置飞机上通信模块的回传功率
|
||||
/// </summary>
|
||||
/// <returns></returns>
|
||||
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);
|
||||
}
|
||||
|
||||
|
||||
/// <summary>
|
||||
/// 写入航点
|
||||
/// </summary>
|
||||
/// <param name="id">飞机ID</param>
|
||||
/// <param name="missions">航点列表</param>
|
||||
/// <returns></returns>
|
||||
public async Task<bool> WriteMissionListAsync(short id, List<IMission> 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);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 返航偏移
|
||||
/// </summary>
|
||||
/// <param name="copters"></param>
|
||||
/// <returns></returns>
|
||||
public async Task RLTOffsetAsync(float lat, float lng, IEnumerable<ICopter> 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);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// LED闪烁白灯
|
||||
/// </summary>
|
||||
@ -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<ICopter> 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);
|
||||
}
|
||||
|
||||
@ -21,6 +21,9 @@ namespace Plane.CommunicationManagement
|
||||
public event EventHandler<CommunicationConnectEventArgs> CommunicationConnected;
|
||||
private Dictionary<int, CommWriteMissinState> missionWriteState = new Dictionary<int, CommWriteMissinState>();
|
||||
private static readonly object MissinLocker = new object();
|
||||
private byte[] configData1 = null;
|
||||
private byte[] configData2 = null;
|
||||
private byte[] configurationData = null;
|
||||
|
||||
public Dictionary<int, CommWriteMissinState> 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();
|
||||
|
||||
@ -84,6 +84,21 @@ namespace Plane.Protocols
|
||||
/// </summary>
|
||||
public const short COMM_UPDATE_COPTER_MODULE = 0xFD;
|
||||
|
||||
/// <summary>
|
||||
/// 修改飞机回传功率
|
||||
/// </summary>
|
||||
public const short COMM_SET_BACK_POWER = 0x00FA;
|
||||
|
||||
/// <summary>
|
||||
/// 读取当前配置信息
|
||||
/// </summary>
|
||||
public const short COMM_GET_CURR_SYS_PARM = 0x00CC;
|
||||
|
||||
/// <summary>
|
||||
/// 改写当前配置信息
|
||||
/// </summary>
|
||||
public const short COMM_SET_CURR_SYS_PARM = 0x00EC;
|
||||
|
||||
#endregion
|
||||
|
||||
public enum CommMode
|
||||
|
||||
Loading…
Reference in New Issue
Block a user