935 lines
45 KiB
C#
935 lines
45 KiB
C#
using System;
|
|
using System.Collections.Generic;
|
|
using System.Text;
|
|
using System.Collections;
|
|
|
|
namespace SafeNetLib
|
|
{
|
|
public class LocationDecoder
|
|
{
|
|
private int ACCEPTED_LEVEL_OF_CONFIDENCE = 20;
|
|
public htCell_t cell;
|
|
public byte result;
|
|
private string radioID;
|
|
public string RadioID
|
|
{
|
|
get { return radioID; }
|
|
set { radioID = value; }
|
|
}
|
|
|
|
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)
|
|
{
|
|
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(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();
|
|
retCell.lat = "";
|
|
retCell.lng = "";
|
|
retCell.di = "0";
|
|
retCell.seq_ID = "";
|
|
retCell.seqID = 0;
|
|
retCell.isCSBK = false;
|
|
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];
|
|
|
|
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.isCSBK = false;
|
|
cell.location_time = DateTime.MinValue;
|
|
cell.activity_time = DateTime.MinValue;
|
|
cell.triggered = false;
|
|
cell.level_confidence = -1;
|
|
|
|
result = (byte)Result_Codes_ENUM.SUCCESS;
|
|
|
|
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)
|
|
Utils.ConsWrite(DebugMSG_Type.always, Document_Identifiers_ENUM.Triggered_Location_Stop_Answer_NoCDT + "");
|
|
else
|
|
Utils.ConsWrite(DebugMSG_Type.always, Document_Identifiers_ENUM.Triggered_Location_Answer_NoCDT + "");
|
|
|
|
pdata = (int)data[1];
|
|
//Console.WriteLine("Length =" + pdata + "(0x" + pdata.ToString("X") + ")");
|
|
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);
|
|
//Console.WriteLine(Common_Element_Tokens_ENUM.request_id + ": " + cell.seq_ID);
|
|
i = i + 1 + length;
|
|
break;
|
|
}
|
|
case (byte)Report_Messages_Tokens_ENUM.result:
|
|
case (byte)Report_Messages_Tokens_ENUM.result2:
|
|
Console.WriteLine("Result: " + Byte2String(data, i + 1, 1));
|
|
if (data[i + 1] != 0)
|
|
{
|
|
result = data[i + 1];
|
|
Utils.ConsWrite(DebugMSG_Type.ARS, "Location Error = " + ProcessError(data[i + 1]));
|
|
}
|
|
//Console.WriteLine("OpaqueData: " + Byte2String(data, i + 2, data[1] - i));
|
|
i = data[1] + 1; // exit
|
|
break;
|
|
case (byte)Report_Messages_Tokens_ENUM.result1:
|
|
//Console.WriteLine("Result: OK");
|
|
break;
|
|
}
|
|
}
|
|
break;
|
|
|
|
// -----------------------------------------------------------------------------------------------------------------------
|
|
case (byte)Document_Identifiers_ENUM.Triggered_Location_Report_NoCDT:
|
|
// cell.triggered = true;
|
|
case (byte)Document_Identifiers_ENUM.Immediate_Location_Report_NoCDT:
|
|
{
|
|
if (data[0] == (byte)Document_Identifiers_ENUM.Triggered_Location_Report_NoCDT)
|
|
{
|
|
cell.triggered = true;
|
|
//Utils.ConsWrite(DebugMSG_Type.always, Document_Identifiers_ENUM.Triggered_Location_Report_NoCDT + "");
|
|
}
|
|
else
|
|
{
|
|
//Utils.ConsWrite(DebugMSG_Type.always, 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();
|
|
//Console.WriteLine("Point_2d: " + data[i].ToString("X"));
|
|
//Console.WriteLine("Lat: " + Byte2String(data, i + 1, 4) + " =" + cell.lat);
|
|
//Console.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);
|
|
|
|
//Console.WriteLine("Circle_2d: " + data[i].ToString("X"));
|
|
//Console.WriteLine("Lat: " + Byte2String(data, i + 1, 4) + " =" + cell.lat);
|
|
//Console.WriteLine("Lng: " + Byte2String(data, i + 1 + 4, 4) + " =" + cell.lng);
|
|
//Console.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;
|
|
|
|
//Console.WriteLine("Point_2d: " + data[i].ToString("X"));
|
|
//Console.WriteLine("Lat: " + Byte2String(data, i + 1, 4) + " =" + cell.lat);
|
|
//Console.WriteLine("Lng: " + Byte2String(data, i + 1 + 4, 4) + " =" + cell.lng);
|
|
//Console.WriteLine("Alt: " + Byte2String(data, i + 1 + 8, 2) + " =" + cell.altitude);
|
|
//Console.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();
|
|
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;
|
|
|
|
//Console.WriteLine("Circle_3d: " + data[i].ToString("X"));
|
|
//Console.WriteLine("Lat: " + Byte2String(data, i + 1, 4) + " =" + cell.lat);
|
|
//Console.WriteLine("Lng: " + Byte2String(data, i + 1 + 4, 4) + " =" + cell.lng);
|
|
//Console.WriteLine("Rad: " + Byte2String(data, i + 1 + 8, 2) + " =" + cell.radius);
|
|
//Console.WriteLine("Alt: " + Byte2String(data, i + 1 + 10, 2) + " =" + cell.altitude);
|
|
//Console.WriteLine("Ala: " + Byte2String(data, i + 1 + 12, 2) + " =" + cell.altitude_accuracy);
|
|
break;
|
|
}
|
|
|
|
case (byte)Report_Messages_Tokens_ENUM.speed_vrt: //!! not tested
|
|
cell.spd_v = Math.Round((ProcessUFloat2B(data, i + 1) * 3.6)).ToString();
|
|
Console.WriteLine(Report_Messages_Tokens_ENUM.speed_vrt + ": " + Byte2String(data, i + 1, 2) + " =" + cell.spd_v);
|
|
LOGS.LOG("speed_ver : " + Byte2String(data, i + 1, 2) + " = " + cell.spd_v);
|
|
i += 2;
|
|
break;
|
|
case (byte)Report_Messages_Tokens_ENUM.speed_hor:
|
|
cell.spd = Math.Round((ProcessUFloat2B(data, i + 1) * 3.6)).ToString();
|
|
Console.WriteLine(Report_Messages_Tokens_ENUM.speed_hor + ": " + Byte2String(data, i + 1, 2) + " =" + cell.spd);
|
|
LOGS.LOG("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];
|
|
//Console.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);
|
|
//Console.WriteLine(Common_Element_Tokens_ENUM.request_id + ": " + cell.seq_ID);
|
|
i = i + 1 + length;
|
|
break;
|
|
}
|
|
//cell.seq_ID = Byte2Int64(data, i + 2, 5)+"";
|
|
//Console.WriteLine(Common_Element_Tokens_ENUM.request_id + ": " + (cell.seq_ID = Byte2String(data, i + 1, 5) + ""));
|
|
//Console.WriteLine(Common_Element_Tokens_ENUM.request_id + ": " + (cell.seq_ID = Byte2Int64(data, i + 2, 5)+""));
|
|
//i += 5;
|
|
//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:
|
|
//Console.WriteLine("Result: " + Byte2String(data, i + 1, 1));
|
|
// fix case when subscriber is inside and sends more data than normal, so the parsing is messed up
|
|
// and in some cases the altitude contains 0x37 = result
|
|
if (cell.lat.Length > 0)
|
|
break;
|
|
|
|
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;
|
|
//Console.WriteLine("INFO got QUERY_INFO_NOT_ATTAINABLE ...adding values lat=0,lng=0,time =DateTime.UtcNow");
|
|
}
|
|
else
|
|
{
|
|
parsingError = true;
|
|
/*
|
|
Utils.ConsWrite(DebugMSG_Type.DB, "___________________________");
|
|
Utils.ConsWrite(DebugMSG_Type.DB, "Error processing position " + ProcessError(data[i + 1]));
|
|
Utils.ConsWrite(DebugMSG_Type.DB, "___________________________");
|
|
*/
|
|
}
|
|
}
|
|
//Console.WriteLine("OpaqueData: " + Byte2String(data, i + 2, data[1] - i));
|
|
i = data[1] + 1; // exit
|
|
break;
|
|
case (byte)Report_Messages_Tokens_ENUM.result1:
|
|
result = data[i + 1];
|
|
//Console.WriteLine("Result: OK");
|
|
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;
|
|
}
|
|
|
|
if (cell.level_confidence == -1)
|
|
{
|
|
// message didn't contained any level of confidence information
|
|
}
|
|
else if (cell.level_confidence < ACCEPTED_LEVEL_OF_CONFIDENCE)
|
|
{
|
|
cell.lat = "0";
|
|
cell.lng = "0";
|
|
cell.spd = "0";
|
|
cell.location_time = DateTime.UtcNow;
|
|
}
|
|
|
|
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.seq_ID = "";
|
|
cell.seqID = 0;
|
|
cell.isCSBK = false;
|
|
cell.location_time = DateTime.MinValue;
|
|
cell.activity_time = DateTime.MinValue;
|
|
cell.level_confidence = -1;
|
|
|
|
result = (byte)Result_Codes_ENUM.SUCCESS;
|
|
|
|
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)
|
|
Console.WriteLine(Document_Identifiers_ENUM.Triggered_Location_Stop_Answer_NoCDT);
|
|
else Console.WriteLine(Document_Identifiers_ENUM.Triggered_Location_Answer_NoCDT);
|
|
|
|
pdata = (int)data[1];
|
|
//Console.WriteLine("Length =" + pdata + "(0x" + pdata.ToString("X") + ")");
|
|
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);
|
|
//Console.WriteLine(Common_Element_Tokens_ENUM.request_id + ": " + cell.seq_ID);
|
|
i = i + 1 + length;
|
|
break;
|
|
}
|
|
|
|
case (byte)Report_Messages_Tokens_ENUM.result:
|
|
case (byte)Report_Messages_Tokens_ENUM.result2:
|
|
Console.WriteLine("Result: " + Byte2String(data, i + 1, 1));
|
|
result = data[i + 1];
|
|
|
|
if (data[i + 1] != 0)
|
|
{
|
|
Console.WriteLine("Error = " + ProcessError(data[i + 1]));
|
|
}
|
|
//Console.WriteLine("OpaqueData: " + Byte2String(data, i + 2, data[1] - i));
|
|
i = data[1] + 1; // exit
|
|
break;
|
|
case (byte)Report_Messages_Tokens_ENUM.result1:
|
|
Console.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();
|
|
//Console.WriteLine("Point_2d: " + data[i].ToString("X"));
|
|
//Console.WriteLine("Lat: " + Byte2String(data, i + 1, 4) + " =" + cell.lat);
|
|
//Console.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);
|
|
|
|
//Console.WriteLine("Circle_2d: " + data[i].ToString("X"));
|
|
//Console.WriteLine("Lat: " + Byte2String(data, i + 1, 4) + " =" + cell.lat);
|
|
//Console.WriteLine("Lng: " + Byte2String(data, i + 1 + 4, 4) + " =" + cell.lng);
|
|
//Console.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;
|
|
|
|
//Console.WriteLine("Point_2d: " + data[i].ToString("X"));
|
|
//Console.WriteLine("Lat: " + Byte2String(data, i + 1, 4) + " =" + cell.lat);
|
|
//Console.WriteLine("Lng: " + Byte2String(data, i + 1 + 4, 4) + " =" + cell.lng);
|
|
//Console.WriteLine("Alt: " + Byte2String(data, i + 1 + 8, 2) + " =" + cell.altitude);
|
|
//Console.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();
|
|
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;
|
|
|
|
//Console.WriteLine("Circle_3d: " + data[i].ToString("X"));
|
|
//Console.WriteLine("Lat: " + Byte2String(data, i + 1, 4) + " =" + cell.lat);
|
|
//Console.WriteLine("Lng: " + Byte2String(data, i + 1 + 4, 4) + " =" + cell.lng);
|
|
//Console.WriteLine("Rad: " + Byte2String(data, i + 1 + 8, 2) + " =" + cell.radius);
|
|
//Console.WriteLine("Alt: " + Byte2String(data, i + 1 + 10, 2) + " =" + cell.altitude);
|
|
//Console.WriteLine("Ala: " + Byte2String(data, i + 1 + 12, 2) + " =" + cell.altitude_accuracy);
|
|
break;
|
|
}
|
|
|
|
|
|
case (byte)Report_Messages_Tokens_ENUM.speed_vrt: //!! not tested
|
|
cell.spd_v = ((int)data[i+1] * 3.6).ToString();
|
|
Console.WriteLine(Report_Messages_Tokens_ENUM.speed_vrt + ": " + Byte2String(data, i + 1, 2) + " =" + cell.spd_v);
|
|
i += 2;
|
|
break;
|
|
case (byte)Report_Messages_Tokens_ENUM.speed_hor:
|
|
cell.spd = Math.Round((ProcessUFloat2B(data, i + 1) * 3.6)).ToString();
|
|
Console.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) + "";
|
|
cell.seqID = Byte2Int64(data, i + 2, length);
|
|
//Console.WriteLine(Common_Element_Tokens_ENUM.request_id + ": " + cell.seq_ID);
|
|
i = i + 1 + length;
|
|
break;
|
|
}
|
|
//cell.seq_ID = Byte2Int64(data, i + 2, 5)+"";
|
|
//Console.WriteLine(Common_Element_Tokens_ENUM.request_id + ": " + (cell.seq_ID = Byte2String(data, i + 1, 5) + ""));
|
|
//Console.WriteLine(Common_Element_Tokens_ENUM.request_id + ": " + (cell.seq_ID = Byte2Int64(data, i + 2, 5)+""));
|
|
//i += 5;
|
|
//break;
|
|
case (byte)Report_Messages_Tokens_ENUM.lev_conf: //!! not tested
|
|
cell.level_confidence = (int)Math.Round((ProcessUFloat2B(data, i + 1)));
|
|
Console.WriteLine(Report_Messages_Tokens_ENUM.lev_conf + ": " + Byte2String(data, i + 1, 2) + " =" + cell.level_confidence);
|
|
i += 2;
|
|
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:
|
|
//Console.WriteLine("Result: " + Byte2String(data, i + 1, 1));
|
|
// fix case when subscriber is inside and sends more data than normal, so the parsing is messed up
|
|
// and in some cases the altitude contains 0x37 = result
|
|
if (cell.lat.Length > 0)
|
|
break;
|
|
|
|
if (data[i + 1] != 0)
|
|
{
|
|
result = data[i + 1];
|
|
if (data[i + 1] == (byte)Result_Codes_ENUM.QUERY_INFO_NOT_ATTAINABLE)
|
|
{
|
|
cell.lat = "0";
|
|
cell.lng = "0";
|
|
cell.spd = "0";
|
|
gps_time = DateTime.UtcNow;
|
|
//Console.WriteLine("INFO got QUERY_INFO_NOT_ATTAINABLE ...adding values lat=0,lng=0,time =DateTime.UtcNow");
|
|
}
|
|
else
|
|
{
|
|
parsingError = true;
|
|
Utils.ConsWrite(DebugMSG_Type.DB, "___________________________");
|
|
Utils.ConsWrite(DebugMSG_Type.DB, "Error processing position " + ProcessError(data[i + 1]));
|
|
Utils.ConsWrite(DebugMSG_Type.DB, "___________________________");
|
|
}
|
|
}
|
|
//Console.WriteLine("OpaqueData: " + Byte2String(data, i + 2, data[1] - i));
|
|
i = data[1] + 1; // exit
|
|
break;
|
|
case (byte)Report_Messages_Tokens_ENUM.result1:
|
|
Console.WriteLine("Result: OK");
|
|
break;
|
|
case (byte)Report_Messages_Tokens_ENUM.SU_IPv4:
|
|
radioID = ProcessSU_ID(data, i + 1).ToString();
|
|
i += 4;
|
|
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;
|
|
}
|
|
|
|
if (cell.level_confidence == -1)
|
|
{
|
|
// message didn't contained any level of confidence information
|
|
}
|
|
else if (cell.level_confidence < ACCEPTED_LEVEL_OF_CONFIDENCE)
|
|
{
|
|
cell.lat = "0";
|
|
cell.lng = "0";
|
|
cell.spd = "0";
|
|
cell.location_time = DateTime.UtcNow;
|
|
}
|
|
|
|
return cell;
|
|
}
|
|
|
|
|
|
// -------------------------------------------------------------------
|
|
// Aux functions
|
|
// -------------------------------------------------------------------
|
|
|
|
uint DateTo70Format(DateTime param)
|
|
{
|
|
long nOfSeconds;
|
|
//Console.WriteLine("DateTo70Format param=" + param);
|
|
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 = 0, j = len-1;
|
|
try
|
|
{
|
|
for (i = startIndex; i < startIndex + len && i < data.Length; i++)
|
|
{
|
|
val = val + (int)data[i] * ((Int64)Math.Pow(16, j--));
|
|
}
|
|
}
|
|
catch (Exception ee) { Console.WriteLine("\n" + ee.ToString()); }
|
|
|
|
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.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 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++;
|
|
while ((data[startIndex + i] & 0x80) != 0)
|
|
{
|
|
i++;
|
|
}
|
|
|
|
//Console.WriteLine();
|
|
return (double)int_part;
|
|
}
|
|
/*
|
|
private double ProcessUFloat2B(Byte[] data, int startIndex, int endIndex)
|
|
{
|
|
int aux = data[startIndex];
|
|
aux &= 0xff;
|
|
int int_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++;
|
|
while ((data[startIndex + i] & 0x80) != 0)
|
|
{
|
|
i++;
|
|
}
|
|
|
|
Console.WriteLine();
|
|
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);
|
|
|
|
//Console.WriteLine("GPSTime : " + YY + "/" + MM + "/" + DD + " " + hh + ":" + mm + ":" + ss);
|
|
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];
|
|
|
|
|
|
//Console.WriteLine("lat ulong=0x" + l.ToString("X"));
|
|
|
|
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;
|
|
}
|
|
//Console.WriteLine("data[]=" + data[startIndex].ToString("X") + " sign=" + sign);
|
|
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;
|
|
}
|
|
}
|
|
}
|