495 lines
23 KiB
C#
495 lines
23 KiB
C#
using System;
|
|
using System.Net;
|
|
using System.Net.Sockets;
|
|
using System.Threading;
|
|
using MotoTRBO_GW;
|
|
using SafeMobileLib;
|
|
using SafeMobileLib.MessageDecoders;
|
|
using System.Threading.Tasks;
|
|
using System.Collections.Generic;
|
|
|
|
namespace MotoTrbo_GW
|
|
{
|
|
class LocationThread
|
|
{
|
|
private static Int32 port;
|
|
private byte[] data = new byte[1024];
|
|
private UdpMulticast udpMulticast;
|
|
private static UdpClient udpClient;
|
|
private String mIP;
|
|
private Int32 mPort;
|
|
private static byte reqID_lastByte=0;
|
|
|
|
private Thread retryLocationRequest;
|
|
|
|
private static Int32 numberOfPositions = 0;
|
|
public static Int32 NumberOfPositions
|
|
{
|
|
get { return numberOfPositions; }
|
|
set
|
|
{
|
|
numberOfPositions = value;
|
|
numberOfPositionsSinceLastRead++;
|
|
}
|
|
}
|
|
|
|
private static Int32 numberOfPositionsSinceLastRead = 0;
|
|
public static Int32 NumberOfPositionsSinceLastRead
|
|
{
|
|
get
|
|
{
|
|
Int32 val = numberOfPositionsSinceLastRead;
|
|
numberOfPositionsSinceLastRead = 0;
|
|
return val;
|
|
}
|
|
set { numberOfPositionsSinceLastRead = value; }
|
|
}
|
|
|
|
private static Dictionary<String, LocationRequest> radioIdToLocReqTime = new Dictionary<String, LocationRequest>();
|
|
private static object lockDictionary = new object();
|
|
|
|
|
|
public LocationThread(Int32 locationPort, String multicastID, String multicastPort)
|
|
{
|
|
port = locationPort;
|
|
mIP = multicastID;
|
|
mPort = Int32.Parse(multicastPort);
|
|
}
|
|
|
|
private void RetryLocationRequestHandler()
|
|
{
|
|
while (Program.isRunning)
|
|
{
|
|
|
|
lock(lockDictionary)
|
|
{
|
|
Thread.Sleep(10);
|
|
Dictionary<String, LocationRequest> copyDictionary = new Dictionary<string, LocationRequest>(radioIdToLocReqTime);
|
|
|
|
foreach (KeyValuePair<String, LocationRequest> pair in copyDictionary)
|
|
{
|
|
if ((DateTime.Now - pair.Value.RequestTime).TotalSeconds > pair.Value.ReportingSeconds + 10)
|
|
{
|
|
SafeMobileLib.Utils.WriteLine($"Retrying Location Requests for {pair.Key}[{pair.Value.NrOfRetry}]", ConsoleColor.Yellow);
|
|
sendTriggeredLocationReq(pair.Value.RadioID, pair.Value.ReportingSeconds);
|
|
}
|
|
|
|
if (pair.Value.NrOfRetry >= 3)
|
|
{
|
|
SafeMobileLib.Utils.WriteLine($"Stopping retrying Location Requests for {pair.Key}", ConsoleColor.Yellow);
|
|
radioIdToLocReqTime.Remove(pair.Key);
|
|
}
|
|
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
public void handleConnection()
|
|
{
|
|
SafeMobileLib.Utils.WriteLine("Location Thread - Initialized on port " + port, ConsoleType.GPS);
|
|
|
|
Task.Factory.StartNew(RetryLocationRequestHandler);
|
|
|
|
try
|
|
{
|
|
udpClient = new UdpClient();
|
|
udpClient.Client.SetSocketOption(SocketOptionLevel.Socket, SocketOptionName.ReuseAddress, true);
|
|
udpClient.Client.Bind(new IPEndPoint(IPAddress.Any, port));
|
|
}
|
|
catch (Exception ex)
|
|
{
|
|
SafeMobileLib.Utils.WriteLine("LocationThread handleConnection exception while creating udpClient: " + ex.ToString(), ConsoleType.GPS);
|
|
}
|
|
|
|
try
|
|
{
|
|
udpMulticast = new UdpMulticast(mIP, mPort);
|
|
SafeMobileLib.Utils.WriteLine("Location thread successfully registered to multicast group", ConsoleType.GPS);
|
|
udpMulticast.OnNewDataRecv += new UdpMulticast.newData4Send(udpMulticast_OnNewDataRecv);
|
|
udpMulticast.StartListen(Main.LocalIP);
|
|
}
|
|
catch (Exception ex)
|
|
{
|
|
SafeMobileLib.Utils.WriteLine("Location Thread exception while joining the multicast group: " + ex.ToString(), ConsoleType.GPS);
|
|
}
|
|
|
|
IPEndPoint remoteIpEndPoint = new IPEndPoint(IPAddress.Any, 0);
|
|
|
|
while (true)
|
|
{
|
|
try
|
|
{
|
|
// Blocks until a message returns on this socket from a remote host.
|
|
Byte[] receivedBytes = udpClient.Receive(ref remoteIpEndPoint);
|
|
|
|
char[] separator = { '.' };
|
|
string[] su = remoteIpEndPoint.Address.ToString().Split(separator);
|
|
uint radioID = (Convert.ToUInt32(su[1])) * 256 * 256 + (Convert.ToUInt32(su[2])) * 256 + Convert.ToUInt32(su[3]);
|
|
string suid = radioID.ToString();
|
|
|
|
SafeMobileLib.Utils.WriteLine("Location Thread received data from radio " + radioID, ConsoleColor.White, ConsoleType.GPS);
|
|
//Utils.printBytesArray(receivedBytes);
|
|
|
|
//put information on message bus
|
|
try
|
|
{
|
|
LocationDecoder locDecoder = new LocationDecoder(suid, receivedBytes, Main.TESTMODE);
|
|
|
|
try
|
|
{
|
|
Result_Codes_ENUM enumDisplayStatus = (Result_Codes_ENUM)locDecoder.result;
|
|
string stringValue = enumDisplayStatus.ToString();
|
|
if(locDecoder.result != (byte)Result_Codes_ENUM.SUCCESS)
|
|
SafeMobileLib.Utils.WriteLine($"Location Result code is {stringValue} for {radioID}", ConsoleColor.DarkYellow);
|
|
|
|
}
|
|
catch (Exception) { SafeMobileLib.Utils.WriteLine($"Location Result code is {locDecoder.result} for {radioID}", ConsoleColor.DarkYellow); }
|
|
|
|
|
|
if (locDecoder.result == (byte)Result_Codes_ENUM.REPORTING_WILL_STOP ||
|
|
locDecoder.result == (byte)Result_Codes_ENUM.DUPLICATE_REQUEST_ID)
|
|
{
|
|
|
|
SafeMobileLib.Utils.WriteLine($"{(locDecoder.result == (byte)Result_Codes_ENUM.REPORTING_WILL_STOP ? "REPORTING_WILL_STOP" : "DUPLICATE_REQUEST_ID")} " +
|
|
$"for {suid}!!!!!", ConsoleColor.Red);
|
|
|
|
int requestingInterval = ARSThread.vehiclesManager.getReportingInterval(suid);
|
|
if (requestingInterval < 5)
|
|
{
|
|
SafeMobileLib.Utils.WriteLine("Requesting interval for " + suid + " set to default value : 180 [found: "
|
|
+ requestingInterval + "]", ConsoleColor.DarkMagenta);
|
|
|
|
requestingInterval = 180;
|
|
}
|
|
|
|
if (locDecoder.result == (byte)Result_Codes_ENUM.REPORTING_WILL_STOP)
|
|
{
|
|
SafeMobileLib.Utils.WriteLine($"Should request location updates again for {radioID}");
|
|
SendTriggeredLocationRequest(suid, requestingInterval);
|
|
}
|
|
else if (locDecoder.result == (byte)Result_Codes_ENUM.DUPLICATE_REQUEST_ID)
|
|
{
|
|
|
|
|
|
delayWaitingReponseForUnit(suid, requestingInterval);
|
|
}
|
|
}
|
|
|
|
else if (locDecoder.result == (byte)Result_Codes_ENUM.SYNTAX_ERROR)
|
|
{
|
|
SafeMobileLib.Utils.WriteLine("SYNTAX_ERROR", ConsoleColor.Yellow);
|
|
}
|
|
else if (locDecoder.result == (byte)Result_Codes_ENUM.PROTOCOL_ELEMENT_NOT_SUPPORTED)
|
|
{
|
|
int requestingInterval = ARSThread.vehiclesManager.getReportingInterval(suid);
|
|
if (requestingInterval < 5)
|
|
{
|
|
SafeMobileLib.Utils.WriteLine("Requesting interval for " + suid + " set to default value : 180 [found: "
|
|
+ requestingInterval + "]", ConsoleColor.DarkMagenta);
|
|
|
|
requestingInterval = 180;
|
|
}
|
|
|
|
SafeMobileLib.Utils.WriteLine("PROTOCOL_ELEMENT_NOT_SUPPORTED " + (locDecoder.cell.isCSBK ? " [CSBK]" : "[LEV_OF_CONFIDENCE]"));
|
|
try
|
|
{
|
|
Thread p = new Thread(delegate ()
|
|
{
|
|
int coun = 0;
|
|
while (Program.isRunning && (coun++) < 6)
|
|
{
|
|
|
|
Thread.Sleep(500);
|
|
}
|
|
// send Triggered Location Request which contains the level of confidence
|
|
SendTriggeredLocationRequest(suid, requestingInterval);
|
|
|
|
});
|
|
p.Start();
|
|
}
|
|
catch (Exception)
|
|
{
|
|
|
|
}
|
|
}
|
|
else if(locDecoder.result == (byte)Result_Codes_ENUM.SUCCESS)
|
|
{
|
|
if (locDecoder.cell.lat == null)
|
|
{
|
|
int requestingInterval = ARSThread.vehiclesManager.getReportingInterval(suid);
|
|
if (requestingInterval < 5)
|
|
{
|
|
SafeMobileLib.Utils.WriteLine("Requesting interval for " + suid + " set to default value : 180 [found: "
|
|
+ requestingInterval + "]", ConsoleColor.DarkMagenta);
|
|
|
|
requestingInterval = 180;
|
|
}
|
|
delayWaitingReponseForUnit(suid, requestingInterval);
|
|
}
|
|
}
|
|
if (locDecoder.cell.lat != null)
|
|
{
|
|
if (Main.TESTMODE)
|
|
suid = locDecoder.RadioID;
|
|
|
|
// remove the unit from the list of unanswered location triggers
|
|
lock (lockDictionary)
|
|
{
|
|
if (radioIdToLocReqTime.ContainsKey(suid))
|
|
{
|
|
SafeMobileLib.Utils.WriteLine($"Radio {suid} answered to location request, removing it for dictionary", ConsoleColor.Green);
|
|
radioIdToLocReqTime.Remove(suid);
|
|
}
|
|
}
|
|
|
|
// get the time difference between the GPS Position and the Global Time (which can be an internet time)
|
|
TimeSpan locationDifference = (DateTime.Now.AddSeconds(MotoTrbo_GW.Main.timeDifference.TotalSeconds)).Subtract(locDecoder.cell.location_time);
|
|
/*
|
|
Console.ForegroundColor = ConsoleColor.Cyan;
|
|
SafeMobileLib.Utils.WriteLine("CRT Time is " + DateTime.UtcNow.AddSeconds(MotoTrbo_GW.Main.timeDifference.TotalSeconds).ToString());
|
|
SafeMobileLib.Utils.WriteLine("Location Time is " + locDecoder.cell.location_time.ToString());
|
|
Console.ForegroundColor = ConsoleColor.Red;
|
|
SafeMobileLib.Utils.WriteLine("POSITION DIFFERENCE IS " + locationDifference.Seconds);
|
|
Console.ForegroundColor = ConsoleColor.Gray;
|
|
*/
|
|
// if position in the future with more than 2 days
|
|
if (Math.Abs(locationDifference.TotalSeconds) > 172800)
|
|
{
|
|
// skip this position
|
|
continue;
|
|
}
|
|
|
|
String[] toSendString = new String[4];
|
|
toSendString[0] = DateTo70Format(locDecoder.cell.location_time).ToString();
|
|
toSendString[1] = locDecoder.cell.spd.ToString();
|
|
toSendString[2] = locDecoder.cell.lat.ToString();
|
|
toSendString[3] = locDecoder.cell.lng.ToString();
|
|
|
|
// increment the number of positions received
|
|
NumberOfPositions++;
|
|
|
|
Int64 suId = 0;
|
|
Int64.TryParse(suid, out suId);
|
|
|
|
int speed = 0;
|
|
int.TryParse(locDecoder.cell.spd, out speed);
|
|
|
|
double lat = 0;
|
|
double.TryParse(locDecoder.cell.lat, out lat);
|
|
|
|
double lng = 0;
|
|
double.TryParse(locDecoder.cell.lng, out lng);
|
|
|
|
if (locDecoder.cell.triggered)
|
|
{
|
|
// trigger event if listener registered
|
|
OnLocationReceived?.Invoke(suId, locDecoder.cell.location_time, speed, lat, lng);
|
|
SafeMobileLib.Utils.WriteLine(String.Format("»»» Position [{0:0.0000},{1:0.0000}] from {2}", Math.Round(Double.Parse(locDecoder.cell.lat), 4),
|
|
Math.Round(Double.Parse(locDecoder.cell.lng), 4), suid), ConsoleColor.White);
|
|
|
|
Byte[] toSendMulticast = Utils.createLocationMessage(131, suid, toSendString);
|
|
udpMulticast.Send(toSendMulticast, toSendMulticast.Length);
|
|
//SafeMobileLib.Utils.WriteLine("Location thread successfully sent data to message bus: " + toSendString);
|
|
}
|
|
else
|
|
{
|
|
// trigger event if listener registered
|
|
OnPollResponseReceived?.Invoke(suId, locDecoder.cell.location_time, speed, lat, lng, locDecoder.cell.seqID);
|
|
|
|
SafeMobileLib.Utils.WriteLine(String.Format("»»» Poll Res [{0:0.0000},{1:0.0000}] from {2} [{3}]", Math.Round(Double.Parse(locDecoder.cell.lat), 4),
|
|
Math.Round(Double.Parse(locDecoder.cell.lng), 4), suid, locDecoder.cell.seq_ID), ConsoleColor.White);
|
|
|
|
Byte[] toSendMulticast = Utils.createLocationMessage(231, suid, toSendString);
|
|
udpMulticast.Send(toSendMulticast, toSendMulticast.Length);
|
|
//SafeMobileLib.Utils.WriteLine("Location thread->location poll response", ConsoleType.GPS);
|
|
}
|
|
}
|
|
}
|
|
catch (Exception ex)
|
|
{
|
|
SafeMobileLib.Utils.WriteLine("LocaltionThread [handleConnection]" + ex.ToString(), ConsoleColor.Red, ConsoleType.GPS);
|
|
}
|
|
|
|
Thread.Sleep(100);
|
|
}
|
|
catch (Exception e)
|
|
{
|
|
SafeMobileLib.Utils.WriteLine("##### LocationThread Exception #########\n" + e.ToString(), ConsoleType.GPS);
|
|
}
|
|
}
|
|
}
|
|
|
|
|
|
private void delayWaitingReponseForUnit(String radioId, int reportingInterval)
|
|
{
|
|
SafeMobileLib.Utils.WriteLine($"Should wait {reportingInterval + 15} seconds for location updates before requesting again for {radioId}", ConsoleColor.DarkYellow);
|
|
lock (lockDictionary)
|
|
{
|
|
if (radioIdToLocReqTime.ContainsKey(radioId))
|
|
{
|
|
radioIdToLocReqTime[radioId].NrOfRetry = 0;
|
|
radioIdToLocReqTime[radioId].RequestTime = DateTime.Now.AddSeconds(reportingInterval + 15);
|
|
}
|
|
else
|
|
{
|
|
radioIdToLocReqTime.Add(radioId, new LocationRequest()
|
|
{
|
|
NrOfRetry = 0,
|
|
RadioID = radioId,
|
|
ReportingSeconds = reportingInterval,
|
|
RequestTime = DateTime.Now.AddSeconds(75)
|
|
});
|
|
}
|
|
}
|
|
}
|
|
|
|
|
|
|
|
void udpMulticast_OnNewDataRecv(byte[] data, int dataLen)
|
|
{
|
|
try
|
|
{
|
|
string str = System.Text.Encoding.ASCII.GetString(data, 0, dataLen);
|
|
String[] tempArray = str.Trim().Split('#');
|
|
if (tempArray.Length > 3)
|
|
{
|
|
if (tempArray[3].Equals("154"))
|
|
{
|
|
Int64 gwid_recv = Convert.ToInt64((tempArray[4].Split('.'))[0]);
|
|
if (gwid_recv == Main.GWID)
|
|
{
|
|
|
|
SafeMobileLib.Utils.WriteLine("LocationThread received from multicast bus: " + str.Trim(), ConsoleType.GPS);
|
|
string SUID = (tempArray[4].Split('.'))[2];
|
|
SafeMobileLib.Utils.WriteLine($"Poll location request for {SUID}", ConsoleColor.DarkCyan);
|
|
SendPollRequest(SUID);
|
|
}
|
|
}
|
|
}
|
|
}
|
|
catch (Exception ex)
|
|
{
|
|
SafeMobileLib.Utils.WriteLine(ex.ToString());
|
|
}
|
|
}
|
|
|
|
private uint DateTo70Format(DateTime param)
|
|
{
|
|
long nOfSeconds;
|
|
//SafeMobileLib.Utils.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);
|
|
}
|
|
|
|
|
|
private static void sendTriggeredLocationReq(string SUID, int report_time)
|
|
{
|
|
reqID_lastByte++;
|
|
if (reqID_lastByte > 0xfe) reqID_lastByte = 0x00;
|
|
|
|
|
|
reqID_lastByte = 0x05;
|
|
|
|
Byte[] sendBytes = { (byte)Document_Identifiers_ENUM.Triggered_Location_Request_NoCDT,
|
|
0x0B, // length in Bytes
|
|
(byte)Common_Element_Tokens_ENUM.request_id,
|
|
//0x04, 0x24, 0x68, 0xAC, 0xE0,
|
|
0x04, 0x24, 0x68, 0xAC, reqID_lastByte,
|
|
|
|
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 (report_time < 0x80)
|
|
{
|
|
//sendBytes[10] = (byte)report_time;
|
|
sendBytes[12] = (byte)report_time;
|
|
}
|
|
else
|
|
{
|
|
sendBytes[1] += 1; // adjust length
|
|
|
|
int process = report_time; // MSB
|
|
process >>= 7;
|
|
process &= 0x007F;
|
|
process |= 0x0080;
|
|
|
|
sendBytes[12] = (byte)process;
|
|
sendBytes[13] = (byte)(report_time & 0x007F); //LSB
|
|
}
|
|
udpClient.Client.SendTo(sendBytes, new IPEndPoint(IPAddress.Parse(Utils.ID2IP("12", SUID)), port));
|
|
//SafeMobileLib.Utils.WriteLine("Triggered location request sent to radio ID " + SUID, ConsoleType.GPS);
|
|
SafeMobileLib.Utils.WriteLine("««« Triggered location request [" + report_time + " sec] for " + SUID, ConsoleColor.White, ConsoleType.GPS);
|
|
|
|
//locationUdpClient.Close();
|
|
|
|
lock (lockDictionary)
|
|
{
|
|
if (radioIdToLocReqTime.ContainsKey(SUID))
|
|
{
|
|
radioIdToLocReqTime[SUID].NrOfRetry++;
|
|
radioIdToLocReqTime[SUID].RequestTime = DateTime.Now;
|
|
}
|
|
else
|
|
{
|
|
radioIdToLocReqTime.Add(SUID, new LocationRequest() { NrOfRetry = 0, RadioID = SUID, ReportingSeconds = report_time, RequestTime = DateTime.Now });
|
|
}
|
|
}
|
|
}
|
|
catch (Exception ex)
|
|
{
|
|
SafeMobileLib.Utils.WriteLine("Location Thread SendTriggeredLocationRequest exception: " + ex.ToString(), ConsoleType.GPS);
|
|
}
|
|
}
|
|
|
|
public static void SendTriggeredLocationRequest(string SUID, int report_time)
|
|
{
|
|
lock (lockDictionary)
|
|
{
|
|
// remove the previous requests because this is caused by a new ARS
|
|
if (radioIdToLocReqTime.ContainsKey(SUID))
|
|
radioIdToLocReqTime.Remove(SUID);
|
|
}
|
|
|
|
// send the request
|
|
sendTriggeredLocationReq(SUID, report_time);
|
|
}
|
|
|
|
public static void SendPollRequest(string SUID)
|
|
{
|
|
reqID_lastByte++;
|
|
if (reqID_lastByte > 0xff) reqID_lastByte = 0x00;
|
|
|
|
|
|
Byte[] sendBytes = { (byte)Document_Identifiers_ENUM.Immediate_Location_Request_NoCDT,
|
|
0x08, // length in Bytes
|
|
(byte)Common_Element_Tokens_ENUM.request_id,
|
|
//0x04, 0x24, 0x68, 0xAC, 0xE0,
|
|
0x04, 0x24, 0x68, 0xAC, reqID_lastByte,
|
|
|
|
0x51,
|
|
//(byte)Query_Request_Messages_Tokens_ENUM.require_altitude};
|
|
(byte)Query_Request_Messages_Tokens_ENUM.request_speed_hor};
|
|
|
|
udpClient.Client.SendTo(sendBytes, new IPEndPoint(IPAddress.Parse(Utils.ID2IP("12", SUID)), port));
|
|
SafeMobileLib.Utils.WriteLine("««« POLL location request for " + SUID + " [" + reqID_lastByte + "]", ConsoleColor.White);
|
|
//SafeMobileLib.Utils.WriteLine("POLL location request sent to radio ID " + SUID, ConsoleType.GPS);
|
|
}
|
|
|
|
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;
|
|
}
|
|
}
|