diff --git a/include/dsd.h b/include/dsd.h index eb1307f..a97a7b4 100644 --- a/include/dsd.h +++ b/include/dsd.h @@ -1399,8 +1399,7 @@ void dmr_embedded_gps (dsd_opts * opts, dsd_state * state, uint8_t lc_bits[]); void apx_embedded_gps (dsd_opts * opts, dsd_state * state, uint8_t lc_bits[]); void lip_protocol_decoder (dsd_opts * opts, dsd_state * state, uint8_t * input); void nmea_iec_61162_1 (dsd_opts * opts, dsd_state * state, uint8_t * input, uint32_t src, int type); -void nmea_harris (dsd_opts * opts, dsd_state * state, uint8_t * input, uint32_t src, int slot); -void harris_gps(dsd_opts * opts, dsd_state * state, int slot, uint8_t * input); +void harris_lptt (dsd_opts * opts, dsd_state * state, uint8_t * input, uint32_t src, int slot, int phase); void utf16_to_text (dsd_state * state, uint8_t wr, uint16_t len, uint8_t * input); void utf8_to_text (dsd_state * state, uint8_t wr, uint16_t len, uint8_t * input); diff --git a/src/dsd_gps.c b/src/dsd_gps.c index f0067cc..d72e325 100644 --- a/src/dsd_gps.c +++ b/src/dsd_gps.c @@ -281,115 +281,129 @@ void nmea_iec_61162_1 (dsd_opts * opts, dsd_state * state, uint8_t * input, uint } -//restructured the Harris GPS to flow more like the DMR UDT NMEA Format when possible -void nmea_harris (dsd_opts * opts, dsd_state * state, uint8_t * input, uint32_t src, int slot) +//harris location on PTT (MAC) 3.100 +void harris_lptt (dsd_opts * opts, dsd_state * state, uint8_t * input, uint32_t src, int slot, int phase) { - //GPS working, Time Working (old method), COG and Speed are logical on due west sample - //TODO: Fiddle/Test various ns and ew bits for proper signs in case its located elsewhere - //UPDATE: Found two contiguous bits that always work on all my accumulated samples - //the bits also place it with the 'lone' flagged bit observed prior + //I really wonder about the engineers who laid out this message - uint8_t nmea_ns = input[88]; //north/south (lat sign) //72 previously used bit - uint8_t nmea_ew = input[89]; //east/west (lon sign) //88 previously used bit - uint8_t nmea_q = input[147]; //Quality Indicator (last fix or current fix) - // uint8_t nmea_speed = (uint8_t)ConvertBitIntoBytes(&input[129], 7); //129,7 - uint8_t nmea_speed = (uint8_t)ConvertBitIntoBytes(&input[129], 6); //this is one bit shorter than called for, but I believe it works + //NOTE: I have only seen the MAC variation of this message description, I cannot 100% confirm + //the two block LCW format is identical, but it does carry the same amount of relevant bytes + //observation shows the GPS coordinates are accurate on LCW format, but I cannot + //vouch for the COG,SPEED,TIME, EMG and GPS Quality elements when in LCW format. - //Latitude Bits - uint8_t nmea_ndeg = (uint8_t)ConvertBitIntoBytes(&input[65], 7); //Latitude Degrees - uint8_t nmea_nmin = (uint8_t)ConvertBitIntoBytes(&input[58], 6); //Latitude Minutes - uint16_t nmea_nminf = (uint16_t)ConvertBitIntoBytes(&input[42], 14); //Latitude Fractions of Minutes + //starting at relevant information, bytes 0,1,2,3 + uint16_t lat_min_dec = (uint16_t)ConvertBitIntoBytes(input+0, 16); + uint16_t lat_ns = input[16]; //0 == N, 1 == S + uint16_t lat_min_int = (uint16_t)ConvertBitIntoBytes(input+17, 7); + uint16_t lat_deg_int = (uint16_t)ConvertBitIntoBytes(input+24, 8); - //Longitude Bits - uint8_t nmea_edeg = (uint8_t)ConvertBitIntoBytes(&input[96], 8); //Longitude Degrees - uint8_t nmea_emin = (uint8_t)ConvertBitIntoBytes(&input[90], 6); //Longitude Minutes - uint16_t nmea_eminf = (uint16_t)ConvertBitIntoBytes(&input[74], 14); //Longitude Fractions of Minutes + //starting at relevant information, bytes 4,5,6,7 + uint16_t lon_min_dec = (uint16_t)ConvertBitIntoBytes(input+32, 16); + uint16_t lon_ew = input[48]; //0 == N, 1 == S + uint16_t lon_min_int = (uint16_t)ConvertBitIntoBytes(input+49, 7); + uint16_t lon_deg_int = (uint16_t)ConvertBitIntoBytes(input+56, 8); - //NOTE: Removed UTC hh.mm.ss6 format found in DMR UDT, could not make it work - //with bits already established for time and it works extremely well as is + //starting at relevant information, bytes 8,9 + uint32_t time_secs_low = (uint32_t)ConvertBitIntoBytes(input+64, 16); - //Course Over Ground in Degrees (0-360) - uint16_t nmea_cog = (uint16_t)ConvertBitIntoBytes(&input[135], 9) % 360; //course over ground in degrees //working on due west sample without %360 + //starting at relevant information, bytes 10,11,12,13 + uint16_t course_low = (uint16_t)ConvertBitIntoBytes(input+80, 8); + uint16_t speed_low = (uint16_t)ConvertBitIntoBytes(input+88, 8); + uint16_t course_high = (uint16_t)ConvertBitIntoBytes(input+96+4, 4); //says 3,2,1,0, + uint16_t speed_high = (uint16_t)ConvertBitIntoBytes(input+96+0, 4); //says 7,6,5,4 - //timestamp (16-bit value w/ appended 17th bit to MSB) - uint32_t rtime = (uint32_t)ConvertBitIntoBytes(&input[104], 16); - rtime |= input[144] << 16; //this bit is flagged on after 0xFFFF rollover in the late afternoon - uint32_t thour = rtime / 3600; - uint32_t tmin = (rtime % 3600) / 60; - uint32_t tsec = (rtime % 3600) % 60; + //starting at relevant information, byte 14 + uint32_t time_secs_high = input[104]; + uint8_t status_emergency = input[105]; + uint8_t gps_quality = (uint16_t)ConvertBitIntoBytes(input+106, 2); + uint8_t gps_sat_num = (uint16_t)ConvertBitIntoBytes(input+108, 4); - //lat and lon conversion - char deg_glyph[4]; - sprintf (deg_glyph, "%s", "°"); - float latitude = 0.0f; - float longitude = 0.0f; - float m_unit = 1.0f / 60.0f; - float mm_unit = 1.0f / 600000.0f; + //start calculating things + float lat_DDMMmm = (float)lat_deg_int + ((float)lat_min_int / 60.0f) + ((float)lat_min_dec / 600000.0f); + float lon_DDMMmm = (float)lon_deg_int + ((float)lon_min_int / 60.0f) + ((float)lon_min_dec / 600000.0f); + + if (lat_ns) + lat_DDMMmm *= -1.0f; + if (lon_ew) + lon_DDMMmm *= -1.0f; + + //course conversion + uint32_t course = (course_high << 4) | course_low; //tenths of degrees + float course_deg = (float)course / 10.0f; //actual value //speed conversion + uint32_t speed_knots = (speed_high << 4) | speed_low; //tenths of degrees float fmps, fmph, fkph = 0.0f; //conversion of knots to mps, kph, and mph values - fmps = (float)nmea_speed * 0.514444; UNUSED(fmps); - fmph = (float)nmea_speed * 1.15078f; UNUSED(fmph); - fkph = (float)nmea_speed * 1.852f; UNUSED(fkph); + fmps = ((float)speed_knots / 10.0f) * 0.514444; UNUSED(fmps); + fmph = ((float)speed_knots / 10.0f) * 1.15078f; UNUSED(fmph); + fkph = ((float)speed_knots / 10.0f) * 1.852f; UNUSED(fkph); - //calculate decimal representation of latidude and longitude (need some samples to test) - latitude = ( (float)nmea_ndeg + ((float)nmea_nmin*m_unit) + ((float)nmea_nminf*mm_unit) ); - longitude = ( (float)nmea_edeg + ((float)nmea_emin*m_unit) + ((float)nmea_eminf*mm_unit) ); + //seconds since midnight as a 17-bit representation and calculation to HHMMSS format + uint32_t sec_m = (time_secs_high << 16) | time_secs_low; + uint32_t thour = sec_m / 3600; + uint32_t tmin = (sec_m % 3600) / 60; + uint32_t tsec = (sec_m % 3600) % 60; - //This is opposite (seemingly) compared to the version specified by DMR UDT NMEA, could still be different bits involved (to be tested) - // if (nmea_ns) latitude *= -1.0f; //1 is South, 0 is North - // if (nmea_ew) longitude *= -1.0f; //1 is West, 0 is East - - //DMR UDT NMEA original version (working now with new bit selection 88 and 89 like how DMR UDT NMEA format specifies) - if (!nmea_ns) latitude *= -1.0f; //0 is South, 1 is North - if (!nmea_ew) longitude *= -1.0f; //0 is West, 1 is East - - //is this from LCW, or MAC? - uint16_t header = (uint16_t)ConvertBitIntoBytes(&input[0], 16); + char deg_glyph[4]; + sprintf (deg_glyph, "%s", "°"); + //print results fprintf (stderr, "\n"); - if (header == 0x2AA4) - fprintf (stderr, " SRC: %08d;", src); - else - fprintf (stderr, " VCH: %d - SRC: %08d;", slot, src); - fprintf (stderr, " GPS: %f%s, %f%s;", latitude, deg_glyph, longitude, deg_glyph); - - //Speed in Knots (assuming in knots, and not in mps, or kph) - if (header != 0x2AA4) //can't yet verify this is present or accurate on LCW + if (phase == 2) { - if (nmea_speed > 126) - fprintf (stderr, " SPD > 126 knots or %f MPH;", fmph); //using MPH here, its Harris, they're in the U.S. + fprintf (stderr, " LCH: %d;", slot); + if (src != 0) + fprintf (stderr, " SRC: %08d;", src); else - fprintf (stderr, " SPD: %d knots; %f MPH;", nmea_speed, fmph); + fprintf (stderr, " SRC: UNK;"); + + if (gps_quality < 3) //GPS hardware present + { + fprintf (stderr, " GPS: %f%s, %f%s;", lat_DDMMmm, deg_glyph, lon_DDMMmm, deg_glyph); + if (gps_quality == 0) + fprintf (stderr, " Last Fix;"); //No fix still holds last values + else if (gps_quality == 1) + fprintf (stderr, " Curr Fix;"); + else if (gps_quality == 2) + fprintf (stderr, " Diff Fix;"); //presumably either lat or lon only changed, not both? + + fprintf (stderr, " #Sats: %d;", gps_sat_num); + } + else if (gps_quality == 3) + fprintf (stderr, " GPS: Not Enabled / No Hardware Present;"); + + fprintf (stderr, " COG: %f%s;", course_deg, deg_glyph); + fprintf (stderr, " SPD: %f k/h;", fkph); + + if (sec_m != 0x1FFFF) // && sec_m != 0 (so says the doc, but midnight is a valid time) + fprintf (stderr, " T: %02d:%02d:%02d;", thour, tmin, tsec); + + if (status_emergency) + fprintf (stderr, " Emergency;"); + } + else if (phase == 1) + { + if (src != 0) + fprintf (stderr, " SRC: %08d;", src); + else + fprintf (stderr, " SRC: UNK;"); + fprintf (stderr, " GPS: %f%s, %f%s;", lat_DDMMmm, deg_glyph, lon_DDMMmm, deg_glyph); + + //again, can't vouch 100%, so just going to dump the GPS Coordinates and zero out other values for LRRP file + fkph = 0.0f; + course_deg = 0.0f; } - //Course Over Ground (COG) - fprintf (stderr, " COG: %03d%s;", nmea_cog, deg_glyph); //%360 - - //debug speed - // fprintf (stderr, " RSPD: %02X;", nmea_speed); - - //debug cog - // fprintf (stderr, " RCOG: %03X;", nmea_cog >> 0); - - //UTC Time - fprintf (stderr, " T: %02d:%02d:%02d UTC;", thour, tmin, tsec); - - //Fix Indicator - if (nmea_q) fprintf (stderr, " Current Fix;"); - else fprintf (stderr, " Last Fix;"); - //save to ncurses string - sprintf (state->dmr_embedded_gps[slot], "(%f%s, %f%s)", latitude, deg_glyph, longitude, deg_glyph); + sprintf (state->dmr_embedded_gps[slot], "(%f%s, %f%s)", lat_DDMMmm, deg_glyph, lon_DDMMmm, deg_glyph); //save to event history string if (state->event_history_s[slot].Event_History_Items[0].source_id == src && src != 0) sprintf (state->event_history_s[slot].Event_History_Items[0].gps_s, "%s", state->dmr_embedded_gps[slot]); //save to LRRP report for mapping/logging - FILE * pFile; //file pointer - if (opts->lrrp_file_output == 1) + if (opts->lrrp_file_output == 1 && src != 0 && gps_quality != 3) { char * datestr = getDate(); @@ -397,153 +411,16 @@ void nmea_harris (dsd_opts * opts, dsd_state * state, uint8_t * input, uint32_t //rounded interger formats for the log report int s = (int)fkph; - int a = nmea_cog; - - //open file by name that is supplied in the ncurses terminal, or cli - pFile = fopen (opts->lrrp_out_file, "a"); - fprintf (pFile, "%s\t", datestr ); - fprintf (pFile, "%s\t", timestr ); //could switch to UTC time on PDU if desired - fprintf (pFile, "%08d\t", src); - fprintf (pFile, "%.6lf\t", latitude); - fprintf (pFile, "%.6lf\t", longitude); - fprintf (pFile, "%d\t ", s); - fprintf (pFile, "%d\t ", a); - fprintf (pFile, "\n"); - fclose (pFile); - - if (timestr != NULL) - { - free (timestr); - timestr = NULL; - } - if (datestr != NULL) - { - free (datestr); - datestr = NULL; - } - - } - - //NOTE: There seems to be a few more octets left undecoded in this PDU - //not including the CRC obviously, unsure of their meaning, could relate - //to other GPS functions like precision, etc, unknown. - - //NOTE2: All these values are just best effort based on observation and - //making map points match using logic and distance over time for speed and course - //without any documentation, any of these values may still be wrong - -} - -//fallback version (if desired/required) -void harris_gps(dsd_opts * opts, dsd_state * state, int slot, uint8_t * input) -{ - - uint8_t lat_sign, lat_deg, lat_min = 0; - uint8_t lon_sign, lon_deg, lon_min = 0; - uint16_t lat_mmin = 0; - uint16_t lon_mmin = 0; - - //potentially in this PDU, but unverifiable without documentation or reasonable - //mathematical proof vs map points and direction of travel / distance over time (assuming the radio is facing that direction) - uint16_t rspeed = (uint16_t)ConvertBitIntoBytes(&input[136], 8); //MSB //136 - rspeed = (rspeed << 8) + (uint16_t)ConvertBitIntoBytes(&input[128], 8); //LSB //128 - - //this works okay for example of driver driving due west at the speed limit of the road (fluke?) - uint16_t rangle = (uint16_t)ConvertBitIntoBytes(&input[120], 8); //120,8 - float fspeed = (float)rspeed; - float fangle = (float)rangle; - - fspeed /= 255.0f; //unit value of 1 bit - fspeed *= 1.56f; //this is based on an observation of a driver moving approx 210 meters in about 8 seconds and this makes it just under the speed limit on the road there - // fangle *= 360.0f/255.0f; //unit value of 1 bit - fangle *= 2.0f; //may exceed 360 degrees as is (use %) - - int s, a = 0; - s = (int)fspeed * 3.6; - a = (int)fangle % 360; - - //fix and quality indicators? - uint8_t fix = input[147]; UNUSED(fix); - uint8_t quality = (uint16_t)ConvertBitIntoBytes(&input[148], 3); UNUSED(quality); - - //timestamp (16-bit value w/ appended 17th bit to MSB) - uint32_t rtime = (uint32_t)ConvertBitIntoBytes(&input[104], 16); //seconds since midnight since last GPS fix (whatever the radio has as internal time) - rtime |= input[144] << 16; //test appending this as bit 17 (observed this bit set after the afternoon 0xFFFF rollover) - uint32_t thour = rtime / 3600; - uint32_t tmin = (rtime % 3600) / 60; - uint32_t tsec = (rtime % 3600) % 60; - - float lat_dec = 0.0f; - float lon_dec = 0.0f; - - char deg_glyph[4]; - sprintf (deg_glyph, "%s", "°"); - - //This appears to be similar to the NMEA GPGGA format (DDmm.mm) but - //octets are ordered in least significant to most significant value - lat_mmin = (uint16_t)ConvertBitIntoBytes(&input[40], 16); //? bits required, but grabbing two octets - lat_min = (uint8_t)ConvertBitIntoBytes(&input[58], 6); //6 bits required to get 60 - lat_deg = (uint8_t)ConvertBitIntoBytes(&input[65], 7); //7 bits required to get 90 - lat_sign = input[72]; //64 - - lon_mmin = (uint16_t)ConvertBitIntoBytes(&input[72], 16); //? bits required, but grabbing two octets - lon_min = (uint8_t)ConvertBitIntoBytes(&input[90], 6); //6 bits required to get 60 - lon_deg = (uint8_t)ConvertBitIntoBytes(&input[96], 8); //8 bits required to get 180 - lon_sign = input[88]; //88, unsure of a correct location, but on the sample with 0 minutes, this lonely bit was flagged on - - int src = 0; - if (slot == 0) src = state->lastsrc; - if (slot == 1) src = state->lastsrcR; - - //calculate decimal representation (was a pain to figure out the sub minute values) - lat_dec = ( (float)lat_deg + ((float)lat_min/60.0f) + ((float)lat_mmin/600000.0f) ); - lon_dec = ( (float)lon_deg + ((float)lon_min/60.0f) + ((float)lon_mmin/600000.0f) ); - - if (lat_sign) - lat_dec *= -1.0f; - - if (lon_sign) - lon_dec *= -1.0f; - - //line break - fprintf (stderr, "\n"); - fprintf (stderr, " GPS: %f%s, %f%s;", lat_dec, deg_glyph, lon_dec, deg_glyph); - - //gps fix time - // fprintf (stderr, " RT: %05X;", rtime); //debug - fprintf (stderr, " LTS: %02d:%02d:%02d UTC;", thour, tmin, tsec); //last time synced to GPS - - //speed and direction - // fprintf (stderr, " RSPD: %04X;", rspeed); //debug - // fprintf (stderr, " RANG: %02X;", rangle); //debug - // fprintf (stderr, " MPS: %06.02f;", fspeed); - // fprintf (stderr, " KPH: %06.02f;", fspeed * 3.6f); - // fprintf (stderr, " MPH: %06.02f;", fspeed * 2.2369f); - fprintf (stderr, " DIR: %03d%s;", a, deg_glyph); - - // fprintf (stderr, " FIX: %d", fix); - // fprintf (stderr, " QLT: %02d", quality); - - // fprintf (stderr, " SRC: %08d;", src); - - //save to array for ncurses - sprintf (state->dmr_embedded_gps[slot], "GPS: (%f%s, %f%s)", lat_dec, deg_glyph, lon_dec, deg_glyph); - - //save to LRRP report for mapping/logging - FILE * pFile; //file pointer - if (opts->lrrp_file_output == 1) - { - - char * datestr = getDate(); - char * timestr = getTime(); + int a = (int)course_deg; //open file by name that is supplied in the ncurses terminal, or cli + FILE * pFile; //file pointer pFile = fopen (opts->lrrp_out_file, "a"); fprintf (pFile, "%s\t", datestr ); fprintf (pFile, "%s\t", timestr ); fprintf (pFile, "%08d\t", src); - fprintf (pFile, "%.6lf\t", lat_dec); - fprintf (pFile, "%.6lf\t", lon_dec); + fprintf (pFile, "%.6lf\t", lat_DDMMmm); + fprintf (pFile, "%.6lf\t", lon_DDMMmm); fprintf (pFile, "%d\t ", s); fprintf (pFile, "%d\t ", a); fprintf (pFile, "\n"); @@ -562,9 +439,14 @@ void harris_gps(dsd_opts * opts, dsd_state * state, int slot, uint8_t * input) } - //NOTE: Thanks to DSheirer (SDRTrunk) for helping me work out a few of the things in here - //not entirely convinced on some of these calcs (speed, angle, and TS) but sure these bits - //are actual speed/direction/timestamps judging from what the coordinates show on a map + //debugging dump of this message + // if (opts->payload == 1) + // { + // fprintf (stderr, "\n "); + // for (int i = 0; i < 14; i++) + // fprintf (stderr, "%02X ", (uint8_t)ConvertBitIntoBytes(input+(i*8), 8)); + // } + } //externalize embedded GPS - Confirmed working now on NE, NW, SE, and SW coordinates diff --git a/src/p25_lcw.c b/src/p25_lcw.c index ff8856d..b18a594 100644 --- a/src/p25_lcw.c +++ b/src/p25_lcw.c @@ -506,16 +506,16 @@ void p25_lcw (dsd_opts * opts, dsd_state * state, uint8_t LCW_bits[], uint8_t ir fprintf (stderr, " MFIDA4 (Harris) GPS Block 1"); memset(state->dmr_pdu_sf[0], 0, sizeof(state->dmr_pdu_sf[0])); memcpy(state->dmr_pdu_sf[0], LCW_bits, 16*sizeof(uint8_t)); //opcode and mfid for check - memcpy(state->dmr_pdu_sf[0]+40, LCW_bits+16, 56*sizeof(uint8_t)); //+40 offset to match the vPDU decoder + memcpy(state->dmr_pdu_sf[0]+16, LCW_bits+16, 56*sizeof(uint8_t)); //contents of message in this block } else if (lc_mfid == 0xA4 && lc_opcode == 0x2B) { fprintf (stderr, " MFIDA4 (Harris) GPS Block 2"); - memcpy(state->dmr_pdu_sf[0]+40+56, LCW_bits+16, 56*sizeof(uint8_t)); //+40 +56 to offset first block + memcpy(state->dmr_pdu_sf[0]+16+56, LCW_bits+16, 56*sizeof(uint8_t)); //+16 +56 to offset first block uint16_t check = (uint16_t)ConvertBitIntoBytes(&state->dmr_pdu_sf[0][0], 16); if (check == 0x2AA4) - nmea_harris(opts, state, state->dmr_pdu_sf[0], (uint32_t)state->lastsrc, 0); + harris_lptt(opts, state, state->dmr_pdu_sf[0]+16, (uint32_t)state->lastsrc, 0, 1); else fprintf (stderr, " Missing GPS Block 1"); memset(state->dmr_pdu_sf[0], 0, sizeof(state->dmr_pdu_sf[0])); } diff --git a/src/p25p2_vpdu.c b/src/p25p2_vpdu.c index 4c372ed..9c4a5e6 100644 --- a/src/p25p2_vpdu.c +++ b/src/p25p2_vpdu.c @@ -1816,12 +1816,12 @@ void process_MAC_VPDU(dsd_opts * opts, dsd_state * state, int type, unsigned lon if (MAC[len_a+1] == 0x80 && MAC[len_a+2] != 0xA4 && MAC[len_a+2] != 0x90) { int unk1 = MAC[len_a+1]; //assuming this is the octet set for the 'manufacturer specific' message, may only be the MSBit - int unk2 = MAC[len_a+2]; //This field is observed as 0xAA, unknown if this is an opcode, or other MFID + int unk2 = MAC[len_a+2]; //This field is observed as 0xAA, its possible its a derived message of 0x2A from P25p1 LCW format int mfid = MAC[len_a+3]; //This is where the 0xA4 (Harris) Identifier is found in this message, as opposed to +2 - int len = MAC[len_a+4] & 0x3F;; //0x11 or 17 dec sounds reasonable, but cannot verify + int len = MAC[len_a+4] & 0x3F; //len of this message (should always be 17) //bugfix observed on random errant second MAC message on Phase 2 - //although can't 100% confirm, pretty sure these all have same len value + //confirmed this MAC message is always 17 (0x11) if (len != 0x11) goto END_PDU; @@ -1841,8 +1841,7 @@ void process_MAC_VPDU(dsd_opts * opts, dsd_state * state, int type, unsigned lon if (slot == 0 && state->lastsrc != 0) tsrc = state->lastsrc; if (slot == 1 && state->lastsrcR != 0) tsrc = state->lastsrcR; - // harris_gps (opts, state, slot, mac_bits); //fallback - nmea_harris (opts, state, mac_bits+0, tsrc, slot); //new + harris_lptt (opts, state, mac_bits+40, tsrc, slot, 2); //debug - just dump payload // for (i = 0; i < 24; i++)