using SafeMobileLib; using SDRGatewayService.Enums; using System; using System.Collections.Generic; using System.Diagnostics; using System.Linq; using System.Text; using System.Threading.Tasks; namespace SDRGatewayService { 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 { Utils.WriteEventLog(Program.COMPANY, "UNS_LOC_REP not TKNICE_REQID_OI_1", EventLogEntryType.Warning, (int)EventId.EVENT_LRRP); } } 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 { Utils.WriteEventLog(Program.COMPANY, "IMM_LOC_REP not TKNICE_REQID_OI", EventLogEntryType.Warning, (int)EventId.EVENT_LRRP); } } 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 { Utils.WriteEventLog(Program.COMPANY, "TRI_LOC_ANS not TKNICE_REQID_OI", EventLogEntryType.Warning, (int)EventId.EVENT_LRRP); } } 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 { Utils.WriteEventLog(Program.COMPANY, "TRI_LOC_REP not TKNICE_REQID_OI", EventLogEntryType.Warning, (int)EventId.EVENT_LRRP); } } 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 { Utils.WriteEventLog(Program.COMPANY, "TRI_STP_ANS not TKNICE_REQID_OI", EventLogEntryType.Warning, (int)EventId.EVENT_LRRP); } } else { Utils.WriteEventLog(Program.COMPANY, "GPS data\n", EventLogEntryType.Warning, (int)EventId.EVENT_LRRP); } return gps_pos; } #region response type private MOTO.GPS_POS GpsUnsRep(byte[] Gpsbuf) { Utils.WriteEventLog(Program.COMPANY, "GPSUnsRep \n", EventLogEntryType.Information, (int)EventId.EVENT_LRRP); 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) { Utils.WriteEventLog(Program.COMPANY, "GPSImmResp \n", EventLogEntryType.Information, (int)EventId.EVENT_LRRP); 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]; Utils.WriteEventLog(Program.COMPANY, " max_index "+ max_index + " req_len " + req_len + "\n", EventLogEntryType.Information, (int)EventId.EVENT_LRRP); // 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')) { Utils.WriteEventLog(Program.COMPANY, "Immediate Loc.Request Resp " + Environment.NewLine + " Req_id Anne", EventLogEntryType.Information, (int)EventId.EVENT_LRRP); 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); Utils.WriteEventLog(Program.COMPANY, str_err, EventLogEntryType.Information, (int)EventId.EVENT_LRRP); 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: Utils.WriteEventLog(Program.COMPANY, "GPSImmResp default...\n", EventLogEntryType.Information, (int)EventId.EVENT_LRRP); index = max_index + 2; //end while break; } }// end while } return ret_pos; } private string GpsTrigStartResp(byte[] Gpsbuf) { Utils.WriteEventLog(Program.COMPANY, "GPSTrigStartResp \n", EventLogEntryType.Information, (int)EventId.EVENT_LRRP); string str = ""; int index, max_index; max_index = Gpsbuf[2]; index = 4; int req_len = Gpsbuf[index]; Utils.WriteEventLog(Program.COMPANY, "max_index " + max_index + "req_len " + req_len + "\n", EventLogEntryType.Information, (int)EventId.EVENT_LRRP); // 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')) { Utils.WriteEventLog(Program.COMPANY, " Trigger Start Request Resp ", EventLogEntryType.Information, (int)EventId.EVENT_LRRP); 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); Utils.WriteEventLog(Program.COMPANY, str_err, EventLogEntryType.Information, (int)EventId.EVENT_LRRP); break; default: Utils.WriteEventLog(Program.COMPANY, "GPSTrigStartResp default...\n", EventLogEntryType.Information, (int)EventId.EVENT_LRRP); index = max_index + 2; //end while break; } }// end while } return str; } private MOTO.GPS_POS GpsTrigLoc(byte[] Gpsbuf) { Utils.WriteEventLog(Program.COMPANY, "GPSTrigLoc \n", EventLogEntryType.Information, (int)EventId.EVENT_LRRP); 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]; Utils.WriteEventLog(Program.COMPANY, " max_index " + max_index + " req_len " + req_len + "\n", EventLogEntryType.Information, (int)EventId.EVENT_LRRP); // 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')) { Utils.WriteEventLog(Program.COMPANY, "Trigger Location ", EventLogEntryType.Information, (int)EventId.EVENT_LRRP); 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: Utils.WriteEventLog(Program.COMPANY, "GPSTrigLoc default... \n", EventLogEntryType.Information, (int)EventId.EVENT_LRRP); index = max_index + 2; //end while break; } }// end while } return ret_pos; } private string GpsTrigStopResp(byte[] Gpsbuf) { Utils.WriteEventLog(Program.COMPANY, "GPSTrigStopResp \n", EventLogEntryType.Information, (int)EventId.EVENT_LRRP); string str = ""; int index, max_index; max_index = Gpsbuf[2]; index = 4; int req_len = Gpsbuf[index]; Utils.WriteEventLog(Program.COMPANY, " max_index " + max_index + " req_len " + req_len + "\n", EventLogEntryType.Information, (int)EventId.EVENT_LRRP); // 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')) { Utils.WriteEventLog(Program.COMPANY, " Trigger Stop Request Resp ", EventLogEntryType.Information, (int)EventId.EVENT_LRRP); 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); Utils.WriteEventLog(Program.COMPANY, str_err, EventLogEntryType.Information, (int)EventId.EVENT_LRRP); break; default: Utils.WriteEventLog(Program.COMPANY, " GPSTrigStopResp default... \n", EventLogEntryType.Information, (int)EventId.EVENT_LRRP); index = max_index + 2; //end while break; } }// end while } return str; } #endregion string GpsErrorDec(ref int i, byte[] buf) { Utils.WriteEventLog(Program.COMPANY, String.Format("GpsErrorDec 0x{0:X} \n", i) + Environment.NewLine + String.Format("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]), EventLogEntryType.Information, (int)EventId.EVENT_LRRP); 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) { Utils.WriteEventLog(Program.COMPANY, String.Format(" GPSTimeDec {0:X} \n" ,i), EventLogEntryType.Information, (int)EventId.EVENT_LRRP); 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; Utils.WriteEventLog(Program.COMPANY, String.Format(" 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), EventLogEntryType.Information, (int)EventId.EVENT_LRRP); i = i + 5; return conv_time; } MOTO.lrrs_con_pos_t Gps2DDec(ref int i, byte[] buf) { Utils.WriteEventLog(Program.COMPANY, String.Format("GPS2Dec {0} \n", i), EventLogEntryType.Information, (int)EventId.EVENT_LRRP); 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); Utils.WriteEventLog(Program.COMPANY, String.Format(" Longitude {0} {1}° {2}", con_pos.direction_lat, con_pos.degrees_lat, con_pos.minutes_lat + con_pos.fraction_lat), EventLogEntryType.Information, (int)EventId.EVENT_LRRP); i = i + 4; //decode longitude decode_gps_position(ref con_pos, false, buf, i); Utils.WriteEventLog(Program.COMPANY, String.Format(" Latitude {0} {1}° {2}", con_pos.direction_lng, con_pos.degrees_lng, con_pos.minutes_lng + con_pos.fraction_lng), EventLogEntryType.Information, (int)EventId.EVENT_LRRP); i = i + 4; return con_pos; } MOTO.lrrs_con_pos_t Gps3DDec(ref int i, byte[] buf) { Utils.WriteEventLog(Program.COMPANY, String.Format(" GPS3DDec {0} \n", i), EventLogEntryType.Information, (int)EventId.EVENT_LRRP); 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); Utils.WriteEventLog(Program.COMPANY, String.Format(" Longitude {0} {1}° {2}", con_pos.direction_lat, con_pos.degrees_lat, con_pos.minutes_lat + con_pos.fraction_lat), EventLogEntryType.Information, (int)EventId.EVENT_LRRP); i = i + 4; //decode longitude decode_gps_position(ref con_pos, false, buf, i); Utils.WriteEventLog(Program.COMPANY, String.Format(" Latitude {0} {1}° {2}", con_pos.direction_lng, con_pos.degrees_lng, con_pos.minutes_lng + con_pos.fraction_lng), EventLogEntryType.Information, (int)EventId.EVENT_LRRP); 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) Utils.WriteEventLog(Program.COMPANY, String.Format(" Altitude 0"), EventLogEntryType.Information, (int)EventId.EVENT_LRRP); else if (positive) Utils.WriteEventLog(Program.COMPANY, String.Format(" Altitude -{0}", a), EventLogEntryType.Information, (int)EventId.EVENT_LRRP); else Utils.WriteEventLog(Program.COMPANY, String.Format(" Altitude {0}", a), EventLogEntryType.Information, (int)EventId.EVENT_LRRP); //uintvar string.Format(" Altitude Accuracy {0} meters", buf[i]); i++; return con_pos; } string GpsHorDec(ref int i, byte[] buf) { Utils.WriteEventLog(Program.COMPANY, String.Format("GPSHorDec {0} \n", i), EventLogEntryType.Information, (int)EventId.EVENT_LRRP); 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]); } Utils.WriteEventLog(Program.COMPANY, str, EventLogEntryType.Information, (int)EventId.EVENT_LRRP); i++; return str; } string GpsLevDec(ref int i, byte[] buf) { Utils.WriteEventLog(Program.COMPANY, String.Format("GPSLevDec %02X \n", i), EventLogEntryType.Information, (int)EventId.EVENT_LRRP); string str; i++; str = string.Format(" Level of Confidence %02d%% ", buf[i]); Utils.WriteEventLog(Program.COMPANY, str, EventLogEntryType.Information, (int)EventId.EVENT_LRRP); 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; } } }