public AP_GPS_SBF()
{
crc_error_counter = 0;
last_hdop = 999;
sbf_state = readstate.PREAMBLE1;
sbf_msg.data = new byte[1024 * 20];
read();
}
public AP_GPS_SBF()
{
crc_error_counter = 0;
last_hdop = 999;
sbf_state = readstate.PREAMBLE1;
sbf_msg.data = new byte[1024 * 20];
read();
}
Stream port = null;//File.Open(@"T:\rtk4\SEPTX.15_", FileMode.Open);
public AP_GPS_SBF()
{
crc_error_counter = 0;
last_hdop = 999;
sbf_state = readstate.PREAMBLE1;
var sport = new SerialPort("com10", 115200);
sport.Open();
port = sport.BaseStream;
// setcommsettings
//string command1 = "scs, COM1, baud115200\n";
// setdatainout
//string command2 = "sdio, COM1, auto, SBF\n";
// copy current config to boot config - wont use
//eccf, Current, Boot <CR>
//setreceiverdynamics
//srd, High, UAV
// setelevationmask
//sem, PVT, 5
// setSBFOutput
validcommand = false;
string command2 = "sso, Stream1, USB1, PVTGeodetic+DOP+ExtEventPVTGeodetic, msec100\n";
port.Write(ASCIIEncoding.ASCII.GetBytes(command2), 0, command2.Length);
System.Threading.Thread.Sleep(70);
//string command3 = "sso, Stream2, USB1, PVTGeodetic+DOP+ExtEventPVTGeodetic, msec100\n";
//port.Write(ASCIIEncoding.ASCII.GetBytes(command3),0,command3.Length);
//System.Threading.Thread.Sleep(70);
//command3 = "sso, Stream3, USB2, PVTGeodetic+DOP+ExtEventPVTGeodetic, msec100\n";
//port.Write(ASCIIEncoding.ASCII.GetBytes(command3), 0, command3.Length);
//System.Threading.Thread.Sleep(70);
string command4 = "srd, High, UAV\n";
port.Write(ASCIIEncoding.ASCII.GetBytes(command4), 0, command4.Length);
System.Threading.Thread.Sleep(70);
string command5 = "sem, PVT, 5\n";
port.Write(ASCIIEncoding.ASCII.GetBytes(command5), 0, command5.Length);
System.Threading.Thread.Sleep(70);
// enable sbas "+SBAS"
string command6 = "spm, Rover, StandAlone+DGPS+RTK\n";
port.Write(ASCIIEncoding.ASCII.GetBytes(command6), 0, command6.Length);
System.Threading.Thread.Sleep(70);
int btr = sport.BytesToRead;
byte[] data = new byte[btr];
//port.Read(data, 0, btr);
//Console.WriteLine(ASCIIEncoding.ASCII.GetString(data));
//System.Threading.Thread.Sleep(100);
validcommand = true;
sbf_msg.data = new byte[1024 * 20];
read();
}
private bool parse(uint8_t temp)
{
switch (sbf_state)
{
case readstate.PREAMBLE1:
if (temp == SBF_PREAMBLE1)
{
sbf_state++;
}
else if (temp == '$')
{
sbf_state++;
}
else
{
Console.Write(".");
}
sbf_msg.read = 0;
break;
case readstate.PREAMBLE2:
if (temp == SBF_PREAMBLE2)
{
sbf_state++;
}
else if (temp == 'R')
{
validcommand = true;
}
else
{
Console.WriteLine("Bad Sync " + temp);
sbf_state = readstate.PREAMBLE1;
}
break;
case readstate.CRC1:
sbf_msg.crc = temp;
sbf_state++;
break;
case readstate.CRC2:
sbf_msg.crc += (uint16_t)(temp << 8);
sbf_state++;
break;
case readstate.BLOCKID1:
sbf_msg.blockid = temp;
sbf_state++;
break;
case readstate.BLOCKID2:
sbf_msg.blockid += (uint16_t)(temp << 8);
sbf_state++;
break;
case readstate.LENGTH1:
sbf_msg.length = temp;
sbf_state++;
break;
case readstate.LENGTH2:
sbf_msg.length += (uint16_t)(temp << 8);
sbf_state++;
sbf_msg.data = new uint8_t[sbf_msg.length];
//Console.WriteLine((sbf_msg.blockid & 4095u) + " len " + sbf_msg.length);
if (sbf_msg.length % 4 != 0)
{
sbf_state = readstate.PREAMBLE1;
}
break;
case readstate.DATA:
sbf_msg.data[sbf_msg.read] = temp;
sbf_msg.read++;
if (sbf_msg.read >= (sbf_msg.length - 8))
{
uint16_t crc = crc16.ccitt(sbf_msg.blockid, 2, 0);
crc = crc16.ccitt(sbf_msg.length, 2, crc);
crc = crc16.ccitt(sbf_msg.data, sbf_msg.length - 8, crc);
sbf_state = readstate.PREAMBLE1;
if (sbf_msg.crc == crc)
{
return(process_message());
}
else
{
crc_error_counter++;
}
}
break;
}
return(false);
}
private bool parse(uint8_t temp)
{
switch (gsof_state)
{
case readstate.STARTTX:
if (temp == GSOF_STX)
{
gsof_msg.starttx = temp;
gsof_state++;
}
else
{
Console.Write(".");
}
gsof_msg.read = 0;
gsof_msg.checksumcalc = 0;
break;
case readstate.STATUS:
gsof_msg.status = temp;
gsof_state++;
gsof_msg.checksumcalc += temp;
break;
case readstate.PACKETTYPE:
gsof_msg.packettype = temp;
gsof_state++;
gsof_msg.checksumcalc += temp;
break;
case readstate.LENGTH:
gsof_msg.length = temp;
gsof_state++;
gsof_msg.checksumcalc += temp;
break;
case readstate.DATA:
gsof_msg.data[gsof_msg.read] = temp;
gsof_msg.read++;
gsof_msg.checksumcalc += temp;
if (gsof_msg.read >= gsof_msg.length)
{
gsof_state++;
}
break;
case readstate.CHECKSUM:
gsof_msg.checksum = temp;
gsof_state++;
if (gsof_msg.checksum == gsof_msg.checksumcalc)
{
return(process_message());
}
break;
case readstate.ENDTX:
gsof_msg.endtx = temp;
gsof_state = 0;
break;
}
return(false);
}
Stream port = null;//File.Open(@"T:\rtk4\SEPTX.15_", FileMode.Open);
public AP_GPS_SBF()
{
crc_error_counter = 0;
last_hdop = 999;
sbf_state = readstate.PREAMBLE1;
var sport = new SerialPort("com10", 115200);
sport.Open();
port = sport.BaseStream;
// setcommsettings
//string command1 = "scs, COM1, baud115200\n";
// setdatainout
//string command2 = "sdio, COM1, auto, SBF\n";
// copy current config to boot config - wont use
//eccf, Current, Boot <CR>
//setreceiverdynamics
//srd, High, UAV
// setelevationmask
//sem, PVT, 5
// setSBFOutput
validcommand = false;
string command2 = "sso, Stream1, USB1, PVTGeodetic+DOP+ExtEventPVTGeodetic, msec100\n";
port.Write(ASCIIEncoding.ASCII.GetBytes(command2), 0, command2.Length);
System.Threading.Thread.Sleep(70);
//string command3 = "sso, Stream2, USB1, PVTGeodetic+DOP+ExtEventPVTGeodetic, msec100\n";
//port.Write(ASCIIEncoding.ASCII.GetBytes(command3),0,command3.Length);
//System.Threading.Thread.Sleep(70);
//command3 = "sso, Stream3, USB2, PVTGeodetic+DOP+ExtEventPVTGeodetic, msec100\n";
//port.Write(ASCIIEncoding.ASCII.GetBytes(command3), 0, command3.Length);
//System.Threading.Thread.Sleep(70);
string command4 = "srd, High, UAV\n";
//port.Write(ASCIIEncoding.ASCII.GetBytes(command4), 0, command4.Length);
System.Threading.Thread.Sleep(70);
string command5 = "sem, PVT, 5\n";
//port.Write(ASCIIEncoding.ASCII.GetBytes(command5), 0, command5.Length);
System.Threading.Thread.Sleep(70);
// enable sbas "+SBAS"
string command6 = "spm, Rover, StandAlone+DGPS+RTK\n";
//port.Write(ASCIIEncoding.ASCII.GetBytes(command6), 0, command6.Length);
System.Threading.Thread.Sleep(70);
int btr = sport.BytesToRead;
byte[] data = new byte[btr];
//port.Read(data, 0, btr);
//Console.WriteLine(ASCIIEncoding.ASCII.GetString(data));
//System.Threading.Thread.Sleep(100);
sbf_msg.data = new byte[1024*20];
read();
}
private bool parse(uint8_t temp)
{
switch (sbf_state)
{
case readstate.PREAMBLE1:
if (temp == SBF_PREAMBLE1)
sbf_state++;
else if (temp == '$')
sbf_state++;
else
Console.Write(".");
sbf_msg.read = 0;
break;
case readstate.PREAMBLE2:
if (temp == SBF_PREAMBLE2)
{
sbf_state++;
}
else if (temp == 'R')
{
validcommand = true;
}
else
{
Console.WriteLine("Bad Sync " + temp);
sbf_state = readstate.PREAMBLE1;
}
break;
case readstate.CRC1:
sbf_msg.crc = temp;
sbf_state++;
break;
case readstate.CRC2:
sbf_msg.crc += (uint16_t) (temp << 8);
sbf_state++;
break;
case readstate.BLOCKID1:
sbf_msg.blockid = temp;
sbf_state++;
break;
case readstate.BLOCKID2:
sbf_msg.blockid += (uint16_t) (temp << 8);
sbf_state++;
break;
case readstate.LENGTH1:
sbf_msg.length = temp;
sbf_state++;
break;
case readstate.LENGTH2:
sbf_msg.length += (uint16_t) (temp << 8);
sbf_state++;
sbf_msg.data = new uint8_t[sbf_msg.length];
//Console.WriteLine((sbf_msg.blockid & 4095u) + " len " + sbf_msg.length);
if (sbf_msg.length%4 != 0)
sbf_state = readstate.PREAMBLE1;
break;
case readstate.DATA:
sbf_msg.data[sbf_msg.read] = temp;
sbf_msg.read++;
if (sbf_msg.read >= (sbf_msg.length - 8))
{
uint16_t crc = crc16.ccitt(sbf_msg.blockid, 2, 0);
crc = crc16.ccitt(sbf_msg.length, 2, crc);
crc = crc16.ccitt(sbf_msg.data, sbf_msg.length - 8, crc);
sbf_state = readstate.PREAMBLE1;
if (sbf_msg.crc == crc)
{
return process_message();
}
else
{
crc_error_counter++;
}
}
break;
}
return false;
}
private bool parse(uint8_t temp)
{
switch (gsof_state)
{
case readstate.STARTTX:
if (temp == GSOF_STX)
{
gsof_msg.starttx = temp;
gsof_state++;
}
else
Console.Write(".");
gsof_msg.read = 0;
gsof_msg.checksumcalc = 0;
break;
case readstate.STATUS:
gsof_msg.status = temp;
gsof_state++;
gsof_msg.checksumcalc += temp;
break;
case readstate.PACKETTYPE:
gsof_msg.packettype = temp;
gsof_state++;
gsof_msg.checksumcalc += temp;
break;
case readstate.LENGTH:
gsof_msg.length = temp;
gsof_state++;
gsof_msg.checksumcalc += temp;
break;
case readstate.DATA:
gsof_msg.data[gsof_msg.read] = temp;
gsof_msg.read++;
gsof_msg.checksumcalc += temp;
if (gsof_msg.read >= gsof_msg.length)
{
gsof_state++;
}
break;
case readstate.CHECKSUM:
gsof_msg.checksum = temp;
gsof_state++;
if (gsof_msg.checksum == gsof_msg.checksumcalc)
{
return process_message();
}
break;
case readstate.ENDTX:
gsof_msg.endtx = temp;
gsof_state = 0;
break;
}
return false;
}
