219 lines
8.4 KiB
C#
219 lines
8.4 KiB
C#
|
using System;
|
|||
|
|
using System.Collections.Generic;
|
|||
|
|
using System.Linq;
|
|||
|
|
using System.Text;
|
|||
|
|
|
|||
|
|
namespace Hytera_SOC
|
|||
|
|
{
|
|||
|
|
class GPSconvertor
|
|||
|
|
{
|
|||
|
|
public string GPSmsg_str = "";
|
|||
|
|
public GPSconvertor(int radioID, byte[] pbPayload, int size, int start, int poll, string gw_id)
|
|||
|
|
{
|
|||
|
|
try
|
|||
|
|
{
|
|||
|
|
int idx = start;
|
|||
|
|
|
|||
|
|
//gps status Data valid bit（A/V）
|
|||
|
|
char gps_status = (char)pbPayload[idx];
|
|||
|
|
Console.WriteLine("gps_status: {0}", gps_status);
|
|||
|
|
idx++;
|
|||
|
|
//gps time Greenwich time，Hour/minute/second occupy 2bytes each
|
|||
|
|
int gps_h = (pbPayload[idx] - 0x30) * 10 + (pbPayload[idx + 1] - 0x30);
|
|||
|
|
idx += 2;
|
|||
|
|
int gps_m = (pbPayload[idx] - 0x30) * 10 + (pbPayload[idx + 1] - 0x30);
|
|||
|
|
idx += 2;
|
|||
|
|
int gps_s = (pbPayload[idx] - 0x30) * 10 + (pbPayload[idx + 1] - 0x30);
|
|||
|
|
idx += 2;
|
|||
|
|
//Console.WriteLine("time {0}:{1}:{2}", gps_h, gps_m, gps_s);
|
|||
|
|
//gps date Day/month/year occupy 2 bytes each,
|
|||
|
|
int gps_d = (pbPayload[idx] - 0x30) * 10 + (pbPayload[idx + 1] - 0x30);
|
|||
|
|
idx += 2;
|
|||
|
|
int gps_mon = (pbPayload[idx] - 0x30) * 10 + (pbPayload[idx + 1] - 0x30);
|
|||
|
|
idx += 2;
|
|||
|
|
int gps_y = (pbPayload[idx] - 0x30) * 10 + (pbPayload[idx + 1] - 0x30);
|
|||
|
|
idx += 2;
|
|||
|
|
//Console.WriteLine("date {0}/{1}/{2}", gps_d, gps_mon, gps_y);
|
|||
|
|
//gps northsouth N：North S：South
|
|||
|
|
string gps_northsouth = ((char)pbPayload[idx]).ToString();
|
|||
|
|
idx++;
|
|||
|
|
//std::cout<<"gps_northsouth:"<<gps_northsouth<<endl;
|
|||
|
|
//gps latitude Latitude Format: DDFF.FFFF
|
|||
|
|
string lat = "";
|
|||
|
|
for (int i = 0; i < 9; i++)
|
|||
|
|
{
|
|||
|
|
lat += ((char)pbPayload[idx + i]).ToString();
|
|||
|
|
}
|
|||
|
|
idx += 9;
|
|||
|
|
//_cprintf("gps latitude: \n",lat);
|
|||
|
|
//gps_eastwest E：East，W：West
|
|||
|
|
string gps_eastwest = ((char)pbPayload[idx]).ToString();
|
|||
|
|
idx++;
|
|||
|
|
//std::cout<<"gps_eastwest:"<<gps_eastwest<<endl;
|
|||
|
|
//gps_longitude Latitude Format: DDDFF.FFFF
|
|||
|
|
string lng = "";
|
|||
|
|
for (int i = 0; i < 10; i++)
|
|||
|
|
{
|
|||
|
|
lng += ((char)pbPayload[idx + i]).ToString();
|
|||
|
|
}
|
|||
|
|
idx += 10;
|
|||
|
|
//gps speed
|
|||
|
|
string speed = "";
|
|||
|
|
for (int i = 0; i < 3; i++)
|
|||
|
|
{
|
|||
|
|
speed += ((char)pbPayload[idx + i]).ToString();
|
|||
|
|
//Console.WriteLine("spd:" + ((char)pbPayload[idx + i]).ToString());
|
|||
|
|
}
|
|||
|
|
idx += 3;
|
|||
|
|
Console.WriteLine("speed:" + speed);
|
|||
|
|
|
|||
|
|
// add by bigu for system with are not English
|
|||
|
|
string decimalSeparator = System.Globalization.CultureInfo.CurrentCulture.NumberFormat.NumberDecimalSeparator;
|
|||
|
|
if (decimalSeparator == ",") speed = speed.Replace('.', ',');
|
|||
|
|
try
|
|||
|
|
{
|
|||
|
|
speed = Convert.ToString((Int32)Math.Round(Convert.ToDouble(speed) * (Double)1.85));
|
|||
|
|
}
|
|||
|
|
catch (Exception ex)
|
|||
|
|
{
|
|||
|
|
Console.WriteLine("Error on conver Speed:\n\r" + ex.ToString());
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
DateTime dt = DateTime.Now;
|
|||
|
|
if (Hytera_GW.cfg.useRadioTime)
|
|||
|
|
if (gps_status == 'A') dt = new DateTime(gps_y + 2000, gps_mon, gps_d, gps_h, gps_m, gps_s);
|
|||
|
|
string strTime = DateTo70Format(dt).ToString();
|
|||
|
|
|
|||
|
|
string lat_str = ProcessGPSLat(lat, gps_northsouth).ToString();
|
|||
|
|
string lng_str = ProcessGPSLong(lng, gps_eastwest).ToString();
|
|||
|
|
//send to message bus
|
|||
|
|
string msgOPcode = "131";
|
|||
|
|
if (poll == 1) msgOPcode = "231";
|
|||
|
|
|
|||
|
|
GPSmsg_str = "#" + gw_id + "." + DateTo70Format(dt) + "#" + msgOPcode + "#"
|
|||
|
|
+ radioID + "#" + strTime + "#" +
|
|||
|
|
speed + "#" + lat_str + "#" + lng_str + "#";
|
|||
|
|
|
|||
|
|
int len2 = GPSmsg_str.Length + 1;
|
|||
|
|
len2 = len2 + ((len2).ToString()).Length;
|
|||
|
|
GPSmsg_str = "#" + len2.ToString() + GPSmsg_str;
|
|||
|
|
}
|
|||
|
|
catch (Exception ex)
|
|||
|
|
{
|
|||
|
|
Console.WriteLine("GPSconvertor ERROR:\n\r" + ex.ToString());
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
float ProcessGPSLat(String latitude, String lat_dir)
|
|||
|
|
{
|
|||
|
|
try
|
|||
|
|
{
|
|||
|
|
long LAT, LAT2;
|
|||
|
|
char LAT3;
|
|||
|
|
long llat, grade, zec;
|
|||
|
|
char[] param = new char[50];
|
|||
|
|
String[] latList;
|
|||
|
|
float flat;
|
|||
|
|
|
|||
|
|
param = lat_dir.ToCharArray();
|
|||
|
|
if (param[0] != 'N' && param[0] != 'S')
|
|||
|
|
return 0;
|
|||
|
|
|
|||
|
|
//Console.WriteLine("Latitude " + latitude + " dir " + lat_dir);
|
|||
|
|
|
|||
|
|
latList = latitude.Split('.');
|
|||
|
|
LAT = Convert.ToInt32(latList[0]);
|
|||
|
|
if (latList[1].Length > 4)
|
|||
|
|
latList[1] = latList[1].Remove(4);
|
|||
|
|
LAT2 = Convert.ToInt32(latList[1]);
|
|||
|
|
LAT3 = param[0];
|
|||
|
|
|
|||
|
|
// process the lat and lng for display
|
|||
|
|
grade = (LAT / 100L);
|
|||
|
|
zec = (LAT % 100L) * 1000L + LAT2 / 10; // get MMMMM*1000, from MM.mmmmm by MM*1000+mmm (0-59999)
|
|||
|
|
zec = (zec * 100L) / 60L; // translate MMMMM*1000 to DD * 100 * 1000 (0-99998)
|
|||
|
|
grade = grade * 100000 + zec; // translate all to DDddddd
|
|||
|
|
llat = grade;
|
|||
|
|
|
|||
|
|
flat = (float)llat / 100000;
|
|||
|
|
|
|||
|
|
if (param[0] == 'S')
|
|||
|
|
flat = -flat;
|
|||
|
|
|
|||
|
|
if (flat < -90 || flat > 90)
|
|||
|
|
{
|
|||
|
|
Console.WriteLine("[warning \"overflow lat\": flat={0} llat={1}]",
|
|||
|
|
flat, llat);
|
|||
|
|
return 0;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
return flat;
|
|||
|
|
}
|
|||
|
|
catch (Exception ee)
|
|||
|
|
{
|
|||
|
|
Console.WriteLine("Error in ProcessGPSLat" + ee.ToString());
|
|||
|
|
return 0;
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
float ProcessGPSLong(String longitude, String lng_dir)
|
|||
|
|
{
|
|||
|
|
try
|
|||
|
|
{
|
|||
|
|
long LNG, LNG2;
|
|||
|
|
char LNG3;
|
|||
|
|
long llng, grade, zec;
|
|||
|
|
char[] param = new char[50];
|
|||
|
|
String[] lngList;
|
|||
|
|
float flong;
|
|||
|
|
|
|||
|
|
param = lng_dir.ToCharArray();
|
|||
|
|
if (param[0] != 'E' && param[0] != 'W')
|
|||
|
|
return 0;
|
|||
|
|
|
|||
|
|
lngList = longitude.Split('.');
|
|||
|
|
LNG = Convert.ToInt32(lngList[0]);
|
|||
|
|
if (lngList[1].Length > 4)
|
|||
|
|
lngList[1] = lngList[1].Remove(4);
|
|||
|
|
LNG2 = Convert.ToInt32(lngList[1]);
|
|||
|
|
LNG3 = param[0];
|
|||
|
|
|
|||
|
|
grade = LNG / 100; // get DD (0-90)
|
|||
|
|
zec = (LNG % 100L) * 1000L + LNG2 / 10; // get MMMMM*1000, from MM.mmmmm by MM*1000+mmm (0-59999)
|
|||
|
|
zec = (zec * 100L) / 60L; // translate MMMMM*1000 to DD * 100 * 1000 (0-99998)
|
|||
|
|
grade = grade * 100000 + zec; // translate all to DDddddd
|
|||
|
|
llng = grade;
|
|||
|
|
|
|||
|
|
flong = (float)llng / 100000;
|
|||
|
|
|
|||
|
|
if (param[0] == 'W')
|
|||
|
|
flong = -flong;
|
|||
|
|
|
|||
|
|
if (flong < -180 || flong > 180)
|
|||
|
|
{
|
|||
|
|
Console.WriteLine("[warning \"overflow lng\": flng={0} llng={1}]",
|
|||
|
|
flong, llng);
|
|||
|
|
return 0;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
return flong;
|
|||
|
|
}
|
|||
|
|
catch (Exception ee)
|
|||
|
|
{
|
|||
|
|
Console.WriteLine("Error in ProcessGPSLong" + ee.ToString());
|
|||
|
|
return 0;
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
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);
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
}
|