SafeNet/Teltonika_SOC/UdpConnectionThread.cs
2021-02-24 13:50:23 +02:00

417 lines
16 KiB
C#

using SafeNetLib;
using System;
using System.Collections.Generic;
using System.Text;
using System.Threading;
namespace Teltonika_SOC
{
internal class UdpConnectionThread
{
public string CFG_FILE = "config.ini";
public static volatile int serverState = 0;
private UdpServer udpServer;
public UdpConnectionThread()
{
serverState = 1;
int port = 10000;
port = (int)DBhandle.GetGatewayPort();
if (port == 0) port = Program.cfg.locPort;
udpServer = new UdpServer(port);
udpServer.OnNewDataEndPointRecv += new UdpServer.dataRecvEndPointDel(udpServer_OnNewDataEndPointRecv);
udpServer.Start();
while (true)
{
Thread.Sleep(1000000000);
}
}
private void udpServer_OnNewDataEndPointRecv(byte[] data, int dataLen, System.Net.IPEndPoint ipEndPoint)
{
Console.WriteLine("Got data from :" + ipEndPoint.ToString());
string imei = "";
Byte[] response = ProcessPacket2(data, dataLen, out imei);
Utils.WriteLine("Send ACK to IP:" + ipEndPoint.Address + " PORT:" + ipEndPoint.Port, ConsoleColor.Green, Program.isUnix);
//IPEndPoint destEP = new IPEndPoint(ip, GPRS_PORT);//use the actual port
udpServer.Send(response, response.Length, ipEndPoint);
Utils.WriteLine("ACK sent", ConsoleColor.Green, Program.isUnix);
#region setup mode NOT SUPPORTED
//if (Program.setupMode)
//{
// if (Program.ImeiHT.Contains(imei))
// {
// if ((int)Program.ImeiHT[imei] < Program.parametersCount)
// {
// string str = Program.commands[(int)Program.ImeiHT[imei]];
// Byte[] response2 = ConnectionThread.getByteCommand(str);
// udpServer.Send(response2, response2.Length, ipEndPoint);
// Program.ImeiHT[imei] = (int)Program.ImeiHT[imei] + 1;
// Utils.WriteLine("Command <<" + str + ">> has been sent to imei " + imei.ToString(), ConsoleColor.Yellow, Program.isUnix);
// if ((int)Program.ImeiHT[imei] == Program.parametersCount)
// {
// Utils.WriteLine("Setup finish for imei: " + imei.ToString(), ConsoleColor.Yellow, Program.isUnix);
// }
// }
// }
//}
#endregion setup mode
}
// ---------------------------------------------------
// Packet and command processing
// ---------------------------------------------------
public Byte[] ProcessPacket2(byte[] data, int len, out string imei)
{
Byte[] response = null;
Int32 step = 0;
StringBuilder IMEIString = new StringBuilder();
String pLat = "0.0", pLong = "0.0";
Int64 itime70 = 0;
Int32 iSpeed = 0;
Int64 ilat, ilong;
Int16 IOID;
Int32 dataLenght = 0;
Int32 packetID = 0;
#region data lenght
for (int j = 0; j < 2; j++)
{
dataLenght <<= 8;
dataLenght |= (Int32)data[step];
step++;
}
#endregion data lenght
#region packet ID
for (int j = 0; j < 2; j++)
{
packetID <<= 8;
packetID |= (Int32)data[step];
step++;
}
#endregion packet ID
#region packet type
int packetType = data[step++];
#endregion packet type
#region AVL packet id
int avlPacketID = data[step++];
#endregion AVL packet id
#region imei
IMEIString.AppendFormat("{0}", Encoding.ASCII.GetString(data, 8, 15));
imei = IMEIString.ToString();
step = 23;
#endregion imei
#region codec ID
if (data[step++] != 0x08) { return response; }
#endregion codec ID
#region number of messages
Int32 numberofMessage = 0;
numberofMessage = data[step++];
#endregion number of messages
#region AVL data array
for (int i = 0; i < numberofMessage; i++)
{
//itime70 = data[step]*256+data[step]*256
//get time
//Console.WriteLine();
//Console.Write("Bytes for time:");
itime70 = data[step];
//Console.Write(" 0x" + data[step].ToString("X2"));
step++;
for (int j = 0; j < 7; j++)
{
itime70 <<= 8;
itime70 |= (Int64)data[step];
//Console.Write(" 0x" + data[step].ToString("X2"));
step++;
}
//jump priority 0
step++;
//proccess LONG
//Console.WriteLine();
//Console.Write("Bytes for LONG:");
bool sign = false;
if ((data[step] & 0x80) != 0)
{
sign = true;
}
ilong = data[step];
//Console.Write(" byte: 0x" + data[step].ToString("X2"));
step++;
for (int j = 0; j < 3; j++)
{
ilong <<= 8;
ilong |= (Int64)data[step];
//Console.Write(" 0x" + data[step].ToString("X2"));
step++;
}
if (sign)
{
ilong = ilong - 0xFFFFFFFF;
}
double dlong = (Double)((Double)ilong / 10000000);
pLong = Convert.ToString(dlong);
//proccess LAT
// Console.WriteLine();
// Console.Write("Bytes for LAT:");
sign = false;
if ((data[step] & 0x80) != 0)
{
sign = true;
}
ilat = data[step];
// Console.Write(" 0x" + data[step].ToString("X2"));
step++;
for (int j = 0; j < 3; j++)
{
ilat <<= 8;
ilat |= (Int64)data[step];
// Console.Write(" 0x" + data[step].ToString("X2"));
step++;
}
if (sign)
ilat = ilat - 0xFFFFFFFF;
double dlat = (Double)((Double)ilat / 10000000);
pLat = Convert.ToString(dlat);
//jump altitude 2 bytes
step += 2;
//jump angle 2 bytes
step += 2;
//jump number of sattelites
step++;
//proccess speed
// Console.WriteLine();
//Console.Write("Bytes for Speed:");
#region SPEED
// get MSB
iSpeed = data[step];
// Console.Write(" 0x" + data[step].ToString("X2"));
// LEFT SHIFT == multiply with 2^8
iSpeed <<= 8;
step++;
// LOGIC OR
iSpeed |= (Int32)data[step];
//Console.Write(" 0x" + data[step].ToString("X2"));
#endregion SPEED
step++;
#region IOID
List<Telemetry> sourceTelemetry = new List<Telemetry>();
IOID = (Int16)data[step++];
//Console.WriteLine("IOID:" + IOID);
int IOrecords = (Int32)data[step];
//Console.WriteLine("IOrecords:" + IOrecords);
step++;
int DImask = 0;
int NR1ByteIO = (Int32)data[step];
//Console.WriteLine("NR1ByteIO:" + NR1ByteIO);
step++;
for (int k = 0; k < NR1ByteIO; k++)
{
Telemetry tel = new Telemetry();
tel.ID = (Int32)data[step];
tel.TelemetryValue = (Int32)data[step + 1];
sourceTelemetry.Add(tel);
//Console.WriteLine("DI 1 byte: <elementID: 0x" + data[step].ToString("X2")
// + " value: 0x" + data[step+1].ToString("X2") +">");
if (data[step + 1] == 1)
DImask += (int)Math.Pow(2, data[step]);
step += 2; //1 byte ID 1 byte value
}
int NR2ByteIO = (Int32)data[step];
Console.WriteLine("NR2ByteIO:" + NR2ByteIO);
step++;
for (int k = 0; k < NR2ByteIO; k++)
{
Telemetry tel = new Telemetry();
tel.ID = (Int32)data[step];
step++;
tel.TelemetryValue = data[step];
// Console.Write(" 0x" + data[step].ToString("X2"));
// LEFT SHIFT == multiply with 2^8
tel.TelemetryValue <<= 8;
step++;
// LOGIC OR
tel.TelemetryValue |= (Int32)data[step + 2];
sourceTelemetry.Add(tel);
//Console.WriteLine("DI 2 byte: <elementID: 0x" + data[step].ToString("X2")
// + " value: 0x" + data[step + 1].ToString("X2")
// + " 0x"+ data[step + 2].ToString("X2") + ">");
step++; //1 byte ID 2 byte value
}
int NR4ByteIO = (Int32)data[step];
//Console.WriteLine("NR4ByteIO:" + NR4ByteIO);
step++;
for (int k = 0; k < NR4ByteIO; k++)
{
Telemetry tel = new Telemetry();
tel.ID = (Int32)data[step];
step++;
for (int j = 0; j < 3; j++)
{
tel.TelemetryValue <<= 8;
tel.TelemetryValue |= (Int32)data[step];
// Console.Write(" 0x" + data[step].ToString("X2"));
step++;
}
sourceTelemetry.Add(tel);
//Console.WriteLine("DI 4 byte: <elementID: 0x" + data[step].ToString("X2")
// + " value: 0x" + data[step + 1].ToString("X2")
// + " 0x" + data[step + 2].ToString("X2")
// + " 0x" + data[step + 4].ToString("X2")
// + " 0x" + data[step + 5].ToString("X2") + ">");
//step += 5; //1 byte ID 4 byte value
step++;
}
int NR8ByteIO = (Int32)data[step];
//Console.WriteLine("NR8ByteIO:" + NR8ByteIO);
step++;
for (int k = 0; k < NR8ByteIO; k++)
{
Telemetry tel = new Telemetry();
tel.ID = (Int32)data[step];
step++;
for (int j = 0; j < 7; j++)
{
tel.TelemetryValue <<= 8;
tel.TelemetryValue |= (Int32)data[step];
// Console.Write(" 0x" + data[step].ToString("X2"));
step++;
}
sourceTelemetry.Add(tel);
//Console.WriteLine("DI 8 byte: <elementID: 0x" + data[step].ToString("X2")
// + " value: 0x" + data[step + 1].ToString("X2")
// + " 0x" + data[step + 2].ToString("X2")
// + " 0x" + data[step + 3].ToString("X2")
// + " 0x" + data[step + 4].ToString("X2")
// + " 0x" + data[step + 5].ToString("X2")
// + " 0x" + data[step + 6].ToString("X2")
// + " 0x" + data[step + 7].ToString("X2")
// + " 0x" + data[step + 8].ToString("X2") + ">");
//step += 9; //1 byte ID 8 byte value
step++;
}
#endregion IOID
//Console.WriteLine();
//Console.WriteLine("Time:"+itime70+" LONG:"+ilong+" LAT:"+ilat+" Speed:"+iSpeed);
//add data to location QUEUE
try
{
//locManager.SendLoc2messagebus(imei, (Int32)(itime70/1000), iSpeed.ToString(), pLat, pLong);
htCell_t cell = new htCell_t();
cell.suid = imei;
cell.spd = iSpeed.ToString();
cell.lat = pLat;
cell.lng = pLong;
cell.d_lat = dlat;
cell.d_lng = dlong;
cell.location_time = Utils.UnixTimeStampToDateTime(itime70 / 1000);
cell.telemetryList = new List<Telemetry>(sourceTelemetry);
Utils.ConsWrite(DebugMSG_Type.GPS, "GPS for SUID: " + imei + " with time: " + cell.location_time + " lat: " + pLat + " lng: " + pLong);
LOGS.LOG(DateTime.Now.ToString() + " : GPS for SUID: " + imei + " with time: " + cell.location_time + " lat: " + pLat + " lng: " + pLong);
SN_Queues.DBQueueLocation.PostItem(cell);
Utils.ConsWrite(DebugMSG_Type.GPS, "Message added to queue!!!");
LOGS.LOG(DateTime.Now.ToString() + " : Message added to queue!!!");
//process ALERTS
if (SN_Queues.ht_SUInfo.ContainsKey(imei))
{
Utils.ConsWrite(DebugMSG_Type.ALERTS, "Processing alerts for: " + imei);
Utils.ConsWrite(DebugMSG_Type.ALERTS, "Message mask: " + DImask);
SUinfo sui = (SUinfo)SN_Queues.ht_SUInfo[imei];
foreach (Alert alert in sui.alertList)
{
if (alert.Type == Alert_TYPE.DI)
if (alert.DImask1 == DImask)
{
alert.Alert_latitude = pLat;
alert.Alert_longitude = pLong;
alert.Speed = iSpeed;
alert.Digital_in = DImask;
alert.Position_time = Utils.UnixTimeStampToDateTime(itime70 / 1000).ToString("yyyy:MM:dd HH:mm:ss");
SN_Queues.alertQueue.PostItem(alert);
Utils.ConsWrite(DebugMSG_Type.ALERTS, "Alert: " + alert.Alert_name + " added to queue");
LOGS.LOG(DateTime.Now.ToString() + " : Alert: " + alert.Alert_name + " added to queue");
}
}
}
}
catch (Exception ex)
{
Utils.ConsWrite(DebugMSG_Type.always, "ConnectionThread.cs =>> Error adding location to queue!!!");
Utils.ConsWrite(DebugMSG_Type.always, ex.ToString());
}
}
#endregion AVL data array
#region compute response
List<byte> sendByteArray = new List<byte>();
#region packet lenght
sendByteArray.Add(0x00);
sendByteArray.Add(0x05);
#endregion packet lenght
#region packet ID
sendByteArray.Add(0xAB);
sendByteArray.Add(0xCD);
#endregion packet ID
#region packet type
sendByteArray.Add((byte)packetType);
#endregion packet type
#region avl packet id
sendByteArray.Add((byte)avlPacketID);
#endregion avl packet id
#region number of accepted data
sendByteArray.Add((byte)numberofMessage);
#endregion number of accepted data
response = sendByteArray.ToArray();
#endregion compute response
return response;
}
}
}