599 lines
21 KiB
C#
599 lines
21 KiB
C#
|
using System;
|
|||
|
|
using System.Collections.Generic;
|
|||
|
|
using System.Linq;
|
|||
|
|
using System.Text;
|
|||
|
|
using SafeMobileLib;
|
|||
|
|
|
|||
|
|
namespace SDRgateway
|
|||
|
|
{
|
|||
|
|
class Parse_LRRP
|
|||
|
|
{
|
|||
|
|
public MOTO.GPS_POS ProcessCommand(byte[] Gpsbuf, uint len)
|
|||
|
|
{
|
|||
|
|
string ret ="";
|
|||
|
|
MOTO.GPS_POS gps_pos = null;
|
|||
|
|
|
|||
|
|
// check for LRRP appl_id and DID
|
|||
|
|
if ((Gpsbuf[0] == 0x80) && (Gpsbuf[1] == MOTO.DOCID_L_V1_UNS_LOC_REP))
|
|||
|
|
{
|
|||
|
|
if (Gpsbuf[3] == MOTO.TKNICE_REQID_OI_1)
|
|||
|
|
{
|
|||
|
|
gps_pos = GpsUnsRep(Gpsbuf);
|
|||
|
|
}
|
|||
|
|
else
|
|||
|
|
{
|
|||
|
|
Console.WriteLine("UNS_LOC_REP not TKNICE_REQID_OI_1");
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
else if ((Gpsbuf[0] == 0x80) && (Gpsbuf[1] == MOTO.DOCID_L_V1_IMM_LOC_REP))
|
|||
|
|
{
|
|||
|
|
if (Gpsbuf[3] == MOTO.TKNICE_REQID_OI)
|
|||
|
|
{
|
|||
|
|
gps_pos = GpsImmResp(Gpsbuf);
|
|||
|
|
}
|
|||
|
|
else
|
|||
|
|
{
|
|||
|
|
Console.WriteLine("IMM_LOC_REP not TKNICE_REQID_OI");
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
else if ((Gpsbuf[0] == 0x80) && (Gpsbuf[1] == MOTO.DOCID_L_V1_TRI_LOC_ANS))
|
|||
|
|
{
|
|||
|
|
if (Gpsbuf[3] == MOTO.TKNICE_REQID_OI)
|
|||
|
|
{
|
|||
|
|
ret =GpsTrigStartResp(Gpsbuf);
|
|||
|
|
}
|
|||
|
|
else
|
|||
|
|
{
|
|||
|
|
Console.WriteLine("TRI_LOC_ANS not TKNICE_REQID_OI");
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
else if ((Gpsbuf[0] == 0x80) && (Gpsbuf[1] == MOTO.DOCID_L_V1_TRI_LOC_REP))
|
|||
|
|
{
|
|||
|
|
if (Gpsbuf[3] == MOTO.TKNICE_REQID_OI)
|
|||
|
|
{
|
|||
|
|
gps_pos = GpsTrigLoc(Gpsbuf);
|
|||
|
|
}
|
|||
|
|
else
|
|||
|
|
{
|
|||
|
|
Console.WriteLine("TRI_LOC_REP not TKNICE_REQID_OI");
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
else if ((Gpsbuf[0] == 0x80) && (Gpsbuf[1] == MOTO.DOCID_L_V1_TRI_STP_ANS))
|
|||
|
|
{
|
|||
|
|
if (Gpsbuf[3] == MOTO.TKNICE_REQID_OI)
|
|||
|
|
{
|
|||
|
|
ret =GpsTrigStopResp(Gpsbuf);
|
|||
|
|
}
|
|||
|
|
else
|
|||
|
|
{
|
|||
|
|
Console.WriteLine("TRI_STP_ANS not TKNICE_REQID_OI");
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
else
|
|||
|
|
{
|
|||
|
|
Console.WriteLine("GPS data\n");
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
return gps_pos;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
#region response type
|
|||
|
|
private MOTO.GPS_POS GpsUnsRep(byte[] Gpsbuf)
|
|||
|
|
{
|
|||
|
|
Console.WriteLine("GpsUnsRep \n");
|
|||
|
|
MOTO.GPS_POS ret_pos = new MOTO.GPS_POS() ;
|
|||
|
|
string ret_string;
|
|||
|
|
|
|||
|
|
int index, max_index;
|
|||
|
|
max_index = Gpsbuf[2];
|
|||
|
|
index = 4;
|
|||
|
|
switch (Gpsbuf[index]) // Req_id Table 3 in LRRP Protocol Spec.
|
|||
|
|
{
|
|||
|
|
case 0x00: ret_string = string.Format(" Req_id Power On ");
|
|||
|
|
break;
|
|||
|
|
case 0x07: ret_string = string.Format(" Req_id TMO On ");
|
|||
|
|
break;
|
|||
|
|
case 0x10: ret_string = string.Format(" Req_id Leave Service ");
|
|||
|
|
break;
|
|||
|
|
case 0x0F: ret_string = string.Format(" Req_id Loss of GPS coverage ");
|
|||
|
|
break;
|
|||
|
|
case 0x2F: ret_string = string.Format(" Req_id Periodic report ");
|
|||
|
|
break;
|
|||
|
|
default: ret_string = string.Format(" Req_id {0:X} ", Gpsbuf[index]);
|
|||
|
|
break;
|
|||
|
|
|
|||
|
|
}// end req_id
|
|||
|
|
|
|||
|
|
index++;
|
|||
|
|
|
|||
|
|
while (index - 2 < max_index)
|
|||
|
|
{
|
|||
|
|
switch (Gpsbuf[index])
|
|||
|
|
{
|
|||
|
|
case MOTO.TKNIR_RESULT_OI_0:
|
|||
|
|
ret_string += GpsErrorDec(ref index, Gpsbuf);
|
|||
|
|
break;
|
|||
|
|
case MOTO.TKNIR_INFO_TM_OI_5:
|
|||
|
|
ret_pos.time = GpsTimeDec(ref index, Gpsbuf);
|
|||
|
|
break;
|
|||
|
|
case MOTO.TKNLR_POINT_2D:
|
|||
|
|
ret_pos.pos = Gps2DDec(ref index, Gpsbuf);
|
|||
|
|
break;
|
|||
|
|
case MOTO.TKNLR_DIR_HOR_U8:
|
|||
|
|
ret_string += GpsHorDec(ref index, Gpsbuf);
|
|||
|
|
break;
|
|||
|
|
case MOTO.TKNLR_LEV_CONF_U8:
|
|||
|
|
ret_string += GpsLevDec(ref index, Gpsbuf);
|
|||
|
|
break;
|
|||
|
|
default:
|
|||
|
|
break;
|
|||
|
|
}
|
|||
|
|
index++;
|
|||
|
|
|
|||
|
|
}// end while
|
|||
|
|
|
|||
|
|
return ret_pos;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
|
|||
|
|
private MOTO.GPS_POS GpsImmResp(byte[] Gpsbuf)
|
|||
|
|
{
|
|||
|
|
Console.WriteLine("GpsImmResp \n");
|
|||
|
|
string str;
|
|||
|
|
MOTO.GPS_POS ret_pos = new MOTO.GPS_POS();
|
|||
|
|
string ret_string = "";
|
|||
|
|
|
|||
|
|
int index, max_index;
|
|||
|
|
max_index = Gpsbuf[2];
|
|||
|
|
index = 4;
|
|||
|
|
|
|||
|
|
int req_len = Gpsbuf[index];
|
|||
|
|
Console.WriteLine(" max_index {0} req_len {1} \n", max_index, req_len);
|
|||
|
|
|
|||
|
|
// compare TX req_id with this RX req_id
|
|||
|
|
if ((req_len == 4) && (Gpsbuf[index + 1] == 'A') &&
|
|||
|
|
(Gpsbuf[index + 2] == 'n') && (Gpsbuf[index + 3] == 'n') &&
|
|||
|
|
(Gpsbuf[index + 4] == 'e'))
|
|||
|
|
{
|
|||
|
|
Console.WriteLine(" Immediate Loc.Request Resp ");
|
|||
|
|
Console.WriteLine(" Req_id Anne");
|
|||
|
|
|
|||
|
|
index = index + req_len + 1;
|
|||
|
|
while ((index - 2) < max_index)
|
|||
|
|
{
|
|||
|
|
switch (Gpsbuf[index])
|
|||
|
|
{
|
|||
|
|
case MOTO.TKNIR_RESULT_OI_0:
|
|||
|
|
string str_err = GpsErrorDec(ref index, Gpsbuf);
|
|||
|
|
Console.WriteLine(str_err);
|
|||
|
|
break;
|
|||
|
|
case MOTO.TKNIR_INFO_TM_OI_5:
|
|||
|
|
ret_pos.time = GpsTimeDec(ref index, Gpsbuf);
|
|||
|
|
break;
|
|||
|
|
case MOTO.TKNLR_POINT_2D:
|
|||
|
|
ret_pos.pos = Gps2DDec(ref index, Gpsbuf);
|
|||
|
|
break;
|
|||
|
|
case MOTO.TKNLR_DIR_HOR_U8:
|
|||
|
|
ret_string += GpsHorDec(ref index, Gpsbuf);
|
|||
|
|
break;
|
|||
|
|
case MOTO.TKNLR_LEV_CONF_U8:
|
|||
|
|
ret_string += GpsLevDec(ref index, Gpsbuf);
|
|||
|
|
break;
|
|||
|
|
case MOTO.TKNLR_POINT_3D_2:
|
|||
|
|
ret_pos.pos = Gps3DDec(ref index, Gpsbuf);
|
|||
|
|
break;
|
|||
|
|
default:
|
|||
|
|
Console.WriteLine("GpsImmResp default...\n");
|
|||
|
|
index = max_index + 2; //end while
|
|||
|
|
break;
|
|||
|
|
}
|
|||
|
|
}// end while
|
|||
|
|
|
|||
|
|
}
|
|||
|
|
return ret_pos;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
private string GpsTrigStartResp(byte[] Gpsbuf)
|
|||
|
|
{
|
|||
|
|
Console.WriteLine("GpsTrigStartResp \n");
|
|||
|
|
string str = "";
|
|||
|
|
|
|||
|
|
int index, max_index;
|
|||
|
|
max_index = Gpsbuf[2];
|
|||
|
|
index = 4;
|
|||
|
|
|
|||
|
|
int req_len = Gpsbuf[index];
|
|||
|
|
Console.WriteLine(" max_index %02X req_len %02X \n", max_index, req_len);
|
|||
|
|
|
|||
|
|
// compare TX req_id with this RX req_id
|
|||
|
|
if ((req_len == 4) && (Gpsbuf[index + 1] == 'U') &&
|
|||
|
|
(Gpsbuf[index + 2] == 'f') && (Gpsbuf[index + 3] == 'f') &&
|
|||
|
|
(Gpsbuf[index + 4] == 'e'))
|
|||
|
|
{
|
|||
|
|
Console.WriteLine(" Trigger Start Request Resp ");
|
|||
|
|
|
|||
|
|
index = index + req_len + 1;
|
|||
|
|
while ((index - 2) <= max_index)
|
|||
|
|
{
|
|||
|
|
switch (Gpsbuf[index])
|
|||
|
|
{
|
|||
|
|
case MOTO.TKNIR_RESULT_0_OI_0: // result OK
|
|||
|
|
string.Format(" Result code 0 ");
|
|||
|
|
index++;
|
|||
|
|
break;
|
|||
|
|
case MOTO.TKNIR_RESULT_OI_0:
|
|||
|
|
string str_err = GpsErrorDec(ref index, Gpsbuf); Console.WriteLine(str_err);
|
|||
|
|
break;
|
|||
|
|
default:
|
|||
|
|
Console.WriteLine("GpsTrigStartResp default...\n");
|
|||
|
|
index = max_index + 2; //end while
|
|||
|
|
break;
|
|||
|
|
}
|
|||
|
|
}// end while
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
return str;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
private MOTO.GPS_POS GpsTrigLoc(byte[] Gpsbuf)
|
|||
|
|
{
|
|||
|
|
Console.WriteLine("GpsTrigLoc \n");
|
|||
|
|
string str;
|
|||
|
|
MOTO.GPS_POS ret_pos = new MOTO.GPS_POS();
|
|||
|
|
|
|||
|
|
int index, max_index;
|
|||
|
|
max_index = Gpsbuf[2];
|
|||
|
|
index = 4;
|
|||
|
|
|
|||
|
|
int req_len = Gpsbuf[index];
|
|||
|
|
Console.WriteLine(" max_index %02X req_len %02X \n", max_index, req_len);
|
|||
|
|
|
|||
|
|
// compare TX req_id with this RX req_id
|
|||
|
|
if ((req_len == 4) && (Gpsbuf[index + 1] == 'U') &&
|
|||
|
|
(Gpsbuf[index + 2] == 'f') && (Gpsbuf[index + 3] == 'f') &&
|
|||
|
|
(Gpsbuf[index + 4] == 'e'))
|
|||
|
|
{
|
|||
|
|
Console.WriteLine(" Trigger Location ");
|
|||
|
|
|
|||
|
|
|
|||
|
|
index = index + req_len + 1;
|
|||
|
|
while ((index - 2) < max_index)
|
|||
|
|
{
|
|||
|
|
switch (Gpsbuf[index])
|
|||
|
|
{
|
|||
|
|
case MOTO.TKNLR_POINT_3D_2:
|
|||
|
|
ret_pos.pos = Gps3DDec(ref index, Gpsbuf);
|
|||
|
|
break;
|
|||
|
|
default:
|
|||
|
|
Console.WriteLine("GpsTrigLoc default...\n");
|
|||
|
|
index = max_index + 2; //end while
|
|||
|
|
break;
|
|||
|
|
}
|
|||
|
|
}// end while
|
|||
|
|
}
|
|||
|
|
return ret_pos;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
private string GpsTrigStopResp(byte[] Gpsbuf)
|
|||
|
|
{
|
|||
|
|
Console.WriteLine("GpsTrigStopResp \n");
|
|||
|
|
string str = "";
|
|||
|
|
|
|||
|
|
int index, max_index;
|
|||
|
|
max_index = Gpsbuf[2];
|
|||
|
|
index = 4;
|
|||
|
|
|
|||
|
|
int req_len = Gpsbuf[index];
|
|||
|
|
Console.WriteLine(" max_index {0} req_len {1} \n", max_index, req_len);
|
|||
|
|
|
|||
|
|
// compare TX req_id with this RX req_id
|
|||
|
|
if ((req_len == 4) && (Gpsbuf[index + 1] == 'U') &&
|
|||
|
|
(Gpsbuf[index + 2] == 'f') && (Gpsbuf[index + 3] == 'f') &&
|
|||
|
|
(Gpsbuf[index + 4] == 'e'))
|
|||
|
|
{
|
|||
|
|
Console.WriteLine(" Trigger Stop Request Resp ");
|
|||
|
|
|
|||
|
|
index = index + req_len + 1;
|
|||
|
|
while ((index - 2) <= max_index)
|
|||
|
|
{
|
|||
|
|
switch (Gpsbuf[index])
|
|||
|
|
{
|
|||
|
|
case MOTO.TKNIR_RESULT_0_OI_0: // result OK
|
|||
|
|
string.Format(" Result code 0 ");
|
|||
|
|
index++;
|
|||
|
|
break;
|
|||
|
|
case MOTO.TKNIR_RESULT_OI_0:
|
|||
|
|
string str_err = GpsErrorDec(ref index, Gpsbuf); Console.WriteLine(str_err);
|
|||
|
|
break;
|
|||
|
|
default:
|
|||
|
|
Console.WriteLine("GpsTrigStopResp default...\n");
|
|||
|
|
index = max_index + 2; //end while
|
|||
|
|
break;
|
|||
|
|
}
|
|||
|
|
}// end while
|
|||
|
|
}
|
|||
|
|
return str;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
#endregion
|
|||
|
|
string GpsErrorDec(ref int i, byte[] buf)
|
|||
|
|
{
|
|||
|
|
Console.WriteLine("GpsErrorDec 0x{0:X} \n", i);
|
|||
|
|
Console.WriteLine("GpsErrorDec 0x{0:X} 0x{1:X} 0x{2:X} 0x{3:X} 0x{4:X} 0x{5:X}\n",
|
|||
|
|
buf[0], buf[1], buf[2], buf[3], buf[4], buf[5]);
|
|||
|
|
string ret_string;
|
|||
|
|
i++;
|
|||
|
|
if (buf[i] == MOTO.TIME_EXPIRED)
|
|||
|
|
{
|
|||
|
|
ret_string = string.Format(" Operation Error - TIME EXPIRED");
|
|||
|
|
}
|
|||
|
|
else if (buf[i] == MOTO.NO_SUCH_REQUEST)
|
|||
|
|
{
|
|||
|
|
ret_string = string.Format(" Operation Error - NO_SUCH_REQUEST");
|
|||
|
|
}
|
|||
|
|
else if (buf[i] == MOTO.DUPLICATE_REQUEST_ID)
|
|||
|
|
{
|
|||
|
|
ret_string = string.Format(" Operation Error - DUPLICATE_REQUEST_ID");
|
|||
|
|
}
|
|||
|
|
else if (buf[i] == MOTO.IRRS_RES_SYNTAX_ERROR)
|
|||
|
|
{
|
|||
|
|
ret_string = string.Format(" Operation Error - SYNTAX_ERROR");
|
|||
|
|
}
|
|||
|
|
else
|
|||
|
|
{
|
|||
|
|
ret_string = string.Format(" Operation Error {0:X} ", buf[i]);
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
i++;
|
|||
|
|
return ret_string;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
MOTO.irrs_con_time_t GpsTimeDec(ref int i, byte[] buf)
|
|||
|
|
{
|
|||
|
|
Console.WriteLine("GpsTimeDec {0:X} \n", i);
|
|||
|
|
|
|||
|
|
MOTO.irrs_con_time_t conv_time;
|
|||
|
|
i++;
|
|||
|
|
conv_time.year = (buf[i] << 6) | (buf[i + 1] >> 2);
|
|||
|
|
conv_time.month = ((buf[i + 1] & 0x03) << 2) | (buf[i + 2] >> 6);
|
|||
|
|
conv_time.day = (buf[i + 2] >> 1) & 0x1F;
|
|||
|
|
conv_time.hour = ((buf[i + 2] & 0x01) << 4) | ((buf[i + 3] & 0xF0) >> 4);
|
|||
|
|
conv_time.minute = ((buf[i + 3] & 0xF) << 2) | ((buf[i + 4] & 0xC0) >> 6);
|
|||
|
|
conv_time.second = buf[i + 4] & 0x3F;
|
|||
|
|
|
|||
|
|
Console.WriteLine(" Year {0:X} Month {1:X} Day {2:X} Hour {3:X} Minute {4:X} Second {5:X} \n",
|
|||
|
|
conv_time.year, conv_time.month, conv_time.day, conv_time.hour,
|
|||
|
|
conv_time.minute, conv_time.second);
|
|||
|
|
|
|||
|
|
i = i + 5;
|
|||
|
|
|
|||
|
|
return conv_time;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
MOTO.lrrs_con_pos_t Gps2DDec(ref int i, byte[] buf)
|
|||
|
|
{
|
|||
|
|
Console.WriteLine("Gps2DDec {0} \n", i);
|
|||
|
|
i++;
|
|||
|
|
|
|||
|
|
MOTO.lrrs_con_pos_t con_pos = new MOTO.lrrs_con_pos_t();
|
|||
|
|
|
|||
|
|
//decode latitude
|
|||
|
|
decode_gps_position(ref con_pos,true, buf, i);
|
|||
|
|
Console.WriteLine(" Longitude {0} {1}° {2}", con_pos.direction_lat, con_pos.degrees_lat, con_pos.minutes_lat + con_pos.fraction_lat);
|
|||
|
|
|
|||
|
|
i = i + 4;
|
|||
|
|
|
|||
|
|
//decode longitude
|
|||
|
|
decode_gps_position(ref con_pos, false, buf, i);
|
|||
|
|
Console.WriteLine(" Latitude {0} {1}° {2}", con_pos.direction_lng, con_pos.degrees_lng, con_pos.minutes_lng + con_pos.fraction_lng);
|
|||
|
|
|
|||
|
|
i = i + 4;
|
|||
|
|
|
|||
|
|
return con_pos;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
MOTO.lrrs_con_pos_t Gps3DDec(ref int i, byte[] buf)
|
|||
|
|
{
|
|||
|
|
Console.WriteLine("Gps3DDec {0} \n", i);
|
|||
|
|
i++;
|
|||
|
|
|
|||
|
|
MOTO.lrrs_con_pos_t con_pos = new MOTO.lrrs_con_pos_t();
|
|||
|
|
|
|||
|
|
//decode latitude
|
|||
|
|
decode_gps_position(ref con_pos, true, buf, i);
|
|||
|
|
Console.WriteLine(" Longitude {0} {1}° {2}", con_pos.direction_lat, con_pos.degrees_lat, con_pos.minutes_lat + con_pos.fraction_lat);
|
|||
|
|
|
|||
|
|
i = i + 4;
|
|||
|
|
|
|||
|
|
//decode longitude
|
|||
|
|
decode_gps_position(ref con_pos, false, buf, i);
|
|||
|
|
Console.WriteLine(" Latitude {0} {1}° {2}", con_pos.direction_lng, con_pos.degrees_lng, con_pos.minutes_lng + con_pos.fraction_lng);
|
|||
|
|
|
|||
|
|
i = i + 4;
|
|||
|
|
|
|||
|
|
//sintvar i.e. variable size, signed integer
|
|||
|
|
bool two_byte = ((buf[i] & 0x80) == 1) ? true : false; //continue
|
|||
|
|
bool positive = ((buf[i] & 0x40) == 1) ? true : false;
|
|||
|
|
int a = 0;
|
|||
|
|
a = buf[i] & 0x3F;
|
|||
|
|
i++;
|
|||
|
|
if (two_byte)
|
|||
|
|
{
|
|||
|
|
a = (a << 7 | buf[i]);
|
|||
|
|
i++;
|
|||
|
|
}
|
|||
|
|
if (a == 0)
|
|||
|
|
Console.WriteLine(" Altitude 0");
|
|||
|
|
else if (positive)
|
|||
|
|
Console.WriteLine(" Altitude -{0}", a);
|
|||
|
|
else
|
|||
|
|
Console.WriteLine(" Altitude {0}", a);
|
|||
|
|
|
|||
|
|
//uintvar
|
|||
|
|
string.Format(" Altitude Accuracy {0} meters", buf[i]);
|
|||
|
|
i++;
|
|||
|
|
|
|||
|
|
return con_pos;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
string GpsHorDec(ref int i, byte[] buf)
|
|||
|
|
{
|
|||
|
|
Console.WriteLine("GpsHorDec {0} \n", i);
|
|||
|
|
string str;
|
|||
|
|
i++;
|
|||
|
|
str = string.Format("GpsHorDec buf[i] {0} \n", buf[i]);
|
|||
|
|
if (buf[i] < 180)
|
|||
|
|
{
|
|||
|
|
str = string.Format(" Horizontal Direction {0} * 2 degrees ", buf[i]);
|
|||
|
|
|
|||
|
|
}
|
|||
|
|
Console.WriteLine(str);
|
|||
|
|
i++;
|
|||
|
|
return str;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
string GpsLevDec(ref int i, byte[] buf)
|
|||
|
|
{
|
|||
|
|
Console.WriteLine("GpsLevDec %02X \n", i);
|
|||
|
|
string str;
|
|||
|
|
i++;
|
|||
|
|
str = string.Format(" Level of Confidence %02d%% ", buf[i]);
|
|||
|
|
Console.WriteLine(str);
|
|||
|
|
i++;
|
|||
|
|
return str;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
void decode_gps_position(ref MOTO.lrrs_con_pos_t lrrs_con_pos, bool bLat, byte[] buf, int i)
|
|||
|
|
{
|
|||
|
|
ulong degrees=0;
|
|||
|
|
ulong minutes=0;
|
|||
|
|
double fraction=0;
|
|||
|
|
double dbl;
|
|||
|
|
ulong ulat;
|
|||
|
|
bool non_neg_sign= false;
|
|||
|
|
// Converts the input string to an unsigned long, keeps track of the sign seperately.
|
|||
|
|
ulat = 0;
|
|||
|
|
for (int t = i; t < 4; ++t)
|
|||
|
|
{
|
|||
|
|
ulat = (ulat << 8) + buf[t];
|
|||
|
|
}
|
|||
|
|
if (bLat)//latitude
|
|||
|
|
{
|
|||
|
|
non_neg_sign = true;
|
|||
|
|
if ((ulat & MOTO.LRRS_LAT_SIGN_MSK) == 0)
|
|||
|
|
{
|
|||
|
|
non_neg_sign = false;
|
|||
|
|
ulat &= (~MOTO.LRRS_LAT_SIGN_MSK);
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
// Converts to double, including setting the sign.
|
|||
|
|
dbl = (double)ulat / MOTO.LRRS_LAT_MULTIPLIER;
|
|||
|
|
if (non_neg_sign == false)
|
|||
|
|
dbl = -dbl;
|
|||
|
|
}
|
|||
|
|
else //longitude
|
|||
|
|
{
|
|||
|
|
// Convert the specified string into a unsigned long.
|
|||
|
|
if (ulat == 0x80000000)
|
|||
|
|
{
|
|||
|
|
dbl = -MOTO.LRRS_MAX_LON_DBL;
|
|||
|
|
}
|
|||
|
|
else
|
|||
|
|
{
|
|||
|
|
// Convert to double
|
|||
|
|
dbl = (double)ulat / MOTO.LRRS_LON_MULTIPLIER;
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
if (bLat)
|
|||
|
|
convert_to_con_components(dbl,ref non_neg_sign,
|
|||
|
|
ref degrees, ref minutes, ref fraction, MOTO.LRRS_MAX_LAT_INT);
|
|||
|
|
else
|
|||
|
|
convert_to_con_components(dbl, ref non_neg_sign,
|
|||
|
|
ref degrees, ref minutes, ref fraction, MOTO.LRRS_MAX_LON_INT);
|
|||
|
|
|
|||
|
|
if (bLat)
|
|||
|
|
{
|
|||
|
|
if (non_neg_sign == true)
|
|||
|
|
lrrs_con_pos.direction_lat = 'N';
|
|||
|
|
else
|
|||
|
|
lrrs_con_pos.direction_lat = 'S';
|
|||
|
|
|
|||
|
|
lrrs_con_pos.degrees_lat = degrees;
|
|||
|
|
lrrs_con_pos.minutes_lat = minutes;
|
|||
|
|
lrrs_con_pos.fraction_lat = fraction;
|
|||
|
|
}
|
|||
|
|
else
|
|||
|
|
{
|
|||
|
|
if (non_neg_sign == true)
|
|||
|
|
lrrs_con_pos.degrees_lng = 'E';
|
|||
|
|
else
|
|||
|
|
lrrs_con_pos.degrees_lng = 'W';
|
|||
|
|
|
|||
|
|
lrrs_con_pos.degrees_lng = degrees;
|
|||
|
|
lrrs_con_pos.minutes_lng = minutes;
|
|||
|
|
lrrs_con_pos.fraction_lng = fraction;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
|
|||
|
|
//return lrrs_con_pos;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
/*
|
|||
|
|
* Convert latitude or longitude information (double dbl
|
|||
|
|
* into the sign, degrees, minutes, and fraction parts.
|
|||
|
|
*/
|
|||
|
|
bool
|
|||
|
|
convert_to_con_components(double dbl,
|
|||
|
|
ref bool non_neg_sign_ptr, ref ulong dgr_ptr,
|
|||
|
|
ref ulong minu_ptr, ref double frc_ptr,
|
|||
|
|
ulong max_degrees)
|
|||
|
|
{
|
|||
|
|
|
|||
|
|
// Convert the specified value into its absolute value, keeping track of the sign separetely.
|
|||
|
|
non_neg_sign_ptr = true;
|
|||
|
|
if (dbl < 0)
|
|||
|
|
{
|
|||
|
|
dbl = -dbl;
|
|||
|
|
non_neg_sign_ptr = false;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
// Perform a quick validation against the worst case, to avoid the potential for overflow conditions.
|
|||
|
|
if (dbl > ((double)(max_degrees + MOTO.LAT_LON_DOUBLE_ROUNDING)))
|
|||
|
|
return false;
|
|||
|
|
|
|||
|
|
// Determine the values for the various components of the latitude or longitude value.
|
|||
|
|
dgr_ptr = (ulong)dbl;
|
|||
|
|
dbl -= dgr_ptr;
|
|||
|
|
|
|||
|
|
minu_ptr = (ulong)(dbl * MOTO.LRRS_LL_MIN_IN_DEG_INT);
|
|||
|
|
dbl -= ((double)minu_ptr / MOTO.LRRS_LL_MIN_IN_DEG_DBL);
|
|||
|
|
|
|||
|
|
frc_ptr = dbl * MOTO.LRRS_LL_MIN_IN_DEG_DBL;
|
|||
|
|
|
|||
|
|
// Handle any overflow condition. It is not likely that an overflow condition could exist. The error checking is mostly "to be on the
|
|||
|
|
if (frc_ptr < 0.0)
|
|||
|
|
frc_ptr = 0.0; /* rounding correction */
|
|||
|
|
if (frc_ptr > (1.0 - MOTO.LAT_LON_DOUBLE_ROUNDING))
|
|||
|
|
frc_ptr = 1.0 - MOTO.LAT_LON_DOUBLE_ROUNDING; /* rounding correction */
|
|||
|
|
|
|||
|
|
if (minu_ptr >= MOTO.LRRS_LL_MIN_IN_DEG_INT)
|
|||
|
|
{
|
|||
|
|
(minu_ptr) -= MOTO.LRRS_LL_MIN_IN_DEG_INT;
|
|||
|
|
(dgr_ptr)++;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
if (dgr_ptr > max_degrees)
|
|||
|
|
return false;
|
|||
|
|
if (dgr_ptr == max_degrees)
|
|||
|
|
{
|
|||
|
|
if ((minu_ptr > 0) || (frc_ptr > MOTO.MAX_FRC_VARIANCE))
|
|||
|
|
return false;
|
|||
|
|
frc_ptr = 0;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
return true;
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
}
|