952 lines
43 KiB
C#
952 lines
43 KiB
C#
|
using System;
|
|||
|
|
using System.Collections.Generic;
|
|||
|
|
using System.Text;
|
|||
|
|
using System.Collections;
|
|||
|
|
|
|||
|
|
namespace SafeMobileLib.MessageDecoders
|
|||
|
|
{
|
|||
|
|
public class LocationDecoder
|
|||
|
|
{
|
|||
|
|
public htCell_t cell;
|
|||
|
|
public byte result;
|
|||
|
|
private string radioID;
|
|||
|
|
public string RadioID
|
|||
|
|
{
|
|||
|
|
get { return radioID; }
|
|||
|
|
set { radioID = value; }
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
private bool DisplayConsole = true;
|
|||
|
|
|
|||
|
|
public LocationDecoder(string radioID, byte[] data, bool displayConsole, bool testmode = false)
|
|||
|
|
{
|
|||
|
|
this.DisplayConsole = displayConsole;
|
|||
|
|
this.radioID = radioID;
|
|||
|
|
|
|||
|
|
try
|
|||
|
|
{
|
|||
|
|
if (!testmode)
|
|||
|
|
cell = DecodePacket(data);
|
|||
|
|
else
|
|||
|
|
cell = DecodePacket_Connect_plus(data);
|
|||
|
|
if (cell.lat.Length == 0) //invalid position
|
|||
|
|
cell.lat = null;
|
|||
|
|
}
|
|||
|
|
catch (Exception e)
|
|||
|
|
{
|
|||
|
|
cell.lat = null;
|
|||
|
|
Console.WriteLine("Error decoding data: " + Byte2String(data));
|
|||
|
|
Console.WriteLine(e.ToString());
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
public LocationDecoder(string radioID, String[] data)
|
|||
|
|
{
|
|||
|
|
this.radioID = radioID;
|
|||
|
|
try
|
|||
|
|
{
|
|||
|
|
cell = DecodePacket(data);
|
|||
|
|
if (cell.lat.Length == 0) //invalid position
|
|||
|
|
cell.lat = null;
|
|||
|
|
}
|
|||
|
|
catch (Exception e)
|
|||
|
|
{
|
|||
|
|
cell.lat = null;
|
|||
|
|
Console.WriteLine("Error decoding data: " + data);
|
|||
|
|
Console.WriteLine(e.ToString());
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
public LocationDecoder(string radioID, byte[] data, bool testmode=false)
|
|||
|
|
{
|
|||
|
|
this.radioID = radioID;
|
|||
|
|
try
|
|||
|
|
{
|
|||
|
|
if (!testmode)
|
|||
|
|
cell = DecodePacket(data);
|
|||
|
|
else
|
|||
|
|
cell = DecodePacket_Connect_plus(data);
|
|||
|
|
if (cell.lat.Length == 0) //invalid position
|
|||
|
|
cell.lat = null;
|
|||
|
|
}
|
|||
|
|
catch (Exception e)
|
|||
|
|
{
|
|||
|
|
Console.WriteLine("Error decoding data: " + Byte2String(data));
|
|||
|
|
Console.WriteLine(e.ToString());
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
public LocationDecoder(byte[] data)
|
|||
|
|
{
|
|||
|
|
try
|
|||
|
|
{
|
|||
|
|
cell = DecodePacket_Connect_plus(data);
|
|||
|
|
if (cell.lat.Length == 0) //invalid position
|
|||
|
|
cell.lat = null;
|
|||
|
|
}
|
|||
|
|
catch (Exception e)
|
|||
|
|
{
|
|||
|
|
cell.lat = null;
|
|||
|
|
Console.WriteLine("Error decoding data: " + Byte2String(data));
|
|||
|
|
Console.WriteLine(e.ToString());
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
public htCell_t DecodePacket(String[] dataString)
|
|||
|
|
{
|
|||
|
|
htCell_t retCell = new htCell_t();
|
|||
|
|
try
|
|||
|
|
{
|
|||
|
|
retCell.lat = "";
|
|||
|
|
retCell.lng = "";
|
|||
|
|
retCell.alt = "0";
|
|||
|
|
retCell.di = "0";
|
|||
|
|
retCell.seq_ID = "";
|
|||
|
|
retCell.seqID = 0;
|
|||
|
|
retCell.isCSBK = false;
|
|||
|
|
retCell.alt = "0";
|
|||
|
|
retCell.altitude = 0;
|
|||
|
|
retCell.heading = "";
|
|||
|
|
retCell.location_time = DateTime.MinValue;
|
|||
|
|
retCell.activity_time = DateTime.MinValue;
|
|||
|
|
System.DateTime dateTime = new DateTime(1970, 1, 1, 0, 0, 0, DateTimeKind.Utc);
|
|||
|
|
dateTime = dateTime.AddSeconds(long.Parse(dataString[0]));
|
|||
|
|
retCell.location_time = dateTime;
|
|||
|
|
retCell.spd = dataString[1];
|
|||
|
|
retCell.lat = dataString[2];
|
|||
|
|
retCell.lng = dataString[3];
|
|||
|
|
|
|||
|
|
try
|
|||
|
|
{
|
|||
|
|
|
|||
|
|
if (dataString.Length > 4)
|
|||
|
|
{
|
|||
|
|
int alt = 0;
|
|||
|
|
bool result = Int32.TryParse(dataString[4], out alt);
|
|||
|
|
if (result)
|
|||
|
|
retCell.alt = dataString[4];
|
|||
|
|
else
|
|||
|
|
retCell.heading = dataString[4];
|
|||
|
|
|
|||
|
|
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
catch (Exception ex)
|
|||
|
|
{
|
|||
|
|
SM.Debug("Error convert altitude:" + ex.ToString());
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
try
|
|||
|
|
{
|
|||
|
|
if (dataString.Length > 5) retCell.logId = dataString[5];
|
|||
|
|
}
|
|||
|
|
catch (Exception ex)
|
|||
|
|
{
|
|||
|
|
SM.Debug("Error convert log id:" + ex.ToString());
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
catch (Exception ex)
|
|||
|
|
{
|
|||
|
|
SM.Debug("Error on decode packet"+ex.ToString());
|
|||
|
|
}
|
|||
|
|
return retCell;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
public htCell_t DecodePacket(Byte[] data)
|
|||
|
|
{
|
|||
|
|
int i = 0, pdata;
|
|||
|
|
|
|||
|
|
htCell_t cell = new htCell_t();
|
|||
|
|
cell.lat = "";
|
|||
|
|
cell.lng = "";
|
|||
|
|
cell.di = "0";
|
|||
|
|
cell.seq_ID = "";
|
|||
|
|
cell.seqID = 0;
|
|||
|
|
cell.alt = "0";
|
|||
|
|
cell.altitude = 0;
|
|||
|
|
cell.heading = "";
|
|||
|
|
cell.location_time = DateTime.MinValue;
|
|||
|
|
cell.activity_time = DateTime.MinValue;
|
|||
|
|
cell.triggered = false;
|
|||
|
|
cell.level_confidence = 0;
|
|||
|
|
cell.isCSBK = false;
|
|||
|
|
cell.spd = "0";
|
|||
|
|
|
|||
|
|
result = (byte)Result_Codes_ENUM.SUCCESS;
|
|||
|
|
|
|||
|
|
byte bb = (byte)Document_Identifiers_ENUM.Immediate_Location_Request_NoCDT;
|
|||
|
|
byte d0 = data[0];
|
|||
|
|
|
|||
|
|
|
|||
|
|
switch (data[0])
|
|||
|
|
{
|
|||
|
|
// -----------------------------------------------------------------------------------------------------------------------
|
|||
|
|
case (byte)Document_Identifiers_ENUM.Triggered_Location_Stop_Answer_NoCDT:
|
|||
|
|
case (byte)Document_Identifiers_ENUM.Triggered_Location_Answer_NoCDT:
|
|||
|
|
if (data[0] == (byte)Document_Identifiers_ENUM.Triggered_Location_Stop_Answer_NoCDT)
|
|||
|
|
{
|
|||
|
|
if(DisplayConsole)
|
|||
|
|
Console.WriteLine(Document_Identifiers_ENUM.Triggered_Location_Stop_Answer_NoCDT);
|
|||
|
|
}
|
|||
|
|
else
|
|||
|
|
{
|
|||
|
|
if (DisplayConsole)
|
|||
|
|
Console.WriteLine(Document_Identifiers_ENUM.Triggered_Location_Answer_NoCDT);
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
pdata = (int)data[1];
|
|||
|
|
for (i = 2; i < data[1] + 2; i++)
|
|||
|
|
{
|
|||
|
|
switch (data[i])
|
|||
|
|
{
|
|||
|
|
case (byte)Common_Element_Tokens_ENUM.request_id:
|
|||
|
|
{
|
|||
|
|
int length = (int)data[i + 1];
|
|||
|
|
// CSBK Location has a request ID of 3 Bytes
|
|||
|
|
if (length == 3)
|
|||
|
|
cell.isCSBK = true;
|
|||
|
|
cell.seq_ID = Byte2String(data, i + 2, length) + "";
|
|||
|
|
cell.seqID = Byte2Int64(data, i + 2, length);
|
|||
|
|
i = i + 1 + length;
|
|||
|
|
break;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
case (byte)Report_Messages_Tokens_ENUM.result:
|
|||
|
|
case (byte)Report_Messages_Tokens_ENUM.result2:
|
|||
|
|
if (data[i + 1] != 0)
|
|||
|
|
{
|
|||
|
|
result = data[i + 1];
|
|||
|
|
}
|
|||
|
|
i = data[1] + 1; // exit
|
|||
|
|
break;
|
|||
|
|
case (byte)Report_Messages_Tokens_ENUM.result1:
|
|||
|
|
break;
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
break;
|
|||
|
|
|
|||
|
|
// -----------------------------------------------------------------------------------------------------------------------
|
|||
|
|
case (byte)Document_Identifiers_ENUM.Triggered_Location_Report_NoCDT:
|
|||
|
|
case (byte)Document_Identifiers_ENUM.Immediate_Location_Report_NoCDT:
|
|||
|
|
{
|
|||
|
|
if (data[0] == (byte)Document_Identifiers_ENUM.Triggered_Location_Report_NoCDT)
|
|||
|
|
{
|
|||
|
|
cell.triggered = true;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
pdata = (int)data[1];
|
|||
|
|
Boolean parsingError = false;
|
|||
|
|
DateTime gps_time = DateTime.MinValue;
|
|||
|
|
for (i = 2; i < data[1] + 2; i++)
|
|||
|
|
{
|
|||
|
|
// stop parsing if out of bound
|
|||
|
|
if (i >= data.Length)
|
|||
|
|
break;
|
|||
|
|
|
|||
|
|
switch (data[i])
|
|||
|
|
{
|
|||
|
|
case (byte)Report_Messages_Tokens_ENUM.point_2d:
|
|||
|
|
cell.lat = ProcessLat(data, i + 1, 4).ToString();
|
|||
|
|
cell.lng = ProcessLng(data, i + 1 + 4, 4).ToString();
|
|||
|
|
if (gps_time == DateTime.MinValue)
|
|||
|
|
cell.location_time = DateTime.Now.ToUniversalTime();
|
|||
|
|
if (DisplayConsole)
|
|||
|
|
{
|
|||
|
|
Utils.WriteLine("Point_2d: " + data[i].ToString("X"));
|
|||
|
|
Utils.WriteLine("Lat: " + Byte2String(data, i + 1, 4) + " =" + cell.lat);
|
|||
|
|
Utils.WriteLine("Lng: " + Byte2String(data, i + 1 + 4, 4) + " =" + cell.lng);
|
|||
|
|
}
|
|||
|
|
i += 8;
|
|||
|
|
break;
|
|||
|
|
|
|||
|
|
case (byte)Report_Messages_Tokens_ENUM.circle_2d:
|
|||
|
|
cell.lat = ProcessLat(data, i + 1, 4).ToString();
|
|||
|
|
cell.lng = ProcessLng(data, i + 1 + 4, 4).ToString();
|
|||
|
|
if (gps_time == DateTime.MinValue)
|
|||
|
|
cell.location_time = DateTime.Now.ToUniversalTime();
|
|||
|
|
cell.radius = ProcessUFloat2B(data, i + 1 + 8);
|
|||
|
|
|
|||
|
|
if (DisplayConsole)
|
|||
|
|
{
|
|||
|
|
Utils.WriteLine("Circle_2d: " + data[i].ToString("X"));
|
|||
|
|
Utils.WriteLine("Lat: " + Byte2String(data, i + 1, 4) + " =" + cell.lat);
|
|||
|
|
Utils.WriteLine("Lng: " + Byte2String(data, i + 1 + 4, 4) + " =" + cell.lng);
|
|||
|
|
Utils.WriteLine("Rad: " + Byte2String(data, i + 1 + 8, 2) + " =" + cell.radius);
|
|||
|
|
}
|
|||
|
|
i += 10;
|
|||
|
|
break;
|
|||
|
|
|
|||
|
|
|
|||
|
|
case (byte)Report_Messages_Tokens_ENUM.point_3d:
|
|||
|
|
case (byte)Report_Messages_Tokens_ENUM.point_3d1:
|
|||
|
|
{
|
|||
|
|
byte opCode = data[i];
|
|||
|
|
cell.lat = ProcessLat(data, i + 1, 4).ToString();
|
|||
|
|
cell.lng = ProcessLng(data, i + 1 + 4, 4).ToString();
|
|||
|
|
|
|||
|
|
if (gps_time == DateTime.MinValue)
|
|||
|
|
cell.location_time = DateTime.Now.ToUniversalTime();
|
|||
|
|
|
|||
|
|
cell.altitude = (int)ProcessUFloat2B(data, i + 1 + 8);
|
|||
|
|
|
|||
|
|
if (opCode == (byte)Report_Messages_Tokens_ENUM.point_3d1)
|
|||
|
|
{
|
|||
|
|
cell.altitude_accuracy = (int)ProcessUFloat2B(data, i + 1 + 10);
|
|||
|
|
i += 12;
|
|||
|
|
}
|
|||
|
|
else
|
|||
|
|
i += 10;
|
|||
|
|
|
|||
|
|
if (DisplayConsole)
|
|||
|
|
{
|
|||
|
|
Utils.WriteLine("Point_2d: " + data[i].ToString("X"));
|
|||
|
|
Utils.WriteLine("Lat: " + Byte2String(data, i + 1, 4) + " =" + cell.lat);
|
|||
|
|
Utils.WriteLine("Lng: " + Byte2String(data, i + 1 + 4, 4) + " =" + cell.lng);
|
|||
|
|
Utils.WriteLine("Alt: " + Byte2String(data, i + 1 + 8, 2) + " =" + cell.altitude);
|
|||
|
|
Utils.WriteLine("Ala: " + Byte2String(data, i + 1 + 10, 2) + " =" + cell.altitude_accuracy);
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
break;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
|
|||
|
|
case (byte)Report_Messages_Tokens_ENUM.circle_3d:
|
|||
|
|
case (byte)Report_Messages_Tokens_ENUM.circle_3d1:
|
|||
|
|
{
|
|||
|
|
byte opCode = data[i];
|
|||
|
|
|
|||
|
|
cell.lat = ProcessLat(data, i + 1, 4).ToString();
|
|||
|
|
cell.lng = ProcessLng(data, i + 1 + 4, 4).ToString();
|
|||
|
|
if (gps_time == DateTime.MinValue)
|
|||
|
|
cell.location_time = DateTime.Now.ToUniversalTime();
|
|||
|
|
if (data.Length > i + 1 + 8)
|
|||
|
|
{
|
|||
|
|
cell.radius = ProcessUFloat2B(data, i + 1 + 8);
|
|||
|
|
|
|||
|
|
cell.altitude = (int)ProcessUFloat2B(data, i + 1 + 10);
|
|||
|
|
|
|||
|
|
if (opCode == (byte)Report_Messages_Tokens_ENUM.circle_3d1)
|
|||
|
|
{
|
|||
|
|
cell.altitude_accuracy = (int)ProcessUFloat2B(data, i + 1 + 12);
|
|||
|
|
i += 14;
|
|||
|
|
}
|
|||
|
|
else
|
|||
|
|
i += 12;
|
|||
|
|
}
|
|||
|
|
else
|
|||
|
|
i += 6;
|
|||
|
|
|
|||
|
|
if (DisplayConsole)
|
|||
|
|
{
|
|||
|
|
Utils.WriteLine("Circle_3d: " + data[i].ToString("X"));
|
|||
|
|
Utils.WriteLine("Lat: " + Byte2String(data, i + 1, 4) + " =" + cell.lat);
|
|||
|
|
Utils.WriteLine("Lng: " + Byte2String(data, i + 1 + 4, 4) + " =" + cell.lng);
|
|||
|
|
Utils.WriteLine("Rad: " + Byte2String(data, i + 1 + 8, 2) + " =" + cell.radius);
|
|||
|
|
Utils.WriteLine("Alt: " + Byte2String(data, i + 1 + 10, 2) + " =" + cell.altitude);
|
|||
|
|
Utils.WriteLine("Ala: " + Byte2String(data, i + 1 + 12, 2) + " =" + cell.altitude_accuracy);
|
|||
|
|
}
|
|||
|
|
break;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
case (byte)Report_Messages_Tokens_ENUM.speed_vrt: //!! not tested
|
|||
|
|
i += 2;
|
|||
|
|
break;
|
|||
|
|
|
|||
|
|
case (byte)Report_Messages_Tokens_ENUM.speed_hor:
|
|||
|
|
cell.spd = Math.Round((ProcessUFloat2B(data, i + 1) * 3.6)).ToString();
|
|||
|
|
if (DisplayConsole)
|
|||
|
|
Utils.WriteLine(Report_Messages_Tokens_ENUM.speed_hor + ": " + Byte2String(data, i + 1, 2) + " =" + cell.spd);
|
|||
|
|
i += 2;
|
|||
|
|
break;
|
|||
|
|
case (byte)Report_Messages_Tokens_ENUM.lev_conf: //!! not tested
|
|||
|
|
cell.level_confidence = (int)data[i + 1];
|
|||
|
|
if (DisplayConsole)
|
|||
|
|
Utils.WriteLine(Report_Messages_Tokens_ENUM.lev_conf + ": " + Byte2String(data, i + 1, 1) + " =" + cell.level_confidence) ;
|
|||
|
|
i += 1;
|
|||
|
|
break;
|
|||
|
|
case (byte)Common_Element_Tokens_ENUM.request_id:
|
|||
|
|
{
|
|||
|
|
int length = (int)data[i + 1];
|
|||
|
|
// CSBK Location has a request ID of 3 Bytes
|
|||
|
|
if (length == 3)
|
|||
|
|
cell.isCSBK = true;
|
|||
|
|
cell.seq_ID = Byte2String(data, i + 2, length) + "";
|
|||
|
|
cell.seqID = Byte2Int64(data, i + 2, length);
|
|||
|
|
if (DisplayConsole)
|
|||
|
|
Utils.WriteLine(Common_Element_Tokens_ENUM.request_id + ": " + cell.seq_ID);
|
|||
|
|
i = i + 1 + length;
|
|||
|
|
break;
|
|||
|
|
}
|
|||
|
|
case (byte)Report_Messages_Tokens_ENUM.info_time:
|
|||
|
|
case (byte)Report_Messages_Tokens_ENUM.info_time1:
|
|||
|
|
gps_time = ProcessTime(data, i + 1, 5);
|
|||
|
|
Console.WriteLine(Report_Messages_Tokens_ENUM.info_time + ": " + Byte2String(data, i + 1, 5) + " =" + gps_time);
|
|||
|
|
i += 5;
|
|||
|
|
break;
|
|||
|
|
|
|||
|
|
// RESULTS
|
|||
|
|
case (byte)Report_Messages_Tokens_ENUM.result:
|
|||
|
|
case (byte)Report_Messages_Tokens_ENUM.result2:
|
|||
|
|
if (data[i + 1] != 0)
|
|||
|
|
{
|
|||
|
|
result = data[i + 1];
|
|||
|
|
if (data[i + 1] == (byte)Result_Codes_ENUM.QUERY_INFO_NOT_ATTAINABLE
|
|||
|
|
|| data[i + 1] == (byte)Result_Codes_ENUM.QUERY_INFO_NOT_CURRENTLY_ATTAINABLE)
|
|||
|
|
{
|
|||
|
|
cell.lat = "0";
|
|||
|
|
cell.lng = "0";
|
|||
|
|
cell.spd = "0";
|
|||
|
|
gps_time = DateTime.UtcNow;
|
|||
|
|
if (DisplayConsole)
|
|||
|
|
Utils.WriteLine("QUERY_INFO_NOT_ATTAINABLE");
|
|||
|
|
}
|
|||
|
|
else
|
|||
|
|
{
|
|||
|
|
parsingError = true;
|
|||
|
|
if (DisplayConsole)
|
|||
|
|
Utils.WriteLine("Error = " + ProcessError(data[i + 1]));
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
i = data[1] + 1; // exit
|
|||
|
|
break;
|
|||
|
|
case (byte)Report_Messages_Tokens_ENUM.result1:
|
|||
|
|
result = data[i + 1];
|
|||
|
|
break;
|
|||
|
|
case (byte)Report_Messages_Tokens_ENUM.direction_hor:
|
|||
|
|
i += 1;
|
|||
|
|
break;
|
|||
|
|
|
|||
|
|
default:
|
|||
|
|
Console.WriteLine("Unknown: 0x" + data[i].ToString("X"));
|
|||
|
|
break;
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
if (!parsingError)
|
|||
|
|
{
|
|||
|
|
cell.location_time = gps_time;
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
break;
|
|||
|
|
|
|||
|
|
// -----------------------------------------------------------------------------------------------------------------------
|
|||
|
|
default:
|
|||
|
|
Console.WriteLine("UNDEFINED 0x" + data[0].ToString("X"));
|
|||
|
|
break;
|
|||
|
|
}
|
|||
|
|
return cell;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
public htCell_t DecodePacket_Connect_plus(Byte[] data)
|
|||
|
|
{
|
|||
|
|
int i = 0, pdata;
|
|||
|
|
|
|||
|
|
htCell_t cell = new htCell_t();
|
|||
|
|
cell.lat = "";
|
|||
|
|
cell.lng = "";
|
|||
|
|
cell.di = "0";
|
|||
|
|
cell.heading = "";
|
|||
|
|
cell.isCSBK = false;
|
|||
|
|
cell.location_time = DateTime.MinValue;
|
|||
|
|
cell.activity_time = DateTime.MinValue;
|
|||
|
|
|
|||
|
|
switch (data[0])
|
|||
|
|
{
|
|||
|
|
// -----------------------------------------------------------------------------------------------------------------------
|
|||
|
|
case (byte)Document_Identifiers_ENUM.Triggered_Location_Stop_Answer_NoCDT:
|
|||
|
|
case (byte)Document_Identifiers_ENUM.Triggered_Location_Answer_NoCDT:
|
|||
|
|
if (data[0] == (byte)Document_Identifiers_ENUM.Triggered_Location_Stop_Answer_NoCDT)
|
|||
|
|
{
|
|||
|
|
if (DisplayConsole)
|
|||
|
|
Console.WriteLine(Document_Identifiers_ENUM.Triggered_Location_Stop_Answer_NoCDT);
|
|||
|
|
}
|
|||
|
|
else
|
|||
|
|
{
|
|||
|
|
if (DisplayConsole)
|
|||
|
|
Console.WriteLine(Document_Identifiers_ENUM.Triggered_Location_Answer_NoCDT);
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
|
|||
|
|
pdata = (int)data[1];
|
|||
|
|
for (i = 2; i < data[1] + 2; i++)
|
|||
|
|
{
|
|||
|
|
switch (data[i])
|
|||
|
|
{
|
|||
|
|
case (byte)Common_Element_Tokens_ENUM.request_id:
|
|||
|
|
{
|
|||
|
|
int length = (int)data[i + 1];
|
|||
|
|
// CSBK Location has a request ID of 3 Bytes
|
|||
|
|
if (length == 3)
|
|||
|
|
cell.isCSBK = true;
|
|||
|
|
cell.seq_ID = Byte2String(data, i + 2, length) + "";
|
|||
|
|
i = i + 1 + length;
|
|||
|
|
break;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
case (byte)Report_Messages_Tokens_ENUM.result:
|
|||
|
|
case (byte)Report_Messages_Tokens_ENUM.result2:
|
|||
|
|
if (DisplayConsole)
|
|||
|
|
Utils.WriteLine("Result: " + Byte2String(data, i + 1, 1));
|
|||
|
|
if (data[i + 1] != 0)
|
|||
|
|
{
|
|||
|
|
if (DisplayConsole)
|
|||
|
|
Utils.WriteLine("Error = " + ProcessError(data[i + 1]));
|
|||
|
|
}
|
|||
|
|
i = data[1] + 1; // exit
|
|||
|
|
break;
|
|||
|
|
case (byte)Report_Messages_Tokens_ENUM.result1:
|
|||
|
|
if (DisplayConsole)
|
|||
|
|
Utils.WriteLine("Result: OK");
|
|||
|
|
break;
|
|||
|
|
case (byte)Report_Messages_Tokens_ENUM.SU_IPv4:
|
|||
|
|
radioID = ProcessSU_ID(data, i + 1).ToString();
|
|||
|
|
i += 4;
|
|||
|
|
|
|||
|
|
break;
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
break;
|
|||
|
|
|
|||
|
|
// -----------------------------------------------------------------------------------------------------------------------
|
|||
|
|
case (byte)Document_Identifiers_ENUM.Triggered_Location_Report_NoCDT:
|
|||
|
|
case (byte)Document_Identifiers_ENUM.Immediate_Location_Report_NoCDT:
|
|||
|
|
{
|
|||
|
|
if (data[0] == (byte)Document_Identifiers_ENUM.Triggered_Location_Report_NoCDT)
|
|||
|
|
{
|
|||
|
|
cell.triggered = true;
|
|||
|
|
Console.WriteLine(Document_Identifiers_ENUM.Triggered_Location_Report_NoCDT);
|
|||
|
|
}
|
|||
|
|
else
|
|||
|
|
Console.WriteLine(Document_Identifiers_ENUM.Immediate_Location_Report_NoCDT);
|
|||
|
|
|
|||
|
|
pdata = (int)data[1];
|
|||
|
|
Boolean parsingError = false;
|
|||
|
|
DateTime gps_time = DateTime.MinValue;
|
|||
|
|
for (i = 2; i < data[1] + 2; i++)
|
|||
|
|
{
|
|||
|
|
switch (data[i])
|
|||
|
|
{
|
|||
|
|
case (byte)Report_Messages_Tokens_ENUM.point_2d:
|
|||
|
|
cell.lat = ProcessLat(data, i + 1, 4).ToString();
|
|||
|
|
cell.lng = ProcessLng(data, i + 1 + 4, 4).ToString();
|
|||
|
|
if (gps_time == DateTime.MinValue)
|
|||
|
|
cell.location_time = DateTime.Now.ToUniversalTime();
|
|||
|
|
if (DisplayConsole)
|
|||
|
|
{
|
|||
|
|
Utils.WriteLine("Point_2d: " + data[i].ToString("X"));
|
|||
|
|
Utils.WriteLine("Lat: " + Byte2String(data, i + 1, 4) + " =" + cell.lat);
|
|||
|
|
Utils.WriteLine("Lng: " + Byte2String(data, i + 1 + 4, 4) + " =" + cell.lng);
|
|||
|
|
}
|
|||
|
|
i += 8;
|
|||
|
|
break;
|
|||
|
|
|
|||
|
|
case (byte)Report_Messages_Tokens_ENUM.circle_2d:
|
|||
|
|
cell.lat = ProcessLat(data, i + 1, 4).ToString();
|
|||
|
|
cell.lng = ProcessLng(data, i + 1 + 4, 4).ToString();
|
|||
|
|
if (gps_time == DateTime.MinValue)
|
|||
|
|
cell.location_time = DateTime.Now.ToUniversalTime();
|
|||
|
|
cell.radius = ProcessUFloat2B(data, i + 1 + 8);
|
|||
|
|
|
|||
|
|
if (DisplayConsole)
|
|||
|
|
{
|
|||
|
|
Utils.WriteLine("Circle_2d: " + data[i].ToString("X"));
|
|||
|
|
Utils.WriteLine("Lat: " + Byte2String(data, i + 1, 4) + " =" + cell.lat);
|
|||
|
|
Utils.WriteLine("Lng: " + Byte2String(data, i + 1 + 4, 4) + " =" + cell.lng);
|
|||
|
|
Utils.WriteLine("Rad: " + Byte2String(data, i + 1 + 8, 2) + " =" + cell.radius);
|
|||
|
|
}
|
|||
|
|
i += 10;
|
|||
|
|
break;
|
|||
|
|
|
|||
|
|
|
|||
|
|
case (byte)Report_Messages_Tokens_ENUM.point_3d:
|
|||
|
|
case (byte)Report_Messages_Tokens_ENUM.point_3d1:
|
|||
|
|
{
|
|||
|
|
byte opCode = data[i];
|
|||
|
|
cell.lat = ProcessLat(data, i + 1, 4).ToString();
|
|||
|
|
cell.lng = ProcessLng(data, i + 1 + 4, 4).ToString();
|
|||
|
|
|
|||
|
|
if (gps_time == DateTime.MinValue)
|
|||
|
|
cell.location_time = DateTime.Now.ToUniversalTime();
|
|||
|
|
|
|||
|
|
cell.altitude = (int)ProcessUFloat2B(data, i + 1 + 8);
|
|||
|
|
|
|||
|
|
if (opCode == (byte)Report_Messages_Tokens_ENUM.point_3d1)
|
|||
|
|
{
|
|||
|
|
cell.altitude_accuracy = (int)ProcessUFloat2B(data, i + 1 + 10);
|
|||
|
|
i += 12;
|
|||
|
|
}
|
|||
|
|
else
|
|||
|
|
i += 10;
|
|||
|
|
|
|||
|
|
if (DisplayConsole)
|
|||
|
|
{
|
|||
|
|
Utils.WriteLine("Point_2d: " + data[i].ToString("X"));
|
|||
|
|
Utils.WriteLine("Lat: " + Byte2String(data, i + 1, 4) + " =" + cell.lat);
|
|||
|
|
Utils.WriteLine("Lng: " + Byte2String(data, i + 1 + 4, 4) + " =" + cell.lng);
|
|||
|
|
}
|
|||
|
|
break;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
|
|||
|
|
case (byte)Report_Messages_Tokens_ENUM.circle_3d:
|
|||
|
|
case (byte)Report_Messages_Tokens_ENUM.circle_3d1:
|
|||
|
|
{
|
|||
|
|
byte opCode = data[i];
|
|||
|
|
|
|||
|
|
cell.lat = ProcessLat(data, i + 1, 4).ToString();
|
|||
|
|
cell.lng = ProcessLng(data, i + 1 + 4, 4).ToString();
|
|||
|
|
if (gps_time == DateTime.MinValue)
|
|||
|
|
cell.location_time = DateTime.Now.ToUniversalTime();
|
|||
|
|
cell.radius = ProcessUFloat2B(data, i + 1 + 8);
|
|||
|
|
|
|||
|
|
cell.altitude = (int)ProcessUFloat2B(data, i + 1 + 10);
|
|||
|
|
|
|||
|
|
if (opCode == (byte)Report_Messages_Tokens_ENUM.circle_3d1)
|
|||
|
|
{
|
|||
|
|
cell.altitude_accuracy = (int)ProcessUFloat2B(data, i + 1 + 12);
|
|||
|
|
i += 14;
|
|||
|
|
}
|
|||
|
|
else
|
|||
|
|
i += 12;
|
|||
|
|
|
|||
|
|
if (DisplayConsole)
|
|||
|
|
{
|
|||
|
|
Utils.WriteLine("Circle_3d: " + data[i].ToString("X"));
|
|||
|
|
Utils.WriteLine("Lat: " + Byte2String(data, i + 1, 4) + " =" + cell.lat);
|
|||
|
|
Utils.WriteLine("Lng: " + Byte2String(data, i + 1 + 4, 4) + " =" + cell.lng);
|
|||
|
|
Utils.WriteLine("Rad: " + Byte2String(data, i + 1 + 8, 2) + " =" + cell.radius);
|
|||
|
|
Utils.WriteLine("Alt: " + Byte2String(data, i + 1 + 10, 2));
|
|||
|
|
Utils.WriteLine("Ala: " + Byte2String(data, i + 1 + 12, 2));
|
|||
|
|
}
|
|||
|
|
break;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
case (byte)Report_Messages_Tokens_ENUM.speed_vrt: //!! not tested
|
|||
|
|
if (DisplayConsole)
|
|||
|
|
Utils.WriteLine(Report_Messages_Tokens_ENUM.speed_vrt + ": " + Byte2String(data, i + 1, 2));
|
|||
|
|
i += 2;
|
|||
|
|
break;
|
|||
|
|
case (byte)Report_Messages_Tokens_ENUM.speed_hor:
|
|||
|
|
cell.spd = Math.Round((ProcessUFloat2B(data, i + 1) * 3.6)).ToString();
|
|||
|
|
if (DisplayConsole)
|
|||
|
|
Utils.WriteLine(Report_Messages_Tokens_ENUM.speed_hor + ": " + Byte2String(data, i + 1, 2) + " =" + cell.spd);
|
|||
|
|
i += 2;
|
|||
|
|
break;
|
|||
|
|
case (byte)Common_Element_Tokens_ENUM.request_id:
|
|||
|
|
{
|
|||
|
|
int length = (int)data[i + 1];
|
|||
|
|
// CSBK Location has a request ID of 3 Bytes
|
|||
|
|
if (length == 3)
|
|||
|
|
cell.isCSBK = true;
|
|||
|
|
cell.seq_ID = Byte2String(data, i + 2, length) + "";
|
|||
|
|
i = i + 1 + length;
|
|||
|
|
break;
|
|||
|
|
}
|
|||
|
|
case (byte)Report_Messages_Tokens_ENUM.info_time:
|
|||
|
|
case (byte)Report_Messages_Tokens_ENUM.info_time1:
|
|||
|
|
gps_time = ProcessTime(data, i + 1, 5);
|
|||
|
|
if (DisplayConsole)
|
|||
|
|
Utils.WriteLine(Report_Messages_Tokens_ENUM.info_time + ": " + Byte2String(data, i + 1, 5) + " =" + gps_time);
|
|||
|
|
i += 5;
|
|||
|
|
break;
|
|||
|
|
|
|||
|
|
// RESULTS
|
|||
|
|
case (byte)Report_Messages_Tokens_ENUM.result:
|
|||
|
|
case (byte)Report_Messages_Tokens_ENUM.result2:
|
|||
|
|
if (data[i + 1] != 0)
|
|||
|
|
{
|
|||
|
|
result = data[i + 1];
|
|||
|
|
if (data[i + 1] == (byte)Result_Codes_ENUM.QUERY_INFO_NOT_ATTAINABLE
|
|||
|
|
|| data[i + 1] == (byte)Result_Codes_ENUM.QUERY_INFO_NOT_CURRENTLY_ATTAINABLE)
|
|||
|
|
{
|
|||
|
|
cell.lat = "0";
|
|||
|
|
cell.lng = "0";
|
|||
|
|
cell.spd = "0";
|
|||
|
|
gps_time = DateTime.UtcNow;
|
|||
|
|
if (DisplayConsole)
|
|||
|
|
Utils.WriteLine("QUERY_INFO_NOT_ATTAINABLE");
|
|||
|
|
}
|
|||
|
|
else
|
|||
|
|
{
|
|||
|
|
parsingError = true;
|
|||
|
|
if (DisplayConsole)
|
|||
|
|
Utils.WriteLine("Error = " + ProcessError(data[i + 1]));
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
i = data[1] + 1; // exit
|
|||
|
|
break;
|
|||
|
|
case (byte)Report_Messages_Tokens_ENUM.result1:
|
|||
|
|
break;
|
|||
|
|
case (byte)Report_Messages_Tokens_ENUM.SU_IPv4:
|
|||
|
|
radioID = ProcessSU_ID(data, i + 1).ToString();
|
|||
|
|
i += 4;
|
|||
|
|
break;
|
|||
|
|
|
|||
|
|
default:
|
|||
|
|
break;
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
if (!parsingError)
|
|||
|
|
{
|
|||
|
|
cell.location_time = gps_time;
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
break;
|
|||
|
|
|
|||
|
|
// -----------------------------------------------------------------------------------------------------------------------
|
|||
|
|
default:
|
|||
|
|
Console.WriteLine("UNDEFINED 0x" + data[0].ToString("X"));
|
|||
|
|
break;
|
|||
|
|
}
|
|||
|
|
return cell;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
public void UpdateDBDirect(DBsubsOperationManager DBsub)
|
|||
|
|
{
|
|||
|
|
DBsub.insertDIRECTWithAltitude(radioID, cell.lat, cell.lng, (Int32)DateTo70Format(cell.location_time), cell.spd, cell.di, "", cell.alt, cell.logId);
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
// -------------------------------------------------------------------
|
|||
|
|
// Aux functions
|
|||
|
|
// -------------------------------------------------------------------
|
|||
|
|
|
|||
|
|
uint DateTo70Format(DateTime param)
|
|||
|
|
{
|
|||
|
|
long nOfSeconds;
|
|||
|
|
System.DateTime dt70 = new DateTime(1970, 1, 1, 0, 0, 0, DateTimeKind.Utc);
|
|||
|
|
TimeSpan span = param - dt70;
|
|||
|
|
nOfSeconds = (long)span.TotalSeconds;
|
|||
|
|
return ((uint)nOfSeconds);
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
public string Byte2String(byte[] data)
|
|||
|
|
{
|
|||
|
|
|
|||
|
|
string sdata = "";
|
|||
|
|
int i;
|
|||
|
|
|
|||
|
|
try
|
|||
|
|
{
|
|||
|
|
for (i = 0; i < data.Length; i++)
|
|||
|
|
{
|
|||
|
|
int ii;
|
|||
|
|
ii = (int)data[i];
|
|||
|
|
sdata += "0x" + ii.ToString("X") + " ";
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
catch (Exception e)
|
|||
|
|
{
|
|||
|
|
Console.WriteLine("Error converting to string!\n"+e.ToString());
|
|||
|
|
}
|
|||
|
|
return sdata;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
public string Byte2String(byte[] data, int startIndex, int len)
|
|||
|
|
{
|
|||
|
|
string sdata = "";
|
|||
|
|
int i;
|
|||
|
|
|
|||
|
|
if (startIndex > data.Length)
|
|||
|
|
return "";
|
|||
|
|
|
|||
|
|
for (i = startIndex; i < startIndex + len && i < data.Length; i++)
|
|||
|
|
{
|
|||
|
|
int ii;
|
|||
|
|
ii = (int)data[i];
|
|||
|
|
sdata += "0x" + ii.ToString("X") + " ";
|
|||
|
|
}
|
|||
|
|
return sdata;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
/* convert a byte array to the corresponding Int64*/
|
|||
|
|
public Int64 Byte2Int64(byte[] data, int startIndex, int len)
|
|||
|
|
{
|
|||
|
|
|
|||
|
|
Int64 val = 0;
|
|||
|
|
int i, j = len;
|
|||
|
|
|
|||
|
|
if (startIndex > data.Length)
|
|||
|
|
return -1;
|
|||
|
|
|
|||
|
|
for (i = startIndex; i < startIndex + len && i < data.Length; i++)
|
|||
|
|
{
|
|||
|
|
Int64 ii;
|
|||
|
|
ii = (Int64)data[i];
|
|||
|
|
|
|||
|
|
val = val + ii * (Int64)(Math.Pow(256, --j));
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
return val;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
|
|||
|
|
|
|||
|
|
private string ProcessError(Byte err)
|
|||
|
|
{
|
|||
|
|
switch (err)
|
|||
|
|
{
|
|||
|
|
case (byte)Result_Codes_ENUM.SUCCESS: return "" + Result_Codes_ENUM.SUCCESS;
|
|||
|
|
case (byte)Result_Codes_ENUM.UNSUPPORTED_VERSION: return "" + Result_Codes_ENUM.UNSUPPORTED_VERSION;
|
|||
|
|
case (byte)Result_Codes_ENUM.SYNTAX_ERROR: return "" + Result_Codes_ENUM.SYNTAX_ERROR;
|
|||
|
|
case (byte)Result_Codes_ENUM.PROTOCOL_ELEMENT_NOT_SUPPORTED: return "" + Result_Codes_ENUM.PROTOCOL_ELEMENT_NOT_SUPPORTED;
|
|||
|
|
case (byte)Result_Codes_ENUM.PROTOCOL_ELEMENT_VALUE_OUT_OF_RANGE: return "" + Result_Codes_ENUM.PROTOCOL_ELEMENT_VALUE_OUT_OF_RANGE;
|
|||
|
|
case (byte)Result_Codes_ENUM.QUERY_INFO_NOT_ATTAINABLE: return "" + Result_Codes_ENUM.QUERY_INFO_NOT_ATTAINABLE;
|
|||
|
|
case (byte)Result_Codes_ENUM.QUERY_INFO_NOT_CURRENTLY_ATTAINABLE: return "" + Result_Codes_ENUM.QUERY_INFO_NOT_CURRENTLY_ATTAINABLE;
|
|||
|
|
case (byte)Result_Codes_ENUM.REPORTING_WILL_STOP: return "" + Result_Codes_ENUM.REPORTING_WILL_STOP;
|
|||
|
|
case (byte)Result_Codes_ENUM.NO_SUCH_REQUEST: return "" + Result_Codes_ENUM.NO_SUCH_REQUEST;
|
|||
|
|
case (byte)Result_Codes_ENUM.DUPLICATE_REQUEST_ID: return "" + Result_Codes_ENUM.DUPLICATE_REQUEST_ID;
|
|||
|
|
}
|
|||
|
|
return "Unknown err";
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
private double ProcessUFloat2B(Byte[] data, int startIndex)
|
|||
|
|
{
|
|||
|
|
int aux = data[startIndex];
|
|||
|
|
aux &= 0xff;
|
|||
|
|
int int_part = 0;
|
|||
|
|
int float_part = 0;
|
|||
|
|
int i = 0;
|
|||
|
|
|
|||
|
|
// get the int part
|
|||
|
|
while ((aux & 0x80) != 0)
|
|||
|
|
{
|
|||
|
|
int_part |= (aux ^ 0x80);
|
|||
|
|
int_part <<= 7;
|
|||
|
|
i++;
|
|||
|
|
aux = data[startIndex + i];
|
|||
|
|
aux &= 0xff;
|
|||
|
|
}
|
|||
|
|
int_part |= aux;
|
|||
|
|
|
|||
|
|
// ignore the float part
|
|||
|
|
i++;
|
|||
|
|
aux = data[startIndex + i];
|
|||
|
|
aux &= 0xff;
|
|||
|
|
while ((aux & 0x80) != 0)
|
|||
|
|
{
|
|||
|
|
float_part |= (aux ^ 0x80);
|
|||
|
|
float_part <<= 7;
|
|||
|
|
i++;
|
|||
|
|
aux = data[startIndex + i];
|
|||
|
|
aux &= 0xff;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
float_part |= aux;
|
|||
|
|
|
|||
|
|
return (double)int_part;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
private DateTime ProcessTime(Byte[] data, int startIndex, int len)
|
|||
|
|
{
|
|||
|
|
// (yyyy*2^26) + (MM*2^22) + (DD*2^17) + (hh*2^12) + (mm*2^6) + ss
|
|||
|
|
Byte B1 = data[startIndex];
|
|||
|
|
Byte B2 = data[startIndex + 1];
|
|||
|
|
Byte B3 = data[startIndex + 2];
|
|||
|
|
Byte B4 = data[startIndex + 3];
|
|||
|
|
Byte B5 = data[startIndex + 4];
|
|||
|
|
|
|||
|
|
int hh, mm, ss, YY, MM, DD;
|
|||
|
|
ss = B5 & 0x3F; // remove first 2b
|
|||
|
|
|
|||
|
|
mm = B4 & 0x0F;
|
|||
|
|
mm <<= 2;
|
|||
|
|
mm |= ((B5 >> 6) & 0x03);
|
|||
|
|
|
|||
|
|
hh = B3 & 0x01;
|
|||
|
|
hh <<= 4;
|
|||
|
|
hh |= ((B4 & 0xf0) >> 4);
|
|||
|
|
|
|||
|
|
DD = B3 & 0x3E;
|
|||
|
|
DD >>= 1;
|
|||
|
|
|
|||
|
|
MM = B2 & 0x03;
|
|||
|
|
MM <<= 2;
|
|||
|
|
MM |= ((B3 >> 6) & 0x03);
|
|||
|
|
|
|||
|
|
YY = B1;
|
|||
|
|
YY <<= 6;
|
|||
|
|
YY |= ((B2 >> 2) & 0x3F);
|
|||
|
|
|
|||
|
|
return new DateTime(YY, MM, DD, hh, mm, ss, DateTimeKind.Utc);
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
private double ProcessLat(Byte[] data, int startIndex, int len)
|
|||
|
|
{
|
|||
|
|
bool sign = false;
|
|||
|
|
if ((data[startIndex] & 0x80) != 0)
|
|||
|
|
{
|
|||
|
|
sign = true;
|
|||
|
|
}
|
|||
|
|
ulong l = (ulong)data[startIndex];
|
|||
|
|
if (sign)
|
|||
|
|
l ^= 0x80; // remove the sign
|
|||
|
|
|
|||
|
|
l <<= 8;
|
|||
|
|
l |= (ulong)data[startIndex + 1];
|
|||
|
|
l <<= 8;
|
|||
|
|
l |= (ulong)data[startIndex + 2];
|
|||
|
|
l <<= 8;
|
|||
|
|
l |= (ulong)data[startIndex + 3];
|
|||
|
|
|
|||
|
|
|
|||
|
|
double ld = (double)l;
|
|||
|
|
ld *= 90;
|
|||
|
|
ld /= 1024;
|
|||
|
|
ld /= 1024;
|
|||
|
|
ld /= 1024;
|
|||
|
|
ld /= 2;
|
|||
|
|
if (sign)
|
|||
|
|
ld *= -1;
|
|||
|
|
|
|||
|
|
return ld;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
private double ProcessLng(Byte[] data, int startIndex, int len)
|
|||
|
|
{
|
|||
|
|
bool sign = false;
|
|||
|
|
if ((data[startIndex] & 0x80) != 0)
|
|||
|
|
{
|
|||
|
|
sign = true;
|
|||
|
|
}
|
|||
|
|
ulong l = (ulong)data[startIndex];
|
|||
|
|
if (sign)
|
|||
|
|
l ^= 0x80; // remove the sign
|
|||
|
|
|
|||
|
|
l <<= 8;
|
|||
|
|
l |= (ulong)data[startIndex + 1];
|
|||
|
|
l <<= 8;
|
|||
|
|
l |= (ulong)data[startIndex + 2];
|
|||
|
|
l <<= 8;
|
|||
|
|
l |= (ulong)data[startIndex + 3];
|
|||
|
|
|
|||
|
|
double ld = (double)l;
|
|||
|
|
ld *= 360;
|
|||
|
|
ld /= 1024;
|
|||
|
|
ld /= 1024;
|
|||
|
|
ld /= 1024;
|
|||
|
|
ld /= 4;
|
|||
|
|
|
|||
|
|
if (sign)
|
|||
|
|
{
|
|||
|
|
ld = 180 - ld;
|
|||
|
|
ld *= -1;
|
|||
|
|
}
|
|||
|
|
return ld;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
private int ProcessSU_ID(Byte[] data, int startIndex)
|
|||
|
|
{
|
|||
|
|
Byte B1 = data[startIndex];
|
|||
|
|
Byte B2 = data[startIndex + 1];
|
|||
|
|
Byte B3 = data[startIndex + 2];
|
|||
|
|
Byte B4 = data[startIndex + 3];
|
|||
|
|
|
|||
|
|
int radioID = B4 + B3 * 256 + B2 * 256 * 256;
|
|||
|
|
return radioID;
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
}
|