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

2将降低广播数据作为可选项在发送界面(只影响通讯模块,不影响端口转发)
3修改一个严重BUG:
   RTCM协议解析BUG,这个BUG是因为F9P地面站有UBX协议数据和RTCM协议数据,由于之前没有解析UBX数据导致RTCM数据也解析不了,导致3秒才发一次RTCM数据到飞机
This commit is contained in:
pxzleo 2023-10-19 22:12:53 +08:00
parent 1403000ef9
commit 79669581f1
7 changed files with 1719 additions and 1069 deletions

View File

@ -22,6 +22,10 @@ namespace Plane.FormationCreator.Formation
public class RtcmManager: ObservableObject
{
private rtcm3 rtcm = new rtcm3();
// ubx detection
private static Ubx ubx_m8p = new Ubx();
static nmea nmea = new nmea();
private Hashtable msgseen = new Hashtable();
private ICommsSerial comPort;
@ -31,17 +35,20 @@ namespace Plane.FormationCreator.Formation
private ControlPanelViewModel ControlPanelVM = ServiceLocator.Current.GetInstance<ControlPanelViewModel>();
private CopterManager _copterManager = ServiceLocator.Current.GetInstance<CopterManager>();
private bool _rtcmthreadrun = false;
private bool _smallrtcmdata = false; //减少传输数据--用于带宽不够的通讯模块-对数传广播无效
private bool _enrecom = false;//是否转发到另外串口
// rtcm发送类型0:直接发送,
// 11秒只发一种卫星1秒发GPS,第2秒发北斗第3秒发格洛纳斯其他数据随来随发
private int _rtcmsendtype = 1;
private int _resendtocom = 1;
const int MSG_GPS = 1074;
const int MSG_GLONASS = 1084;
const int MSG_Beidou = 1124;
const int MSG_BasePos = 1005;
const int MSG_REV = 1033;
const int MSG_GLONASS_PHASE = 1230;
public string rtcm_typename(int msgtype)
{
@ -54,6 +61,9 @@ namespace Plane.FormationCreator.Formation
case MSG_GLONASS:
typename = "GLONASS";
break;
case MSG_GLONASS_PHASE:
typename = "GLONASS相位差";
break;
case MSG_Beidou:
typename = "北斗";
break;
@ -84,7 +94,18 @@ namespace Plane.FormationCreator.Formation
}
}
public bool SmallRtcmData
{
get { return _smallrtcmdata; }
set
{
Set(nameof(SmallRtcmData), ref _smallrtcmdata, value);
}
}
//接收速率
private int bps;
//发送速率
private int bpsusefull;
private int _bps = 0;
@ -180,7 +201,7 @@ namespace Plane.FormationCreator.Formation
dispatcherTimer.Start();
}
//用于刷新界面,显示消息状态,基站和卫星是否有效等
//用于刷新界面,显示消息状态,基站和卫星是否有效等---1HZ
private void OnTimedEvent(object sender, EventArgs e)
{
BaseState = baseTime > DateTime.Now;
@ -386,6 +407,8 @@ namespace Plane.FormationCreator.Formation
//await _commModuleManager.StartRtcmLoop();
while (Rtcmthreadrun)
{
// Console.WriteLine(string.Format("{0:T} 1", DateTime.Now));
try
{
try
@ -405,97 +428,151 @@ namespace Plane.FormationCreator.Formation
}
}
catch { }
if (!comPort.IsOpen)
{
return;
}
//rtcm size=180
byte[] buffer = new byte[180];// new byte[180];
//一次读取180个字节
int read = comPort.Read(buffer, 0, Math.Min(buffer.Length, comPort.BytesToRead));
if (read > 0)
/////////////////////////////////重写发送RTCM数据部分---23/10/18 by panxu
///
// limit to 180 byte packet if using new packet
byte[] buffer = new byte[180];
while (comPort.BytesToRead > 0)
{
lastrecv = DateTime.Now;
}
bps += read;
for (int a = 0; a < read; a++)
{
int seenmsg = -1;
// rtcm类解析RTCM数据 如果 rtcm.Read>0解析成功返回消息号码-1解析失败
if ((seenmsg = rtcm.Read(buffer[a])) > 0)
int read = comPort.Read(buffer, 0, Math.Min(buffer.Length, comPort.BytesToRead));
Console.WriteLine(string.Format("{0:T} - read:{1:D}", DateTime.Now, read));
if (read > 0)
lastrecv = DateTime.Now;
bps += read; //接收速率bps,byte需要*8到bit
// check for valid rtcm/sbp/ubx packets
for (int a = 0; a < read; a++)
{
bpsusefull += rtcm.length;
string msgshowname = rtcm_typename(seenmsg);
_commModuleManager.SetAllCoptersForWifi(_copterManager.Copters);
if (_rtcmsendtype == 0)
int seenmsg = -1;
// rtcm and not can
if ((seenmsg = rtcm.Read(buffer[a])) > 0)
{
await _commModuleManager.InjectGpsDataAsync(rtcm.packet, (ushort)rtcm.length);
}else
if (_rtcmsendtype == 1)
{
Ensend = false;
//第1秒是gps,第2秒是GLONASS第3秒是 Beidou
//1秒内可能发送多个同一种类型的数据看数据大小和原始频率
long curr_s = CurrentTimeMillis()/1000;
int sendtype = (int) (curr_s % 3);
switch (seenmsg)
// Console.WriteLine(string.Format("{0:T}------------RTCM Send {1:D}", DateTime.Now, rtcm.length));
ubx_m8p.resetParser();
nmea.resetParser();
string msgshowname = rtcm_typename(seenmsg);
//广播数据直接发送不受小带宽数据选项影响
if (_enrecom)
{
case MSG_GPS:
if (sendtype==0) //第1秒是gps,
Ensend = true;
break;
Console.WriteLine(DateTime.Now.ToString("HH:mm:ss") + "--广播Rtcm长度: " + (ushort)rtcm.length + " 类型: " + msgshowname + " (" + seenmsg + ")");
_commModuleManager.BroadcastGpsDataAsync(rtcm.packet, (ushort)rtcm.length);
}
case MSG_GLONASS:
if (sendtype == 1) //第2秒是GLONASS
Ensend = true;
break;
case MSG_Beidou:
if (sendtype == 2) //第3秒是 Beidou
Ensend = true;
break;
//位置4秒发一次就可以了
case MSG_BasePos:
{
long curr_send_pos = CurrentTimeMillis();
if ((curr_send_pos - last_send_pos) > 4000)
{
last_send_pos = curr_send_pos;
//直接发送
if (!SmallRtcmData)
{
//发送到飞机
Console.WriteLine(DateTime.Now.ToString("HH:mm:ss") + "--通讯模块发送Rtcm长度: " + (ushort)rtcm.length + " 类型: " + msgshowname + " (" + seenmsg + ")");
await _commModuleManager.InjectGpsDataAsync(rtcm.packet, (ushort)rtcm.length);
//累加消息数量,用于界面显示
bpsusefull += rtcm.length;
}
else
//为了减少发送数据量:
//11秒只发一种卫星1秒发GPS,第2秒发北斗第3秒发格洛纳斯其他数据随来随发
{
Ensend = false; //是否发送
//第1秒是gps,第2秒是GLONASS第3秒是 Beidou
//1秒内可能发送多个同一种类型的数据看数据大小和原始频率
long curr_s = CurrentTimeMillis() / 1000;
int sendtype = (int)(curr_s % 3);
switch (seenmsg)
{
case MSG_GPS:
if (sendtype == 0) //第1秒是gps,
Ensend = true;
break;
case MSG_GLONASS:
case MSG_GLONASS_PHASE:
if (sendtype == 1) //第2秒是GLONASS
Ensend = true;
break;
case MSG_Beidou:
if (sendtype == 2) //第3秒是 Beidou
Ensend = true;
break;
//位置4秒发一次就可以了
case MSG_BasePos:
{
long curr_send_pos = CurrentTimeMillis();
if ((curr_send_pos - last_send_pos) > 4000)
{
last_send_pos = curr_send_pos;
Ensend = true;
}
}
}
break;
case MSG_REV: //这个没必要发
Ensend = false;
break;
default: //其他类型数据随到随发
Ensend = true;
break;
}
if (Ensend)
{
// Plane.Windows.Messages.Message.Show(DateTime.Now.ToString("HH:mm:ss")+"--Rtcm长度: " + (ushort)rtcm.length+" , 类型: "+ seenmsg);
Console.WriteLine(DateTime.Now.ToString("HH:mm:ss") + "--Rtcm长度: " + (ushort)rtcm.length + " 类型: " + msgshowname + " ("+ seenmsg+")");
await _commModuleManager.InjectGpsDataAsync(rtcm.packet, (ushort)rtcm.length, _enrecom);
}
//else
// Console.WriteLine(DateTime.Now.ToString("HH:mm:ss") + "--放弃发送-----Rtcm长度: " + (ushort)rtcm.length + " 类型: " + msgshowname + " (" + seenmsg + ")");
break;
case MSG_REV: //这个没必要发
Ensend = false;
break;
default: //其他类型数据随到随发
Ensend = true;
break;
}
if (Ensend)
{
Console.WriteLine(DateTime.Now.ToString("HH:mm:ss") + "--通讯模块发送Rtcm长度: " + (ushort)rtcm.length + " 类型: " + msgshowname + " (" + seenmsg + ")");
await _commModuleManager.InjectGpsDataAsync(rtcm.packet, (ushort)rtcm.length);
//累加消息数量,用于界面显示
bpsusefull += rtcm.length;
}
}
string msgname = rtcm_typename(seenmsg);
if (!msgseen.ContainsKey(msgname))
msgseen[msgname] = 0;
msgseen[msgname] = (int)msgseen[msgname] + 1;
//检测基站定位状态和卫星定位状态,用于在界面上显示
await ExtractBasePos(seenmsg);
}
// ubx
if ((seenmsg = ubx_m8p.Read(buffer[a])) > 0)
{
Console.WriteLine(string.Format("{0:T}----------------------------ubx_m8p ", DateTime.Now));
rtcm.resetParser();
nmea.resetParser();
/*
//Ubx协议数据不用显示
string msgname = "Ubx" + seenmsg.ToString("X4");
if (!msgseen.ContainsKey(msgname))
msgseen[msgname] = 0;
msgseen[msgname] = (int)msgseen[msgname] + 1;
*/
}
// nmea
if ((seenmsg = nmea.Read(buffer[a])) > 0)
{
Console.WriteLine(string.Format("{0:T}----------------------------nmea ", DateTime.Now));
rtcm.resetParser();
ubx_m8p.resetParser();
/*
//NMEA协议数据不用显示
string msgname = "NMEA";
if (!msgseen.ContainsKey(msgname))
msgseen[msgname] = 0;
msgseen[msgname] = (int)msgseen[msgname] + 1;
*/
}
//累加消息数量,用于界面显示
if (!msgseen.ContainsKey(msgshowname))
msgseen[msgshowname] = 0;
msgseen[msgshowname] = (int)msgseen[msgshowname] + 1;
//检测基站定位状态和卫星定位状态,用于在界面上显示
await ExtractBasePos(seenmsg);
}
////////////////////////////解析/发送RTCM结束
await Task.Delay(10);
}
await Task.Delay(10);
}
catch(Exception ex)
catch (Exception ex)
{
Plane.Windows.Messages.Message.Show(ex.Message);
}

View File

@ -211,10 +211,12 @@
</Compile>
<Compile Include="Util\ICommsSerial.cs" />
<Compile Include="Util\ICorrections.cs" />
<Compile Include="Util\nmea.cs" />
<Compile Include="Util\OptimizeRoute.cs" />
<Compile Include="Util\ParamFile.cs" />
<Compile Include="Util\PasswordBoxHelper.cs" />
<Compile Include="Util\rtcm3.cs" />
<Compile Include="Util\ubx_m8p.cs" />
<Compile Include="Util\VersionControl.cs" />
<Compile Include="ViewModels\CalibrationViewModel.cs" />
<Compile Include="ViewModels\ConfigVirtualIdViewModel.cs" />

View File

@ -0,0 +1,123 @@
using System;
using uint8_t = System.Byte;
using uint16_t = System.UInt16;
using int32_t = System.Int32;
using uint32_t = System.UInt32;
using int8_t = System.SByte;
using System.Runtime.InteropServices;
namespace Plane.Util
{
public class nmea : ICorrections
{
int step = 0;
public byte[] buffer = new byte[1024 * 1];
int payloadlen = 0;
int msglencount = 0;
public int length
{
get
{
return 2 + 2 + 2 + 2 + payloadlen; // header2, class,subclass,length2,data,crc2
}
}
public byte[] packet
{
get
{
return buffer;
}
}
public bool resetParser()
{
step = 0;
return true;
}
public int Read(byte data)
{
switch (step)
{
default:
case 0:
if (data == '$')
{
step = 1;
msglencount = 0;
buffer[0] = data;
}
break;
case 1:
if (data == 'G')
{
buffer[1] = data;
step++;
}
else
step = 0;
break;
case 2:
if (msglencount > 1000)
{
step = 0;
}
buffer[msglencount + 2] = data;
msglencount++;
if(data == '\n')
{
var line = System.Text.ASCIIEncoding.ASCII.GetString(buffer, 0, msglencount + 2);
string[] items = line.Trim().Split(',', '*');
if (items[items.Length-1] == GetChecksum(line))
{
return 1;
}
step = 0;
}
break;
}
return -1;
}
// Calculates the checksum for a sentence
string GetChecksum(string sentence)
{
// Loop through all chars to get a checksum
int Checksum = 0;
foreach (char Character in sentence.ToCharArray())
{
switch (Character)
{
case '$':
// Ignore the dollar sign
break;
case '*':
// Stop processing before the asterisk
return Checksum.ToString("X2");
default:
// Is this the first value for the checksum?
if (Checksum == 0)
{
// Yes. Set the checksum to the value
Checksum = Convert.ToByte(Character);
}
else
{
// No. XOR the checksum with this character's value
Checksum = Checksum ^ Convert.ToByte(Character);
}
break;
}
}
// Return the checksum formatted as a two-character hexadecimal
return Checksum.ToString("X2");
}
}
}

File diff suppressed because it is too large Load Diff

View File

@ -0,0 +1,573 @@
using System;
using uint8_t = System.Byte;
using uint16_t = System.UInt16;
using int32_t = System.Int32;
using uint32_t = System.UInt32;
using int8_t = System.SByte;
using System.Runtime.InteropServices;
using System.Threading;
namespace Plane.Util
{
public class Ubx : ICorrections
{
int step = 0;
public byte[] buffer = new byte[1024 * 8];
int payloadlen = 0;
int msglencount = 0;
public byte @class
{
get { return buffer[2]; }
}
public byte subclass
{
get { return buffer[3]; }
}
public int length
{
get
{
return 2 + 2 + 2 + 2 + payloadlen; // header2, class,subclass,length2,data,crc2
}
}
public byte[] packet
{
get { return buffer; }
}
public bool resetParser()
{
step = 0;
return true;
}
public int Read(byte data)
{
switch (step)
{
default:
case 0:
if (data == 0xb5)
{
step = 1;
buffer[0] = data;
}
break;
case 1:
if (data == 0x62)
{
buffer[1] = data;
step++;
}
else
step = 0;
break;
case 2:
buffer[2] = data;
step++;
break;
case 3:
buffer[3] = data;
step++;
break;
case 4:
buffer[4] = data;
payloadlen = data;
step++;
break;
case 5:
buffer[5] = data;
step++;
payloadlen += (data << 8);
msglencount = 0;
// reset on oversize packet
if (payloadlen > buffer.Length)
step = 0;
break;
case 6:
if (msglencount < (payloadlen))
{
buffer[msglencount + 6] = data;
msglencount++;
if (msglencount == payloadlen)
step++;
}
break;
case 7:
buffer[msglencount + 6] = data;
step++;
break;
case 8:
buffer[msglencount + 6 + 1] = data;
var crc = ubx_checksum(buffer, payloadlen + 6);
var crcpacket = new byte[] { buffer[msglencount + 6], data };
if (crc[0] == crcpacket[0] && crc[1] == crcpacket[1])
{
step = 0;
return (@class << 8) + subclass;
}
step = 0;
break;
}
return -1;
}
private static byte[] ubx_checksum(byte[] packet, int size, int offset = 2)
{
uint a = 0x00;
uint b = 0x00;
var i = offset;
while (i < size)
{
a += packet[i++];
b += a;
}
var ans = new byte[2];
ans[0] = (byte)(a & 0xFF);
ans[1] = (byte)(b & 0xFF);
return ans;
}
public static byte[] generate(byte cl, byte subclass, byte[] payload)
{
var data = new byte[2 + 2 + 2 + 2 + payload.Length];
data[0] = 0xb5;
data[1] = 0x62;
data[2] = cl;
data[3] = subclass;
data[4] = (byte)(payload.Length & 0xff);
data[5] = (byte)((payload.Length >> 8) & 0xff);
Array.ConstrainedCopy(payload, 0, data, 6, payload.Length);
var checksum = ubx_checksum(data, data.Length - 2);
data[data.Length - 2] = checksum[0];
data[data.Length - 1] = checksum[1];
return data;
}
[StructLayout(LayoutKind.Sequential, Pack = 1)]
public struct ubx_mon_ver
{
[MarshalAs(UnmanagedType.ByValArray, SizeConst = 30)]
public byte[] swVersion;
[MarshalAs(UnmanagedType.ByValArray, SizeConst = 10)]
public Byte[] hwVersion;
[MarshalAs(UnmanagedType.ByValArray, SizeConst = 30)]
public byte[] extension;
}
[StructLayout(LayoutKind.Sequential, Pack = 1)]
public struct ubx_mon_hw
{
public int pinSel;
public int pinBank;
public int pinDir;
public int pinVal;
public ushort noisePerMS;
public ushort agcCnt;
public byte aStatus;
public byte aPower;
public byte flags;
public byte reserved1;
public int usedMask;
[MarshalAs(UnmanagedType.ByValArray, SizeConst = 17)]
public byte[] VP;
public byte jamInd;
public ushort reserved3;
public int pinIrq;
public int pullH;
public int pullL;
}
[StructLayout(LayoutKind.Sequential, Pack = 1)]
public struct ubx_nav_pvt
{
public uint32_t itow;
public uint16_t year;
public uint8_t month, day, hour, min, sec;
public uint8_t valid;
public uint32_t t_acc;
public int32_t nano;
public uint8_t fix_type;
public uint8_t flags;
public uint8_t flags2;
public uint8_t num_sv;
public int32_t lon, lat;
public int32_t height, h_msl;
public uint32_t h_acc, v_acc;
public int32_t velN, velE, velD, gspeed;
public int32_t head_mot;
public uint32_t s_acc;
public uint32_t head_acc;
public uint16_t p_dop;
[MarshalAs(UnmanagedType.ByValArray, SizeConst = 6)]
public uint8_t[] reserved1;
public uint32_t headVeh;
[MarshalAs(UnmanagedType.ByValArray, SizeConst = 4)]
public uint8_t[] reserved2;
}
[StructLayout(LayoutKind.Sequential, Pack = 1)]
public struct ubx_nav_svin
{
public uint8_t version;
[MarshalAs(UnmanagedType.ByValArray, SizeConst = 3)]
public uint8_t[] reserved1;
public uint32_t iTOW;
public uint32_t dur;
public int32_t meanX;
public int32_t meanY;
public int32_t meanZ;
public int8_t meanXHP;
public int8_t meanYHP;
public int8_t meanZHP;
public uint8_t reserved2;
public uint32_t meanAcc;
public uint32_t obs;
public uint8_t valid;
public uint8_t active;
[MarshalAs(UnmanagedType.ByValArray, SizeConst = 2)]
public uint8_t[] reserved3;
public double[] getECEF()
{
var X = meanX / 100.0 + meanXHP * 0.0001;
var Y = meanY / 100.0 + meanYHP * 0.0001;
var Z = meanZ / 100.0 + meanZHP * 0.0001;
return new double[] { X, Y, Z };
}
}
[StructLayout(LayoutKind.Sequential, Pack = 1)]
public struct ubx_nav_velned
{
public uint iTOW;
public int velN;
public int velE;
public int velD;
public uint speed;
public uint gSpeed;
public int heading;
public uint sAcc;
public uint cAcc;
}
[StructLayout(LayoutKind.Sequential, Pack = 1)]
public struct ubx_cfg_nav5_getset_s
{
public uint16_t mask;
public uint8_t dynModel;
public uint8_t fixMode;
public int32_t fixedAlt;
public uint32_t fixedAltVar;
public int8_t minElev;
public uint8_t drLimit;
public uint16_t pDop;
public uint16_t tDop;
public uint16_t pAcc;
public uint16_t tAcc;
public uint8_t staticHoldThresh;
public uint8_t dgnssTimeout;
public uint8_t cnoThreshNumSVs;
public uint8_t cnoThresh;
[MarshalAs(UnmanagedType.ByValArray, SizeConst = 2)]
public uint8_t[] reserved1;
public uint16_t staticHoldMaxDist;
public uint8_t utcStandard;
[MarshalAs(UnmanagedType.ByValArray, SizeConst = 5)]
public uint8_t[] reserved2;
}
[StructLayout(LayoutKind.Sequential, Pack = 1, Size = 40)]
public struct ubx_cfg_tmode3
{
public ubx_cfg_tmode3(double lat, double lng, double alt, double acc = 0.001)
{
version = 0;
reserved1 = 0;
if (Math.Abs(lat) > 90)
{
flags = 2; // fixed mode ecef
ecefXorLat = (int)(lat * 100);
ecefYorLon = (int)(lng * 100);
ecefZorAlt = (int)(alt * 100);
ecefXOrLatHP = (sbyte)((lat * 100 - ecefXorLat) * 100.0);
ecefYOrLonHP = (sbyte)((lng * 100 - ecefYorLon) * 100.0);
ecefZOrAltHP = (sbyte)((alt * 100 - ecefZorAlt) * 100.0);
}
else
{
flags = 256 + 2; // lla + fixed mode
ecefXorLat = (int)(lat * 1e7);
ecefYorLon = (int)(lng * 1e7);
ecefZorAlt = (int)(alt * 100.0);
ecefXOrLatHP = (sbyte)((lat * 1e7 - ecefXorLat) * 100.0);
ecefYOrLonHP = (sbyte)((lng * 1e7 - ecefYorLon) * 100.0);
ecefZOrAltHP = (sbyte)((alt * 100.0 - ecefZorAlt) * 100.0);
}
reserved2 = 0;
fixedPosAcc = (uint)(acc * 1000.0);
svinMinDur = 60;
svinAccLimit = 2000;
reserved3 = new byte[8];
}
public ubx_cfg_tmode3(uint DurationS, double AccLimit)
{
version = 0;
reserved1 = 0;
flags = 1; // surveyin mode
ecefXorLat = 0;
ecefYorLon = 0;
ecefZorAlt = 0;
ecefXOrLatHP = (sbyte)0;
ecefYOrLonHP = (sbyte)0;
ecefZOrAltHP = (sbyte)0;
reserved2 = 0;
fixedPosAcc = 0;
svinMinDur = DurationS;
svinAccLimit = (uint)(AccLimit * 10000);
reserved3 = new byte[8];
}
public static ubx_cfg_tmode3 Disable
{
get
{
return new ubx_cfg_tmode3()
{
flags = 0, // disable
reserved3 = new byte[8]
};
}
}
public static implicit operator byte[] (ubx_cfg_tmode3 input)
{
return null;// MavlinkUtil.StructureToByteArray(input);
}
public enum modeflags
{
Disabled = 0,
SurveyIn = 1,
FixedECEF = 2,
LLA = 256,
FixedLLA = 258
}
public byte version;
public byte reserved1;
public ushort flags;
public int ecefXorLat; // 1e7
public int ecefYorLon;
public int ecefZorAlt;
public sbyte ecefXOrLatHP;
public sbyte ecefYOrLonHP;
public sbyte ecefZOrAltHP;
public byte reserved2;
public uint fixedPosAcc;
public uint svinMinDur;
public uint svinAccLimit;
[MarshalAs(UnmanagedType.ByValArray, SizeConst = 8)]
public byte[] reserved3;
public void SetupM8P(ICommsSerial port, bool m8p_130plus = false, bool movingbase = false)
{
port.BaseStream.Flush();
var bauds = new[] { port.BaudRate, 9600, 38400, 57600, 115200, 230400, 460800 };
// change the baudrate
foreach (var baud in bauds)
{
port.BaudRate = baud;
System.Threading.Thread.Sleep(50);
// U = bit 01010101 - often used for autobaud
port.Write("UU");
// port config - 460800 - uart1
var packet = generate(0x6, 0x00, new byte[]
{
0x01, 0x00, 0x00, 0x00, 0xD0, 0x08, 0x00, 0x00, 0x00, 0x08,
0x07, 0x00, 0x23, 0x00, 0x23, 0x00, 0x00, 0x00, 0x00, 0x00
});
port.Write(packet, 0, packet.Length);
port.BaseStream.Flush();
System.Threading.Thread.Sleep(100);
}
{
// port config - usb
var packet = generate(0x6, 0x00, new byte[]
{
0x03, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
0x00, 0x00, 0x23, 0x00, 0x23, 0x00, 0x00, 0x00, 0x00, 0x00
});
port.Write(packet, 0, packet.Length);
System.Threading.Thread.Sleep(300);
port.BaseStream.Flush();
port.BaudRate = 460800;
}
{
// set rate to 1hz
var packet = generate(0x6, 0x8, new byte[] { 0xE8, 0x03, 0x01, 0x00, 0x01, 0x00 });
port.Write(packet, 0, packet.Length);
System.Threading.Thread.Sleep(200);
// set navmode to stationary
{
packet = generate(0x6, 0x24,
new byte[]
{
0xFF, 0xFF, 0x02, 0x03, 0x00, 0x00, 0x00, 0x00, 0x10, 0x27, 0x00, 0x00, 0x0F, 0x00, 0xFA,
0x00,
0xFA, 0x00, 0x64, 0x00, 0x2C, 0x01, 0x00, 0x00, 0x00, 0x23, 0x10, 0x27, 0x00, 0x00, 0x00,
0x00,
0x00, 0x00, 0x00, 0x00
});
port.Write(packet, 0, packet.Length);
System.Threading.Thread.Sleep(200);
}
}
// turn off all nmea
for (int a = 0; a <= 0xf; a++)
{
if (a == 0xb || a == 0xc || a == 0xe)
continue;
turnon_off(port, 0xf0, (byte)a, 0);
}
// mon-ver
poll_msg(port, 0xa, 0x4);
// surveyin msg - for feedback
turnon_off(port, 0x01, 0x3b, 1);
// pvt msg - for feedback
turnon_off(port, 0x01, 0x07, 1);
// 1005 - 5s
turnon_off(port, 0xf5, 0x05, 5);
byte rate1 = 1;
byte rate2 = 0;
if (m8p_130plus)
{
rate1 = 0;
rate2 = 1;
}
// 1074 - 1s
turnon_off(port, 0xf5, 0x4a, rate2);
// 1077 - 1s
turnon_off(port, 0xf5, 0x4d, rate1);
// 1084 - 1s
turnon_off(port, 0xf5, 0x54, rate2);
// 1087 - 1s
turnon_off(port, 0xf5, 0x57, rate1);
// 1094 - 1s
turnon_off(port, 0xf5, 0x5e, rate2);
// 1097 - 1s
turnon_off(port, 0xf5, 0x61, rate1);
// 1124 - 1s
turnon_off(port, 0xf5, 0x7c, rate2);
// 1127 - 1s
turnon_off(port, 0xf5, 0x7f, rate1);
// 4072
turnon_off(port, 0xf5, 0xFE, 0);
// 1230 - 5s
turnon_off(port, 0xf5, 0xE6, 5);
// NAV-VELNED - 1s
turnon_off(port, 0x01, 0x12, 1);
// rxm-raw/rawx - 1s
turnon_off(port, 0x02, 0x15, 1);
turnon_off(port, 0x02, 0x10, 1);
// rxm-sfrb/sfrb - 2s
turnon_off(port, 0x02, 0x13, 2);
turnon_off(port, 0x02, 0x11, 2);
// mon-hw - 2s
turnon_off(port, 0x0a, 0x09, 2);
System.Threading.Thread.Sleep(100);
}
public void turnon_off(ICommsSerial port, byte clas, byte subclass, byte every_xsamples)
{
byte[] datastruct1 = { clas, subclass, 0, every_xsamples, 0, every_xsamples, 0, 0 };
var packet = generate(0x6, 0x1, datastruct1);
port.Write(packet, 0, packet.Length);
System.Threading.Thread.Sleep(10);
}
public void poll_msg(ICommsSerial port, byte clas, byte subclass)
{
byte[] datastruct1 = { };
var packet = generate(clas, subclass, datastruct1);
port.Write(packet, 0, packet.Length);
System.Threading.Thread.Sleep(10);
}
}
}
}

View File

@ -40,6 +40,17 @@ namespace Plane.FormationCreator.ViewModels
set { Set(nameof(SerialPortResend), ref _serialPortResend, value); }
}
private bool _smalldatamk;
public bool SmalldataMK
{
get { return _smalldatamk; }
set
{
Set(nameof(SmalldataMK), ref _smalldatamk, value);
_rtcmManager.SmallRtcmData= value;
}
}
private bool _resendtocommk;
public bool ResendToComMK
@ -83,6 +94,7 @@ namespace Plane.FormationCreator.ViewModels
public void RefreshPorts()
{
string lastSelectValue = SerialPortsSelectdValue;
string lastSelectValue_re = SerialPortResend;
serialPorts.Clear();
serialPorts.Add("魔方基站");
serialPorts.Add("千寻");
@ -93,6 +105,17 @@ namespace Plane.FormationCreator.ViewModels
}
if (serialPorts.Contains(lastSelectValue))
SerialPortsSelectdValue = lastSelectValue;
serialRePorts.Clear();
foreach (var item in commports)
{
serialRePorts.Add(item);
}
if (serialRePorts.Contains(lastSelectValue_re))
SerialPortResend = lastSelectValue_re;
}
private ICommand _ConnectRtcmCommand;

View File

@ -5,24 +5,43 @@
xmlns:d="http://schemas.microsoft.com/expression/blend/2008"
xmlns:local="clr-namespace:Plane.FormationCreator.Views"
xmlns:c="http://metro.mahapps.com/winfx/xaml/controls"
mc:Ignorable="d"
d:DesignHeight="300" d:DesignWidth="800"
Height="350" Width="1500" WindowStartupLocation="CenterScreen" Background="#FF2D2D2D" BorderBrush="#FF039AF0" Title="RTK控制" ResizeMode="NoResize">
mc:Ignorable="d" d:DesignWidth="800"
Height="426" Width="1500" WindowStartupLocation="CenterScreen" Background="#FF2D2D2D" BorderBrush="#FF039AF0" Title="RTK控制" ResizeMode="NoResize">
<Grid Margin="0,0,0,0">
<Grid.RowDefinitions>
<RowDefinition Height="Auto"/>
<RowDefinition />
</Grid.RowDefinitions>
<StackPanel Margin="5" Orientation="Horizontal" VerticalAlignment="Center">
<ComboBox Width="120" Height="25" Foreground="White" SelectedIndex="0"
<Border Margin="10,5" BorderBrush="#FFB9B8B8" BorderThickness="1">
<StackPanel Margin="0" Orientation="Vertical">
<StackPanel Margin="0" Orientation="Horizontal">
<ComboBox Width="120" Height="25" Foreground="White" SelectedIndex="0"
Margin="10" ItemsSource="{Binding serialPorts, Mode=OneWay}"
SelectedValue="{Binding SerialPortsSelectdValue}"
DropDownOpened="ComboBox_DropDownOpened"/>
<Button Margin="6" Width="80" Height="28"
<Button Margin="6" Width="80" Height="28"
Content="{Binding RtcmManager.Rtcmthreadrun,Converter={StaticResource SendConverter}}"
Command="{Binding ConnectRtcmCommand}"/>
<Label Margin="6,6,0,6" Height="28" Content="总数:"/>
<Label Margin="0,6,6,6" Height="28" Content="{Binding RtcmManager.rtcmInfoList.Count,UpdateSourceTrigger=PropertyChanged}"></Label>
<Label Margin="6,6,0,6" Height="28" Content="卫星数:"/>
<Label Margin="0,6,6,6" Height="28" Content="{Binding RtcmManager.rtcmInfoList.Count,UpdateSourceTrigger=PropertyChanged}"></Label>
</StackPanel>
<StackPanel Margin="6,0,0,6" Orientation="Horizontal">
<CheckBox x:Name="checkBox" Content="同时发送到:"
Margin="4,5,5,0" Width="90"
IsChecked="{Binding ResendToComMK}"/>
<ComboBox Width="90" Height="25" Foreground="White" SelectedIndex="0"
Margin="0,0,5,0" ItemsSource="{Binding serialRePorts, Mode=OneWay}"
SelectedValue="{Binding SerialPortResend}"
DropDownOpened="ComboBox_DropDownOpened"/>
<CheckBox x:Name="cBsmalldata" Content="减少广播数据"
Margin="20,5,5,0" Width="95"
IsChecked="{Binding SmalldataMK}"/>
</StackPanel>
</StackPanel>
</Border>
<Border Margin="10,5" BorderBrush="#FFB9B8B8" BorderThickness="1">
<StackPanel Margin="5" Orientation="Vertical">
@ -30,10 +49,10 @@
<TextBlock Text="接收速率: "/>
<TextBlock TextAlignment="Right" Width="30"
Text="{Binding RtcmManager.Bps}"/>
<TextBlock Text=" bps;发送速率:"/>
<TextBlock Text=" 字节/秒;发送速率:"/>
<TextBlock TextAlignment="Right" Width="30"
Text="{Binding RtcmManager.Bpsusefull}"/>
<TextBlock Text=" bps"/>
<TextBlock Text=" 字节/秒"/>
</StackPanel>
<StackPanel Margin="5" Orientation="Horizontal">
@ -42,8 +61,8 @@
/>
</StackPanel>
</StackPanel>
</Border>
<Border Margin="10,5" BorderBrush="#FFB9B8B8" BorderThickness="1">
@ -70,22 +89,15 @@
<TextBlock Margin="2,5" Text="-"/>
<TextBlock Margin="2,5" Text="{Binding RtcmManager.StationTime}"/>
</StackPanel>
</StackPanel>
</Border>
<CheckBox x:Name="checkBox" Content="同时发送到:"
Margin="20,30,5,20" Width="90"
IsChecked="{Binding ResendToComMK}"/>
<ComboBox Width="120" Height="25" Foreground="White" SelectedIndex="0"
Margin="10" ItemsSource="{Binding serialRePorts, Mode=OneWay}"
SelectedValue="{Binding SerialPortResend}"
DropDownOpened="ComboBox_DropDownOpened"/>
</StackPanel>
<ItemsControl Grid.Row="1"
ItemsSource="{Binding Path = RtcmManager.rtcmInfoList}"
ItemsSource="{Binding Path = RtcmManager.rtcmInfoList}" Margin="0,0.333,-0.333,-80" Height="303" VerticalAlignment="Top"
>
<ItemsControl.ItemsPanel>
<ItemsPanelTemplate>
@ -116,7 +128,7 @@
<Line Stroke="Red" X2="1000" StrokeThickness="3" />
<TextBlock Text="40" Foreground="Black"/>
</StackPanel>
</Grid>