RTCM广播采用和通讯模块一致的MAVLINK_MSG_ID_GPS_INJECT_DATA消息,飞控响应一致
This commit is contained in:
parent
f82a285f8d
commit
2045f87a83
@ -243,6 +243,7 @@ namespace Plane.CommunicationManagement
|
|||||||
*/
|
*/
|
||||||
|
|
||||||
int times = 0;
|
int times = 0;
|
||||||
|
//这里容易报空引用异常 ,没有连接通讯设备时
|
||||||
private void Reconnect()
|
private void Reconnect()
|
||||||
{
|
{
|
||||||
// Message.Show($"正在重新连接...");
|
// Message.Show($"正在重新连接...");
|
||||||
|
@ -1203,7 +1203,9 @@ namespace Plane.CommunicationManagement
|
|||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
byte rtcm_tmpser = 0;
|
byte rtcm_tmpser = 0; //用于通讯模块的
|
||||||
|
byte rtcm_Broadser = 0;//用于广播的
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
public void BroadcastGpsDataAsync(byte[] data, ushort length)
|
public void BroadcastGpsDataAsync(byte[] data, ushort length)
|
||||||
@ -1211,7 +1213,43 @@ namespace Plane.CommunicationManagement
|
|||||||
//广播发送RTCM数据采用专用数据可以一次发180个字节
|
//广播发送RTCM数据采用专用数据可以一次发180个字节
|
||||||
if (!Recomisopen)
|
if (!Recomisopen)
|
||||||
return;
|
return;
|
||||||
|
MAVLink.mavlink_gps_inject_data_t gps = new MAVLink.mavlink_gps_inject_data_t();
|
||||||
|
var msglen = 110;
|
||||||
|
int datalen = 0;
|
||||||
|
var len = (length % msglen) == 0 ? length / msglen : (length / msglen) + 1;
|
||||||
|
for (int a = 0; a < len; a++)
|
||||||
|
{
|
||||||
|
datalen = Math.Min(length - a * msglen, msglen);
|
||||||
|
gps.data = new byte[msglen];
|
||||||
|
Array.Copy(data, a * msglen, gps.data, 0, datalen);
|
||||||
|
//gps.data[0] = rtcm_Broadser++;
|
||||||
|
gps.len = (byte)datalen;
|
||||||
|
gps.target_component = rtcm_Broadser++;
|
||||||
|
//实测一旦收到数据都是正确的,张伟已经做过crc检验了,为了兼容性不再做了
|
||||||
|
//如果需要做,飞控里面需要把注释的代码放开,并且不再兼容之前的固件
|
||||||
|
gps.target_system = 1;// checkrtrcmsum(gps.data, (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);
|
||||||
|
|
||||||
|
try
|
||||||
|
{
|
||||||
|
RecomPort.Write(packet, 0, packet.Length);
|
||||||
|
}
|
||||||
|
catch (Exception ex)
|
||||||
|
{
|
||||||
|
Windows.Messages.Message.Show("转发端口发送失败" + ex.Message);
|
||||||
|
}
|
||||||
|
|
||||||
|
// await Task.Delay(80).ConfigureAwait(false); //没有延时得话如果超过110长度,连续发可能收不到
|
||||||
|
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
/*
|
||||||
MAVLink.mavlink_gps_rtcm_data_t gps = new mavlink_gps_rtcm_data_t();
|
MAVLink.mavlink_gps_rtcm_data_t gps = new mavlink_gps_rtcm_data_t();
|
||||||
var msglen = 180;
|
var msglen = 180;
|
||||||
|
|
||||||
@ -1268,7 +1306,7 @@ namespace Plane.CommunicationManagement
|
|||||||
}
|
}
|
||||||
}
|
}
|
||||||
inject_seq_no++;
|
inject_seq_no++;
|
||||||
|
*/
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
|
Loading…
Reference in New Issue
Block a user