group/Plane.Sdk3
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

1加入RTCM数据广播到指定端口功能

Browse Source 
2临时修改协议头到通用的FE，后面要改回来
This commit is contained in:
pxzleo 2023-10-19 22:14:37 +08:00
parent c72b6273b6
commit 14d1022775
2 changed files with 77 additions and 17 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

88
PlaneGcsSdk_Shared/CommunicationManagement/CommModuleGenerateMavLink.cs
View File

@ -8,6 +8,7 @@ using System.Text;
using System.Threading.Tasks;
using System.Windows.Media;
using static Plane.Copters.PlaneCopter;
using static Plane.Protocols.MAVLink;
namespace Plane.CommunicationManagement
{
@ -1128,14 +1129,84 @@ namespace Plane.CommunicationManagement
byte rtcm_tmpser = 0;
public void BroadcastGpsDataAsync(byte[] data, ushort length)
{
//广播发送RTCM数据采用专用数据可以一次发180个字节
if (!Recomisopen)
return;
MAVLink.mavlink_gps_rtcm_data_t gps = new mavlink_gps_rtcm_data_t();
var msglen = 180;
if (length > msglen * 4)
{
Windows.Messages.Message.Show($"RTCM数据太长:" + length);
return;
}
// number of packets we need, including a termination packet if needed
var nopackets = (length % msglen) == 0 ? length / msglen + 1 : (length / msglen) + 1;
if (nopackets >= 4)
nopackets = 4;
// flags = isfrag(1)/frag(2)/seq(5)
for (int a = 0; a < nopackets; a++)
{
// check if its a fragment
if (nopackets > 1)
gps.flags = 1;
else
gps.flags = 0;
// add fragment number
gps.flags += (byte)((a & 0x3) << 1);
// add seq number
gps.flags += (byte)((inject_seq_no & 0x1f) << 3);
// create the empty buffer
gps.data = new byte[msglen];
// calc how much data we are copying
int copy = Math.Min(length - a * msglen, msglen);
// copy the data
Array.Copy(data, a * msglen, gps.data, 0, copy);
// set the length
gps.len = (byte)copy;
byte[] packet = GenerateRTKPacketAsync(MAVLink.MAVLINK_MSG_ID_GPS_RTCM_DATA, gps, gps.len);
//Console.WriteLine(string.Format("{0:T} rtcm send :{1:D}", DateTime.Now, packet.Length));
try
{
RecomPort.Write(packet, 0, packet.Length);
}
catch (Exception ex)
{
Windows.Messages.Message.Show("转发端口发送失败" + ex.Message);
}
}
inject_seq_no++;
}
/// <summary>
///广播Rtk //-------------使用中的RTK广播函数------------
/// </summary>
/// <param name="data"></param>
/// <param name="length"></param>
/// <returns></returns>
public async Task InjectGpsDataAsync(byte[] data, ushort length, bool resendtocom = false)
public async Task InjectGpsDataAsync(byte[] data, ushort length)
{
//由于通讯模块限制一次只能发110个字节
if (UseTransModule)
{
MAVLink.mavlink_gps_inject_data_t gps = new MAVLink.mavlink_gps_inject_data_t();
@ -1152,23 +1223,12 @@ namespace Plane.CommunicationManagement
gps.target_component = rtcm_tmpser++;
gps.target_system = 1;
Console.WriteLine(" ["+ gps.target_component + "]总长: "+ length +" 发送第 "+ (a+1) + " 个包,长度: " + (ushort)datalen );
// Console.WriteLine(" ["+ gps.target_component + "]总长: "+ length +" 发送第 "+ (a+1) + " 个包,长度: " + (ushort)datalen );
byte[] packet = GenerateRTKPacketAsync(MAVLink.MAVLINK_MSG_ID_GPS_INJECT_DATA, gps, gps.len);
await WriteCommPacketAsync(0, MavComm.COMM_DOWNLOAD_COMM, packet).ConfigureAwait(false);
//转发到广播串口
if (resendtocom&& Recomisopen)
{
try
{
RecomPort.Write(packet, 0, packet.Length);
}
catch (Exception ex)
{
// Alert.Show("转发端口打开失败" + ex.Message);
}
}
await Task.Delay(50).ConfigureAwait(false); //没有延时得话如果超过110长度，连续发可能收不到
//重发一次，有序列号(target_component)飞机可以检测出来重复接收的

6
PlaneGcsSdk_Shared/Protocols/MAVLinkTypes.cs
View File

@ -17,9 +17,9 @@ namespace Plane.Protocols
public const int MAVLINK_LITTLE_ENDIAN = 1;
public const int MAVLINK_BIG_ENDIAN = 0;
//飞控4.0以后 254 - 240
//public const byte MAVLINK_STX = 254;
public const byte MAVLINK_STX = 240;
//协议头 飞控4.0以后 254 - 240 Mavlink头 FE 改为 F0
public const byte MAVLINK_STX = 254;
//public const byte MAVLINK_STX = 240;
public const byte MAVLINK_ENDIAN = MAVLINK_LITTLE_ENDIAN;