SafeDispatch/MotoTrbo_GW/LocationThread.cs

495 lines
23 KiB
C#
Raw Permalink Normal View History

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