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);
|
||
}
|
||
}
|
||
}
|