1802 lines
84 KiB
C#
1802 lines
84 KiB
C#
|
using System;
|
|||
|
|
using System.Collections.Generic;
|
|||
|
|
using System.ComponentModel;
|
|||
|
|
using System.Data;
|
|||
|
|
|
|||
|
|
using System.Text;
|
|||
|
|
|
|||
|
|
using System.Net;
|
|||
|
|
using System.Net.Sockets;
|
|||
|
|
using System.Threading;
|
|||
|
|
using System.Collections;
|
|||
|
|
using SafeMobileLib;
|
|||
|
|
using System.Linq;
|
|||
|
|
|
|||
|
|
|
|||
|
|
|
|||
|
|
namespace CPlus_GW
|
|||
|
|
{
|
|||
|
|
|
|||
|
|
class LocationThread
|
|||
|
|
{
|
|||
|
|
private static Int32 reqID_send = 0x01;
|
|||
|
|
private string reqID;
|
|||
|
|
private Int32 reqID_recv;
|
|||
|
|
public int port;
|
|||
|
|
//public MotoTRBOGW parent;
|
|||
|
|
private byte[] data = new byte[1024];
|
|||
|
|
private static UdpClient udpClient;
|
|||
|
|
// -------------------------------------------------------------------
|
|||
|
|
// Main
|
|||
|
|
// -------------------------------------------------------------------
|
|||
|
|
private bool start_read = true;
|
|||
|
|
private UdpMulticast udpMulticast;
|
|||
|
|
|
|||
|
|
private int ACCEPTED_LEVEL_OF_CONFIDENCE = 20;
|
|||
|
|
|
|||
|
|
|
|||
|
|
#region EVENTS
|
|||
|
|
public delegate void PollResponseReceived(Int64 radioID, DateTime locationTime, int speed, double latitude, double longitude, Int64 seqID);
|
|||
|
|
public event PollResponseReceived OnPollResponseReceived;
|
|||
|
|
|
|||
|
|
public delegate void LocationReceived(Int64 radioID, DateTime locationTime, int speed, double latitude, double longitude);
|
|||
|
|
public event LocationReceived OnLocationReceived;
|
|||
|
|
#endregion
|
|||
|
|
|
|||
|
|
public LocationThread()
|
|||
|
|
{
|
|||
|
|
Random rnd1 = new Random();
|
|||
|
|
reqID_send = (byte)rnd1.Next(0xffffff);
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
public void HandleConnection()
|
|||
|
|
{
|
|||
|
|
//TestPacket();
|
|||
|
|
//return;
|
|||
|
|
EndPoint reponseEndPoint = new IPEndPoint(0, 0);
|
|||
|
|
|
|||
|
|
try
|
|||
|
|
{
|
|||
|
|
udpMulticast = new UdpMulticast(Program.cfg.multi_IP, Program.cfg.multi_port);
|
|||
|
|
SafeMobileLib.Utils.WriteLine($"Location thread successfully registered to multicast group [{Program.cfg.multi_IP}:{Program.cfg.multi_port}]", ConsoleColor.Cyan);
|
|||
|
|
//udpMulticast.OnNewDataRecv += UdpMulticast_OnNewDataRecv;
|
|||
|
|
//udpMulticast.OnNewDataRecv += new UdpMulticast.newData4Send(UdpMulticast_OnNewDataRecv);
|
|||
|
|
CPlusGW.TextQueue.PostItem("LocationThread started!!!");
|
|||
|
|
//udpMulticast.StartListen();
|
|||
|
|
}
|
|||
|
|
catch (Exception ex)
|
|||
|
|
{
|
|||
|
|
SafeMobileLib.Utils.WriteLine("LocationThread exception while joining the multicast group: " + ex.ToString(), ConsoleColor.Red);
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
while (true)
|
|||
|
|
{
|
|||
|
|
try
|
|||
|
|
{
|
|||
|
|
MotoTRBOcmdMsg msg = CPlusGW.locationQueue.GetItem(100);
|
|||
|
|
if (msg != null)
|
|||
|
|
{
|
|||
|
|
/*
|
|||
|
|
if(udpClient!=null)
|
|||
|
|
udpClient.Close();
|
|||
|
|
udpClient = null;
|
|||
|
|
*/
|
|||
|
|
|
|||
|
|
ProcessCommand(msg);
|
|||
|
|
start_read = true;
|
|||
|
|
}
|
|||
|
|
// read async
|
|||
|
|
if (start_read)
|
|||
|
|
{
|
|||
|
|
//SafeMobileLib.Utils.WriteLine("Location Thread waiting message...");
|
|||
|
|
|
|||
|
|
start_read = false;
|
|||
|
|
try
|
|||
|
|
{
|
|||
|
|
if (udpClient == null)
|
|||
|
|
{
|
|||
|
|
SafeMobileLib.Utils.WriteLine("Location udpClient is null. Recreating it on port " + Program.cfg.locPort_r, ConsoleColor.Yellow);
|
|||
|
|
//Program.cfg.locPort_r = 9191;
|
|||
|
|
|
|||
|
|
RecreateAndBindUDPClient();
|
|||
|
|
|
|||
|
|
SafeMobileLib.Utils.WriteLine("ReBinding location udpClient", ConsoleColor.Yellow);
|
|||
|
|
}
|
|||
|
|
IPEndPoint remoteIpEndPoint = new IPEndPoint(IPAddress.Any, 0);
|
|||
|
|
|
|||
|
|
//SafeMobileLib.Utils.WriteLine("Before start async");
|
|||
|
|
// Blocks until a message returns on this socket from a remote host.
|
|||
|
|
udpClient.Client.BeginReceiveFrom(data, 0, data.Length,
|
|||
|
|
SocketFlags.None, ref reponseEndPoint,
|
|||
|
|
new AsyncCallback(AsyncRead),
|
|||
|
|
(object)this);
|
|||
|
|
//SafeMobileLib.Utils.WriteLine("After start async location udpClient");
|
|||
|
|
Thread.Sleep(100);
|
|||
|
|
//Console.WriteLine("AsyncRead started for Location thread on port:" + MotoTRBOGW.cfg.locPort_r);
|
|||
|
|
}
|
|||
|
|
catch (Exception ex)
|
|||
|
|
{
|
|||
|
|
SafeMobileLib.Utils.WriteLine("HandleConnection(): exception " + ex, ConsoleColor.Red);
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
else
|
|||
|
|
Thread.Sleep(100);
|
|||
|
|
}
|
|||
|
|
catch (Exception ex)
|
|||
|
|
{
|
|||
|
|
SafeMobileLib.Utils.WriteLine("Exception in LocationThread:HandleConnection(): \r\n\tData=" +
|
|||
|
|
ex.Data + "\r\n\tSource=" +
|
|||
|
|
ex.Source + "\r\n\tStackTrace=" +
|
|||
|
|
ex.StackTrace + "\r\n\tMessage=" +
|
|||
|
|
ex.Message + "\r\n\tInnerException=" +
|
|||
|
|
ex.InnerException, ConsoleColor.Red);
|
|||
|
|
}
|
|||
|
|
} // end while (true)
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
void AsyncRead(IAsyncResult result)
|
|||
|
|
{
|
|||
|
|
EndPoint remoteEndPoint = new IPEndPoint(0, 0);
|
|||
|
|
try
|
|||
|
|
{
|
|||
|
|
|
|||
|
|
int? bytesRead = udpClient?.Client?.EndReceiveFrom(result, ref remoteEndPoint);
|
|||
|
|
|
|||
|
|
//SafeMobileLib.Utils.WriteLine($"Location Thread read {bytesRead} bytes", ConsoleColor.Yellow);
|
|||
|
|
|
|||
|
|
if (bytesRead > 1)
|
|||
|
|
{
|
|||
|
|
|
|||
|
|
//Console.WriteLine("\n--------------------");
|
|||
|
|
//Console.WriteLine("AsyncRead endded processing data!!!");
|
|||
|
|
|
|||
|
|
DecodePacket_Connect_plus(data);
|
|||
|
|
start_read = true;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
else
|
|||
|
|
udpClient = null;
|
|||
|
|
|
|||
|
|
if (bytesRead == null)
|
|||
|
|
{
|
|||
|
|
SafeMobileLib.Utils.WriteLine($"Location thread udpclient is {(udpClient == null ? "null" : "not null")}", ConsoleColor.Yellow);
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
catch(Exception ex)
|
|||
|
|
{
|
|||
|
|
//string str = Encoding.ASCII.GetString(data, 0, data.Length);
|
|||
|
|
|
|||
|
|
SafeMobileLib.Utils.WriteLine($"Location Thread AsyncRead exception: {ex.ToString()}", ConsoleColor.Red);
|
|||
|
|
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
|
|||
|
|
public void DecodePacket_Connect_plus_SDVersion(Byte[] data)
|
|||
|
|
{
|
|||
|
|
int i = 0, pdata;
|
|||
|
|
|
|||
|
|
htCell_t cell = new htCell_t();
|
|||
|
|
cell.lat = "";
|
|||
|
|
cell.lng = "";
|
|||
|
|
cell.di = "0";
|
|||
|
|
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)
|
|||
|
|
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:
|
|||
|
|
//Console.WriteLine(Common_Element_Tokens_ENUM.request_id + ": " + Byte2String(data, i + 1, 5));
|
|||
|
|
i += 5;
|
|||
|
|
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)
|
|||
|
|
{
|
|||
|
|
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:
|
|||
|
|
string suid = ProcessSU_ID(data, i + 1).ToString();
|
|||
|
|
cell.suid = suid;
|
|||
|
|
i += 4;
|
|||
|
|
//Console.Write("SUIDc: " + suid + " ### ");
|
|||
|
|
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)
|
|||
|
|
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;
|
|||
|
|
byte parsErrorBYte = (byte)Result_Codes_ENUM.SYNTAX_ERROR;
|
|||
|
|
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.circle_3d:
|
|||
|
|
case (byte)Report_Messages_Tokens_ENUM.circle_3d1:
|
|||
|
|
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_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));
|
|||
|
|
Console.WriteLine("Ala: " + Byte2String(data, i + 1 + 12, 2));
|
|||
|
|
*/
|
|||
|
|
i += 14;
|
|||
|
|
break;
|
|||
|
|
|
|||
|
|
case (byte)Report_Messages_Tokens_ENUM.speed_vrt: //!! not tested
|
|||
|
|
//Console.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 = (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:
|
|||
|
|
//Console.WriteLine(Common_Element_Tokens_ENUM.request_id + ": " + Byte2String(data, i + 1, 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));
|
|||
|
|
if (data[i + 1] != 0)
|
|||
|
|
{
|
|||
|
|
parsErrorBYte = 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
|
|||
|
|
{
|
|||
|
|
SafeMobileLib.Utils.WriteLine("GPS returned with <20><> " + ProcessError(data[i + 1]));
|
|||
|
|
parsingError = true;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
}
|
|||
|
|
//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:
|
|||
|
|
string suid = ProcessSU_ID(data, i + 1).ToString();
|
|||
|
|
cell.suid = suid;
|
|||
|
|
//Console.Write("SUID: " +suid+" ### ");
|
|||
|
|
i += 4;
|
|||
|
|
break;
|
|||
|
|
|
|||
|
|
default:
|
|||
|
|
Console.WriteLine("Unknown: 0x" + data[i].ToString("X"));
|
|||
|
|
break;
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
if (!parsingError)
|
|||
|
|
{
|
|||
|
|
try
|
|||
|
|
{
|
|||
|
|
if (cell!= null && cell.suid != null)
|
|||
|
|
{
|
|||
|
|
cell.spd = cell.spd ?? "0";
|
|||
|
|
cell.lat = cell.lat ?? "0";
|
|||
|
|
cell.lng = cell.lng ?? "0";
|
|||
|
|
|
|||
|
|
double d = 0;
|
|||
|
|
if (!Double.TryParse(cell.spd, out d))
|
|||
|
|
cell.spd = "0";
|
|||
|
|
if (!Double.TryParse(cell.lat, out d))
|
|||
|
|
cell.lat = "0";
|
|||
|
|
if (!Double.TryParse(cell.lng, out d))
|
|||
|
|
cell.lng = "0";
|
|||
|
|
|
|||
|
|
|
|||
|
|
String[] toSendString = new String[4];
|
|||
|
|
toSendString[0] = DateTo70Format(DateTime.UtcNow).ToString();
|
|||
|
|
toSendString[1] = cell.spd;
|
|||
|
|
toSendString[2] = cell.lat;
|
|||
|
|
toSendString[3] = cell.lng;
|
|||
|
|
|
|||
|
|
String consoleMessage = String.Format("Position [{0:0.0000},{1:0.0000}] from {2}", Math.Round(Double.Parse(cell.lat), 4),
|
|||
|
|
Math.Round(Double.Parse(cell.lng), 4), cell.suid);
|
|||
|
|
|
|||
|
|
SafeMobileLib.Utils.WriteLine("<22><><EFBFBD> " + consoleMessage, ConsoleColor.Green);
|
|||
|
|
|
|||
|
|
CPlusGW.TextQueue.PostItem(consoleMessage);//"GPS for unit:" + cell.suid + " LAT:" + cell.lat.ToString() + " LNG:" + cell.lng.ToString());
|
|||
|
|
Byte[] toSendMulticast = createLocationMessage(131, cell.suid, toSendString);
|
|||
|
|
if (udpMulticast != null)
|
|||
|
|
udpMulticast.Send(toSendMulticast, toSendMulticast.Length);
|
|||
|
|
else
|
|||
|
|
SafeMobileLib.Utils.WriteLine("Unable to send position on multicast (it is null)", ConsoleColor.Red);
|
|||
|
|
|
|||
|
|
|
|||
|
|
//Console.WriteLine("SUID:" + cell.suid + " LAT:" + cell.lat.ToString() + " LNG:" + cell.lng.ToString());
|
|||
|
|
//SM.Debug("Location thread successfully sent data to message bus");
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
catch (Exception ex)
|
|||
|
|
{
|
|||
|
|
SafeMobileLib.Utils.WriteLine("Decode Message error: " + ex.ToString(), ConsoleColor.Red);
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
break;
|
|||
|
|
|
|||
|
|
// -----------------------------------------------------------------------------------------------------------------------
|
|||
|
|
default:
|
|||
|
|
Console.WriteLine("UNDEFINED 0x" + data[0].ToString("X"));
|
|||
|
|
break;
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
|
|||
|
|
|
|||
|
|
public void DecodePacket_Connect_plus(Byte[] data)
|
|||
|
|
{
|
|||
|
|
int i = 0, pdata;
|
|||
|
|
|
|||
|
|
htCell_t cell = new htCell_t();
|
|||
|
|
cell.lat = "";
|
|||
|
|
cell.lng = "";
|
|||
|
|
cell.di = "0";
|
|||
|
|
cell.spd = "0";
|
|||
|
|
cell.spd_v = "0";
|
|||
|
|
cell.suid = "";
|
|||
|
|
cell.location_time = DateTime.MinValue;
|
|||
|
|
cell.activity_time = DateTime.MinValue;
|
|||
|
|
cell.triggered = false;
|
|||
|
|
cell.level_confidence = -1;
|
|||
|
|
|
|||
|
|
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)
|
|||
|
|
SafeMobileLib.Utils.WriteLine(Document_Identifiers_ENUM.Triggered_Location_Stop_Answer_NoCDT + "");
|
|||
|
|
else
|
|||
|
|
SafeMobileLib.Utils.WriteLine(Document_Identifiers_ENUM.Triggered_Location_Answer_NoCDT + "");
|
|||
|
|
*/
|
|||
|
|
pdata = (int)data[1];
|
|||
|
|
string result = "";
|
|||
|
|
//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:
|
|||
|
|
//Console.WriteLine(Common_Element_Tokens_ENUM.request_id + ": " + Byte2String(data, i + 1, 5));
|
|||
|
|
|
|||
|
|
//Console.WriteLine(data[i + 5]);
|
|||
|
|
byte[] bytes = { data[i + 5], data[i + 4], data[i + 3], data[i + 2] };
|
|||
|
|
reqID = Byte2String(data, i + 1, 5);
|
|||
|
|
reqID_recv = BitConverter.ToInt32(bytes, 0);
|
|||
|
|
|
|||
|
|
cell.seq_ID = Byte2String(data, i + 2, data[i + 1]) + "";
|
|||
|
|
cell.requestID = data[i + 5];
|
|||
|
|
i += 5;
|
|||
|
|
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)
|
|||
|
|
{
|
|||
|
|
SafeMobileLib.Utils.WriteLine("GPS returned with <20> " + ProcessError(data[i + 1]), ConsoleType.GPS);
|
|||
|
|
result = 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");
|
|||
|
|
result = "OK";
|
|||
|
|
break;
|
|||
|
|
case (byte)Report_Messages_Tokens_ENUM.SU_IPv4:
|
|||
|
|
string suid = ProcessSU_ID(data, i + 1).ToString();
|
|||
|
|
cell.suid = suid;
|
|||
|
|
i += 4;
|
|||
|
|
//Console.Write("SUIDc: " + suid + " ### ");
|
|||
|
|
break;
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
/*
|
|||
|
|
LOGS.LOG("Debug: LOCK3");
|
|||
|
|
//remove message from pending
|
|||
|
|
lock (ht_pendingMsg.SyncRoot)
|
|||
|
|
{
|
|||
|
|
try
|
|||
|
|
{
|
|||
|
|
if (reqID_recv != null && ht_pendingMsg.ContainsKey(reqID_recv))
|
|||
|
|
{
|
|||
|
|
//LOGS.LOG(ConnectPlus_GW.cfg.gw_id + cell.suid + " removing from pending \t regID:" + reqID);
|
|||
|
|
ht_pendingMsg.Remove(reqID_recv);
|
|||
|
|
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
catch (Exception e)
|
|||
|
|
{
|
|||
|
|
LOGS.LOG("Error: cannot delete pending messages HashTable" + e.ToString());
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
LOGS.LOG("Debug: UN-LOCK3");
|
|||
|
|
*/
|
|||
|
|
/*
|
|||
|
|
if (data[0] == (byte)Document_Identifiers_ENUM.Triggered_Location_Stop_Answer_NoCDT)
|
|||
|
|
SafeMobileLib.Utils.WriteLine(cell.suid + " Triggered_Location_Stop_Answer_NoCDT \t regID:" + reqID + " \t result:" + result);
|
|||
|
|
else
|
|||
|
|
SafeMobileLib.Utils.WriteLine(cell.suid + " Triggered_Location_Answer_NoCDT \t regID:" + reqID + " \t result:" + result);*/
|
|||
|
|
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)
|
|||
|
|
Console.WriteLine(Document_Identifiers_ENUM.Triggered_Location_Report_NoCDT);
|
|||
|
|
else
|
|||
|
|
Console.WriteLine(Document_Identifiers_ENUM.Immediate_Location_Report_NoCDT);
|
|||
|
|
*/
|
|||
|
|
if (data[0] == (byte)Document_Identifiers_ENUM.Triggered_Location_Report_NoCDT)
|
|||
|
|
{
|
|||
|
|
cell.triggered = true;
|
|||
|
|
}
|
|||
|
|
else
|
|||
|
|
{
|
|||
|
|
//Utils.ConsWrite(DebugMSG_Type.DEV, "Got request id:" + cell.requestID);
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
pdata = (int)data[1];
|
|||
|
|
Boolean parsingError = false;
|
|||
|
|
byte parsErrorBYte = (byte)Result_Codes_ENUM.SYNTAX_ERROR;
|
|||
|
|
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();
|
|||
|
|
cell.d_lat = ProcessLat(data, i + 1, 4);
|
|||
|
|
cell.d_lng = ProcessLng(data, i + 1 + 4, 4);
|
|||
|
|
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.point_3d:
|
|||
|
|
//Console.WriteLine("### Point 3d ###");
|
|||
|
|
cell.lat = ProcessLat(data, i + 1, 4).ToString();
|
|||
|
|
cell.lng = ProcessLng(data, i + 1 + 4, 4).ToString();
|
|||
|
|
cell.d_lat = ProcessLat(data, i + 1, 4);
|
|||
|
|
cell.d_lng = ProcessLng(data, i + 1 + 4, 4);
|
|||
|
|
if (gps_time == DateTime.MinValue)
|
|||
|
|
cell.location_time = DateTime.Now.ToUniversalTime();
|
|||
|
|
|
|||
|
|
i += 11;
|
|||
|
|
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();
|
|||
|
|
cell.d_lat = ProcessLat(data, i + 1, 4);
|
|||
|
|
cell.d_lng = ProcessLng(data, i + 1 + 4, 4);
|
|||
|
|
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.circle_3d:
|
|||
|
|
case (byte)Report_Messages_Tokens_ENUM.circle_3d1:
|
|||
|
|
cell.lat = ProcessLat(data, i + 1, 4).ToString();
|
|||
|
|
cell.lng = ProcessLng(data, i + 1 + 4, 4).ToString();
|
|||
|
|
cell.d_lat = ProcessLat(data, i + 1, 4);
|
|||
|
|
cell.d_lng = ProcessLng(data, i + 1 + 4, 4);
|
|||
|
|
if (gps_time == DateTime.MinValue)
|
|||
|
|
cell.location_time = DateTime.Now.ToUniversalTime();
|
|||
|
|
cell.radius = ProcessUFloat2B(data, i + 1 + 8);
|
|||
|
|
/*
|
|||
|
|
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));
|
|||
|
|
Console.WriteLine("Ala: " + Byte2String(data, i + 1 + 12, 2));
|
|||
|
|
*/
|
|||
|
|
//if (!cell.triggered) Console.WriteLine("Lat: " + Byte2String(data, i + 1, 4) + " =" + cell.lat);
|
|||
|
|
//if (!cell.triggered) Console.WriteLine("Lng: " + Byte2String(data, i + 1 + 4, 4) + " =" + cell.lng);
|
|||
|
|
i += 14;
|
|||
|
|
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);
|
|||
|
|
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)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:
|
|||
|
|
cell.seq_ID = Byte2String(data, i + 2, data[i + 1]) + "";
|
|||
|
|
cell.requestID = data[i + 1 + data[i + 1]];
|
|||
|
|
//if (!cell.triggered) Console.WriteLine(Common_Element_Tokens_ENUM.request_id + ": " + Byte2String(data, i + 1, 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("INFO_TIME: " + Report_Messages_Tokens_ENUM.info_time + ": " + Byte2String(data, i + 1, 5) + " =" + gps_time);
|
|||
|
|
i += 5;
|
|||
|
|
break;
|
|||
|
|
case (byte)Report_Messages_Tokens_ENUM.SU_IPv4:
|
|||
|
|
string suid = ProcessSU_ID(data, i + 1).ToString();
|
|||
|
|
cell.suid = suid;
|
|||
|
|
//Utils.ConsWrite(DebugMSG_Type.GPS, "SUID: " + suid + " ### ");
|
|||
|
|
//if (!cell.triggered)
|
|||
|
|
// Utils.ConsWrite(DebugMSG_Type.DEV, "SUID: " + suid + " ### ");
|
|||
|
|
i += 4;
|
|||
|
|
break;
|
|||
|
|
// RESULTS
|
|||
|
|
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)
|
|||
|
|
{
|
|||
|
|
parsErrorBYte = 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
|
|||
|
|
{
|
|||
|
|
SafeMobileLib.Utils.WriteLine("GPS returned with <20><> " + ProcessError(data[i + 1]), ConsoleType.GPS);
|
|||
|
|
parsingError = true;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
}
|
|||
|
|
//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.direction_hor:
|
|||
|
|
i += 1;
|
|||
|
|
break;
|
|||
|
|
|
|||
|
|
default:
|
|||
|
|
SafeMobileLib.Utils.WriteLine("Unknown: 0x" + data[i].ToString("X"), ConsoleType.GPS);
|
|||
|
|
break;
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
Vehicles veh = MainForm.vehicles.FirstOrDefault(d => d.Imei.Equals(cell.suid));
|
|||
|
|
|
|||
|
|
if (veh == null || !veh.Active)
|
|||
|
|
{
|
|||
|
|
SafeMobileLib.Utils.WriteLine($"Location received from unit {cell.suid} that is not defined in the admin", ConsoleColor.White);
|
|||
|
|
Thread.Sleep(100);
|
|||
|
|
break;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
|
|||
|
|
if (gps_time == DateTime.MinValue)
|
|||
|
|
SafeMobileLib.Utils.WriteLine("GPS gps_time == DateTime.MinValue", ConsoleType.GPS);
|
|||
|
|
else if (!parsingError ||
|
|||
|
|
(((parsErrorBYte == (byte)Result_Codes_ENUM.QUERY_INFO_NOT_ATTAINABLE) ||
|
|||
|
|
(parsErrorBYte == (byte)Result_Codes_ENUM.QUERY_INFO_NOT_CURRENTLY_ATTAINABLE)) && (!cell.triggered)))
|
|||
|
|
{
|
|||
|
|
//Utils.ConsWrite(DebugMSG_Type.DEV, "Got location from" + cell.suid);
|
|||
|
|
cell.location_time = DateTime.UtcNow;
|
|||
|
|
|
|||
|
|
// check if the position fulfilled the requirements for LoC
|
|||
|
|
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;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
try
|
|||
|
|
{
|
|||
|
|
if (cell != null && cell.suid != null)
|
|||
|
|
{
|
|||
|
|
cell.spd = cell.spd ?? "0";
|
|||
|
|
cell.lat = cell.lat ?? "0";
|
|||
|
|
cell.lng = cell.lng ?? "0";
|
|||
|
|
|
|||
|
|
double d = 0;
|
|||
|
|
if (!Double.TryParse(cell.spd, out d))
|
|||
|
|
cell.spd = "0";
|
|||
|
|
if (!Double.TryParse(cell.lat, out d))
|
|||
|
|
cell.lat = "0";
|
|||
|
|
if (!Double.TryParse(cell.lng, out d))
|
|||
|
|
cell.lng = "0";
|
|||
|
|
|
|||
|
|
|
|||
|
|
String[] toSendString = new String[4];
|
|||
|
|
toSendString[0] = DateTo70Format(DateTime.UtcNow).ToString();
|
|||
|
|
toSendString[1] = cell.spd;
|
|||
|
|
toSendString[2] = cell.lat;
|
|||
|
|
toSendString[3] = cell.lng;
|
|||
|
|
|
|||
|
|
String consoleMessage = String.Format((cell.triggered ? "Position " : "Poll response ") + "[{0:0.0000},{1:0.0000}] from {2}", Math.Round(Double.Parse(cell.lat), 4),
|
|||
|
|
Math.Round(Double.Parse(cell.lng), 4), cell.suid);
|
|||
|
|
SafeMobileLib.Utils.WriteLine("<22><><EFBFBD> " + consoleMessage, ConsoleColor.Green);
|
|||
|
|
|
|||
|
|
// post message to be added in the UI
|
|||
|
|
CPlusGW.TextQueue.PostItem(consoleMessage);
|
|||
|
|
|
|||
|
|
Byte[] toSendMulticast = SafeMobileLib.Utils.createLocationMessage(cell.triggered ? 131 : 231, cell.suid, toSendString);
|
|||
|
|
udpMulticast.Send(toSendMulticast, toSendMulticast.Length);
|
|||
|
|
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
}
|
|||
|
|
catch (Exception ex)
|
|||
|
|
{
|
|||
|
|
SafeMobileLib.Utils.WriteLine("[Location Thread]: " + ex.ToString(), ConsoleColor.Red);
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
else if (parsErrorBYte == (byte)Result_Codes_ENUM.PROTOCOL_ELEMENT_NOT_SUPPORTED)
|
|||
|
|
{
|
|||
|
|
// this is error is generated whenever a location request sent to a field radio contains
|
|||
|
|
// required information that are not supported by that station
|
|||
|
|
//SendTriggeredLocationSTOP(cell.suid, cell.requestID);
|
|||
|
|
|
|||
|
|
// send Triggered Location Request which contains the level of confidence
|
|||
|
|
reqID_send++;
|
|||
|
|
if (reqID_send == Int32.MaxValue)
|
|||
|
|
reqID_send = 1;
|
|||
|
|
SafeMobileLib.Utils.WriteLine("PROTOCOL_ELEMENT_NOT_SUPPORTED for unit " + cell.suid, ConsoleColor.Magenta);
|
|||
|
|
//SendTriggeredLocationRequest(cell.suid, (uint)((SUinfo)SN_Queues.ht_SUInfo[cell.suid]).repInterval, reqID_send);
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
break;
|
|||
|
|
|
|||
|
|
// -----------------------------------------------------------------------------------------------------------------------
|
|||
|
|
default:
|
|||
|
|
SafeMobileLib.Utils.WriteLine("UNDEFINED 0x" + data[0].ToString("X"), ConsoleType.GPS);
|
|||
|
|
break;
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
|
|||
|
|
|
|||
|
|
void ProcessCommand(MotoTRBOcmdMsg p_msg)
|
|||
|
|
{
|
|||
|
|
reqID_send++;
|
|||
|
|
if (reqID_send == Int32.MaxValue)
|
|||
|
|
reqID_send = 1;
|
|||
|
|
|
|||
|
|
try
|
|||
|
|
{
|
|||
|
|
if (p_msg.m_cmd == (byte)MotoTRBOcmd.SET_REPORT_INTERVAL)
|
|||
|
|
SendTriggeredLocationRequest(p_msg.m_suid, Convert.ToUInt32(p_msg.m_payload));
|
|||
|
|
}
|
|||
|
|
catch (Exception e)
|
|||
|
|
{
|
|||
|
|
Console.WriteLine("Could not send Location Request to unit");
|
|||
|
|
Console.WriteLine(e.Message);
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
private static UdpClient sendUdpClient = new UdpClient();
|
|||
|
|
|
|||
|
|
public void SendTriggeredLocationRequest(string SUID, uint p_reportSec)
|
|||
|
|
{
|
|||
|
|
byte[] intBytes = BitConverter.GetBytes(reqID_send);
|
|||
|
|
if (BitConverter.IsLittleEndian)
|
|||
|
|
Array.Reverse(intBytes);
|
|||
|
|
|
|||
|
|
byte[] arr_radioID = Utils.ID2ByteARR("12", SUID);
|
|||
|
|
Byte[] sendBytes = { (byte)Document_Identifiers_ENUM.Triggered_Location_Request_NoCDT,
|
|||
|
|
0x10, // length in Bytes
|
|||
|
|
(byte)Common_Element_Tokens_ENUM.request_id,
|
|||
|
|
0x04, intBytes[0], intBytes[1], intBytes[2], intBytes[3],
|
|||
|
|
//0x04, 0x24, 0x68, 0xAC, 0xE0,
|
|||
|
|
(byte)Query_Request_Messages_Tokens_ENUM.SU_IPv4,
|
|||
|
|
arr_radioID[0],
|
|||
|
|
arr_radioID[1],
|
|||
|
|
arr_radioID[2],
|
|||
|
|
arr_radioID[3],
|
|||
|
|
0x51,
|
|||
|
|
(byte)Query_Request_Messages_Tokens_ENUM.request_speed_hor,
|
|||
|
|
|
|||
|
|
(byte)Query_Request_Messages_Tokens_ENUM.periodic_trigger,
|
|||
|
|
(byte)Query_Request_Messages_Tokens_ENUM.interval,
|
|||
|
|
0x00, 0x00 //(byte)report_time// in seconds
|
|||
|
|
};
|
|||
|
|
|
|||
|
|
try
|
|||
|
|
{
|
|||
|
|
if (sendUdpClient == null)
|
|||
|
|
{
|
|||
|
|
//SafeMobileLib.Utils.WriteLine("Creating udp client for sending Location request on port " + Program.cfg.locPort, ConsoleColor.Cyan);
|
|||
|
|
sendUdpClient = new UdpClient(Program.cfg.locPort);
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
int report_time = (int)p_reportSec;
|
|||
|
|
|
|||
|
|
if (report_time < 0x80)
|
|||
|
|
{
|
|||
|
|
//sendBytes[10] = (byte)report_time;
|
|||
|
|
sendBytes[17] = (byte)report_time;
|
|||
|
|
}
|
|||
|
|
else
|
|||
|
|
{
|
|||
|
|
sendBytes[1] += 1; // adjust length
|
|||
|
|
|
|||
|
|
int process = report_time; // MSB
|
|||
|
|
process >>= 7;
|
|||
|
|
process &= 0x007F;
|
|||
|
|
process |= 0x0080;
|
|||
|
|
|
|||
|
|
sendBytes[17] = (byte)process;
|
|||
|
|
sendBytes[18] = (byte)(report_time & 0x007F); //LSB
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
SendOnUDP(sendBytes, sendBytes.Length);
|
|||
|
|
SafeMobileLib.Utils.WriteLine($"Triggered location request sent to radio ID {SUID} [{report_time} seconds]", ConsoleColor.White);
|
|||
|
|
/*
|
|||
|
|
//Print(sendBytes, sendBytes.Length, false);
|
|||
|
|
udpClient.Send(sendBytes, sendBytes.Length, new IPEndPoint(IPAddress.Parse(Program.cfg.ctrlIP), Program.cfg.locPort));
|
|||
|
|
//udpClient.Send(sendBytes, new IPEndPoint(IPAddress.Parse(MotoTRBOGW.cfg.ctrlIP), MotoTRBOGW.cfg.locPort));
|
|||
|
|
|
|||
|
|
udpClient.Close();
|
|||
|
|
udpClient = null;*/
|
|||
|
|
}
|
|||
|
|
catch (Exception ex)
|
|||
|
|
{
|
|||
|
|
SafeMobileLib.Utils.WriteLine("Location Thread SendTriggeredLocationRequest exception: " + ex.ToString(), ConsoleColor.Red);
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
|
|||
|
|
private void RecreateAndBindUDPClient()
|
|||
|
|
{
|
|||
|
|
|
|||
|
|
udpClient = new UdpClient();
|
|||
|
|
udpClient.Client.SetSocketOption(SocketOptionLevel.Socket, SocketOptionName.ReuseAddress, true);
|
|||
|
|
udpClient.Client.Bind(new IPEndPoint(IPAddress.Any, Program.cfg.locPort_r));
|
|||
|
|
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
|
|||
|
|
private static void RecreateSendingUdpClient()
|
|||
|
|
{
|
|||
|
|
if (sendUdpClient == null)
|
|||
|
|
{
|
|||
|
|
SafeMobileLib.Utils.WriteLine("Creating udp client for sending Location request on port " + Program.cfg.locPort, ConsoleColor.Cyan);
|
|||
|
|
sendUdpClient = new UdpClient(Program.cfg.locPort);
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
private void UdpMulticast_OnNewDataRecv(byte[] data, int dataLen)
|
|||
|
|
{
|
|||
|
|
string str = Encoding.ASCII.GetString(data, 0, dataLen);
|
|||
|
|
try
|
|||
|
|
{
|
|||
|
|
SafeMobileLib.Utils.WriteLine("RECEIVED FROM LOCATION THREAD " + str);
|
|||
|
|
String[] tempArray = str.Trim().Split('#');
|
|||
|
|
if (tempArray.Length > 3)
|
|||
|
|
if (tempArray[3].Equals("154"))
|
|||
|
|
{
|
|||
|
|
string SUID = tempArray[4];
|
|||
|
|
|
|||
|
|
String consoleMessage = String.Format($"Poll request received for unit {SUID}");
|
|||
|
|
SafeMobileLib.Utils.WriteLine("<22><><EFBFBD> " + consoleMessage, ConsoleColor.White);
|
|||
|
|
CPlusGW.TextQueue.PostItem(consoleMessage);
|
|||
|
|
|
|||
|
|
SendPollLocationRequest(SUID);
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
catch (Exception ex)
|
|||
|
|
{
|
|||
|
|
SafeMobileLib.Utils.WriteLine("udpMulticast_OnNewDataRecv Exception: " + str, ConsoleColor.Red);
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
|
|||
|
|
|
|||
|
|
/// <summary>
|
|||
|
|
/// Request a field radio to report immediate its location [also know as poll]
|
|||
|
|
/// </summary>
|
|||
|
|
/// <param name="SUID">The field radio ID which needs to send imeediate its location</param>
|
|||
|
|
/// <param name="reqID">The triggered location ID for this poll request</param>
|
|||
|
|
public static void SendPollLocationRequest(string SUID)
|
|||
|
|
{
|
|||
|
|
Int32 reqID = ++reqID_send;
|
|||
|
|
byte[] arr_radioID = Utils.ID2ByteARR("12", SUID);
|
|||
|
|
//get reqid bytes
|
|||
|
|
byte[] intBytes = BitConverter.GetBytes(reqID);
|
|||
|
|
if (BitConverter.IsLittleEndian)
|
|||
|
|
Array.Reverse(intBytes);
|
|||
|
|
|
|||
|
|
Byte[] sendBytes = { (byte)Document_Identifiers_ENUM.Immediate_Location_Request_NoCDT,
|
|||
|
|
0x0D, // length in Bytes
|
|||
|
|
(byte)Common_Element_Tokens_ENUM.request_id,
|
|||
|
|
0x04, intBytes[0], intBytes[1], intBytes[2], intBytes[3],
|
|||
|
|
(byte)Query_Request_Messages_Tokens_ENUM.SU_IPv4,
|
|||
|
|
arr_radioID[0],
|
|||
|
|
arr_radioID[1],
|
|||
|
|
arr_radioID[2],
|
|||
|
|
arr_radioID[3],
|
|||
|
|
0x51,
|
|||
|
|
(byte)Query_Request_Messages_Tokens_ENUM.request_speed_hor
|
|||
|
|
};
|
|||
|
|
|
|||
|
|
bool result = SendOnUDP(sendBytes, sendBytes.Length);
|
|||
|
|
string consoleMessage = String.Format("{0} POLL location request for {1} [{2}]", (result ? "<22><><EFBFBD> Sent" : "ooo Not Sent "), SUID, reqID);
|
|||
|
|
|
|||
|
|
SafeMobileLib.Utils.WriteLine(consoleMessage, ConsoleColor.DarkGreen);
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
|
|||
|
|
|
|||
|
|
/// <summary>
|
|||
|
|
/// Send a byte array on UDP
|
|||
|
|
/// </summary>
|
|||
|
|
/// <param name="sendBytes">The byte array which needs to be sent on UDP</param>
|
|||
|
|
/// <param name="length"></param>
|
|||
|
|
/// <returns></returns>
|
|||
|
|
private static bool SendOnUDP(byte[] sendBytes, int length)
|
|||
|
|
{
|
|||
|
|
try
|
|||
|
|
{
|
|||
|
|
RecreateSendingUdpClient();
|
|||
|
|
|
|||
|
|
sendUdpClient.Send(sendBytes, length, new IPEndPoint(IPAddress.Parse(Program.cfg.ctrlIP), Program.cfg.locPort));
|
|||
|
|
sendUdpClient.Close();
|
|||
|
|
sendUdpClient = null;
|
|||
|
|
|
|||
|
|
return true;
|
|||
|
|
}
|
|||
|
|
catch (Exception ex)
|
|||
|
|
{
|
|||
|
|
SafeMobileLib.Utils.WriteLine("[Location Thread] Send on UDP exception: " + ex.ToString(), ConsoleColor.Red);
|
|||
|
|
return false;
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
|
|||
|
|
|
|||
|
|
// -------------------------------------------------------------------
|
|||
|
|
// Aux functions
|
|||
|
|
// -------------------------------------------------------------------
|
|||
|
|
|
|||
|
|
|
|||
|
|
static public string Byte2String(byte[] data)
|
|||
|
|
{
|
|||
|
|
string sdata = "";
|
|||
|
|
int i;
|
|||
|
|
|
|||
|
|
for (i = 0; i < data.Length; i++)
|
|||
|
|
{
|
|||
|
|
int ii;
|
|||
|
|
ii = (int)data[i];
|
|||
|
|
sdata += "0x" + ii.ToString("X") + " ";
|
|||
|
|
}
|
|||
|
|
return sdata;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
static 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;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
static 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";
|
|||
|
|
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
static public 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)
|
|||
|
|
{
|
|||
|
|
//Console.Write("<" + aux.ToString("X") + ">");
|
|||
|
|
int_part |= (aux ^ 0x80);
|
|||
|
|
int_part <<= 7;
|
|||
|
|
i++;
|
|||
|
|
aux = data[startIndex+i];
|
|||
|
|
aux &= 0xff;
|
|||
|
|
//Console.Write("[" + int_part.ToString("X") + "]");
|
|||
|
|
}
|
|||
|
|
//Console.Write("<" + aux.ToString("X") + ">");
|
|||
|
|
int_part |= aux;
|
|||
|
|
//Console.Write("[" + int_part.ToString("X") + "]");
|
|||
|
|
|
|||
|
|
// ignore the float part
|
|||
|
|
i++;
|
|||
|
|
while ((data[startIndex + i] & 0x80) != 0)
|
|||
|
|
{
|
|||
|
|
//Console.Write("<" + data[startIndex + i] + ">");
|
|||
|
|
i++;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
//Console.WriteLine();
|
|||
|
|
return (double)int_part;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
static 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("YY=" + YY.ToString("X"));
|
|||
|
|
//Console.WriteLine("MM=" + MM.ToString("X"));
|
|||
|
|
//Console.WriteLine("DD=" + DD.ToString("X"));
|
|||
|
|
//Console.WriteLine("hh=" + hh.ToString("X"));
|
|||
|
|
//Console.WriteLine("mm=" + mm.ToString("X"));
|
|||
|
|
//Console.WriteLine("ss=" + ss.ToString("X"));
|
|||
|
|
|
|||
|
|
//Console.WriteLine("GPSTime : " + YY + "/" + MM + "/" + DD + " " + hh + ":" + mm + ":" + ss);
|
|||
|
|
return new DateTime(YY, MM, DD, hh, mm, ss, DateTimeKind.Utc);
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
static 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;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
static 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];
|
|||
|
|
|
|||
|
|
|
|||
|
|
//Console.WriteLine("lng ulong=0x" + l.ToString("X"));
|
|||
|
|
|
|||
|
|
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;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
private static void Print(byte[] data, int length, bool inOut)
|
|||
|
|
{
|
|||
|
|
//Console.Clear();
|
|||
|
|
Console.WriteLine("--------------------------------------------------------------------------- " + length);
|
|||
|
|
Console.Write("Data (" + ((inOut) ? "RECEIVED" : "SENT") + "): ");
|
|||
|
|
for (int i = 0; i < length; i++)
|
|||
|
|
Console.Write(" 0x" + data[i].ToString("X2"));
|
|||
|
|
Console.WriteLine("");
|
|||
|
|
Console.WriteLine("--------------------------------------------------------------------------- ");
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
public static Byte[] createLocationMessage(int opCode, string suid, String[] dataString)
|
|||
|
|
{
|
|||
|
|
string msg = "";
|
|||
|
|
msg = "#1." + GetSecondsLocalFromDT(DateTime.Now).ToString() + DateTime.Now.Millisecond.ToString() + "#" + opCode + "#" + suid + "#";
|
|||
|
|
msg = msg + dataString[0] + "#" + dataString[1] + "#" + dataString[2] + "#" + dataString[3];
|
|||
|
|
int totalSize = 5 + msg.Length;
|
|||
|
|
string sizeMsg = String.Format("#{0:000}", totalSize);
|
|||
|
|
msg = sizeMsg + msg;
|
|||
|
|
Byte[] toSendMulticast = new Byte[totalSize];
|
|||
|
|
System.Text.ASCIIEncoding encoding = new System.Text.ASCIIEncoding();
|
|||
|
|
Byte[] toSendMulticastPartial = encoding.GetBytes(msg);
|
|||
|
|
for (int i = 0; i < toSendMulticastPartial.Length; i++)
|
|||
|
|
{
|
|||
|
|
toSendMulticast[i] = toSendMulticastPartial[i];
|
|||
|
|
}
|
|||
|
|
toSendMulticast[totalSize - 1] = 35;//'#'
|
|||
|
|
return toSendMulticast;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
public static Int32 GetSecondsLocalFromDT(DateTime param)
|
|||
|
|
{
|
|||
|
|
System.DateTime datetime = param;
|
|||
|
|
long nOfSeconds;
|
|||
|
|
System.DateTime dt70 = new DateTime(1970, 1, 1, 0, 0, 0, 0);
|
|||
|
|
System.DateTime dttmp1 = new DateTime(1970, 1, 1, 0, 0, 0, 0);
|
|||
|
|
System.DateTime dttmp2;
|
|||
|
|
dttmp2 = dttmp1.ToLocalTime();
|
|||
|
|
TimeSpan span2 = dttmp2 - dttmp1;
|
|||
|
|
TimeSpan span = datetime - dt70;
|
|||
|
|
//nOfSeconds = (long)span.TotalSeconds - (long)span2.TotalSeconds -3600; //mai scot o ora - 3600
|
|||
|
|
if (System.TimeZone.CurrentTimeZone.IsDaylightSavingTime(param)) nOfSeconds = (long)span.TotalSeconds - (long)span2.TotalSeconds - 3600; //mai scot o ora - 3600
|
|||
|
|
else nOfSeconds = (long)span.TotalSeconds - (long)span2.TotalSeconds;
|
|||
|
|
return ((Int32)nOfSeconds);
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
private 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);
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
class LocationThread3
|
|||
|
|
{
|
|||
|
|
public UInt16 port;
|
|||
|
|
//public MotoTRBOGW parent;
|
|||
|
|
|
|||
|
|
private byte[] data = new byte[1024];
|
|||
|
|
private static UdpClient udpClient;
|
|||
|
|
|
|||
|
|
// -------------------------------------------------------------------
|
|||
|
|
// Main
|
|||
|
|
// -------------------------------------------------------------------
|
|||
|
|
private bool start_read = true;
|
|||
|
|
public void HandleConnection()
|
|||
|
|
{
|
|||
|
|
//TestPacket();
|
|||
|
|
//return;
|
|||
|
|
EndPoint reponseEndPoint = new IPEndPoint(0, 0);
|
|||
|
|
while (true)
|
|||
|
|
{
|
|||
|
|
try
|
|||
|
|
{
|
|||
|
|
MotoTRBOcmdMsg msg = CPlusGW.locationQueue.GetItem(100);
|
|||
|
|
if (msg != null)
|
|||
|
|
{
|
|||
|
|
if (udpClient != null)
|
|||
|
|
udpClient.Close();
|
|||
|
|
udpClient = null;
|
|||
|
|
ProcessCommand(msg);
|
|||
|
|
start_read = true;
|
|||
|
|
}
|
|||
|
|
// read async
|
|||
|
|
if (start_read)
|
|||
|
|
{
|
|||
|
|
start_read = false;
|
|||
|
|
try
|
|||
|
|
{
|
|||
|
|
if (udpClient == null)
|
|||
|
|
{
|
|||
|
|
udpClient = new UdpClient();
|
|||
|
|
udpClient.Client.SetSocketOption(SocketOptionLevel.Socket, SocketOptionName.ReuseAddress, true);
|
|||
|
|
udpClient.Client.Bind(new IPEndPoint(IPAddress.Any, Program.cfg.locPort_r));
|
|||
|
|
}
|
|||
|
|
IPEndPoint remoteIpEndPoint = new IPEndPoint(IPAddress.Any, 0);
|
|||
|
|
// Blocks until a message returns on this socket from a remote host.
|
|||
|
|
Byte[] receivedBytes = udpClient.Receive(ref remoteIpEndPoint);
|
|||
|
|
//Console.WriteLine("Received form Location thread on port:" + MotoTRBOGW.cfg.locPort_r);
|
|||
|
|
Print(receivedBytes, receivedBytes.Length, true);
|
|||
|
|
|
|||
|
|
if (receivedBytes.Length > 1)
|
|||
|
|
{
|
|||
|
|
DecodePacket_Connect_plus(receivedBytes);
|
|||
|
|
start_read = true;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
Thread.Sleep(100);
|
|||
|
|
}
|
|||
|
|
catch (Exception ex)
|
|||
|
|
{
|
|||
|
|
SafeMobileLib.Utils.WriteLine("HandleConnection(): exception " + ex, ConsoleColor.Red);
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
else Thread.Sleep(100);
|
|||
|
|
}
|
|||
|
|
catch (Exception ex)
|
|||
|
|
{
|
|||
|
|
SafeMobileLib.Utils.WriteLine("Exception in LocationThread:HandleConnection(): \r\n\tData=" +
|
|||
|
|
ex.Data + "\r\n\tSource=" +
|
|||
|
|
ex.Source + "\r\n\tStackTrace=" +
|
|||
|
|
ex.StackTrace + "\r\n\tMessage=" +
|
|||
|
|
ex.Message + "\r\n\tInnerException=" +
|
|||
|
|
ex.InnerException, ConsoleColor.Red);
|
|||
|
|
}
|
|||
|
|
} // end while (true)
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
|
|||
|
|
public void DecodePacket_Connect_plus(Byte[] data)
|
|||
|
|
{
|
|||
|
|
int i = 0, pdata;
|
|||
|
|
|
|||
|
|
htCell_t cell = new htCell_t();
|
|||
|
|
cell.lat = "";
|
|||
|
|
cell.lng = "";
|
|||
|
|
cell.di = "0";
|
|||
|
|
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)
|
|||
|
|
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:
|
|||
|
|
//Console.WriteLine(Common_Element_Tokens_ENUM.request_id + ": " + Byte2String(data, i + 1, 5));
|
|||
|
|
i += 5;
|
|||
|
|
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)
|
|||
|
|
{
|
|||
|
|
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:
|
|||
|
|
string suid = ProcessSU_ID(data, i + 1).ToString();
|
|||
|
|
cell.suid = suid;
|
|||
|
|
i += 4;
|
|||
|
|
Console.Write("SUIDe: " + suid + " ## ");
|
|||
|
|
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)
|
|||
|
|
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.circle_3d:
|
|||
|
|
case (byte)Report_Messages_Tokens_ENUM.circle_3d1:
|
|||
|
|
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_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));
|
|||
|
|
Console.WriteLine("Ala: " + Byte2String(data, i + 1 + 12, 2));
|
|||
|
|
*/
|
|||
|
|
i += 14;
|
|||
|
|
break;
|
|||
|
|
|
|||
|
|
case (byte)Report_Messages_Tokens_ENUM.speed_vrt: //!! not tested
|
|||
|
|
//Console.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 = (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:
|
|||
|
|
//Console.WriteLine(Common_Element_Tokens_ENUM.request_id + ": " + Byte2String(data, i + 1, 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));
|
|||
|
|
if (data[i + 1] != 0)
|
|||
|
|
{
|
|||
|
|
Console.WriteLine("Error = " + ProcessError(data[i + 1]));
|
|||
|
|
parsingError = true;
|
|||
|
|
}
|
|||
|
|
//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:
|
|||
|
|
string suid = ProcessSU_ID(data, i + 1).ToString();
|
|||
|
|
cell.suid = suid;
|
|||
|
|
Console.Write("SUIDa: " + suid + " ## ");
|
|||
|
|
i += 4;
|
|||
|
|
break;
|
|||
|
|
|
|||
|
|
default:
|
|||
|
|
Console.WriteLine("Unknown: 0x" + data[i].ToString("X"));
|
|||
|
|
break;
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
if (!parsingError)
|
|||
|
|
{
|
|||
|
|
cell.location_time = gps_time;
|
|||
|
|
CPlusGW.DBQueueLocation.PostItem(cell);
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
break;
|
|||
|
|
|
|||
|
|
// -----------------------------------------------------------------------------------------------------------------------
|
|||
|
|
default:
|
|||
|
|
Console.WriteLine("UNDEFINED 0x" + data[0].ToString("X"));
|
|||
|
|
break;
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
void ProcessCommand(MotoTRBOcmdMsg p_msg)
|
|||
|
|
{
|
|||
|
|
try
|
|||
|
|
{
|
|||
|
|
if (p_msg.m_cmd == (byte)MotoTRBOcmd.SET_REPORT_INTERVAL)
|
|||
|
|
SendTriggeredLocationRequest(p_msg.m_suid, Convert.ToUInt32(p_msg.m_payload));
|
|||
|
|
}
|
|||
|
|
catch (Exception e)
|
|||
|
|
{
|
|||
|
|
Console.WriteLine("Could not send Location Request to unit");
|
|||
|
|
Console.WriteLine(e.Message);
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
public void SendTriggeredLocationRequest(string SUID, uint p_reportSec)
|
|||
|
|
{
|
|||
|
|
byte[] arr_radioID = Utils.ID2ByteARR("12", SUID);
|
|||
|
|
Byte[] sendBytes = { (byte)Document_Identifiers_ENUM.Triggered_Location_Request_NoCDT,
|
|||
|
|
0x10, // length in Bytes
|
|||
|
|
(byte)Common_Element_Tokens_ENUM.request_id,
|
|||
|
|
0x04, 0x24, 0x68, 0xAC, 0xE0,
|
|||
|
|
(byte)Query_Request_Messages_Tokens_ENUM.SU_IPv4,
|
|||
|
|
arr_radioID[0],
|
|||
|
|
arr_radioID[1],
|
|||
|
|
arr_radioID[2],
|
|||
|
|
arr_radioID[3],
|
|||
|
|
0x51,
|
|||
|
|
(byte)Query_Request_Messages_Tokens_ENUM.request_speed_hor,
|
|||
|
|
|
|||
|
|
(byte)Query_Request_Messages_Tokens_ENUM.periodic_trigger,
|
|||
|
|
(byte)Query_Request_Messages_Tokens_ENUM.interval,
|
|||
|
|
0x00, 0x00 //(byte)report_time// in seconds
|
|||
|
|
};
|
|||
|
|
|
|||
|
|
try
|
|||
|
|
{
|
|||
|
|
UdpClient udpClient = new UdpClient(Program.cfg.locPort);
|
|||
|
|
udpClient.Connect(Program.cfg.ctrlIP, Program.cfg.locPort);
|
|||
|
|
int report_time = (int)p_reportSec;
|
|||
|
|
|
|||
|
|
if (report_time < 0x80)
|
|||
|
|
{
|
|||
|
|
//sendBytes[10] = (byte)report_time;
|
|||
|
|
sendBytes[17] = (byte)report_time;
|
|||
|
|
}
|
|||
|
|
else
|
|||
|
|
{
|
|||
|
|
sendBytes[1] += 1; // adjust length
|
|||
|
|
|
|||
|
|
int process = report_time; // MSB
|
|||
|
|
process >>= 7;
|
|||
|
|
process &= 0x007F;
|
|||
|
|
process |= 0x0080;
|
|||
|
|
|
|||
|
|
sendBytes[17] = (byte)process;
|
|||
|
|
sendBytes[18] = (byte)(report_time & 0x007F); //LSB
|
|||
|
|
}
|
|||
|
|
Print(sendBytes, sendBytes.Length, false);
|
|||
|
|
|
|||
|
|
udpClient.Client.SendTo(sendBytes, new IPEndPoint(IPAddress.Parse(Program.cfg.ctrlIP), Program.cfg.locPort));
|
|||
|
|
Console.WriteLine("Triggered location request sent to radio ID " + SUID);
|
|||
|
|
udpClient.Close();
|
|||
|
|
}
|
|||
|
|
catch (Exception ex)
|
|||
|
|
{
|
|||
|
|
Console.WriteLine("Location Thread SendTriggeredLocationRequest exception: " + ex.ToString());
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
|
|||
|
|
// -------------------------------------------------------------------
|
|||
|
|
// Aux functions
|
|||
|
|
// -------------------------------------------------------------------
|
|||
|
|
|
|||
|
|
|
|||
|
|
static public string Byte2String(byte[] data)
|
|||
|
|
{
|
|||
|
|
string sdata = "";
|
|||
|
|
int i;
|
|||
|
|
|
|||
|
|
for (i = 0; i < data.Length; i++)
|
|||
|
|
{
|
|||
|
|
int ii;
|
|||
|
|
ii = (int)data[i];
|
|||
|
|
sdata += "0x" + ii.ToString("X") + " ";
|
|||
|
|
}
|
|||
|
|
return sdata;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
static 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;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
static 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";
|
|||
|
|
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
static public 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)
|
|||
|
|
{
|
|||
|
|
//Console.Write("<" + aux.ToString("X") + ">");
|
|||
|
|
int_part |= (aux ^ 0x80);
|
|||
|
|
int_part <<= 7;
|
|||
|
|
i++;
|
|||
|
|
aux = data[startIndex + i];
|
|||
|
|
aux &= 0xff;
|
|||
|
|
//Console.Write("[" + int_part.ToString("X") + "]");
|
|||
|
|
}
|
|||
|
|
//Console.Write("<" + aux.ToString("X") + ">");
|
|||
|
|
int_part |= aux;
|
|||
|
|
//Console.Write("[" + int_part.ToString("X") + "]");
|
|||
|
|
|
|||
|
|
// ignore the float part
|
|||
|
|
i++;
|
|||
|
|
while ((data[startIndex + i] & 0x80) != 0)
|
|||
|
|
{
|
|||
|
|
//Console.Write("<" + data[startIndex + i] + ">");
|
|||
|
|
i++;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
//Console.WriteLine();
|
|||
|
|
return (double)int_part;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
static 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("YY=" + YY.ToString("X"));
|
|||
|
|
//Console.WriteLine("MM=" + MM.ToString("X"));
|
|||
|
|
//Console.WriteLine("DD=" + DD.ToString("X"));
|
|||
|
|
//Console.WriteLine("hh=" + hh.ToString("X"));
|
|||
|
|
//Console.WriteLine("mm=" + mm.ToString("X"));
|
|||
|
|
//Console.WriteLine("ss=" + ss.ToString("X"));
|
|||
|
|
|
|||
|
|
//SafeMobileLib.Utils.WriteLine("GPSTime : " + YY + "/" + MM + "/" + DD + " " + hh + ":" + mm + ":" + ss);
|
|||
|
|
return new DateTime(YY, MM, DD, hh, mm, ss, DateTimeKind.Utc);
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
static 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;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
static 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];
|
|||
|
|
|
|||
|
|
|
|||
|
|
//Console.WriteLine("lng ulong=0x" + l.ToString("X"));
|
|||
|
|
|
|||
|
|
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;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
private static void Print(byte[] data, int length, bool inOut)
|
|||
|
|
{
|
|||
|
|
//Console.Clear();
|
|||
|
|
Console.WriteLine("--------------------------------------------------------------------------- " + length);
|
|||
|
|
Console.Write("Data (" + ((inOut) ? "RECEIVED" : "SENT") + "): ");
|
|||
|
|
for (int i = 0; i < length; i++)
|
|||
|
|
Console.Write(" 0x" + data[i].ToString("X2"));
|
|||
|
|
Console.WriteLine("");
|
|||
|
|
Console.WriteLine("--------------------------------------------------------------------------- ");
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
|
|||
|
|
}
|