private bool process_message()
{
Console.WriteLine("packet: " + gsof_msg.packettype.ToString("X") + " len: " + gsof_msg.length + " status: " +
gsof_msg.status);
if (gsof_msg.packettype == 0x57) // RAWDATA
{
uint8_t record_type = gsof_msg.data[0];
uint8_t page_total = (byte)(gsof_msg.data[1] & 0xf);
uint8_t page_number = (byte)((gsof_msg.data[1] >> 4) & 0xf);
uint8_t reply_number = (byte)gsof_msg.data[2];
uint8_t recordintflags = gsof_msg.data[3];
bool consise = (recordintflags & 0x1) > 0;
Console.WriteLine(DateTime.Now.Second + " record type: " + record_type + " page_no " + page_number + "/" + page_total + " consise " + consise);
if (record_type == 7)
{
double p40 = Math.Pow(2, 40);
double p39 = Math.Pow(2, 39);
double p11 = Math.Pow(2, 11);
double p12 = Math.Pow(2, 12);
double p14 = Math.Pow(2, 14);
double p21 = Math.Pow(2, 21);
double p26 = Math.Pow(2, 26);
double p17 = Math.Pow(2, 17);
double p4 = Math.Pow(2, 4);
var test2 = (rt27_7_29)gsof_msg.data.ByteArrayToStructureBigEndian <rt27_7_29>(4);
long lat = byte2long(test2.Lat);
long lng = byte2long(test2.Lng);
Console.WriteLine("{0} {1} {2} {3} {4} {5} ", lat / p40, lng / p39, test2.Alt / p12,
(test2.veln / p21), (test2.vele / p21), (test2.velu / p21),
test2.rx_clock_offset / p26, test2.rx_clock_drift / p17, test2.hdop / p4);
}
else if (record_type == 1)
{
var test2 = (rt27_1_11)gsof_msg.data.ByteArrayToStructureBigEndian <rt27_1_11>(4);
Console.WriteLine("{0} {1} {2}", ToDeg(test2.Latitude * Math.PI), ToDeg(test2.Longitude * Math.PI), test2.Altitude);
}
else if (record_type == 2)
{
var test2 = (rt27_2_19)gsof_msg.data.ByteArrayToStructureBigEndian <rt27_2_19>(4);
Console.WriteLine("Event {0} {1} {2} {3}", test2.Source, test2.Port, test2.Number, test2.TOW);
}
else if (record_type == 6) // there is no documentation on this
{
if (page_number == 1)
{
obsdata.Initialize();
Array.ConstrainedCopy(gsof_msg.data, 0, obsdata, 0, gsof_msg.length);
}
else
{
Array.ConstrainedCopy(gsof_msg.data, 4, obsdata, 248 + (page_number - 2) * 244, gsof_msg.length - 4);
}
if (page_number == page_total)
{
var m_epochHeader =
(rt27_6_27_epochHeader)
obsdata.ByteArrayToStructureBigEndian <rt27_6_27_epochHeader>(4);
var head_len = m_epochHeader.blockLength;
var m_sysOffsets = new rt27_6_27_sysOffsets();
var obj = m_sysOffsets as object;
MavlinkUtil.ByteArrayToStructureEndian(obsdata, ref obj, (4 + head_len));
m_sysOffsets = (rt27_6_27_sysOffsets)obj;
var sysoffset_lemn = m_sysOffsets.blockLength;
var m_measurement = new List <rt27_6_27_measurement>();
var SVs = m_epochHeader.nSVs;
var offset = 4 + head_len + sysoffset_lemn;
for (int index = 0; index < SVs; ++index)
{
var measheader = new rt27_6_27_measurementHeader();
obj = measheader as object;
MavlinkUtil.ByteArrayToStructureEndian(obsdata, ref obj, offset);
measheader = (rt27_6_27_measurementHeader)obj;
var meashead_len = measheader.blockLength;
offset += meashead_len;
string type = "";
switch (measheader.svType)
{
case 0:
type = "Gps";
break;
case 1:
type = "Sbas";
break;
case 2:
type = "Glonass";
break;
case 3:
type = "Galileo";
break;
case 4:
type = "QZSS";
break;
case 7:
type = "beidou";
break;
}
Console.WriteLine("prn {0} type {1} el {2} az {3} blocks {4}", measheader.prn, type, (double)(sbyte)measheader.elevation, (double)measheader.azimuth / Math.Pow(2.0, -1.0), measheader.nBlocks);
for (int a = 0; a < measheader.nBlocks; a++)
{
var measblock = new rt27_6_27_measurementBlock();
obj = measblock as object;
MavlinkUtil.ByteArrayToStructureEndian(obsdata, ref obj, offset);
measblock = (rt27_6_27_measurementBlock)obj;
if (a == 0)
{
Console.WriteLine("{0} {1}", measblock.pseudorange / Math.Pow(2.0, 7.0), byte2long(measblock.phase) / Math.Pow(2.0, 15.0));
}
else
{
}
var measblock_len = measblock.blockLength;
offset += measblock_len;
m_measurement.Add(new rt27_6_27_measurement()
{
block = measblock, header = measheader
});
}
}
}
}
}
if (gsof_msg.packettype == 0x40) // GSOF
{
uint8_t trans_number = gsof_msg.data[0];
uint8_t pageidx = gsof_msg.data[1];
uint8_t maxpageidx = gsof_msg.data[2];
//http://www.trimble.com/OEM_ReceiverHelp/V4.81/en/default.html
//http://www.trimble.com/EC_ReceiverHelp/V4.19/en/GSOFmessages_Overview.htm
Console.WriteLine("GSOF page: " + pageidx + " of " + maxpageidx);
// want 2 8 9 38
for (uint a = 3; a < gsof_msg.length; a++)
{
uint8_t output_type = gsof_msg.data[a];
a++;
uint8_t output_length = gsof_msg.data[a];
a++;
Console.WriteLine("GSOF type: " + output_type + " len: " + output_length);
if (output_type == 2) // position
{
state.location.lat = (int32_t)(ToDeg(SwapDouble(gsof_msg.data, a)) * 1e7);
state.location.lng = (int32_t)(ToDeg(SwapDouble(gsof_msg.data, a + 8)) * 1e7);
state.location.alt = (int32_t)(SwapDouble(gsof_msg.data, a + 16) * 1e2);
state.last_gps_time_ms = state.time_week_ms;
}
else if (output_type == 8) // velocity
{
uint8_t vflag = gsof_msg.data[a];
if ((vflag & 1) == 1)
{
state.ground_speed = SwapFloat(gsof_msg.data, a + 1);
state.ground_course = (float)(ToDeg(SwapFloat(gsof_msg.data, a + 5)));
fill_3d_velocity();
state.velocity.z = -SwapFloat(gsof_msg.data, a + 9);
state.have_vertical_velocity = true;
}
}
else if (output_type == 9) //dop
{
state.hdop = (uint16_t)(SwapFloat(gsof_msg.data, a + 4) * 100);
}
else if (output_type == 12) // position sigma
{
state.horizontal_accuracy = (SwapFloat(gsof_msg.data, a + 4) + SwapFloat(gsof_msg.data, a + 8)) /
2;
state.vertical_accuracy = SwapFloat(gsof_msg.data, a + 16);
state.have_horizontal_accuracy = true;
state.have_vertical_accuracy = true;
}
else if (output_type == 1) // pos time
{
state.time_week_ms = SwapUint32(gsof_msg.data, a);
state.time_week = SwapUint16(gsof_msg.data, a + 4);
state.num_sats = gsof_msg.data[a + 6];
uint8_t posf1 = gsof_msg.data[a + 7];
uint8_t posf2 = gsof_msg.data[a + 8];
Console.WriteLine("POSTIME: " + posf1 + " " + posf2);
if ((posf1 & 1) == 1)
{
state.status = AP_GPS.GPS_OK_FIX_3D;
if ((posf2 & 1) == 1)
{
state.status = AP_GPS.GPS_OK_FIX_3D_DGPS;
if ((posf2 & 4) == 4)
{
state.status = AP_GPS.GPS_OK_FIX_3D_RTK;
}
}
}
else
{
state.status = AP_GPS.NO_FIX;
}
}
a += output_length - 1u;
}
Type t = state.GetType(); //where obj is object whose properties you need.
FieldInfo[] pi = t.GetFields();
foreach (var p in pi)
{
System.Console.WriteLine(p.Name + " " + p.GetValue(state).ToString());
}
return(true);
}
return(false);
}
private bool process_message()
{
Console.WriteLine("packet: " + gsof_msg.packettype.ToString("X") + " len: " + gsof_msg.length + " status: " +
gsof_msg.status);
if (gsof_msg.packettype == 0x57) // RAWDATA
{
uint8_t record_type = gsof_msg.data[0];
uint8_t page_total = (byte)(gsof_msg.data[1] & 0xf);
uint8_t page_number = (byte)((gsof_msg.data[1] >> 4) & 0xf);
uint8_t reply_number =(byte) gsof_msg.data[2];
uint8_t recordintflags = gsof_msg.data[3];
bool consise = (recordintflags & 0x1) > 0;
Console.WriteLine(DateTime.Now.Second + " record type: " + record_type + " page_no " + page_number + "/" + page_total + " consise " + consise);
if (record_type == 7)
{
double p40 = Math.Pow(2, 40);
double p39 = Math.Pow(2, 39);
double p11 = Math.Pow(2, 11);
double p12 = Math.Pow(2, 12);
double p14 = Math.Pow(2, 14);
double p21 = Math.Pow(2, 21);
double p26 = Math.Pow(2, 26);
double p17 = Math.Pow(2, 17);
double p4 = Math.Pow(2, 4);
var test2 = (rt27_7_29) gsof_msg.data.ByteArrayToStructureBigEndian<rt27_7_29>(4);
long lat = byte2long(test2.Lat);
long lng = byte2long(test2.Lng);
Console.WriteLine("{0} {1} {2} {3} {4} {5} ", lat/p40, lng/p39, test2.Alt/p12,
(test2.veln/p21), (test2.vele/p21), (test2.velu/p21),
test2.rx_clock_offset/p26, test2.rx_clock_drift/p17, test2.hdop/p4);
}
else if (record_type == 1)
{
var test2 = (rt27_1_11)gsof_msg.data.ByteArrayToStructureBigEndian<rt27_1_11>(4);
Console.WriteLine("{0} {1} {2}", ToDeg(test2.Latitude*Math.PI), ToDeg(test2.Longitude*Math.PI), test2.Altitude);
}
else if (record_type == 2)
{
var test2 = (rt27_2_19)gsof_msg.data.ByteArrayToStructureBigEndian<rt27_2_19>(4);
Console.WriteLine("Event {0} {1} {2} {3}", test2.Source, test2.Port, test2.Number, test2.TOW);
}
else if (record_type == 6) // there is no documentation on this
{
if (page_number == 1)
{
obsdata.Initialize();
Array.ConstrainedCopy(gsof_msg.data, 0, obsdata, 0, gsof_msg.length);
}
else
{
Array.ConstrainedCopy(gsof_msg.data, 4, obsdata, 248 + (page_number - 2) * 244, gsof_msg.length - 4);
}
if (page_number == page_total)
{
var m_epochHeader =
(rt27_6_27_epochHeader)
obsdata.ByteArrayToStructureBigEndian<rt27_6_27_epochHeader>(4);
var head_len = m_epochHeader.blockLength;
var m_sysOffsets = new rt27_6_27_sysOffsets();
var obj = m_sysOffsets as object;
MavlinkUtil.ByteArrayToStructureEndian(obsdata, ref obj, (4 + head_len));
m_sysOffsets = (rt27_6_27_sysOffsets) obj;
var sysoffset_lemn = m_sysOffsets.blockLength;
var m_measurement = new List<rt27_6_27_measurement>();
var SVs = m_epochHeader.nSVs;
var offset = 4 + head_len + sysoffset_lemn;
for (int index = 0; index < SVs; ++index)
{
var measheader = new rt27_6_27_measurementHeader();
obj = measheader as object;
MavlinkUtil.ByteArrayToStructureEndian(obsdata, ref obj, offset);
measheader = (rt27_6_27_measurementHeader)obj;
var meashead_len = measheader.blockLength;
offset += meashead_len;
string type = "";
switch (measheader.svType)
{
case 0:
type = "Gps";
break;
case 1:
type = "Sbas";
break;
case 2:
type = "Glonass";
break;
case 3:
type = "Galileo";
break;
case 4:
type = "QZSS";
break;
case 7:
type = "beidou";
break;
}
Console.WriteLine("prn {0} type {1} el {2} az {3} blocks {4}", measheader.prn, type, (double)(sbyte)measheader.elevation, (double)measheader.azimuth / Math.Pow(2.0, -1.0), measheader.nBlocks);
for (int a = 0; a < measheader.nBlocks;a++)
{
var measblock = new rt27_6_27_measurementBlock();
obj = measblock as object;
MavlinkUtil.ByteArrayToStructureEndian(obsdata, ref obj, offset);
measblock = (rt27_6_27_measurementBlock)obj;
if (a == 0)
{
Console.WriteLine("{0} {1}", measblock.pseudorange / Math.Pow(2.0, 7.0), byte2long(measblock.phase) / Math.Pow(2.0, 15.0));
}
else
{
}
var measblock_len = measblock.blockLength;
offset += measblock_len;
m_measurement.Add(new rt27_6_27_measurement() { block = measblock, header = measheader });
}
}
}
}
}
if (gsof_msg.packettype == 0x40) // GSOF
{
uint8_t trans_number = gsof_msg.data[0];
uint8_t pageidx = gsof_msg.data[1];
uint8_t maxpageidx = gsof_msg.data[2];
//http://www.trimble.com/OEM_ReceiverHelp/V4.81/en/default.html
//http://www.trimble.com/EC_ReceiverHelp/V4.19/en/GSOFmessages_Overview.htm
Console.WriteLine("GSOF page: " + pageidx + " of " + maxpageidx);
// want 2 8 9 38
for (uint a = 3; a < gsof_msg.length; a++)
{
uint8_t output_type = gsof_msg.data[a];
a++;
uint8_t output_length = gsof_msg.data[a];
a++;
Console.WriteLine("GSOF type: " + output_type + " len: " + output_length);
if (output_type == 2) // position
{
state.location.lat = (int32_t) (ToDeg(SwapDouble(gsof_msg.data, a))*1e7);
state.location.lng = (int32_t) (ToDeg(SwapDouble(gsof_msg.data, a + 8))*1e7);
state.location.alt = (int32_t) (SwapDouble(gsof_msg.data, a + 16)*1e2);
state.last_gps_time_ms = state.time_week_ms;
}
else if (output_type == 8) // velocity
{
uint8_t vflag = gsof_msg.data[a];
if ((vflag & 1) == 1)
{
state.ground_speed = SwapFloat(gsof_msg.data, a + 1);
state.ground_course = (float)(ToDeg(SwapFloat(gsof_msg.data, a + 5)));
fill_3d_velocity();
state.velocity.z = -SwapFloat(gsof_msg.data, a + 9);
state.have_vertical_velocity = true;
}
}
else if (output_type == 9) //dop
{
state.hdop = (uint16_t) (SwapFloat(gsof_msg.data, a + 4)*100);
}
else if (output_type == 12) // position sigma
{
state.horizontal_accuracy = (SwapFloat(gsof_msg.data, a + 4) + SwapFloat(gsof_msg.data, a + 8))/
2;
state.vertical_accuracy = SwapFloat(gsof_msg.data, a + 16);
state.have_horizontal_accuracy = true;
state.have_vertical_accuracy = true;
}
else if (output_type == 1) // pos time
{
state.time_week_ms = SwapUint32(gsof_msg.data, a);
state.time_week = SwapUint16(gsof_msg.data, a + 4);
state.num_sats = gsof_msg.data[a + 6];
uint8_t posf1 = gsof_msg.data[a + 7];
uint8_t posf2 = gsof_msg.data[a + 8];
Console.WriteLine("POSTIME: " + posf1 + " " + posf2);
if ((posf1 & 1) == 1)
{
state.status = AP_GPS.GPS_OK_FIX_3D;
if ((posf2 & 1) == 1)
{
state.status = AP_GPS.GPS_OK_FIX_3D_DGPS;
if ((posf2 & 4) == 4)
{
state.status = AP_GPS.GPS_OK_FIX_3D_RTK;
}
}
}
else
{
state.status = AP_GPS.NO_FIX;
}
}
a += output_length - 1u;
}
Type t = state.GetType(); //where obj is object whose properties you need.
FieldInfo[] pi = t.GetFields();
foreach (var p in pi)
{
System.Console.WriteLine(p.Name + " " + p.GetValue(state).ToString());
}
return true;
}
return false;
}
