SafeDispatch/SafeMobileLIB_DLL/MessageDecoders/LocationDecoder.cs

952 lines
43 KiB
C#
Raw Normal View History

2024-02-22 16:43:59 +00:00
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;
}
}
}