SafeDispatch/SDRgateway/Parse_LRRP.cs

599 lines
21 KiB
C#
Raw Permalink Normal View History

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